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@20:dbec34f0e76b, 2014-02-27 (annotated)

Committer:
DavidEGrayson
Date:
Thu Feb 27 19:46:35 2014 +0000
Revision:
20:dbec34f0e76b
Parent:
19:a11ffc903774
Child:
21:c279c6a83671
Broke up driveHome into two functions.

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 20:dbec34f0e76b 28 //testDriveHome();
DavidEGrayson 20:dbec34f0e76b 29 testFinalSettleIn();
DavidEGrayson 2:968338353aef 30
DavidEGrayson 4:1b20a11765c8 31 while(1)
DavidEGrayson 4:1b20a11765c8 32 {
DavidEGrayson 9:9734347b5756 33
DavidEGrayson 0:e77a0edb9878 34 }
DavidEGrayson 0:e77a0edb9878 35 }
DavidEGrayson 12:835a4d24ae3b 36
DavidEGrayson 12:835a4d24ae3b 37 void updateReckonerFromEncoders()
DavidEGrayson 12:835a4d24ae3b 38 {
DavidEGrayson 12:835a4d24ae3b 39 while(encoderBuffer.hasEvents())
DavidEGrayson 12:835a4d24ae3b 40 {
DavidEGrayson 12:835a4d24ae3b 41 PololuEncoderEvent event = encoderBuffer.readEvent();
DavidEGrayson 12:835a4d24ae3b 42 switch(event)
DavidEGrayson 12:835a4d24ae3b 43 {
DavidEGrayson 17:2df9861f53ee 44 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 17:2df9861f53ee 45 reckoner.handleTickLeftForward();
DavidEGrayson 17:2df9861f53ee 46 break;
DavidEGrayson 17:2df9861f53ee 47 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 17:2df9861f53ee 48 reckoner.handleTickLeftBackward();
DavidEGrayson 17:2df9861f53ee 49 break;
DavidEGrayson 17:2df9861f53ee 50 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 17:2df9861f53ee 51 reckoner.handleTickRightForward();
DavidEGrayson 17:2df9861f53ee 52 break;
DavidEGrayson 17:2df9861f53ee 53 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 17:2df9861f53ee 54 reckoner.handleTickRightBackward();
DavidEGrayson 17:2df9861f53ee 55 break;
DavidEGrayson 12:835a4d24ae3b 56 }
DavidEGrayson 12:835a4d24ae3b 57 }
DavidEGrayson 12:835a4d24ae3b 58 }
DavidEGrayson 17:2df9861f53ee 59
DavidEGrayson 19:a11ffc903774 60 float magnitude()
DavidEGrayson 19:a11ffc903774 61 {
DavidEGrayson 19:a11ffc903774 62 return sqrt((float)reckoner.x * reckoner.x + (float)reckoner.y * reckoner.y);
DavidEGrayson 19:a11ffc903774 63 }
DavidEGrayson 19:a11ffc903774 64
DavidEGrayson 20:dbec34f0e76b 65 float dotProduct()
DavidEGrayson 20:dbec34f0e76b 66 {
DavidEGrayson 20:dbec34f0e76b 67 float s = (float)reckoner.sin / (1 << 30);
DavidEGrayson 20:dbec34f0e76b 68 float c = (float)reckoner.cos / (1 << 30);
DavidEGrayson 20:dbec34f0e76b 69 float magn = magnitude();
DavidEGrayson 20:dbec34f0e76b 70 if (magn == 0){ return 0; }
DavidEGrayson 20:dbec34f0e76b 71 return ((float)reckoner.x * c + (float)reckoner.y * s) / magn;
DavidEGrayson 20:dbec34f0e76b 72 }
DavidEGrayson 20:dbec34f0e76b 73
DavidEGrayson 18:b65fbb795396 74 // The closer this is to zero, the closer we are to pointing towards the home position.
DavidEGrayson 18:b65fbb795396 75 // It is basically a cross product of the two vectors (x, y) and (cos, sin).
DavidEGrayson 19:a11ffc903774 76 float determinant()
DavidEGrayson 18:b65fbb795396 77 {
DavidEGrayson 18:b65fbb795396 78 // TODO: get rid of the magic numbers here (i.e. 30)
DavidEGrayson 18:b65fbb795396 79 float s = (float)reckoner.sin / (1 << 30);
DavidEGrayson 18:b65fbb795396 80 float c = (float)reckoner.cos / (1 << 30);
DavidEGrayson 19:a11ffc903774 81 return (reckoner.x * s - reckoner.y * c) / magnitude();
DavidEGrayson 19:a11ffc903774 82 }
DavidEGrayson 19:a11ffc903774 83
DavidEGrayson 19:a11ffc903774 84 int16_t reduceSpeed(int16_t speed, int16_t reduction)
DavidEGrayson 19:a11ffc903774 85 {
DavidEGrayson 19:a11ffc903774 86 if (reduction > speed)
DavidEGrayson 19:a11ffc903774 87 {
DavidEGrayson 19:a11ffc903774 88 return 0;
DavidEGrayson 19:a11ffc903774 89 }
DavidEGrayson 19:a11ffc903774 90 else
DavidEGrayson 19:a11ffc903774 91 {
DavidEGrayson 19:a11ffc903774 92 return speed - reduction;
DavidEGrayson 19:a11ffc903774 93 }
DavidEGrayson 18:b65fbb795396 94 }
DavidEGrayson 18:b65fbb795396 95
DavidEGrayson 20:dbec34f0e76b 96 void driveHomeAlmost();
DavidEGrayson 20:dbec34f0e76b 97 void finalSettleIn();
DavidEGrayson 20:dbec34f0e76b 98
DavidEGrayson 20:dbec34f0e76b 99 void driveHome()
DavidEGrayson 20:dbec34f0e76b 100 {
DavidEGrayson 20:dbec34f0e76b 101 driveHomeAlmost();
DavidEGrayson 20:dbec34f0e76b 102 finalSettleIn();
DavidEGrayson 20:dbec34f0e76b 103 }
DavidEGrayson 20:dbec34f0e76b 104
DavidEGrayson 20:dbec34f0e76b 105 void driveHomeAlmost()
DavidEGrayson 18:b65fbb795396 106 {
DavidEGrayson 18:b65fbb795396 107 led1 = 1; led2 = 1; led3 = 0; led4 = 0;
DavidEGrayson 18:b65fbb795396 108
DavidEGrayson 18:b65fbb795396 109 bool dir = false;
DavidEGrayson 18:b65fbb795396 110 uint16_t transitions = 0;
DavidEGrayson 18:b65fbb795396 111 Timer timer;
DavidEGrayson 18:b65fbb795396 112 timer.start();
DavidEGrayson 19:a11ffc903774 113
DavidEGrayson 19:a11ffc903774 114 const int16_t maxSpeed = 300;
DavidEGrayson 19:a11ffc903774 115
DavidEGrayson 19:a11ffc903774 116 while(1)
DavidEGrayson 18:b65fbb795396 117 {
DavidEGrayson 18:b65fbb795396 118 updateReckonerFromEncoders();
DavidEGrayson 19:a11ffc903774 119
DavidEGrayson 19:a11ffc903774 120 float magn = magnitude();
DavidEGrayson 19:a11ffc903774 121
DavidEGrayson 19:a11ffc903774 122 if (magn < (1<<17))
DavidEGrayson 18:b65fbb795396 123 {
DavidEGrayson 19:a11ffc903774 124 // We are within 8 encoder ticks, so go to the next step.
DavidEGrayson 19:a11ffc903774 125 break;
DavidEGrayson 19:a11ffc903774 126 }
DavidEGrayson 19:a11ffc903774 127
DavidEGrayson 19:a11ffc903774 128 float det = determinant();
DavidEGrayson 19:a11ffc903774 129
DavidEGrayson 19:a11ffc903774 130 {
DavidEGrayson 19:a11ffc903774 131 bool nextDir = det > 0;
DavidEGrayson 18:b65fbb795396 132 if (nextDir != dir) { transitions++; }
DavidEGrayson 18:b65fbb795396 133 dir = nextDir;
DavidEGrayson 18:b65fbb795396 134 }
DavidEGrayson 18:b65fbb795396 135
DavidEGrayson 19:a11ffc903774 136 int16_t speedLeft = maxSpeed;
DavidEGrayson 19:a11ffc903774 137 int16_t speedRight = maxSpeed;
DavidEGrayson 19:a11ffc903774 138 if (magn < (1<<20)) // Within 64 encoder ticks of the origin, so slow down.
DavidEGrayson 18:b65fbb795396 139 {
DavidEGrayson 19:a11ffc903774 140 int16_t reduction = (1 - magn/(1<<20)) * maxSpeed;
DavidEGrayson 19:a11ffc903774 141 speedLeft = reduceSpeed(speedLeft, reduction);
DavidEGrayson 19:a11ffc903774 142 speedRight = reduceSpeed(speedRight, reduction);
DavidEGrayson 19:a11ffc903774 143 }
DavidEGrayson 19:a11ffc903774 144
DavidEGrayson 19:a11ffc903774 145 if (det > 0)
DavidEGrayson 19:a11ffc903774 146 {
DavidEGrayson 19:a11ffc903774 147 speedLeft = reduceSpeed(speedLeft, det * 1000);
DavidEGrayson 18:b65fbb795396 148 }
DavidEGrayson 18:b65fbb795396 149 else
DavidEGrayson 18:b65fbb795396 150 {
DavidEGrayson 19:a11ffc903774 151 speedRight = reduceSpeed(speedRight, -det * 1000);
DavidEGrayson 18:b65fbb795396 152 }
DavidEGrayson 19:a11ffc903774 153 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 18:b65fbb795396 154 }
DavidEGrayson 18:b65fbb795396 155
DavidEGrayson 20:dbec34f0e76b 156 motorsSpeedSet(0, 0);
DavidEGrayson 20:dbec34f0e76b 157 }
DavidEGrayson 20:dbec34f0e76b 158
DavidEGrayson 20:dbec34f0e76b 159 void finalSettleIn()
DavidEGrayson 20:dbec34f0e76b 160 {
DavidEGrayson 20:dbec34f0e76b 161 led1 = 1; led2 = 1; led3 = 1; led4 = 0;
DavidEGrayson 19:a11ffc903774 162
DavidEGrayson 20:dbec34f0e76b 163 const int16_t settleSpeed = 200;
DavidEGrayson 20:dbec34f0e76b 164 const int16_t settleModificationStrength = 100;
DavidEGrayson 20:dbec34f0e76b 165
DavidEGrayson 20:dbec34f0e76b 166 Timer timer;
DavidEGrayson 20:dbec34f0e76b 167 timer.start();
DavidEGrayson 20:dbec34f0e76b 168
DavidEGrayson 20:dbec34f0e76b 169 // State 0: rotating
DavidEGrayson 20:dbec34f0e76b 170 // State 1: Trying to get into final orientation: want [cos, sin] == [1<<30, 0].
DavidEGrayson 20:dbec34f0e76b 171 uint8_t state = 0;
DavidEGrayson 20:dbec34f0e76b 172
DavidEGrayson 19:a11ffc903774 173 while(1)
DavidEGrayson 19:a11ffc903774 174 {
DavidEGrayson 20:dbec34f0e76b 175 updateReckonerFromEncoders();
DavidEGrayson 20:dbec34f0e76b 176
DavidEGrayson 20:dbec34f0e76b 177 float dot = dotProduct();
DavidEGrayson 20:dbec34f0e76b 178 int16_t speedModification = -dot * settleModificationStrength;
DavidEGrayson 20:dbec34f0e76b 179 if (speedModification > settleModificationStrength)
DavidEGrayson 20:dbec34f0e76b 180 {
DavidEGrayson 20:dbec34f0e76b 181 speedModification = settleModificationStrength;
DavidEGrayson 20:dbec34f0e76b 182 }
DavidEGrayson 20:dbec34f0e76b 183 else if (speedModification < -settleModificationStrength)
DavidEGrayson 20:dbec34f0e76b 184 {
DavidEGrayson 20:dbec34f0e76b 185 speedModification = -settleModificationStrength;
DavidEGrayson 20:dbec34f0e76b 186 }
DavidEGrayson 20:dbec34f0e76b 187
DavidEGrayson 20:dbec34f0e76b 188 if (state == 0 && timer.read_ms() >= 6000 && reckoner.cos > (1 << 29))
DavidEGrayson 19:a11ffc903774 189 {
DavidEGrayson 20:dbec34f0e76b 190 led1 = 1; led2 = 0; led3 = 1; led4 = 0;
DavidEGrayson 20:dbec34f0e76b 191 state = 1;
DavidEGrayson 20:dbec34f0e76b 192 }
DavidEGrayson 20:dbec34f0e76b 193
DavidEGrayson 20:dbec34f0e76b 194 int16_t rotationSpeed;
DavidEGrayson 20:dbec34f0e76b 195 if (state == 1)
DavidEGrayson 20:dbec34f0e76b 196 {
DavidEGrayson 20:dbec34f0e76b 197 float s = (float)reckoner.sin / (1 << 30);
DavidEGrayson 20:dbec34f0e76b 198 rotationSpeed = -s * 300;
DavidEGrayson 20:dbec34f0e76b 199 }
DavidEGrayson 20:dbec34f0e76b 200 else
DavidEGrayson 20:dbec34f0e76b 201 {
DavidEGrayson 20:dbec34f0e76b 202 rotationSpeed = settleSpeed;
DavidEGrayson 20:dbec34f0e76b 203 }
DavidEGrayson 20:dbec34f0e76b 204
DavidEGrayson 20:dbec34f0e76b 205 int16_t speedLeft = -rotationSpeed + speedModification;
DavidEGrayson 20:dbec34f0e76b 206 int16_t speedRight = rotationSpeed + speedModification;
DavidEGrayson 20:dbec34f0e76b 207 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 19:a11ffc903774 208 }
DavidEGrayson 20:dbec34f0e76b 209
DavidEGrayson 20:dbec34f0e76b 210 led1 = 1; led2 = 1; led3 = 1; led4 = 1;
DavidEGrayson 20:dbec34f0e76b 211 motorsSpeedSet(0, 0);
DavidEGrayson 18:b65fbb795396 212 }