diff options
author | neoraider <devnull@localhost> | 2007-04-16 00:01:04 +0200 |
---|---|---|
committer | neoraider <devnull@localhost> | 2007-04-16 00:01:04 +0200 |
commit | d02fbc84105ff2da74f03fd658ace8919e3e9437 (patch) | |
tree | a9ab5298ca9584e7eabc0bcb6d8b34ca1ad55726 /LineSensorArray.cpp | |
parent | ccc0183cee7c410d668a0b1a9153b061a4785e42 (diff) | |
download | rc2007-rescue-d02fbc84105ff2da74f03fd658ace8919e3e9437.tar rc2007-rescue-d02fbc84105ff2da74f03fd658ace8919e3e9437.zip |
LineSensorArray + Srf10 added.
Diffstat (limited to 'LineSensorArray.cpp')
-rw-r--r-- | LineSensorArray.cpp | 70 |
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; +} |