![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2023春ロボ2班 足回りコード
Dependencies: CalPID_2022 Encoder_2022 MPU6050 Madgwickfilter MotorController_2022 mbed
main.cpp@0:1b4780d434ab, 22 months ago (annotated)
- Committer:
- tmpi5235
- Date:
- Thu Oct 27 05:39:07 2022 +0000
- Revision:
- 0:1b4780d434ab
- Child:
- 2:f1e26c8084f2
kekkani
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の順に調整 |
tmpi5235 | 0:1b4780d434ab | 21 | CalPID speed_pidA(0.000,0.0,0.000,DELTA_T,DUTY_MAX); |
tmpi5235 | 0:1b4780d434ab | 22 | CalPID speed_pidB(0.00,0.0,0.000,DELTA_T,DUTY_MAX); |
tmpi5235 | 0:1b4780d434ab | 23 | CalPID speed_pidC(0.00,0.0,0.000,DELTA_T,DUTY_MAX); |
tmpi5235 | 0:1b4780d434ab | 24 | CalPID speed_pidD(0.00,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) |
tmpi5235 | 0:1b4780d434ab | 29 | /////////////R1(f446re)//////////////// |
tmpi5235 | 0:1b4780d434ab | 30 | |
tmpi5235 | 0:1b4780d434ab | 31 | //タイヤA |
tmpi5235 | 0:1b4780d434ab | 32 | Ec2multi ecA(PC_6,PC_8,RESOLUTION); |
tmpi5235 | 0:1b4780d434ab | 33 | MotorController motorA(PA_8,PA_9,50,ecA,speed_pidA,angle_omega_pid); |
tmpi5235 | 0:1b4780d434ab | 34 | //タイヤB |
tmpi5235 | 0:1b4780d434ab | 35 | Ec2multi ecB(PB_14,PB_15,RESOLUTION); |
tmpi5235 | 0:1b4780d434ab | 36 | MotorController motorB(PB_1,PA_11,50,ecB,speed_pidB,angle_omega_pid); |
tmpi5235 | 0:1b4780d434ab | 37 | //タイヤC |
tmpi5235 | 0:1b4780d434ab | 38 | Ec2multi ecC(PC_12,PC_10,RESOLUTION); |
tmpi5235 | 0:1b4780d434ab | 39 | MotorController motorC(PA_0,PA_1,50,ecC,speed_pidC,angle_omega_pid); |
tmpi5235 | 0:1b4780d434ab | 40 | //タイヤD |
tmpi5235 | 0:1b4780d434ab | 41 | Ec2multi ecD(PC_2,PC_3,RESOLUTION); |
tmpi5235 | 0:1b4780d434ab | 42 | MotorController motorD(PB_4,PB_5,50,ecD,speed_pidC,angle_omega_pid); |
tmpi5235 | 0:1b4780d434ab | 43 | |
tmpi5235 | 0:1b4780d434ab | 44 | |
tmpi5235 | 0:1b4780d434ab | 45 | ////////////////R2(f303k8)/////////////////////// |
tmpi5235 | 0:1b4780d434ab | 46 | /* |
tmpi5235 | 0:1b4780d434ab | 47 | //タイヤA |
tmpi5235 | 0:1b4780d434ab | 48 | Ec2multi ecA(PB_4,PB_5,RESOLUTION); |
tmpi5235 | 0:1b4780d434ab | 49 | MotorController motorA(PA_10,PA_9,50,ecA,speed_pidA,angle_omega_pid); |
tmpi5235 | 0:1b4780d434ab | 50 | //タイヤB |
tmpi5235 | 0:1b4780d434ab | 51 | Ec2multi ecB(PB_7,PB_6,RESOLUTION); |
tmpi5235 | 0:1b4780d434ab | 52 | MotorController motorB(PA_6,PA_7,50,ecB,speed_pidB,angle_omega_pid); |
tmpi5235 | 0:1b4780d434ab | 53 | //タイヤC |
tmpi5235 | 0:1b4780d434ab | 54 | Ec2multi ecC(PB_3,PF_1,RESOLUTION); |
tmpi5235 | 0:1b4780d434ab | 55 | MotorController motorC(PA_1,PA_3,50,ecC,speed_pidC,angle_omega_pid); |
tmpi5235 | 0:1b4780d434ab | 56 | //タイヤD |
tmpi5235 | 0:1b4780d434ab | 57 | Ec2multi ecD(PA_8,PF_0,RESOLUTION); |
tmpi5235 | 0:1b4780d434ab | 58 | MotorController motorD(PB_1,PB_0,50,ecD,speed_pidC,angle_omega_pid); |
tmpi5235 | 0:1b4780d434ab | 59 | */ |
tmpi5235 | 0:1b4780d434ab | 60 | |
tmpi5235 | 0:1b4780d434ab | 61 | Ticker ticker; //割り込みタイマー |
tmpi5235 | 0:1b4780d434ab | 62 | |
tmpi5235 | 0:1b4780d434ab | 63 | |
tmpi5235 | 0:1b4780d434ab | 64 | //角速度を保存する変数と関数 |
tmpi5235 | 0:1b4780d434ab | 65 | float omega_savedA[NUM_DATA]= {}; |
tmpi5235 | 0:1b4780d434ab | 66 | float omega_savedB[NUM_DATA]= {}; |
tmpi5235 | 0:1b4780d434ab | 67 | float omega_savedC[NUM_DATA]= {}; |
tmpi5235 | 0:1b4780d434ab | 68 | float omega_savedD[NUM_DATA]= {}; |
tmpi5235 | 0:1b4780d434ab | 69 | int omega_count=0; |
tmpi5235 | 0:1b4780d434ab | 70 | void saveOmega() |
tmpi5235 | 0:1b4780d434ab | 71 | { |
tmpi5235 | 0:1b4780d434ab | 72 | if(omega_count<NUM_DATA) { |
tmpi5235 | 0:1b4780d434ab | 73 | omega_savedA[omega_count]=ecA.getOmega(); |
tmpi5235 | 0:1b4780d434ab | 74 | omega_savedB[omega_count]=ecB.getOmega(); |
tmpi5235 | 0:1b4780d434ab | 75 | omega_savedC[omega_count]=ecC.getOmega(); |
tmpi5235 | 0:1b4780d434ab | 76 | omega_savedD[omega_count]=ecD.getOmega(); |
tmpi5235 | 0:1b4780d434ab | 77 | omega_count++; |
tmpi5235 | 0:1b4780d434ab | 78 | } |
tmpi5235 | 0:1b4780d434ab | 79 | } |
tmpi5235 | 0:1b4780d434ab | 80 | |
tmpi5235 | 0:1b4780d434ab | 81 | |
tmpi5235 | 0:1b4780d434ab | 82 | float target_speed=0; |
tmpi5235 | 0:1b4780d434ab | 83 | int save_count; |
tmpi5235 | 0:1b4780d434ab | 84 | #define SAVE_COUNT_THRESHOLD 5 //0.05秒ごとに角速度を配列に格納 |
tmpi5235 | 0:1b4780d434ab | 85 | void motorOut()//モーターを目標角速度で動かそうとし、一定の間隔で角速度を配列に保存 |
tmpi5235 | 0:1b4780d434ab | 86 | { |
tmpi5235 | 0:1b4780d434ab | 87 | motorA.Sc(target_speed); |
tmpi5235 | 0:1b4780d434ab | 88 | motorB.Sc(target_speed); |
tmpi5235 | 0:1b4780d434ab | 89 | motorC.Sc(target_speed); |
tmpi5235 | 0:1b4780d434ab | 90 | motorD.Sc(target_speed); |
tmpi5235 | 0:1b4780d434ab | 91 | save_count++; |
tmpi5235 | 0:1b4780d434ab | 92 | if(save_count>=SAVE_COUNT_THRESHOLD) { |
tmpi5235 | 0:1b4780d434ab | 93 | saveOmega(); |
tmpi5235 | 0:1b4780d434ab | 94 | save_count=0; |
tmpi5235 | 0:1b4780d434ab | 95 | } |
tmpi5235 | 0:1b4780d434ab | 96 | } |
tmpi5235 | 0:1b4780d434ab | 97 | |
tmpi5235 | 0:1b4780d434ab | 98 | void displayData()//保存したデータをprintf |
tmpi5235 | 0:1b4780d434ab | 99 | { |
tmpi5235 | 0:1b4780d434ab | 100 | printf("A\r\n"); |
tmpi5235 | 0:1b4780d434ab | 101 | for(int i=0; i<omega_count; i++) { |
tmpi5235 | 0:1b4780d434ab | 102 | printf("%f,",omega_savedA[i]); |
tmpi5235 | 0:1b4780d434ab | 103 | wait(0.05);//printf重いのでマイコンが落ちないように |
tmpi5235 | 0:1b4780d434ab | 104 | } |
tmpi5235 | 0:1b4780d434ab | 105 | printf("\r\nB\r\n"); |
tmpi5235 | 0:1b4780d434ab | 106 | for(int i=0; i<omega_count; i++) { |
tmpi5235 | 0:1b4780d434ab | 107 | printf("%f,",omega_savedB[i]); |
tmpi5235 | 0:1b4780d434ab | 108 | wait(0.05);//printf重いのでマイコンが落ちないように |
tmpi5235 | 0:1b4780d434ab | 109 | } |
tmpi5235 | 0:1b4780d434ab | 110 | printf("\r\nC\r\n"); |
tmpi5235 | 0:1b4780d434ab | 111 | for(int i=0; i<omega_count; i++) { |
tmpi5235 | 0:1b4780d434ab | 112 | printf("%f,",omega_savedC[i]); |
tmpi5235 | 0:1b4780d434ab | 113 | wait(0.05);//printf重いのでマイコンが落ちないように |
tmpi5235 | 0:1b4780d434ab | 114 | } |
tmpi5235 | 0:1b4780d434ab | 115 | printf("\r\nD\r\n"); |
tmpi5235 | 0:1b4780d434ab | 116 | for(int i=0; i<omega_count; i++) { |
tmpi5235 | 0:1b4780d434ab | 117 | printf("%f,",omega_savedD[i]); |
tmpi5235 | 0:1b4780d434ab | 118 | wait(0.05);//printf重いのでマイコンが落ちないように |
tmpi5235 | 0:1b4780d434ab | 119 | } |
tmpi5235 | 0:1b4780d434ab | 120 | omega_count=0; |
tmpi5235 | 0:1b4780d434ab | 121 | } |
tmpi5235 | 0:1b4780d434ab | 122 | |
tmpi5235 | 0:1b4780d434ab | 123 | int main () |
tmpi5235 | 0:1b4780d434ab | 124 | { |
tmpi5235 | 0:1b4780d434ab | 125 | //モーター加速度の上限 |
tmpi5235 | 0:1b4780d434ab | 126 | //無視するために大きくしてある |
tmpi5235 | 0:1b4780d434ab | 127 | //機体が動き始めたときに動きがずれるなら加速度の上限を下げる |
tmpi5235 | 0:1b4780d434ab | 128 | motorA.setAccelMax(10000000); |
tmpi5235 | 0:1b4780d434ab | 129 | motorB.setAccelMax(10000000); |
tmpi5235 | 0:1b4780d434ab | 130 | motorC.setAccelMax(10000000); |
tmpi5235 | 0:1b4780d434ab | 131 | motorD.setAccelMax(10000000); |
tmpi5235 | 0:1b4780d434ab | 132 | //ギア比 タイヤ:エンコーダ=1:rとしたときのrの値 |
tmpi5235 | 0:1b4780d434ab | 133 | //R1 |
tmpi5235 | 0:1b4780d434ab | 134 | /* |
tmpi5235 | 0:1b4780d434ab | 135 | ecA.setGearRatio(0.6667); |
tmpi5235 | 0:1b4780d434ab | 136 | ecB.setGearRatio(0.6667); |
tmpi5235 | 0:1b4780d434ab | 137 | ecC.setGearRatio(0.6667); |
tmpi5235 | 0:1b4780d434ab | 138 | ecD.setGearRatio(0.6667); |
tmpi5235 | 0:1b4780d434ab | 139 | */ |
tmpi5235 | 0:1b4780d434ab | 140 | //R2 |
tmpi5235 | 0:1b4780d434ab | 141 | ecA.setGearRatio(0.50); |
tmpi5235 | 0:1b4780d434ab | 142 | ecB.setGearRatio(0.50); |
tmpi5235 | 0:1b4780d434ab | 143 | ecC.setGearRatio(0.50); |
tmpi5235 | 0:1b4780d434ab | 144 | ecD.setGearRatio(0.50); |
tmpi5235 | 0:1b4780d434ab | 145 | |
tmpi5235 | 0:1b4780d434ab | 146 | |
tmpi5235 | 0:1b4780d434ab | 147 | //角速度とduty比の関係式設定 |
tmpi5235 | 0:1b4780d434ab | 148 | //横軸をタイヤの角速度[rad/s]とduty比とした時の傾きとy切片 |
tmpi5235 | 0:1b4780d434ab | 149 | //正転時の傾き,y切片,逆転時の傾き,y切片 |
tmpi5235 | 0:1b4780d434ab | 150 | /////////R1/////////// |
tmpi5235 | 0:1b4780d434ab | 151 | /*motorA.setEquation(0.009911272,0.008627973,-0.009919857,-0.009919857); |
tmpi5235 | 0:1b4780d434ab | 152 | motorB.setEquation(0.009657781,0.009657781,-0.010037358,-0.010037358); |
tmpi5235 | 0:1b4780d434ab | 153 | motorC.setEquation(0.010134584,0.010134584,-0.010120037,0.014440573); |
tmpi5235 | 0:1b4780d434ab | 154 | motorD.setEquation(0.010158297,0.010158297,-0.010374406,-0.010374406); */ |
tmpi5235 | 0:1b4780d434ab | 155 | //////////R2////////// |
tmpi5235 | 0:1b4780d434ab | 156 | motorA.setEquation(0.009436162,0.009436162,-0.00948947,-0.00948947); |
tmpi5235 | 0:1b4780d434ab | 157 | motorB.setEquation(0.009421729,0.009421729,-0.009465773,-0.009465773); |
tmpi5235 | 0:1b4780d434ab | 158 | motorC.setEquation(0.008833138,0.008833138,-0.009762915,0.058112534); |
tmpi5235 | 0:1b4780d434ab | 159 | motorD.setEquation(0.010214862,0.010214862,-0.009606806,-0.009606806); |
tmpi5235 | 0:1b4780d434ab | 160 | |
tmpi5235 | 0:1b4780d434ab | 161 | //事前にプログラムをマイコンに読み込んでおく |
tmpi5235 | 0:1b4780d434ab | 162 | //緊急停止スイッチで切った状態で電源をつなぎ、マイコンをリセットする |
tmpi5235 | 0:1b4780d434ab | 163 | //改行をしたら緊急停止スイッチを解除する |
tmpi5235 | 0:1b4780d434ab | 164 | printf("\r\n"); |
tmpi5235 | 0:1b4780d434ab | 165 | wait(4); |
tmpi5235 | 0:1b4780d434ab | 166 | //STARTが書かれた時点で開始する |
tmpi5235 | 0:1b4780d434ab | 167 | printf("START\r\n"); |
tmpi5235 | 0:1b4780d434ab | 168 | |
tmpi5235 | 0:1b4780d434ab | 169 | target_speed=-18.0;//ここで目標角速度を設定 最初はduty2の角速度で上手くいきそうならduty4.5の角速度 |
tmpi5235 | 0:1b4780d434ab | 170 | ticker.attach(&motorOut,DELTA_T);//割り込みタイマーをオンにする |
tmpi5235 | 0:1b4780d434ab | 171 | |
tmpi5235 | 0:1b4780d434ab | 172 | wait(3.0);//モーターが割り込みでまわり10秒分のデータを取る |
tmpi5235 | 0:1b4780d434ab | 173 | ticker.detach();//割り込みを止める(止めないとモーターを動かし続けてしまう) |
tmpi5235 | 0:1b4780d434ab | 174 | motorA.stop();//モーターを止める(dutyを0にしている)nn |
tmpi5235 | 0:1b4780d434ab | 175 | motorB.stop(); |
tmpi5235 | 0:1b4780d434ab | 176 | motorC.stop(); |
tmpi5235 | 0:1b4780d434ab | 177 | motorD.stop(); |
tmpi5235 | 0:1b4780d434ab | 178 | displayData();//角速度のデータが表示されるのでエクセルにいれてグラフにする |
tmpi5235 | 0:1b4780d434ab | 179 | printf("\r\n"); |
tmpi5235 | 0:1b4780d434ab | 180 | } |