File content as of revision 1:58103ad56096:

#include "mbed.h"
#include "ikarashiMDC.h"
#include "controller.h"

Serial serial(PC_6, PC_7);
DigitalOut serialcontrol(D2);
DigitalOut led1(LED1);
Serial pc(USBTX, USBRX,115200);
AnalogIn analog_value(A0);
DigitalOut led(LED1);
Controller pad(PC_10,PC_11,208);


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),
    ikarashiMDC(&serialcontrol,3,3,SM,&serial)
};
PwmOut pwm(LED1);

int main()
{
    serial.baud(115200);
    float i = -1;
    ikarashi[0].braking = true;
    float pwm = 0;
    float meas;
    
    printf("\nAnalogIn example\n");
    
    int button[11];
    double SPe[4];
    float stickRadian[2], stickNorn[2];
    
    serial.baud(115200);
    ikarashi[0].braking = true;
        
    while(1) {
        if(pad.receiveState()){
            for(int i = 0; i < 13; i++){
                button[i] = pad.getButton1(i);
                pc.printf("%d ", button[i]);
            }
            for(int i = 0; i < 2; i++){
                stickRadian[i] = pad.getRadian(i);
                stickNorn[i] = pad.getNorm(i);
                pc.printf("%f %f\t", stickRadian[i], stickNorn[i]);
            }
            pc.printf("\n\r");
        
        //足回り
        if(stickNorn[0] != 0){
            SPe[0] = sin(stickRadian[0] - PI * 3/4);
            SPe[1] = sin(stickRadian[0] - PI * (-3/4));
            SPe[2] = sin(stickRadian[0] - PI * (-1/4));
            SPe[3] = sin(stickRadian[0] - PI * 1/4);
        }
        
        //微調整
        if(button[3] == 0){        //前
            SPe[0] = 0.4;
            SPe[1] = 0.4;
            SPe[2] = -0.4;
            SPe[3] = -0.4;
        }
        if(button[5] == 0){        //後ろ
            SPe[0] = -0.4;
            SPe[1] = -0.4;
            SPe[2] = 0.4;
            SPe[3] = 0.4;
        }
        if(button[4] == 0){        //右
            SPe[0] = -0.4;
            SPe[1] = 0.4;
            SPe[2] = -0.4;
            SPe[3] = 0.4;
        }
        if(button[2] == 0){         //左
            SPe[0] = 0.4;
            SPe[1] = -0.4;
            SPe[2] = 0.4;
            SPe[3] = -0.4;
        }
        
        //右回転
        if(button[7] == 0){
            SPe[0] = -0.4;
            SPe[1] = -0.4;
            SPe[2] = -0.4;
            SPe[3] = -0.4;
        }
        //左回転
        if(button[8] == 0){
            SPe[0] = 0.4;
            SPe[1] = 0.4;
            SPe[2] = 0.4;
            SPe[3] = 0.4;
        }
        
        //機構
        if(button[1] == 0){
            ikarashi[4].setSpeed(-0.3);    //閉じる
        }
        
        if(button[0] == 0){                     //開く
            ikarashi[4].setSpeed(0.3);
        }
        
        
        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");
        
         }else{
            pc.printf("error\n\r");
            
            ikarashi[0].setSpeed(0);
            ikarashi[1].setSpeed(0);
            ikarashi[2].setSpeed(0);
            ikarashi[3].setSpeed(0);
        }
    }
}