yusei sugimoeo
/
Control_by_localization_test
test
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?
User | Revision | Line number | New 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 | } |