aaaaaaaa
Dependencies: mbed Servo MPU6050
main.cpp@4:fad5b5a598bf, 2022-02-25 (annotated)
- Committer:
- kosukesuzuki
- Date:
- Fri Feb 25 04:38:59 2022 +0000
- Revision:
- 4:fad5b5a598bf
- Parent:
- 3:05a780930d12
PID
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kosukesuzuki | 0:1ce8dacdff38 | 1 | //PID制御 |
kosukesuzuki | 0:1ce8dacdff38 | 2 | #include "mbed.h" |
kosukesuzuki | 0:1ce8dacdff38 | 3 | #include "MPU6050.h" |
kosukesuzuki | 3:05a780930d12 | 4 | #include "Servo.h" |
kosukesuzuki | 0:1ce8dacdff38 | 5 | |
kosukesuzuki | 1:036305ced8fd | 6 | const double dt = 0.01; //微小時間 |
kosukesuzuki | 1:036305ced8fd | 7 | |
kosukesuzuki | 3:05a780930d12 | 8 | const double Kp =5; //変更必須 |
kosukesuzuki | 3:05a780930d12 | 9 | const double Ki =3; |
kosukesuzuki | 3:05a780930d12 | 10 | const double Kd =3; |
kosukesuzuki | 0:1ce8dacdff38 | 11 | |
kosukesuzuki | 3:05a780930d12 | 12 | const double mc = 1995; //最大トルク |
kosukesuzuki | 1:036305ced8fd | 13 | |
kosukesuzuki | 1:036305ced8fd | 14 | const double T =0; //目標角度 |
kosukesuzuki | 0:1ce8dacdff38 | 15 | |
kosukesuzuki | 0:1ce8dacdff38 | 16 | MPU6050 mpu(p9,p10); |
kosukesuzuki | 0:1ce8dacdff38 | 17 | Serial pc(USBTX,USBRX); |
kosukesuzuki | 1:036305ced8fd | 18 | |
kosukesuzuki | 3:05a780930d12 | 19 | Servo motor1(p21); |
kosukesuzuki | 3:05a780930d12 | 20 | Servo motor2(p22); |
kosukesuzuki | 3:05a780930d12 | 21 | Servo motor3(p23); |
kosukesuzuki | 3:05a780930d12 | 22 | Servo motor4(p24); |
kosukesuzuki | 3:05a780930d12 | 23 | |
kosukesuzuki | 3:05a780930d12 | 24 | /* |
kosukesuzuki | 3:05a780930d12 | 25 | PwmOut led1(LED1); //led |
kosukesuzuki | 1:036305ced8fd | 26 | PwmOut led2(LED2); |
kosukesuzuki | 1:036305ced8fd | 27 | PwmOut led3(LED3); |
kosukesuzuki | 1:036305ced8fd | 28 | PwmOut led4(LED4); |
kosukesuzuki | 3:05a780930d12 | 29 | */ |
kosukesuzuki | 0:1ce8dacdff38 | 30 | |
kosukesuzuki | 0:1ce8dacdff38 | 31 | int gyro[3]; |
kosukesuzuki | 0:1ce8dacdff38 | 32 | |
kosukesuzuki | 1:036305ced8fd | 33 | double GX,GY,GZ; |
kosukesuzuki | 1:036305ced8fd | 34 | double Tgx,Tgy,Tgz; |
kosukesuzuki | 0:1ce8dacdff38 | 35 | |
kosukesuzuki | 0:1ce8dacdff38 | 36 | int main() { |
kosukesuzuki | 1:036305ced8fd | 37 | GX = GY = GZ= 0; |
kosukesuzuki | 1:036305ced8fd | 38 | Tgx = Tgy = Tgz = 0; |
kosukesuzuki | 4:fad5b5a598bf | 39 | pc.printf("start\r\n"); |
kosukesuzuki | 3:05a780930d12 | 40 | |
kosukesuzuki | 3:05a780930d12 | 41 | |
kosukesuzuki | 3:05a780930d12 | 42 | motor1 = motor2 = motor3 = motor4 = 0.0; |
kosukesuzuki | 3:05a780930d12 | 43 | wait(0.5); |
kosukesuzuki | 3:05a780930d12 | 44 | motor1 = motor2 = motor3 = motor4 = 1.0; |
kosukesuzuki | 3:05a780930d12 | 45 | wait(8); |
kosukesuzuki | 3:05a780930d12 | 46 | motor1 = motor2 = motor3 = motor4 = 0.0; |
kosukesuzuki | 3:05a780930d12 | 47 | wait(8); |
kosukesuzuki | 3:05a780930d12 | 48 | |
kosukesuzuki | 3:05a780930d12 | 49 | /* |
kosukesuzuki | 3:05a780930d12 | 50 | //準備 |
kosukesuzuki | 3:05a780930d12 | 51 | for (float p=0.0; p<=0.75; p += 0.25) { |
kosukesuzuki | 3:05a780930d12 | 52 | motor1 = motor2 = motor3 = motor4 = p; |
kosukesuzuki | 3:05a780930d12 | 53 | wait(0.5); |
kosukesuzuki | 3:05a780930d12 | 54 | } |
kosukesuzuki | 3:05a780930d12 | 55 | |
kosukesuzuki | 3:05a780930d12 | 56 | wait(1); |
kosukesuzuki | 3:05a780930d12 | 57 | |
kosukesuzuki | 3:05a780930d12 | 58 | motor1 = motor2 = motor3 = motor4 = 0; |
kosukesuzuki | 3:05a780930d12 | 59 | wait(1); |
kosukesuzuki | 3:05a780930d12 | 60 | |
kosukesuzuki | 3:05a780930d12 | 61 | motor1 = motor2 = motor3 = motor4 = 1; |
kosukesuzuki | 3:05a780930d12 | 62 | |
kosukesuzuki | 3:05a780930d12 | 63 | wait(1); |
kosukesuzuki | 3:05a780930d12 | 64 | */ |
kosukesuzuki | 3:05a780930d12 | 65 | |
kosukesuzuki | 3:05a780930d12 | 66 | //準備終了 |
kosukesuzuki | 3:05a780930d12 | 67 | |
kosukesuzuki | 3:05a780930d12 | 68 | |
kosukesuzuki | 1:036305ced8fd | 69 | |
kosukesuzuki | 0:1ce8dacdff38 | 70 | while(1){ |
kosukesuzuki | 0:1ce8dacdff38 | 71 | //角度求める |
kosukesuzuki | 0:1ce8dacdff38 | 72 | mpu.readGyroData(gyro); |
kosukesuzuki | 4:fad5b5a598bf | 73 | int gx = gyro[0]+3656-3505-50-1850+2223-24; //変更必須 |
kosukesuzuki | 4:fad5b5a598bf | 74 | int gy = gyro[1]-30+710-140-300+375-50; |
kosukesuzuki | 4:fad5b5a598bf | 75 | int gz = gyro[2]+5+195-453+300-244-20-10; |
kosukesuzuki | 3:05a780930d12 | 76 | //printf("%d %d %d\r\n",gx,gy,gz); |
kosukesuzuki | 0:1ce8dacdff38 | 77 | |
kosukesuzuki | 3:05a780930d12 | 78 | double gX = gx*0.02562299;//0.025622990.0128114995 |
kosukesuzuki | 1:036305ced8fd | 79 | double gY = gy*0.02562299; |
kosukesuzuki | 1:036305ced8fd | 80 | double gZ = gz*0.02562299; |
kosukesuzuki | 0:1ce8dacdff38 | 81 | int gX1 = gX; |
kosukesuzuki | 1:036305ced8fd | 82 | int gY1 = gY; |
kosukesuzuki | 1:036305ced8fd | 83 | int gZ1 = gZ; |
kosukesuzuki | 1:036305ced8fd | 84 | double gX2 = gX1*dt; |
kosukesuzuki | 1:036305ced8fd | 85 | double gY2 = gY1*dt; |
kosukesuzuki | 1:036305ced8fd | 86 | double gZ2 = gZ1*dt; |
kosukesuzuki | 0:1ce8dacdff38 | 87 | |
kosukesuzuki | 0:1ce8dacdff38 | 88 | GX = GX + gX2; |
kosukesuzuki | 1:036305ced8fd | 89 | GY = GY + gY2; |
kosukesuzuki | 1:036305ced8fd | 90 | GZ = GZ + gZ2; |
kosukesuzuki | 0:1ce8dacdff38 | 91 | |
kosukesuzuki | 0:1ce8dacdff38 | 92 | Tgx = Tgx + abs(gX2); |
kosukesuzuki | 1:036305ced8fd | 93 | Tgy = Tgy + abs(gY2); |
kosukesuzuki | 1:036305ced8fd | 94 | Tgz = Tgz + abs(gZ2); |
kosukesuzuki | 1:036305ced8fd | 95 | |
kosukesuzuki | 0:1ce8dacdff38 | 96 | |
kosukesuzuki | 4:fad5b5a598bf | 97 | if(Tgx > 3){ |
kosukesuzuki | 0:1ce8dacdff38 | 98 | if(GX > 0){ |
kosukesuzuki | 0:1ce8dacdff38 | 99 | GX = GX - 0.3; |
kosukesuzuki | 3:05a780930d12 | 100 | }else if(GX < 0){ |
kosukesuzuki | 0:1ce8dacdff38 | 101 | GX = GX + 0.3; |
kosukesuzuki | 3:05a780930d12 | 102 | }else{ |
kosukesuzuki | 3:05a780930d12 | 103 | GX =GX; |
kosukesuzuki | 3:05a780930d12 | 104 | } |
kosukesuzuki | 0:1ce8dacdff38 | 105 | Tgx=0; |
kosukesuzuki | 1:036305ced8fd | 106 | } |
kosukesuzuki | 4:fad5b5a598bf | 107 | if(Tgy > 3){ |
kosukesuzuki | 1:036305ced8fd | 108 | if(GY > 0){ |
kosukesuzuki | 1:036305ced8fd | 109 | GY = GY - 0.3; |
kosukesuzuki | 3:05a780930d12 | 110 | }else if(GY < 0){ |
kosukesuzuki | 1:036305ced8fd | 111 | GY = GY + 0.3; |
kosukesuzuki | 3:05a780930d12 | 112 | }else{ |
kosukesuzuki | 3:05a780930d12 | 113 | GY = GY; |
kosukesuzuki | 3:05a780930d12 | 114 | } |
kosukesuzuki | 1:036305ced8fd | 115 | Tgy=0; |
kosukesuzuki | 1:036305ced8fd | 116 | } |
kosukesuzuki | 4:fad5b5a598bf | 117 | if(Tgz > 3){ |
kosukesuzuki | 1:036305ced8fd | 118 | if(GZ > 0){ |
kosukesuzuki | 1:036305ced8fd | 119 | GZ = GZ - 0.3; |
kosukesuzuki | 3:05a780930d12 | 120 | }else if(GZ < 0){ |
kosukesuzuki | 1:036305ced8fd | 121 | GZ = GZ + 0.3; |
kosukesuzuki | 3:05a780930d12 | 122 | }else{ |
kosukesuzuki | 3:05a780930d12 | 123 | GZ = GZ; |
kosukesuzuki | 3:05a780930d12 | 124 | } |
kosukesuzuki | 1:036305ced8fd | 125 | Tgz=0; |
kosukesuzuki | 0:1ce8dacdff38 | 126 | } |
kosukesuzuki | 4:fad5b5a598bf | 127 | printf("%.2f %.2f %.2f\r\n",GX,GY,GZ); |
kosukesuzuki | 0:1ce8dacdff38 | 128 | |
kosukesuzuki | 3:05a780930d12 | 129 | wait(dt); |
kosukesuzuki | 1:036305ced8fd | 130 | |
kosukesuzuki | 0:1ce8dacdff38 | 131 | //PID() |
kosukesuzuki | 1:036305ced8fd | 132 | double c1,c2; |
kosukesuzuki | 1:036305ced8fd | 133 | double f1,f2; |
kosukesuzuki | 0:1ce8dacdff38 | 134 | f1 = T - GX; |
kosukesuzuki | 1:036305ced8fd | 135 | f2 = T - GY; |
kosukesuzuki | 1:036305ced8fd | 136 | |
kosukesuzuki | 1:036305ced8fd | 137 | c1 = Kp*f1+Ki*f1*dt+Kd*gX1; //トルク |
kosukesuzuki | 3:05a780930d12 | 138 | c2 = Kp*f2+Ki*f2*dt+Kd*gY1; |
kosukesuzuki | 4:fad5b5a598bf | 139 | // |
kosukesuzuki | 4:fad5b5a598bf | 140 | printf("%.2f\r\n",c1); |
kosukesuzuki | 0:1ce8dacdff38 | 141 | |
kosukesuzuki | 0:1ce8dacdff38 | 142 | //回転トルク→duty比 |
kosukesuzuki | 1:036305ced8fd | 143 | double d1,d2; |
kosukesuzuki | 1:036305ced8fd | 144 | d1 = c1/mc; |
kosukesuzuki | 1:036305ced8fd | 145 | d2 = c2/mc; |
kosukesuzuki | 0:1ce8dacdff38 | 146 | |
kosukesuzuki | 3:05a780930d12 | 147 | if(d1 > 0.9){ |
kosukesuzuki | 3:05a780930d12 | 148 | d1 = 0.9; |
kosukesuzuki | 3:05a780930d12 | 149 | } |
kosukesuzuki | 3:05a780930d12 | 150 | if(d2 > 0.9){ |
kosukesuzuki | 3:05a780930d12 | 151 | d2 = 0.9; |
kosukesuzuki | 0:1ce8dacdff38 | 152 | } |
kosukesuzuki | 4:fad5b5a598bf | 153 | //printf("%.2f",GX); |
kosukesuzuki | 4:fad5b5a598bf | 154 | //printf("%.2f %.2f\r\n",d1,d2); |
kosukesuzuki | 3:05a780930d12 | 155 | |
kosukesuzuki | 3:05a780930d12 | 156 | |
kosukesuzuki | 3:05a780930d12 | 157 | //duty比からmotorを動かす。 |
kosukesuzuki | 1:036305ced8fd | 158 | if(GX > 0){ |
kosukesuzuki | 3:05a780930d12 | 159 | motor1 = abs(d1); |
kosukesuzuki | 4:fad5b5a598bf | 160 | motor2 = (1-abs(d1))/2; |
kosukesuzuki | 3:05a780930d12 | 161 | }else if(c1 < 0){ |
kosukesuzuki | 3:05a780930d12 | 162 | motor2 = abs(d1); |
kosukesuzuki | 3:05a780930d12 | 163 | motor1 = 0.5-abs(d1)/2; |
kosukesuzuki | 3:05a780930d12 | 164 | }else{ |
kosukesuzuki | 3:05a780930d12 | 165 | motor1 = motor2 = 0; |
kosukesuzuki | 3:05a780930d12 | 166 | } |
kosukesuzuki | 3:05a780930d12 | 167 | |
kosukesuzuki | 3:05a780930d12 | 168 | if(GX > 0){ |
kosukesuzuki | 3:05a780930d12 | 169 | motor3 = abs(d2); |
kosukesuzuki | 3:05a780930d12 | 170 | motor4 = 1-abs(d2); |
kosukesuzuki | 3:05a780930d12 | 171 | }else if(c2 < 0){ |
kosukesuzuki | 3:05a780930d12 | 172 | motor4 = abs(d2); |
kosukesuzuki | 3:05a780930d12 | 173 | motor3 = 1-abs(d2); |
kosukesuzuki | 3:05a780930d12 | 174 | }else{ |
kosukesuzuki | 3:05a780930d12 | 175 | motor3 = motor4 = 0; |
kosukesuzuki | 3:05a780930d12 | 176 | } |
kosukesuzuki | 0:1ce8dacdff38 | 177 | } |
kosukesuzuki | 0:1ce8dacdff38 | 178 | } |