File content as of revision 0:b2c626a3336d:

#include "mbed.h"
#include "ikarashiMDC.h"
#include "PS3.h"
#include "wheel.h"
#define PI 3.14159265
Serial serial(PC_6,PC_7);
DigitalOut serialcontrol(D2);
DigitalOut stop(PB_1,PB_15);

ikarashiMDC ikarashi[] {
    ikarashiMDC(&serialcontrol,2,0,SM,&serial),
    ikarashiMDC(&serialcontrol,2,1,SM,&serial),
    ikarashiMDC(&serialcontrol,2,2,SM,&serial),
    ikarashiMDC(&serialcontrol,2,3,SM,&serial)
};

PS3 ps3(PC_12, PD_2);
Serial pc(USBTX,USBRX,115200);

int main()
{
    serial.baud(115200);
    ikarashi[0].braking = true;
    int b[12], stick[4], trigger[2],neutral[4],count = 0;
    double X,Y,acc[5],power;
    /*ジョイスティック ニュートラル設定*/
    do{
        for(int i = 0; i < 4; i++) {
            neutral[i] =  ps3.getStick(i);
            pc.printf("%4d",neutral[i]);
        }
        count++;
    } while(count <= 50);
    for(int i = 0; i < 4; i++) {
        neutral[i] =  ps3.getStick(i);
        pc.printf("%4d",neutral[i]);
    }
    while(1){
        stop = 1;
    /*ボタン0=上　1=下 2=左　 3=右 4=L1 5=R1 6=△ 7=× 8=□ 9=〇 10=L3 11=R3*/
        for(int i = 0; i < 12; i++) {
            b[i] = ps3.getButton(i);
            pc.printf("%2d",b[i]);
        }
    /*ジョイスティック*/
        for(int i = 0; i < 4; i++) {
            stick[i] = ps3.getStick(i);
            pc.printf("%4d",stick[i] - neutral[i]);
        }
    /*トリガースイッチ*/
        for(int i = 0; i < 2; i++) {
            trigger[i] = ps3.getTrigger(i);
            pc.printf("%4d",trigger[i]);
        }
        X = stick[0] - neutral[0];
        Y = neutral[1] - stick[1];
    /*微調整用コマンド*/
        power = 1;
        if(b[0] == 1 && b[1] == 0 && b[2] == 0 && b[3] == 0 && (stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20)) {
            X = 0;
            Y = 1;
            power = 0.2;
        }
        if(b[0] == 0 && b[1] == 1 && b[2] == 0 && b[3] == 0 && (stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20)) {
            X = 0;
            Y = -1;
            power = 0.2;
        }
        if(b[0] == 0 && b[1] == 0 && b[2] == 1 && b[3] == 0 && (stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20)) {
            X = -1;
            Y = 0;
            power = 0.2;
        }
        if(b[0] == 0 && b[1] == 0 && b[2] == 0 && b[3] == 1 && (stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20)) {
            X = 1;
            Y = 0;
            power = 0.2;
        }
    /*オムニ制御*/
        if((stick[0] == 0 && stick[1] == 0 && stick[2] == 0 && stick [3] == 0) || ((stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20))) {
            acc[0] = 0;
            acc[1] = 0;
            acc[2] = 0;
        } else { 
            acc[0] = omuni_speed(X,Y,PI * 1 / 3) * power;
            acc[1] = omuni_speed(X,Y,PI * 3 / 3) * power;
            acc[2] = omuni_speed(X,Y,PI * 5 / 3) * power;
       }
       if((trigger[0] <= 200 && trigger[1] >= 200) && ((stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20))) {
            acc[0] = -0.5;
            acc[1] = -0.5;
            acc[2] = -0.5;
        }
        if((trigger[1] <= 200 && trigger[0] >= 200) && ((stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20))) {
            acc[0] = 0.5;
            acc[1] = 0.5;
            acc[2] = 0.5;
        }
    /*機構制御*/
        if(b[4] == 1 && b[5] == 0){
            acc[3] = -0.2;
        }
        if(b[4] == 0 && b[5] == 1){
            acc[3] = 0.2; 
        }
        if(b[4] == 0 && b[5] == 0){
            acc[3] = 0;
        }
    /*速度制限*/
        for(int i = 0;i < 4;i++) {
            if(acc[i] >= 0.5){
                acc[i] = 0.5;
            }
            if(acc[i] <= -0.5) {
                acc[i] = -0.5;
            }
        }
   /*タイムアウト制御 & 非常停止*/
        if((ps3.status == false) || (b[10] == 1 && b[11] == 1)) {
            stop = 0;
        }

        ikarashi[0].setSpeed(acc[0]);
        ikarashi[1].setSpeed(acc[1]);
        ikarashi[2].setSpeed(acc[2]);
        ikarashi[3].setSpeed(acc[3]);
        for(int i = 0;i < 5;i++) {
            pc.printf(" %f",acc[i]);
        }
        pc.printf("\r\n");
    }
}