Yuki Nakamura / Mbed 2 deprecated 2023_harurobo_2han_asimawari

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?

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