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

aaaaaaaa

Dependencies:   mbed Servo MPU6050

main.cpp@0:1ce8dacdff38, 2022-02-23 (annotated)

Committer:
kosukesuzuki
Date:
Wed Feb 23 06:16:49 2022 +0000
Revision:
0:1ce8dacdff38
Child:
1:036305ced8fd
PIDledcontrol

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 0:1ce8dacdff38 5 const double Kp =0.3;
kosukesuzuki 0:1ce8dacdff38 6 const double Ki =0.4;
kosukesuzuki 0:1ce8dacdff38 7 const double Kd =0.5;
kosukesuzuki 0:1ce8dacdff38 8
kosukesuzuki 0:1ce8dacdff38 9 const double T =0; //目標
kosukesuzuki 0:1ce8dacdff38 10
kosukesuzuki 0:1ce8dacdff38 11 MPU6050 mpu(p9,p10);
kosukesuzuki 0:1ce8dacdff38 12 Serial pc(USBTX,USBRX);
kosukesuzuki 0:1ce8dacdff38 13 PwmOut led(LED1);
kosukesuzuki 0:1ce8dacdff38 14
kosukesuzuki 0:1ce8dacdff38 15 int gyro[3];
kosukesuzuki 0:1ce8dacdff38 16
kosukesuzuki 0:1ce8dacdff38 17 double GX;
kosukesuzuki 0:1ce8dacdff38 18 double Tgx;
kosukesuzuki 0:1ce8dacdff38 19
kosukesuzuki 0:1ce8dacdff38 20 int main() {
kosukesuzuki 0:1ce8dacdff38 21 GX = 0;
kosukesuzuki 0:1ce8dacdff38 22 while(1){
kosukesuzuki 0:1ce8dacdff38 23 //角度求める
kosukesuzuki 0:1ce8dacdff38 24 mpu.readGyroData(gyro);
kosukesuzuki 0:1ce8dacdff38 25 int gx = gyro[0]+3656-3505;
kosukesuzuki 0:1ce8dacdff38 26 //printf("%d\r\n",gx);
kosukesuzuki 0:1ce8dacdff38 27
kosukesuzuki 0:1ce8dacdff38 28 double gX = gx*0.02562299;
kosukesuzuki 0:1ce8dacdff38 29 int gX1 = gX;
kosukesuzuki 0:1ce8dacdff38 30 double gX2 = gX1*0.01;
kosukesuzuki 0:1ce8dacdff38 31
kosukesuzuki 0:1ce8dacdff38 32 GX = GX + gX2;
kosukesuzuki 0:1ce8dacdff38 33
kosukesuzuki 0:1ce8dacdff38 34 Tgx = Tgx + abs(gX2);
kosukesuzuki 0:1ce8dacdff38 35
kosukesuzuki 0:1ce8dacdff38 36 if(Tgx > 5){
kosukesuzuki 0:1ce8dacdff38 37 if(GX > 0){
kosukesuzuki 0:1ce8dacdff38 38 GX = GX - 0.3;
kosukesuzuki 0:1ce8dacdff38 39 }else{
kosukesuzuki 0:1ce8dacdff38 40 GX = GX + 0.3;
kosukesuzuki 0:1ce8dacdff38 41 }
kosukesuzuki 0:1ce8dacdff38 42 Tgx=0;
kosukesuzuki 0:1ce8dacdff38 43 }
kosukesuzuki 0:1ce8dacdff38 44 //printf("GX %.2f\r\n",GX);
kosukesuzuki 0:1ce8dacdff38 45
kosukesuzuki 0:1ce8dacdff38 46 wait(0.01);
kosukesuzuki 0:1ce8dacdff38 47 //PID()
kosukesuzuki 0:1ce8dacdff38 48 double c;
kosukesuzuki 0:1ce8dacdff38 49 double f1;
kosukesuzuki 0:1ce8dacdff38 50 f1 = T - GX;
kosukesuzuki 0:1ce8dacdff38 51 c = Kp*f1+Ki*f1*0.01+Kd*gX1;
kosukesuzuki 0:1ce8dacdff38 52 //printf("c %.2f\r\n",c);
kosukesuzuki 0:1ce8dacdff38 53
kosukesuzuki 0:1ce8dacdff38 54 //回転トルク→duty比
kosukesuzuki 0:1ce8dacdff38 55
kosukesuzuki 0:1ce8dacdff38 56 double d1;
kosukesuzuki 0:1ce8dacdff38 57 d1 = abs(c)/77;
kosukesuzuki 0:1ce8dacdff38 58
kosukesuzuki 0:1ce8dacdff38 59 if(d1 > 1){
kosukesuzuki 0:1ce8dacdff38 60 d1 = 1;
kosukesuzuki 0:1ce8dacdff38 61 }
kosukesuzuki 0:1ce8dacdff38 62
kosukesuzuki 0:1ce8dacdff38 63 //duty比からledを動かす。
kosukesuzuki 0:1ce8dacdff38 64
kosukesuzuki 0:1ce8dacdff38 65 led = d1;
kosukesuzuki 0:1ce8dacdff38 66 }
kosukesuzuki 0:1ce8dacdff38 67 }