File content as of revision 4:94f943530940:

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

Controller pad(PA_0, PA_1, );
Serial serial(PC_10, PC_11, 115200);
Serial PC(USBTX,USBRX,115200);
DigitalOut serialcontrol(D2);

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),
};
PwmOut pwm(LED1);

int main() {
    int b[11];
    double value[4], rad[2], dis[2];
    double spV = 0.3;/////////speed Vertical
    double spS = 0.3;/////////speed Side
    double spT = 0.15;////////speed Turn
    double Fm = 1.0;//////////Front magnification
    double Bm = 1.0;//////////Back magnification
    double Sm = 1.0///////////Side magnification
    double acc = 25;//////////acceleration
    double dec = 5;///////////deceleration
    ikarashi[0].braking = true;
    while (1) {
        //button stick trigger//
         if(pad.receiveState()){
            for(int i = 0; i < 13; i++){
                b[i] = pad.getButton1(i);
                pc.printf("%d ", [i]);
            }
            for(int i = 0; i < 2; i++){
                rad[i] = pad.getRadian(i);
                dis[i] = pad.getNorm(i);
                PC.printf("%f %f\t", rad[i], dis[i]);
            }
        }
        /***************************
        ***************************/
        ////acceleration////
        if (b[2] == 0 && b[3] + b[4] + b[5] == 3 && value[0] < spS && value[1] < spS * Sm) {
            value[0] += spS / acc;
            value[1] -= spS / acc * Sm;
            value[2] -= spS / acc * Sm;
            value[3] += spS / acc;
        }
        if (b[3] == 0 && b[2] + b[4] + b[5] == 3 && value[0] < spV && value[2] < spV * Fm) {
            value[0] += spV / acc;
            value[1] += spV / acc;
            value[2] -= spV / acc * Fm;
            value[3] -= spV / acc * Fm;
        }
        if (b[4] == 0 && b[2] + b[3] + b[5] == 3 && value[1] < spS && value[0] < spS * Sm) {
            value[0] -= spS / acc * Sm;
            value[1] += spS / acc;
            value[2] += spS / acc;
            value[3] -= spS / acc * Sm;
        }
        if (b[5] == 0 && b[2] + b[3] + b[4] == 3 && value[2] < spV && value[0] < spV * Bm) {
            value[0] -= spV / acc * Bm;
            value[1] -= spV / acc * Bm;
            value[2] += spV / acc;
            value[3] += spV / acc;
        }
        /********/
        ////turn////
        if (b[0] + b[1] + b[2] + b[3] == 4) {
            if (b[8] == 0 && b[10] == 1) {
                for (int i = 0;i < 4; i++) {
                    value[i] = spT;
                }
            }
            if (b[10] == 0 && b[8] == 1) {
                for (int i = 0; i < 4; i++) {
                    value[i] = -spT;
                }
            }
        }
        /********/
        if (b[2] + b[3] + b[4] + b[5] <= 2 || b[2] + b[3] + b[4] + b[5] == 4 && b[8] + b[10] == 2) {
            for (int i = 0; i < 4; i++) {
                value[i] -= value[i] / dec;
            }
        }
        /***************************
        ***************************/
        for (int i = 0; i < 4; i++) {
            PC.printf("%.2f,", value[i]);
        }
        PC.printf("||");
        ikarashi[0].setSpeed(value[0]);
        ikarashi[1].setSpeed(value[1]);
        ikarashi[2].setSpeed(value[2]);
        ikarashi[3].setSpeed(value[3]);
        
      for (int i = 0; i < 4; i++) {
            PC.printf("%.2f,", value[i]);
        }
        PC.printf("\n\r");
    }
}