![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2023春ロボ2班 足回りコード
Dependencies: CalPID_2022 Encoder_2022 MPU6050 Madgwickfilter MotorController_2022 mbed
main.cpp
- Committer:
- yn_4009
- Date:
- 22 months ago
- Revision:
- 3:7c29db78fa24
- Parent:
- 2:f1e26c8084f2
- Child:
- 4:fc9688f7614a
File content as of revision 3:7c29db78fa24:
//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.01,0.0,0.000,DELTA_T,DUTY_MAX); CalPID speed_pidB(0.01,0.0,0.000,DELTA_T,DUTY_MAX); CalPID speed_pidC(0.01,0.0,0.000,DELTA_T,DUTY_MAX); CalPID speed_pidD(0.01,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); */ //タイヤA Ec2multi ecA(PC_3,PC_2,RESOLUTION); MotorController motorA(PA_8,PA_9,50,ecA,speed_pidA,angle_omega_pid); //タイヤB Ec2multi ecB(PC_12,PC_10,RESOLUTION); MotorController motorB(PB_1,PA_11,50,ecB,speed_pidB,angle_omega_pid); //タイヤC Ec2multi ecC(PC_6,PC_8,RESOLUTION); MotorController motorC(PA_0,PA_1,50,ecC,speed_pidC,angle_omega_pid); //タイヤD Ec2multi ecD(PB_14,PB_15,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(1); //STARTが書かれた時点で開始する printf("START\r\n"); target_speed=-18.0;//ここで目標角速度を設定 最初はduty2の角速度で上手くいきそうならduty4.5の角速度 ticker.attach(&motorOut,DELTA_T);//割り込みタイマーをオンにする wait(5.0);//モーターが割り込みでまわり5秒分のデータを取る ticker.detach();//割り込みを止める(止めないとモーターを動かし続けてしまう) motorA.stop();//モーターを止める(dutyを0にしている)nn motorB.stop(); motorC.stop(); motorD.stop(); displayData();//角速度のデータが表示されるのでエクセルにいれてグラフにする printf("\r\n"); }