File content as of revision 2:3d1a5025c32a:

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

Serial pi(USBTX, USBRX);
InterruptIn leftEncoder(p22);
InterruptIn 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 float leftMotorSpeed = 0.0;
volatile float rightMotorSpeed = 0.0;
volatile int xpadState = 0; //left-right Dpad
volatile int ypadState = 0; //up-down Dpad
volatile int leftJoystickState = 0;
volatile int rightJoystickState = 0;
volatile int leftEncoderCount = 0;
volatile int rightEncoderCount = 0;
volatile bool interruptAttached = false;

void LeftCount()
{
    leftEncoderCount = leftEncoderCount + 1;
}

void RightCount()
{
    rightEncoderCount = rightEncoderCount + 1;
}

//rotate the pan/tilt servos
void RotateServos()
{
    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(xpadState == -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()
{
    if(leftJoystickState != 0 && rightJoystickState != 0 && leftJoystickState == rightJoystickState) //only need to match speeds if they're moving in same direction
    {
        leftMotor.speed(leftMotorSpeed);
        rightMotor.speed(rightMotorSpeed);
        if(interruptAttached == false)
        {
            leftEncoder.fall(&LeftCount);
            rightEncoder.fall(&RightCount);
            interruptAttached = true;
        }
        
        if(leftEncoderCount > rightEncoderCount)
        {
            leftMotor.speed(leftMotor.ReadSpeed() - 0.01);
            leftEncoderCount = 0;
            rightEncoderCount = 0;
        }
        else if(leftEncoderCount < rightEncoderCount)
        {
            rightMotor.speed(rightMotor.ReadSpeed() - 0.01);
            leftEncoderCount = 0;
            rightEncoderCount = 0;
        }
        else Thread::yield(); //if counts are same, nothing left to do
    }
    else //stop counting encoders when motor speeds unequal
    {
        leftMotor.speed(leftJoystickState);
        rightMotor.speed(rightJoystickState);
        if(interruptAttached == true)
        {
            leftEncoder.fall(NULL);
            rightEncoder.fall(NULL);
            interruptAttached = false;
            leftEncoderCount = 0;
            rightEncoderCount = 0;
        }
    }
}

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;
                }
                else if (rightMotNum == '1') {
                    rightJoystickState = 1;
                }
                else if (rightMotNum == '2') {
                    rightJoystickState = -1;
                }
            }
        }
        Thread::yield();
    }
}