File content as of revision 0:2c7c889dbf1a:

#include "mbed.h"
#include "RoboClaw.h"
#include "string"

#define SQRT_3 1.73205080757
#define MAX_QPPS1 40960
#define MAX_QPPS2 40960
#define INT_TIME 0.02
#define ADRS1 129
#define ADRS2 130

Ticker timer;

RawSerial Slave (p9,p10,115200);
RawSerial controller (p13,p14,115200);

RoboClaw MD(115200,p28,p27);

string recv_str = "";
char c_x = 127, c_y = 127, c_rot  = 127, buttons = 0b00000000;



void controller_recv(){
    char recv_c = controller.getc();
    
    if(recv_str.size() == 4 && recv_c == '\n'){
        c_x = recv_str[0];
        c_y = recv_str[1];
        c_rot = recv_str[2];
        buttons = recv_str[3];
        recv_str = "";
    }
    
    else{
        recv_str += recv_c;
        if(recv_str.size() >= 5) recv_str = "";
    }
}

void Roboclaw_send(double V1, double V2, double V3){
    MD.SpeedM1(ADRS1,int(V1*MAX_QPPS1));
    MD.SpeedM2(ADRS1,int(V2*MAX_QPPS1));
    MD.SpeedM1(ADRS2,int(V3*MAX_QPPS2));
}

void timer_interrupt(){
    int i_x = (int)c_x - 127;
    int i_y = (int)c_y - 127;
    int i_rot = (int)c_rot - 127;
    
    double V1 = (double)i_x + (double)i_rot;
    double V2 = -0.5*(double)i_x + SQRT_3*0.5*(double)i_y + (double)i_rot;
    double V3 = -0.5*(double)i_x - SQRT_3*0.5*(double)i_y + (double)i_rot;
    
    Roboclaw_send(V1,V2,V3);
    Slave.putc(buttons);
    Slave.putc('\n');
}

int main() {
    
    timer.attach(&timer_interrupt,INT_TIME);
    
    controller.attach(&controller_recv,RawSerial::RxIrq);
    
    while(true) {}
}