aaaaaaaa
Dependencies: mbed Servo MPU6050
main.cpp
- Committer:
- kosukesuzuki
- Date:
- 2022-02-23
- Revision:
- 2:c329b0c4ec92
- Parent:
- 1:036305ced8fd
- Child:
- 3:05a780930d12
File content as of revision 2:c329b0c4ec92:
//PID制御 #include "mbed.h" #include "MPU6050.h" const double dt = 0.01; //微小時間 const double Kp =0.3; //変更必須 const double Ki =0.4; const double Kd =0.5; const double mc = 77; //最大トルク const double T =0; //目標角度 MPU6050 mpu(p9,p10); Serial pc(USBTX,USBRX); PwmOut led1(LED1); //motor PwmOut led2(LED2); PwmOut led3(LED3); PwmOut led4(LED4); int gyro[3]; double GX,GY,GZ; double Tgx,Tgy,Tgz; int main() { GX = GY = GZ= 0; Tgx = Tgy = Tgz = 0; while(1){ //角度求める mpu.readGyroData(gyro); int gx = gyro[0]+3656-3505-50; //変更必須 int gy = gyro[1]-30; int gz = gyro[2]+5; printf("%d %d %d\r\n",gx,gy,gz); double gX = gx*0.02562299; double gY = gy*0.02562299; double gZ = gz*0.02562299; int gX1 = gX; int gY1 = gY; int gZ1 = gZ; double gX2 = gX1*dt; double gY2 = gY1*dt; double gZ2 = gZ1*dt; GX = GX + gX2; GY = GY + gY2; GZ = GZ + gZ2; Tgx = Tgx + abs(gX2); Tgy = Tgy + abs(gY2); Tgz = Tgz + abs(gZ2); if(Tgx > 5){ if(GX > 0){ GX = GX - 0.3; }else{ GX = GX + 0.3; } Tgx=0; } if(Tgy > 5){ if(GY > 0){ GY = GY - 0.3; }else{ GY = GY + 0.3; } Tgy=0; } if(Tgz > 5){ if(GZ > 0){ GZ = GZ - 0.3; }else{ GZ = GZ + 0.3; } Tgz=0; } //printf("%.2f %.2f %.2f\r\n",GX,GY,GZ); wait(0.01); //PID() double c1,c2; double f1,f2; f1 = T - GX; f2 = T - GY; c1 = Kp*f1+Ki*f1*dt+Kd*gX1; //トルク c2 = Kp+f2+Ki*f2*dt+Kd*gY1; //printf("%.2f %.2f\r\n",c1,c2); //回転トルク→duty比 double d1,d2; d1 = c1/mc; d2 = c2/mc; if(d1 > 1){ d1 = 1; } if(d2 > 1){ d2 = 1; } //duty比からledを動かす。 if(GX > 0){ led1 = abs(d1); led2 = -abs(d1); }else{ led2 = abs(d1); led1 = -abs(d1); } if(GY > 0){ led3 = abs(d2); led4 = -abs(d2); }else{ led4 = abs(d2); led3 = -abs(d2); } } }