File content as of revision 1:e6a7e0b92032:

/// @file  main.cpp
/// @brief ロボカップラジコンのプログラム
/// 
/// @note トグルは上が１で下が０
///
/// @details 右端トグルで速度切り替え
/// @note １：速め　０：遅め
///
/// @details 中央トグル２つでモード切替
/// @note 中央トグルでのモード切替;
///       左０右０：スタート
///       左１右０：キッカーテスト
///       左０右１：ドリブラーテスト
///       左１右１：オムニテスト
/// 
/// 
/// 
/// @attention １番機は起動後にArduinoのリセットボタンを押す必要あり
/// 

//--------操作方法--------


#include "Master.h"
#include "jy901.h"
#include "PID.h"
#include "SerialArduino.h"

DigitalOut led(LED1);
Master Master;// 出力等の基本クラス
OmniWheel omni(4);
SerialArduino mini(A0, A1, 115200);// Arduinoを用いてPS3コントローラと通信するクラス
JY901 jy(jysda, jyscl);
#ifdef RobotNumberIsOne
PID Rotate(6.0, 50.0, 0.0005, 0.01);
#else
PID Rotate(5.0, 50.0, 0.0005, 0.01);
#endif
Timer Time_Shot;

void PS3get(bool flag_PS3print);

uint8_t h1,h2;
bool    b[12]= {};
uint8_t st[4]= {};
uint8_t tr[2]= {};

