ジャイロと加速度計の積分値を読み取ってロボットを動かす

Question

今、私はロボットをまっすぐに動かすためにエンコーダを使用していますが、75％で動いています。精度を高めるために、私はジャイロと加速度計の読みを選択することに決めました。

現在、MPU 6050加速度計とジャイロスコープを使用してデバイスの加速度、ピッチ、およびロール加速度x、y、zを取得しますが、その情報を使用して直線運動の速度を調整する方法はわかりません。

また、読み取り値（ジャイロ、accelero）が正しいかどうかは疑問です。

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

同様加速度yおよび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; 
} 

出力：

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