File content as of revision 0:f80ae940a2f6:

#include "mbed.h"
#include "MODSERIAL.h"

/* Initialize Serial Ports for XBee Communication */
MODSERIAL xbee_send(p28, p27); // tx, rx
Serial pc(USBTX, USBRX);

/* Initialize ADC Pins for Sensors */
AnalogIn xAxisPin(p15);
AnalogIn yAxisPin(p16);
AnalogIn leftJoy(p19);  // left joystick controls the left wheel speed - vertical
AnalogIn rightJoy(p18); // right joystick controls the right wheel speed - vertical
AnalogIn wrist(p20);    // left joystick controls the wrist - horizontal
AnalogIn elbow(p17);   // right joystick controls the elbow - horizontal

/* Initialize Digital Pins for gripper open and close */
DigitalIn open(p6); // left joystick select button
DigitalIn close(p5); // right joystick select button
DigitalIn mode(p30); // control the mode : user control or semi-autonomous

DigitalOut l1(LED1);
DigitalOut l2(LED2);
DigitalOut l3(LED3);
DigitalOut l4(LED4);

short modeVal = 0;

typedef struct {
    short xAxis;
    short yAxis;
    short grip;
    short left_velocity;
    short right_velocity;
    short wrist;
    short elbow;
    short mode;
} xbee_packet;

xbee_packet packet;

short get_xAxis();
short get_yAxis();
short get_grip();
short get_left_velocity();
short get_right_velocity();
short get_wrist();
short get_elbow();
short get_mode();

void prepare_xbee_packet() {
    packet.xAxis = get_xAxis();
    packet.yAxis = get_yAxis();
    packet.grip = get_grip();
    packet.left_velocity = get_left_velocity();
    packet.right_velocity = get_right_velocity();
    packet.wrist = get_wrist();
    packet.elbow = get_elbow();
    packet.mode = get_mode();
}

void send_xbee_packet(xbee_packet packet) {
    char cs;
    
    xbee_send.putc(0xF1);
    pc.printf("%X\r\n", 0xF1);
    xbee_send.putc(0xE2);
    pc.printf("%X\r\n", 0xE2);
    xbee_send.putc(0x0A);
    pc.printf("%X\r\n", 0x0A);
    
    cs = 0x0A;
    
    // send xAxis
    xbee_send.putc((char) (packet.xAxis));
    cs ^= ((char) (packet.xAxis));
    xbee_send.putc((char) (packet.xAxis >> 8));
    cs ^= ((char) (packet.xAxis >> 8));
    pc.printf("Byte\r\n");
    
    //send yAxis
    xbee_send.putc((char) (packet.yAxis));
    cs ^= ((char) (packet.yAxis));
    xbee_send.putc((char) (packet.yAxis >> 8));
    cs ^= ((char) (packet.yAxis >> 8));
    pc.printf("Byte\r\n");
    
    // send grip 
    xbee_send.putc((char) (packet.grip));
    cs ^= ((char) (packet.grip));
    xbee_send.putc((char) (packet.grip >> 8));
    cs ^= ((char) (packet.grip >> 8));
    pc.printf("Byte\r\n");
    
    // send left_velocity
    xbee_send.putc((char) (packet.left_velocity));
    cs ^= ((char) (packet.left_velocity));
    xbee_send.putc((char) (packet.left_velocity >> 8));
    cs ^= ((char) (packet.left_velocity >> 8));
    pc.printf("Byte\r\n");
    
    // send right_velocity
    xbee_send.putc((char) (packet.right_velocity));
    cs ^= ((char) (packet.right_velocity));
    xbee_send.putc((char) (packet.right_velocity >> 8));
    cs ^= ((char) (packet.right_velocity >> 8));
    pc.printf("Byte\r\n");
    
    // send wrist
    xbee_send.putc((char) (packet.wrist));
    cs ^= ((char) (packet.wrist));
    xbee_send.putc((char) (packet.wrist >> 8));
    cs ^= ((char) (packet.wrist >> 8));
    pc.printf("Byte\r\n");

    // send elbow
    xbee_send.putc((char) (packet.elbow));
    cs ^= ((char) (packet.elbow));
    xbee_send.putc((char) (packet.elbow >> 8));
    cs ^= ((char) (packet.elbow >> 8));
    pc.printf("Byte\r\n");
    
    // send mode
    xbee_send.putc((char) (packet.mode));
    cs ^= ((char) (packet.mode));
    xbee_send.putc((char) (packet.mode >> 8));
    cs ^= ((char) (packet.mode >> 8));
    pc.printf("Byte\r\n");
            
    xbee_send.putc(cs);
    pc.printf("%X\r\n", cs);
}

///////////////////// GETTER METHODS /////////////////////

/* Returns the value of xAxis */
short get_xAxis() {
    // xAxis varies between 0.42 and 0.52
    float x = xAxisPin.read();
    int xVal = x * 500 - 250;
    if (xVal < -50){
        return -50;
    } else if (xVal > 50) {
        return 50;
    } else if (xVal < 10 && xVal > -10) {
        return 0;
    } else {
        return xVal;
    }
}

/* Returns the value of yAxis */
short get_yAxis() {
    // yAxis varies between 0.42 and 0.52
    float y = yAxisPin.read();
    int yVal = y * 500 - 250;
    if (yVal < -50){
        return -50;
    } else if (yVal > 50) {
        return 50;
    } else if (yVal < 10 && yVal > -10) {
        return 0;
    } else {
        return yVal;
    }
}

short get_grip(){
    if (open && !close){
        return 1;
    } else if (close && !open){
        return -1;
    } else {
    return 0;
    }
}

short get_left_velocity(){
    float left = leftJoy.read();
    int leftVal = (left - 0.5) * 1000;
    if (leftVal < -500){
        return -500;
    } else if (leftVal > 500) {
        return 500;
    } else if (leftVal < 30 && leftVal > -30) {
        return 0;
    } else {
        return leftVal;
    }
}

short get_right_velocity(){
     float right = rightJoy.read();
     int rightVal = (right - 0.5) * 1000;
    if (rightVal < -500){
        return -500;
    } else if (rightVal > 500) {
        return 500;
    } else if (rightVal < 30 && rightVal > -30) {
        return 0;
    } else {
        return rightVal;
    }
}

short get_wrist(){
    float wristty = wrist.read();
    int wristVal = (wristty - 0.5) * 100;
    if (wristVal < -50){
        return -50;
    } else if (wristVal > 50) {
        return 50;
    } else if (wristVal < 3 && wristVal > -3) {
        return 0;
    } else {
        return wristVal;
    }
}

short get_elbow(){
    float elbowww = elbow.read();
    int elbowVal = (elbowww - 0.5) * 100;
    if (elbowVal < -50){
        return -50;
    } else if (elbowVal > 50) {
        return 50;
    } else if (elbowVal < 3 && elbowVal > -3) {
        return 0;
    } else {
        return elbowVal;
    }
}

short get_mode(){
    if(!mode){
        return modeVal;
    }
    while(mode);
    modeVal = !modeVal;
    return modeVal;
}
/////////////////////////////////////////////////////////////

int main() {
  //  xbee_send_init();
    while (1) {
        prepare_xbee_packet();
        // light led 
        if(modeVal){        // semi-autonomous robot     
            l1 = 0;
            l2 = 0;
            l3 = 0;
            l4 = 0;
        }
        else{               // user control robot
            l1 = 1;
            l2 = 1;
            l3 = 1;
            l4 = 1;
        }
        send_xbee_packet(packet);
        wait_ms(10);
    }
}