main.cpp

00001 #include "mbed.h"
00002 #include "MODSERIAL.h"
00003 
00004 /* Initialize Serial Ports for XBee Communication */
00005 MODSERIAL xbee_send(p28, p27); // tx, rx
00006 Serial pc(USBTX, USBRX);
00007 
00008 /* Initialize ADC Pins for Sensors */
00009 AnalogIn xAxisPin(p15);
00010 AnalogIn yAxisPin(p16);
00011 AnalogIn leftJoy(p19);  // left joystick controls the left wheel speed - vertical
00012 AnalogIn rightJoy(p18); // right joystick controls the right wheel speed - vertical
00013 AnalogIn wrist(p20);    // left joystick controls the wrist - horizontal
00014 AnalogIn elbow(p17);   // right joystick controls the elbow - horizontal
00015 
00016 /* Initialize Digital Pins for gripper open and close */
00017 DigitalIn open(p6); // left joystick select button
00018 DigitalIn close(p5); // right joystick select button
00019 DigitalIn mode(p30); // control the mode : user control or semi-autonomous
00020 
00021 DigitalOut l1(LED1);
00022 DigitalOut l2(LED2);
00023 DigitalOut l3(LED3);
00024 DigitalOut l4(LED4);
00025 
00026 short modeVal = 0;
00027 
00028 typedef struct {
00029     short xAxis;
00030     short yAxis;
00031     short grip;
00032     short left_velocity;
00033     short right_velocity;
00034     short wrist;
00035     short elbow;
00036     short mode;
00037 } xbee_packet;
00038 
00039 xbee_packet packet;
00040 
00041 short get_xAxis();
00042 short get_yAxis();
00043 short get_grip();
00044 short get_left_velocity();
00045 short get_right_velocity();
00046 short get_wrist();
00047 short get_elbow();
00048 short get_mode();
00049 
00050 void prepare_xbee_packet() {
00051     packet.xAxis = get_xAxis();
00052     packet.yAxis = get_yAxis();
00053     packet.grip = get_grip();
00054     packet.left_velocity = get_left_velocity();
00055     packet.right_velocity = get_right_velocity();
00056     packet.wrist = get_wrist();
00057     packet.elbow = get_elbow();
00058     packet.mode = get_mode();
00059 }
00060 
00061 void send_xbee_packet(xbee_packet packet) {
00062     char cs;
00063     
00064     xbee_send.putc(0xF1);
00065     pc.printf("%X\r\n", 0xF1);
00066     xbee_send.putc(0xE2);
00067     pc.printf("%X\r\n", 0xE2);
00068     xbee_send.putc(0x0A);
00069     pc.printf("%X\r\n", 0x0A);
00070     
00071     cs = 0x0A;
00072     
00073     // send xAxis
00074     xbee_send.putc((char) (packet.xAxis));
00075     cs ^= ((char) (packet.xAxis));
00076     xbee_send.putc((char) (packet.xAxis >> 8));
00077     cs ^= ((char) (packet.xAxis >> 8));
00078     pc.printf("Byte\r\n");
00079     
00080     //send yAxis
00081     xbee_send.putc((char) (packet.yAxis));
00082     cs ^= ((char) (packet.yAxis));
00083     xbee_send.putc((char) (packet.yAxis >> 8));
00084     cs ^= ((char) (packet.yAxis >> 8));
00085     pc.printf("Byte\r\n");
00086     
00087     // send grip 
00088     xbee_send.putc((char) (packet.grip));
00089     cs ^= ((char) (packet.grip));
00090     xbee_send.putc((char) (packet.grip >> 8));
00091     cs ^= ((char) (packet.grip >> 8));
00092     pc.printf("Byte\r\n");
00093     
00094     // send left_velocity
00095     xbee_send.putc((char) (packet.left_velocity));
00096     cs ^= ((char) (packet.left_velocity));
00097     xbee_send.putc((char) (packet.left_velocity >> 8));
00098     cs ^= ((char) (packet.left_velocity >> 8));
00099     pc.printf("Byte\r\n");
00100     
00101     // send right_velocity
00102     xbee_send.putc((char) (packet.right_velocity));
00103     cs ^= ((char) (packet.right_velocity));
00104     xbee_send.putc((char) (packet.right_velocity >> 8));
00105     cs ^= ((char) (packet.right_velocity >> 8));
00106     pc.printf("Byte\r\n");
00107     
00108     // send wrist
00109     xbee_send.putc((char) (packet.wrist));
00110     cs ^= ((char) (packet.wrist));
00111     xbee_send.putc((char) (packet.wrist >> 8));
00112     cs ^= ((char) (packet.wrist >> 8));
00113     pc.printf("Byte\r\n");
00114 
00115     // send elbow
00116     xbee_send.putc((char) (packet.elbow));
00117     cs ^= ((char) (packet.elbow));
00118     xbee_send.putc((char) (packet.elbow >> 8));
00119     cs ^= ((char) (packet.elbow >> 8));
00120     pc.printf("Byte\r\n");
00121     
00122     // send mode
00123     xbee_send.putc((char) (packet.mode));
00124     cs ^= ((char) (packet.mode));
00125     xbee_send.putc((char) (packet.mode >> 8));
00126     cs ^= ((char) (packet.mode >> 8));
00127     pc.printf("Byte\r\n");
00128             
00129     xbee_send.putc(cs);
00130     pc.printf("%X\r\n", cs);
00131 }
00132 
00133 ///////////////////// GETTER METHODS /////////////////////
00134 
00135 /* Returns the value of xAxis */
00136 short get_xAxis() {
00137     // xAxis varies between 0.42 and 0.52
00138     float x = xAxisPin.read();
00139     int xVal = x * 500 - 250;
00140     if (xVal < -50){
00141         return -50;
00142     } else if (xVal > 50) {
00143         return 50;
00144     } else if (xVal < 10 && xVal > -10) {
00145         return 0;
00146     } else {
00147         return xVal;
00148     }
00149 }
00150 
00151 /* Returns the value of yAxis */
00152 short get_yAxis() {
00153     // yAxis varies between 0.42 and 0.52
00154     float y = yAxisPin.read();
00155     int yVal = y * 500 - 250;
00156     if (yVal < -50){
00157         return -50;
00158     } else if (yVal > 50) {
00159         return 50;
00160     } else if (yVal < 10 && yVal > -10) {
00161         return 0;
00162     } else {
00163         return yVal;
00164     }
00165 }
00166 
00167 short get_grip(){
00168     if (open && !close){
00169         return 1;
00170     } else if (close && !open){
00171         return -1;
00172     } else {
00173     return 0;
00174     }
00175 }
00176 
00177 short get_left_velocity(){
00178     float left = leftJoy.read();
00179     int leftVal = (left - 0.5) * 1000;
00180     if (leftVal < -500){
00181         return -500;
00182     } else if (leftVal > 500) {
00183         return 500;
00184     } else if (leftVal < 30 && leftVal > -30) {
00185         return 0;
00186     } else {
00187         return leftVal;
00188     }
00189 }
00190 
00191 short get_right_velocity(){
00192      float right = rightJoy.read();
00193      int rightVal = (right - 0.5) * 1000;
00194     if (rightVal < -500){
00195         return -500;
00196     } else if (rightVal > 500) {
00197         return 500;
00198     } else if (rightVal < 30 && rightVal > -30) {
00199         return 0;
00200     } else {
00201         return rightVal;
00202     }
00203 }
00204 
00205 short get_wrist(){
00206     float wristty = wrist.read();
00207     int wristVal = (wristty - 0.5) * 100;
00208     if (wristVal < -50){
00209         return -50;
00210     } else if (wristVal > 50) {
00211         return 50;
00212     } else if (wristVal < 3 && wristVal > -3) {
00213         return 0;
00214     } else {
00215         return wristVal;
00216     }
00217 }
00218 
00219 short get_elbow(){
00220     float elbowww = elbow.read();
00221     int elbowVal = (elbowww - 0.5) * 100;
00222     if (elbowVal < -50){
00223         return -50;
00224     } else if (elbowVal > 50) {
00225         return 50;
00226     } else if (elbowVal < 3 && elbowVal > -3) {
00227         return 0;
00228     } else {
00229         return elbowVal;
00230     }
00231 }
00232 
00233 short get_mode(){
00234     if(!mode){
00235         return modeVal;
00236     }
00237     while(mode);
00238     modeVal = !modeVal;
00239     return modeVal;
00240 }
00241 /////////////////////////////////////////////////////////////
00242 
00243 int main() {
00244   //  xbee_send_init();
00245     while (1) {
00246         prepare_xbee_packet();
00247         // light led 
00248         if(modeVal){        // semi-autonomous robot     
00249             l1 = 0;
00250             l2 = 0;
00251             l3 = 0;
00252             l4 = 0;
00253         }
00254         else{               // user control robot
00255             l1 = 1;
00256             l2 = 1;
00257             l3 = 1;
00258             l4 = 1;
00259         }
00260         send_xbee_packet(packet);
00261         wait_ms(10);
00262     }
00263 }