arduinogyroscopeservompu6050

Program stops after while loop begins


I'm trying to move my servos according to data from my gyro. Unfortunately, my program stops after the while loop begins. For instance, in my code once the gyroX value reaches 3000 the serial monitor stops giving me gyro data and the servo does not run. Any suggestions on how to solve this problem would be appreciated.

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

Servo servo;

long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ;

long gyroX, gyroY, gyroZ;
float rotX, rotY, rotZ;

int pos1 = 90;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  setupMPU();
}


void loop() {
  recordAccelRegisters();
  recordGyroRegisters();
  printData();
  while( gyroX > 3000 ){ //problem happens here
    pos1 += 5;
    servo.write(pos1);
  } 
}

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(); 
}

void recordAccelRegisters() {
  Wire.beginTransmission(0b1101000); 
  Wire.write(0x43);
  Wire.endTransmission();
  Wire.requestFrom(0b1101000,6);
  while(Wire.available() < 6);
  accelX = Wire.read()<<8|Wire.read();
  accelY = Wire.read()<<8|Wire.read();
  accelZ = Wire.read()<<8|Wire.read();
}

void printData() {
  Serial.print("Gyro (deg)");
  Serial.print(" X=");
  Serial.print(gyroX);
  Serial.print(" Y=");
  Serial.print(gyroY);
  Serial.print(" Z=");
  Serial.print(gyroZ);
  Serial.print(" Accel (g)");
  Serial.print(" X=");
  Serial.print(accelX);
  Serial.print(" Y=");
  Serial.print(accelY);
  Serial.print(" Z=");
  Serial.println(accelZ);
}

Solution

  • Inside of that while loop there are no commands which could change the value of gyroX. So once you are inside it becomes an infinite loop. You need to call your recordGyroRegisters function inside the while loop so that variable gets updated during it. Or you could take advantage of the fact that the loop function is already looping and simply get rid of the for loop and opt for something based on an if statement instead.