Diff: laserSensingDisplay.cpp

Revision:

40:3ba2b0ea9f33

Child:

43:1dd4cfc30788

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/laserSensingDisplay.cpp	Wed Oct 16 16:14:27 2013 +0000
@@ -0,0 +1,263 @@
+#include "laserSensingDisplay.h"
+
+laserSensingDisplay lsd; // pre-instantiated cross-file global object
+
+// The constructor:
+laserSensingDisplay::laserSensingDisplay()
+{
+// pointDisplayCounter=65535;
+    runningState=false;// this is important for the FIRST time (could be better in an "init" function?).
+}
+
+void laserSensingDisplay::run()  // start the ticker on laserDisplayThread
+{
+    timerForRendering.attach(this, &laserSensingDisplay::laserDisplayThread, RENDER_INTERVAL); // the address of the object, member function, and interval (in seconds)
+    runningState=true;
+}
+
+void laserSensingDisplay::stop()   // stop the ticker on laserDisplayThread
+{
+    timerForRendering.detach();
+    runningState=false;
+}
+
+bool laserSensingDisplay::isRunning()
+{
+    return(runningState);
+}
+
+void laserSensingDisplay::setSceneToDisplay(Scene* ptScene)
+{
+    //Note: if the scene is recreated, it is important to first stop the displaying, and call to this function again, and only then re-attach the interrupt
+
+    ptSceneToDisplay=ptScene;
+    totalObjects=ptSceneToDisplay->totalObjects();
+
+    // overlap display to avoid deformed saccade and give time to the mirrors to be well in the saccade trajectory
+    // NOTE: ideally, numOverlapPoints depends on the number of points of EACH blob, as well as the distance between the spots.
+    //       But for the time being, this will be a fixed quantity (DEFAULT_OVERLAP_POINTS).
+    if (totalObjects>1) numOverlapPoints=DEFAULT_OVERLAP_POINTS;
+    else numOverlapPoints=0;
+
+    configTotalPoints=ptSceneToDisplay->totalPoints();
+    // 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;
+
+    // Set time counters to 0:
+    // NOTE: the waiting times (normal, start and end point) can be OBJECT dependent. This may be a nice future (TO DO?).
+    waitFirst=0;
+    waitFirstLaser=0;
+    waitNormal=0;
+    waitLaser=0;
+    waitLast=0;
+
+    // IMPORTANT: we have to start like this:
+    stateLsd=START_FIRST_OBJECT;
+}
+
+bool laserSensingDisplay::isDisplayingOver()
+{
+    return(displayingFinished); // the value of displayingFinished will become true when the renderer finished displaying all points of all objects.
+}
+
+void laserSensingDisplay::startDisplayCheck()
+{
+    displayingFinished=false; // we set it to false, wherever we where in the displaying process; when it becomes true, it means we had
+    // completed at least one full display of the unchanged scene.
+}
+
+// THE CORE OF THE DISPLAYING ENGINE:
+// Note: this routine should run in a thread - but in fact it is running in an interrupt routine for the time being.
+void laserSensingDisplay::laserDisplayThread()
+{
+    // For tests:
+    myLed1=!myLed1;
+    // pc.printf("Point nb: %d\n", currentPoint);// does serial works in the interrupt?
+
+    switch (stateLsd) {
+        case NORMAL_POINT:
+            if (currentPoint<currentTotalPoints+numOverlapPoints) { // Attention: use modulo currentTotalPoints when accessing trajectory index.
+                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= ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.lsdTrajectory[currentPointWrap].v2.x;
+                    y= ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.lsdTrajectory[currentPointWrap].v2.y;
+
+                    IO.writeOutX(x);
+                    IO.writeOutY(y);
+                    // for tests:
+                    // pc.printf("%d - %d\n", x, y);// does serial works in the interrupt?
+                }
+                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:
+#ifdef SENSING_LASER_BLANKING
+                IO.setLaserLockinPower(1);
+#endif  
+#ifndef debugDelayMirrors
+                    IO.setRGBPower(currentColor); 
+#else               // TEST MODE for delay using blue laser:
+                        uint8_t delayedPoint=(currentPoint+currentTotalPoints-ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.delayMirrorSamples)%currentTotalPoints;
+                        if ( ptSceneToDisplay->objectArray[currentObject]->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.setRGBPower(currentColor|0x02); // add blue (if blue was on, nothing happens...)
+                        } else {
+                            IO.setGreenPower(currentColor);
+                        }
+#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;
+                        ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.lsdTrajectory[currentPointWrap].intensity=(unsigned char)(255.0*IO.lockInCorrectedValue(x,y));
+                        ptSceneToDisplay->objectArray[currentObject]->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). But attention! this counter may OVERFLOW!
+                        //pointDisplayCounter++;
+                    }
+                }
+            } else { // this means we ended rendering this blob, with or without partial duplication
+            
+#ifdef debugDelayMirrors // this means that we will process the saccade data all the time, not only when querying the data! can be useful for tests only
+                ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.processSensedData();
+#endif
+            
+                if (totalObjects>1) stateLsd=LAST_POINT;
+                else { // this means we are rendering a unique blob:
+                    // currentObject 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;
+                    currentColor=ptSceneToDisplay->objectArray[currentObject]->myColor;
+
+                    // Also, note that this means we ended displaying a whole "configuration", hence:
+                    displayingFinished=true; // (whatever the previous state was).
+                }
+            }
+            break;
+        case LAST_POINT:
+            // pc.printf("LAST\n");// does serial works in the interrupt?
+
+            // We need to pause for a while (this is for avoiding a deformed end of a blob when there are more than one blob AND we did not properly correct the mirror delay - this may be because
+            // we want a faster display, in which case we will need to adjust the mirrorDelay variable to something different from 0)
+            if (waitLast<WAIT_LAST) waitLast++;
+            else {
+                // switch off displaying lasers AND if required, the sensing laser (NOTE: there is no need to wait for switch off time)
+                IO.setRGBPower(0x00); 
+#ifdef SENSING_LASER_BLANKING
+                IO.setLaserLockinPower(0);
+#endif        
+                waitLast=0;
+                stateLsd=MOVE_NEXT_OBJECT;
+            }
+            break;
+
+        case START_FIRST_OBJECT:
+            // pc.printf("START NEW OBJECT\n");// does serial works in the interrupt?
+
+            currentObject=0;
+
+            // currentMirrorDelay=ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.delayMirrorSamples; // per blob delay!
+            currentTotalPoints=ptSceneToDisplay->objectArray[currentObject]->size(); // or using: displaySensingBuffer.lsdTrajectory.size() (but now I made it private to mantain size consistancy between 3d and 2d array size)
+            currentColor=ptSceneToDisplay->objectArray[currentObject]->myColor;
+            currentPoint=0;
+
+            if (totalObjects>1) stateLsd=START_POINT;
+            else stateLsd=NORMAL_POINT; // in this case, we can skip the waiting for the last point (and first point too)
+            break;
+
+        case MOVE_NEXT_OBJECT:
+            // TO DO: line and counter to avoid overshoot?
+
+            // Start processing next blob:
+            currentObject=(currentObject+1)%totalObjects;
+
+            // NOTE: check if this was the last object:
+            if (currentObject==0) {
+                displayingFinished=true; // that meant we cycle over the whole configuration
+            }
+
+            // currentMirrorDelay=ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.delayMirrorSamples; // per blob delay!
+            currentTotalPoints=ptSceneToDisplay->objectArray[currentObject]->size();// displaySensingBuffer.lsdTrajectory.size();
+            currentColor=ptSceneToDisplay->objectArray[currentObject]->myColor;
+            currentPoint=0;
+
+            if (totalObjects>1) stateLsd=START_POINT;
+            else stateLsd=NORMAL_POINT; // in this case, we can skip the waiting for the last point (and first point too)
+
+            break;
+
+        case START_POINT:
+            if (waitFirst==0) {
+                // Send command to position the mirrors on the first point of NEXT blob (laser is flying in between during this time... )
+                x= ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.lsdTrajectory[0].v2.x;
+                y= ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.lsdTrajectory[0].v2.y;
+                IO.writeOutX(x);
+                IO.writeOutY(y);
+            }
+            if (waitFirst<WAIT_FIRST) waitFirst++; // time for positioning of mirrors on next blob.
+            else { //mirrors are positioned: activate laser and lock in (needs time):
+                if (waitFirstLaser==0) {
+                    // activate laser - important in particular for giving time to the Lock-in to catch signal, then laser rouge:
+                    IO.setRGBPower(currentColor);
+#ifdef SENSING_LASER_BLANKING     
+                    IO.setLaserLockinPower(1);
+#endif
+                }
+                if (waitFirstLaser<WAIT_FIRST_LASER) waitFirstLaser++;
+                else  {
+                    waitFirst=0;
+                    waitFirstLaser=0;
+                    stateLsd=NORMAL_POINT; // start normal point
+                }
+            }
+            break;
+    }
+}
+
+/*
+void laserSensingDisplay::laserRenderThreadONEBLOBONLY() {
+    // When we arrive here, we ASSUME the mirrors are well positioned at the currentPoint-1, so we need to process the currentPoint:
+
+    // Current mirror position:
+    x= ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.lsdTrajectory[currentPoint].v2.x;
+    y= ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.lsdTrajectory[currentPoint].v2.y;
+
+    // (2) Send command to position the mirrors to the next position:
+    IO.writeOutX(x);
+    IO.writeOutY(y);
+
+//   int delayedPoint=(currentPoint+currentMirrorDelay)%currentTotalPoints;
+
+#ifdef debugDelayMirrors
+    if ( ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.lsdTrajectory[currentPoint].lightZone<0) {
+        IO.setBluePower(0);
+        // myled3=0;
+    } else {
+        IO.setBluePower(1);
+        // myled3=1;
+    }
+    //IO.setRGBPower(0x04); else  IO.setRGBPower(0x07);
+#endif
+
+    // (1) SENSING (on the current blob and particle index with mirror delay: )
+    ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.lsdTrajectory[currentPoint].intensity=(unsigned char)(255.0*IO.lockInCorrectedValue(x,y));
+    //=lockin.getMedianValue(); //lockin.getLastValue();//
+
+    // increment the current point index:
+    currentPoint=(currentPoint+1)%currentTotalPoints;
+
+}
+*/
+