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     }