File content as of revision 5:8500e9326652:

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

Serial pi(USBTX, USBRX);
DigitalIn leftEncoder(p22);
DigitalIn rightEncoder(p21);
Motor leftMotor(p23, p30, p29);
Motor rightMotor(p24, p5, p6);
Servo topServo(p25);
Servo bottomServo(p26);


volatile float topServoPos = 0.0;
volatile float bottomServoPos = 0.0;
volatile int xpadState = 0; //left-right Dpad
volatile int ypadState = 0; //up-down Dpad
volatile float leftJoystickState = 0.0;
volatile float rightJoystickState = 0.0;

//rotate the pan/tilt servos
void RotateServos()
{
    while(1){
    if(xpadState == 1)
    {
        bottomServoPos = bottomServoPos + 0.05;
        if(bottomServoPos > 1.0) bottomServoPos = 1.0;
    }
    else if(xpadState == -1)
    {
        bottomServoPos = bottomServoPos - 0.05;
        if(bottomServoPos < 0.0) bottomServoPos = 0.0;
    }
    
    if(ypadState == 1)
    {
        topServoPos = topServoPos + 0.05;
        if(topServoPos > 1.0) topServoPos = 1.0;
    }
    else if(ypadState == -1)
    {
        topServoPos = topServoPos - 0.05;
        if(topServoPos < 0.0) topServoPos = 0.0;
    }
    
    topServo = topServoPos;
    bottomServo = bottomServoPos;
    Thread::wait(150);
    }
}


//Sets and keeps motor speed matched using encoder output
void SpeedBalance()
{
    while(1){
    leftMotor.speed(leftJoystickState);
    rightMotor.speed(rightJoystickState);
    if(leftJoystickState != 0.0 && rightJoystickState != 0.0 && leftJoystickState == rightJoystickState) //only need to match speeds if they're moving in same direction
    {
        
    }
    }
}

int main() 
{
    Thread motorControl(SpeedBalance);
    Thread servoControl(RotateServos);
    
    char servoNum = '0';
    char leftMotNum = '0';
    char rightMotNum = '0';
    
    while(1) 
    {
        if(pi.readable()) //check if new command from pi available
        {
            if (pi.getc() == '!' ) {
                servoNum = pi.getc();
                if (servoNum == '0') {
                    xpadState = 0;
                    ypadState = 0;
                }
                else if (servoNum == '1') {
                    xpadState = -1;
                }
                else if (servoNum == '2') {
                    xpadState = 1;
                }
                else if (servoNum == '3') {
                    ypadState = 1;
                }
                else if (servoNum == '4') {
                    ypadState = -1;
                }
            }
            else if (pi.getc() == '(') {
                leftMotNum = pi.getc();
                if (leftMotNum == '0') {
                    leftJoystickState = 0;
                }
                else if (leftMotNum == '1') {
                    leftJoystickState = 1;
                }
                else if (leftMotNum == '2') {
                    leftJoystickState = -1;
                }
            }
            else if (pi.getc() == ')' ) {
                rightMotNum = pi.getc();
                if (rightMotNum == '0') {
                    rightJoystickState = 0.0;
                }
                else if (rightMotNum == '1') {
                    rightJoystickState = 1.0;
                }
                else if (rightMotNum == '2') {
                    rightJoystickState = -1.0;
                }
            }
        }
        Thread::yield();
    }
}