Jerwin Prabu A Jerwin Prabu A - 27 days ago 11
C++ Question

Integrating Gyro and Accelerometer Readings to move Robot Straight

Now, I am using encoder to move my robot straight, but it is working 75% only. For more accuracy, I have decided to choose gyro and accelerometer reading.

Currently using MPU 6050 accelerometer and gyroscope to get the yaw, pitch, and roll, Acceleration x,y,z of the device, but do not know how to use that information to adjust the speed for straight motion ?

Also I have doubt whether the readings (gyro, accelero) correct or not ?

dmpmpu6050.cpp

float DmpMPU6050_Demo::Loop_Yaw()
{
if (!dmpReady)
{
return 1;
}
fifoCount = mpu.getFIFOCount();

if (fifoCount == 1024)
{
mpu.resetFIFO();
printf("FIFO overflow!\n");
}

else if (fifoCount >= 42 )
{
mpu.getFIFOBytes(fifoBuffer, packetSize);

#ifdef OUTPUT_READABLE_YAWPITCHROLL
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

return ( ypr[0] * 180/M_PI);
#endif

printf("\n");
}
}


Similar pitch and roll.

float DmpMPU6050_Demo::Loop_Accelx()
{
if (!dmpReady)
{
return 1;
}
fifoCount = mpu.getFIFOCount();

if (fifoCount == 1024)
{
mpu.resetFIFO();
printf("FIFO overflow!\n");
}

else if (fifoCount >= 42 )
{
mpu.getFIFOBytes(fifoBuffer, packetSize);

#ifdef OUTPUT_READABLE_REALACCEL
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);

return 1;
#endif

printf("\n");
}
}


Similar Accelerometer y and z

Gyroscopethread.cpp

int numbercount=0;
float yawdata;
float accelX;

void GyroScopeThread::run()
{
gscope = new DmpMPU6050_Demo();
accelerometer = new DmpMPU6050_Demo();

gscope->Setup();
accelerometer->Setup();
usleep(10000);

int number = 100;
while (true)
{
if (this->gyrostop) break;

yawdata = gscope->Loop_Yaw();

for(int i = 0; i<2; i++)
{
float yawdata1 = gscope->Loop_Yaw();

yawdata = yawdata + yawdata1;

delay(1);
}

yawdata = yawdata/3;

if(numbercount == number){

emit Yaw_Data(yawdata);
cout<<"yaw :"<<yawdata<<endl;

similar calculation for accelero meter

numbercount = 0;// count value of data
}

}
numbercount = numbercount+1; // data count increment
}

delete gscope;
delete accelerometer;
}


Output :

yaw :-14.9574 pitch :-18.3952 roll :-18.3952
Accelx :1.33333 Accely :1 Accelz :1
yaw :-5.5584 pitch :-5.5584 roll :-6.57062
Accelx :0.333333 Accely :0.666667 Accelz :0.666667
yaw :-11.8345 pitch :-10.9161 roll :-10.9161
Accelx :0.666667 Accely :1 Accelz :1
yaw :-4.5936 pitch :-4.5936 roll :-4.5936
Accelx :1.33333 Accely :1.33333 Accelz :1.33333
yaw :-9.574 pitch :-9.574 roll :-9.574
Accelx :0.666667 Accely :1 Accelz :1
yaw :-10.1267 pitch :-10.1267 roll :-10.1267
Accelx :1.33333 Accely :1.33333 Accelz :1.33333


At the time of right 90:

yaw :-12.2344 pitch :-11.8448 roll :-11.8448
Accelx :1.33333 Accely :1.33333 Accelz :1.33333
yaw :0.726291 pitch :-4.36679 roll :-4.36679
Accelx :1.33333 Accely :1.33333 Accelz :1.33333
yaw :7.62387 pitch :7.62387 roll :7.62387
Accelx :1 Accely :1 Accelz :1
yaw :18.6464 pitch :18.6464 roll :18.6464
Accelx :1.33333 Accely :1.33333 Accelz :1
yaw :-4.06193 pitch :-8.62676 roll :-7.67034
Accelx :1 Accely :1 Accelz :1
yaw :-18.9466 pitch :-17.4917 roll :-12.0176
Accelx :1 Accely :1 Accelz :1
yaw :-4.94824 pitch :-9.12684 roll :-9.12684
Accelx :1 Accely :1 Accelz :1
yaw :-6.94877 pitch :-10.4829 roll :-10.4829
Accelx :1 Accely :1 Accelz :1
yaw :-19.0769 pitch :-17.6077 roll :-12.0728
Accelx :1 Accely :1 Accelz :1
yaw :-3.13396 pitch :-11.7479 roll :-10.2981
Accelx :1 Accely :1 Accelz :1
yaw :12.7717 pitch :1.98726 roll :1.98726
Accelx :0.333336 Accely :0.666668 Accelz :0.666668
yaw :-6.66976 pitch :-6.66976 roll :-6.66976
Accelx :1 Accely :1 Accelz :1


Exiting from RightMotion 90

Again straight motion :

yaw :-14.1805 pitch :-14.1805 roll :-10.3879
Accelx :0.333508 Accely :0.333508 Accelz :0.666783

Answer

The gyro returns a value indicating the number of degrees positive or negative the robot deviated from its initial heading. As long as the robot continues to go straight, the heading will be zero. For ex.

    class GyroSample : public SampleRobot
{
 RobotDrive myRobot; // robot drive system
 AnalogGyro gyro;
 static const float kP = 0.03;

public:
 GyroSample():
  myRobot(1, 2), // initialize the sensors in initilization list
  gyro(1)
 {
  myRobot.SetExpiration(0.1);
 }

 void Autonomous()
 {
  gyro.Reset();
  while (IsAutonomous())
  {
   float angle = gyro.GetAngle(); // get heading
   myRobot.Drive(-1.0, -angle * kP); // turn to correct heading
   Wait(0.004);
  }
  myRobot.Drive(0.0, 0.0); // stop robot
 }
};
Comments