Diff: main.cpp

Revision:

4:46c10d34af27

Parent:

3:e7807f60c9bf

Child:

5:fe91d31db27e

--- a/main.cpp	Sat Nov 09 03:35:33 2019 +0000
+++ b/main.cpp	Sat Nov 16 06:28:18 2019 +0000
@@ -2,16 +2,21 @@
 #include "mbed.h"
 #include "EC.h" //Encoderライブラリをインクルード
 #include "SpeedController.h"    //SpeedControlライブラリをインクルード
+#include "can.h"
 #define RESOLUTION 512  //分解能
 #define BASIC_SPEED 40   //動確用
 //#define TEST_DUTY 0.3   //動確用
 
 
-Ec4multi EC_1(p15,p16,RESOLUTION);
-Ec4multi EC_2(p17,p18,RESOLUTION);
+Ec4multi EC_1(p16,p15,RESOLUTION);
+Ec4multi EC_2(p18,p17,RESOLUTION);
+//Ec4multi EC_1(p15,p16,RESOLUTION);
+//Ec4multi EC_2(p17,p18,RESOLUTION);
 
 SpeedControl motor_1(p21,p22,50,EC_1);
-SpeedControl motor_2(p23,p24,50,EC_2);
+SpeedControl motor_2(p24,p23,50,EC_2);
+//SpeedControl motor_1(p21,p22,50,EC_1);
+//SpeedControl motor_2(p24,p23,50,EC_2);
 
 Ticker motor_tick;  //角速度計算用ticker
 Serial pc(USBTX,USBRX);
@@ -20,58 +25,48 @@
 
 
 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]= {};
+double rotate_1=40,rotate_2=40;
+//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();
-    motor2_count[j]=EC_2.getCount();
+    //motor1_count[j]=EC_1.getCount();
+    //motor2_count[j]=EC_2.getCount();
     EC_1.calOmega();
     EC_2.calOmega();
-    motor1_omega[j]=EC_1.getOmega();
-    motor2_omega[j]=EC_2.getOmega();
+    //motor1_omega[j]=EC_1.getOmega();
+    //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);
+    else motor_1.Sc(mt_out1);
     if(rotate_2==0) motor_2.stop();
-    else motor_2.Sc(rotate_2);
+    else motor_2.Sc(mt_out2);
 
     j++;
 }
 int main()
 {
+    UserLoopSetting_can();
     motor_1.period_us(50);
     motor_2.period_us(50);
-    motor_1.setEquation(0.0322,0.08,0.0322,0.0800);    //求めたC,Dの値を設定
-    motor_2.setEquation(0.0302,0.0755,0.0241,0.1301);    //求めたC,Dの値を設定
+    motor_1.setEquation(0.0429,0.05,0.0429,0.05);    //求めたC,Dの値を設定
+    motor_2.setEquation(0.0429,0.05,0.0429,0.05);    //求めたC,Dの値を設定
     motor_1.setPDparam(0,0.0);  //PIDの係数を設定
     motor_2.setPDparam(0,0.0);  //PIDの係数を設定
     motor_tick.attach(&CalOmega,0.05);
 
+    int k = 0;
+    
     while(1) {
-        if(pc.readable()) {
-            char sel=pc.getc();
-            if(sel=='q') {
-                for(int i=0; i<1000; i++) {
-//                    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]);
-                }
-            } else if(sel=='i') {
-                pc.printf("fffff\r\n");
-                rotate_1=1*BASIC_SPEED;
-                rotate_2=1*BASIC_SPEED;
-            } else if(sel=='m') {
-                pc.printf("bbbbb\r\n");
-                rotate_1=-1*BASIC_SPEED;
-                rotate_2=-1*BASIC_SPEED;
-            }else if(sel=='s') {
-                pc.printf("sssss\r\n");
-                rotate_1=0;
-                rotate_2=0;
+        // rotate_1 = mt_out1;
+//        rotate_2 = mt_out2;
+    
+        if(k>100){
+            printf("\nloop\n\n\r");
+            k=0;
         }
-        }
+        k++;
     }
+    printf("finish\n\r");
 }
\ No newline at end of file