jeudi 5 octobre 2017

Program stops after I add more if statements

Recently I've been having a problem where I add 2 more if statements and it stops my code all together. For example, the code below will work with the two gyroX if statements but once I add those two gyroY if statements the whole code won't run. Any suggestions on how to solve this issue would be appreciated.

#include <Wire.h>
#include <Servo.h>

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

int pos1 = 0;
int pos2 = 0;

long gyroX, gyroY, gyroZ;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  setupMPU();
  servo1.attach(4);
  servo2.attach(5);
  servo3.attach(6);
  servo4.attach(7);
}

void loop() { 
  recordGyroRegisters();
  if(gyroX <= 0)
  {
      pos1 += 1;
      servo1.writeMicroseconds(pos1);
      servo3.writeMicroseconds(pos1);
  }
  if(gyroX > 0)
  {
      pos1 -= 1;
      servo1.writeMicroseconds(pos1);
      servo3.writeMicroseconds(pos1);
  }
  if(gyroY <= 0)  //PROBLEM OCCURS WHEN I ADD THIS FUNCTION
  {
      pos2 += 1;
      servo2.writeMicroseconds(pos2);
      servo4.writeMicroseconds(pos2);
  }
  if(gyroY > 0)   //PROBLEM OCCURS WHEN I ADD THIS FUNCTION
  {
      pos2 -= 1;
      servo2.writeMicroseconds(pos2);
      servo4.writeMicroseconds(pos2);
  }
}

void setupMPU(){
  Wire.beginTransmission(0b1101000); 
  Wire.write(0x6B); 
  Wire.write(0b00000000); 
  Wire.endTransmission();  
  Wire.beginTransmission(0b1101000); 
  Wire.write(0x1B); 
  Wire.write(0x00000000); 
  Wire.endTransmission(); 
  Wire.beginTransmission(0b1101000);
  Wire.write(0x1C);  
  Wire.write(0b00000000); 
  Wire.endTransmission(); 
}

void recordGyroRegisters() {
  Wire.beginTransmission(0b1101000);
  Wire.write(0x3B); 
  Wire.endTransmission();
  Wire.requestFrom(0b1101000,6);
  while(Wire.available() < 6);
  gyroX = Wire.read()<<8|Wire.read();
  gyroY = Wire.read()<<8|Wire.read(); 
  gyroZ = Wire.read()<<8|Wire.read(); 
}

Aucun commentaire:

Enregistrer un commentaire