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@19:a11ffc903774, 2014-02-25 (annotated)

Committer:
DavidEGrayson
Date:
Tue Feb 25 02:58:16 2014 +0000
Revision:
19:a11ffc903774
Parent:
18:b65fbb795396
Child:
20:dbec34f0e76b
Got the robot to get back to its home position and stop.

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 19:a11ffc903774 3 #include <math.h>
DavidEGrayson 0:e77a0edb9878 4
DavidEGrayson 8:78b1ff957cba 5 #include "motors.h"
DavidEGrayson 8:78b1ff957cba 6 #include "encoders.h"
DavidEGrayson 9:9734347b5756 7 #include "leds.h"
DavidEGrayson 8:78b1ff957cba 8 #include "pc_serial.h"
DavidEGrayson 9:9734347b5756 9 #include "test.h"
DavidEGrayson 12:835a4d24ae3b 10 #include "reckoner.h"
DavidEGrayson 16:8eaa5bc2bdb1 11 #include "buttons.h"
DavidEGrayson 0:e77a0edb9878 12
DavidEGrayson 10:e4dd36148539 13 int __attribute__((noreturn)) main()
DavidEGrayson 2:968338353aef 14 {
DavidEGrayson 2:968338353aef 15 pc.baud(115200);
DavidEGrayson 2:968338353aef 16
DavidEGrayson 2:968338353aef 17 // Enable pull-ups on encoder pins and give them a chance to settle.
DavidEGrayson 9:9734347b5756 18 encodersInit();
DavidEGrayson 9:9734347b5756 19 motorsInit();
DavidEGrayson 16:8eaa5bc2bdb1 20 buttonsInit();
DavidEGrayson 4:1b20a11765c8 21
DavidEGrayson 8:78b1ff957cba 22 // Test routines
DavidEGrayson 9:9734347b5756 23 //testMotors();
DavidEGrayson 10:e4dd36148539 24 //testEncoders();
DavidEGrayson 12:835a4d24ae3b 25 //testLineSensors();
DavidEGrayson 16:8eaa5bc2bdb1 26 //testReckoner();
DavidEGrayson 17:2df9861f53ee 27 //testButtons();
DavidEGrayson 17:2df9861f53ee 28 testDriveHome();
DavidEGrayson 2:968338353aef 29
DavidEGrayson 4:1b20a11765c8 30 while(1)
DavidEGrayson 4:1b20a11765c8 31 {
DavidEGrayson 9:9734347b5756 32
DavidEGrayson 0:e77a0edb9878 33 }
DavidEGrayson 0:e77a0edb9878 34 }
DavidEGrayson 12:835a4d24ae3b 35
DavidEGrayson 12:835a4d24ae3b 36 void updateReckonerFromEncoders()
DavidEGrayson 12:835a4d24ae3b 37 {
DavidEGrayson 12:835a4d24ae3b 38 while(encoderBuffer.hasEvents())
DavidEGrayson 12:835a4d24ae3b 39 {
DavidEGrayson 12:835a4d24ae3b 40 PololuEncoderEvent event = encoderBuffer.readEvent();
DavidEGrayson 12:835a4d24ae3b 41 switch(event)
DavidEGrayson 12:835a4d24ae3b 42 {
DavidEGrayson 17:2df9861f53ee 43 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 17:2df9861f53ee 44 reckoner.handleTickLeftForward();
DavidEGrayson 17:2df9861f53ee 45 break;
DavidEGrayson 17:2df9861f53ee 46 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 17:2df9861f53ee 47 reckoner.handleTickLeftBackward();
DavidEGrayson 17:2df9861f53ee 48 break;
DavidEGrayson 17:2df9861f53ee 49 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 17:2df9861f53ee 50 reckoner.handleTickRightForward();
DavidEGrayson 17:2df9861f53ee 51 break;
DavidEGrayson 17:2df9861f53ee 52 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 17:2df9861f53ee 53 reckoner.handleTickRightBackward();
DavidEGrayson 17:2df9861f53ee 54 break;
DavidEGrayson 12:835a4d24ae3b 55 }
DavidEGrayson 12:835a4d24ae3b 56 }
DavidEGrayson 12:835a4d24ae3b 57 }
DavidEGrayson 17:2df9861f53ee 58
DavidEGrayson 19:a11ffc903774 59 float magnitude()
DavidEGrayson 19:a11ffc903774 60 {
DavidEGrayson 19:a11ffc903774 61 return sqrt((float)reckoner.x * reckoner.x + (float)reckoner.y * reckoner.y);
DavidEGrayson 19:a11ffc903774 62 }
DavidEGrayson 19:a11ffc903774 63
DavidEGrayson 18:b65fbb795396 64 // The closer this is to zero, the closer we are to pointing towards the home position.
DavidEGrayson 18:b65fbb795396 65 // It is basically a cross product of the two vectors (x, y) and (cos, sin).
DavidEGrayson 19:a11ffc903774 66 float determinant()
DavidEGrayson 18:b65fbb795396 67 {
DavidEGrayson 18:b65fbb795396 68 // TODO: get rid of the magic numbers here (i.e. 30)
DavidEGrayson 18:b65fbb795396 69 float s = (float)reckoner.sin / (1 << 30);
DavidEGrayson 18:b65fbb795396 70 float c = (float)reckoner.cos / (1 << 30);
DavidEGrayson 19:a11ffc903774 71 return (reckoner.x * s - reckoner.y * c) / magnitude();
DavidEGrayson 19:a11ffc903774 72 }
DavidEGrayson 19:a11ffc903774 73
DavidEGrayson 19:a11ffc903774 74 int16_t reduceSpeed(int16_t speed, int16_t reduction)
DavidEGrayson 19:a11ffc903774 75 {
DavidEGrayson 19:a11ffc903774 76 if (reduction > speed)
DavidEGrayson 19:a11ffc903774 77 {
DavidEGrayson 19:a11ffc903774 78 return 0;
DavidEGrayson 19:a11ffc903774 79 }
DavidEGrayson 19:a11ffc903774 80 else
DavidEGrayson 19:a11ffc903774 81 {
DavidEGrayson 19:a11ffc903774 82 return speed - reduction;
DavidEGrayson 19:a11ffc903774 83 }
DavidEGrayson 18:b65fbb795396 84 }
DavidEGrayson 18:b65fbb795396 85
DavidEGrayson 18:b65fbb795396 86 void __attribute__((noreturn)) driveHome()
DavidEGrayson 18:b65fbb795396 87 {
DavidEGrayson 18:b65fbb795396 88 led1 = 1; led2 = 1; led3 = 0; led4 = 0;
DavidEGrayson 18:b65fbb795396 89
DavidEGrayson 18:b65fbb795396 90 // First, point the robot at the goal.
DavidEGrayson 18:b65fbb795396 91 bool dir = false;
DavidEGrayson 18:b65fbb795396 92 uint16_t transitions = 0;
DavidEGrayson 18:b65fbb795396 93 Timer timer;
DavidEGrayson 18:b65fbb795396 94 timer.start();
DavidEGrayson 19:a11ffc903774 95
DavidEGrayson 19:a11ffc903774 96 const int16_t maxSpeed = 300;
DavidEGrayson 19:a11ffc903774 97
DavidEGrayson 19:a11ffc903774 98 while(1)
DavidEGrayson 18:b65fbb795396 99 {
DavidEGrayson 18:b65fbb795396 100 updateReckonerFromEncoders();
DavidEGrayson 19:a11ffc903774 101
DavidEGrayson 19:a11ffc903774 102 float magn = magnitude();
DavidEGrayson 19:a11ffc903774 103
DavidEGrayson 19:a11ffc903774 104 if (magn < (1<<17))
DavidEGrayson 18:b65fbb795396 105 {
DavidEGrayson 19:a11ffc903774 106 // We are within 8 encoder ticks, so go to the next step.
DavidEGrayson 19:a11ffc903774 107 break;
DavidEGrayson 19:a11ffc903774 108 }
DavidEGrayson 19:a11ffc903774 109
DavidEGrayson 19:a11ffc903774 110 float det = determinant();
DavidEGrayson 19:a11ffc903774 111
DavidEGrayson 19:a11ffc903774 112 {
DavidEGrayson 19:a11ffc903774 113 bool nextDir = det > 0;
DavidEGrayson 18:b65fbb795396 114 if (nextDir != dir) { transitions++; }
DavidEGrayson 18:b65fbb795396 115 dir = nextDir;
DavidEGrayson 18:b65fbb795396 116 }
DavidEGrayson 18:b65fbb795396 117
DavidEGrayson 19:a11ffc903774 118 int16_t speedLeft = maxSpeed;
DavidEGrayson 19:a11ffc903774 119 int16_t speedRight = maxSpeed;
DavidEGrayson 19:a11ffc903774 120 if (magn < (1<<20)) // Within 64 encoder ticks of the origin, so slow down.
DavidEGrayson 18:b65fbb795396 121 {
DavidEGrayson 19:a11ffc903774 122 int16_t reduction = (1 - magn/(1<<20)) * maxSpeed;
DavidEGrayson 19:a11ffc903774 123 speedLeft = reduceSpeed(speedLeft, reduction);
DavidEGrayson 19:a11ffc903774 124 speedRight = reduceSpeed(speedRight, reduction);
DavidEGrayson 19:a11ffc903774 125 }
DavidEGrayson 19:a11ffc903774 126
DavidEGrayson 19:a11ffc903774 127 if (det > 0)
DavidEGrayson 19:a11ffc903774 128 {
DavidEGrayson 19:a11ffc903774 129 speedLeft = reduceSpeed(speedLeft, det * 1000);
DavidEGrayson 18:b65fbb795396 130 }
DavidEGrayson 18:b65fbb795396 131 else
DavidEGrayson 18:b65fbb795396 132 {
DavidEGrayson 19:a11ffc903774 133 speedRight = reduceSpeed(speedRight, -det * 1000);
DavidEGrayson 18:b65fbb795396 134 }
DavidEGrayson 19:a11ffc903774 135 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 18:b65fbb795396 136 }
DavidEGrayson 18:b65fbb795396 137
DavidEGrayson 19:a11ffc903774 138 //while(1)
DavidEGrayson 18:b65fbb795396 139 {
DavidEGrayson 18:b65fbb795396 140
DavidEGrayson 18:b65fbb795396 141 }
DavidEGrayson 19:a11ffc903774 142
DavidEGrayson 19:a11ffc903774 143 motorsSpeedSet(0, 0);
DavidEGrayson 19:a11ffc903774 144 Pacer reportPacer(200000);
DavidEGrayson 19:a11ffc903774 145 while(1)
DavidEGrayson 19:a11ffc903774 146 {
DavidEGrayson 19:a11ffc903774 147 if(reportPacer.pace())
DavidEGrayson 19:a11ffc903774 148 {
DavidEGrayson 19:a11ffc903774 149 led4 = 1;
DavidEGrayson 19:a11ffc903774 150 pc.printf("%11d %11d %11d %11d | %11f\r\n",
DavidEGrayson 19:a11ffc903774 151 reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
DavidEGrayson 19:a11ffc903774 152 determinant());
DavidEGrayson 19:a11ffc903774 153 led4 = 0;
DavidEGrayson 19:a11ffc903774 154 }
DavidEGrayson 19:a11ffc903774 155 }
DavidEGrayson 18:b65fbb795396 156 }