File content as of revision 9:431b67beb44d:

#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 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; //keep track of encoder ticks
volatile int rightEncoderCount = 0;

//increment left encoder counts
void LeftCount()
{
    leftEncoderCount = leftEncoderCount + 1;
}

//increment right encoder counts
void RightCount()
{
    rightEncoderCount = rightEncoderCount + 1;
}

//rotate the pan/tilt servos
void RotateServos()
{
    float topServoPos = 0.0;
    float bottomServoPos = 0.0;
    
    while(1){
    if(xpadState == -1) //pan right
    {
        bottomServoPos = bottomServoPos + 0.05;
        if(bottomServoPos > 1.0) bottomServoPos = 1.0;
    }
    else if(xpadState == 1) //pan left
    {
        bottomServoPos = bottomServoPos - 0.05;
        if(bottomServoPos < 0.0) bottomServoPos = 0.0;
    }
    
    if(ypadState == 1) //tilt up
    {
        topServoPos = topServoPos + 0.05;
        if(topServoPos > 1.0) topServoPos = 1.0;
    }
    else if(ypadState == -1) //tilt down
    {
        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()
{
    bool interruptAttached = false; //flag indicating whether encoders are being counted
    bool speedSet = false; //flag indicating if the initial motor speed has been set before switching to encoder control
    
    //used when speed adjusting using encoders
    float leftMotorSpeed = 0.0;
    float rightMotorSpeed = 0.0;
    
    int encoderCountDifference = 0;
    int countDifferenceTolerance = 2; //How many counts difference between encoders is allowed
    
    while(1){
    if(leftJoystickState != 0 && rightJoystickState != 0 && leftJoystickState == rightJoystickState) //only need to match speeds if they're moving in same direction
    {
        if(speedSet == false) //set motor speed initally
        {
            leftMotorSpeed = leftJoystickState;
            rightMotorSpeed = rightJoystickState;
            speedSet = true;
        }
        
        //start counting encoders if not already
        if(interruptAttached == false)
        {
            leftEncoder.fall(&LeftCount);
            rightEncoder.fall(&RightCount);
            interruptAttached = true;
        }
        
        encoderCountDifference = leftEncoderCount - rightEncoderCount;
        if(encoderCountDifference > countDifferenceTolerance) //if left encoder counted more, left motor is faster
        {
            //right motor stays constant, left motor slows down to follow it
            rightMotor.speed(rightJoystickState);
            
            //add or subtract motor speed, depending on if its positive or negative
            if(leftMotorSpeed < 0){
                leftMotorSpeed = leftMotorSpeed + 0.01;
                leftMotor.speed(leftMotorSpeed);
            }
            else if(leftMotorSpeed > 0){
                leftMotorSpeed = leftMotorSpeed - 0.01;
                leftMotor.speed(leftMotorSpeed);
            }
            leftEncoderCount = 0; //reset encoder counts
            rightEncoderCount = 0;
            Thread::yield();
        }
        else if(encoderCountDifference < (-1 * countDifferenceTolerance)) //if left encoder counted less, right motor is faster
        {
            //left motor speed stays constant, right motor slows down to follow it
            leftMotor.speed(leftJoystickState);
            
            //add or subtract motor speed, depending on if its positive or negative
            if(leftMotorSpeed < 0){
                rightMotorSpeed = rightMotorSpeed + 0.01;
                rightMotor.speed(leftMotorSpeed);
            }
            else if(leftMotorSpeed > 0){
                rightMotorSpeed = rightMotorSpeed - 0.01;
                rightMotor.speed(leftMotorSpeed);
            }
            leftEncoderCount = 0; //reset encoder counts
            rightEncoderCount = 0;
            Thread::yield();
        }
        else //if counts within tolerances, keep speeds the same
        {
            rightMotor.speed(rightMotorSpeed);
            leftMotor.speed(leftMotorSpeed);
            Thread::yield(); //if counts are same, nothing left to do
        } 
    }
    else //The case where joysticks arent in the same direction
    {
        //set motors using joystick states, since motor speed wont need to be dynamically changed in this case
        leftMotor.speed(leftJoystickState);
        rightMotor.speed(rightJoystickState);
        speedSet = false;
        
        //Stop counting encoders because they aren't being used
        if(interruptAttached == true)
        {
            leftEncoder.fall(NULL);
            rightEncoder.fall(NULL);
            interruptAttached = false;
            leftEncoderCount = 0;
            rightEncoderCount = 0;
        }
        Thread::yield();
    }
    }
}

int main() 
{
    Thread motorControl(SpeedBalance);
    Thread servoControl(RotateServos);
    
    char servoNum = '0';
    char leftMotNum = '0';
    char rightMotNum = '0';
    char initialCheck = '0';
    
    while(1) 
    {
        if(pi.readable()) //check if new command from pi available
        {
            initialCheck = pi.getc();
            if (initialCheck == '!' ) { //look for dpad update
                servoNum = pi.getc();
                if (servoNum == '0') { //dpad released
                    xpadState = 0;
                    ypadState = 0;
                }
                else if (servoNum == '1') { //dpad left
                    xpadState = -1;
                }
                else if (servoNum == '2') { //dpad right
                    xpadState = 1;
                }
                else if (servoNum == '3') { //dpad up
                    ypadState = 1;
                }
                else if (servoNum == '4') { //dpad down
                    ypadState = -1;
                }
            }
            else if (initialCheck == '(') { //look for left joystick update
                leftMotNum = pi.getc();
                if (leftMotNum == '0') { //joystick center
                    leftJoystickState = 0;
                }
                else if (leftMotNum == '1') { //joystick up
                    leftJoystickState = 1;
                }
                else if (leftMotNum == '2') { //joystick down
                    leftJoystickState = -1;
                }
            }
            else if (initialCheck == ')' ) { //look for right joystick update
                rightMotNum = pi.getc();
                if (rightMotNum == '0') { //joystick center
                    rightJoystickState = 0;
                }
                else if (rightMotNum == '1') { //joystick up
                    rightJoystickState = 1;
                }
                else if (rightMotNum == '2') { //joystick down
                    rightJoystickState = -1;
                }
            }
        }
    }
}