summaryrefslogtreecommitdiffstats
path: root/LineSensorArray.cpp
diff options
context:
space:
mode:
authorneoraider <devnull@localhost>2007-04-16 00:01:04 +0200
committerneoraider <devnull@localhost>2007-04-16 00:01:04 +0200
commitd02fbc84105ff2da74f03fd658ace8919e3e9437 (patch)
treea9ab5298ca9584e7eabc0bcb6d8b34ca1ad55726 /LineSensorArray.cpp
parentccc0183cee7c410d668a0b1a9153b061a4785e42 (diff)
downloadrc2007-rescue-d02fbc84105ff2da74f03fd658ace8919e3e9437.tar
rc2007-rescue-d02fbc84105ff2da74f03fd658ace8919e3e9437.zip
LineSensorArray + Srf10 added.
Diffstat (limited to 'LineSensorArray.cpp')
-rw-r--r--LineSensorArray.cpp70
1 files changed, 70 insertions, 0 deletions
diff --git a/LineSensorArray.cpp b/LineSensorArray.cpp
new file mode 100644
index 0000000..48590cc
--- /dev/null
+++ b/LineSensorArray.cpp
@@ -0,0 +1,70 @@
+#include "LineSensorArray.h"
+
+LineSensorArray::LineSensorArray(LineSensor *sensors[5])
+{
+ for(int i = 0; i < 5; i++)
+ this->sensors[i] = sensors[i];
+
+ line = 0.0;
+ sharpness = 0.0;
+ gw = 0.0;
+
+ min_diff = 0.0;
+ max_white_diff = 0.0;
+}
+
+LineSensorArray::LineSensorArray(LineSensor *sensor1, LineSensor *sensor2, LineSensor *sensor3, LineSensor *sensor4, LineSensor *sensor5)
+{
+ sensors[0] = sensor1;
+ sensors[1] = sensor2;
+ sensors[2] = sensor3;
+ sensors[3] = sensor4;
+ sensors[4] = sensor5;
+
+ line = 0.0;
+ sharpness = 0.0;
+ gw = 0.0;
+
+ min_diff = 0.0;
+ max_white_diff = 0.0;
+}
+
+void LineSensorArray::update() {
+ int values[5];
+ int nBlack = 0;
+ bool strange = false;
+
+
+ for(int i = 0; i < 5; i++) {
+ sensors[i]->updateValue();
+ if((values[i] = int(isSensorBlack(i))) == 1)
+ nBlack++;
+ else if(nBlack > 0)
+ strange = true;
+ }
+
+ if(nBlack == 0) {
+ line = 0.0;
+ sharpness = 0.0;
+ }
+ else {
+ line = (-2*values[0]-1*values[1]+values[3]+2*values[4])/(float)nBlack;
+
+ if(strange) sharpness = -1.0;
+ else sharpness = 1/(float)nBlack;
+ }
+}
+
+bool LineSensorArray::calibrate() {
+ float white = (sensors[1]->getValue() + sensors[3]->getValue())/2;
+
+ if((sensors[2]->getValue() - white) < min_diff) return false;
+ if(ABS(sensors[1]->getValue() - sensors[3]->getValue()) > max_white_diff) return false;
+
+ if(gw == 0.0)
+ gw = sensors[2]->getValue()/2 + white/2;
+ else
+ gw = sensors[2]->getValue()/4 + white/4 + gw/2;
+
+ return true;
+}