자이로와 가속도계 판독 값을 통합하여 로봇 스트레이트 이동

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 :-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 

,536 : 오른쪽 90시

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 

RightMotion 90

에서 종료 91,363,210

다시 직선 운동 :

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

Answer 1

자이로 포지티브 또는 네거티브도 초기 호 어긋나 로봇의 수를 나타내는 값을 반환한다. 로봇이 계속 직진하면 표제는 0이됩니다. 예 :

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