Diff: classLaserSensingTrajectory.cpp

Revision:

0:df6fdd9b99f0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/classLaserSensingTrajectory.cpp	Tue Dec 02 04:39:15 2014 +0000
@@ -0,0 +1,100 @@
+#include "classLaserSensingTrajectory.h"
+using namespace std;
+
+LaserSensingTrajectory::LaserSensingTrajectory():lightTouched(false), lightState(ALL_DARK), 
+min_contrast_ratio(MIN_CONTRAST_RATIO), threshold_factor(THRESHOLD_FACTOR), 
+min_acceptable_intensity(MIN_ACCEPTABLE_INTENSITY), fixedThreshold(FIXED_THRESHOLD), delayMirrorSamples(0) 
+{
+    lsdTrajectory.clear(); // no need in principle! the constructor of the vector will give an empty vector!
+    // attention: properly set the state of the threshold switch in the IO.init (which by the way, in THIS hardware
+    // implementation indicates the state for ALL the objects..)
+   modeThreshold=AUTO;
+  // IO.setSwitchOneState(true);
+}
+
+LaserSensingTrajectory::~LaserSensingTrajectory() {
+   // lsdTrajectory.clear(); // there is no need to clear the vector, the destructor of this vector is called by default (and it's NOT a vector of pointers)
+}
+
+  
+void LaserSensingTrajectory::setDelayMirrors(int delay) {
+    delayMirrorSamples=delay;
+}
+
+void LaserSensingTrajectory::addDelayMirrors(int add_delay) {
+    delayMirrorSamples+=add_delay;
+}
+
+bool LaserSensingTrajectory::processSensedData() {
+    // Compute max and min intensity on the loop
+    maxI=0;
+    minI=255; // ratio has been normalized between 0 and 255
+    unsigned short auxSize=lsdTrajectory.size(); // could be an unsigned char in principle... no more than 255 points per object, but well, memory in future versions
+    // of the microprocessor can be larger.
+    
+    // Compute minimum and maximum intensities:
+    for (unsigned short i = 0; i < auxSize; i++) {
+        unsigned char mesI=lsdTrajectory[i].intensity; 
+        if (maxI<mesI)  maxI=mesI;
+        if (minI>mesI)  minI=mesI;
+    }
+
+    // Compute autoThreshold:
+    switch(modeThreshold) {
+    case AUTO:
+        if (minI==0) minI=1;
+        if (maxI<min_acceptable_intensity) autoThreshold=255;// (we consider that the saccade is FULL on something black - this is noise)
+        else if (1.0*maxI/minI > min_contrast_ratio ) {
+            autoThreshold = (unsigned char) (1.0 * (maxI-minI) * threshold_factor + minI);    // threshold_factor = 2/3 or 1/2 is a good value.
+        } else {// ... otherwise, we consider that the saccade is FULL on something WHITE (convention...)
+            autoThreshold=0;
+        }
+    break;
+    case FIXED:
+        autoThreshold=fixedThreshold;
+    break;
+    }
+  
+
+    // Segment the trajectory (only two levels for the time being, but we can have more - meaning different forces, real or even complex values to have different angle forces...):
+    // NOTE: if using 1 and -1 instead of 1 and 0, we can avoid having to add a "blob internal pressure"! -1 means a force towards the interior, and +1 outwards...
+    // This means that the loop will naturally become inside-out depending on the color of the main surface! (but will mantain its normal size). Much better and elegant solution than the
+    // idea of the blob "constant" internal pressure...
+    bool isLightZone=false, isDarkZone=false;
+    for (unsigned short i = 0; i <  auxSize; i++) {
+        unsigned short delayedpoint=(i+auxSize+delayMirrorSamples)%auxSize; // this way we can have negative delayMirrorSamples if required (would be absurd though)
+        if (lsdTrajectory[delayedpoint].intensity>=autoThreshold) { // this means a WHITE zone:
+            lsdTrajectory[i].lightZone= -1;
+            isLightZone=true;
+        } else { // this means DARK ZONE
+            lsdTrajectory[i].lightZone= 2;
+            isDarkZone=true;
+        }
+    }
+    
+    // In the case of AUTO mode, we assume that something is touching the object when the trajectory has at least one light zone and one dark zone;
+    // in the case of FIXED mode, we assume something is touching the object when there is at least one light zone (this is a convention, because it depends
+    // on the background - but we can argue that "no background" corresponds to no reflection (then dark), and, say, a ping pong ball or finger is front 
+    // will be "light".
+    // Mmm... not convinced by the arguments above. Let's rather do this: lightTouched will mean ALWAYS that the saccade is touched by something, meaning it's intensity
+    // goes from dark to light, i.e. at least one light AND one dark zone. In fact we should have THREE states: touched, all light, all dark. 
+    if (modeThreshold==FIXED)  {
+        lightTouched=isLightZone;
+        lightZone=isLightZone;
+        darkZone=isDarkZone;
+        if (lightTouched) lightState=TOUCHED;
+        else if (isLightZone) lightState=ALL_LIGHT;
+        else lightState=ALL_DARK;
+    } 
+    else {
+        lightTouched=(isLightZone&&isDarkZone); // assuming only two modes for modeThreshold
+        lightZone=isLightZone;
+        darkZone=isDarkZone;
+        if (lightTouched) lightState=TOUCHED;
+        else if (isLightZone) lightState=ALL_LIGHT;
+        else lightState=ALL_DARK;
+    }
+    // Return lightTouched for commodity: 
+    //return(lightTouched);
+    return(lightState);
+}