File content as of revision 2:39bf6e4bb403:

#include "mbed.h"
#include "DualShock.h"

#define LIFTSPEED 40
#define ARMSPEED 40
#define SPEED 25
#define ROLLSPEED 30
#define ARMROLLSPEED 40

Serial DS_serial(PC_10, PC_11);
Serial pc(SERIAL_TX, SERIAL_RX);

PwmOut MD1PWM1(PA_0);
PwmOut MD1PWM2(PA_1);
PwmOut MD2PWM1(PA_7);
PwmOut MD2PWM2(PC_7);
PwmOut MD3PWM1(PB_15);
PwmOut MD3PWM2(PB_13);

DigitalOut myled(LED1);
DigitalOut MD1CW1(PB_0);
DigitalOut MD1CCW1(PC_1);
DigitalOut MD1DIS1(PA_4);
DigitalOut MD1CW2(PC_2);
DigitalOut MD1CCW2(PC_0);
DigitalOut MD1DIS2(PC_3);

DigitalOut MD2CW1(PA_8);
DigitalOut MD2CCW1(PA_10);
DigitalOut MD2DIS1(PB_10);

DigitalOut MD2CW2(PB_4);
DigitalOut MD2CCW2(PB_3);
DigitalOut MD2DIS2(PB_5);
DigitalOut MD3CW1(PB_12);
DigitalOut MD3CCW1(PB_14);
DigitalOut MD3DIS1(PA_11);
DigitalOut MD3CW2(PA_12);
DigitalOut MD3CCW2(PC_4);
DigitalOut MD3DIS2(PC_5);

DigitalOut magnet1(PB_8);
DigitalOut magnet2(PB_9);
DigitalOut magnet3(PA_5);

