File content as of revision 2:e7910c124624:

#include "mbed.h"
// #include "QEI.h"
#include "ikarashiMDC.h"
//#include "jy901.h"
#include "ikarashiSV.h"
#include "controller.h"
#include "pinconfig.h"
#include "omni_wheel.h"
#include "math.h"

#define PI 3.141592653589

Bcon mycon(fepTX, fepRX, fepad);
Serial pc(pcTX, pcRX, 115200);
Serial RS485(mdcTX,mdcRX,115200);

//JY901 jy(PB_9, PB_8); //sda, scl
DigitalOut stop(em_stop);

//QEI enc1(PA_6,PA_7,NC,100, QEI::X4_ENCODING);
//QEI enc2(PA_8,PA_9,NC,100, QEI::X4_ENCODING);
//QEI enc3(PB_6,PB_7,NC,100, QEI::X4_ENCODING);

ikarashiMDC motor[] = {
    ikarashiMDC(0,0,SM,&RS485),/*足*/
    ikarashiMDC(0,1,SM,&RS485),/*足*/
    ikarashiMDC(0,2,SM,&RS485),/*足*/
    ikarashiMDC(0,3,SM,&RS485),/*足*/
    ikarashiMDC(1,0,SM,&RS485),/*腕昇降*/
    ikarashiMDC(1,1,SM,&RS485),/*腕前後*/
};

ikarashiSV slv(PC_7,PB_10,PB_4,PB_5);

OmniWheel omni(4);

void add(){
    slv.add_state();
}



int main(void){

//float x,y,z;
//jy.calibrateAll(50);

mycon.StartReceive();
uint8_t b[8];
int16_t stick[4];
double accele[4];
double speed[6];
double spin_power;


//int i=0;

omni.wheel[0].setRadian(PI * 1.0 / 4.0);
omni.wheel[1].setRadian(PI * 3.0 / 4.0);
omni.wheel[2].setRadian(PI * 5.0 / 4.0);
omni.wheel[3].setRadian(PI * 7.0 / 4.0);
    
    while (1)
    {
        /*非常停止*/
        stop = 1;

        /*ジャイロ読み取り*/
//        x = jy.getXaxisAngle();             
//        y = jy.getYaxisAngle();
//        z = jy.getZaxisAngle();
//        pc.printf("x:%3.0f | y:%3.0f | z:%3.0f | ", x, y, z);
//        x = x/180;
//        y = y/180;
//        z = z/180;
        
        /*コントローラー受信*/
        for (int i=0; i<8; i++) b[i] = mycon.getButton(i);     
        for (int i=0; i<4; i++) stick[i] = mycon.getStick(i);
        
        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");

        for(int i=0; i<6; i++){
            speed[i] = 0;
        }

        /*全方位制御*/
        for(int i=0; i<4; i++){
            if(abs(stick[i]) > 10){
            accele[i] = stick[i]/128.0;
            }else{
                accele[i] = 0;
            }
        }

        /*旋回*/
        if(abs(stick[2]) > 10){
            spin_power = accele[2];
        }else{
            spin_power = 0;
        }
        
        omni.computeXY(accele[0],accele[1],-spin_power);
        for(int i = 0; i < 4; i++) speed[i] = omni.wheel[i];

        /*腕昇降*/
        if(b[0] != 0){
            speed[4] = 0.3;
        }else if(b[2] != 0){
            speed[4] = -0.3;
        }else{
            speed[4] = 0;
        }

        /*腕前後*/
        if(b[3] != 0){
            speed[5] = 0.3;
        }else if(b[1] != 0){
            speed[5] = -0.3;
        }else{
            speed[5] = 0;
        }

        for(int i=0; i<6; i++) motor[i].setSpeed(speed[i]);

        /*
        if(b[0]==1){
            motor[0].setSpeed(0.3);
        }else if(b[1]){
            motor[1].setSpeed(0.3);
        }else if(b[2]){
            motor[2].setSpeed(0.3);
        }else if(b[3]){
            motor[3].setSpeed(0.3);
        }else if(b[4]){
            motor[0].setSpeed(-0.3);
        }else if(b[5]){
            motor[1].setSpeed(-0.3);
        }else if(b[6]){         
            motor[2].setSpeed(-0.3);    
        }else if(b[7]){        
            motor[3].setSpeed(-0.3);
        }else{
            motor[0].setSpeed(0);
            motor[1].setSpeed(0);
            motor[2].setSpeed(0);
            motor[3].setSpeed(0);
        }*/
    }
    
}