File content as of revision 0:316cfb89e9cd:

// full-speed reverse (-1.0)
// full speed forwards (1.0)

#include "mbed.h"
#include "motordriver.h"
#include "Servo.h"

Motor left(p22, p12, p11, 1); // pwm, fwd, rev (Motor serialChar)
Motor right(p21, p10, p9, 1); // pwm, fwd, rev (Motor B)
Servo shoulder(p23);
Servo elbow(p24);
Servo wrist(p26);
Servo wrist(p25);
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);


//set up xbee
Serial xbee(p13, p14);//TX not used
Serial pc(USBTX, USBRX);
DigitalOut rst(p7);

//speed variables for motors
volatile float speed1;
volatile float speed2;

void go(){
    left.speed(speed1);
    right.speed(speed2);    
}

int main() {
    
    // reset xbee (at least 200ns)
    rst = 0;
    wait_ms(1);
    rst = 1;
    wait_ms(1); 
    
    //initial speed is 0
    speed1 = 0.0;
    speed2 = 0.0;
    
    //variables to hold serial characters for xbee
    char serialChar;
    char prev;
    
    //varaibles for arm servos
    volatile  float shoulderNum = 0.5;
    volatile  float elbowNum = 0.10;
    volatile  float wristNum = 0.0;
    
    //loop
    while (1) {

        if(xbee.readable()){
            prev = serialChar;
            serialChar = xbee.getc();
            
            
            //left right rotation
            if (prev == 'a' && serialChar < 60){
                shoulderNum = shoulderNum +  0.01;
                if (shoulderNum > 1)
                    shoulderNum = 1;
            shoulder = shoulderNum;
            led1 = 1;
            }
            else if (prev == 'a' && serialChar > 110){
                shoulderNum = shoulderNum - 0.01;
                if (shoulderNum < 0)
                    shoulderNum = 0;
            shoulder = shoulderNum;
            led2 = 1;
            }
            else{
                shoulder = shoulderNum;
            }
            
            
            //up down rotation
            if (prev == 'b' && serialChar < 60){
                elbowNum = elbowNum +  0.01;
                if (elbowNum > 1)
                    elbowNum = 1;
                elbow = elbowNum;
                led1 = 1;
            }
            else if (prev == 'b' && serialChar > 110){
                elbowNum = elbowNum - 0.01;
                if (elbowNum < 0)
                    elbowNum = 0;
                elbow = elbowNum;
                led2 = 1;
            }
            else{
                elbow = elbowNum;
            }
            
            //motor rotation
            if (prev == 'c' && serialChar < 60){
                speed1 = 1.0;
                speed2 = -1.0;
                go();
            }
            else if (prev == 'c' && serialChar > 110){
                speed1 = -1;
                speed2 = 1;
                go();
            }
            else if (prev == 'c' && serialChar > 60 && serialChar < 110 && speed1 != speed2){
                speed1 = 0;
                speed2 = 0;
                go();
            }
            
            //motor forward and reverse
            if (prev == 'd' && serialChar < 60){
                speed1 = -1.0;
                speed2 = -1.0;
                go();
            }
            else if (prev == 'd' && serialChar > 110){
                speed1 = 1;
                speed2 = 1;
                go();
            }
            else if (prev == 'd' && serialChar > 60 && serialChar < 110 && speed1 == speed2){
                speed1 = 0;
                speed2 = 0;
                go();
            }
            
            
            
            
            //wrist controller
            if (prev == 'e' && serialChar == 36){
                wristNum = wristNum +  0.01;
                if (wristNum > 1)
                    wristNum = 1;
                wrist = wristNum;
                led1 = 1;
            }
            else if (prev == 'f' && serialChar == 36){
                wristNum = wristNum - 0.01;
                if (wristNum < 0)
                    wristNum = 0;
                wrist = wristNum;
                led2 = 1;
            }
            else{
                wrist = wristNum;
            }
            
            //print to pc for debugging (Tera Term etc.)
            pc.putc(serialChar);
        }
    }
}