Diff: main.cpp

Revision:

0:079a31fc71bb

--- /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