68 lines
1.3 KiB
C++
68 lines
1.3 KiB
C++
#include "LineSensor.h"
|
|
|
|
#define DARK_THRESHOLD 500;
|
|
|
|
#define LINE_THRESHOLD 110;
|
|
#define INTERSECTION_THRESHOLD 500;
|
|
|
|
void LineSensor::Initialize(void)
|
|
{
|
|
for (int i = 0; i<sensorCount; i++) {
|
|
pinMode(sensors[i], INPUT);
|
|
}
|
|
}
|
|
|
|
float LineSensor::AverageReflectance(void)
|
|
{
|
|
float sum = 0;
|
|
for (int i = 0; i<sensorCount; i++) {
|
|
int ret = analogRead(sensors[i]);
|
|
sum += ret;
|
|
}
|
|
return sum/sensorCount;
|
|
}
|
|
|
|
bool LineSensor::LineDetected() {
|
|
#ifdef DARK
|
|
return AverageReflectance() > LINE_THRESHOLD;
|
|
#endif
|
|
#ifndef DARK
|
|
return AverageReflectance() < 200;
|
|
#endif
|
|
}
|
|
|
|
float LineSensor::CalcError(void)
|
|
{
|
|
float sum_pos = 0;
|
|
float sum = 0;
|
|
for (int i = 0; i<sensorCount; i++) {
|
|
int ret = analogRead(sensors[i]);
|
|
#ifndef DARK
|
|
ret = 1000 - ret;
|
|
#endif
|
|
sum_pos += ret * (i+1);
|
|
sum += ret;
|
|
#ifdef __TRACK_DEBUG__
|
|
Serial.print(ret);
|
|
Serial.print(" ");
|
|
#endif
|
|
}
|
|
float pos = sum_pos / sum;
|
|
#ifdef __TRACK_DEBUG__
|
|
Serial.print(AverageReflectance());
|
|
Serial.print(" ");
|
|
Serial.println(pos);
|
|
#endif
|
|
return pos;
|
|
}
|
|
|
|
|
|
bool LineSensor::CheckIntersection(void)
|
|
{
|
|
#ifdef DARK
|
|
return AverageReflectance() > INTERSECTION_THRESHOLD;
|
|
#endif
|
|
#ifndef DARK
|
|
return AverageReflectance() < 100;
|
|
#endif
|
|
}
|