File content as of revision 5:e5bf798e0522:

#include "mbed.h"
#include "rtos.h"
#include "Serial.h"
#include "Servo.h"
#include "Motor.h"

RawSerial blue(p9, p10);
Serial pc(USBTX, USBRX);
PwmOut servo(p24);
Motor left_motor(p23, p15, p16); // pwm, fwd, rev
Motor right_motor(p22, p17, p18); // pwm, fwd, rev

volatile float speed_left = 0;
volatile float speed_right = 0;
volatile float turret_delta = 0;
volatile bool  fire = false;


Thread thread_drive;
Thread thread_turret;
Thread thread_fire;

void input_loop();
inline void set_speed(float l, float r);
inline void set_turret_delta(float delta);
inline void fire_cannon(bool f);
 
void input_thread() {
    
}

void drive_thread() {
    while (1) {
        
        left_motor.speed(speed_left);
        right_motor.speed(speed_right);
        //pc.printf("left speed: %f, right speed: %f\n", speed_left, speed_right);
        Thread::wait(100);
    }
}

void turret_thread() {
    float pulsewidth = 0.0015;      // servo is between 1 ms and 2 ms    
    
    while (1) {
        servo.period(0.020);            // servo requires a 20ms period
        pulsewidth += turret_delta;
        if (pulsewidth > 0.002) pulsewidth = 0.002;
        else if (pulsewidth < 0.001) pulsewidth = 0.001;
        servo.pulsewidth(pulsewidth);
        
        Thread::wait(50);
    }
}

void fire_thread() {
    
}

 
int main() {
    thread_drive.start(drive_thread);
    //thread_turret.start(turret_thread);
    thread_fire.start(fire_thread);
    
    input_loop();
}

void input_loop() {
    char bnum=0;
    char bhit=0;
    while(1) {
        if (blue.getc()=='!') {
            if (blue.getc()=='B') { //button data packet
                bnum = blue.getc(); //button number
                bhit = blue.getc(); //1=hit, 0=release
                if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
                    switch(bnum) {
                        case '1': //Turret Left Button
                            if (bhit == '1') {
                                set_turret_delta(-0.000025);
                            } else {
                                set_turret_delta(0);
                            }
                            break;
                        case '2': //Turret Right Button
                            if (bhit == '1') {
                                set_turret_delta(0.000025);
                            } else {
                                set_turret_delta(0);
                            }
                            break;
                        case '3': //Fire Button
                            if (bhit == '1') {
                                fire_cannon(true);
                            } else {
                                fire_cannon(false);
                            }
                            break;
                        case '5': //button 5 up arrow
                            if (bhit=='1') {
                                set_speed(0.5, 0.5);
                            } else {
                                set_speed(0, 0);
                            }
                            break;
                        case '6': //button 6 down arrow
                            if (bhit=='1') {
                                set_speed(-0.5, -0.5);
                            } else {
                                set_speed(0, 0);
                            }
                            break;
                        case '7': //button 7 left arrow
                            if (bhit=='1') {
                                set_speed(-0.5, 0.5);
                            } else {
                                set_speed(0, 0);
                            }
                            break;
                        case '8': //button 8 right arrow
                            if (bhit=='1') {
                                set_speed(0.5, -0.5);
                            } else {
                                set_speed(0, 0);
                            }
                            break;
                        default:
                            break;
                    }
                }
            }
        }
        Thread::wait(50);
    }
}

inline void set_speed(float left, float right) {
    speed_left = left;
    speed_right = right;
}

inline void set_turret_delta(float delta) {
    turret_delta = delta;
}

inline void fire_cannon(bool f) {
    fire = f;
}