File content as of revision 6:ac8204439a89:

///
/// @file  main.cpp
/// @brief ロボカップラジコンのプログラム
/// 
/// @note トグルは上が１で下が０
///
/// @details 右端トグルで速度切り替え
/// @note １：速め　０：遅め
///
/// @details 中央トグル２つでモード切替
/// @note Refer
/// - 中央トグルでのモード切替;
/// - 左０右０：スタート
/// - 左１右０：キッカーテスト
/// - 左０右１：ドリブラーテスト
/// - 左１右１：オムニテスト
/// - ボタンを押すかトグルスイッチを変えるとワイヤレスモード中止
/// 
/// @details スタート後にPSボタンを押してペアリング 以下操作方法
/// @note Refer
/// - 左スティックで移動(絶対角)
/// - 右スティックX軸で回転
/// - 〇でドリブル、△でシュート
/// 
/// @attention １番機は起動後にArduinoのリセットボタンを押す必要あり
/// 
#include "Master.h"
#include "jy901.h"
#include "PID.h"
#include "omni_wheel.h"
#include "SerialArduino.h"

DigitalOut led(LED1);
DigitalIn toggle[3] = {
    DigitalIn(Pin_toggle_0),
    DigitalIn(Pin_toggle_1),
    DigitalIn(Pin_toggle_2)
};

SerialArduino mini(Pin_Arduino_TX, Pin_Arduino_RX, 115200);
Master Master;
JY901 jy(Pin_JY901_sda, Pin_JY901_scl);
DigitalIn rawButton(Pin_button);
OmniWheel omni(4);
Timer Time_Shot;
Timer Time_MotorTest;

#ifdef RobotNumberIsOne
PID Rotate(25.0, 100.0, 0.0001, 0.01);
#else
PID Rotate(15.0, 100.0, 0.0001, 0.01);
#endif


///
/// @fn PS3get(bool flag_PS3print)
/// @brief ループ毎に実行。SerialArduinoライブラリを用いてPS3コントローラの値を取得。
/// @param flag_PS3print 受け取った値を表示したい時にtrue
///
void PS3get(bool flag_PS3print);

///
/// @fn StartDemo();
/// DualShock3を用いたラジコン操作
/// @brief modeが0の時にボタンを押すとスタート
/// @attention ロボットの機体番号に応じて出力とピンを変える必要アリ
///
void StartDemo();

///
/// @fn MotorTest()
/// @brief モーターを4つ連続で回す。1秒で正転反転が切り替わる
/// @brief modeが2の時にボタンを押すとスタート
///
void MotorTest();

bool flag_Passfunc;
uint8_t preButton;
uint8_t Button;
uint8_t Toggle_mode=0, Toggle_speed=0;
uint8_t h1,h2;
bool    b[12]= {}; 
uint8_t st[4]= {};
uint8_t tr[2]= {};

int main()
{
    while(1) {
        Button = (int)rawButton;
        Toggle_mode = (int)(!toggle[0])*2 + (int)(!toggle[1]);
        Toggle_speed = (int)(!toggle[2]);
        
        PS3get(false);
        
        printf("mode:%d|speed:%d | button:(%d,%d)  ", Toggle_mode, Toggle_speed, Button, preButton);
        
        if (Button) {
            switch (Toggle_mode) {
            case 0 : 
                StartDemo();
                break;
            case 1 : 
                printf("Kicker Test \r\n");
                Master.Shot();
                break;
            case 2 : 
                printf("Dribbler Test \r\n");
                Master.Dribble(0.7);
                break;
            case 3 : 
                MotorTest();
                break;
            }
        } else {
            printf("\r\n");
            Master.Dribble(0.0);
            for (int i=0; i<4; i++) Master.SetValueMotor(i, 0.0);
        }
        preButton = Button;
    }
}

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();
    }
}



