Yuki Nakamura / Mbed 2 deprecated 2023_harurobo_2han_asimawari

2023春ロボ2班 足回りコード

Dependencies:   CalPID_2022 Encoder_2022 MPU6050 Madgwickfilter MotorController_2022 mbed

main.cpp@3:7c29db78fa24, 2022-10-27 (annotated)

Committer:
yn_4009
Date:
Thu Oct 27 08:16:01 2022 +0000
Revision:
3:7c29db78fa24
Parent:
2:f1e26c8084f2
Child:
4:fc9688f7614a
test

Who changed what in which revision?

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