![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
aaa
Dependencies: BNO055 SBDBT_lib___muratani mbed
Diff: main.cpp
- Revision:
- 0:079a31fc71bb
diff -r 000000000000 -r 079a31fc71bb main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Dec 14 07:50:11 2022 +0000 @@ -0,0 +1,63 @@ +#include <mbed.h> +#include <DCmotor.hpp> +#include <rotary_inc.hpp> + +DCmotor MOTOR(PA_0,PA_1, 0.5,0.0, 1); +RotaryInc Rotary(PC_3, PC_2, 256, 1, 4); +Timer TIME; + +int main() +{ + TIME.start(); + double e1 = 0; + double e2 = 0; + double e3 = 0; + double Kp = 0.00040;//0.00000264; + double Ki = 0.000021; + double Kd = 0.000003; + double target = 360; + double now = 0; + double now_ang_v = 0.0; + double P = 0; + double I = 0; + double D = 0; + double dt = 0.01; + double time = 0; + double pwm = 0.0; + double last = 0.0; + while(1) + { + MOTOR.drive(pwm); + last = now; + now = Rotary.degree_now();//角度 + //now_ang_v = (now - last) / dt; + //now_ang_v = (now - last); + + e3 = e2; + e2 = e1; + e1 = target - now; + P = Kp * (e1 - e2); + I = Ki * e1 * dt; + D = Kd * (e1 - 2 * e2 + e3) / dt; + pwm += P + I + D; + printf("%lf,%lf\n",now,target); + //printf("ang_vel %lf\tpwm %lf\te1 %lf\te2 %lf\te3 %lf\n",now,pwm, e1, e2, e3); + while(TIME.read_ms() - time <= 10); + time = TIME.read_ms(); + /*if(target * 0.99 <= now_ang_v && now_ang_v <= target * 1.01){ + printf("Finish\n"); + MOTOR.drive(0); + break; + }*/ + } +} + //sum = Rotary.degree_diff(); + //printf("P : %lf I : %lf D : %lf pwm : %d\n",P, I, D, pwm); + //MOTOR.out(pwm); + //e2 = e1; + //e1 = now = Rotary.degree_now(); + //e3 = (e1 - e2) / dt; + //now = Rotary.getSpeed(); + //printf("%lf %lf\n",pwm,now); + //MOTOR.drive(pwm += 0.001); + //pwm += P + I; \ No newline at end of file