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