File content as of revision 7:76790bcece4b:

#include "mbed.h"
#include "FEP_RX22.h"
#include "pinconfig.h"
#include "ikarashiMDC.h"
#include "QEI.h"
#include "PID.h"
#include "can_tr.h"
#include "SEKIkikou.h"
//#include "HOSOKIkikou.h"

FEP_RX22 mycon(fepTX, fepRX, fepad);
Serial pc(USBTX, USBRX, 115200);
Serial serial(motorTX, motorRX, 115200);
//QEI enc1(encoder1_A, encoder1_B, NC, 100, QEI::X4_ENCODING);
QEI enc2(encoder2_A, encoder2_B, NC, 100, QEI::X4_ENCODING);
QEI enc3(encoder3_A, encoder3_B, NC, 100, QEI::X4_ENCODING);
QEI enc4(encoder4_A, encoder4_B, NC, 100, QEI::X4_ENCODING);
PID front(2.0, 0, 0.001, 0.01);
//PID rink(2.0, 0, 0.001, 0.01);
DigitalOut stop(stop_pin);
DigitalOut led(LED2);

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)
};

uint8_t data[128];
double frontAngle=0;

void recieveController(){
    mycon.getData(data);
    for (int i=7,j=128; j>0; i--,j/=2){
        b[i]=data[0]/j;
        data[0]%=j;
    }
//    for (int i=0; i<8; i++) pc.printf("%d ", b[i]);
    for (int i=0; i<4; i++) stick[i]=128-data[i+1];
    if(mycon.getStatus()==false){
        for (int i=0; i<8; i++) b[i]=0;
        for (int i=0; i<4; i++) stick[i]=0;
    }
    canTR();
//    for (int i=0; i<4; i++) pc.printf("%d ", stick[i]);
//    pc.printf(" | ");
//    if (mycon.getStatus()) pc.printf("received\r\n");
//    else pc.printf("anything error...\r\n");
}

void updateenc(){
//    encoderValue[0] = (float)enc1.getPulses();
    encoderValue[4] = (float)enc2.getPulses();
    encoderValue[5] = (float)enc3.getPulses();
    encoderValue[6] = (float)enc4.getPulses();
    canTR();
//    for (int i=0; i<4; i++) pc.printf("%d  ", encoderValue[i]);
//    pc.printf("\r\n ");
}

void allanglemove(double one,double two,double three,double four){
    if(stick[0]>20||stick[0]<-20||stick[1]>20||stick[1]<-20){//全方位移動
        for(int i=0; i < 4; i++){
            motorSpeed[i+8] = sin((atan2((double)stick[1],(double)stick[0])-3.14*(i*2-1) / 4)+(frontAngle*3.14/180));
        }
    } else {
        motorSpeed[8]=one;
        motorSpeed[9]=two;
        motorSpeed[10]=three;
        motorSpeed[11]=four;
    }
    canTR();
}

void PIDset(){
    front.setInputLimits (-180,180);
    front.setOutputLimits (-1,1);
    front.setBias(0);
    front.setMode (1);
    front.setSetPoint(0);
    /*
    rink.setInputLimits (-180,180);
    rink.setOutputLimits (-1,1);
    rink.setBias(0);
    rink.setMode (1);
    rink.setSetPoint(0);
    */
}

void addPID(){
    front.setProcessValue (frontAngle);
    for(int i=8; i < 12; i++){
        motorSpeed[i]-=front.compute();
    }
    canTR();
}
/*
void hosoki(double m8, double m9, double m10, double m11){
    motorSpeed[8]=m8;
    motorSpeed[9]=m9;
    motorSpeed[10]=m10;
    motorSpeed[11]=m11;
} 

void seki(double m12, double m13, double m14){
    motorSpeed[12]=m12;
    motorSpeed[13]=m13;
    motorSpeed[14]=m14;
    canTR();
}      
*/
int main()
{
    mycon.StartReceive();
    stop = 1;
    led = 1;
    
    int currentMode=1;
    int bc=0,bn=0;
    int sa=0;
    
    printf("success!\r\n");
    
    canAllReset();   
    PIDset();
    SEKIkikou seki(&motor[6], &motor[4], &motor[5], &b[0], &b[1], &b[2], &b[3], &encoderValue[4], &encoderValue[5], &encoderValue[6]);
    seki.stopAll();
//    HOSOKIkikou hosoki(&motor[10],&motor[11], &motor[8], &motor[9], NULL, NULL, NULL, NULL, NULL, NULL, NULL);
    
    while(1) {
        recieveController();
        updateenc();
        
        
        bc=b[7]-bn;
        bn=b[7];
        
        if(currentMode==1){
            if(bc==1){
                currentMode=2;
                seki.init(&motor[6],&motor[4], &motor[5], &b[0], &b[1], &b[2], &b[3], &encoderValue[4], &encoderValue[5], &encoderValue[6]);
                allanglemove(0,0,0,0);
            }else if(b[0]){
                allanglemove(-1,-1,1,1);
            }else if(b[1]){
                allanglemove(1,-1,-1,1);
            }else if(b[2]){
                allanglemove(1,1,-1,-1);
            }else if(b[3]){
                allanglemove(-1,1,1,-1);
            }else if(b[4]||b[6]){
                sa+=b[4]-b[6];
                if(sa==360)sa=0;
                frontAngle=jyroValue-sa;
                if(frontAngle<=-180){
                    frontAngle+=360;
                }else if(frontAngle>180){
                    frontAngle-=360;
                }
                allanglemove(0,0,0,0);
            }else if(b[5]){
                sa=0;
                frontAngle=jyroValue;
                allanglemove(0,0,0,0);
            }else{
                allanglemove(0,0,0,0);
            }
            addPID();
        }else if(currentMode==2){
            if(bc==1){
                currentMode=3;
                seki.stopAll();
            }else{
                seki.runAll(-0.3,0.3,0.3);
            }
            allanglemove(0,0,0,0);
            addPID();
        }else if(currentMode==3){
            if(bc==1){
                currentMode=1;
                seki.stopAll();
            }
            allanglemove(0,0,0,0);
            addPID();
        }
        
        for(int i=0; i < 4; i++){
            motor[i].setSpeed(motorSpeed[i+8]*0.8);
        }
        
        for(int i=0; i < 8; i++){
            printf("%d  ",b[i]);
        }
        printf("|  ");
        for(int i=0; i < 4; i++){
            printf("%d  ",stick[i]);
        }
        printf("|  ");
        for(int i=2; i < 15; i++){
            printf("%.3f  ",motorSpeed[i]);
        }
        printf("|  ");
        for(int i=0; i < 12; i++){
            printf("%d  ",encoderValue[i]);
        }
        printf("|  ");
        printf("%.4f\r\n",jyroValue);
        
        canTR();
    }
}