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 #include "QEI.h" 00003 00004 Timer t; 00005 QEI enc_X(PA_14,PA_13,NC,200,&t,QEI::X4_ENCODING);//NUCLEO-F446REのPINと分解能の指定 00006 QEI enc_Y(PC_3,PC_2,NC,200,&t,QEI::X4_ENCODING);//NUCLEO-F446REのPINと分解能の指定 00007 RawSerial sabertooth1(PC_10,NC,115200);//モータードライバー出力PINの設定 00008 RawSerial sabertooth2(PC_12,NC,115200);//モータードライバー出力PINの設定 00009 Serial pc(USBTX,USBRX,9600);//tera term の準備 00010 Ticker flipper;//タイム関数用 00011 DigitalIn swich(PA_0);//スタートスイッチの入力PINの設定 00012 AnalogIn laser(PA_5);//ボール感知センサーの入力PINの設定 00013 int i = true; 00014 int S; 00015 double kyori_X; 00016 double kyori_Y; 00017 double cnt_X; 00018 double cnt_Y; 00019 double hennsa_X = 0;//偏差値 00020 double hennsa_Y = 0;//偏差値 00021 double I_X = 0;//偏差の積分値 00022 double I_Y = 0;//偏差の積分値 00023 double D_X = 0;//偏差の微分値 00024 double D_Y = 0;//偏差の微分値 00025 double pre_hennsa_X;//Xのひとつ前の偏差 00026 double pre_hennsa_Y;//Yのひとつ前の偏差 00027 double Kp_X = 100;//PID制御のPの係数 00028 double Ki_X = 10;//PID制御のIの係数 00029 double Kd_X = 10;//PID制御のDの係数 00030 double Kp_Y = 100;//PID制御のPの係数 00031 double Ki_Y = 10;//PID制御のIの係数 00032 double Kd_Y = 10;//PID制御のDの係数 00033 double P_X = 0;//PID制御の計算値 00034 double P_Y = 0;//PID制御の計算値 00035 double PW1 = 0;//モーターの力 00036 double PW2 = 0;//モーターの力 00037 double PW3 = 0;//モーターの力 00038 double PW4 = 0;//モーターの力 00039 double dst_Y = 334.5;//X目的地の座標 00040 double dst_X = 600;//Y目的地の座標 00041 double taiyanoennshuu = 150.8; 00042 int bunnkainou = 200; 00043 double ball;//ボールセンサーの値 00044 int exist;//while文の条件のballの値 00045 #define HAZIMARI 1 00046 #define NIKAIME 2 00047 #define HENNKOU 3 00048 #define TEISI 4 00049 int mokuteki = HAZIMARI; 00050 bool flag = false; 00051 00052 void flip()//タイマー割込み 00053 { 00054 kyori_X = taiyanoennshuu * (cnt_X / (bunnkainou * 4)); 00055 kyori_Y = taiyanoennshuu * (cnt_Y / (bunnkainou * 4)); 00056 cnt_X = -1.0 * enc_X.getPulses();//エンコーダーの値の代入 00057 cnt_Y = -1.0 * enc_Y.getPulses();//エンコーダーの値の代入 00058 hennsa_X = dst_X - taiyanoennshuu * (cnt_X / (bunnkainou * 4));//距離の偏差 00059 hennsa_Y = dst_Y - taiyanoennshuu * (cnt_Y / (bunnkainou * 4));//距離の偏差 00060 I_X += (pre_hennsa_X + hennsa_X) * 0.01 / 2;//積分計算 00061 I_Y += (pre_hennsa_Y + hennsa_Y) * 0.01 / 2;//積分計算 00062 D_X = (hennsa_X - pre_hennsa_X) / 0.01 ;//微分計算 00063 D_X = (hennsa_X - pre_hennsa_X) / 0.01 ;//微分計算 00064 pre_hennsa_X = hennsa_X;//一個前の値に設定 00065 pre_hennsa_Y = hennsa_Y;//一個前の値に設定 00066 P_X = Kp_X * hennsa_X / (bunnkainou * 4) + Ki_X * I_X / (bunnkainou * 4) + Kd_X * D_X / (bunnkainou * 4);//PID制御の計算 00067 P_Y = Kp_Y * hennsa_Y / (bunnkainou * 4) + Ki_Y * I_Y / (bunnkainou * 4) + Kd_Y * D_Y / (bunnkainou * 4);//PID制御の計算 00068 PW3 =2.0 * (P_Y - P_X); 00069 PW4 = 2.0 * (P_Y + P_X); 00070 PW1 = -1.0 * 2.0 * (P_Y - P_X); 00071 PW2 = -1.0 * 2.0 * (P_Y + P_X); 00072 if(abs(hennsa_X) <= 10 && abs(hennsa_Y)) { 00073 PW3 = 0; 00074 PW4 = 0; 00075 PW1 = 0; 00076 PW2 = 0; 00077 } 00078 switch(mokuteki) { 00079 case HAZIMARI: 00080 if(abs(hennsa_X) <= 100 && abs(hennsa_Y) <= 100){ 00081 dst_Y = 643.3; 00082 dst_X = 1154; 00083 mokuteki = NIKAIME; 00084 } 00085 case NIKAIME: 00086 if(abs(hennsa_X) <= 10 && abs(hennsa_Y) <= 10) { 00087 dst_X = 2135; 00088 dst_Y = 96.5; 00089 Kp_X = 50; 00090 Ki_X = 5; 00091 Kd_X = 5; 00092 Kp_Y = 125; 00093 Ki_Y = 12.5; 00094 Kd_Y = 10; 00095 mokuteki = HENNKOU; 00096 } 00097 break; 00098 case HENNKOU: 00099 if(abs(hennsa_X) <= 10 && abs(hennsa_Y) <= 10) { 00100 dst_X = 2135; 00101 dst_Y = 437; 00102 Kp_X = 50; 00103 Ki_X = 5; 00104 Kd_X = 5; 00105 Kp_Y = 200; 00106 Ki_Y = 20; 00107 Kd_Y = 10; 00108 mokuteki = TEISI; 00109 } 00110 break; 00111 } 00112 flag = true; 00113 } 00114 int main() 00115 { 00116 while(i) { 00117 S = swich.read(); 00118 pc.printf("%d \r\n",i); 00119 if(S == 0) { 00120 i = false; 00121 } 00122 } 00123 flipper.attach(&flip,0.01); 00124 while(1) { 00125 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); 00126 sabertooth1.printf("M1:%d\r\n",(int)PW1); 00127 sabertooth1.printf("M2:%d\r\n",(int)PW2); 00128 sabertooth2.printf("M1:%d\r\n",(int)PW3); 00129 sabertooth2.printf("M2:%d\r\n",(int)PW4); 00130 flag = false; 00131 00132 } 00133 } 00134
Generated on Fri Sep 23 2022 07:24:04 by
1.7.2