Kunal Mahajan
Program for Sending an XBee packet from the controller to the MoboRobo.
main.cpp@0:f80ae940a2f6, 2012-05-05 (annotated)
- Committer:
- kunaljacy
- Date:
- Sat May 05 04:07:45 2012 +0000
- Revision:
- 0:f80ae940a2f6
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| kunaljacy | 0:f80ae940a2f6 | 1 | #include "mbed.h" |
| kunaljacy | 0:f80ae940a2f6 | 2 | #include "MODSERIAL.h" |
| kunaljacy | 0:f80ae940a2f6 | 3 | |
| kunaljacy | 0:f80ae940a2f6 | 4 | /* Initialize Serial Ports for XBee Communication */ |
| kunaljacy | 0:f80ae940a2f6 | 5 | MODSERIAL xbee_send(p28, p27); // tx, rx |
| kunaljacy | 0:f80ae940a2f6 | 6 | Serial pc(USBTX, USBRX); |
| kunaljacy | 0:f80ae940a2f6 | 7 | |
| kunaljacy | 0:f80ae940a2f6 | 8 | /* Initialize ADC Pins for Sensors */ |
| kunaljacy | 0:f80ae940a2f6 | 9 | AnalogIn xAxisPin(p15); |
| kunaljacy | 0:f80ae940a2f6 | 10 | AnalogIn yAxisPin(p16); |
| kunaljacy | 0:f80ae940a2f6 | 11 | AnalogIn leftJoy(p19); // left joystick controls the left wheel speed - vertical |
| kunaljacy | 0:f80ae940a2f6 | 12 | AnalogIn rightJoy(p18); // right joystick controls the right wheel speed - vertical |
| kunaljacy | 0:f80ae940a2f6 | 13 | AnalogIn wrist(p20); // left joystick controls the wrist - horizontal |
| kunaljacy | 0:f80ae940a2f6 | 14 | AnalogIn elbow(p17); // right joystick controls the elbow - horizontal |
| kunaljacy | 0:f80ae940a2f6 | 15 | |
| kunaljacy | 0:f80ae940a2f6 | 16 | /* Initialize Digital Pins for gripper open and close */ |
| kunaljacy | 0:f80ae940a2f6 | 17 | DigitalIn open(p6); // left joystick select button |
| kunaljacy | 0:f80ae940a2f6 | 18 | DigitalIn close(p5); // right joystick select button |
| kunaljacy | 0:f80ae940a2f6 | 19 | DigitalIn mode(p30); // control the mode : user control or semi-autonomous |
| kunaljacy | 0:f80ae940a2f6 | 20 | |
| kunaljacy | 0:f80ae940a2f6 | 21 | DigitalOut l1(LED1); |
| kunaljacy | 0:f80ae940a2f6 | 22 | DigitalOut l2(LED2); |
| kunaljacy | 0:f80ae940a2f6 | 23 | DigitalOut l3(LED3); |
| kunaljacy | 0:f80ae940a2f6 | 24 | DigitalOut l4(LED4); |
| kunaljacy | 0:f80ae940a2f6 | 25 | |
| kunaljacy | 0:f80ae940a2f6 | 26 | short modeVal = 0; |
| kunaljacy | 0:f80ae940a2f6 | 27 | |
| kunaljacy | 0:f80ae940a2f6 | 28 | typedef struct { |
| kunaljacy | 0:f80ae940a2f6 | 29 | short xAxis; |
| kunaljacy | 0:f80ae940a2f6 | 30 | short yAxis; |
| kunaljacy | 0:f80ae940a2f6 | 31 | short grip; |
| kunaljacy | 0:f80ae940a2f6 | 32 | short left_velocity; |
| kunaljacy | 0:f80ae940a2f6 | 33 | short right_velocity; |
| kunaljacy | 0:f80ae940a2f6 | 34 | short wrist; |
| kunaljacy | 0:f80ae940a2f6 | 35 | short elbow; |
| kunaljacy | 0:f80ae940a2f6 | 36 | short mode; |
| kunaljacy | 0:f80ae940a2f6 | 37 | } xbee_packet; |
| kunaljacy | 0:f80ae940a2f6 | 38 | |
| kunaljacy | 0:f80ae940a2f6 | 39 | xbee_packet packet; |
| kunaljacy | 0:f80ae940a2f6 | 40 | |
| kunaljacy | 0:f80ae940a2f6 | 41 | short get_xAxis(); |
| kunaljacy | 0:f80ae940a2f6 | 42 | short get_yAxis(); |
| kunaljacy | 0:f80ae940a2f6 | 43 | short get_grip(); |
| kunaljacy | 0:f80ae940a2f6 | 44 | short get_left_velocity(); |
| kunaljacy | 0:f80ae940a2f6 | 45 | short get_right_velocity(); |
| kunaljacy | 0:f80ae940a2f6 | 46 | short get_wrist(); |
| kunaljacy | 0:f80ae940a2f6 | 47 | short get_elbow(); |
| kunaljacy | 0:f80ae940a2f6 | 48 | short get_mode(); |
| kunaljacy | 0:f80ae940a2f6 | 49 | |
| kunaljacy | 0:f80ae940a2f6 | 50 | void prepare_xbee_packet() { |
| kunaljacy | 0:f80ae940a2f6 | 51 | packet.xAxis = get_xAxis(); |
| kunaljacy | 0:f80ae940a2f6 | 52 | packet.yAxis = get_yAxis(); |
| kunaljacy | 0:f80ae940a2f6 | 53 | packet.grip = get_grip(); |
| kunaljacy | 0:f80ae940a2f6 | 54 | packet.left_velocity = get_left_velocity(); |
| kunaljacy | 0:f80ae940a2f6 | 55 | packet.right_velocity = get_right_velocity(); |
| kunaljacy | 0:f80ae940a2f6 | 56 | packet.wrist = get_wrist(); |
| kunaljacy | 0:f80ae940a2f6 | 57 | packet.elbow = get_elbow(); |
| kunaljacy | 0:f80ae940a2f6 | 58 | packet.mode = get_mode(); |
| kunaljacy | 0:f80ae940a2f6 | 59 | } |
| kunaljacy | 0:f80ae940a2f6 | 60 | |
| kunaljacy | 0:f80ae940a2f6 | 61 | void send_xbee_packet(xbee_packet packet) { |
| kunaljacy | 0:f80ae940a2f6 | 62 | char cs; |
| kunaljacy | 0:f80ae940a2f6 | 63 | |
| kunaljacy | 0:f80ae940a2f6 | 64 | xbee_send.putc(0xF1); |
| kunaljacy | 0:f80ae940a2f6 | 65 | pc.printf("%X\r\n", 0xF1); |
| kunaljacy | 0:f80ae940a2f6 | 66 | xbee_send.putc(0xE2); |
| kunaljacy | 0:f80ae940a2f6 | 67 | pc.printf("%X\r\n", 0xE2); |
| kunaljacy | 0:f80ae940a2f6 | 68 | xbee_send.putc(0x0A); |
| kunaljacy | 0:f80ae940a2f6 | 69 | pc.printf("%X\r\n", 0x0A); |
| kunaljacy | 0:f80ae940a2f6 | 70 | |
| kunaljacy | 0:f80ae940a2f6 | 71 | cs = 0x0A; |
| kunaljacy | 0:f80ae940a2f6 | 72 | |
| kunaljacy | 0:f80ae940a2f6 | 73 | // send xAxis |
| kunaljacy | 0:f80ae940a2f6 | 74 | xbee_send.putc((char) (packet.xAxis)); |
| kunaljacy | 0:f80ae940a2f6 | 75 | cs ^= ((char) (packet.xAxis)); |
| kunaljacy | 0:f80ae940a2f6 | 76 | xbee_send.putc((char) (packet.xAxis >> 8)); |
| kunaljacy | 0:f80ae940a2f6 | 77 | cs ^= ((char) (packet.xAxis >> 8)); |
| kunaljacy | 0:f80ae940a2f6 | 78 | pc.printf("Byte\r\n"); |
| kunaljacy | 0:f80ae940a2f6 | 79 | |
| kunaljacy | 0:f80ae940a2f6 | 80 | //send yAxis |
| kunaljacy | 0:f80ae940a2f6 | 81 | xbee_send.putc((char) (packet.yAxis)); |
| kunaljacy | 0:f80ae940a2f6 | 82 | cs ^= ((char) (packet.yAxis)); |
| kunaljacy | 0:f80ae940a2f6 | 83 | xbee_send.putc((char) (packet.yAxis >> 8)); |
| kunaljacy | 0:f80ae940a2f6 | 84 | cs ^= ((char) (packet.yAxis >> 8)); |
| kunaljacy | 0:f80ae940a2f6 | 85 | pc.printf("Byte\r\n"); |
| kunaljacy | 0:f80ae940a2f6 | 86 | |
| kunaljacy | 0:f80ae940a2f6 | 87 | // send grip |
| kunaljacy | 0:f80ae940a2f6 | 88 | xbee_send.putc((char) (packet.grip)); |
| kunaljacy | 0:f80ae940a2f6 | 89 | cs ^= ((char) (packet.grip)); |
| kunaljacy | 0:f80ae940a2f6 | 90 | xbee_send.putc((char) (packet.grip >> 8)); |
| kunaljacy | 0:f80ae940a2f6 | 91 | cs ^= ((char) (packet.grip >> 8)); |
| kunaljacy | 0:f80ae940a2f6 | 92 | pc.printf("Byte\r\n"); |
| kunaljacy | 0:f80ae940a2f6 | 93 | |
| kunaljacy | 0:f80ae940a2f6 | 94 | // send left_velocity |
| kunaljacy | 0:f80ae940a2f6 | 95 | xbee_send.putc((char) (packet.left_velocity)); |
| kunaljacy | 0:f80ae940a2f6 | 96 | cs ^= ((char) (packet.left_velocity)); |
| kunaljacy | 0:f80ae940a2f6 | 97 | xbee_send.putc((char) (packet.left_velocity >> 8)); |
| kunaljacy | 0:f80ae940a2f6 | 98 | cs ^= ((char) (packet.left_velocity >> 8)); |
| kunaljacy | 0:f80ae940a2f6 | 99 | pc.printf("Byte\r\n"); |
| kunaljacy | 0:f80ae940a2f6 | 100 | |
| kunaljacy | 0:f80ae940a2f6 | 101 | // send right_velocity |
| kunaljacy | 0:f80ae940a2f6 | 102 | xbee_send.putc((char) (packet.right_velocity)); |
| kunaljacy | 0:f80ae940a2f6 | 103 | cs ^= ((char) (packet.right_velocity)); |
| kunaljacy | 0:f80ae940a2f6 | 104 | xbee_send.putc((char) (packet.right_velocity >> 8)); |
| kunaljacy | 0:f80ae940a2f6 | 105 | cs ^= ((char) (packet.right_velocity >> 8)); |
| kunaljacy | 0:f80ae940a2f6 | 106 | pc.printf("Byte\r\n"); |
| kunaljacy | 0:f80ae940a2f6 | 107 | |
| kunaljacy | 0:f80ae940a2f6 | 108 | // send wrist |
| kunaljacy | 0:f80ae940a2f6 | 109 | xbee_send.putc((char) (packet.wrist)); |
| kunaljacy | 0:f80ae940a2f6 | 110 | cs ^= ((char) (packet.wrist)); |
| kunaljacy | 0:f80ae940a2f6 | 111 | xbee_send.putc((char) (packet.wrist >> 8)); |
| kunaljacy | 0:f80ae940a2f6 | 112 | cs ^= ((char) (packet.wrist >> 8)); |
| kunaljacy | 0:f80ae940a2f6 | 113 | pc.printf("Byte\r\n"); |
| kunaljacy | 0:f80ae940a2f6 | 114 | |
| kunaljacy | 0:f80ae940a2f6 | 115 | // send elbow |
| kunaljacy | 0:f80ae940a2f6 | 116 | xbee_send.putc((char) (packet.elbow)); |
| kunaljacy | 0:f80ae940a2f6 | 117 | cs ^= ((char) (packet.elbow)); |
| kunaljacy | 0:f80ae940a2f6 | 118 | xbee_send.putc((char) (packet.elbow >> 8)); |
| kunaljacy | 0:f80ae940a2f6 | 119 | cs ^= ((char) (packet.elbow >> 8)); |
| kunaljacy | 0:f80ae940a2f6 | 120 | pc.printf("Byte\r\n"); |
| kunaljacy | 0:f80ae940a2f6 | 121 | |
| kunaljacy | 0:f80ae940a2f6 | 122 | // send mode |
| kunaljacy | 0:f80ae940a2f6 | 123 | xbee_send.putc((char) (packet.mode)); |
| kunaljacy | 0:f80ae940a2f6 | 124 | cs ^= ((char) (packet.mode)); |
| kunaljacy | 0:f80ae940a2f6 | 125 | xbee_send.putc((char) (packet.mode >> 8)); |
| kunaljacy | 0:f80ae940a2f6 | 126 | cs ^= ((char) (packet.mode >> 8)); |
| kunaljacy | 0:f80ae940a2f6 | 127 | pc.printf("Byte\r\n"); |
| kunaljacy | 0:f80ae940a2f6 | 128 | |
| kunaljacy | 0:f80ae940a2f6 | 129 | xbee_send.putc(cs); |
| kunaljacy | 0:f80ae940a2f6 | 130 | pc.printf("%X\r\n", cs); |
| kunaljacy | 0:f80ae940a2f6 | 131 | } |
| kunaljacy | 0:f80ae940a2f6 | 132 | |
| kunaljacy | 0:f80ae940a2f6 | 133 | ///////////////////// GETTER METHODS ///////////////////// |
| kunaljacy | 0:f80ae940a2f6 | 134 | |
| kunaljacy | 0:f80ae940a2f6 | 135 | /* Returns the value of xAxis */ |
| kunaljacy | 0:f80ae940a2f6 | 136 | short get_xAxis() { |
| kunaljacy | 0:f80ae940a2f6 | 137 | // xAxis varies between 0.42 and 0.52 |
| kunaljacy | 0:f80ae940a2f6 | 138 | float x = xAxisPin.read(); |
| kunaljacy | 0:f80ae940a2f6 | 139 | int xVal = x * 500 - 250; |
| kunaljacy | 0:f80ae940a2f6 | 140 | if (xVal < -50){ |
| kunaljacy | 0:f80ae940a2f6 | 141 | return -50; |
| kunaljacy | 0:f80ae940a2f6 | 142 | } else if (xVal > 50) { |
| kunaljacy | 0:f80ae940a2f6 | 143 | return 50; |
| kunaljacy | 0:f80ae940a2f6 | 144 | } else if (xVal < 10 && xVal > -10) { |
| kunaljacy | 0:f80ae940a2f6 | 145 | return 0; |
| kunaljacy | 0:f80ae940a2f6 | 146 | } else { |
| kunaljacy | 0:f80ae940a2f6 | 147 | return xVal; |
| kunaljacy | 0:f80ae940a2f6 | 148 | } |
| kunaljacy | 0:f80ae940a2f6 | 149 | } |
| kunaljacy | 0:f80ae940a2f6 | 150 | |
| kunaljacy | 0:f80ae940a2f6 | 151 | /* Returns the value of yAxis */ |
| kunaljacy | 0:f80ae940a2f6 | 152 | short get_yAxis() { |
| kunaljacy | 0:f80ae940a2f6 | 153 | // yAxis varies between 0.42 and 0.52 |
| kunaljacy | 0:f80ae940a2f6 | 154 | float y = yAxisPin.read(); |
| kunaljacy | 0:f80ae940a2f6 | 155 | int yVal = y * 500 - 250; |
| kunaljacy | 0:f80ae940a2f6 | 156 | if (yVal < -50){ |
| kunaljacy | 0:f80ae940a2f6 | 157 | return -50; |
| kunaljacy | 0:f80ae940a2f6 | 158 | } else if (yVal > 50) { |
| kunaljacy | 0:f80ae940a2f6 | 159 | return 50; |
| kunaljacy | 0:f80ae940a2f6 | 160 | } else if (yVal < 10 && yVal > -10) { |
| kunaljacy | 0:f80ae940a2f6 | 161 | return 0; |
| kunaljacy | 0:f80ae940a2f6 | 162 | } else { |
| kunaljacy | 0:f80ae940a2f6 | 163 | return yVal; |
| kunaljacy | 0:f80ae940a2f6 | 164 | } |
| kunaljacy | 0:f80ae940a2f6 | 165 | } |
| kunaljacy | 0:f80ae940a2f6 | 166 | |
| kunaljacy | 0:f80ae940a2f6 | 167 | short get_grip(){ |
| kunaljacy | 0:f80ae940a2f6 | 168 | if (open && !close){ |
| kunaljacy | 0:f80ae940a2f6 | 169 | return 1; |
| kunaljacy | 0:f80ae940a2f6 | 170 | } else if (close && !open){ |
| kunaljacy | 0:f80ae940a2f6 | 171 | return -1; |
| kunaljacy | 0:f80ae940a2f6 | 172 | } else { |
| kunaljacy | 0:f80ae940a2f6 | 173 | return 0; |
| kunaljacy | 0:f80ae940a2f6 | 174 | } |
| kunaljacy | 0:f80ae940a2f6 | 175 | } |
| kunaljacy | 0:f80ae940a2f6 | 176 | |
| kunaljacy | 0:f80ae940a2f6 | 177 | short get_left_velocity(){ |
| kunaljacy | 0:f80ae940a2f6 | 178 | float left = leftJoy.read(); |
| kunaljacy | 0:f80ae940a2f6 | 179 | int leftVal = (left - 0.5) * 1000; |
| kunaljacy | 0:f80ae940a2f6 | 180 | if (leftVal < -500){ |
| kunaljacy | 0:f80ae940a2f6 | 181 | return -500; |
| kunaljacy | 0:f80ae940a2f6 | 182 | } else if (leftVal > 500) { |
| kunaljacy | 0:f80ae940a2f6 | 183 | return 500; |
| kunaljacy | 0:f80ae940a2f6 | 184 | } else if (leftVal < 30 && leftVal > -30) { |
| kunaljacy | 0:f80ae940a2f6 | 185 | return 0; |
| kunaljacy | 0:f80ae940a2f6 | 186 | } else { |
| kunaljacy | 0:f80ae940a2f6 | 187 | return leftVal; |
| kunaljacy | 0:f80ae940a2f6 | 188 | } |
| kunaljacy | 0:f80ae940a2f6 | 189 | } |
| kunaljacy | 0:f80ae940a2f6 | 190 | |
| kunaljacy | 0:f80ae940a2f6 | 191 | short get_right_velocity(){ |
| kunaljacy | 0:f80ae940a2f6 | 192 | float right = rightJoy.read(); |
| kunaljacy | 0:f80ae940a2f6 | 193 | int rightVal = (right - 0.5) * 1000; |
| kunaljacy | 0:f80ae940a2f6 | 194 | if (rightVal < -500){ |
| kunaljacy | 0:f80ae940a2f6 | 195 | return -500; |
| kunaljacy | 0:f80ae940a2f6 | 196 | } else if (rightVal > 500) { |
| kunaljacy | 0:f80ae940a2f6 | 197 | return 500; |
| kunaljacy | 0:f80ae940a2f6 | 198 | } else if (rightVal < 30 && rightVal > -30) { |
| kunaljacy | 0:f80ae940a2f6 | 199 | return 0; |
| kunaljacy | 0:f80ae940a2f6 | 200 | } else { |
| kunaljacy | 0:f80ae940a2f6 | 201 | return rightVal; |
| kunaljacy | 0:f80ae940a2f6 | 202 | } |
| kunaljacy | 0:f80ae940a2f6 | 203 | } |
| kunaljacy | 0:f80ae940a2f6 | 204 | |
| kunaljacy | 0:f80ae940a2f6 | 205 | short get_wrist(){ |
| kunaljacy | 0:f80ae940a2f6 | 206 | float wristty = wrist.read(); |
| kunaljacy | 0:f80ae940a2f6 | 207 | int wristVal = (wristty - 0.5) * 100; |
| kunaljacy | 0:f80ae940a2f6 | 208 | if (wristVal < -50){ |
| kunaljacy | 0:f80ae940a2f6 | 209 | return -50; |
| kunaljacy | 0:f80ae940a2f6 | 210 | } else if (wristVal > 50) { |
| kunaljacy | 0:f80ae940a2f6 | 211 | return 50; |
| kunaljacy | 0:f80ae940a2f6 | 212 | } else if (wristVal < 3 && wristVal > -3) { |
| kunaljacy | 0:f80ae940a2f6 | 213 | return 0; |
| kunaljacy | 0:f80ae940a2f6 | 214 | } else { |
| kunaljacy | 0:f80ae940a2f6 | 215 | return wristVal; |
| kunaljacy | 0:f80ae940a2f6 | 216 | } |
| kunaljacy | 0:f80ae940a2f6 | 217 | } |
| kunaljacy | 0:f80ae940a2f6 | 218 | |
| kunaljacy | 0:f80ae940a2f6 | 219 | short get_elbow(){ |
| kunaljacy | 0:f80ae940a2f6 | 220 | float elbowww = elbow.read(); |
| kunaljacy | 0:f80ae940a2f6 | 221 | int elbowVal = (elbowww - 0.5) * 100; |
| kunaljacy | 0:f80ae940a2f6 | 222 | if (elbowVal < -50){ |
| kunaljacy | 0:f80ae940a2f6 | 223 | return -50; |
| kunaljacy | 0:f80ae940a2f6 | 224 | } else if (elbowVal > 50) { |
| kunaljacy | 0:f80ae940a2f6 | 225 | return 50; |
| kunaljacy | 0:f80ae940a2f6 | 226 | } else if (elbowVal < 3 && elbowVal > -3) { |
| kunaljacy | 0:f80ae940a2f6 | 227 | return 0; |
| kunaljacy | 0:f80ae940a2f6 | 228 | } else { |
| kunaljacy | 0:f80ae940a2f6 | 229 | return elbowVal; |
| kunaljacy | 0:f80ae940a2f6 | 230 | } |
| kunaljacy | 0:f80ae940a2f6 | 231 | } |
| kunaljacy | 0:f80ae940a2f6 | 232 | |
| kunaljacy | 0:f80ae940a2f6 | 233 | short get_mode(){ |
| kunaljacy | 0:f80ae940a2f6 | 234 | if(!mode){ |
| kunaljacy | 0:f80ae940a2f6 | 235 | return modeVal; |
| kunaljacy | 0:f80ae940a2f6 | 236 | } |
| kunaljacy | 0:f80ae940a2f6 | 237 | while(mode); |
| kunaljacy | 0:f80ae940a2f6 | 238 | modeVal = !modeVal; |
| kunaljacy | 0:f80ae940a2f6 | 239 | return modeVal; |
| kunaljacy | 0:f80ae940a2f6 | 240 | } |
| kunaljacy | 0:f80ae940a2f6 | 241 | ///////////////////////////////////////////////////////////// |
| kunaljacy | 0:f80ae940a2f6 | 242 | |
| kunaljacy | 0:f80ae940a2f6 | 243 | int main() { |
| kunaljacy | 0:f80ae940a2f6 | 244 | // xbee_send_init(); |
| kunaljacy | 0:f80ae940a2f6 | 245 | while (1) { |
| kunaljacy | 0:f80ae940a2f6 | 246 | prepare_xbee_packet(); |
| kunaljacy | 0:f80ae940a2f6 | 247 | // light led |
| kunaljacy | 0:f80ae940a2f6 | 248 | if(modeVal){ // semi-autonomous robot |
| kunaljacy | 0:f80ae940a2f6 | 249 | l1 = 0; |
| kunaljacy | 0:f80ae940a2f6 | 250 | l2 = 0; |
| kunaljacy | 0:f80ae940a2f6 | 251 | l3 = 0; |
| kunaljacy | 0:f80ae940a2f6 | 252 | l4 = 0; |
| kunaljacy | 0:f80ae940a2f6 | 253 | } |
| kunaljacy | 0:f80ae940a2f6 | 254 | else{ // user control robot |
| kunaljacy | 0:f80ae940a2f6 | 255 | l1 = 1; |
| kunaljacy | 0:f80ae940a2f6 | 256 | l2 = 1; |
| kunaljacy | 0:f80ae940a2f6 | 257 | l3 = 1; |
| kunaljacy | 0:f80ae940a2f6 | 258 | l4 = 1; |
| kunaljacy | 0:f80ae940a2f6 | 259 | } |
| kunaljacy | 0:f80ae940a2f6 | 260 | send_xbee_packet(packet); |
| kunaljacy | 0:f80ae940a2f6 | 261 | wait_ms(10); |
| kunaljacy | 0:f80ae940a2f6 | 262 | } |
| kunaljacy | 0:f80ae940a2f6 | 263 | } |