Diff: main.cpp

Revision:

0:6db935b161f8

Child:

1:0e8ec231cb2f

diff -r 000000000000 -r 6db935b161f8 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 09 11:19:20 2019 +0000
@@ -0,0 +1,54 @@
+//CAN通信で受け取った(double)rotate‗1～rotate‗3に対し、実際に回転数制御を行う
+#include "mbed.h"
+#include "EC.h" //Encoderライブラリをインクルード
+#include "SpeedController.h"    //SpeedControlライブラリをインクルード
+#define RESOLUTION 512  //分解能
+//#define BASIC_SPEED 1   //動確用
+//#define TEST_DUTY 0.3   //動確用
+
+
+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);
+
+
+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;
+
+    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(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;
+        }
+    }
+}
\ No newline at end of file