Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
main.cpp
00001 #include "mbed.h" 00002 00003 Timer t; 00004 QEI enc_X(pinname_A,pinname_B,NC,4000,&t,QEI::X4_ENCODING);//NUCLEO-F446REのPINと分解能の指定 00005 QEI enc_Y(pinname_A,pinname_B,NC,4000,&t,QEI::X4_ENCODING);//NUCLEO-F446REのPINと分解能の指定 00006 00007 Serial pc(USBTX,USBRX,9600);//tera term の準備 00008 Ticker flipper;//タイム関数用 00009 int cnt;//エンコーダの値 00010 RawSerial sabertooth1(pinname_1,NC,115200);//mota_1のモータードライバー出力PINの設定 00011 RawSerial sabertooth1(pinname_2,NC,115200);//mota_2のモータードライバー出力PINの設定 00012 RawSerial sabertooth2(pinname_3,NC,115200);//mota_3のモータードライバー出力PINの設定 00013 RawSerial sabertooth2(pinname_4,NC,115200);//mota_4のモータードライバー出力PINの設定 00014 DIgitalIn swich(pinname);//スタートスイッチの入力PINの設定 00015 AnalogIn laser(pinname);//ボール感知センサーの入力PINの設定 00016 double hennsa = 0;//偏差値 00017 double I_X = 0;//偏差の積分値 00018 double I_Y = 0;//偏差の積分値 00019 double D_X = 0;//偏差の微分値 00020 double D_Y = 0;//偏差の微分値 00021 double pre_hennsa_X;//Xのひとつ前の偏差 00022 double pre_hennsa_Y;//Yのひとつ前の偏差 00023 double Kp = 1.5;//PID制御のPの係数 00024 double Ki = 0.7;//PID制御のIの係数 00025 double Kd = 0.7;//PID制御のDの係数 00026 double P_X = 0;//PID制御の計算値 00027 double P_Y = 0;//PID制御の計算値 00028 double PW1 = 0;//モーターの力 00029 double PW2 = 0;//モーターの力 00030 double PW3 = 0;//モーターの力 00031 double PW4 = 0;//モーターの力 00032 double dst_X;//目的地のX座標 00033 double dst_Y;//目的地のY座標 00034 int ball;//ボールセンサーの値 00035 int exist;//while文の条件のballの値 00036 bool flag = false; 00037 00038 void flip()//タイマー割込み 00039 { 00040 cnt_X = enc_X.getPulses();//エンコーダーの値の代入 00041 cnt_y = enc_Y.getPulses();//エンコーダーの値の代入 00042 hennsa_X = dst_X - taiyanoennshuu * (cnt_X / (bunnkainou * 4));//距離の偏差 00043 hennsa_Y = dst_Y - taiyanoennshuu * (cnt_Y / (bunnkainou * 4));//距離の偏差 00044 I_X += (pre_hennsa_X + hennsa_X) * 0.01 / 2;//積分計算 00045 I_Y += (pre_hennsa_Y + hennsa_Y) * 0.01 / 2;//積分計算 00046 D_X = (pre_hennsa_X - hennsa_X) * 0.01 / 2;//微分計算 00047 D_X = (pre_hennsa_X - hennsa_X) * 0.01 / 2;//微分計算 00048 pre_hennsa_X = hennsa_X;//一個前の値に設定 00049 pre_hennsa_Y = hennsa_Y;//一個前の値に設定 00050 P_X = Kp * hennsa_X / (bunnkanou * 4) + Ki * I_X / (bunnkanou * 4) + Kd * D_X / (bunnkanou * 4);//PID制御の計算 00051 P_Y = Kp * hennsa_Y / (bunnkanou * 4) + Ki * I_Y / (bunnkanou * 4) + Kd * D_Y / (bunnkanou * 4);//PID制御の計算 00052 ball = laser.read(); 00053 if(ball >= 1) { 00054 ball == 1; 00055 } else if(ball < 1) { 00056 ball == 0; 00057 } 00058 flag = true; 00059 } 00060 00061 int main()//出力と方向の指定 00062 { 00063 while(swich.read()) { 00064 flipper.attach(&flip,0.01); 00065 dst_X = 334.5;//目的地座標の設定(PA) 00066 dst_Y = 600;//目的座標の設定(PA) 00067 exist = 1;//while文の条件の設定 00068 //一回目の目的座標の再設定(PB) 00069 if(ball == 1) { 00070 dst_X = 643.3; 00071 dst_Y = 1154; 00072 exist = 0;//while文の条件の再設定 00073 //回収機構が動き終わったらスタートする文 00074 } 00075 //モーターの動き 00076 while(ball == exist) { 00077 if(flag) { 00078 //移動の出力設定 00079 if(abs(hennsa_X) > 1 && abs(hennsa_Y) > 1) { 00080 PW1 = P_Y - P_X; 00081 PW2 = P_Y + P_X; 00082 PW3 = -1.0 * (P_Y - P_X); 00083 PW4 = -1.0 * (P_Y + P_X); 00084 //二回目の目的座標の再設定(PC) 00085 } else if(dst_X = 643.3 && dst_Y = 1154 && abs(hennsa_X) <= 1 && abs(hennsa_Y) <= 1) { 00086 dst_X = 96.5; 00087 dst_Y = 2135; 00088 PW13 = 0; 00089 PW24 = 0; 00090 //三回目の目的座標の再設定(PD) 00091 } else if(dst_X = 96.5 && dst_Y = 2135 && abs(hennsa_X) <= 1 && abs(hennsa_Y) <= 1) { 00092 dst_X = 437; 00093 dst_Y = 2135; 00094 PW13 = 0; 00095 PW24 = 0; 00096 } 00097 pc.printf("%d %lf \r\n",cnt,P); 00098 //モーターの出力 00099 sabertooth1.printf("M1:%d\r\n",PW1); 00100 sabertooth1.printf("M2:%d\r\n",PW2); 00101 sabertooth2.printf("M1:%d\r\n",PW3); 00102 sabertooth2.printf("M2:%d\r\n",PW4); 00103 flag = false; 00104 00105 } 00106 } 00107 }
Generated on Sat Sep 17 2022 08:11:56 by
1.7.2