code.ino (8224B)
1 #include <EEPROM.h> 2 #include "helper.h" 3 4 #define READING_INTERVAL 300 5 6 #include "reading.h" 7 8 #define DEBUG 9 10 #ifdef DEBUG 11 #define DBG(expr) expr; 12 #else 13 #define DBG(expr) 14 #endif 15 16 #define FLASHTIME 256 / 2 17 #define TESTREADINGS 10 18 19 typedef struct state { 20 bool left, mid, right; /* is on black */ 21 int rawleft, rawmid, rawright; /* raw readings from LDRS */ 22 enum DIRECTIONS prevDir; /* enum to store which direction we moved in last, used to error correct */ 23 long timeStamp; /* barcode timestamp, used to see how long it has been since we went over a barcode */ 24 long lastmove, lastturn; /* time the last stepmove call was made, used to turn the motor off when we stop calling the functions */ 25 long gstate, ystate, rstate; /*used to store the time values of the LEDs to turn them off when needed */ 26 } state; 27 28 const char *colors[] = {"white", "black"}; 29 30 /* these functions are only used for debugging, and do not need to take up memory when not in use */ 31 #ifdef DEBUG 32 void 33 testLDR() 34 { 35 SEprintf("Testing LDRS\n"); 36 for (int i = 0; i < 2; i++) { 37 SEprintf("Please turn on the lights, and place the robot on a %s surface (press button A to continue)\n", colors[i]); 38 waitButton(BUTTONA, HIGH); 39 reading r = readSensorsMean(TESTREADINGS); 40 if (strcmp(colors[i], "black") == 0) { 41 EEPROM.put(6, (uint16_t)r.left + 100); 42 EEPROM.put(10,(uint16_t)r.mid + 100); 43 EEPROM.put(14, (uint16_t)r.right + 100); 44 } else { 45 EEPROM.put(4, (uint16_t)r.left); 46 EEPROM.put(8,(uint16_t)r.mid); 47 EEPROM.put(12, (uint16_t)r.right); 48 } 49 50 SEprintf("Test complete, please turn off the lights (press button A to continue)\n"); 51 waitButton(BUTTONA, HIGH); 52 readSensorsMean(TESTREADINGS); 53 } 54 SEprintf("finished testing LDRS, please check the values are expected for lighting conditions\n"); 55 } 56 57 void 58 testButton(int pin) 59 { 60 SEprintf("Testing button on pin %d, please press it 10 times in a row\n", pin); 61 for (int i = 0; i < 10; i++) { 62 waitButton(pin, HIGH); 63 SEprintf("Button pressed\n"); 64 } 65 SEprintf("Button testing complete, 10 values read, if you did not press the button 10 times, this test failed\n"); 66 delay(3); 67 } 68 69 void 70 testServo(Servo s, int speed, int zero) 71 { 72 SEprintf("Testing Servo on pin %d, it will spin for 10 seconds\n"); 73 s.write(zero + speed); 74 delay(10000); 75 s.write(zero); 76 SEprintf("Servo test complete, if the servo did not spin for 10 seconds, this test failed\n"); 77 } 78 79 void 80 testIR() 81 { 82 int seen = 0; 83 84 SEprintf("Testing IR sensor, please place an object infront of the IR sensor\n"); 85 delay(5000); 86 for (int i = 0; i < 10; i++) { 87 if (isObstacle()) { 88 SEprintf("Detected object\n"); 89 seen++; 90 } else 91 SEprintf("Did not detect object\n"); 92 93 delay(1000); 94 } 95 if (seen < 8) 96 SEprintf("Only saw the object %d times, test failed\n", seen); 97 else 98 SEprintf("Saw the object %d times, test passed\n", seen); 99 } 100 #endif 101 102 void 103 flash(int pin, state *s) 104 { 105 /* user facing function to start flashing of the LEDs */ 106 switch (pin) { 107 case GREEN_LED: s->gstate = millis(); break; 108 case YELLOW_LED: s->ystate = millis(); break; 109 case RED_LED: s->rstate = millis(); break; 110 } 111 } 112 113 void 114 stepFlash(int pin) 115 { 116 /* we define this a static to preserve its value across function calls */ 117 static uint8_t flashCount; 118 119 if (flashCount < FLASHTIME) 120 digitalWrite(pin, HIGH); 121 else if (flashCount > FLASHTIME / 2) 122 digitalWrite(pin, LOW); 123 124 /* this value wraps which is by design, this will cause the flashing to reset */ 125 flashCount++; 126 } 127 128 void 129 updateFlash(state *s) 130 { 131 /* turn off the leds if they arent ment to be on anymore */ 132 if (millis() - s->gstate > FLASHTIME) { 133 s->gstate = 0; 134 digitalWrite(GREEN_LED, LOW); 135 } if (millis() - s->ystate > FLASHTIME) { 136 s->ystate = 0; 137 digitalWrite(YELLOW_LED, LOW); 138 } if (millis() - s->rstate > FLASHTIME) { 139 s->rstate = 0; 140 digitalWrite(RED_LED, LOW); 141 } 142 143 /* turn on the leds if needed */ 144 if (s->gstate) stepFlash(GREEN_LED); 145 if (s->ystate) stepFlash(YELLOW_LED); 146 if (s->rstate) stepFlash(RED_LED); 147 } 148 149 void 150 stepMove(int dir, state *s) 151 { 152 /* move the robot fwd or bwd */ 153 servoA.write(SERVOA_ZERO + (SPEEDA * dir)); 154 servoB.write(SERVOB_ZERO - (SPEEDB * dir)); 155 s->lastmove = millis(); 156 } 157 158 void 159 stepTurn(int dir, state *s) 160 { 161 /* turn the robot lft or rgt */ 162 servoA.write(SERVOA_ZERO + (SPEEDA * dir)); 163 servoB.write(SERVOB_ZERO - (SPEEDB * -dir)); 164 s->lastturn = millis(); 165 } 166 167 void 168 updateMotor(state *s) 169 { 170 /* turns of the motor if we hacent called step move/turn recently */ 171 if (millis() - s->lastmove > 10 && millis() - s->lastturn) { 172 servoA.write(SERVOA_ZERO); 173 servoB.write(SERVOB_ZERO); 174 } 175 } 176 177 state * 178 getState() 179 { 180 static state s; 181 182 /* update all of the robots LDR readings */ 183 s.left = !isLDRBright(LDRA); 184 s.mid = !isLDRBright(LDRB); 185 s.right = !isLDRBright(LDRC); 186 s.rawleft = analogRead(LDRA); 187 s.rawmid = analogRead(LDRB); 188 s.rawright = analogRead(LDRC); 189 190 updateFlash(&s); 191 updateMotor(&s); 192 193 194 return &s; 195 } 196 197 void 198 stepAvoidanceBehave(state *s) 199 { 200 flash(RED_LED, s); 201 DBG({ 202 /* WARNING this causes the program to stutter when left on, 203 not too bad for debugging but very bad for general use! */ 204 SEprintf("Avoid: %d, %d, %d\n", s->rawleft, s->rawmid, s->rawright); 205 }); 206 207 /* left is noticably larger */ 208 if (s->rawleft > s->rawright + 50) 209 stepTurn(RGT,s); 210 /* right is noticably larger */ 211 else if (s->rawright > s->rawleft + 50) 212 stepTurn(LFT,s); 213 else 214 stepMove(FWD, s); 215 } 216 217 218 void 219 stepAttractionBehave(state *s) 220 { 221 flash(GREEN_LED, s); 222 DBG({ 223 SEprintf("Attract: %d, %d, %d\n", s->rawleft, s->rawmid, s->rawright); 224 }); 225 226 /* left is noticably larger */ 227 if (s->rawleft > s->rawright + 50) 228 stepTurn(LFT,s); 229 /* right is noticably larger */ 230 else if (s->rawright > s->rawleft +50) 231 stepTurn(RGT,s); 232 else 233 stepMove(FWD, s); 234 } 235 236 void 237 runBehave(void (*stepBehave)(state *)) 238 { 239 while (true) { 240 state *s = getState(); 241 stepBehave(s); 242 } 243 } 244 245 void 246 checkBarcode(state *s) 247 { 248 long t = millis(); 249 if (s->timeStamp > 500) { 250 /* long barcode */ 251 if ((t - s->timeStamp) > 3000) 252 runBehave(&stepAvoidanceBehave); 253 254 /* short barcode */ 255 else if ((t - s->timeStamp) > 800) 256 runBehave(&stepAttractionBehave); 257 } 258 s->timeStamp = t; 259 } 260 261 void 262 move(state *s) 263 { 264 /* small adjustments */ 265 if (s->right && s->mid) 266 stepTurn(RGT, s); 267 else if (s->left && s->mid) 268 stepTurn(LFT, s); 269 270 /* large adjustments */ 271 else if (s->right) { 272 s->prevDir = RGT; 273 stepTurn(RGT, s); 274 } else if (s->left) { 275 s->prevDir = LFT; 276 stepTurn(LFT, s); 277 278 /* drive straight, nominal */ 279 } else if (s->mid) { 280 for (int i = 0; i < 20; i++) 281 stepMove(FWD, s); 282 } else { 283 /* try to recover from not being on the line by moving in the prevDir 284 only updated on large turns */ 285 while (!s->mid) { 286 s = getState(); 287 stepTurn(s->prevDir,s); 288 } 289 } 290 } 291 292 void 293 setup() 294 { 295 /* let the user attach the battery, attaching the battery while code is running caused strange behavior */ 296 delay(3000); 297 setupRobot(); 298 299 /* servo speed values */ 300 EEPROM.get(0, SERVOA_ZERO); 301 EEPROM.get(1, SPEEDA); 302 EEPROM.get(2, SERVOB_ZERO); 303 EEPROM.get(3, SPEEDB); 304 SEprintf("A zero: %d\nB zero: %d\nA speed: %d\nB speed: %d\n", SERVOA_ZERO, SERVOB_ZERO, SPEEDA, SPEEDB); 305 306 /* because this is static, I can modify these values to set defaults */ 307 state *s = getState(); 308 s->prevDir = BWD; 309 s->timeStamp = 0; 310 311 /* test all the sensors and motors */ 312 DBG({ 313 testLDR(); 314 delay(1000); 315 testButton(BUTTONA); 316 delay(1000); 317 testButton(BUTTONB); 318 delay(1000); 319 testServo(servoA, SPEEDA, SERVOA_ZERO); 320 delay(1000); 321 testServo(servoB, SPEEDB, SERVOB_ZERO); 322 delay(1000); 323 testIR(); 324 delay(1000); 325 SEprintf("finished testing\n"); 326 }); 327 328 /* read the calibrated values that should be in EEPROM, these values will be made by the tests above program */ 329 EEPROM.get(6, LDRA_SWITCH); 330 EEPROM.get(10, LDRB_SWITCH); 331 EEPROM.get(14, LDRC_SWITCH); 332 } 333 334 void 335 loop() 336 { 337 state *s = getState(); 338 339 /* do nothing while there is an obstacle */ 340 while (isObstacle()) { 341 flash(YELLOW_LED, s); 342 return; 343 } 344 345 /* the robots on a barcode */ 346 if (s->right && s->left && s->mid) { 347 checkBarcode(s); 348 349 /* run through the barcode */ 350 for (int i = 0; i < 20; i++) 351 stepMove(FWD, s); 352 } 353 354 /* normal movement */ 355 else move(s); 356 }