Mura kazu / Mbed 2 deprecated 0_PID_renew

aaa

Dependencies:   BNO055 SBDBT_lib___muratani mbed

main.cpp@0:079a31fc71bb, 17 months ago (annotated)

Committer:
e2137
Date:
Wed Dec 14 07:50:11 2022 +0000
Revision:
0:079a31fc71bb
muratani;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
e2137 0:079a31fc71bb 1 #include <mbed.h>
e2137 0:079a31fc71bb 2 #include <DCmotor.hpp>
e2137 0:079a31fc71bb 3 #include <rotary_inc.hpp>
e2137 0:079a31fc71bb 4
e2137 0:079a31fc71bb 5 DCmotor MOTOR(PA_0,PA_1, 0.5,0.0, 1);
e2137 0:079a31fc71bb 6 RotaryInc Rotary(PC_3, PC_2, 256, 1, 4);
e2137 0:079a31fc71bb 7 Timer TIME;
e2137 0:079a31fc71bb 8
e2137 0:079a31fc71bb 9 int main()
e2137 0:079a31fc71bb 10 {
e2137 0:079a31fc71bb 11 TIME.start();
e2137 0:079a31fc71bb 12 double e1 = 0;
e2137 0:079a31fc71bb 13 double e2 = 0;
e2137 0:079a31fc71bb 14 double e3 = 0;
e2137 0:079a31fc71bb 15 double Kp = 0.00040;//0.00000264;
e2137 0:079a31fc71bb 16 double Ki = 0.000021;
e2137 0:079a31fc71bb 17 double Kd = 0.000003;
e2137 0:079a31fc71bb 18 double target = 360;
e2137 0:079a31fc71bb 19 double now = 0;
e2137 0:079a31fc71bb 20 double now_ang_v = 0.0;
e2137 0:079a31fc71bb 21 double P = 0;
e2137 0:079a31fc71bb 22 double I = 0;
e2137 0:079a31fc71bb 23 double D = 0;
e2137 0:079a31fc71bb 24 double dt = 0.01;
e2137 0:079a31fc71bb 25 double time = 0;
e2137 0:079a31fc71bb 26 double pwm = 0.0;
e2137 0:079a31fc71bb 27 double last = 0.0;
e2137 0:079a31fc71bb 28 while(1)
e2137 0:079a31fc71bb 29 {
e2137 0:079a31fc71bb 30 MOTOR.drive(pwm);
e2137 0:079a31fc71bb 31 last = now;
e2137 0:079a31fc71bb 32 now = Rotary.degree_now();//角度
e2137 0:079a31fc71bb 33 //now_ang_v = (now - last) / dt;
e2137 0:079a31fc71bb 34 //now_ang_v = (now - last);
e2137 0:079a31fc71bb 35
e2137 0:079a31fc71bb 36 e3 = e2;
e2137 0:079a31fc71bb 37 e2 = e1;
e2137 0:079a31fc71bb 38 e1 = target - now;
e2137 0:079a31fc71bb 39 P = Kp * (e1 - e2);
e2137 0:079a31fc71bb 40 I = Ki * e1 * dt;
e2137 0:079a31fc71bb 41 D = Kd * (e1 - 2 * e2 + e3) / dt;
e2137 0:079a31fc71bb 42 pwm += P + I + D;
e2137 0:079a31fc71bb 43 printf("%lf,%lf\n",now,target);
e2137 0:079a31fc71bb 44 //printf("ang_vel %lf\tpwm %lf\te1 %lf\te2 %lf\te3 %lf\n",now,pwm, e1, e2, e3);
e2137 0:079a31fc71bb 45 while(TIME.read_ms() - time <= 10);
e2137 0:079a31fc71bb 46 time = TIME.read_ms();
e2137 0:079a31fc71bb 47 /*if(target * 0.99 <= now_ang_v && now_ang_v <= target * 1.01){
e2137 0:079a31fc71bb 48 printf("Finish\n");
e2137 0:079a31fc71bb 49 MOTOR.drive(0);
e2137 0:079a31fc71bb 50 break;
e2137 0:079a31fc71bb 51 }*/
e2137 0:079a31fc71bb 52 }
e2137 0:079a31fc71bb 53 }
e2137 0:079a31fc71bb 54 //sum = Rotary.degree_diff();
e2137 0:079a31fc71bb 55 //printf("P : %lf I : %lf D : %lf pwm : %d\n",P, I, D, pwm);
e2137 0:079a31fc71bb 56 //MOTOR.out(pwm);
e2137 0:079a31fc71bb 57 //e2 = e1;
e2137 0:079a31fc71bb 58 //e1 = now = Rotary.degree_now();
e2137 0:079a31fc71bb 59 //e3 = (e1 - e2) / dt;
e2137 0:079a31fc71bb 60 //now = Rotary.getSpeed();
e2137 0:079a31fc71bb 61 //printf("%lf %lf\n",pwm,now);
e2137 0:079a31fc71bb 62 //MOTOR.drive(pwm += 0.001);
e2137 0:079a31fc71bb 63 //pwm += P + I;