Mike Panetta
PiSlingers library for AHRC competitions
Revision 2:a9351d7f92b4, committed 2012-11-03
- Comitter:
- mpanetta
- Date:
- Sat Nov 03 01:03:16 2012 +0000
- Parent:
- 1:695f4f4442d3
- Commit message:
- Fixed some bugs in the IR code that allowed negative values through.
Changed in this revision
--- a/Beacon.cpp Fri Nov 02 03:00:29 2012 +0000
+++ b/Beacon.cpp Sat Nov 03 01:03:16 2012 +0000
@@ -1,14 +1,137 @@
#include "Beacon.h"
+#include "MODSERIAL.h"
+
+uint16_t
+Beacon::readAndAverageR(int times)
+{
+ int32_t avg = 0;
+ int i;
+
+ for (i = 0; i < times; i++)
+ {
+ avg += ainR.read_u16();
+ }
+
+ return (avg / times);
+}
+
+uint16_t
+Beacon::readAndAverageL(int times)
+{
+ int32_t avg = 0;
+ int i;
+
+ for (i = 0; i < times; i++)
+ {
+ avg += ainL.read_u16();
+ }
+
+ return (avg / times);
+}
+
+#define SAMPLES_TO_AVERAGE 8
+
+void
+Beacon::calibrate(MODSERIAL * debug)
+{
+ int32_t absMinL = 65536;
+ int32_t absMaxL = 0;
+ int32_t absMinR = 65536;
+ int32_t absMaxR = 0;
+
+ uint16_t readingL;
+ uint16_t readingR;
+
+ debug->printf("Calibrating offset and gain for beacon. Turn off all beacons. Press any key when complete.\r\n");
+ while ( !debug->readable() )
+ {
+ wait(0.1);
+ }
+ debug->rxBufferFlush();
+
+ debug->printf("Starting calibration.\r\n");
+ debug->printf("Absolute max and min starting values:\r\n");
+ debug->printf("\tminL = %d\r\n", absMinL);
+ debug->printf("\tmaxL = %d\r\n", absMaxL);
+ debug->printf("\tminR = %d\r\n", absMinR);
+ debug->printf("\tmaxR = %d\r\n", absMaxR);
+
+ debug->printf("\r\nReading and averaging %d readings for left channel no beacon:\r\n", SAMPLES_TO_AVERAGE);
+ readingL = readAndAverageL(SAMPLES_TO_AVERAGE);
+ debug->printf("\tLeft channel average (beacon off): %5.5d\r\n", readingL);
+
+ debug->printf("\r\nReading and averaging %d readings for right channel no beacon:\r\n", SAMPLES_TO_AVERAGE);
+ readingR = readAndAverageR(SAMPLES_TO_AVERAGE);
+ debug->printf("\tRight channel average (beacon off): %5.5d\r\n", readingR);
+
+ absMinL = readingL;
+ absMinR = readingR;
+
+ debug->printf("Minimums gathered:\r\n");
+ debug->printf("\tminL = %d\r\n", absMinL);
+ debug->printf("\tminR = %d\r\n", absMinR);
+
+ debug->printf("Power on beacon and place directly in front and center of robot. Press any key to continue.\r\n");
+ while ( !debug->readable() )
+ {
+ debug->printf("rssiL = %d\r\n", readAndAverageL(SAMPLES_TO_AVERAGE));
+ debug->printf("rssiR = %d\r\n", readAndAverageR(SAMPLES_TO_AVERAGE));
+ wait(1);
+ }
+ debug->rxBufferFlush();
+
+
+ debug->printf("\r\nReading and averaging %d readings for left channel with beacon:\r\n", SAMPLES_TO_AVERAGE);
+ readingL = readAndAverageL(SAMPLES_TO_AVERAGE);
+ debug->printf("\tLeft channel average (beacon on): %5.5d\r\n", readingL);
+
+ debug->printf("\r\nReading and averaging %d readings for right channel with beacon:\r\n", SAMPLES_TO_AVERAGE);
+ readingR = readAndAverageR(SAMPLES_TO_AVERAGE);
+ debug->printf("\tRight channel average (beacon on): %5.5d\r\n", readingR);
+
+ absMaxL = readingL;
+ absMaxR = readingR;
+
+ debug->printf("Maximums gathered:\r\n");
+ debug->printf("\tmaxL = %d\r\n", absMaxL);
+ debug->printf("\tmaxR = %d\r\n", absMaxR);
+
+ debug->printf("Calculating gains:\r\n");
+ gainL = 65535 / (absMaxL - absMinL);
+ gainR = 65535 / (absMaxR - absMinR);
+ debug->printf("\tLeft gain = %d\r\n", gainL);
+ debug->printf("\tRight gain = %d\r\n", gainR);
+
+ debug->printf("Calculating offsets:\r\n");
+ offsL = absMinL;
+ offsR = absMinR;
+ debug->printf("\tLeft offset = %d\r\n", offsL);
+ debug->printf("\tRight offset = %d\r\n", offsR);
+
+ debug->printf("Calibration complete! Press any key to continue.\r\n");
+ while ( !debug->readable() );
+ debug->rxBufferFlush();
+}
void
Beacon::scan(void)
{
- valL = ainL.read_u16() >> 8;
- valR = ainR.read_u16() >> 8;
+ int32_t tmpL = readAndAverageL(4) - offsL;
+ int32_t tmpR = readAndAverageR(4) - offsR;
+
+ if (tmpL < 0) tmpL = 0;
+ if (tmpR < 0) tmpR = 0;
+
+ valL = (tmpL * gainL) >> 8;
+ valR = (tmpR * gainR) >> 8;
+
+// valL = ainL.read_u16() >> 8;
+// valR = ainR.read_u16() >> 8;
+
// valR = ainL.read_u16() >> 8;
// valL = ainR.read_u16() >> 8;
- valL > valR ? max = valL : max = valR;
+ (valL > valR) ? max = valL : max = valR;
calc_centeroid();
}
@@ -30,19 +153,19 @@
return centeroid;
}
-uint8_t
+uint16_t
Beacon::get_max_rssi(void)
{
return max;
}
-uint8_t
+uint16_t
Beacon::getL(void)
{
return valL;
}
-uint8_t
+uint16_t
Beacon::getR(void)
{
return valR;
--- a/Beacon.h Fri Nov 02 03:00:29 2012 +0000
+++ b/Beacon.h Sat Nov 03 01:03:16 2012 +0000
@@ -2,29 +2,45 @@
#define _BEACON_H_
#include "mbed.h"
+#include "MODSERIAL.h"
class Beacon
{
public:
- Beacon(void) : ainL(p16), ainR(p17) {};
+ Beacon(void) : ainL(p16), ainR(p17)
+ {
+ offsL = 0;
+ gainL = 1;
+ offsR = 0;
+ gainR = 1;
+ };
void scan(void);
- uint8_t get_max_rssi(void);
+ void calibrate(MODSERIAL * debug);
+ uint16_t get_max_rssi(void);
float get_centeroid(void);
- uint8_t getL(void);
- uint8_t getR(void);
+ uint16_t getL(void);
+ uint16_t getR(void);
private:
+ uint16_t readAndAverageL(int times);
+ uint16_t readAndAverageR(int times);
+
void calc_centeroid(void);
AnalogIn ainL;
AnalogIn ainR;
- uint8_t valL;
- uint8_t valR;
+ uint16_t valL;
+ uint16_t valR;
- uint8_t max;
+ int32_t offsL;
+ int32_t gainL;
+ int32_t offsR;
+ int32_t gainR;
+
+ uint16_t max;
float centeroid;
};
--- a/IRObjDetector.cpp Fri Nov 02 03:00:29 2012 +0000
+++ b/IRObjDetector.cpp Sat Nov 03 01:03:16 2012 +0000
@@ -42,10 +42,13 @@
tmp += ain.read_u16();
// Take reading and subtract off ambient.
- tmp = tmp / 4 - data[7];
+ tmp = (tmp / 4) - data[7];
- if (tmp < 0)
+ if (tmp < 0)
+ {
data[i] = 0;
+ tmp = 0;
+ }
else
data[i] = tmp >> 8;