![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2023春ロボ2班 足回りコード
Dependencies: CalPID_2022 Encoder_2022 MPU6050 Madgwickfilter MotorController_2022 mbed
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); + //ギア比 タイヤ:エンコーダ=1: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"); +}