David Grayson / Mbed 2 deprecated DeadReckoning

David's dead reckoning code for the LVBots competition on March 6th. Uses the mbed LPC1768, DRV8835, QTR-3RC, and two DC motors with encoders.

Dependencies:   PololuEncoder Pacer mbed GeneralDebouncer

main.cpp@18:b65fbb795396, 2014-02-24 (annotated)

Committer:
DavidEGrayson
Date:
Mon Feb 24 02:32:59 2014 +0000
Revision:
18:b65fbb795396
Parent:
17:2df9861f53ee
Child:
19:a11ffc903774
Got the robot to face towards home!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DavidEGrayson 0:e77a0edb9878 1 #include <mbed.h>
DavidEGrayson 8:78b1ff957cba 2 #include <Pacer.h>
DavidEGrayson 0:e77a0edb9878 3
DavidEGrayson 8:78b1ff957cba 4 #include "motors.h"
DavidEGrayson 8:78b1ff957cba 5 #include "encoders.h"
DavidEGrayson 9:9734347b5756 6 #include "leds.h"
DavidEGrayson 8:78b1ff957cba 7 #include "pc_serial.h"
DavidEGrayson 9:9734347b5756 8 #include "test.h"
DavidEGrayson 12:835a4d24ae3b 9 #include "reckoner.h"
DavidEGrayson 16:8eaa5bc2bdb1 10 #include "buttons.h"
DavidEGrayson 0:e77a0edb9878 11
DavidEGrayson 10:e4dd36148539 12 int __attribute__((noreturn)) main()
DavidEGrayson 2:968338353aef 13 {
DavidEGrayson 2:968338353aef 14 pc.baud(115200);
DavidEGrayson 2:968338353aef 15
DavidEGrayson 2:968338353aef 16 // Enable pull-ups on encoder pins and give them a chance to settle.
DavidEGrayson 9:9734347b5756 17 encodersInit();
DavidEGrayson 9:9734347b5756 18 motorsInit();
DavidEGrayson 16:8eaa5bc2bdb1 19 buttonsInit();
DavidEGrayson 4:1b20a11765c8 20
DavidEGrayson 8:78b1ff957cba 21 // Test routines
DavidEGrayson 9:9734347b5756 22 //testMotors();
DavidEGrayson 10:e4dd36148539 23 //testEncoders();
DavidEGrayson 12:835a4d24ae3b 24 //testLineSensors();
DavidEGrayson 16:8eaa5bc2bdb1 25 //testReckoner();
DavidEGrayson 17:2df9861f53ee 26 //testButtons();
DavidEGrayson 17:2df9861f53ee 27 testDriveHome();
DavidEGrayson 2:968338353aef 28
DavidEGrayson 4:1b20a11765c8 29 while(1)
DavidEGrayson 4:1b20a11765c8 30 {
DavidEGrayson 9:9734347b5756 31
DavidEGrayson 0:e77a0edb9878 32 }
DavidEGrayson 0:e77a0edb9878 33 }
DavidEGrayson 12:835a4d24ae3b 34
DavidEGrayson 12:835a4d24ae3b 35 void updateReckonerFromEncoders()
DavidEGrayson 12:835a4d24ae3b 36 {
DavidEGrayson 12:835a4d24ae3b 37 while(encoderBuffer.hasEvents())
DavidEGrayson 12:835a4d24ae3b 38 {
DavidEGrayson 12:835a4d24ae3b 39 PololuEncoderEvent event = encoderBuffer.readEvent();
DavidEGrayson 12:835a4d24ae3b 40 switch(event)
DavidEGrayson 12:835a4d24ae3b 41 {
DavidEGrayson 17:2df9861f53ee 42 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 17:2df9861f53ee 43 reckoner.handleTickLeftForward();
DavidEGrayson 17:2df9861f53ee 44 break;
DavidEGrayson 17:2df9861f53ee 45 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 17:2df9861f53ee 46 reckoner.handleTickLeftBackward();
DavidEGrayson 17:2df9861f53ee 47 break;
DavidEGrayson 17:2df9861f53ee 48 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 17:2df9861f53ee 49 reckoner.handleTickRightForward();
DavidEGrayson 17:2df9861f53ee 50 break;
DavidEGrayson 17:2df9861f53ee 51 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 17:2df9861f53ee 52 reckoner.handleTickRightBackward();
DavidEGrayson 17:2df9861f53ee 53 break;
DavidEGrayson 12:835a4d24ae3b 54 }
DavidEGrayson 12:835a4d24ae3b 55 }
DavidEGrayson 12:835a4d24ae3b 56 }
DavidEGrayson 17:2df9861f53ee 57
DavidEGrayson 18:b65fbb795396 58 // The closer this is to zero, the closer we are to pointing towards the home position.
DavidEGrayson 18:b65fbb795396 59 // It is basically a cross product of the two vectors (x, y) and (cos, sin).
DavidEGrayson 18:b65fbb795396 60 float det()
DavidEGrayson 18:b65fbb795396 61 {
DavidEGrayson 18:b65fbb795396 62 // TODO: get rid of the magic numbers here (i.e. 30)
DavidEGrayson 18:b65fbb795396 63 float s = (float)reckoner.sin / (1 << 30);
DavidEGrayson 18:b65fbb795396 64 float c = (float)reckoner.cos / (1 << 30);
DavidEGrayson 18:b65fbb795396 65 return reckoner.x * s - reckoner.y * c;
DavidEGrayson 18:b65fbb795396 66 }
DavidEGrayson 18:b65fbb795396 67
DavidEGrayson 18:b65fbb795396 68 void __attribute__((noreturn)) driveHome()
DavidEGrayson 18:b65fbb795396 69 {
DavidEGrayson 18:b65fbb795396 70 led1 = 1; led2 = 1; led3 = 0; led4 = 0;
DavidEGrayson 18:b65fbb795396 71
DavidEGrayson 18:b65fbb795396 72 // First, point the robot at the goal.
DavidEGrayson 18:b65fbb795396 73 bool dir = false;
DavidEGrayson 18:b65fbb795396 74 uint16_t transitions = 0;
DavidEGrayson 18:b65fbb795396 75 Timer timer;
DavidEGrayson 18:b65fbb795396 76 timer.start();
DavidEGrayson 18:b65fbb795396 77 while(transitions < 100 || timer.read_ms() < 500)
DavidEGrayson 18:b65fbb795396 78 {
DavidEGrayson 18:b65fbb795396 79 updateReckonerFromEncoders();
DavidEGrayson 18:b65fbb795396 80 {
DavidEGrayson 18:b65fbb795396 81 bool nextDir = det() > 0;
DavidEGrayson 18:b65fbb795396 82 if (nextDir != dir) { transitions++; }
DavidEGrayson 18:b65fbb795396 83 dir = nextDir;
DavidEGrayson 18:b65fbb795396 84 }
DavidEGrayson 18:b65fbb795396 85
DavidEGrayson 18:b65fbb795396 86 if(dir)
DavidEGrayson 18:b65fbb795396 87 {
DavidEGrayson 18:b65fbb795396 88 led3 = 1;
DavidEGrayson 18:b65fbb795396 89 motorsSpeedSet(-300, 300);
DavidEGrayson 18:b65fbb795396 90 }
DavidEGrayson 18:b65fbb795396 91 else
DavidEGrayson 18:b65fbb795396 92 {
DavidEGrayson 18:b65fbb795396 93 led3 = 0;
DavidEGrayson 18:b65fbb795396 94 motorsSpeedSet(300, -300);
DavidEGrayson 18:b65fbb795396 95 }
DavidEGrayson 18:b65fbb795396 96 }
DavidEGrayson 18:b65fbb795396 97 motorsSpeedSet(0, 0);
DavidEGrayson 18:b65fbb795396 98
DavidEGrayson 18:b65fbb795396 99 while(1)
DavidEGrayson 18:b65fbb795396 100 {
DavidEGrayson 18:b65fbb795396 101
DavidEGrayson 18:b65fbb795396 102 }
DavidEGrayson 18:b65fbb795396 103 }