File content as of revision 0:450a7208c682:

#include "mbed.h"
#include "CAN.h"
#include "PS4_Controller.h"
#include "MDDS30_oit.h"
#include "URF.h"

PS4_controller PS4(PC_12,PD_2);

Serial pc(USBTX, USBRX, 115200);

URF usdRight(D6);
URF usdLeft(D8);

Ticker update;

DigitalOut led(LED1);
DigitalOut led2(PC_4);
 
DigitalIn btn(USER_BUTTON);
MDDS30oit MD1(PC_10, PC_11);  //left:モーター番号1（右後輪）、right:モーター番号2（左後輪）
MDDS30oit MD2(PA_0, PA_1);  //left:モーター番号3（右前輪）、right:モーター番号4（左前輪）

void inter(void)
{
    usdRight.send();
    usdLeft.send();
}

int i;
int right = 0, left = 0;
float duty[4]={0.0f};

void Yonrin(float x,float y)       //四輪オムニのpwm値出し
{
    duty[0] =-x/sqrtf(2)+y/sqrtf(2);
    duty[1] =-x/sqrtf(2)-y/sqrtf(2);
    duty[2] = x/sqrtf(2)-y/sqrtf(2);
    duty[3] = x/sqrtf(2)+y/sqrtf(2);
    //pc.printf("a=%f, b=%f, c=%f, d=%f\r\n",duty[0],duty[1],duty[2],duty[3]);
        
        MD1.left(duty[0]);
        MD1.right(duty[1]);
        MD2.left(duty[2]);
        MD2.right(duty[3]);
        wait(0.01);
}

void Yonrin(float x)
{
        //pc.printf("senkai = %f\r\n",x);
        
        MD1.left(x);
        MD1.right(x);
        MD2.left(x);
        MD2.right(x);
        
        wait(0.01);
}

void Yonrin(int mode)
{
    switch(mode) {
        case 0:
        MD1.left(0);
        MD1.right(0);
        MD2.left(0);
        MD2.right(0);
    }
}

int main(){
    
    wait_ms(100);
    
    PS4.send_UNO(1);

    while(!PS4.connect()){
        pc.printf("1");
        led=!led;
        PS4.read_PAD();
        pc.printf("2");
        wait_ms(500);
    }
    led = 1;
    
    update.attach(&inter, 0.06);

    while(1){
        int dataRight = usdRight.read(mm);
        int dataLeft = usdLeft.read(mm);
        pc.printf("Left:%d, Right:%d\n", dataLeft, dataRight);
        //pc.printf("right: %d, left: %d\n", dataRight, dataLeft);

        //right = 0;  //機体の右側に障害物なし
        //left = 0;   //機体の左側に障害物なし

        PS4.read_PAD();
        if(PS4.button(PS))PS4.disconnect();
        if(PS4.connect()){
            if(PS4.button(R1)==1){                
                if(PS4.analog(PS_LX)<100||PS4.analog(PS_LX)>146||PS4.analog(PS_LY)<100||PS4.analog(PS_LY)>146){
                float x_duty = ((PS4.analog(PS_LX)-128.0f)/127.0f)*0.7f;
                float y_duty = -((PS4.analog(PS_LY)-128.0f)/127.0f)*0.7f;
                //pc.printf("x_duty=%f y_duty=%f\r\n", x_duty, y_duty);
                Yonrin(x_duty, y_duty);
                /*wait(1.0);
                Yonrin(0);*/
                }
                else if(PS4.analog(PS_R2)>10){
                    float x=-((PS4.analog(PS_R2)/255.0f))*0.7f;
                    Yonrin(x);
                }
                else if(PS4.analog(PS_L2)>10){
                    float x=((PS4.analog(PS_L2)/255.0f))*0.7f;
                    Yonrin(x);
                }
                else if(PS4.button(UP)==1){
                    Yonrin(0,0.5f);
                }
                else if(PS4.button(DOWN)==1){
                    Yonrin(0,-0.5f);
                }
                else if(PS4.button(RIGHT)==1){
                    if (dataRight >= 400){
                        Yonrin(-0.5f,0);
                    }
                    else{
                        if (dataRight <= 400 && dataRight >= 200){
                            Yonrin(-0.3f, 0);
                       }
                    }
                }
                else if(PS4.button(LEFT)==1){
                    if (dataLeft >= 400){
                        Yonrin(0.5f,0);
                    }else{
                        if (dataLeft <= 400 && dataLeft >= 200){
                            Yonrin(0.3f, 0);
                        }
                    }
                    
                }
                else Yonrin(0);
            }
            else Yonrin(0);
        }
        else Yonrin(0);
    }
    /*if(PS4.button(PS)){
        PS4.disconnect();
    }
    Yonrin(0);*/
}