void StartDemo()
{
    bool flag_Dribble=false;
    bool flag_Shot=false;
    bool flag_Rotate=false;
    bool b_[12];

    double pwm[4]={};
    double DribblePower = 0.5;
    double Vx=0;
    double Vy=0;
    double Vr=0;
    double RotatePower=0;
    double V=0;
    double theta=0;
    
/// @var rawAngle
/// @brief jy901の生データ
/// @var targetAngle
/// @brief PIDの目標角(-180 ~ 180)
/// @var limitedAngle
/// @brief rawAngleを(-180~180)の範囲で表す
/// @var processAngle
/// @brief PIDで補正される値
    double rawAngle=0;
    double targetAngle=0;
    double limitedAngle=0;
    double processAngle=0;
    
    Time_Shot.start();
    jy.calibrateAll(50);
    
#ifdef RobotNumberIsOne
// 機体ナンバーが　１
    double VxLimit=0.75;  // Ｘ軸方向ベクトルの最大値
    double VyLimit=0.75;  // Ｙ軸方向ベクトルの最大値
    double VrLimit=0.25;  // 回転方向ベクトルの最大値
    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.3;  // 回転方向ベクトルの最大値
    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(-360, 360);
    Rotate.setOutputLimits(-1*VrLimit,VrLimit);
    Rotate.setBias(0);
    Rotate.setMode(1);
    Rotate.setSetPoint(targetAngle);

// 以下ループ
    while (true) {
        PS3get(true);
        Button = rawButton;
        
// UIボードの処理
        Toggle_mode  = (int)(!toggle[0])*2 + (int)(!toggle[1]);
        Toggle_speed = (int)(!toggle[2]);
        if (!Button && preButton) {
            if (!flag_Passfunc) {
                flag_Passfunc = true;
            } else {
                Toggle_mode = 4;
            }
        }
        if (Toggle_mode!=0) {
            for (int i=0; i<4; i++) Master.SetValueMotor(i, 0.0);
            Master.Dribble(0.0);
            Time_MotorTest.reset();
            Time_MotorTest.stop();
            flag_Passfunc = false;
            return;
        }
        
// 以下ペアリング中のループ
        if (mini.getState()) {
            
// ジャイロ
            rawAngle = jy.getZaxisAngle();
            if (rawAngle > 180.0) rawAngle -= 360;
            if (rawAngle < -180.0) rawAngle += 360;
            limitedAngle = rawAngle;
            
// 右スティックで回転
            if (fabs(st[2]-128.0) > 30) {
                targetAngle = limitedAngle;
                flag_Rotate = true;
            } else {
                flag_Rotate = false;
            }
            
            if ((st[3] > 220) && b[10] && !b_[10]) {
                targetAngle = fmod((targetAngle + 180), 360);
            }
            
// 零点出し
            if (b[1]) {
                jy.yawcalibrate();
                Rotate.setSetPoint(0);
                targetAngle = 0;
            }
            
// PIDの調整
            processAngle = limitedAngle - targetAngle;
            if (processAngle > 180.0) processAngle = processAngle - 360.0;
            if (processAngle < -180.0) processAngle = processAngle + 360.0;
            Rotate.setProcessValue(processAngle);
            RotatePower = Rotate.compute();
            
// モーター出力値の計算
#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;
#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;
#endif

            if (!flag_Rotate) {
                Vr = RotatePower;
            }
            
            V = hypot(Vx, Vy);
            if (!Toggle_speed) V *= 0.5;
            theta = atan2(Vy, Vx);
#ifdef RobotNumberIsOne
            omni.computeCircular(V, theta - limitedAngle/180.0*PI, Vr);
#else
            omni.computeCircular(V, theta - limitedAngle/180.0*PI, -Vr);
#endif
            for(int i=0; i < 4; i++) {
                pwm[i] = ((double)omni.wheel[i] * Bias_Motor);
            }
    
// フラグ処理
            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) 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.05) 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
            
            printf("(Vx,Vy, Vr):(%3d,%3d,%3d)|theta:%d | angle:%d | speed:%d | "
            , (int)(Vx*100), (int)(Vy*100), (int)(Vr*100), (int)(theta), (int)limitedAngle, Toggle_speed);
        }
        
// １ループ前のボタンの値
        preButton = Button;
        for (int i=0; i<12; i++) b_[i] = b[i];
    }
}

void MotorTest()
{
    Time_MotorTest.start();
    uint8_t Num_MotorTest = 0;
    double pwm=0.0;
    
    while(1) {
        Button = rawButton;
        pwm = sin(Time_MotorTest.read() * PI);
        Master.SetValueMotor(Num_MotorTest, pwm);
        if (Time_MotorTest.read() > 2.0) {
            Master.SetValueMotor(Num_MotorTest, 0.0);
            Num_MotorTest = (Num_MotorTest+1) % 4;
            Time_MotorTest.reset();
        }
        if (!Button && preButton) {
            if (!flag_Passfunc) {
                flag_Passfunc = true;
            } else {
                for (int i=0; i<4; i++) Master.SetValueMotor(i, 0.0);
                Time_MotorTest.reset();
                Time_MotorTest.stop();
                flag_Passfunc = false;
                return;
            }
        }
        printf("Motor Test | motor%d:%3d | button:(%d,%d) | \r\n"
        , Num_MotorTest, (int)((pwm)*100), (int)Button, preButton);
        preButton = Button;
    }
}