Yuki Nakamura / Mbed 2 deprecated 2023_harurobo_2han_asimawari

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

Dependencies:   CalPID_2022 Encoder_2022 MPU6050 Madgwickfilter MotorController_2022 mbed

main.cpp@0:1b4780d434ab, 2022-10-27 (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?

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の順に調整
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 //ギア比　タイヤ:エンコーダ＝１: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 }