Интеграция показаний гироскопа и акселерометра для перемещения робота прямо

Question

Теперь я использую кодировщик для прямого перемещения робота, но он работает только на 75%. Для большей точности я решил выбрать показания гироскопа и акселерометра.

В настоящее время используется акселерометр и гироскоп MPU 6050 для получения рыскания, высоты тона и рулона, ускорения x, y, z устройства, но не знаете, как использовать эту информацию для регулировки скорости для прямого движения?

Также у меня есть сомнения в правильности показаний (гироскоп, ускоритель) или нет?

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

Похожие тангажа и крена.

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

Похожие акселерометр у и г

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

Выход:

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 

На момент правой 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 

Выход из RightMotion 90

Опять прямые движения:

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

Answer 1

Гироскоп возвращает значение, указывающее число степеней положительных или отрицательных робот отклонилась от своего первоначального заголовка. Пока робот продолжает двигаться прямо, заголовок будет равен нулю. Для примера.

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