Problem bei if & & & eine Vorgabe auszuführen

International Deutsch
1

Hallo,
habe ein Problem bei der if Abfrage in Kombination mit & & &. :woozy_face:
Es soll wenn der Pin 22 = HIGH und bei einer minimalen Bewegung von 2° sofort
alle digitalWrite's auf HIGH gehen. Wenn der Pin 22 = LOW funktioniert alles super, wenn
er dann auf High gesetzt wird funktioniert nichts mehr. Wo habe ich den Bock verbaut?


#include <Wire.h>


//Declaring some global variables
int gyro_x, gyro_y, gyro_z;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
boolean set_gyro_angles;
long acc_x, acc_y, acc_z, acc_total_vector;
float angle_roll_acc, angle_pitch_acc;
float angle_pitch, angle_roll;
int angle_pitch_buffer, angle_roll_buffer;
float angle_pitch_output, angle_roll_output;
const int inPin = 22;

long loop_timer;
int temp;

void setup() {

  pinMode(3, OUTPUT);       //relais
  pinMode(4, OUTPUT);       //led rot
  pinMode(5, OUTPUT);       //led gelb
  pinMode(6, OUTPUT);       //led weiß
  pinMode(7, OUTPUT);       //mittlere led weiß
  pinMode(8, OUTPUT);       //led weiß
  pinMode(9, OUTPUT);       //led gelb
  pinMode(10, OUTPUT);      //led rot

  Wire.begin();                                                        //Start I2C as master
  setup_mpu_6050_registers();                                          //Setup the registers of the MPU-6050
  for (int cal_int = 0; cal_int < 1000 ; cal_int ++) {                 //Read the raw acc and gyro data from the MPU-6050 for 1000 times
    read_mpu_6050_data();
    gyro_x_cal += gyro_x;                                              //Add the gyro x offset to the gyro_x_cal variable
    gyro_y_cal += gyro_y;                                              //Add the gyro y offset to the gyro_y_cal variable
    gyro_z_cal += gyro_z;                                              //Add the gyro z offset to the gyro_z_cal variable
    delay(3);                                                          //Delay 3us to have 250Hz for-loop
  }
  // divide by 1000 to get avarage offset
  gyro_x_cal /= 1000;
  gyro_y_cal /= 1000;
  gyro_z_cal /= 1000;
  Serial.begin(115200);
  loop_timer = micros();                                               //Reset the loop timer
}
void loop() {

  read_mpu_6050_data();

  //Subtract the offset values from the raw gyro values

  gyro_x -= gyro_x_cal;

  gyro_y -= gyro_y_cal;

  gyro_z -= gyro_z_cal;


  //Gyro angle calculations . Note 0.0000611 = 1 / (250Hz x 65.5)

  angle_pitch += gyro_x * 0.0000611;                                   //Calculate the traveled pitch angle and add this to the angle_pitch variable

  angle_roll += gyro_y * 0.0000611;                                    //Calculate the traveled roll angle and add this to the angle_roll variable

  //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians

  angle_pitch += angle_roll * sin(gyro_z * 0.000001066);               //If the IMU has yawed transfer the roll angle to the pitch angel

  angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);               //If the IMU has yawed transfer the pitch angle to the roll angel

  //Accelerometer angle calculations

  acc_total_vector = sqrt((acc_x * acc_x) + (acc_y * acc_y) + (acc_z * acc_z)); //Calculate the total accelerometer vector

  //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians

  angle_pitch_acc = asin((float)acc_y / acc_total_vector) * 57.296;    //Calculate the pitch angle

  angle_roll_acc = asin((float)acc_x / acc_total_vector) * -57.296;    //Calculate the roll angle



  angle_pitch_acc -= 0.0;                                              //Accelerometer calibration value for pitch

  angle_roll_acc -= 0.0;                                               //Accelerometer calibration value for roll


  if (set_gyro_angles) {                                               //If the IMU is already started

    angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;     //Correct the drift of the gyro pitch angle with the accelerometer pitch angle

    angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;        //Correct the drift of the gyro roll angle with the accelerometer roll angle

  }

  else {                                                               //At first start

    angle_pitch = angle_pitch_acc;                                     //Set the gyro pitch angle equal to the accelerometer pitch angle

    angle_roll = angle_roll_acc;                                       //Set the gyro roll angle equal to the accelerometer roll angle

    set_gyro_angles = true;                                            //Set the IMU started flag
  }
  //To dampen the pitch and roll angles a complementary filter is used

  angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1;   //Take 90% of the output pitch value and add 10% of the raw pitch value

  angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1;      //Take 90% of the output roll value and add 10% of the raw roll value

  Serial.print(" | Angle  = "); Serial.println(angle_pitch_output);

  if (digitalRead(inPin) == HIGH && angle_pitch_output > 0 && angle_pitch_output < 2 && angle_pitch_output < -2)
    //if(digitalRead(BP_PinB) == HIGH)
    //if(angle_pitch_output > 0 && angle_pitch_output < 2)
    //if(angle_pitch_output > 0 && angle_pitch_output < -2)
  {
    digitalWrite(3, HIGH);
    digitalWrite(4, HIGH);
    digitalWrite(5, HIGH);
    digitalWrite(6, HIGH);
    digitalWrite(7, HIGH);
    digitalWrite(8, HIGH);
    digitalWrite(9, HIGH);
    digitalWrite(10, HIGH);
  }
  else if (angle_pitch_output >= 5 && angle_pitch_output <= 10)
  {
    digitalWrite(3, HIGH);
    digitalWrite(4, LOW);
    digitalWrite(5, LOW);
    digitalWrite(6, LOW);
    digitalWrite(7, HIGH);
    digitalWrite(8, LOW);
    digitalWrite(9, LOW);
    digitalWrite(10, LOW);
  }
  else if (angle_pitch_output >= 10 && angle_pitch_output <= 20)
  {
    digitalWrite(3, HIGH);
    digitalWrite(4, LOW);
    digitalWrite(5, LOW);
    digitalWrite(6, HIGH);
    digitalWrite(7, HIGH);
    digitalWrite(8, LOW);
    digitalWrite(9, LOW);
    digitalWrite(10, LOW);
  }
  else if (angle_pitch_output >= 20 && angle_pitch_output <= 30)
  {
    digitalWrite(3, HIGH);
    digitalWrite(4, LOW);
    digitalWrite(5, HIGH);
    digitalWrite(6, HIGH);
    digitalWrite(7, HIGH);
    digitalWrite(8, LOW);
    digitalWrite(9, LOW);
    digitalWrite(10, LOW);
  }
  else if (angle_pitch_output >= 30 && angle_pitch_output <= 40)
  {
    digitalWrite(3, LOW);
    digitalWrite(4, HIGH);
    digitalWrite(5, HIGH);
    digitalWrite(6, HIGH);
    digitalWrite(7, HIGH);
    digitalWrite(8, LOW);
    digitalWrite(9, LOW);
    digitalWrite(10, LOW);
  }
  else if (angle_pitch_output <= 0 && angle_pitch_output >= -5)
  {
    digitalWrite(3, HIGH);
    digitalWrite(4, LOW);
    digitalWrite(5, LOW);
    digitalWrite(6, LOW);
    digitalWrite(7, HIGH);
    digitalWrite(8, LOW);
    digitalWrite(9, LOW);
    digitalWrite(10, LOW);
  }
  else if (angle_pitch_output <= -5 && angle_pitch_output >= -10)
  {
    digitalWrite(3, HIGH);
    digitalWrite(4, LOW);
    digitalWrite(5, LOW);
    digitalWrite(6, LOW);
    digitalWrite(7, HIGH);
    digitalWrite(8, HIGH);
    digitalWrite(9, LOW);
    digitalWrite(10, LOW);
  }
  else if (angle_pitch_output <= -10 && angle_pitch_output >= -20)
  {
    digitalWrite(3, HIGH);
    digitalWrite(4, LOW);
    digitalWrite(5, LOW);
    digitalWrite(6, LOW);
    digitalWrite(7, HIGH);
    digitalWrite(8, HIGH);
    digitalWrite(9, HIGH);
    digitalWrite(10, LOW);
  }
  else if (angle_pitch_output <= -20 && angle_pitch_output >= -30)
  {
    digitalWrite(3, HIGH);
    digitalWrite(4, LOW);
    digitalWrite(5, LOW);
    digitalWrite(6, LOW);
    digitalWrite(7, HIGH);
    digitalWrite(8, HIGH);
    digitalWrite(9, HIGH);
    digitalWrite(10, HIGH);
  }



  while (micros() - loop_timer < 4000);                                //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop

  loop_timer = micros();//Reset the loop timer



}





