File content as of revision 6:b426dcaa7c78:

#include "mbed.h"
#include "FEP_RX22.h"
#include "pinconfig.h"
#include "ikarashiMDC.h"
#include "QEI.h"
#include "can_tr.h"
#include "HOSOKIkikou.h"
#include "SEKIkikou.h"
#include "R1370.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);
R1370 r1370(jyro_TX,jyro_RX);
DigitalOut Reset(jyro_RST);
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];

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<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<8; i++) pc.printf("%d ", b[i]);
    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[1] = (float)enc2.getPulses();
    encoderValue[2] = (float)enc3.getPulses();
    encoderValue[3] = (float)enc4.getPulses();
//    for (int i=4; i<8; i++) pc.printf("%d  ", encoderValue[i]);
//    pc.printf("\r\n ");
}

void updatejyro(){
    r1370.update();
    jyroValue=r1370.getAngle();
//    pc.printf("%f  ",jyroValue);
}

int main()
{
//    mycon.StartReceive();
    led = 1;
    pc.printf("success!\r\n");
    int bc=0,bn=0;
    int currentMode=1;
    HOSOKIkikou hosoki(&motor[6],&motor[7], &motor[4], &motor[5], &b[0], &b[1], &b[2], &b[3], &encoderValue[2], &encoderValue[3]);
    while(1) {   
//        recieveController();
        Reset=0;   
        updateenc();
        updatejyro();
        
        bc=b[7]-bn;
        bn=b[7];
        
        if(currentMode==1){
            if(bc==1){
                currentMode=2;
                led = 1;
                hosoki.stopAll();
            }
        }else if(currentMode==2){
            if(bc==1){
                currentMode=3;
                led = 0;
                hosoki.init(&motor[6],&motor[7], &motor[4], &motor[5], &b[0], &b[1], &b[2], &b[3], &encoderValue[2], &encoderValue[3]);
            }
        }else if(currentMode==3){
            if(bc==1){
                currentMode=1;
                led = 1;
                hosoki.stopAll();
            }else{
                hosoki.runAll(-0.9,0.9,0.3,0.3);
            }
        }
        canTR();
        
        if(stick[2]>100||stick[2]<-100||stick[3]>100||stick[3]<-100){//機構回転
            if(stick[2]>100){
                motorSpeed[1] = 0.8;
            }else if(stick[2]<-100){
                motorSpeed[1] = -0.8;
            }
            if(stick[3]>100){
                motorSpeed[2] = 0.8;
            }else if(stick[3]<-100){
                motorSpeed[2] = -0.8;
            }
        } else {
            motorSpeed[2]=0;
            motorSpeed[1]=0;
        }
        
        
        for(int i=1; i < 3; i++){
            motor[i].setSpeed(motorSpeed[i]);
        }
        
        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("|  ");
        for(int i=2; i < 15; i++){
            pc.printf("%.3f  ",motorSpeed[i]);
        }
        pc.printf("|  ");
        for(int i=0; i < 12; i++){
            pc.printf("%d  ",encoderValue[i]);
        }
        pc.printf("|  ");
        pc.printf("%.4f\r\n",jyroValue);
        
    }
}