David Grayson
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
Revision 22:44c032e59ff5, committed 2014-02-28
- Comitter:
- DavidEGrayson
- Date:
- Fri Feb 28 00:16:49 2014 +0000
- Parent:
- 21:c279c6a83671
- Child:
- 23:aae5cbe3b924
- Commit message:
- Fixed the code for calibrating. Added testCalibrate.
Changed in this revision
--- a/line_tracker.cpp Thu Feb 27 23:20:34 2014 +0000
+++ b/line_tracker.cpp Fri Feb 28 00:16:49 2014 +0000
@@ -12,30 +12,7 @@
calibratedMaximum[s] = 0;
calibratedMinimum[s] = 0xFFFF;
}
-}
-
-void LineTracker::updateCalibration()
-{
- const int sampleCount = 10;
-
- uint16_t maxValues[LINE_SENSOR_COUNT];
- uint16_t minValues[LINE_SENSOR_COUNT];
-
- for(uint8_t sample = 0; sample < sampleCount; sample++)
- {
- for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
- {
- uint16_t reading = readSensor(s);
- if (reading > maxValues[s]) { maxValues[s] = reading; }
- if (reading < minValues[s]) { minValues[s] = reading; }
- }
- }
-
- for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
- {
- if (minValues[s] > calibratedMaximum[s]) { calibratedMaximum[s] = minValues[s]; }
- if (maxValues[s] > calibratedMinimum[s]) { calibratedMinimum[s] = maxValues[s]; }
- }
+ calibrationState = 0;
}
void LineTracker::read()
@@ -55,13 +32,12 @@
void LineTracker::updateCalibratedValues()
{
- // Update the calibrated values.
for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
{
uint16_t calmin = calibratedMinimum[s];
uint16_t calmax = calibratedMaximum[s];
uint16_t denominator = calmax - calmin;
- int16_t x = 0;
+ int32_t x = 0;
if(denominator != 0)
{
x = ((int32_t)rawValues[s] - calmin) * 1000 / denominator;
@@ -130,4 +106,36 @@
uint16_t LineTracker::getLinePosition()
{
return linePosition;
-}
\ No newline at end of file
+}
+
+void LineTracker::updateCalibration()
+{
+ if(calibrationState == 0)
+ {
+ for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
+ {
+ recentValuesMin[s] = 0xFFFF;
+ recentValuesMax[s] = 0;
+ }
+ }
+
+ for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
+ {
+ uint16_t value = rawValues[s];
+ if (value < recentValuesMin[s]) { recentValuesMin[s] = value; }
+ if (value > recentValuesMax[s]) { recentValuesMax[s] = value; }
+ }
+
+ calibrationState = calibrationState + 1;
+
+ if (calibrationState == 9)
+ {
+ calibrationState = 0;
+
+ for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
+ {
+ if (recentValuesMin[s] > calibratedMaximum[s]) { calibratedMaximum[s] = recentValuesMin[s]; }
+ if (recentValuesMax[s] < calibratedMinimum[s]) { calibratedMinimum[s] = recentValuesMax[s]; }
+ }
+ }
+}
--- a/line_tracker.h Thu Feb 27 23:20:34 2014 +0000
+++ b/line_tracker.h Fri Feb 28 00:16:49 2014 +0000
@@ -22,6 +22,10 @@
void readRawValues();
void updateCalibratedValues();
void updateLineStatus();
+
+ uint8_t calibrationState;
+ uint16_t recentValuesMax[LINE_SENSOR_COUNT];
+ uint16_t recentValuesMin[LINE_SENSOR_COUNT];
bool lineVisible;
uint16_t linePosition;
--- a/main.cpp Thu Feb 27 23:20:34 2014 +0000
+++ b/main.cpp Fri Feb 28 00:16:49 2014 +0000
@@ -43,6 +43,7 @@
//testButtons();
//testDriveHome();
//testFinalSettleIn();
+ testCalibrate();
// Real routines for the contest.
setLeds(1, 0, 0, 0);
@@ -167,17 +168,17 @@
int32_t reduction = reckoner.sin / (1<<15) * straightDriveStrength / (1 << 15);
if (reduction > 0)
{
- speedRight = reduceSpeed(speedRight, -reduction);
+ speedRight = reduceSpeed(speedRight, reduction);
}
else
{
- speedLeft = reduceSpeed(speedLeft, reduction);
+ speedLeft = reduceSpeed(speedLeft, -reduction);
}
motorsSpeedSet(speedLeft, speedRight);
if (goodCalibration)
{
- if(goodCalibrationTimer.read_us() >= 300000)
+ if(goodCalibrationTimer.read_ms() >= 300)
{
// The calibration was good and we traveled for a bit of time after that,
// so we must be a bit over the line.
--- a/main.h Thu Feb 27 23:20:34 2014 +0000 +++ b/main.h Fri Feb 28 00:16:49 2014 +0000 @@ -13,6 +13,7 @@ void updateReckonerFromEncoders(); float determinant(); float dotProduct(); +bool calibrationLooksGood(); extern Reckoner reckoner; extern LineTracker lineTracker;
--- a/test.cpp Thu Feb 27 23:20:34 2014 +0000
+++ b/test.cpp Fri Feb 28 00:16:49 2014 +0000
@@ -107,6 +107,43 @@
}
}
+// If the calibration stops working, we could just use these values from David's office in the day time:
+// # calmin calmax
+// 0 34872 59726
+// 1 29335 60110
+// 2 23845 58446
+void testCalibrate()
+{
+ Timer timer;
+ timer.start();
+
+ Pacer reportPacer(200000);
+
+ while(1)
+ {
+ lineTracker.read();
+ lineTracker.updateCalibration();
+ led4 = calibrationLooksGood();
+
+ if (reportPacer.pace())
+ {
+ pc.printf("\x1B[0;0H"); // VT100 command for "go to 0,0"
+ for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
+ {
+ pc.printf("%-2d %5d %5d %5d\r\n", s, lineTracker.calibratedMinimum[s], lineTracker.rawValues[s], lineTracker.calibratedMaximum[s]);
+ }
+ if (calibrationLooksGood())
+ {
+ pc.puts("Good. \r\n");
+ }
+ else
+ {
+ pc.puts("Not good yet.\r\n");
+ }
+ }
+ }
+}
+
void testEncoders()
{
Pacer reportPacer(500000);
--- a/test.h Thu Feb 27 23:20:34 2014 +0000 +++ b/test.h Fri Feb 28 00:16:49 2014 +0000 @@ -7,3 +7,4 @@ void __attribute__((noreturn)) testButtons(); void __attribute__((noreturn)) testDriveHome(); void __attribute__((noreturn)) testFinalSettleIn(); +void __attribute__((noreturn)) testCalibrate(); \ No newline at end of file