aaaaaaaa
Dependencies: mbed Servo MPU6050
Diff: main.cpp
- Revision:
- 4:fad5b5a598bf
- Parent:
- 3:05a780930d12
--- a/main.cpp Fri Feb 25 03:15:53 2022 +0000 +++ b/main.cpp Fri Feb 25 04:38:59 2022 +0000 @@ -36,7 +36,7 @@ int main() { GX = GY = GZ= 0; Tgx = Tgy = Tgz = 0; - pc.printf("start"); + pc.printf("start\r\n"); motor1 = motor2 = motor3 = motor4 = 0.0; @@ -70,9 +70,9 @@ while(1){ //角度求める mpu.readGyroData(gyro); - int gx = gyro[0]+3656-3505-50-1850+2223; //変更必須 - int gy = gyro[1]-30+710-140-300; - int gz = gyro[2]+5+195-453+300; + int gx = gyro[0]+3656-3505-50-1850+2223-24; //変更必須 + int gy = gyro[1]-30+710-140-300+375-50; + int gz = gyro[2]+5+195-453+300-244-20-10; //printf("%d %d %d\r\n",gx,gy,gz); double gX = gx*0.02562299;//0.025622990.0128114995 @@ -94,7 +94,7 @@ Tgz = Tgz + abs(gZ2); - if(Tgx > 5){ + if(Tgx > 3){ if(GX > 0){ GX = GX - 0.3; }else if(GX < 0){ @@ -104,7 +104,7 @@ } Tgx=0; } - if(Tgy > 5){ + if(Tgy > 3){ if(GY > 0){ GY = GY - 0.3; }else if(GY < 0){ @@ -114,7 +114,7 @@ } Tgy=0; } - if(Tgz > 5){ + if(Tgz > 3){ if(GZ > 0){ GZ = GZ - 0.3; }else if(GZ < 0){ @@ -124,7 +124,7 @@ } Tgz=0; } - //printf("%.2f %.2f %.2f\r\n",GX,GY,GZ); + printf("%.2f %.2f %.2f\r\n",GX,GY,GZ); wait(dt); @@ -136,7 +136,8 @@ c1 = Kp*f1+Ki*f1*dt+Kd*gX1; //トルク c2 = Kp*f2+Ki*f2*dt+Kd*gY1; - printf("%.2f %.2f",c1,c2); + // + printf("%.2f\r\n",c1); //回転トルク→duty比 double d1,d2; @@ -149,14 +150,14 @@ if(d2 > 0.9){ d2 = 0.9; } - printf("%.2f",GX); - printf("%.2f %.2f\r\n",d1,d2); + //printf("%.2f",GX); + //printf("%.2f %.2f\r\n",d1,d2); //duty比からmotorを動かす。 if(GX > 0){ motor1 = abs(d1); - motor2 = 0.5-abs(d1)/2; + motor2 = (1-abs(d1))/2; }else if(c1 < 0){ motor2 = abs(d1); motor1 = 0.5-abs(d1)/2;