int main()
{
    uint8_t mode=0;
    bool EdgeToggle[3]={}, pretoggle[3]={};
    bool flag_Dribble=false, flag_Shot=false, flag_Rotate;
    Time_Shot.start();

    double pwm[4]={}, DribblePower = 0.5;
    double Vx=0, Vy=0, Vr=0, Rotate_Power=0, V=0, theta=0;
    double rawAngle, angleLimit;

    Master.SetPS3Address(b, tr, st);
    jy.calibrateAll(50);
    
//    DisplayThread.start(Display);
#ifdef RobotNumberIsOne
// 機体ナンバーが　１
    double VxLimit=0.75;  // Ｘ軸方向ベクトルの最大値
    double VyLimit=0.75;  // Ｙ軸方向ベクトルの最大値
    double VrLimit=0.4;  // 回転方向ベクトルの最大値
    double Bias_Motor=0.75; // モーター出力値のバイアス
    omni.wheel[0].setRadian(PI * 1.0 / 4.0);
    omni.wheel[1].setRadian(PI * 7.0 / 4.0);
    omni.wheel[2].setRadian(PI * 5.0 / 4.0);
    omni.wheel[3].setRadian(PI * 3.0 / 4.0);
#else
// 機体ナンバーが　２
    double VxLimit=0.8;  // Ｘ軸方向ベクトルの最大値
    double VyLimit=0.8;  // Ｙ軸方向ベクトルの最大値
    double VrLimit=0.6;  // 回転方向ベクトルの最大値
    double Bias_Motor=0.8;  // モーター出力値のバイアス
    double Bias_DefectiveMD = 0.6;  // ダメダメＭＤ補正用バイアス
    omni.wheel[0].setRadian(PI * 5.0 / 4.0);
    omni.wheel[1].setRadian(PI * 7.0 / 4.0);
    omni.wheel[2].setRadian(PI * 1.0 / 4.0);
    omni.wheel[3].setRadian(PI * 3.0 / 4.0);
#endif

    Rotate.setInputLimits(-180, 180);
    Rotate.setOutputLimits(-1*VrLimit,VrLimit);
    Rotate.setBias(0);
    Rotate.setMode(1);
    Rotate.setSetPoint(0);

// 以下ループ
    while (true) {
        if (mini.getState()) {
            rawAngle = jy.getZaxisAngle();
            if(rawAngle > 180.0) rawAngle -= 360;
    
            angleLimit = rawAngle;
            Rotate.setProcessValue(angleLimit/*-sensor_.ballAngle*/);
            Rotate_Power = Rotate.compute();
            
            PS3get(true);
            
    //        printf("angle : %d|power : %d | ", (int)rawAngle, (int)(Rotate_Power*100));
            printf("(Vx,Vy):(%3d,%3d)|theta:%d|angle:%d | ", (int)(Vx*100), (int)(Vy*100), (int)(theta), (int)angleLimit);
            
            if (fabs(st[2]-128.0) > 50) {
                Rotate.setSetPoint(angleLimit);
                flag_Rotate = true;
            } else {
                flag_Rotate = false;
            }
            
            if (b[1]) {
                jy.yawcalibrate();
                Rotate.setSetPoint(0);
            }
            
    // モーター出力値の計算
#ifdef RobotNumberIsOne
    // 機体ナンバーが　１
            Vx = ((st[0]-128.0)/128.0) * VxLimit;
            Vy = (128.0-st[1])/128.0 * VyLimit;
            Vr = -(st[2]-128.0)/128.0 * VrLimit * 0.5;
#else
    // 機体ナンバーが　２
            Vx = ((st[0]-128.0)/128.0) * VxLimit;
            Vy = (128.0-st[1])/128.0 * VyLimit;
            Vr = -(st[2]-128.0)/128.0 * VrLimit * 0.5;
#endif
            if (!flag_Rotate) {
                Vr = Rotate_Power;
            }
            
            V = hypot(Vx, Vy);
            theta = atan2(Vy, Vx);
#ifdef RobotNumberIsOne
            omni.computeCircular(V, theta - angleLimit/180.0*PI, Vr);
#else
            omni.computeCircular(V, theta - angleLimit/180.0*PI, -Vr);
#endif
            
            for(int i=0; i < 4; i++) {
                if (0) {
                    if (b[i+4]) {
                        pwm[i] = 0.5;
    //                    printf("%d ", i);
                    } else if (b[i+4+4]) {
                        pwm[i] = -0.5;
                    } else {
                        pwm[i] = 0;
                    }
                } else {
                    pwm[i] = ((double)omni.wheel[i] * Bias_Motor);
                }
            }
    //        printf("\r\n");
    
    // フラグ処理
            if (b[9]) flag_Dribble = true;
            else flag_Dribble = false;
            
            if (b[8] && (Time_Shot.read_ms() > 1000)) {
                flag_Shot = true;
                Time_Shot.reset();
            } else {
                flag_Shot = false;
            }
            
    // ドリブラー・キッカー
    //        if (flag_Dribble) esc.setspeed(0.7);
    //        else esc.setspeed(0.0);
            if (flag_Dribble) Master.Dribble(0.7);
            else Master.Dribble(0.0);
            if (flag_Shot) Master.Shot();
            
    // ニュートラル処理
            for (int i=0; i<4; i++) {
                if (fabs(pwm[i]) < 0.1) pwm[i] = 0.0;
            }
    
#ifdef RobotNumberIsOne
            Master.SetValueMotor(0, pwm[0]);
            Master.SetValueMotor(1, pwm[1]);
            Master.SetValueMotor(2, -pwm[2]);
            Master.SetValueMotor(3, -pwm[3]);
#else
            Master.SetValueMotor(0, pwm[0] * Bias_DefectiveMD);
            Master.SetValueMotor(1, pwm[1]);
            Master.SetValueMotor(2, pwm[2]);
            Master.SetValueMotor(3, pwm[3] * Bias_DefectiveMD);
#endif
        }
    }
}

void PS3get(bool flag_PS3print)
{
    h1 = mini.getHedder1();
    h2 = mini.getHedder2();
    for(int i=0; i<12; i++) {
        b[i] = mini.getButton(i);
    }
    for(int i=0; i<2; i++) {
        tr[i] = mini.getTrigger(i);
    }
    for(int i=0; i<4; i++) {
        st[i] = mini.getStick(i);
    }
    if (flag_PS3print) {
        for(int i=0; i<12; i++) printf("%d ",b[i]);
        printf("|");
        for(int i=0; i<2; i++) printf("%3d ",tr[i]);
        printf("|");
        for(int i=0; i<4; i++) printf("%3d ",st[i]);
        if(mini.getState()) printf("ok");
        else printf("bad");
        printf("\r\n");
    } else {
        mini.getState();
    }
}