From d02fbc84105ff2da74f03fd658ace8919e3e9437 Mon Sep 17 00:00:00 2001 From: neoraider Date: Sun, 15 Apr 2007 22:01:04 +0000 Subject: LineSensorArray + Srf10 added. --- LineSensorArray.cpp | 70 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 70 insertions(+) create mode 100644 LineSensorArray.cpp (limited to 'LineSensorArray.cpp') 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; +} -- cgit v1.2.3