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

aaaaaaaa

Dependencies:   mbed Servo MPU6050

main.cpp@2:c329b0c4ec92, 2022-02-23 (annotated)

Committer:
kosukesuzuki
Date:
Wed Feb 23 12:53:39 2022 +0000
Revision:
2:c329b0c4ec92
Parent:
1:036305ced8fd
Child:
3:05a780930d12
a

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 0:1ce8dacdff38 4
kosukesuzuki 1:036305ced8fd 5 const double dt = 0.01; //微小時間
kosukesuzuki 1:036305ced8fd 6
kosukesuzuki 1:036305ced8fd 7 const double Kp =0.3; //変更必須
kosukesuzuki 0:1ce8dacdff38 8 const double Ki =0.4;
kosukesuzuki 0:1ce8dacdff38 9 const double Kd =0.5;
kosukesuzuki 0:1ce8dacdff38 10
kosukesuzuki 1:036305ced8fd 11 const double mc = 77; //最大トルク
kosukesuzuki 1:036305ced8fd 12
kosukesuzuki 1:036305ced8fd 13 const double T =0; //目標角度
kosukesuzuki 0:1ce8dacdff38 14
kosukesuzuki 0:1ce8dacdff38 15 MPU6050 mpu(p9,p10);
kosukesuzuki 0:1ce8dacdff38 16 Serial pc(USBTX,USBRX);
kosukesuzuki 1:036305ced8fd 17
kosukesuzuki 1:036305ced8fd 18 PwmOut led1(LED1); //motor
kosukesuzuki 1:036305ced8fd 19 PwmOut led2(LED2);
kosukesuzuki 1:036305ced8fd 20 PwmOut led3(LED3);
kosukesuzuki 1:036305ced8fd 21 PwmOut led4(LED4);
kosukesuzuki 0:1ce8dacdff38 22
kosukesuzuki 0:1ce8dacdff38 23 int gyro[3];
kosukesuzuki 0:1ce8dacdff38 24
kosukesuzuki 1:036305ced8fd 25 double GX,GY,GZ;
kosukesuzuki 1:036305ced8fd 26 double Tgx,Tgy,Tgz;
kosukesuzuki 0:1ce8dacdff38 27
kosukesuzuki 0:1ce8dacdff38 28 int main() {
kosukesuzuki 1:036305ced8fd 29 GX = GY = GZ= 0;
kosukesuzuki 1:036305ced8fd 30 Tgx = Tgy = Tgz = 0;
kosukesuzuki 1:036305ced8fd 31
kosukesuzuki 0:1ce8dacdff38 32 while(1){
kosukesuzuki 0:1ce8dacdff38 33 //角度求める
kosukesuzuki 0:1ce8dacdff38 34 mpu.readGyroData(gyro);
kosukesuzuki 1:036305ced8fd 35 int gx = gyro[0]+3656-3505-50; //変更必須
kosukesuzuki 1:036305ced8fd 36 int gy = gyro[1]-30;
kosukesuzuki 1:036305ced8fd 37 int gz = gyro[2]+5;
kosukesuzuki 1:036305ced8fd 38 printf("%d %d %d\r\n",gx,gy,gz);
kosukesuzuki 0:1ce8dacdff38 39
kosukesuzuki 0:1ce8dacdff38 40 double gX = gx*0.02562299;
kosukesuzuki 1:036305ced8fd 41 double gY = gy*0.02562299;
kosukesuzuki 1:036305ced8fd 42 double gZ = gz*0.02562299;
kosukesuzuki 0:1ce8dacdff38 43 int gX1 = gX;
kosukesuzuki 1:036305ced8fd 44 int gY1 = gY;
kosukesuzuki 1:036305ced8fd 45 int gZ1 = gZ;
kosukesuzuki 1:036305ced8fd 46 double gX2 = gX1*dt;
kosukesuzuki 1:036305ced8fd 47 double gY2 = gY1*dt;
kosukesuzuki 1:036305ced8fd 48 double gZ2 = gZ1*dt;
kosukesuzuki 0:1ce8dacdff38 49
kosukesuzuki 0:1ce8dacdff38 50 GX = GX + gX2;
kosukesuzuki 1:036305ced8fd 51 GY = GY + gY2;
kosukesuzuki 1:036305ced8fd 52 GZ = GZ + gZ2;
kosukesuzuki 0:1ce8dacdff38 53
kosukesuzuki 0:1ce8dacdff38 54 Tgx = Tgx + abs(gX2);
kosukesuzuki 1:036305ced8fd 55 Tgy = Tgy + abs(gY2);
kosukesuzuki 1:036305ced8fd 56 Tgz = Tgz + abs(gZ2);
kosukesuzuki 1:036305ced8fd 57
kosukesuzuki 0:1ce8dacdff38 58
kosukesuzuki 0:1ce8dacdff38 59 if(Tgx > 5){
kosukesuzuki 0:1ce8dacdff38 60 if(GX > 0){
kosukesuzuki 0:1ce8dacdff38 61 GX = GX - 0.3;
kosukesuzuki 0:1ce8dacdff38 62 }else{
kosukesuzuki 0:1ce8dacdff38 63 GX = GX + 0.3;
kosukesuzuki 0:1ce8dacdff38 64 }
kosukesuzuki 0:1ce8dacdff38 65 Tgx=0;
kosukesuzuki 1:036305ced8fd 66 }
kosukesuzuki 1:036305ced8fd 67 if(Tgy > 5){
kosukesuzuki 1:036305ced8fd 68 if(GY > 0){
kosukesuzuki 1:036305ced8fd 69 GY = GY - 0.3;
kosukesuzuki 1:036305ced8fd 70 }else{
kosukesuzuki 1:036305ced8fd 71 GY = GY + 0.3;
kosukesuzuki 1:036305ced8fd 72 }
kosukesuzuki 1:036305ced8fd 73 Tgy=0;
kosukesuzuki 1:036305ced8fd 74 }
kosukesuzuki 1:036305ced8fd 75 if(Tgz > 5){
kosukesuzuki 1:036305ced8fd 76 if(GZ > 0){
kosukesuzuki 1:036305ced8fd 77 GZ = GZ - 0.3;
kosukesuzuki 1:036305ced8fd 78 }else{
kosukesuzuki 1:036305ced8fd 79 GZ = GZ + 0.3;
kosukesuzuki 1:036305ced8fd 80 }
kosukesuzuki 1:036305ced8fd 81 Tgz=0;
kosukesuzuki 0:1ce8dacdff38 82 }
kosukesuzuki 1:036305ced8fd 83 //printf("%.2f %.2f %.2f\r\n",GX,GY,GZ);
kosukesuzuki 0:1ce8dacdff38 84
kosukesuzuki 0:1ce8dacdff38 85 wait(0.01);
kosukesuzuki 1:036305ced8fd 86
kosukesuzuki 0:1ce8dacdff38 87 //PID()
kosukesuzuki 1:036305ced8fd 88 double c1,c2;
kosukesuzuki 1:036305ced8fd 89 double f1,f2;
kosukesuzuki 0:1ce8dacdff38 90 f1 = T - GX;
kosukesuzuki 1:036305ced8fd 91 f2 = T - GY;
kosukesuzuki 1:036305ced8fd 92
kosukesuzuki 1:036305ced8fd 93 c1 = Kp*f1+Ki*f1*dt+Kd*gX1; //トルク
kosukesuzuki 1:036305ced8fd 94 c2 = Kp+f2+Ki*f2*dt+Kd*gY1;
kosukesuzuki 1:036305ced8fd 95 //printf("%.2f %.2f\r\n",c1,c2);
kosukesuzuki 0:1ce8dacdff38 96
kosukesuzuki 0:1ce8dacdff38 97 //回転トルク→duty比
kosukesuzuki 1:036305ced8fd 98 double d1,d2;
kosukesuzuki 1:036305ced8fd 99 d1 = c1/mc;
kosukesuzuki 1:036305ced8fd 100 d2 = c2/mc;
kosukesuzuki 0:1ce8dacdff38 101
kosukesuzuki 0:1ce8dacdff38 102 if(d1 > 1){
kosukesuzuki 0:1ce8dacdff38 103 d1 = 1;
kosukesuzuki 0:1ce8dacdff38 104 }
kosukesuzuki 1:036305ced8fd 105 if(d2 > 1){
kosukesuzuki 1:036305ced8fd 106 d2 = 1;
kosukesuzuki 1:036305ced8fd 107 }
kosukesuzuki 1:036305ced8fd 108
kosukesuzuki 0:1ce8dacdff38 109 //duty比からledを動かす。
kosukesuzuki 1:036305ced8fd 110 if(GX > 0){
kosukesuzuki 1:036305ced8fd 111 led1 = abs(d1);
kosukesuzuki 2:c329b0c4ec92 112 led2 = -abs(d1);
kosukesuzuki 1:036305ced8fd 113 }else{
kosukesuzuki 1:036305ced8fd 114 led2 = abs(d1);
kosukesuzuki 2:c329b0c4ec92 115 led1 = -abs(d1);
kosukesuzuki 1:036305ced8fd 116 }
kosukesuzuki 1:036305ced8fd 117 if(GY > 0){
kosukesuzuki 1:036305ced8fd 118 led3 = abs(d2);
kosukesuzuki 2:c329b0c4ec92 119 led4 = -abs(d2);
kosukesuzuki 1:036305ced8fd 120 }else{
kosukesuzuki 1:036305ced8fd 121 led4 = abs(d2);
kosukesuzuki 2:c329b0c4ec92 122 led3 = -abs(d2);
kosukesuzuki 1:036305ced8fd 123 }
kosukesuzuki 0:1ce8dacdff38 124 }
kosukesuzuki 0:1ce8dacdff38 125 }