yusei sugimoeo
/
Control_by_localization_test
test
Revision 0:4d2cd14e1e7e, committed 2022-09-17
- Comitter:
- yuseis
- Date:
- Sat Sep 17 08:10:03 2022 +0000
- Commit message:
- test
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
mbed.bld | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat Sep 17 08:10:03 2022 +0000 @@ -0,0 +1,107 @@ +#include "mbed.h" + +Timer t; +QEI enc_X(pinname_A,pinname_B,NC,4000,&t,QEI::X4_ENCODING);//NUCLEO-F446REのPINと分解能の指定 +QEI enc_Y(pinname_A,pinname_B,NC,4000,&t,QEI::X4_ENCODING);//NUCLEO-F446REのPINと分解能の指定 + +Serial pc(USBTX,USBRX,9600);//tera term の準備 +Ticker flipper;//タイム関数用 +int cnt;//エンコーダの値 +RawSerial sabertooth1(pinname_1,NC,115200);//mota_1のモータードライバー出力PINの設定 +RawSerial sabertooth1(pinname_2,NC,115200);//mota_2のモータードライバー出力PINの設定 +RawSerial sabertooth2(pinname_3,NC,115200);//mota_3のモータードライバー出力PINの設定 +RawSerial sabertooth2(pinname_4,NC,115200);//mota_4のモータードライバー出力PINの設定 +DIgitalIn swich(pinname);//スタートスイッチの入力PINの設定 +AnalogIn laser(pinname);//ボール感知センサーの入力PINの設定 +double hennsa = 0;//偏差値 +double I_X = 0;//偏差の積分値 +double I_Y = 0;//偏差の積分値 +double D_X = 0;//偏差の微分値 +double D_Y = 0;//偏差の微分値 +double pre_hennsa_X;//Xのひとつ前の偏差 +double pre_hennsa_Y;//Yのひとつ前の偏差 +double Kp = 1.5;//PID制御のPの係数 +double Ki = 0.7;//PID制御のIの係数 +double Kd = 0.7;//PID制御のDの係数 +double P_X = 0;//PID制御の計算値 +double P_Y = 0;//PID制御の計算値 +double PW1 = 0;//モーターの力 +double PW2 = 0;//モーターの力 +double PW3 = 0;//モーターの力 +double PW4 = 0;//モーターの力 +double dst_X;//目的地のX座標 +double dst_Y;//目的地のY座標 +int ball;//ボールセンサーの値 +int exist;//while文の条件のballの値 +bool flag = false; + +void flip()//タイマー割込み +{ + cnt_X = enc_X.getPulses();//エンコーダーの値の代入 + cnt_y = enc_Y.getPulses();//エンコーダーの値の代入 + hennsa_X = dst_X - taiyanoennshuu * (cnt_X / (bunnkainou * 4));//距離の偏差 + hennsa_Y = dst_Y - taiyanoennshuu * (cnt_Y / (bunnkainou * 4));//距離の偏差 + I_X += (pre_hennsa_X + hennsa_X) * 0.01 / 2;//積分計算 + I_Y += (pre_hennsa_Y + hennsa_Y) * 0.01 / 2;//積分計算 + D_X = (pre_hennsa_X - hennsa_X) * 0.01 / 2;//微分計算 + D_X = (pre_hennsa_X - hennsa_X) * 0.01 / 2;//微分計算 + pre_hennsa_X = hennsa_X;//一個前の値に設定 + pre_hennsa_Y = hennsa_Y;//一個前の値に設定 + P_X = Kp * hennsa_X / (bunnkanou * 4) + Ki * I_X / (bunnkanou * 4) + Kd * D_X / (bunnkanou * 4);//PID制御の計算 + P_Y = Kp * hennsa_Y / (bunnkanou * 4) + Ki * I_Y / (bunnkanou * 4) + Kd * D_Y / (bunnkanou * 4);//PID制御の計算 + ball = laser.read(); + if(ball >= 1) { + ball == 1; + } else if(ball < 1) { + ball == 0; + } + flag = true; +} + +int main()//出力と方向の指定 +{ + while(swich.read()) { + flipper.attach(&flip,0.01); + dst_X = 334.5;//目的地座標の設定(PA) + dst_Y = 600;//目的座標の設定(PA) + exist = 1;//while文の条件の設定 + //一回目の目的座標の再設定(PB) + if(ball == 1) { + dst_X = 643.3; + dst_Y = 1154; + exist = 0;//while文の条件の再設定 + //回収機構が動き終わったらスタートする文 + } + //モーターの動き + while(ball == exist) { + if(flag) { + //移動の出力設定 + if(abs(hennsa_X) > 1 && abs(hennsa_Y) > 1) { + PW1 = P_Y - P_X; + PW2 = P_Y + P_X; + PW3 = -1.0 * (P_Y - P_X); + PW4 = -1.0 * (P_Y + P_X); + //二回目の目的座標の再設定(PC) + } else if(dst_X = 643.3 && dst_Y = 1154 && abs(hennsa_X) <= 1 && abs(hennsa_Y) <= 1) { + dst_X = 96.5; + dst_Y = 2135; + PW13 = 0; + PW24 = 0; + //三回目の目的座標の再設定(PD) + } else if(dst_X = 96.5 && dst_Y = 2135 && abs(hennsa_X) <= 1 && abs(hennsa_Y) <= 1) { + dst_X = 437; + dst_Y = 2135; + PW13 = 0; + PW24 = 0; + } + pc.printf("%d %lf \r\n",cnt,P); + //モーターの出力 + sabertooth1.printf("M1:%d\r\n",PW1); + sabertooth1.printf("M2:%d\r\n",PW2); + sabertooth2.printf("M1:%d\r\n",PW3); + sabertooth2.printf("M2:%d\r\n",PW4); + flag = false; + + } + } + } \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sat Sep 17 08:10:03 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file