Diff: main.cpp

Revision:

1:0e8ec231cb2f

Parent:

0:6db935b161f8

Child:

2:c4e456559941

--- a/main.cpp	Wed Oct 09 11:19:20 2019 +0000
+++ b/main.cpp	Fri Nov 08 05:23:55 2019 +0000
@@ -9,46 +9,57 @@
 
 Ec4multi EC_1(p15,p16,RESOLUTION);
 Ec4multi EC_2(p17,p18,RESOLUTION);
-Ec4multi EC_3(p19,p20,RESOLUTION);
 
 SpeedControl motor_1(p21,p22,50,EC_1);
 SpeedControl motor_2(p23,p24,50,EC_2);
-SpeedControl motor_3(p25,p26,50,EC_3);
 
 Ticker motor_tick;  //角速度計算用ticker
 Serial pc(USBTX,USBRX);
 
+Timer timer;
+
+
+int j=0;
+double rotate_1=0,rotate_2=0;
+double motor1_omega[1000]={},motor2_omega[1000]={},time_when[1000];
+int motor1_count[1000]={},motor2_count[1000]={};
+
+void CalOmega()
+{
+    motor1_count[j]=EC_1.getCount();
+    motor1_omega[j]=EC_1.getOmega();
+    motor2_count[j]=EC_2.getCount();
+    motor2_omega[j]=EC_2.getOmega();
+//    time_when[j]=EC_1.timer_.read();
+    if(rotate_1==0) motor_1.stop();
+    else motor_1.Sc(rotate_1);
+    if(rotate_2==0) motor_2.stop();
+    else motor_2.Sc(rotate_2);
+    j++;
+}
 
 int main()
 {
     motor_1.period_us(50);
     motor_2.period_us(50);
-    motor_3.period_us(50);
-
     motor_1.setEquation(0.322,0.08,0,0.0100);    //求めたC,Dの値を設定
     motor_2.setEquation(0.0302,0.0755,-0.0241,0.1301);    //求めたC,Dの値を設定
-    motor_3.setEquation(0.0302,0.1500,-0.0301,0.2000);    //求めたC,Dの値を設定
-
     motor_1.setPDparam(0,0.0);  //PIDの係数を設定
     motor_2.setPDparam(0,0.0);  //PIDの係数を設定
-    motor_3.setPDparam(0,0.0);  //PIDの係数を設定
-
-    int kai=0;
-    double rotate_1=0,rotate_2=0,rotate_3=0;
+    motor_tick.attach(&CalOmega,0.05);
 
     while(1) {
-        if(rotate_1==0) motor_1.stop();
-        else motor_1.Sc(rotate_1);
-        if(rotate_2==0) motor_2.stop();
-        else motor_2.Sc(rotate_2);
-        if(rotate_3==0) motor_3.stop();
-        else motor_3.Sc(rotate_3);
+
+        if(pc.readable()) {
+            char sel=pc.getc();
+            if(sel=='q') {
+                for(int i=0; i<1000; i++) {
 
-        if(kai>=4) {
-            //printf("\r\n");
-            printf("target:%.2f,%.2f.%.2f  ",rotate_1,rotate_2,rotate_3);
-            printf("omega_=%.2f,%.2f,%.2f \r\n",EC_1.getOmega(),EC_2.getOmega(),EC_3.getOmega());
-            kai=0;
+//                    pc.printf("%f    ",time_when[i]);
+                    pc.printf("%f,%f    ",motor1_omega[i],motor2_omega[i]);
+                    pc.printf("%d,%d\r\n",motor1_count[i],motor2_count[i]);
+                }
+            }
         }
     }
 }
\ No newline at end of file