Gyroscope for a Balancing Robot

I am currently trying to build a balancing robot using this IMU and this Instructable.

The IMU keeps outputting “nan” after initially giving me good values whenever I try to test the motor control as the angle changes. Also, whenever I try using the Arduino with a 9V battery, there is a delay between when the motor changes direction and when the code should register a value that would cause the motors to change direction. I do not understand why this is happening since there is no delay in my code.

My question is this: Why does the IMU output ‘nan’ repeatedly and what else should I test in order to figure out what is wrong with my robot?

Here is the code:

void setup() {
  
  Wire.begin();

  Serial.begin(9600);
  setupL3G4200D(200); // Configure L3G4200  - 250, 500 or 2000 deg/sec
  delay(1500); 
  if(!accel.begin())
  {
    /* There was a problem detecting the ADXL345 ... check your connections */
    Serial.println("Ooops, no ADXL345 detected ... Check your wiring!");
    while(1);
  }
  
    // Motor
    AFMS.begin();
    motor1->run(RELEASE);
    motor2->run(RELEASE);
  
  accel.setRange(ADXL345_RANGE_2_G);
  sensors_event_t event; 
  accel.getEvent(&event);
  
  // Calculate bias for the Gyro i.e. the values it gives when it's not moving
  for(int i=1; i < 100; i++){
    
    getGyroValues();
    gyroBiasX += (int)x;
    gyroBiasY += (int)y;
    gyroBiasZ += (int)z;  

    accel.getEvent(&event);
    accBiasX += event.acceleration.x;
    accBiasY += event.acceleration.y;
    accBiasZ += event.acceleration.z;

    delay(1);
  }
  

  gyroBiasX = gyroBiasX / 100;
  gyroBiasY = gyroBiasY / 100;
  gyroBiasZ = gyroBiasZ / 100;

  accBiasX = accBiasX / 100;
  accBiasY = accBiasY / 100;
  accBiasZ = accBiasZ / 100;
  

  //Get Starting Pitch and Roll
  accel.getEvent(&event);
  accPitch = (atan2(-event.acceleration.x,-event.acceleration.z)+PI)*RAD_TO_DEG;
  accRoll = (atan2(event.acceleration.y,-event.acceleration.z)+PI)*RAD_TO_DEG;

  if (accPitch <= 360 & accPitch >= 180){
    accPitch = accPitch - 360;
  }

  if (accRoll <= 360 & accRoll >= 180){
    accRoll = accRoll - 360;
  }
  

  // Set starting angle for Kalman
  kalmanPitch.setAngle(accPitch); 
  kalmanRoll.setAngle(accRoll);
  
  kalmanRoll.setQangle(0.01);      // 0.001
  kalmanRoll.setQbias(0.0003);    // 0.003
  kalmanRoll.setRmeasure(0.01);    // 0.03
  
  gyroPitch = accPitch;
  gyroRoll = accRoll;

  timer = micros();
  delay(1000);
  InitialValues(); //intial values

}

double tau=0.075;
double a=0.0;
double x_angleC = 0;

double Complementary(double newAngle, double newRate,double looptime) {
  
  double dtC = float(looptime)/1000000.0;
  a=tau/(tau+dtC);
  x_angleC= a* (x_angleC + newRate * dtC) + (1-a) * (newAngle);
  return x_angleC;
  
}

double Setpoint;

void MotorControl(double out){
  
  // Sets direction
  if (out > 0){
    motor1->setSpeed(175);
    motor2->setSpeed(175);
    motor1->run(FORWARD);  
    motor2->run(FORWARD);
  }else if(out < 0){
    motor1->setSpeed(175);
    motor2->setSpeed(175);
    motor1->run(BACKWARD);  
    motor2->run(BACKWARD);
  }
  
  else{
    motor1->setSpeed(0);
    motor2->setSpeed(0);
    motor1->run(RELEASE);
    motor2->run(RELEASE);
  }
  
  
  byte vel = abs(out);    // Absolute value of velocity
  
  // Checks velocity fits the max ouptut range
  if (vel<0)
    vel=0;
  if (vel > 255)
    vel=255;
  
  // Writes the PWM 
}

