File content as of revision 4:3c04f88a8cd8:

#include "mbed.h"
#include "FEP_RX22.h"
//#include "controller.h"
#include "pinconfig.h"
#include "ikarashiMDC.h"
#include "QEI.h"

FEP_RX22 mycon(fepTX, fepRX, fepad);
//Bcon 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);
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),
    ikarashiMDC(2,0,SM,&serial),
    ikarashiMDC(2,1,SM,&serial),
    ikarashiMDC(2,2,SM,&serial),
    ikarashiMDC(2,3,SM,&serial),
    ikarashiMDC(3,0,SM,&serial)
};

uint8_t b[8];
int16_t stick[4];

uint8_t data[128];

float angle[4]={};

double speed[13]={};

void recieveController(){
    mycon.getData(data);
    for (int i=0; i<5; i++) pc.printf("%d ", data[i]);
    pc.printf(" | ");
    if (mycon.getStatus()) pc.printf("received\r\n");
    else pc.printf("anything error...\r\n");
}

void updateangle(){
    //angle[0] = (float)enc1.getPulses();
    angle[1] = (float)enc2.getPulses();
    angle[2] = (float)enc3.getPulses();
    angle[3] = (float)enc4.getPulses();
    for (int i=0; i<4; i++) pc.printf("%f  ", angle[i]);
    pc.printf("\r\n ");
}

int main()
{
    mycon.StartReceive();
    stop = 1;
    led = 1;
    
    printf("success!\r\n");
    
    while(1) {
        
        //recieveController();
        updateangle();
        
        
        
    }
}