int main() {
    DS_serial.baud(115200);     //通信速度設定
    InitDS(&DS_serial);         //受信データ用変数を初期化する
    DS_serial.attach(&getDSdata, Serial::RxIrq);    //「受信したら割り込みして」の宣言
    MD1DIS1 = 0;
    MD1DIS2 = 0;
    MD2DIS1 = 0;
    MD2DIS2 = 0;
    MD3DIS1 = 0;
    MD3DIS2 = 0;
    magnet1 = 0;
    magnet2 = 0;
    magnet3 = 0;
    MD1PWM1.period_us(100);//アーム開閉
    MD1PWM2.period_us(100);//右前オムニ
    MD2PWM1.period_us(100);//リフト上下
    MD2PWM2.period_us(100);//アーム回転
    MD3PWM1.period_us(100);//左前オムニ
    MD3PWM2.period_us(100);//後方オムニ
    int gear;
    gear = 1;
    double root3;
    root3 = 1.732;
    double rightfront;
    double leftfront;
    double back;
    while(1){    
        if(hDS.BUTTON.RIGHT == 1){
            MD1PWM1.pulsewidth_us(ARMSPEED);
            MD1CW1 = 0;
            MD1CCW1 = 1;
        }
        else if(hDS.BUTTON.LEFT == 1){
            MD1PWM1.pulsewidth_us(ARMSPEED);
            MD1CW1 = 1;
            MD1CCW1 = 0;
        }
        else{
            MD1PWM1.pulsewidth_us(0);
            MD1CW1 = 0;
            MD1CCW1 = 0;
        }//アーム開閉
        
    
            
            
        if(hDS.BUTTON.R2 == 1){
            gear = 2;
            }
        if(hDS.BUTTON.L2 == 1){
            gear = 1;
            }//スピード調整
        if(hDS.BUTTON.L1 == 1){
            MD1PWM2.pulsewidth_us(gear*ROLLSPEED);
            MD1CW2 = 1;
            MD1CCW2 = 0;
            MD3PWM1.pulsewidth_us(gear*ROLLSPEED);
            MD3CW1 = 0;
            MD3CCW1 = 1;
            MD3PWM2.pulsewidth_us(gear*ROLLSPEED);
            MD3CW2 = 0;
            MD3CCW2 = 1;
            }//右回転
        else if(hDS.BUTTON.R1 == 1){
            MD1PWM2.pulsewidth_us(gear*ROLLSPEED);
            MD1CW2 = 0;
            MD1CCW2 = 1;
            MD3PWM1.pulsewidth_us(gear*ROLLSPEED);
            MD3CW1 = 1;
            MD3CCW1 = 0;
            MD3PWM2.pulsewidth_us(gear*ROLLSPEED);
            MD3CW2 = 1;
            MD3CCW2 = 0;
            }//左回転
        else{    
            rightfront = hDS.ANALOG.LX*-2+hDS.ANALOG.LY*2/root3;
            leftfront = hDS.ANALOG.LX*-2+hDS.ANALOG.LY*-2/root3;
            back = hDS.ANALOG.LX*2;
            if(rightfront < 0){
                MD1PWM2.pulsewidth_us((int)(rightfront*gear*SPEED*-1));
                MD1CW2 = 0;
                MD1CCW2 = 1;
                }
            else{
                MD1PWM2.pulsewidth_us((int)(rightfront*gear*SPEED));
                MD1CW2 = 1;
                MD1CCW2 = 0;
                }
            if(leftfront < 0){
                MD3PWM1.pulsewidth_us((int)(leftfront*gear*SPEED*-1));
                MD3CW1 = 1;
                MD3CCW1 = 0;
                }
            else{
                MD3PWM1.pulsewidth_us((int)(leftfront*gear*SPEED));
                MD3CW1 = 0;
                MD3CCW1 = 1;
                }
            if(back < 0){
                MD3PWM2.pulsewidth_us((int)(back*gear*SPEED*-1));
                MD3CW2 = 1;
                MD3CCW2 = 0;
                }
            else{
                MD3PWM2.pulsewidth_us((int)(back*gear*SPEED));
                MD3CW2 = 0;
                MD3CCW2 = 1;
                }//移動
            }
            
        if(hDS.ANALOG.RY < 0){
            MD2PWM2.pulsewidth_us(hDS.ANALOG.RY*ARMROLLSPEED*-1);
            MD2CW2 = 0;
            MD2CCW2 = 1;
            }
        else{
            MD2PWM2.pulsewidth_us(hDS.ANALOG.RY*ARMROLLSPEED);
            MD2CW2 = 1;
            MD2CCW2 = 0;
            }//アーム回転
            
        if(hDS.BUTTON.CIRCLE == 1){
            magnet1 = 1;
            magnet2 = 1;
            magnet3 = 1;
            }
        if(hDS.BUTTON.SQUARE == 1){
            magnet1 = 0;
            magnet2 = 0;
            magnet3 = 0;
            }//電磁石
            
                
        if(hDS.BUTTON.UP == 1){
            MD2PWM1.pulsewidth_us(40);
            MD2CW1 = 0;
            MD2CCW1 = 1;
            }
        if(hDS.BUTTON.DOWN == 1){
            MD2PWM1.pulsewidth_us(40);
            MD2CW1 = 1;
            MD2CCW1 = 0;
            }
        if(hDS.BUTTON.DOWN == 0 && hDS.BUTTON.UP == 0){
            MD2PWM1.pulsewidth_us(0);
            MD2CW1 = 0;
            MD2CCW1 = 0;
            }//lift
            
            
        if(hDS.BUTTON.CROSS == 1){
            MD1PWM1.pulsewidth_us(0);
            MD1CW1 = 0;
            MD1CCW1 = 0;
            MD1PWM2.pulsewidth_us(0);
            MD1CW2 = 0;
            MD1CCW2 = 0;
            MD2PWM1.pulsewidth_us(0);
            MD2CW1 = 0;
            MD2CCW1 = 0;
            MD2PWM2.pulsewidth_us(0);
            MD2CW2 = 0;
            MD2CCW2 = 0;
            MD3PWM1.pulsewidth_us(0);
            MD3CW1 = 0;
            MD3CCW1 = 0;
            MD3PWM2.pulsewidth_us(0);
            MD3CW2 = 0;
            MD3CCW2 = 0;
            magnet1 = 0;
            magnet2 = 0;
            magnet3 = 0;
                while(1){
                    }
            }//強制終了
        pc.printf("/t rightfront%d/n",(int)rightfront*gear*SPEED);
        pc.printf("/t leftfront%d/n",(int)leftfront*gear*SPEED);
        pc.printf("/t back%d/n",(int)back*gear*SPEED);
    }
}