File content as of revision 0:ca9732a3ae5c:

#include "mbed.h"
#include "ikarashiMDC.h"
#include "controller.h"
#include <math.h>

DigitalOut stop(D13);
Bcon mycon(PA_9,PA_10,004);//FEP004はコントローラーについてる方の番号
Serial serial(PC_10,PC_11,115200);
Serial pc(USBTX,USBRX,115200);

ikarashiMDC motor[] = {
    ikarashiMDC(0,0,SM,&serial),
    ikarashiMDC(0,1,SM,&serial),
    ikarashiMDC(0,2,SM,&serial),
    ikarashiMDC(0,3,SM,&serial),
    ikarashiMDC(1,0,SM,&serial),
    ikarashiMDC(1,1,SM,&serial),
    ikarashiMDC(1,2,SM,&serial)
    //ikarashiMDC(1,3,SM,&serial)
};


int main()
{
    mycon.StartReceive();
    stop=1;
    uint8_t b[8];//ボタン　b[0]~b[7]の順に　左 黄色 青 赤 緑　右 赤 青 黄色 緑
    int16_t stick[4];//スティック
    double speed[7] = {0,0,0,0,0,0,0};//モータースピード
    double x,y;
    int i;
    while(1) {
        for (i=0; i<8; i++) b[i] = mycon.getButton(i);
        for (i=0; i<4; i++) stick[i] = mycon.getStick(i);
        x=stick[0];
        y=stick[1];
        if(x>20||x<-20||y>20||y<-20){
            for(i=0; i < 4; i++){
                speed[i] = sin(atan2(y,x)-3.14*(i*2-1) / 4);
            }
        } else {
            for(i=0; i < 4; i++){
                speed[i] = 0;
            }
        }
        //ここまでが全方位移動
        
        if(b[0]){
            speed[4] = 1.0;
        }else if(b[1]){
            speed[4] = -1.0;
        }else{
            speed[4] = 0.0;
        }
        //左 黄色と青
        
        if(b[2]){
            speed[5] = 1.0;
        }else if(b[3]){
            speed[5] = -1.0;
        }else{
            speed[5] = 0.0;
        }
        //左 赤と緑
        
        if(b[4]){
            speed[6] = 1.0;
        }else if(b[5]){
            speed[6] = -1.0;
        }else{
            speed[6] = 0.0;
        }
        //右　赤と青
        
        for(i=0; i < 4; i++){
            motor[i].setSpeed(speed[i]*0.5);
        }//足回り
        motor[4].setSpeed(speed[4]*0.3);
        motor[5].setSpeed(speed[5]);
        motor[6].setSpeed(speed[6]*0.5);
        //モータースピードの上げ下げは*0.5を大きくしたり小さくしたりで調整
        
        for (int i=0; i<8; i++) pc.printf("%d ", b[i]);
        pc.printf(" | ");
        for (int i=0; i<4; i++) pc.printf("%3d ", stick[i]);
        pc.printf(" | ");
        if (mycon.status) pc.printf("received\r\n");
        else pc.printf("anything error...\r\n");
        //コントローラーの状態を表示
        
    }
}