File content as of revision 26:223339e60e00:

#define BEAT 140
#include "mbed.h"
#include "math.h"
#include "omni_wheel.h"
#include "motorsmlap.h"
#include "jy901.h"
#include "SerialMultiByte.h"
#include "PID.h"

JY901 jy(PB_9, PB_8); //sda, scl

PID angle(2.0,0,0,0.01);
Serial pc(USBTX, USBRX, 115200);
SerialMultiByte arduino(PC_12,PD_2);
SerialMultiByte m5stack(PC_10,PC_11);

PwmOut beep(PA_0);

DigitalIn  sw(PC_13);
DigitalOut led1(PB_5);
DigitalOut led2(LED2);
DigitalOut led3(PA_1);
DigitalOut led4(PA_10);

motorSmLap motor[] = {
    motorSmLap(PC_6, PC_9, PC_8),
    motorSmLap(PB_6, PA_9, PC_7),
    motorSmLap(PA_8, PB_4, PB_7)
    };
    
const double PII = 3.14159265358979;
OmniWheel omni(3);

Ticker timer;
int setmode=0;
void tick(void)
{
    //pc.printf(" %d\n",setmode);
    if(sw.read()==0)led2=1; else led2=0;
}

void setup(){
        arduino.setHeaders(127,127);
        arduino.startReceive(5);
        m5stack.setHeaders(127,127);
        m5stack.startReceive(9);
        
        omni.wheel[0].setRadian(PII/2.0);
        omni.wheel[1].setRadian(7.0/6.0*PII);
        omni.wheel[2].setRadian(11.0/6.0*PII);
        
        beep.period(1.0/3136);
        beep.write(0.4f);
        wait_ms(BEAT);
        
        beep.period(1.0/2960);
        beep.write(0.4f);
        wait_ms(BEAT);
        
        beep.period(1.0/2489);
        beep.write(0.4f);
        wait_ms(BEAT);
        
        beep.period(1.0/1760);
        beep.write(0.4f);
        wait_ms(BEAT);
        
        beep.period(1.0/1661);
        beep.write(0.4f);
        wait_ms(BEAT);
        
        beep.period(1.0/2637);
        beep.write(0.4f);
        wait_ms(BEAT);
        
        beep.period(1.0/3322);
        beep.write(0.4f);
        wait_ms(BEAT);
        
        beep.period(1.0/4186);
        beep.write(0.4f);
        wait_ms(BEAT);
        
        beep.write(0.0f);
        wait_ms(BEAT);  
}

int main() {
    setup();
    int ssw=1;
    float x, y,now_angle;
    jy.calibrateAll(50);
    uint8_t weight[5];
    uint8_t data[10];
    angle.setInputLimits(-180.0,180.0);
    angle.setOutputLimits(-1,1);
    angle.setBias(0);
    angle.setMode(1);
    angle.setSetPoint(0);
    
    sw.mode(PullUp);
    timer.attach(&tick, 0.1);

    while (true) {
        
/*青スイッチモード切り替え*/
        if(sw.read()==1){
            if(ssw==0){
                ssw=1;
            }
        }else{
            if(ssw==1){
                setmode++;
                ssw=0;
            }
        if(setmode>=2){
            setmode=0;
            }
        }
        
        switch(setmode){
/*RCWCcontrollerモード*/  
            case 0:
                led1=1;
                led3=0;
                led4=0;
                m5stack.getData(data);
                y=(data[3]-127.5)/127.5;
                x=(data[2]-127.5)/127.5;
                //誤差をなくす
                if(x==-1 && y==-1){//
                    x=0;
                    y=0;
                    }
                if((x>=0&&x<=0.2)||(x<=0&&x>=-0.2)){
                    x=0;
                }
                
                if((y>=0&&y<=0.2)||(y<=0&&y>=-0.2)){
                    y=0;
                }
                //十字モード
                if(data[1]==1){
                    y=0.5;
                    }
                if(data[1]==2){
                    y=-0.5;
                    }
                if(data[1]==4){
                    x=0.5;
                    }
                if(data[1]==8){
                    x=-0.5;
                    }
                //iosジャイロモード
                if(data[0]==10){//L1とR1が同時に押された時の値
                    if(data[8]<30){
                        y=0.5;
                        }
                    if(data[8]>230){
                        y=-0.5;
                        }
                    if(data[6]<40){
                        x=-0.5;
                        }
                    if(data[6]>220){
                        x=0.5;
                        }
                    }
                now_angle = jy.getZaxisAngle();
                
                if(now_angle>180 && now_angle<360){
                    now_angle = now_angle-360;
                }
                angle.setProcessValue(now_angle);
                omni.computeXY(
                    x,
                    y,
                    0,
                    0,
                    angle.compute()
                    
                );
                for(int i=0; i<3; i++){
                    motor[i].setMotorSpeed(omni.wheel[i].getOutput()*0.8);
                }
                pc.printf("X = <%f>,Y = <%f>,angle= <%f>\r\n",x,y,now_angle);
                break;
                
/*wii balance　boardモード*/  
            case 1:
                led1=0;
                led3=1;
                led4=0;
                arduino.getData(weight);
                for (uint8_t i = 0; i < 4; i++) {
                    pc.printf("%d",weight[i]);
                    pc.printf("\t");
                }
            
                y = weight[0] + weight[2] - weight[1] - weight[3];
                x = weight[0] + weight[1] - weight[2] - weight[3];
                if(y < -50){
                    y = -0.5;
                }else if(y > 50){
                    y = 0.5;
                }else{
                    y = 0;
                }
                if(x < -50){
                    x = -0.5;
                }else if(x > 50){
                    x = 0.5;
                }else{
                    x = 0;
                }
                now_angle = jy.getZaxisAngle();
                
                if(now_angle>180 && now_angle<360){
                    now_angle = now_angle-360;
                }
                angle.setProcessValue(now_angle);
                omni.computeXY(
                    x,
                    y,
                    0,
                    0,
                    angle.compute()
                    
                );
                for(int i=0; i<3; i++){
                    motor[i].setMotorSpeed(omni.wheel[i].getOutput()*0.8);
                }
                pc.printf("X = <%f>,Y = <%f>,angle= <%f>\r\n",x,y,now_angle);
                break;         
        }
    }
}