File content as of revision 1:2e084048b5f9:

#include "mbed.h"
#include "string"
#include "Sabertooth_Serial.h"
#define INT_TIME 0.02
#define ADRS 133
#define UP    0b00000001
#define DOWN  0b00000010
#define HANDR 0b00000100
#define HANDL 0b00001000
#define OTHER 0b00010000


Ticker timer;

RawSerial pc(USBTX,USBRX,115200);
RawSerial Master(p28, p27, 115200);

SaberSerial MD(115200, p13, p14);

DigitalOut hund_fetR(p15);
DigitalOut hund_fetL(p16);

DigitalIn limit(p25);

string Master_recv = "";
char buttons = 0b00000000, pre_buttons = 0b00000000;

void recv_interrupt(){
    char recv_c = Master.getc();
    
    if (recv_c == '\n'){
        buttons = Master_recv[0];
        Master_recv = "";
    }
    else{
        Master_recv += recv_c;
        if(Master_recv.size() == 2) Master_recv = "";
    }
}

void timer_interrupt(){
    
    int saber_speed = 0, saber_def = 0;
    bool hand_R = buttons & HANDR;
    bool hand_L = buttons & HANDL;
    
    hund_fetR.write(hand_R);
    hund_fetL.write(hand_L);
    
    if(buttons & UP){
        saber_speed = 10;
        saber_def = 4;
    }
    else if(buttons & OTHER){
        saber_speed = 50;
        saber_def = 4;
    }
    else if(buttons & DOWN){
        saber_speed = 10;
        saber_def = 5;
    }
    else {
        saber_speed = 0;
        saber_def = 4;
    }
        
    if(limit.read() && saber_def == 4) saber_speed = 0;
    
    MD.Serial(ADRS,saber_def,saber_speed);
    pc.printf("%d,%d\n",hand_R,hand_L);
    
    pre_buttons = buttons;
}

int main() {
    
    hund_fetR = 0;
    hund_fetL = 0;
    
    timer.attach(&timer_interrupt,INT_TIME);
    
    Master.attach(&recv_interrupt, RawSerial::RxIrq);
    
    while(true) {}
}