File content as of revision 0:14ecdafa55eb:

#include "mbed.h"
#include"PS3.h"
#include "ikarashiMDC.h"

Serial serial(PC_6, PC_7);
DigitalOut serialcontrol(D2);
DigitalOut led1(LED1);
PS3 ps3(PC_12, PD_2);
Serial pc(USBTX, USBRX,115200);

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

int main()
{
    serial.baud(115200);
    float i = -1;
    ikarashi[0].braking = true;
    float pwm = 0;
    while(1) {

        int b[12], stick[4],X,Y,M;
        double SPe[4];
      
        /*ボタンスイッチ*/
        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] );
        }
        
        //ベルト機構
        if(b[6] == 0 || b[7] == 0){ 
            ikarashi[5].setSpeed(0);
        }  
        if(b[6] == 1){   
            ikarashi[4].setSpeed(0.08);        
            ikarashi[5].setSpeed(-0.45);
        }
        if(b[7] == 1){
            ikarashi[4].setSpeed(-0.08); 
            ikarashi[5].setSpeed(0.45);
        }
            
        
        //バケット機構
        if(b[0] == 0 || b[1] == 0){ 
            ikarashi[4].setSpeed(0);
        }  
        if(b[0] == 1){           
            ikarashi[4].setSpeed(0.08);
        }
        if(b[1] == 1){
            ikarashi[4].setSpeed(-0.08);
        }
        
        
        //足回り　　　
        X = stick[0];
        Y = stick[1];
        M = stick[2];
        
        if(X > 100 && X < 155 && Y > 100 && Y < 155) { //停止
            SPe[0] = 0;
            SPe[1] = 0;
            SPe[2] = 0;
            SPe[3] = 0;
            
            pc.printf("  停止　　");
        }
        if(X > 100 && Y <= 100) { //右前方向
            SPe[0] = -0.45;
            SPe[1] = ((127.5 - Y) - (X - 127.5)) * 0.004;
            SPe[2] = 0.45;
            SPe[3] = ((X - 127.5) - (127.5 - Y)) * 0.004;
            
            pc.printf("  右前方向  ");
        }
        if(X <= 100 && Y < 155) { //左前方向
            SPe[0] = ((127.5 - X) - (127.5 - Y)) * 0.004;
            SPe[1] = 0.45;
            SPe[2] = ((127.5 - X) - (127.5 - Y)) * -0.004;
            SPe[3] = -0.45;
            
            pc.printf("  左前方向  ");
        }
        if(X >= 155 && Y > 100) { //右後ろ方向
            SPe[0] = ((X - 127.5) - (Y - 127.5)) * -0.004;
            SPe[1] = -0.45;
            SPe[2] = ((X - 127.5) - (Y - 127.5)) * 0.004;
            SPe[3] = 0.45;
            
            pc.printf("  右後ろ方向　　");
        }
        if(X < 155 && Y >= 155) { //左後ろ方向
            SPe[0] = 0.45;
            SPe[1] = ((X - 127.5) + (127.5 - Y)) * 0.004;
            SPe[2] = -0.45;
            SPe[3] = ((X - 127.5) - (127.5 - Y)) * 0.004;
            
            pc.printf("  左後ろ方向　　");
        }   
        if(M < 10) {  //左回転
            SPe[0] = 0.4;
            SPe[1] = 0.4;
            SPe[2] = 0.4;
            SPe[3] = 0.4;
            
            pc.printf("  左回転  ");
        }
        if(M > 245) {  //右回転
            SPe[0] = -0.4;
            SPe[1] = -0.4;
            SPe[2] = -0.4;
            SPe[3] = -0.4;
            
            pc.printf("  右回転  ");

        }
           

            ikarashi[0].setSpeed(SPe[0]);
            ikarashi[1].setSpeed(SPe[1]);
            ikarashi[2].setSpeed(SPe[2]);
            ikarashi[3].setSpeed(SPe[3]);
            
            pc.printf("%.2f  ",SPe[0]);
            pc.printf("%.2f  ",SPe[1]);
            pc.printf("%.2f  ",SPe[2]);
            pc.printf("%.2f  ",SPe[3]);
            
             pc.printf("\r\n");
    }