yusei sugimoeo / Mbed 2 deprecated Control_by_localization_test

test

Dependencies:   mbed

main.cpp@0:4d2cd14e1e7e, 2022-09-17 (annotated)

Committer:
yuseis
Date:
Sat Sep 17 08:10:03 2022 +0000
Revision:
0:4d2cd14e1e7e
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuseis 0:4d2cd14e1e7e 1 #include "mbed.h"
yuseis 0:4d2cd14e1e7e 2
yuseis 0:4d2cd14e1e7e 3 Timer t;
yuseis 0:4d2cd14e1e7e 4 QEI enc_X(pinname_A,pinname_B,NC,4000,&t,QEI::X4_ENCODING);//NUCLEO-F446REのPINと分解能の指定
yuseis 0:4d2cd14e1e7e 5 QEI enc_Y(pinname_A,pinname_B,NC,4000,&t,QEI::X4_ENCODING);//NUCLEO-F446REのPINと分解能の指定
yuseis 0:4d2cd14e1e7e 6
yuseis 0:4d2cd14e1e7e 7 Serial pc(USBTX,USBRX,9600);//tera term の準備
yuseis 0:4d2cd14e1e7e 8 Ticker flipper;//タイム関数用
yuseis 0:4d2cd14e1e7e 9 int cnt;//エンコーダの値
yuseis 0:4d2cd14e1e7e 10 RawSerial sabertooth1(pinname_1,NC,115200);//mota_1のモータードライバー出力PINの設定
yuseis 0:4d2cd14e1e7e 11 RawSerial sabertooth1(pinname_2,NC,115200);//mota_2のモータードライバー出力PINの設定
yuseis 0:4d2cd14e1e7e 12 RawSerial sabertooth2(pinname_3,NC,115200);//mota_3のモータードライバー出力PINの設定
yuseis 0:4d2cd14e1e7e 13 RawSerial sabertooth2(pinname_4,NC,115200);//mota_4のモータードライバー出力PINの設定
yuseis 0:4d2cd14e1e7e 14 DIgitalIn swich(pinname);//スタートスイッチの入力PINの設定
yuseis 0:4d2cd14e1e7e 15 AnalogIn laser(pinname);//ボール感知センサーの入力PINの設定
yuseis 0:4d2cd14e1e7e 16 double hennsa = 0;//偏差値
yuseis 0:4d2cd14e1e7e 17 double I_X = 0;//偏差の積分値
yuseis 0:4d2cd14e1e7e 18 double I_Y = 0;//偏差の積分値
yuseis 0:4d2cd14e1e7e 19 double D_X = 0;//偏差の微分値
yuseis 0:4d2cd14e1e7e 20 double D_Y = 0;//偏差の微分値
yuseis 0:4d2cd14e1e7e 21 double pre_hennsa_X;//Xのひとつ前の偏差
yuseis 0:4d2cd14e1e7e 22 double pre_hennsa_Y;//Yのひとつ前の偏差
yuseis 0:4d2cd14e1e7e 23 double Kp = 1.5;//PID制御のPの係数
yuseis 0:4d2cd14e1e7e 24 double Ki = 0.7;//PID制御のIの係数
yuseis 0:4d2cd14e1e7e 25 double Kd = 0.7;//PID制御のDの係数
yuseis 0:4d2cd14e1e7e 26 double P_X = 0;//PID制御の計算値
yuseis 0:4d2cd14e1e7e 27 double P_Y = 0;//PID制御の計算値
yuseis 0:4d2cd14e1e7e 28 double PW1 = 0;//モーターの力
yuseis 0:4d2cd14e1e7e 29 double PW2 = 0;//モーターの力
yuseis 0:4d2cd14e1e7e 30 double PW3 = 0;//モーターの力
yuseis 0:4d2cd14e1e7e 31 double PW4 = 0;//モーターの力
yuseis 0:4d2cd14e1e7e 32 double dst_X;//目的地のX座標
yuseis 0:4d2cd14e1e7e 33 double dst_Y;//目的地のY座標
yuseis 0:4d2cd14e1e7e 34 int ball;//ボールセンサーの値
yuseis 0:4d2cd14e1e7e 35 int exist;//while文の条件のballの値
yuseis 0:4d2cd14e1e7e 36 bool flag = false;
yuseis 0:4d2cd14e1e7e 37
yuseis 0:4d2cd14e1e7e 38 void flip()//タイマー割込み
yuseis 0:4d2cd14e1e7e 39 {
yuseis 0:4d2cd14e1e7e 40 cnt_X = enc_X.getPulses();//エンコーダーの値の代入
yuseis 0:4d2cd14e1e7e 41 cnt_y = enc_Y.getPulses();//エンコーダーの値の代入
yuseis 0:4d2cd14e1e7e 42 hennsa_X = dst_X - taiyanoennshuu * (cnt_X / (bunnkainou * 4));//距離の偏差
yuseis 0:4d2cd14e1e7e 43 hennsa_Y = dst_Y - taiyanoennshuu * (cnt_Y / (bunnkainou * 4));//距離の偏差
yuseis 0:4d2cd14e1e7e 44 I_X += (pre_hennsa_X + hennsa_X) * 0.01 / 2;//積分計算
yuseis 0:4d2cd14e1e7e 45 I_Y += (pre_hennsa_Y + hennsa_Y) * 0.01 / 2;//積分計算
yuseis 0:4d2cd14e1e7e 46 D_X = (pre_hennsa_X - hennsa_X) * 0.01 / 2;//微分計算
yuseis 0:4d2cd14e1e7e 47 D_X = (pre_hennsa_X - hennsa_X) * 0.01 / 2;//微分計算
yuseis 0:4d2cd14e1e7e 48 pre_hennsa_X = hennsa_X;//一個前の値に設定
yuseis 0:4d2cd14e1e7e 49 pre_hennsa_Y = hennsa_Y;//一個前の値に設定
yuseis 0:4d2cd14e1e7e 50 P_X = Kp * hennsa_X / (bunnkanou * 4) + Ki * I_X / (bunnkanou * 4) + Kd * D_X / (bunnkanou * 4);//PID制御の計算
yuseis 0:4d2cd14e1e7e 51 P_Y = Kp * hennsa_Y / (bunnkanou * 4) + Ki * I_Y / (bunnkanou * 4) + Kd * D_Y / (bunnkanou * 4);//PID制御の計算
yuseis 0:4d2cd14e1e7e 52 ball = laser.read();
yuseis 0:4d2cd14e1e7e 53 if(ball >= 1) {
yuseis 0:4d2cd14e1e7e 54 ball == 1;
yuseis 0:4d2cd14e1e7e 55 } else if(ball < 1) {
yuseis 0:4d2cd14e1e7e 56 ball == 0;
yuseis 0:4d2cd14e1e7e 57 }
yuseis 0:4d2cd14e1e7e 58 flag = true;
yuseis 0:4d2cd14e1e7e 59 }
yuseis 0:4d2cd14e1e7e 60
yuseis 0:4d2cd14e1e7e 61 int main()//出力と方向の指定
yuseis 0:4d2cd14e1e7e 62 {
yuseis 0:4d2cd14e1e7e 63 while(swich.read()) {
yuseis 0:4d2cd14e1e7e 64 flipper.attach(&flip,0.01);
yuseis 0:4d2cd14e1e7e 65 dst_X = 334.5;//目的地座標の設定(PA)
yuseis 0:4d2cd14e1e7e 66 dst_Y = 600;//目的座標の設定(PA)
yuseis 0:4d2cd14e1e7e 67 exist = 1;//while文の条件の設定
yuseis 0:4d2cd14e1e7e 68 //一回目の目的座標の再設定(PB)
yuseis 0:4d2cd14e1e7e 69 if(ball == 1) {
yuseis 0:4d2cd14e1e7e 70 dst_X = 643.3;
yuseis 0:4d2cd14e1e7e 71 dst_Y = 1154;
yuseis 0:4d2cd14e1e7e 72 exist = 0;//while文の条件の再設定
yuseis 0:4d2cd14e1e7e 73 //回収機構が動き終わったらスタートする文
yuseis 0:4d2cd14e1e7e 74 }
yuseis 0:4d2cd14e1e7e 75 //モーターの動き
yuseis 0:4d2cd14e1e7e 76 while(ball == exist) {
yuseis 0:4d2cd14e1e7e 77 if(flag) {
yuseis 0:4d2cd14e1e7e 78 //移動の出力設定
yuseis 0:4d2cd14e1e7e 79 if(abs(hennsa_X) > 1 && abs(hennsa_Y) > 1) {
yuseis 0:4d2cd14e1e7e 80 PW1 = P_Y - P_X;
yuseis 0:4d2cd14e1e7e 81 PW2 = P_Y + P_X;
yuseis 0:4d2cd14e1e7e 82 PW3 = -1.0 * (P_Y - P_X);
yuseis 0:4d2cd14e1e7e 83 PW4 = -1.0 * (P_Y + P_X);
yuseis 0:4d2cd14e1e7e 84 //二回目の目的座標の再設定(PC)
yuseis 0:4d2cd14e1e7e 85 } else if(dst_X = 643.3 && dst_Y = 1154 && abs(hennsa_X) <= 1 && abs(hennsa_Y) <= 1) {
yuseis 0:4d2cd14e1e7e 86 dst_X = 96.5;
yuseis 0:4d2cd14e1e7e 87 dst_Y = 2135;
yuseis 0:4d2cd14e1e7e 88 PW13 = 0;
yuseis 0:4d2cd14e1e7e 89 PW24 = 0;
yuseis 0:4d2cd14e1e7e 90 //三回目の目的座標の再設定(PD)
yuseis 0:4d2cd14e1e7e 91 } else if(dst_X = 96.5 && dst_Y = 2135 && abs(hennsa_X) <= 1 && abs(hennsa_Y) <= 1) {
yuseis 0:4d2cd14e1e7e 92 dst_X = 437;
yuseis 0:4d2cd14e1e7e 93 dst_Y = 2135;
yuseis 0:4d2cd14e1e7e 94 PW13 = 0;
yuseis 0:4d2cd14e1e7e 95 PW24 = 0;
yuseis 0:4d2cd14e1e7e 96 }
yuseis 0:4d2cd14e1e7e 97 pc.printf("%d %lf \r\n",cnt,P);
yuseis 0:4d2cd14e1e7e 98 //モーターの出力
yuseis 0:4d2cd14e1e7e 99 sabertooth1.printf("M1:%d\r\n",PW1);
yuseis 0:4d2cd14e1e7e 100 sabertooth1.printf("M2:%d\r\n",PW2);
yuseis 0:4d2cd14e1e7e 101 sabertooth2.printf("M1:%d\r\n",PW3);
yuseis 0:4d2cd14e1e7e 102 sabertooth2.printf("M2:%d\r\n",PW4);
yuseis 0:4d2cd14e1e7e 103 flag = false;
yuseis 0:4d2cd14e1e7e 104
yuseis 0:4d2cd14e1e7e 105 }
yuseis 0:4d2cd14e1e7e 106 }
yuseis 0:4d2cd14e1e7e 107 }