![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2023春ロボ2班 足回りコード
Dependencies: CalPID_2022 Encoder_2022 MPU6050 Madgwickfilter MotorController_2022 mbed
main.cpp@4:fc9688f7614a, 20 months ago (annotated)
- Committer:
- yn_4009
- Date:
- Thu Dec 29 14:42:47 2022 +0000
- Revision:
- 4:fc9688f7614a
- Parent:
- 3:7c29db78fa24
test;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tmpi5235 | 0:1b4780d434ab | 1 | //20221027試験用 |
tmpi5235 | 0:1b4780d434ab | 2 | |
tmpi5235 | 0:1b4780d434ab | 3 | #include "mbed.h" |
tmpi5235 | 0:1b4780d434ab | 4 | #include "EC.h" |
tmpi5235 | 0:1b4780d434ab | 5 | #define RESOLUTION 2048 |
tmpi5235 | 0:1b4780d434ab | 6 | #include "CalPID.h" |
tmpi5235 | 0:1b4780d434ab | 7 | #include "MotorController.h" |
tmpi5235 | 0:1b4780d434ab | 8 | |
tmpi5235 | 0:1b4780d434ab | 9 | //0.01秒ごとに角速度を取る 303k8ではこれ以上下げるとうまくいかなかった |
tmpi5235 | 0:1b4780d434ab | 10 | #define DELTA_T 0.01 |
tmpi5235 | 0:1b4780d434ab | 11 | //dutyの絶対値の上限を決めて暴走を防ぐ |
tmpi5235 | 0:1b4780d434ab | 12 | //モーター特性が極端に違うものは上限を変える |
tmpi5235 | 0:1b4780d434ab | 13 | #define DUTY_MAX 0.40 //タイヤA |
tmpi5235 | 0:1b4780d434ab | 14 | //角度の上限 今回は不要だけどCalPIDの引数なので消さずに放置してください |
tmpi5235 | 0:1b4780d434ab | 15 | #define OMEGA_MAX 6 |
tmpi5235 | 0:1b4780d434ab | 16 | //角速度を格納する配列の要素数 |
tmpi5235 | 0:1b4780d434ab | 17 | #define NUM_DATA 500 |
tmpi5235 | 0:1b4780d434ab | 18 | Serial pc(USBTX,USBRX); |
tmpi5235 | 0:1b4780d434ab | 19 | |
tmpi5235 | 0:1b4780d434ab | 20 | //速度制御のPID 前3つが係数なのでこれを調整 P→D→Iの順に調整 |
yn_4009 | 4:fc9688f7614a | 21 | CalPID speed_pidA(0.002,0.0,0.000,DELTA_T,DUTY_MAX); |
yn_4009 | 4:fc9688f7614a | 22 | CalPID speed_pidB(0.002,0.0,0.000,DELTA_T,DUTY_MAX); |
yn_4009 | 4:fc9688f7614a | 23 | CalPID speed_pidC(0.002,0.0,0.000,DELTA_T,DUTY_MAX); |
yn_4009 | 4:fc9688f7614a | 24 | CalPID speed_pidD(0.002,0.0,0.000,DELTA_T,DUTY_MAX); |
tmpi5235 | 0:1b4780d434ab | 25 | //角度制御のPID 今回は不要だけどMotorContorollerの引数なので消さずに放置してください |
tmpi5235 | 0:1b4780d434ab | 26 | CalPID angle_omega_pid(0.0,0.0,0.0,DELTA_T,OMEGA_MAX); |
tmpi5235 | 0:1b4780d434ab | 27 | //エンコーダ(A層,B層,分解能) |
tmpi5235 | 0:1b4780d434ab | 28 | //モーター(正転,逆転,PWM周期,エンコーダ,CALPIDの角速度PID,角度PID) |
yn_4009 | 2:f1e26c8084f2 | 29 | |
yn_4009 | 2:f1e26c8084f2 | 30 | //タイヤA |
yn_4009 | 4:fc9688f7614a | 31 | Ec2multi ecA(PC_12,PC_10,RESOLUTION); |
yn_4009 | 4:fc9688f7614a | 32 | MotorController motorA(PA_11,PB_1,50,ecA,speed_pidA,angle_omega_pid); |
yn_4009 | 2:f1e26c8084f2 | 33 | //タイヤB |
yn_4009 | 4:fc9688f7614a | 34 | Ec2multi ecB(PC_3,PC_2,RESOLUTION); |
yn_4009 | 4:fc9688f7614a | 35 | MotorController motorB(PB_5,PB_4,50,ecB,speed_pidB,angle_omega_pid); |
yn_4009 | 2:f1e26c8084f2 | 36 | //タイヤC |
yn_4009 | 4:fc9688f7614a | 37 | Ec2multi ecC(PB_14,PB_15,RESOLUTION); |
yn_4009 | 2:f1e26c8084f2 | 38 | MotorController motorC(PA_0,PA_1,50,ecC,speed_pidC,angle_omega_pid); |
yn_4009 | 2:f1e26c8084f2 | 39 | //タイヤD |
yn_4009 | 4:fc9688f7614a | 40 | Ec2multi ecD(PC_6,PC_8,RESOLUTION); |
yn_4009 | 4:fc9688f7614a | 41 | MotorController motorD(PA_8,PA_9,50,ecD,speed_pidC,angle_omega_pid); |
tmpi5235 | 0:1b4780d434ab | 42 | |
tmpi5235 | 0:1b4780d434ab | 43 | |
tmpi5235 | 0:1b4780d434ab | 44 | Ticker ticker; //割り込みタイマー |
tmpi5235 | 0:1b4780d434ab | 45 | |
tmpi5235 | 0:1b4780d434ab | 46 | |
tmpi5235 | 0:1b4780d434ab | 47 | //角速度を保存する変数と関数 |
tmpi5235 | 0:1b4780d434ab | 48 | float omega_savedA[NUM_DATA]= {}; |
tmpi5235 | 0:1b4780d434ab | 49 | float omega_savedB[NUM_DATA]= {}; |
tmpi5235 | 0:1b4780d434ab | 50 | float omega_savedC[NUM_DATA]= {}; |
tmpi5235 | 0:1b4780d434ab | 51 | float omega_savedD[NUM_DATA]= {}; |
tmpi5235 | 0:1b4780d434ab | 52 | int omega_count=0; |
tmpi5235 | 0:1b4780d434ab | 53 | void saveOmega() |
tmpi5235 | 0:1b4780d434ab | 54 | { |
tmpi5235 | 0:1b4780d434ab | 55 | if(omega_count<NUM_DATA) { |
tmpi5235 | 0:1b4780d434ab | 56 | omega_savedA[omega_count]=ecA.getOmega(); |
tmpi5235 | 0:1b4780d434ab | 57 | omega_savedB[omega_count]=ecB.getOmega(); |
tmpi5235 | 0:1b4780d434ab | 58 | omega_savedC[omega_count]=ecC.getOmega(); |
tmpi5235 | 0:1b4780d434ab | 59 | omega_savedD[omega_count]=ecD.getOmega(); |
tmpi5235 | 0:1b4780d434ab | 60 | omega_count++; |
tmpi5235 | 0:1b4780d434ab | 61 | } |
tmpi5235 | 0:1b4780d434ab | 62 | } |
tmpi5235 | 0:1b4780d434ab | 63 | |
tmpi5235 | 0:1b4780d434ab | 64 | |
tmpi5235 | 0:1b4780d434ab | 65 | float target_speed=0; |
tmpi5235 | 0:1b4780d434ab | 66 | int save_count; |
tmpi5235 | 0:1b4780d434ab | 67 | #define SAVE_COUNT_THRESHOLD 5 //0.05秒ごとに角速度を配列に格納 |
yn_4009 | 4:fc9688f7614a | 68 | |
tmpi5235 | 0:1b4780d434ab | 69 | void motorOut()//モーターを目標角速度で動かそうとし、一定の間隔で角速度を配列に保存 |
tmpi5235 | 0:1b4780d434ab | 70 | { |
tmpi5235 | 0:1b4780d434ab | 71 | motorA.Sc(target_speed); |
tmpi5235 | 0:1b4780d434ab | 72 | motorB.Sc(target_speed); |
tmpi5235 | 0:1b4780d434ab | 73 | motorC.Sc(target_speed); |
tmpi5235 | 0:1b4780d434ab | 74 | motorD.Sc(target_speed); |
tmpi5235 | 0:1b4780d434ab | 75 | save_count++; |
tmpi5235 | 0:1b4780d434ab | 76 | if(save_count>=SAVE_COUNT_THRESHOLD) { |
tmpi5235 | 0:1b4780d434ab | 77 | saveOmega(); |
tmpi5235 | 0:1b4780d434ab | 78 | save_count=0; |
tmpi5235 | 0:1b4780d434ab | 79 | } |
tmpi5235 | 0:1b4780d434ab | 80 | } |
tmpi5235 | 0:1b4780d434ab | 81 | |
tmpi5235 | 0:1b4780d434ab | 82 | void displayData()//保存したデータをprintf |
tmpi5235 | 0:1b4780d434ab | 83 | { |
tmpi5235 | 0:1b4780d434ab | 84 | printf("A\r\n"); |
tmpi5235 | 0:1b4780d434ab | 85 | for(int i=0; i<omega_count; i++) { |
tmpi5235 | 0:1b4780d434ab | 86 | printf("%f,",omega_savedA[i]); |
tmpi5235 | 0:1b4780d434ab | 87 | wait(0.05);//printf重いのでマイコンが落ちないように |
tmpi5235 | 0:1b4780d434ab | 88 | } |
tmpi5235 | 0:1b4780d434ab | 89 | printf("\r\nB\r\n"); |
tmpi5235 | 0:1b4780d434ab | 90 | for(int i=0; i<omega_count; i++) { |
tmpi5235 | 0:1b4780d434ab | 91 | printf("%f,",omega_savedB[i]); |
tmpi5235 | 0:1b4780d434ab | 92 | wait(0.05);//printf重いのでマイコンが落ちないように |
tmpi5235 | 0:1b4780d434ab | 93 | } |
tmpi5235 | 0:1b4780d434ab | 94 | printf("\r\nC\r\n"); |
tmpi5235 | 0:1b4780d434ab | 95 | for(int i=0; i<omega_count; i++) { |
tmpi5235 | 0:1b4780d434ab | 96 | printf("%f,",omega_savedC[i]); |
tmpi5235 | 0:1b4780d434ab | 97 | wait(0.05);//printf重いのでマイコンが落ちないように |
tmpi5235 | 0:1b4780d434ab | 98 | } |
tmpi5235 | 0:1b4780d434ab | 99 | printf("\r\nD\r\n"); |
tmpi5235 | 0:1b4780d434ab | 100 | for(int i=0; i<omega_count; i++) { |
tmpi5235 | 0:1b4780d434ab | 101 | printf("%f,",omega_savedD[i]); |
tmpi5235 | 0:1b4780d434ab | 102 | wait(0.05);//printf重いのでマイコンが落ちないように |
tmpi5235 | 0:1b4780d434ab | 103 | } |
tmpi5235 | 0:1b4780d434ab | 104 | omega_count=0; |
tmpi5235 | 0:1b4780d434ab | 105 | } |
tmpi5235 | 0:1b4780d434ab | 106 | |
tmpi5235 | 0:1b4780d434ab | 107 | int main () |
tmpi5235 | 0:1b4780d434ab | 108 | { |
tmpi5235 | 0:1b4780d434ab | 109 | //モーター加速度の上限 |
tmpi5235 | 0:1b4780d434ab | 110 | //無視するために大きくしてある |
tmpi5235 | 0:1b4780d434ab | 111 | //機体が動き始めたときに動きがずれるなら加速度の上限を下げる |
tmpi5235 | 0:1b4780d434ab | 112 | motorA.setAccelMax(10000000); |
tmpi5235 | 0:1b4780d434ab | 113 | motorB.setAccelMax(10000000); |
tmpi5235 | 0:1b4780d434ab | 114 | motorC.setAccelMax(10000000); |
tmpi5235 | 0:1b4780d434ab | 115 | motorD.setAccelMax(10000000); |
tmpi5235 | 0:1b4780d434ab | 116 | //ギア比 タイヤ:エンコーダ=1:rとしたときのrの値 |
tmpi5235 | 0:1b4780d434ab | 117 | //R1 |
tmpi5235 | 0:1b4780d434ab | 118 | /* |
tmpi5235 | 0:1b4780d434ab | 119 | ecA.setGearRatio(0.6667); |
tmpi5235 | 0:1b4780d434ab | 120 | ecB.setGearRatio(0.6667); |
tmpi5235 | 0:1b4780d434ab | 121 | ecC.setGearRatio(0.6667); |
tmpi5235 | 0:1b4780d434ab | 122 | ecD.setGearRatio(0.6667); |
tmpi5235 | 0:1b4780d434ab | 123 | */ |
tmpi5235 | 0:1b4780d434ab | 124 | //R2 |
tmpi5235 | 0:1b4780d434ab | 125 | ecA.setGearRatio(0.50); |
tmpi5235 | 0:1b4780d434ab | 126 | ecB.setGearRatio(0.50); |
tmpi5235 | 0:1b4780d434ab | 127 | ecC.setGearRatio(0.50); |
tmpi5235 | 0:1b4780d434ab | 128 | ecD.setGearRatio(0.50); |
tmpi5235 | 0:1b4780d434ab | 129 | |
tmpi5235 | 0:1b4780d434ab | 130 | |
tmpi5235 | 0:1b4780d434ab | 131 | //角速度とduty比の関係式設定 |
tmpi5235 | 0:1b4780d434ab | 132 | //横軸をタイヤの角速度[rad/s]とduty比とした時の傾きとy切片 |
tmpi5235 | 0:1b4780d434ab | 133 | //正転時の傾き,y切片,逆転時の傾き,y切片 |
tmpi5235 | 0:1b4780d434ab | 134 | /////////R1/////////// |
tmpi5235 | 0:1b4780d434ab | 135 | /*motorA.setEquation(0.009911272,0.008627973,-0.009919857,-0.009919857); |
tmpi5235 | 0:1b4780d434ab | 136 | motorB.setEquation(0.009657781,0.009657781,-0.010037358,-0.010037358); |
tmpi5235 | 0:1b4780d434ab | 137 | motorC.setEquation(0.010134584,0.010134584,-0.010120037,0.014440573); |
tmpi5235 | 0:1b4780d434ab | 138 | motorD.setEquation(0.010158297,0.010158297,-0.010374406,-0.010374406); */ |
tmpi5235 | 0:1b4780d434ab | 139 | //////////R2////////// |
tmpi5235 | 0:1b4780d434ab | 140 | motorA.setEquation(0.009436162,0.009436162,-0.00948947,-0.00948947); |
tmpi5235 | 0:1b4780d434ab | 141 | motorB.setEquation(0.009421729,0.009421729,-0.009465773,-0.009465773); |
tmpi5235 | 0:1b4780d434ab | 142 | motorC.setEquation(0.008833138,0.008833138,-0.009762915,0.058112534); |
tmpi5235 | 0:1b4780d434ab | 143 | motorD.setEquation(0.010214862,0.010214862,-0.009606806,-0.009606806); |
tmpi5235 | 0:1b4780d434ab | 144 | |
tmpi5235 | 0:1b4780d434ab | 145 | //事前にプログラムをマイコンに読み込んでおく |
tmpi5235 | 0:1b4780d434ab | 146 | //緊急停止スイッチで切った状態で電源をつなぎ、マイコンをリセットする |
tmpi5235 | 0:1b4780d434ab | 147 | //改行をしたら緊急停止スイッチを解除する |
tmpi5235 | 0:1b4780d434ab | 148 | printf("\r\n"); |
yn_4009 | 3:7c29db78fa24 | 149 | wait(1); |
tmpi5235 | 0:1b4780d434ab | 150 | //STARTが書かれた時点で開始する |
tmpi5235 | 0:1b4780d434ab | 151 | printf("START\r\n"); |
tmpi5235 | 0:1b4780d434ab | 152 | |
yn_4009 | 4:fc9688f7614a | 153 | target_speed=18.0;//ここで目標角速度を設定 最初はduty2の角速度で上手くいきそうならduty4.5の角速度 |
tmpi5235 | 0:1b4780d434ab | 154 | ticker.attach(&motorOut,DELTA_T);//割り込みタイマーをオンにする |
tmpi5235 | 0:1b4780d434ab | 155 | |
yn_4009 | 3:7c29db78fa24 | 156 | wait(5.0);//モーターが割り込みでまわり5秒分のデータを取る |
tmpi5235 | 0:1b4780d434ab | 157 | ticker.detach();//割り込みを止める(止めないとモーターを動かし続けてしまう) |
tmpi5235 | 0:1b4780d434ab | 158 | motorA.stop();//モーターを止める(dutyを0にしている)nn |
tmpi5235 | 0:1b4780d434ab | 159 | motorB.stop(); |
tmpi5235 | 0:1b4780d434ab | 160 | motorC.stop(); |
tmpi5235 | 0:1b4780d434ab | 161 | motorD.stop(); |
tmpi5235 | 0:1b4780d434ab | 162 | displayData();//角速度のデータが表示されるのでエクセルにいれてグラフにする |
tmpi5235 | 0:1b4780d434ab | 163 | printf("\r\n"); |
tmpi5235 | 0:1b4780d434ab | 164 | } |