Jacob Katz
This is the code we used in our final implementation of our spherical robot.
main.cpp@0:63c4c959e87f, 2012-05-06 (annotated)
- Committer:
- katzjacob
- Date:
- Sun May 06 05:41:04 2012 +0000
- Revision:
- 0:63c4c959e87f
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| katzjacob | 0:63c4c959e87f | 1 | #include "mbed.h" |
| katzjacob | 0:63c4c959e87f | 2 | #include "MODSERIAL.h" |
| katzjacob | 0:63c4c959e87f | 3 | |
| katzjacob | 0:63c4c959e87f | 4 | |
| katzjacob | 0:63c4c959e87f | 5 | DigitalOut myled1(LED1); |
| katzjacob | 0:63c4c959e87f | 6 | DigitalOut myled2(LED2); |
| katzjacob | 0:63c4c959e87f | 7 | DigitalOut myled3(LED3); |
| katzjacob | 0:63c4c959e87f | 8 | DigitalOut myled4(LED4); |
| katzjacob | 0:63c4c959e87f | 9 | |
| katzjacob | 0:63c4c959e87f | 10 | |
| katzjacob | 0:63c4c959e87f | 11 | //motor a |
| katzjacob | 0:63c4c959e87f | 12 | PwmOut motFront(p21); |
| katzjacob | 0:63c4c959e87f | 13 | DigitalOut frontIN1(p24); |
| katzjacob | 0:63c4c959e87f | 14 | DigitalOut frontIN2(p25); |
| katzjacob | 0:63c4c959e87f | 15 | |
| katzjacob | 0:63c4c959e87f | 16 | //motor b |
| katzjacob | 0:63c4c959e87f | 17 | PwmOut motLeft(p22); |
| katzjacob | 0:63c4c959e87f | 18 | DigitalOut leftIN1(p27); |
| katzjacob | 0:63c4c959e87f | 19 | DigitalOut leftIN2(p26); |
| katzjacob | 0:63c4c959e87f | 20 | |
| katzjacob | 0:63c4c959e87f | 21 | //motor c/d - motors are same in control just opposite direction so hardware can accomplish this |
| katzjacob | 0:63c4c959e87f | 22 | PwmOut motRight(p23); |
| katzjacob | 0:63c4c959e87f | 23 | DigitalOut rightIN1(p28); |
| katzjacob | 0:63c4c959e87f | 24 | DigitalOut rightIN2(p29); |
| katzjacob | 0:63c4c959e87f | 25 | |
| katzjacob | 0:63c4c959e87f | 26 | //motor standby |
| katzjacob | 0:63c4c959e87f | 27 | DigitalOut motRLstby(p30); |
| katzjacob | 0:63c4c959e87f | 28 | DigitalOut motFstby(p20); |
| katzjacob | 0:63c4c959e87f | 29 | |
| katzjacob | 0:63c4c959e87f | 30 | //xBee configurations |
| katzjacob | 0:63c4c959e87f | 31 | MODSERIAL pc(USBTX, USBRX); // tx, rx |
| katzjacob | 0:63c4c959e87f | 32 | MODSERIAL xbee(p13, p14); //tx, rx |
| katzjacob | 0:63c4c959e87f | 33 | //MODSERIAL bluetooth(p9, p10); |
| katzjacob | 0:63c4c959e87f | 34 | DigitalOut rstXbee(p15); |
| katzjacob | 0:63c4c959e87f | 35 | |
| katzjacob | 0:63c4c959e87f | 36 | //motor globals |
| katzjacob | 0:63c4c959e87f | 37 | int rightMotor; |
| katzjacob | 0:63c4c959e87f | 38 | int leftMotor; |
| katzjacob | 0:63c4c959e87f | 39 | |
| katzjacob | 0:63c4c959e87f | 40 | DigitalOut LEDlow(p19); |
| katzjacob | 0:63c4c959e87f | 41 | DigitalOut LEDmid(p18); |
| katzjacob | 0:63c4c959e87f | 42 | DigitalOut LEDhigh(p17); |
| katzjacob | 0:63c4c959e87f | 43 | DigitalOut LEDantenna(p16); |
| katzjacob | 0:63c4c959e87f | 44 | |
| katzjacob | 0:63c4c959e87f | 45 | /************************************************* |
| katzjacob | 0:63c4c959e87f | 46 | function to receive packets from controller xbee |
| katzjacob | 0:63c4c959e87f | 47 | *************************************************/ |
| katzjacob | 0:63c4c959e87f | 48 | void xbeeReceive() |
| katzjacob | 0:63c4c959e87f | 49 | { |
| katzjacob | 0:63c4c959e87f | 50 | char packetFound = 0; |
| katzjacob | 0:63c4c959e87f | 51 | char currentByte; |
| katzjacob | 0:63c4c959e87f | 52 | if(xbee.rxBufferGetCount() == 0) |
| katzjacob | 0:63c4c959e87f | 53 | { |
| katzjacob | 0:63c4c959e87f | 54 | return; |
| katzjacob | 0:63c4c959e87f | 55 | } |
| katzjacob | 0:63c4c959e87f | 56 | while(xbee.rxBufferGetCount() >= 5) |
| katzjacob | 0:63c4c959e87f | 57 | { |
| katzjacob | 0:63c4c959e87f | 58 | currentByte = xbee.getc(); |
| katzjacob | 0:63c4c959e87f | 59 | if(currentByte == 0xAA) |
| katzjacob | 0:63c4c959e87f | 60 | { |
| katzjacob | 0:63c4c959e87f | 61 | currentByte = xbee.getc(); |
| katzjacob | 0:63c4c959e87f | 62 | if(currentByte == 0xBB) |
| katzjacob | 0:63c4c959e87f | 63 | { |
| katzjacob | 0:63c4c959e87f | 64 | //pc.printf("found the preamble"); |
| katzjacob | 0:63c4c959e87f | 65 | currentByte = xbee.getc(); |
| katzjacob | 0:63c4c959e87f | 66 | if(currentByte == 0x02) |
| katzjacob | 0:63c4c959e87f | 67 | { |
| katzjacob | 0:63c4c959e87f | 68 | //pc.printf("found a packet"); |
| katzjacob | 0:63c4c959e87f | 69 | packetFound = 1; |
| katzjacob | 0:63c4c959e87f | 70 | break; |
| katzjacob | 0:63c4c959e87f | 71 | } |
| katzjacob | 0:63c4c959e87f | 72 | } |
| katzjacob | 0:63c4c959e87f | 73 | } |
| katzjacob | 0:63c4c959e87f | 74 | } |
| katzjacob | 0:63c4c959e87f | 75 | if(packetFound == 1) |
| katzjacob | 0:63c4c959e87f | 76 | { |
| katzjacob | 0:63c4c959e87f | 77 | //pc.printf("reading the packet"); |
| katzjacob | 0:63c4c959e87f | 78 | //read in the motor commands |
| katzjacob | 0:63c4c959e87f | 79 | int rightTemp = xbee.getc(); |
| katzjacob | 0:63c4c959e87f | 80 | int leftTemp = xbee.getc(); |
| katzjacob | 0:63c4c959e87f | 81 | //calculate checksum |
| katzjacob | 0:63c4c959e87f | 82 | char checkSumCalc = 0; |
| katzjacob | 0:63c4c959e87f | 83 | checkSumCalc ^= rightTemp; |
| katzjacob | 0:63c4c959e87f | 84 | checkSumCalc ^= leftTemp; |
| katzjacob | 0:63c4c959e87f | 85 | char checkSumSent = xbee.getc(); |
| katzjacob | 0:63c4c959e87f | 86 | if(checkSumCalc == checkSumSent) |
| katzjacob | 0:63c4c959e87f | 87 | { |
| katzjacob | 0:63c4c959e87f | 88 | rightMotor = rightTemp; |
| katzjacob | 0:63c4c959e87f | 89 | leftMotor = leftTemp; |
| katzjacob | 0:63c4c959e87f | 90 | } |
| katzjacob | 0:63c4c959e87f | 91 | else |
| katzjacob | 0:63c4c959e87f | 92 | { |
| katzjacob | 0:63c4c959e87f | 93 | // pc.printf("\n bad packet \n"); |
| katzjacob | 0:63c4c959e87f | 94 | } |
| katzjacob | 0:63c4c959e87f | 95 | } |
| katzjacob | 0:63c4c959e87f | 96 | } |
| katzjacob | 0:63c4c959e87f | 97 | |
| katzjacob | 0:63c4c959e87f | 98 | /************************************************* |
| katzjacob | 0:63c4c959e87f | 99 | main function |
| katzjacob | 0:63c4c959e87f | 100 | *************************************************/ |
| katzjacob | 0:63c4c959e87f | 101 | int main() { |
| katzjacob | 0:63c4c959e87f | 102 | //reset the xbee |
| katzjacob | 0:63c4c959e87f | 103 | rstXbee = 0; |
| katzjacob | 0:63c4c959e87f | 104 | wait_ms(1); |
| katzjacob | 0:63c4c959e87f | 105 | rstXbee=1; |
| katzjacob | 0:63c4c959e87f | 106 | wait_ms(1); |
| katzjacob | 0:63c4c959e87f | 107 | |
| katzjacob | 0:63c4c959e87f | 108 | //led reset verification |
| katzjacob | 0:63c4c959e87f | 109 | myled1 = 1; |
| katzjacob | 0:63c4c959e87f | 110 | wait(0.1); |
| katzjacob | 0:63c4c959e87f | 111 | myled2 = 1; |
| katzjacob | 0:63c4c959e87f | 112 | myled1 = 0; |
| katzjacob | 0:63c4c959e87f | 113 | wait(0.1); |
| katzjacob | 0:63c4c959e87f | 114 | myled2=0; |
| katzjacob | 0:63c4c959e87f | 115 | |
| katzjacob | 0:63c4c959e87f | 116 | //initialize motor stuff |
| katzjacob | 0:63c4c959e87f | 117 | motRLstby=1; |
| katzjacob | 0:63c4c959e87f | 118 | motFstby=1; |
| katzjacob | 0:63c4c959e87f | 119 | motFront = motLeft = motRight=0; |
| katzjacob | 0:63c4c959e87f | 120 | frontIN1 = 0; |
| katzjacob | 0:63c4c959e87f | 121 | frontIN2 = 0; |
| katzjacob | 0:63c4c959e87f | 122 | leftIN1 = 0; |
| katzjacob | 0:63c4c959e87f | 123 | leftIN2 = 0; |
| katzjacob | 0:63c4c959e87f | 124 | rightIN1 = 0; |
| katzjacob | 0:63c4c959e87f | 125 | rightIN2 = 0; |
| katzjacob | 0:63c4c959e87f | 126 | |
| katzjacob | 0:63c4c959e87f | 127 | rightMotor = 0; |
| katzjacob | 0:63c4c959e87f | 128 | leftMotor = 0; |
| katzjacob | 0:63c4c959e87f | 129 | |
| katzjacob | 0:63c4c959e87f | 130 | LEDlow=LEDmid=LEDhigh=LEDantenna=0; |
| katzjacob | 0:63c4c959e87f | 131 | |
| katzjacob | 0:63c4c959e87f | 132 | while(1) { |
| katzjacob | 0:63c4c959e87f | 133 | |
| katzjacob | 0:63c4c959e87f | 134 | // bluetooth.putc('q'); |
| katzjacob | 0:63c4c959e87f | 135 | |
| katzjacob | 0:63c4c959e87f | 136 | |
| katzjacob | 0:63c4c959e87f | 137 | xbeeReceive(); //read data from xbee |
| katzjacob | 0:63c4c959e87f | 138 | // printf("right: %d ", rightMotor); |
| katzjacob | 0:63c4c959e87f | 139 | // printf("left: %d \r\n", leftMotor); |
| katzjacob | 0:63c4c959e87f | 140 | |
| katzjacob | 0:63c4c959e87f | 141 | //rightMotor=rightMotor-8; |
| katzjacob | 0:63c4c959e87f | 142 | //motor control |
| katzjacob | 0:63c4c959e87f | 143 | |
| katzjacob | 0:63c4c959e87f | 144 | //right motor |
| katzjacob | 0:63c4c959e87f | 145 | if(rightMotor <= 135 && rightMotor >= 128) |
| katzjacob | 0:63c4c959e87f | 146 | { |
| katzjacob | 0:63c4c959e87f | 147 | //turn off motor |
| katzjacob | 0:63c4c959e87f | 148 | motRight = 0.0; |
| katzjacob | 0:63c4c959e87f | 149 | |
| katzjacob | 0:63c4c959e87f | 150 | } |
| katzjacob | 0:63c4c959e87f | 151 | else if(rightMotor > 135) |
| katzjacob | 0:63c4c959e87f | 152 | { |
| katzjacob | 0:63c4c959e87f | 153 | //right motor forward |
| katzjacob | 0:63c4c959e87f | 154 | rightIN1=1; |
| katzjacob | 0:63c4c959e87f | 155 | rightIN2=0; |
| katzjacob | 0:63c4c959e87f | 156 | motRight = (((float)rightMotor-135.0)/120.0); |
| katzjacob | 0:63c4c959e87f | 157 | } |
| katzjacob | 0:63c4c959e87f | 158 | else if(rightMotor < 128) |
| katzjacob | 0:63c4c959e87f | 159 | { |
| katzjacob | 0:63c4c959e87f | 160 | //right motor backwards |
| katzjacob | 0:63c4c959e87f | 161 | rightIN1=0; |
| katzjacob | 0:63c4c959e87f | 162 | rightIN2=1; |
| katzjacob | 0:63c4c959e87f | 163 | motRight = ((255.0-(float)rightMotor)/255.0); |
| katzjacob | 0:63c4c959e87f | 164 | } |
| katzjacob | 0:63c4c959e87f | 165 | //left motor |
| katzjacob | 0:63c4c959e87f | 166 | if(leftMotor <= 142 && leftMotor >= 136) |
| katzjacob | 0:63c4c959e87f | 167 | { |
| katzjacob | 0:63c4c959e87f | 168 | //turn off motor |
| katzjacob | 0:63c4c959e87f | 169 | motLeft = 0.0; |
| katzjacob | 0:63c4c959e87f | 170 | |
| katzjacob | 0:63c4c959e87f | 171 | } |
| katzjacob | 0:63c4c959e87f | 172 | |
| katzjacob | 0:63c4c959e87f | 173 | else if(leftMotor > 142) |
| katzjacob | 0:63c4c959e87f | 174 | { |
| katzjacob | 0:63c4c959e87f | 175 | //left motor forward |
| katzjacob | 0:63c4c959e87f | 176 | leftIN1=0; |
| katzjacob | 0:63c4c959e87f | 177 | leftIN2=1; |
| katzjacob | 0:63c4c959e87f | 178 | motLeft = (((float)leftMotor-142.0)/113.0); |
| katzjacob | 0:63c4c959e87f | 179 | } |
| katzjacob | 0:63c4c959e87f | 180 | else if(leftMotor < 136) |
| katzjacob | 0:63c4c959e87f | 181 | { |
| katzjacob | 0:63c4c959e87f | 182 | //left motor backwards |
| katzjacob | 0:63c4c959e87f | 183 | leftIN1=1; |
| katzjacob | 0:63c4c959e87f | 184 | leftIN2=0; |
| katzjacob | 0:63c4c959e87f | 185 | motLeft = ((255.0-(float)leftMotor)/255.0); |
| katzjacob | 0:63c4c959e87f | 186 | } |
| katzjacob | 0:63c4c959e87f | 187 | //front and back motors |
| katzjacob | 0:63c4c959e87f | 188 | if(rightMotor < 15 && leftMotor > 240) //only kick in if there is a large different in directions of motors |
| katzjacob | 0:63c4c959e87f | 189 | { |
| katzjacob | 0:63c4c959e87f | 190 | //rotate clockwise |
| katzjacob | 0:63c4c959e87f | 191 | motFstby=1; |
| katzjacob | 0:63c4c959e87f | 192 | frontIN1 = 1; |
| katzjacob | 0:63c4c959e87f | 193 | frontIN2 = 0; |
| katzjacob | 0:63c4c959e87f | 194 | motFront = 1; |
| katzjacob | 0:63c4c959e87f | 195 | LEDantenna=LEDlow=LEDmid=LEDhigh=1; |
| katzjacob | 0:63c4c959e87f | 196 | // printf(" |
| katzjacob | 0:63c4c959e87f | 197 | |
| katzjacob | 0:63c4c959e87f | 198 | |
| katzjacob | 0:63c4c959e87f | 199 | } |
| katzjacob | 0:63c4c959e87f | 200 | else if(leftMotor < 15 && rightMotor > 240) |
| katzjacob | 0:63c4c959e87f | 201 | { |
| katzjacob | 0:63c4c959e87f | 202 | //rotate counter-clockwise |
| katzjacob | 0:63c4c959e87f | 203 | motFstby=1; |
| katzjacob | 0:63c4c959e87f | 204 | frontIN1 = 0; |
| katzjacob | 0:63c4c959e87f | 205 | frontIN2 = 1; |
| katzjacob | 0:63c4c959e87f | 206 | motFront = 1; |
| katzjacob | 0:63c4c959e87f | 207 | LEDantenna=LEDlow=LEDmid=LEDhigh=1; |
| katzjacob | 0:63c4c959e87f | 208 | } |
| katzjacob | 0:63c4c959e87f | 209 | else |
| katzjacob | 0:63c4c959e87f | 210 | { |
| katzjacob | 0:63c4c959e87f | 211 | motFront=0; |
| katzjacob | 0:63c4c959e87f | 212 | motFstby = 0; |
| katzjacob | 0:63c4c959e87f | 213 | LEDantenna=LEDlow=LEDmid=LEDhigh=0; |
| katzjacob | 0:63c4c959e87f | 214 | } |
| katzjacob | 0:63c4c959e87f | 215 | |
| katzjacob | 0:63c4c959e87f | 216 | |
| katzjacob | 0:63c4c959e87f | 217 | } |
| katzjacob | 0:63c4c959e87f | 218 | } |