yusei sugimoeo / Mbed 2 deprecated Calibration_motor_copy

case

Dependencies:   QEI2 mbed

main.cpp@0:02e4c332f5b4, 2022-09-22 (annotated)

Committer:
yuseis
Date:
Thu Sep 22 11:45:34 2022 +0000
Revision:
0:02e4c332f5b4
Child:
1:d0d24c45ce32
case

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuseis 0:02e4c332f5b4 1 #include "mbed.h"
yuseis 0:02e4c332f5b4 2 #include "QEI.h"
yuseis 0:02e4c332f5b4 3
yuseis 0:02e4c332f5b4 4 Timer t;
yuseis 0:02e4c332f5b4 5 QEI enc_X(PA_14,PA_13,NC,200,&t,QEI::X4_ENCODING);//NUCLEO-F446REのPINと分解能の指定
yuseis 0:02e4c332f5b4 6 QEI enc_Y(PC_3,PC_2,NC,200,&t,QEI::X4_ENCODING);//NUCLEO-F446REのPINと分解能の指定
yuseis 0:02e4c332f5b4 7 RawSerial sabertooth1(PC_10,NC,115200);//モータードライバー出力PINの設定
yuseis 0:02e4c332f5b4 8 RawSerial sabertooth2(PC_12,NC,115200);//モータードライバー出力PINの設定
yuseis 0:02e4c332f5b4 9 Serial pc(USBTX,USBRX,9600);//tera term の準備
yuseis 0:02e4c332f5b4 10 Ticker flipper;//タイム関数用
yuseis 0:02e4c332f5b4 11 DigitalIn swich(PA_0);//スタートスイッチの入力PINの設定
yuseis 0:02e4c332f5b4 12 AnalogIn laser(PA_5);//ボール感知センサーの入力PINの設定
yuseis 0:02e4c332f5b4 13 int i = true;
yuseis 0:02e4c332f5b4 14 int S;
yuseis 0:02e4c332f5b4 15 double kyori_X;
yuseis 0:02e4c332f5b4 16 double kyori_Y;
yuseis 0:02e4c332f5b4 17 double cnt_X;
yuseis 0:02e4c332f5b4 18 double cnt_Y;
yuseis 0:02e4c332f5b4 19 double hennsa_X = 0;//偏差値
yuseis 0:02e4c332f5b4 20 double hennsa_Y = 0;//偏差値
yuseis 0:02e4c332f5b4 21 double I_X = 0;//偏差の積分値
yuseis 0:02e4c332f5b4 22 double I_Y = 0;//偏差の積分値
yuseis 0:02e4c332f5b4 23 double D_X = 0;//偏差の微分値
yuseis 0:02e4c332f5b4 24 double D_Y = 0;//偏差の微分値
yuseis 0:02e4c332f5b4 25 double pre_hennsa_X;//Xのひとつ前の偏差
yuseis 0:02e4c332f5b4 26 double pre_hennsa_Y;//Yのひとつ前の偏差
yuseis 0:02e4c332f5b4 27 double Kp = 250;//PID制御のPの係数
yuseis 0:02e4c332f5b4 28 double Ki = 10;//PID制御のIの係数
yuseis 0:02e4c332f5b4 29 double Kd = 10;//PID制御のDの係数
yuseis 0:02e4c332f5b4 30 double P_X = 0;//PID制御の計算値
yuseis 0:02e4c332f5b4 31 double P_Y = 0;//PID制御の計算値
yuseis 0:02e4c332f5b4 32 double PW1 = 0;//モーターの力
yuseis 0:02e4c332f5b4 33 double PW2 = 0;//モーターの力
yuseis 0:02e4c332f5b4 34 double PW3 = 0;//モーターの力
yuseis 0:02e4c332f5b4 35 double PW4 = 0;//モーターの力
yuseis 0:02e4c332f5b4 36 double dst_Y = 1154;//目的地の座標
yuseis 0:02e4c332f5b4 37 double dst_X = 643.3;//目的地の座標
yuseis 0:02e4c332f5b4 38 double taiyanoennshuu = 150.8;
yuseis 0:02e4c332f5b4 39 int bunnkainou = 200;
yuseis 0:02e4c332f5b4 40 double ball;//ボールセンサーの値
yuseis 0:02e4c332f5b4 41 int exist;//while文の条件のballの値
yuseis 0:02e4c332f5b4 42 #define HAZIMARI 1
yuseis 0:02e4c332f5b4 43 #define HENNKOU 2
yuseis 0:02e4c332f5b4 44 #define TENNKAI 3
yuseis 0:02e4c332f5b4 45 bool flag = false;
yuseis 0:02e4c332f5b4 46 int mokuteki = HENNKOU;
yuseis 0:02e4c332f5b4 47
yuseis 0:02e4c332f5b4 48 void flip()//タイマー割込み
yuseis 0:02e4c332f5b4 49 {
yuseis 0:02e4c332f5b4 50 kyori_X = taiyanoennshuu * (cnt_X / (bunnkainou * 4));
yuseis 0:02e4c332f5b4 51 kyori_Y = taiyanoennshuu * (cnt_Y / (bunnkainou * 4));
yuseis 0:02e4c332f5b4 52 cnt_X = -1.0 * enc_X.getPulses();//エンコーダーの値の代入
yuseis 0:02e4c332f5b4 53 cnt_Y = -1.0 * enc_Y.getPulses();//エンコーダーの値の代入
yuseis 0:02e4c332f5b4 54 hennsa_X = dst_X - taiyanoennshuu * (cnt_X / (bunnkainou * 4));//距離の偏差
yuseis 0:02e4c332f5b4 55 hennsa_Y = dst_Y - taiyanoennshuu * (cnt_Y / (bunnkainou * 4));//距離の偏差
yuseis 0:02e4c332f5b4 56 I_X += (pre_hennsa_X + hennsa_X) * 0.01 / 2;//積分計算
yuseis 0:02e4c332f5b4 57 I_Y += (pre_hennsa_Y + hennsa_Y) * 0.01 / 2;//積分計算
yuseis 0:02e4c332f5b4 58 D_X = (pre_hennsa_X - hennsa_X) * 0.01 / 2;//微分計算
yuseis 0:02e4c332f5b4 59 D_X = (pre_hennsa_X - hennsa_X) * 0.01 / 2;//微分計算
yuseis 0:02e4c332f5b4 60 pre_hennsa_X = hennsa_X;//一個前の値に設定
yuseis 0:02e4c332f5b4 61 pre_hennsa_Y = hennsa_Y;//一個前の値に設定
yuseis 0:02e4c332f5b4 62 P_X = Kp * hennsa_X / (bunnkainou * 4) + Ki * I_X / (bunnkainou * 4) + Kd * D_X / (bunnkainou * 4);//PID制御の計算
yuseis 0:02e4c332f5b4 63 P_Y = Kp * hennsa_Y / (bunnkainou * 4) + Ki * I_Y / (bunnkainou * 4) + Kd * D_Y / (bunnkainou * 4);//PID制御の計算
yuseis 0:02e4c332f5b4 64 PW3 = 1.5 * (P_Y - P_X);
yuseis 0:02e4c332f5b4 65 PW4 = 1.5 * (P_Y + P_X);
yuseis 0:02e4c332f5b4 66 PW1 = -1.0 * 1.5 * (P_Y - P_X);
yuseis 0:02e4c332f5b4 67 PW2 = -1.0 * 1.5 * (P_Y + P_X);
yuseis 0:02e4c332f5b4 68 flag = true;
yuseis 0:02e4c332f5b4 69 }
yuseis 0:02e4c332f5b4 70 int main()
yuseis 0:02e4c332f5b4 71 {
yuseis 0:02e4c332f5b4 72 flipper.attach(&flip,0.01);
yuseis 0:02e4c332f5b4 73 while(i) {
yuseis 0:02e4c332f5b4 74 S = swich.read();
yuseis 0:02e4c332f5b4 75 pc.printf("%d \r\n",i);
yuseis 0:02e4c332f5b4 76 if(S == 0) {
yuseis 0:02e4c332f5b4 77 i = false;
yuseis 0:02e4c332f5b4 78 }
yuseis 0:02e4c332f5b4 79 }
yuseis 0:02e4c332f5b4 80 flipper.attach(&flip,0.01);
yuseis 0:02e4c332f5b4 81 while(1) {
yuseis 0:02e4c332f5b4 82 switch(mokuteki) {
yuseis 0:02e4c332f5b4 83 case HAZIMARI:
yuseis 0:02e4c332f5b4 84 dst_X = 643.3;
yuseis 0:02e4c332f5b4 85 dst_Y = 1154;
yuseis 0:02e4c332f5b4 86 if(abs(hennsa_X) <= 1 && abs(hennsa_Y) <= 1) {
yuseis 0:02e4c332f5b4 87 mokuteki = HENNKOU;
yuseis 0:02e4c332f5b4 88 }
yuseis 0:02e4c332f5b4 89 break;
yuseis 0:02e4c332f5b4 90 case HENNKOU:
yuseis 0:02e4c332f5b4 91 dst_X = 96.5;
yuseis 0:02e4c332f5b4 92 dst_Y = 2135;
yuseis 0:02e4c332f5b4 93 if(abs(hennsa_X) <= 1 && abs(hennsa_Y) <= 1) {
yuseis 0:02e4c332f5b4 94 mokuteki = TENNKAI;
yuseis 0:02e4c332f5b4 95 }
yuseis 0:02e4c332f5b4 96 break;
yuseis 0:02e4c332f5b4 97 case TENNKAI:
yuseis 0:02e4c332f5b4 98 dst_X = 437;
yuseis 0:02e4c332f5b4 99 dst_Y = 2135;
yuseis 0:02e4c332f5b4 100 break;
yuseis 0:02e4c332f5b4 101 }
yuseis 0:02e4c332f5b4 102 pc.printf("%d %lf %lf %lf %lf %lf %lf %lf %lf \r\n",i,kyori_X,kyori_Y,P_X,P_Y,PW1,PW2,PW3,PW4);
yuseis 0:02e4c332f5b4 103 sabertooth1.printf("M1:%d\r\n",(int)PW1);
yuseis 0:02e4c332f5b4 104 sabertooth1.printf("M2:%d\r\n",(int)PW2);
yuseis 0:02e4c332f5b4 105 sabertooth2.printf("M1:%d\r\n",(int)PW3);
yuseis 0:02e4c332f5b4 106 sabertooth2.printf("M2:%d\r\n",(int)PW4);
yuseis 0:02e4c332f5b4 107 flag = false;
yuseis 0:02e4c332f5b4 108
yuseis 0:02e4c332f5b4 109 }
yuseis 0:02e4c332f5b4 110 }
yuseis 0:02e4c332f5b4 111