Diff: main.cpp

Revision:

1:b74b31a7df07

Parent:

0:cb9f2ec9d902

Child:

2:aeae129daa37

--- a/main.cpp	Mon Apr 12 06:08:54 2021 +0000
+++ b/main.cpp	Wed Apr 14 12:19:14 2021 +0000
@@ -5,29 +5,57 @@
 
 #define DELTA_T 0.01    //制御周期
 #define DUTY_MAX 0.8     //duty比の最大値
-#define OMEGA_MAX 18     //速度制御を利用した角度制御での角速度の最大値
-
+#define OMEGA_MAX 15    //速度制御を利用した角度制御での角速度の最大値
+#define NUM_DATA 150    //記録データ数
 
 #ifndef M_PI
 #define M_PI 3.14159265359f
 #endif
 
-
+Serial pc(USBTX, USBRX);   //printfも通シリアル通信なのでこれを書いて、pc.printfとしないとと最悪通信と干渉します。
 DigitalIn toggle(p6,PullUp);
+DigitalOut led1(LED1);
 
-//Timer timer_loop;
-Ticker ticker;
+Timer timer_loop;  //制御周期用
+
 
 /////////////////               宣言部分                  //////////////////////
-//CalPIDの引数は（kp,pi,pd(各PIDのパラメータ),周期,最大値)//
-CalPID speed_pid(0.2,0,0.0033642,DELTA_T,DUTY_MAX);   //速度制御のPD計算
+CalPID speed_pid(0.2,0,0.00455,DELTA_T,DUTY_MAX);   //速度制御のPD計算
 CalPID angle_duty_pid(0.5,0,0.00056,DELTA_T,DUTY_MAX);   //角度制御（duty式）のPD計算
-CalPID angle_omega_pid(8.926,0,0.04963,DELTA_T,OMEGA_MAX);//角度制御（速度式）のPD計算
+CalPID angle_omega_pid(25.0,0,0.00224,DELTA_T,OMEGA_MAX);//角度制御（速度式）のPD計算
 Amt21 ec(p9,p10,p8); //エンコーダー
 //　上で宣言したインスタンスをMotorControllerに与える　//
 MotorController motor(p22,p21,DELTA_T,ec,speed_pid,angle_duty_pid,angle_omega_pid);
 //////////////////////////////////////////////////////////////////////////
-
+////データ記録まわり/////
+float angle_saved[NUM_DATA]= {};
+int angle_count=0;
+void saveAngle()
+{
+    if(angle_count<NUM_DATA) {
+        angle_saved[angle_count]=ec.getRad();
+        angle_count++;
+    }
+}
+float omega_saved[NUM_DATA]= {};
+int omega_count=0;
+void saveOmega()
+{
+    if(omega_count<NUM_DATA) {
+        omega_saved[omega_count]=ec.getOmega();
+        omega_count++;
+    }
+}
+void displayData()
+{
+    for(int i=0; i<omega_count; i++) {
+        pc.printf("%f\t%f\t%f\r\n",i*DELTA_T,angle_saved[i],omega_saved[i]);
+        wait(0.001);
+    }
+    omega_count=0;
+    angle_count=0;
+}
+////////////////////////////////////////////////////////////////////////////
 
 float target_rad=0;  //目標角速度（速度制御）
 float target_speed=0;//目標角度（角度制御）
@@ -35,25 +63,51 @@
 void speedControll()
 {
     motor.Sc(target_speed);       //速度制御関数
+
+    saveAngle();//データ記録
+    saveOmega();//データ記録
 }
 void angleControll()
 {
-    motor.AcDuty(target_rad);    //duty式角度制御
-//    motor.AcOmega(target_rad);   //速度式角度制御
+//    motor.AcDuty(target_rad);    //duty式角度制御
+    motor.AcOmega(target_rad);   //速度式角度制御
+
+    saveAngle();//データ記録
+    saveOmega();//データ記録
 }
 
-int main ()
-{
-    NVIC_SetPriority(TIMER3_IRQn, 1);//tickerの割り込み優先順位を下げエンコーダが飛ばないようにしている(青mbed仕様)
+//////////////////////////////////////////////////////////////////////////////
 
-    motor.setEquation(0.0031,0.0047,-0.0030,0.0078);//速度制御のC値D値
+int main ()     //調整時を想定しているのでデータを取っている。
+{
+    motor.setEquation(0.022639,0.022651,-0.022297,0.026744);//速度制御のC値D値
 //////////////////////////////////////////////////////////////////////////////
-//    target_speed=5; //目標角速度を5rad/sに
-//    ticker.attach(&speedControll,DELTA_T);
-    target_rad=(M_PI/6);; //目標角度をπ/6（rad）に（単位に注意）
-    ticker.attach(&angleControll,DELTA_T);
+    int state=0;
 
     while(1) {
+        if(state==0) {
+            if(toggle) {//スタート用のトグルスイッチ。要らない場合はこのif部分の処理をwhileの直前に
+                state++;
+                target_speed=5; //目標角速度を5rad/sに
+                target_rad=(M_PI/6); //目標角度をπ/6（rad）に（単位に注意）
+
+                timer_loop.reset();
+                timer_loop.start();
+            }
+        } else if(state==1) {
+            if(timer_loop.read()>DELTA_T) {//Tickerだと通信と割り込みが干渉してしまうのでタイマーで
+//                speedControll();//速度制御
+                angleControll();//角度制御
+
+                timer_loop.reset();
+            }
+            if(omega_count==NUM_DATA) {//データが集まったら表示
+                motor.stop();
+                displayData();
+                timer_loop.stop();
+                state++;
+            }
+        }
         wait_us(10);
     }
 }