Kunal Mahajan
/
XbeeSend
Program for Sending an XBee packet from the controller to the MoboRobo.
main.cpp
- Committer:
- kunaljacy
- Date:
- 2012-05-05
- Revision:
- 0:f80ae940a2f6
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); } }