void setup_mpu_6050_registers() {

  //Activate the MPU-6050

  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050

  Wire.write(0x6B);                                                    //Send the requested starting register

  Wire.write(0x00);                                                    //Set the requested starting register

  Wire.endTransmission();

  //Configure the accelerometer (+/-8g)

  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050

  Wire.write(0x1C);                                                    //Send the requested starting register

  Wire.write(0x10);                                                    //Set the requested starting register

  Wire.endTransmission();

  //Configure the gyro (500dps full scale)

  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050

  Wire.write(0x1B);                                                    //Send the requested starting register

  Wire.write(0x08);                                                    //Set the requested starting register

  Wire.endTransmission();

}



void read_mpu_6050_data() {                                            //Subroutine for reading the raw gyro and accelerometer data

  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050

  Wire.write(0x3B);                                                    //Send the requested starting register

  Wire.endTransmission();                                              //End the transmission

  Wire.requestFrom(0x68, 14);                                          //Request 14 bytes from the MPU-6050

  while (Wire.available() < 14);                                       //Wait until all the bytes are received

  acc_x = Wire.read() << 8 | Wire.read();

  acc_y = Wire.read() << 8 | Wire.read();

  acc_z = Wire.read() << 8 | Wire.read();

  temp = Wire.read() << 8 | Wire.read();

  gyro_x = Wire.read() << 8 | Wire.read();

  gyro_y = Wire.read() << 8 | Wire.read();

  gyro_z = Wire.read() << 8 | Wire.read();


}

