File content as of revision 0:f1fab1a647f6:

#include "mbed.h"
#include "rtos.h"
#include "Motor.h"
#include "ultrasonic.h"

Thread distance, headlightT;
Mutex motorSpeeds;

// DEBUG
Serial pc(USBTX, USBRX);
PwmOut led(LED1);
PwmOut led3(LED2);

PwmOut headlights(p24);

// Light sensor
AnalogIn photocell(p15);

// Bluetooth
Serial blue(p28, p27);
// Motors
Motor leftMotor(p22, p8, p9); // pwm, fwd, rev
Motor rightMotor(p21, p10, p11); // pwm, fwd, rev
 
// Motor controls
float leftMotorForwardMax = 1.0f, rightMotorForwardMax = 1.0f, 
    leftMotorBackwardMax = 1.0f, rightMotorBackwardMax = 1.0f,
    leftMotorCurrentSpeed = 0.0f, rightMotorCurrentSpeed = 0.0f,
    leftCompensationScale = 0.85f, rightCompensationScale = 1.0f;
const float MAX_BRAKING_DISTANCE = 800.0f;

void setMotorSpeeds(float leftScale = 1.0, float rightScale = 1.0) {
    leftScale *= leftCompensationScale;
    rightScale *= rightCompensationScale;
    leftMotorCurrentSpeed = (leftScale > 0.0) ? leftScale * leftMotorForwardMax : leftScale * leftMotorBackwardMax;
    rightMotorCurrentSpeed = (rightScale > 0.0) ? rightScale * rightMotorForwardMax : rightScale * rightMotorBackwardMax;
    //pc.printf("Setting left motor to %d\n", leftMotorCurrentSpeed);
}

void updateMotors() {
    // Set motors
    leftMotor.speed((leftMotorCurrentSpeed > leftMotorForwardMax) ? leftMotorForwardMax : leftMotorCurrentSpeed);
    rightMotor.speed((rightMotorCurrentSpeed > rightMotorForwardMax) ? rightMotorForwardMax : rightMotorCurrentSpeed);
    //pc.printf("Pushing %d to left motor\n", leftMotorCurrentSpeed);
}

void setForwardMaxSpeed(float left = 1.0, float right = 1.0) {
    leftMotorForwardMax = left;
    rightMotorForwardMax = right;
}  

void onDistanceChanged(int distance) {
    // Lock speeds before updating.
    motorSpeeds.lock();
    if (distance > MAX_BRAKING_DISTANCE) {
        // Reset motor max speeds
        setForwardMaxSpeed(1.0f, 1.0f);
        led = 1;
    } else if (distance > MAX_BRAKING_DISTANCE * 0.5){
        setForwardMaxSpeed(0.5f, 0.5f);
        led = 0.5;
    } else {
        setForwardMaxSpeed(0.0f, 0.0f);
        led = 0;
    }
    updateMotors();  
    // Unlock
    motorSpeeds.unlock();
}

ultrasonic mu(p6, p7, .1, 1, &onDistanceChanged);

void distanceThread() {
    mu.startUpdates(); // start measuring the distance
    while (true) {
        // Update sonar
        mu.checkDistance(); 
    }
}

void headlightThread() {
    while (true) {
        // Automatic Headlights
        headlights = 0.8f - photocell;
    }
}

int main() {
    distance.start(distanceThread);
    headlightT.start(headlightThread);
     // Read data
    char bnum = 0;
    while (true) {
         // Read controller input
        if (blue.getc()=='!') {
            pc.printf("Seeing bluetooth input");
            if (blue.getc()=='B') { //button data
                bnum = blue.getc(); //button number
                // Locks motor speeds
                motorSpeeds.lock();
                led3 = 0.0f;
                setMotorSpeeds(0.0f, 0.0f);
                if (bnum == '5') {
                    // Up arrow, go forward
                    setMotorSpeeds(1.0, 1.0);
                    pc.printf("Forward Arrow");
                    led3 = 1.0f;
                } 
                if (bnum == '6') {
                    // Down arrow, go backwards
                    setMotorSpeeds(-1.0, -1.0);
                    led3 = 1.0f;
                } 
                if (bnum == '7') {
                    // Left arrow, turn left
                    setMotorSpeeds(-1.0, 1.0);
                    led3 = 1.0f;
                } 
                if (bnum == '8') {
                    // Right arrow, turn right
                    setMotorSpeeds(1.0, -1.0);
                    led3 = 1.0f;
                }
                // Unlock
                motorSpeeds.unlock();
            } 
        }
        // Locks speeds before pushing to motors
        motorSpeeds.lock();
        updateMotors();
        motorSpeeds.unlock();
    }

}