Diff: main.cpp

Revision:

0:1b4780d434ab

Child:

2:f1e26c8084f2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 27 05:39:07 2022 +0000
@@ -0,0 +1,180 @@
+//20221027試験用
+
+#include "mbed.h"
+#include "EC.h"
+#define RESOLUTION 2048
+#include "CalPID.h"
+#include "MotorController.h"
+
+//0.01秒ごとに角速度を取る 303k8ではこれ以上下げるとうまくいかなかった
+#define DELTA_T 0.01
+//dutyの絶対値の上限を決めて暴走を防ぐ
+//モーター特性が極端に違うものは上限を変える
+#define DUTY_MAX 0.40 //タイヤA
+//角度の上限 今回は不要だけどCalPIDの引数なので消さずに放置してください
+#define OMEGA_MAX 6
+//角速度を格納する配列の要素数
+#define NUM_DATA 500
+Serial pc(USBTX,USBRX);
+
+//速度制御のPID 前3つが係数なのでこれを調整 P→D→Iの順に調整
+CalPID speed_pidA(0.000,0.0,0.000,DELTA_T,DUTY_MAX);
+CalPID speed_pidB(0.00,0.0,0.000,DELTA_T,DUTY_MAX);
+CalPID speed_pidC(0.00,0.0,0.000,DELTA_T,DUTY_MAX);
+CalPID speed_pidD(0.00,0.0,0.000,DELTA_T,DUTY_MAX);
+//角度制御のPID 今回は不要だけどMotorContorollerの引数なので消さずに放置してください
+CalPID angle_omega_pid(0.0,0.0,0.0,DELTA_T,OMEGA_MAX);
+//エンコーダ（A層,B層,分解能）
+//モーター（正転,逆転,PWM周期,エンコーダ,CALPIDの角速度PID,角度PID)
+/////////////R1(f446re)////////////////
+
+//タイヤA
+Ec2multi ecA(PC_6,PC_8,RESOLUTION);
+MotorController motorA(PA_8,PA_9,50,ecA,speed_pidA,angle_omega_pid);
+//タイヤB
+Ec2multi ecB(PB_14,PB_15,RESOLUTION); 
+MotorController motorB(PB_1,PA_11,50,ecB,speed_pidB,angle_omega_pid);
+//タイヤC
+Ec2multi ecC(PC_12,PC_10,RESOLUTION);  
+MotorController motorC(PA_0,PA_1,50,ecC,speed_pidC,angle_omega_pid);
+//タイヤD
+Ec2multi ecD(PC_2,PC_3,RESOLUTION);  
+MotorController motorD(PB_4,PB_5,50,ecD,speed_pidC,angle_omega_pid);
+
+
+////////////////R2(f303k8)///////////////////////
+/*
+//タイヤA
+Ec2multi ecA(PB_4,PB_5,RESOLUTION);
+MotorController motorA(PA_10,PA_9,50,ecA,speed_pidA,angle_omega_pid);
+//タイヤB
+Ec2multi ecB(PB_7,PB_6,RESOLUTION); 
+MotorController motorB(PA_6,PA_7,50,ecB,speed_pidB,angle_omega_pid);
+//タイヤC
+Ec2multi ecC(PB_3,PF_1,RESOLUTION);  
+MotorController motorC(PA_1,PA_3,50,ecC,speed_pidC,angle_omega_pid);
+//タイヤD
+Ec2multi ecD(PA_8,PF_0,RESOLUTION);  
+MotorController motorD(PB_1,PB_0,50,ecD,speed_pidC,angle_omega_pid);
+*/
+
+Ticker ticker;  //割り込みタイマー
+
+
+//角速度を保存する変数と関数
+float omega_savedA[NUM_DATA]= {};
+float omega_savedB[NUM_DATA]= {};
+float omega_savedC[NUM_DATA]= {};
+float omega_savedD[NUM_DATA]= {};
+int omega_count=0;
+void saveOmega()
+{
+    if(omega_count<NUM_DATA) {
+        omega_savedA[omega_count]=ecA.getOmega();
+        omega_savedB[omega_count]=ecB.getOmega();
+        omega_savedC[omega_count]=ecC.getOmega();
+        omega_savedD[omega_count]=ecD.getOmega();
+        omega_count++;
+    }
+}
+
+
+float target_speed=0;
+int save_count;
+#define SAVE_COUNT_THRESHOLD 5 //0.05秒ごとに角速度を配列に格納
+void motorOut()//モーターを目標角速度で動かそうとし、一定の間隔で角速度を配列に保存
+{
+    motorA.Sc(target_speed);
+    motorB.Sc(target_speed);
+    motorC.Sc(target_speed);
+    motorD.Sc(target_speed);
+    save_count++;
+    if(save_count>=SAVE_COUNT_THRESHOLD) {
+        saveOmega();
+        save_count=0;
+    }
+}
+
+void displayData()//保存したデータをprintf
+{
+    printf("A\r\n");
+    for(int i=0; i<omega_count; i++) {
+        printf("%f,",omega_savedA[i]);
+        wait(0.05);//printf重いのでマイコンが落ちないように
+    }
+    printf("\r\nB\r\n");
+    for(int i=0; i<omega_count; i++) {
+        printf("%f,",omega_savedB[i]);
+        wait(0.05);//printf重いのでマイコンが落ちないように
+    }
+    printf("\r\nC\r\n");
+    for(int i=0; i<omega_count; i++) {
+        printf("%f,",omega_savedC[i]);
+        wait(0.05);//printf重いのでマイコンが落ちないように
+    }
+    printf("\r\nD\r\n");
+    for(int i=0; i<omega_count; i++) {
+        printf("%f,",omega_savedD[i]);
+        wait(0.05);//printf重いのでマイコンが落ちないように
+    }
+    omega_count=0;
+}
+
+int main ()
+{
+    //モーター加速度の上限
+    //無視するために大きくしてある
+    //機体が動き始めたときに動きがずれるなら加速度の上限を下げる
+    motorA.setAccelMax(10000000);
+    motorB.setAccelMax(10000000);
+    motorC.setAccelMax(10000000);
+    motorD.setAccelMax(10000000);
+    //ギア比　タイヤ:エンコーダ＝１:rとしたときのrの値
+    //R1
+    /*
+    ecA.setGearRatio(0.6667);
+    ecB.setGearRatio(0.6667);
+    ecC.setGearRatio(0.6667);
+    ecD.setGearRatio(0.6667);
+    */
+    //R2
+    ecA.setGearRatio(0.50);
+    ecB.setGearRatio(0.50);
+    ecC.setGearRatio(0.50);
+    ecD.setGearRatio(0.50);
+    
+    
+    //角速度とduty比の関係式設定
+    //横軸をタイヤの角速度[rad/s]とduty比とした時の傾きとy切片
+    //正転時の傾き,y切片,逆転時の傾き,y切片
+    /////////R1///////////
+    /*motorA.setEquation(0.009911272,0.008627973,-0.009919857,-0.009919857); 
+    motorB.setEquation(0.009657781,0.009657781,-0.010037358,-0.010037358);
+    motorC.setEquation(0.010134584,0.010134584,-0.010120037,0.014440573);
+    motorD.setEquation(0.010158297,0.010158297,-0.010374406,-0.010374406);  */
+    //////////R2//////////
+    motorA.setEquation(0.009436162,0.009436162,-0.00948947,-0.00948947); 
+    motorB.setEquation(0.009421729,0.009421729,-0.009465773,-0.009465773);
+    motorC.setEquation(0.008833138,0.008833138,-0.009762915,0.058112534);
+    motorD.setEquation(0.010214862,0.010214862,-0.009606806,-0.009606806);
+
+    //事前にプログラムをマイコンに読み込んでおく
+    //緊急停止スイッチで切った状態で電源をつなぎ、マイコンをリセットする
+    //改行をしたら緊急停止スイッチを解除する
+    printf("\r\n");
+    wait(4);
+    //STARTが書かれた時点で開始する
+    printf("START\r\n");
+
+    target_speed=-18.0;//ここで目標角速度を設定 最初はduty2の角速度で上手くいきそうならduty4.5の角速度
+    ticker.attach(&motorOut,DELTA_T);//割り込みタイマーをオンにする
+
+    wait(3.0);//モーターが割り込みでまわり10秒分のデータを取る
+    ticker.detach();//割り込みを止める（止めないとモーターを動かし続けてしまう）
+    motorA.stop();//モーターを止める(dutyを0にしている)nn
+    motorB.stop();
+    motorC.stop();
+    motorD.stop();
+    displayData();//角速度のデータが表示されるのでエクセルにいれてグラフにする
+    printf("\r\n");
+}