Diff: main.cpp

Revision:

0:6674c0c8779b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 01 05:53:53 2021 +0000
@@ -0,0 +1,109 @@
+#include "mbed.h"
+#include "QEI.h"
+#define PI 3.14159265359
+
+Ticker warikomi;
+Serial pc(USBTX, USBRX);
+Timer t;
+QEI motor (D10,D5,NC,/*分解能*/,&t,QEI::X4_ENCODING);
+
+//DigitalIn A(D10);
+//DigitalIn B(D5);
+PwmOut motor_r(D9);
+DigitalOut direct(D2);
+float angle,vel, angle_back = 0,goal = 2 * PI,k_p = 0.8,erorr,duty = 0; //P制御
+float k_d = 0.05,deriva,erorr_back = 0;  //D制御
+float k_i = 0.0, integ; //I制御
+
+float erorr_vel ,erorr_vel_back = 0 ,deriva_vel ,integ_vel ,goal_vel = 10.0 ,duty_vel = 0;
+float k_p_vel = 0 ,k_d_vel = 0 ,k_i_vel = 0; 
+
+
+
+int count ;
+void func()
+{
+
+
+    count = motor.getPulses();
+    angle = (float)count * PI / 720.0;
+    vel = (angle - angle_back) / 0.01;
+    erorr = goal - angle;
+    deriva = (erorr - erorr_back) / 0.01;
+    integ += (erorr + erorr_back) * (1/2) * 0.01;
+    angle_back = angle;
+    
+    erorr_vel = goal_vel - vel;
+    deriva_vel = (erorr_vel - erorr_vel_back) / 0.01;
+    integ_vel += (erorr_vel + erorr_vel_back) / (1/2) * 0.01;
+    
+
+    /*erorr_An = erorr;
+    integ_An = integ ;
+    deriva_An = deriva ;*/
+    duty = erorr * k_p + k_d * deriva + k_i * integ;
+    duty_vel =+ erorr_vel * k_p_vel + deriva_vel * k_d_vel + integ_vel * k_i_vel;
+    erorr_back = erorr;
+    erorr_vel_back = erorr_vel;
+}
+
+int main()
+{
+    //pc.printf("check");
+    wait_ms(2000);
+    warikomi.attach(&func,0.01);
+    // int count = 0;
+    int back_A,back_B,Ach,Bch;
+    while(1) {
+
+        if (duty >= 0) {
+            direct = 0;
+            motor_r = duty;
+            motor_r = duty_vel;
+        } else if (duty < 0) {
+            direct = 1;
+            motor_r =-1 * duty ;
+            motor_r =-1 * duty_vel;
+            // duty = duty * 0.1;
+
+        }
+        /*while(1){
+
+            if (angle > 2 * PI ) {
+                direct = 1;
+                motor_r = duty;
+            }
+        if (angle <= 2 *PI) {
+            break;
+        }
+        }*/
+
+
+
+
+        /* if (duty > 0.5) {
+             duty = 0;
+             }*/
+
+        /* if (angle == 2 * PI) {
+             duty = 0;
+             }*/
+
+        /*Ach = A.read();
+        Bch = B.read();*/
+        pc.printf("%d %f  現在の角度は%frad  角速度は%frad/sec  現在のデューティ比は%f  erorr:%f\r\n",
+                  /*Ach ,Bch ,back_A ,back_B ,*/count,(float)count / 1440,angle,vel,duty,erorr);
+        /*if (Bch == back_A ) {
+            count++;
+        }
+        if (Ach == back_B) {
+            count--;
+        }
+        
+        
+        back_A = Ach;
+        back_B = Bch;*/
+        
+        wait_ms(1);
+    }
+}