void InitialValues(){
  
  //////////////////////
  //  Accelerometer   //
  //////////////////////
  sensors_event_t event; 

  accel.getEvent(&event);
  accPitch = (atan2(-event.acceleration.x,-event.acceleration.z)+PI)*RAD_TO_DEG;
  accRoll = (atan2(event.acceleration.y,-event.acceleration.z)+PI)*RAD_TO_DEG;

  if (accPitch <= 360 & accPitch >= 180){
    accPitch = accPitch - 360;
  }

  if (accRoll <= 360 & accRoll >= 180){
    accRoll = accRoll - 360;
  }


  //////////////////////
  //      GYRO        //
  //////////////////////


  getGyroValues();

  // read raw angular velocity measurements from device  
  gyroRateX = ((int)x - gyroBiasX)*.07; //*(.0105);
  gyroRateY = -((int)y - gyroBiasY)*.07; //*(.0105);
  gyroRateZ = ((int)z - gyroBiasZ)*.07; //*(.0105);

  gyroPitch += gyroRateY * ((double)(micros() - timer)/1000000);
  gyroRoll += gyroRateX * ((double)(micros() - timer)/1000000);
  gyroYaw += gyroRateZ * ((double)(micros() - timer)/1000000);

  PitchInicial = kalmanPitch.getAngle(accPitch, gyroPitch, (double)(micros()-timer)/1000000);
  //RollInicial = kalmanRoll.getAngle(accRoll, gyroRoll, (double)(micros()-timer)/1000000);
  timer = micros();
  RollInicial = gyroRoll;
  
  Serial.print("Pitch Initial: ");
  Serial.println(PitchInitial);
  Serial.print("Roll Initial: ");
  Serial.println(RollInitial);
  Setpoint = 0;
  
}

int i=0;
double aaa =0;
void loop() {
  Serial.println("Got here");
    //////////////////////
    //  Accelerometer   //
    //////////////////////
    sensors_event_t event; 
  
    accel.getEvent(&event);
    accPitch = (atan2(-event.acceleration.x,-event.acceleration.z)+PI)*RAD_TO_DEG;
    accRoll = (atan2(event.acceleration.y,-event.acceleration.z)+PI)*RAD_TO_DEG;
  
    if (accPitch <= 360 & accPitch >= 180){
      accPitch = accPitch - 360;
    }
  
    if (accRoll <= 360 & accRoll >= 180){
      accRoll = accRoll - 360;
    }
  
  
    //////////////////////
    //      GYRO        //
    //////////////////////
  
  
    getGyroValues();
  
    // read raw angular velocity measurements from device  
    gyroRateX = -((int)x - gyroBiasX)*.07; //*(.0105);
    gyroRateY = -((int)y - gyroBiasY)*.07; //*(.0105);
    gyroRateZ = ((int)z - gyroBiasZ)*.07; //*(.0105);
  
    gyroPitch += gyroRateY * ((double)(micros() - timer)/1000000);
    double leeeeel = gyroRateX * ((double)(micros() - timer)/1000000);
    gyroRoll += gyroRateX * ((double)(micros() - timer)/1000000);
    gyroYaw += gyroRateZ * ((double)(micros() - timer)/1000000);
  
    InputPitch = kalmanPitch.getAngle(accPitch, gyroPitch, (double)(micros()-timer)/1000000);
    InputRoll = kalmanRoll.getAngle(accRoll, gyroRoll, (double)(micros()-timer)/1000000);
    timer = micros();
    

    byte a= map(abs(Compute(InputRoll-RollInicial)),0,255,0,124);
    aaa = 0.98* (aaa + leeeeel) + 0.02 * (accRoll);
    Serial.println(aaa-RollInicial);
    MotorControl(Compute(aaa-RollInicial));
    
   // i++;
    //if (i=100){
      //i=0;
      //gyroRoll = (InputRoll-RollInicial);
    //}  

int outMax = 255;
int outMin = -255;
float lastInput = 0;
double ITerm =0;
double kp = 100;
double ki = 2;
double kd = 0;

double Compute(double input)
{

      double error = Setpoint - input;
      ITerm+= (ki * error);
      if(ITerm > outMax) ITerm= outMax;
      else if(ITerm < outMin) ITerm= outMin;
      double dInput = (input - lastInput);
 
      /*Compute PID Output*/
      double output = kp * error + ITerm + kd * dInput;
      
  if(output > outMax) output = outMax;
      else if(output < outMin) output = outMin;
  
      /*Remember some variables for next time*/
      lastInput = input;
      return output;
}

If you are trying to power the robot with a 9V PP3 battery, don't. Those batteries are not useful with motors.

Edit: the code is an absolute mess and it is completely unclear what you are trying to do. If you want help with it, post code that will compile without errors.

It would not let me upload my entire code since it was over 9000 characters. I could send you the entire thing. It does compile and runs fine. I am just having trouble with the output.

Also, I have an external 6V power supply to supply power to my motors. The 9V is simply powering the Arduino. I was trying to say that whenever I use the serial port instead of the 9V to run the code from the Arduino, I get a different output from the 9V than the serial port to power JUST the Arduino

You can attach reasonably large files to a post. I have no idea what you mean by this:

I was trying to say that whenever I use the serial port instead of the 9V to run the code from the Arduino, I get a different output from the 9V than the serial port to power JUST the Arduino

Here is the code that will compile. You need a few libraries in order for it to work.

Balacning_single_file.ino (10.8 KB)