File content as of revision 0:620b24ebccae:

#include "mbed.h"
#include "BusSerial.h"
#include "MotorControler.h"
#include "PID.h"
#include "QEI.h"
#include "TetraOmni.h"

//ハード定数
const int PPR = 250;
const double MaxRPS = 12;

//ソフト定数
const uint8_t HeadNum = 250;                        //基板ID
const double Kp=0.05,Ki=0.058,Kd=0.005;             //ホイール速度PID
const double MoveRatio=0.7,RotationRatio=0.3;       //移動と回転の比
const double DiffRange = 0.5;                       //Fake目標値の真目標への変化範囲
const double StopThresholdRPS = 1.5;                //目標値が0の場合，このRPSなら一瞬で出力を0にする
const int TargetRPSStage = 100;                      //目標値の偏差を分割する個数

//変数

//インスタンス生成
Timer timer;
Ticker ticker0,ticker1,ticker2,ticker3;
BusSerial BS(PA_0,PA_1,&timer);
PID MOT_PID[4]= {PID(&timer),PID(&timer),PID(&timer),PID(&timer)};
TetraOmni Omni = TetraOmni(MoveRatio,RotationRatio,MaxRPS,DiffRange);
MotorControler MOT[4]= {MotorControler( PA_8, PC_6, PA_7),MotorControler( PA_9, PC_7, PA_7),
                        MotorControler( PA_10, PC_8, PA_7),MotorControler( PA_11, PC_9, PA_7)
                       };
QEI ENC[4]= {QEI(PB_2, PA_6,PPR,&timer,&ticker0), QEI( PB_5, PB_4,PPR,&timer,&ticker1),
             QEI(PB_8, PB_7,PPR,&timer,&ticker2), QEI( PB_13, PB_12,PPR,&timer,&ticker3)
            };


//プロトタイプ宣言
void classSetting();

//ピン設定
DigitalOut RBG[3]= {DigitalOut(PA_15),DigitalOut(PB_0),DigitalOut(PA_4)};

int main()
{
    printf("boot\n");
    //クラス一括設定
    classSetting();

    for(int i=0; i<4; i++)MOT[i].setSpeed(0);  //初期モーター速度は1にしとく


    for(int i =0; i<3; i++) {
        RBG[i]=1;
        wait(1);
        RBG[i]=0;
    }
    RBG[0]=1;


    while(1) {
        //値直接入力によるデバッグ部分
        //uint8_t RecieveData[5] = {250,0,0,0,0b10000};

        //データ受信
        uint8_t RecieveData[5];
        BS.GetBusSerial(&RecieveData[0],HeadNum,5,100);

        //***データがちゃんと受信できた場合***
        if(RecieveData[0]==HeadNum) {
            //受信データ処理
            double MoveDirection = RecieveData[1]*2;
            double YawDeg = (int)RecieveData[2] << 8 | (int)RecieveData[3];
            bool StickBool = RecieveData[4] >> 4 & 0b1;
            int SpeedBool = RecieveData[4] >> 2 & 0b11;;
            int RotBool = RecieveData[4] & 0b11;

            //オムニターゲット速度計算
            Omni.wheelTGTRPScalc(StickBool,MoveDirection,YawDeg,RotBool,SpeedBool);
            Omni.FakeRPScalc();

            //モーター回転
            for(int i =0; i<4; i++) {
                double ThisWheelRPS = ENC[i].getRPS();
                if(Omni.Wheel_FakeTGTRPS(i)==0 && fabs(ThisWheelRPS)<StopThresholdRPS) MOT[i].setSpeed(0);
                else MOT[i].setSpeed(MOT_PID[i].controlPID(Omni.Wheel_FakeTGTRPS(i),ThisWheelRPS));

            }

            //デバッグエリア
            //for(int i =0; i<5; i++)   printf("%d  ",RecieveData[i]); //受信データ確認
            //for(int i =0; i<4; i++) printf("%.2f  ",ENC[i].getRPS());//ホイールの現在のPRS確認
            //for(int i =0; i<4; i++) printf("%.2f  ",Omni.Wheel_TrueTGTRPS(i));//ホイールの真目標のPRS確認
            //for(int i =0; i<4; i++) printf("%.2f  ",Omni.Wheel_FakeTGTRPS(i));//ホイールの偽目標のPRS確認

            //for(int i =0; i<4; i++) printf("%.2f -> %.2f  | ",ENC[i].getRPS(),Omni.Wheel_TrueTGTRPS(i));//ホイールの現在のPRS確認
            //printf("\n");//改行
        }
    }
}



/*********************************************
**********************************************
******************関数*************************
**********************************************
**********************************************/

void classSetting()
{
    //Omni.SpeedStageSetting(20,35,50);
    for(int i =0; i<4; i++) {
        //MotorControler設定
        MOT[i].setMotorDirection(CW); //setspeedの正の値の回転方向を決める．CWでDIR=0,CCWでDIR=1の動き
        MOT[i].setPwmFrequency(20000);  //PWM周波数
        MOT[i].enableDriver();  //モータ起動

        //QEI設定
        ENC[i].rotReverce();
        ENC[i].useAvePRS();  //ticker使用

        //***PID設定***
        MOT_PID[i].setParameter(Kp,Ki,Kd);
    }
    Omni.TGTRPS_SwitchingStage = TargetRPSStage;//デフォはclass中で75
}