Diff: main.cpp

Revision:

0:f80ae940a2f6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat May 05 04:07:45 2012 +0000
@@ -0,0 +1,263 @@
+#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);
+    }
+}