File content as of revision 8:e1f1a91e9353:

#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);
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[0],(double)stick[1])-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);
}

void addPID(){
    front.setProcessValue (frontAngle);
    for(int i=8; i < 12; i++){
        motorSpeed[i]-=front.compute();
    }
//    canTR();
}

int main()
{
    mycon.StartReceive();
    recieveController();
    stop = 1;
    led = 1;
    
//    int currentMode=1;
//    int bc=0,bn=0;
//    int sa=0;
    
    printf("success!\r\n");
    
    for(int i=0; i < 8; i++){
        motor[i].setSpeed(0);
    }
    
//    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();
//    SEKIkikou seki(&motor[6], &motor[4], &motor[5], NULL, NULL, NULL, NULL, NULL, NULL, NULL);
    
    
    
    
    while(1) {
        recieveController();
        canTR();
        updateenc();
        frontAngle=jyroValue;
        
        /*
        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]);
                seki.runAll(0,0,0);
                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);
                seki.runAll(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();
                seki.runAll(0,0,0);
            }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();
                seki.runAll(0,0,0);
            }
            seki.runAll(0,0,0);*/
            allanglemove(0,0,0,0);
            addPID();
//        }
        
        if(b[5]&&b[6]){
            motorSpeed[15]=0;
        }else if(b[5]){
            motorSpeed[15]=0.5;
        }else if(b[6]){
            motorSpeed[15]=-0.5;
        }else{
            motorSpeed[15]=0;
        }
        
        
        if(b[7]==1){
            seki.runAll(0.1,0.3,-0.3);
        }else{
            seki.runAll(-0.8,0.3,0.3);
        }
        
        for(int i=0; i < 4; i++){
            motor[i].setSpeed(motorSpeed[i+8]*0.8);
        }
        
        motor[7].setSpeed(motorSpeed[15]);
        
        pc.printf("|wheel:  ");
        for(int i=8; i < 12; i++){
            pc.printf("%.2f  ",motorSpeed[i]);
        }
        pc.printf("|spin:  %.2f  ",motorSpeed[15]);
//        pc.printf("|updown:  %.2f  ",motorSpeed[2]);
        pc.printf("|enc:  ");
        for(int i=0; i < 7; i++){
            pc.printf("%d  ",encoderValue[i]);
            
        }
        pc.printf("|  ");
        pc.printf("R1370:%.3f",jyroValue);
        pc.printf("|  ");
        for(int i=0; i < 8; i++){
            pc.printf("%d ",b[i]);
        }
        pc.printf("|  ");
        for(int i=0; i < 4; i++){
            pc.printf("%d  ",stick[i]);
        }
        pc.printf("\r\n");
        
    }
}