コウスケ スズキ / Mbed 2 deprecated PIDw

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?

UserRevisionLine numberNew 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 }