Diff: ccheck.cpp

Revision:

1:798a41fa6515

Parent:

0:112e277bd7f2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ccheck.cpp	Mon Sep 02 01:26:02 2019 +0000
@@ -0,0 +1,145 @@
+//モーターの角速度(x)とduty比(y)の関係を
+//y=Cx+Dであると近似した時の、定数Cを決定するためのプログラム
+//別プログラムで求めたdを使い傾きCを決定する
+//入力はTera Termで行う
+#include "mbed.h"
+  #include "EC.h" //Encoderライブラリをインクルード
+#include "SpeedController.h"
+   #define RESOLUTION 2048//分解能
+  #define BASIC_SPEED 20//目標角速度、一番使う速度帯におおよそ合わせるが吉
+//  #define TEST_DUTY 0.3
+  
+Ec4multi EC_kagawa(PA_5,PC_9,RESOLUTION);
+SpeedControl kagawa(PB_9,PB_8,50,EC_kagawa);
+Ec4multi EC_tokusima(PA_15,PA_14,RESOLUTION);
+SpeedControl tokusima(PC_7,PA_6,50,EC_tokusima);
+Ec4multi EC_kouchi(PC_13,PB_7,RESOLUTION);
+SpeedControl kouchi(PA_9,PB_6,50,EC_kouchi);
+Ec4multi EC_ehime(PC_2,PC_3,RESOLUTION);
+SpeedControl ehime(PB_3,PA_10,50,EC_ehime);
+Serial pc(USBTX,USBRX);
+Ticker motor_tick;
+ 
+/*void calOmega(){
+    motor.CalOmega();    //角速度計算用
+}
+ */
+int main(void){
+// motor_tick.attach(calOmega,0.05);
+      kagawa.period_us(50);
+      tokusima.period_us(50);
+      kouchi.period_us(50);
+      ehime.period_us(50);
+      double kagawa_Cf=0,kagawa_Cb=0;
+      double tokusima_Cf=0,tokusima_Cb=0;
+      double kouchi_Cf=0,kouchi_Cb=0;
+      double ehime_Cf=0,ehime_Cb=0;
+      
+      kagawa.setPDparam(0,0.0);  //PIDの係数を設定
+      tokusima.setPDparam(0,0.0);  //PIDの係数を設定
+      kouchi.setPDparam(0,0.0);  //PIDの係数を設定
+      ehime.setPDparam(0,0.0);  //PIDの係数を設定
+    int loop_time=0;
+    double target_kagawa=0,target_tokusima=0,target_kouchi=0,target_ehime=0;
+    bool print=false;
+ 
+    while(1){
+        loop_time++;
+ 
+        if(target_kagawa==0) kagawa.stop();
+        else kagawa.Sc(target_kagawa);
+        if(target_tokusima==0) tokusima.stop();
+        else tokusima.Sc(target_tokusima);
+        if(target_kouchi==0) kouchi.stop();
+        else kouchi.Sc(target_kouchi);
+        if(target_ehime==0) ehime.stop();
+        else ehime.Sc(target_ehime);
+        
+      kagawa.setEquation(kagawa_Cf,0,kagawa_Cb,0);    //求めたDの値を設定
+      tokusima.setEquation(tokusima_Cf,0,tokusima_Cb,0);    //求めたDの値を設定
+      kouchi.setEquation(kouchi_Cf,0,kouchi_Cb,0);    //求めたDの値を設定
+      ehime.setEquation(ehime_Cf,0,ehime_Cb,0);    //求めたDの値を設定
+ 
+        if(pc.readable()){
+            char sel=pc.getc();
+            if(sel=='s'){
+                kagawa.stop();
+                tokusima.stop();
+                kouchi.stop();
+                ehime.stop();
+            } else if(sel=='e'){
+                pc.printf("kagawa_Cf");
+                if(sel=='i')kagawa_Cf+=0.002;        //e->iを押すとCfがo.o02ずつあがる
+                else if(sel=='m'&&kagawa_Cf>0)kagawa_Cf-=0.002;   //e->mを押すとCfがo.o02ずつさがる
+                pc.printf("duty=%f\r\n",kagawa_Cf);
+            } else if(sel=='d'){
+                pc.printf("kagawa_Cb");
+                if(sel=='i')kagawa_Cb+=0.002;        //e->iを押すとCfがo.o02ずつあがる
+                else if(sel=='m'&&kagawa_Cf>0)kagawa_Cb-=0.002;   //e->mを押すとCfがo.o02ずつさがる
+                pc.printf("duty=%f\r\n",kagawa_Cf);
+            }else if(sel=='r'){
+                pc.printf("tokusima_Cf");
+                if(sel=='i')tokusima_Cf+=0.002;        //r->iを押すとCfがo.o02ずつあがる
+                else if(sel=='m'&&tokusima_Cf>0)tokusima_Cf-=0.002;   //r->mを押すとCfがo.o02ずつさがる
+                pc.printf("duty=%f\r\n",tokusima_Cf);
+            } else if(sel=='f'){
+               pc.printf("tokusima_Cb");
+                if(sel=='i')tokusima_Cb+=0.002;        //f->iを押すとCfがo.o02ずつあがる
+                else if(sel=='m'&&tokusima_Cf>0)tokusima_Cb-=0.002;   //f->mを押すとCfがo.o02ずつさがる
+                pc.printf("duty=%f\r\n",tokusima_Cb);
+                
+            }else if(sel=='t'){
+               pc.printf("kouchi_Cf");
+                if(sel=='i')kouchi_Cf+=0.002;        //t->iを押すとCfがo.o02ずつあがる
+                else if(sel=='m'&&kouchi_Cf>0)kouchi_Cf-=0.002;   //t->mを押すとCfがo.o02ずつさがる
+                pc.printf("duty=%f\r\n",kouchi_Cf);
+            } else if(sel=='g'){
+                pc.printf("kouchi_Cb");
+                if(sel=='i')kouchi_Cb+=0.002;        //g->iを押すとCfがo.o02ずつあがる
+                else if(sel=='m'&&kouchi_Cf>0)kouchi_Cb-=0.002;   //g->mを押すとCfがo.o02ずつさがる
+                pc.printf("duty=%f\r\n",kouchi_Cb);
+                
+            }else if(sel=='y'){
+                pc.printf("ehime_Cf");
+                if(sel=='i')ehime_Cf+=0.002;        //t->iを押すとCfがo.o02ずつあがる
+                else if(sel=='m'&&ehime_Cf>0)ehime_Cf-=0.002;   //t->mを押すとCfがo.o02ずつさがる
+                pc.printf("duty=%f\r\n",ehime_Cf);
+            } else if(sel=='h'){
+              pc.printf("ehime_Cb");
+                if(sel=='i')ehime_Cb+=0.002;        //g->iを押すとCfがo.o02ずつあがる
+                else if(sel=='m'&&ehime_Cf>0)ehime_Cb-=0.002;   //g->mを押すとCfがo.o02ずつさがる
+                pc.printf("duty=%f\r\n",ehime_Cb);
+            }else if(sel=='p'){
+                
+                print=!print;                //pを押すと表示を停止/再開する
+            }else if(sel=='i'){
+                target_kagawa=1*BASIC_SPEED;
+                target_tokusima=1*BASIC_SPEED;
+                target_kouchi=-1*BASIC_SPEED;
+                target_ehime=-1*BASIC_SPEED;
+                
+            }else if(sel=='m'){
+                target_kagawa=1*BASIC_SPEED;
+                target_tokusima=1*BASIC_SPEED;
+                target_kouchi=-1*BASIC_SPEED;
+                target_ehime=-1*BASIC_SPEED;
+            }else if(sel=='k'){
+                target_kagawa=1*BASIC_SPEED;
+                target_tokusima=1*BASIC_SPEED;
+                target_kouchi=-1*BASIC_SPEED;
+                target_ehime=-1*BASIC_SPEED;
+            }else if(sel=='j'){
+                target_kagawa=1*BASIC_SPEED;
+                target_tokusima=1*BASIC_SPEED;
+                target_kouchi=-1*BASIC_SPEED;
+                target_ehime=-1*BASIC_SPEED;
+            }
+            
+        }
+ 
+        if(loop_time%1000==0){
+            
+            if(print) pc.printf("%.2f %.2f %.2f %.2f\r\n",EC_kagawa.getOmega(),EC_tokusima.getOmega(),EC_kouchi.getOmega(),EC_ehime.getOmega());
+        }   
+    }
+}