Diff: simpleLaserRenderer.cpp

Revision:

44:46e25fa1669b

Parent:

30:d8af03f01cd4

Child:

45:a3b984a79d5d

diff -r 5ff1a26d99ac -r 46e25fa1669b simpleLaserRenderer.cpp
--- a/simpleLaserRenderer.cpp	Fri Mar 28 10:25:06 2014 +0000
+++ b/simpleLaserRenderer.cpp	Mon Mar 31 10:57:35 2014 +0000
@@ -39,12 +39,18 @@
     // configTotalPoints contains the number of points of the config, and will be used to ensure that a FULL DISPLAY has been done BEFORE updating and "re-drawing" the trajectory in the buffer,
     // wherever the current point being displayed when we start the update/draw.
     pointDisplayCounter=0;
+  
 }
 
 bool simpleLaserSensingRenderer::endedFullDisplay() {
     return(pointDisplayCounter>configTotalPoints);
 }
 
+bool simpleLaserSensingRenderer::endedFractionDisplay(int frac) {
+    if (frac==0) return(true); 
+    else  return(pointDisplayCounter>configTotalPoints/frac);
+}
+
 void simpleLaserSensingRenderer::startFullDisplay() {
     pointDisplayCounter=0;
 }
@@ -69,16 +75,21 @@
                 } else {   // if we got here, it means the mirrors are well positionned: activate laser:
                     if ((waitLaser==0)&&(currentPoint>numOverlapPoints)) { // change laser output the first time:
 #ifndef debugDelayMirrors
-                        IO.setRGBPower(currentColor|0x04); // Note: RED always on...
+                        IO.setRGBPower(currentColor|0x04); // Note: the "RED" here also affects the lockin laser (now red, in the future IR). 
 #else               // TEST MODE for delay using blue laser:
-                        uint8_t delayedPoint=(currentPoint+currentTotalPoints-ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.delayMirrorSamples)%currentTotalPoints;
+                    // NOTE: we can either CORRECT the delay (and see if the correction is good), or show the "raw" detection (in this case, we need to 
+                    // compute delayedPoint, but exactly the reverse as the calculation made in the classLaserSensingTrajectory...
+                        // (a) "raw":
+                        //uint8_t delayedPoint=(currentPoint+currentTotalPoints-ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.delayMirrorSamples)%currentTotalPoints;
+                        // (b) corrected delay:
+                        uint8_t delayedPoint=currentPoint; 
                         if ( ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[delayedPoint].lightZone<0) { // note: we use PREVIOUS sensing - so as not to wait again for
                             //IO.setRGBPower((currentColor&0x02)|0x04); // RED always on, BLUE OFF (and green whatever it was)
                             // Note: better not use complicated calls?
                             IO.setBluePower(0);
                             IO.setGreenPower(currentColor&0x02);
                         } else {
-                            //IO.setRGBPower((currentColor|0x01)|0x04); // RED always on, BLUE ON (and green whatever it was)
+                            //IO.setRGBPower((currentColor|0x01)|0x04); // RED always ON, BLUE ON (and green whatever it was)
                             IO.setBluePower(1);
                             IO.setGreenPower(currentColor&0x02);
                         }
@@ -107,8 +118,65 @@
                 if (totalBlobs>1) stateLsd=LAST_POINT;
                 else { // this means we are rendering a unique blob:
                     // currentBlob does not change (equal to 0 always), stateLsd is always NORMAL_POINT
-                    // The only thing we need to do is to reset "currentPoint" to 0, and eventually change the color of the blob:
-                    currentPoint=0;
+                    // The only thing we need to do is to reset "currentPoint" to 0, and eventually change the color of the blob.
+                    // NOTE that if only doing this, the point 0 will take two ISR cycles; therefore it is better to move the mirrors NOW and set the
+                    // currentPoint to 1:
+                    currentPoint=0; // and we copy the code in the NORMAL mode (this will increment currentPoint):
+                    
+                    if (waitNormal==0) { // Send mirrors position the first time (note: I don't put this inside the waitNormal<WAIT_NORMAL, because WAIT_NORMAL can be 0!
+                
+                    uint8_t currentPointWrap=currentPoint%currentTotalPoints; 
+                    x= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].x;
+                    y= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].y;
+
+                    IO.writeOutX(x);
+                    IO.writeOutY(y);
+                }
+                if (waitNormal<WAIT_NORMAL)  {
+                    waitNormal++;// wait a little to correct for mirror delay (note: the mirror effective waiting time is WAIT_NORMAL + WAIT_LASER)
+                } else {   // if we got here, it means the mirrors are well positionned: activate laser:
+                    if ((waitLaser==0)&&(currentPoint>numOverlapPoints)) { // change laser output the first time:
+#ifndef debugDelayMirrors
+                        IO.setRGBPower(currentColor|0x04); // Note: the "RED" here also affects the lockin laser (now red, in the future IR). 
+#else               // TEST MODE for delay using blue laser:
+                    // NOTE: we can either CORRECT the delay (and see if the correction is good), or show the "raw" detection (in this case, we need to 
+                    // compute delayedPoint, but exactly the reverse as the calculation made in the classLaserSensingTrajectory...
+                        // (a) "raw":
+                        //uint8_t delayedPoint=(currentPoint+currentTotalPoints-ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.delayMirrorSamples)%currentTotalPoints;
+                        // (b) corrected delay:
+                        uint8_t delayedPoint=currentPoint; 
+                        if ( ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[delayedPoint].lightZone<0) { // note: we use PREVIOUS sensing - so as not to wait again for
+                            //IO.setRGBPower((currentColor&0x02)|0x04); // RED always on, BLUE OFF (and green whatever it was)
+                            // Note: better not use complicated calls?
+                            IO.setBluePower(0);
+                            IO.setGreenPower(currentColor&0x02);
+                        } else {
+                            //IO.setRGBPower((currentColor|0x01)|0x04); // RED always ON, BLUE ON (and green whatever it was)
+                            IO.setBluePower(1);
+                            IO.setGreenPower(currentColor&0x02);
+                        }
+#endif
+                    }
+                    if (waitLaser<WAIT_LASER) {
+                        waitLaser++; // increment wait laser counter
+                    } else { // If we got here, it means that mirrors and laser power are both properly set:
+                        // Read the intensity and move to the next point:
+
+                        uint8_t currentPointWrap=currentPoint%currentTotalPoints;
+                        ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].intensity=(unsigned char)(255.0*IO.lockInCorrectedValue(x,y));
+                        ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].intensity=(unsigned char)(255.0*IO.lockInCorrectedValue(x,y));
+
+                        // Move to next point:
+                        currentPoint++;
+
+                        waitNormal=0;
+                        waitLaser=0;
+
+                        // Update the point display counter (meaning: this point has been properly acquired - we need (at least) configTotalPoints of those good acquisitions before updating and re-draw)
+                        pointDisplayCounter++;
+                    }
+                }
+                    
                     currentColor=ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.displayColor;
                 }
             }