File content as of revision 0:4d2cd14e1e7e:

#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;

            }
        }
    }