Vielen Dank

2

Wie kann angle_pitch_output wahr werden? Es kann doch nie gleichzeitig größer 0 und kleine -2 sein. Daher ist diese Abfrage immer falsch und es tritt nur das else ein.

3

Guten Morgen und erstmal danke.
Ja das ist definitiv auch ein Fehler!
Aber wenn der Pin 22 = High ist funktioniert auch die ELSE nicht mehr.

4

Ich glaube aber ich habe es gefunden, der PINMODE wurde nicht deklariert.
Kann das der Fehler sein oder gibt es noch einen anderen Fehler.
Kann leider nicht testen, mein MPU ist gestern abgeraucht.

5

Ich hab’s jetzt nicht auf die Schnelle gesehen. Aber wo wird pin22 als Input deklariert? Bzw. was bestimmt dessen Pegel? Ein Taster? Gibt es einen PullUp/Down zur Pegelbestimmung, wenn der evtl. vorhandene Taster nicht gedrückt ist?

6

Ja ein Taster bestimmt den Pegel.
Und ja habe vergessen den pin22 zu deklarieren :roll_eyes: ,glaube das müsste es gewesen sein?!?!
Hoffe ich.
Aber vielen Dank für Deine Unterstützung.

7

Vergiss den PullUp/Down nicht! Abhängig von der Verschaltung des Tasters.
Und finde den Fehler heraus, warum dein Arduino kaputt gegangen ist, bevor du den neuen auch zerstörst.

8

Noch eine Frage, muss ich bei den anderen ELSE den PIN auf LOW setzen oder läuft das auch so?

9

Wie meinst du das mit „setzen“? Ich denke du bestimmst den Pegel über den Taster. Warum willst du ihn dann auf LOW setzen?
Ich denke du hast dich unglücklich ausgedrückt.

Oder falls die Frage sich darauf bezieht, ob du bei jedem ELSE IF vergleichen musst, ob der Pin LOW ist, dann ja. Wenn du den Vergleich nicht machst, dann ist es egal welchen Pegel der Pin hat und die IF wird ausgeführt.

Du könntest die IF auch verschachtelt, und nur einmalig den Pegel des Pins abfragen und dementsprechend Code ausführen.

10

Hallo
und guten Morgen.
Schmeisst der Compiler Fehlermeldungen auf den Markt?

angle_pitch_output > 0 && angle_pitch_output < 2 && angle_pitch_output < -2

Evt. fehlen hier ein paar ().

11

Das müsste jetzt korrekt sein oder?

if (digitalRead(inPin) == HIGH && angle_pitch_output >= 2 && angle_pitch_output <= -2)
12

Nein, keinen Einzigen.

13

Sieht erstmal gut aus. Alles natürlich abhängig von deiner dahinter stehenden Logik.

14

Ok

Hab einen geschmeidigen Tag und viel Spass beim Programmieren in C++.

15

Ja das meinte ich, ob jede ELSE IF den Pin Vergleich benötigt. Wenn ich das jetzt richtig verstanden habe, brauchen die anderen das nicht. Weil er soll nur bei inPIN=HIGH bei minimaler Bewegung alles auf HIGH setzen und wenn inPIN=LOW ganz normal die ELSE IF ausführen?!?!

16

Danke!
Dir auch!

17

Nein. Wenn dann müsstest du den pin22 Einzel abfragen, ala
(Pseudocode vom Handy getippt)

if (inPin == HIGH)
{
  if (angle_pitch_output >= 2 && angle_pitch_output <= -2)
  {
  // tue etwas
  }
else
{
  if (a==b)
  {
  // tue etwas wenn inPin LOW und a gleich b
  }
  if ( a < b )
  {
  // tue etwas wenn inPin gleich LOW und a kleiner b
  }
}

Oder jedesmal inPin mit && in den IF Vergleich einziehen.

Muss auf Arbeit, viel Glück weiterhin.

18

Vielen Dank und einen schönen Tag!

19

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.