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