Intégrer les lectures du gyroscope et de l'accéléromètre pour déplacer le robot directement

Question

Maintenant, j'utilise un encodeur pour déplacer mon robot directement, mais il ne fonctionne qu'à 75%. Pour plus de précision, j'ai décidé de choisir le gyro et l'accéléromètre.

Actuellement en utilisant MPU 6050 accéléromètre et gyroscope pour obtenir le lacet, le tangage et le roulis, Accélération x, y, z de l'appareil, mais ne sais pas comment utiliser cette information pour ajuster la vitesse pour le mouvement rectiligne?

Aussi je doute que les lectures (gyro, accelero) correctes ou non?

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

pas similaires 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"); 
    } 
} 

Accéléromètre similaires y et 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; 
} 

sortie:

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 

Au moment de la droite 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 

Sortie de RightMotion 90

mouvement rectiligne Encore une fois:

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

Answer 1

Le gyro retourne une valeur indiquant le nombre de degrés le robot positifs ou négatifs dévié de sa position initiale. Tant que le robot continue d'avancer, le cap sera nul. Par 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 
} 
};