File content as of revision 1:d0d24c45ce32:

#include "mbed.h"
#include "QEI.h"

Timer t;
QEI enc_X(PA_14,PA_13,NC,200,&t,QEI::X4_ENCODING);//NUCLEO-F446REのPINと分解能の指定
QEI enc_Y(PC_3,PC_2,NC,200,&t,QEI::X4_ENCODING);//NUCLEO-F446REのPINと分解能の指定
RawSerial sabertooth1(PC_10,NC,115200);//モータードライバー出力PINの設定
RawSerial sabertooth2(PC_12,NC,115200);//モータードライバー出力PINの設定
Serial pc(USBTX,USBRX,9600);//tera term の準備
Ticker flipper;//タイム関数用
DigitalIn swich(PA_0);//スタートスイッチの入力PINの設定
AnalogIn laser(PA_5);//ボール感知センサーの入力PINの設定
int i = true;
int S;
double kyori_X;
double kyori_Y;
double cnt_X;
double cnt_Y;
double hennsa_X = 0;//偏差値
double hennsa_Y = 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_X = 100;//PID制御のPの係数
double Ki_X = 10;//PID制御のIの係数
double Kd_X = 10;//PID制御のDの係数
double Kp_Y = 100;//PID制御のPの係数
double Ki_Y = 10;//PID制御のIの係数
double Kd_Y = 10;//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_Y = 334.5;//X目的地の座標
double dst_X = 600;//Y目的地の座標
double taiyanoennshuu = 150.8;
int bunnkainou = 200;
double ball;//ボールセンサーの値
int exist;//while文の条件のballの値
#define HAZIMARI 1
#define NIKAIME 2
#define HENNKOU 3
#define TEISI 4
int mokuteki = HAZIMARI;
bool flag = false;

void flip()//タイマー割込み
{
    kyori_X = taiyanoennshuu * (cnt_X / (bunnkainou * 4));
    kyori_Y = taiyanoennshuu * (cnt_Y / (bunnkainou * 4));
    cnt_X = -1.0 * enc_X.getPulses();//エンコーダーの値の代入
    cnt_Y = -1.0 * 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 =  (hennsa_X - pre_hennsa_X) / 0.01 ;//微分計算
    D_X =  (hennsa_X - pre_hennsa_X) / 0.01 ;//微分計算
    pre_hennsa_X = hennsa_X;//一個前の値に設定
    pre_hennsa_Y = hennsa_Y;//一個前の値に設定
    P_X = Kp_X * hennsa_X / (bunnkainou * 4) + Ki_X * I_X / (bunnkainou * 4) + Kd_X * D_X / (bunnkainou * 4);//PID制御の計算
    P_Y = Kp_Y * hennsa_Y / (bunnkainou * 4) + Ki_Y * I_Y / (bunnkainou * 4) + Kd_Y * D_Y / (bunnkainou * 4);//PID制御の計算
    PW3 =2.0 * (P_Y - P_X);
    PW4 = 2.0 * (P_Y + P_X);
    PW1 = -1.0 * 2.0 * (P_Y - P_X);
    PW2 = -1.0 * 2.0 * (P_Y + P_X);
    if(abs(hennsa_X) <= 10 && abs(hennsa_Y)) {
        PW3 = 0;
        PW4 = 0;
        PW1 = 0;
        PW2 = 0;
    }
    switch(mokuteki) {
        case HAZIMARI:
            if(abs(hennsa_X) <= 100 && abs(hennsa_Y) <= 100){
                dst_Y = 643.3;
                dst_X = 1154;
                mokuteki = NIKAIME;
                }  
        case NIKAIME:
            if(abs(hennsa_X) <= 10 && abs(hennsa_Y) <= 10) {
                dst_X = 2135;
                dst_Y = 96.5;
                Kp_X = 50;
                Ki_X = 5;
                Kd_X = 5;
                Kp_Y = 125;
                Ki_Y = 12.5;
                Kd_Y = 10;
                mokuteki = HENNKOU;
            }
            break;
        case HENNKOU:
            if(abs(hennsa_X) <= 10 && abs(hennsa_Y) <= 10) {
                dst_X = 2135;
                dst_Y = 437;
                Kp_X = 50;
                Ki_X = 5;
                Kd_X = 5;
                Kp_Y = 200;
                Ki_Y = 20;
                Kd_Y = 10;
                mokuteki = TEISI;
            }
            break;
    }
    flag = true;
}
int main()
{
    while(i) {
        S = swich.read();
        pc.printf("%d \r\n",i);
        if(S == 0) {
            i = false;
        }
    }
    flipper.attach(&flip,0.01);
    while(1) {
        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);
        sabertooth1.printf("M1:%d\r\n",(int)PW1);
        sabertooth1.printf("M2:%d\r\n",(int)PW2);
        sabertooth2.printf("M1:%d\r\n",(int)PW3);
        sabertooth2.printf("M2:%d\r\n",(int)PW4);
        flag = false;

    }
}