File content as of revision 2:7da4a8e74955:

/** 300_PS3com
 * PS3と通信して表示、ボタンに応じてモーターを動かす
 */

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


#include "main.h"
#include "Master.h"
#include "SerialArduino.h"
//#include "UI.h"

DigitalOut led(LED1);
Master Master;// 出力等の基本クラス
//Thread DisplayThread;
OmniWheel omni(4);
SerialArduino mini(A0, A1, 115200);// Arduinoを用いてPS3コントローラと通信するクラス
Timer Time_Shot;

void PS3get(bool flag_PS3print);
//void Display(void);

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;
    Time_Shot.start();

    double pwm[4]={}, DribblePower = 0.5;
    double Vx=0, Vy=0, Vr=0;
    double VxLimit=0.75, VyLimit=0.75, VrLimit=0.2;

    Master.SetPS3Address(b, tr, st);
    
//    DisplayThread.start(Display);
#if RobotNumberIsOne
    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
    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
// 以下ループ
    while (true) {
        
        PS3get(true);

// モーター出力値の計算
        if (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;
        }
        omni.computeXY(Vx, Vy, Vr);
        
        for(int i=0; i < 4; i++) {
            if (0) {
                if (b[i+4]) {
                    pwm[i] = 0.5;
                } else if (b[i+4+4]) {
                    pwm[i] = -0.5;
                } else {
                    pwm[i] = 0;
                }
            } else {
                pwm[i] = ((double)(omni.wheel[i])*0.75);
            }
        }
        if (b[2]) flag_Dribble = true;
        else flag_Dribble = false;
        
        if (b[3] && (Time_Shot.read_ms() > 1000)) {
            flag_Shot = true;
            Time_Shot.reset();
        } else {
            flag_Shot = false;
        }
        
        if (flag_Dribble) Master.Dribble(0.5);
        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;
        }

#if RobotNumberIsOne
        Master.SetValueMotor(0, pwm[0]);
        Master.SetValueMotor(1, pwm[1]);
        Master.SetValueMotor(2, -pwm[2]);
        Master.SetValueMotor(3, -pwm[3]);
#else
        for (int i=0; i<4; i++) {
            Master.SetValueMotor(i, pwm[i]);
        }
#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);
    }
    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]);
//    printf("%3d %3d ", (int)(st[0]-128.0), (int)(128-st[1]));
    if(mini.getState()) printf("ok");
    else printf("bad");
    printf("\r\n");
}

#if 0
void Display()
{
    char lcdname1[4] = {}, lcdname2[4] = {};
    int  lcdvalue[2] = {};
    double temp[2] = {};
    while(1){
        switch (ui.displayst) {
        case 0:
            strcpy(lcdname1, "mt0");
            strcpy(lcdname2, "mt1");
            lcdvalue[0] = (int)((double)(omni.wheel[0])*100);
            lcdvalue[1] = (int)((double)(omni.wheel[1])*100);
            break;
        case 1:
            strcpy(lcdname1, "mt2");
            strcpy(lcdname2, "mt3");
            lcdvalue[0] = (int)((double)(omni.wheel[2])*100);
            lcdvalue[1] = (int)((double)(omni.wheel[3])*100);
            break;
        }
        ui.change(lcdname1, lcdname2, lcdvalue);
//        thread_sleep_for(3);
    }
}
#endif