laserSensingDisplay.cpp

00001 #include "laserSensingDisplay.h"
00002 
00003 laserSensingDisplay lsd; // pre-instantiated cross-file global object
00004 
00005 // The constructor:
00006 laserSensingDisplay::laserSensingDisplay()
00007 {
00008 // pointDisplayCounter=65535;
00009     runningState=false;// this is important for the FIRST time (could be better in an "init" function?).
00010     displayingFinished=false;
00011 }
00012 
00013 void laserSensingDisplay::run()  // start the ticker on laserDisplayThread
00014 {
00015     timerForRendering.attach(this, &laserSensingDisplay::laserDisplayThread, RENDER_INTERVAL); // the address of the object, member function, and interval (in seconds)
00016     runningState=true;
00017 }
00018 
00019 void laserSensingDisplay::stop()   // stop the ticker on laserDisplayThread
00020 {
00021     timerForRendering.detach();
00022     runningState=false;
00023 }
00024 
00025 bool laserSensingDisplay::isRunning()
00026 {
00027     return(runningState);
00028 }
00029 
00030 void laserSensingDisplay::setSceneToDisplay(Scene* ptScene)
00031 {
00032     //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
00033 
00034     ptSceneToDisplay=ptScene;
00035     totalObjects=ptSceneToDisplay->totalObjects();
00036 
00037     // overlap display to avoid deformed saccade and give time to the mirrors to be well in the saccade trajectory
00038     // NOTE: ideally, numOverlapPoints depends on the number of points of EACH blob, as well as the distance between the spots.
00039     //       But for the time being, this will be a fixed quantity (DEFAULT_OVERLAP_POINTS).
00040     if (totalObjects>1) numOverlapPoints=DEFAULT_OVERLAP_POINTS;
00041     else numOverlapPoints=0;
00042 
00043     configTotalPoints=ptSceneToDisplay->totalPoints();
00044     // 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,
00045     // wherever the current point being displayed when we start the update/draw.
00046     // pointDisplayCounter=0;
00047 
00048     // Set time counters to 0:
00049     // NOTE: the waiting times (normal, start and end point) can be OBJECT dependent. This may be a nice future (TO DO?).
00050     waitFirst=0;
00051     waitFirstLaser=0;
00052     waitNormal=0;
00053     waitLaser=0;
00054     waitLast=0;
00055 
00056     // IMPORTANT: we have to start like this:
00057     stateLsd=START_FIRST_OBJECT;
00058     
00059     displayingFinished=false;
00060 }
00061 
00062 bool laserSensingDisplay::isDisplayingOver()
00063 {
00064     return(displayingFinished); // the value of displayingFinished will become true when the renderer finished displaying all points of all objects.
00065 }
00066 
00067 void laserSensingDisplay::startDisplayCheck()
00068 {
00069     displayingFinished=false; // we set it to false, wherever we where in the displaying process; when it becomes true, it means we had
00070     // completed at least one full display of the unchanged scene.
00071 }
00072 
00073 // THE CORE OF THE DISPLAYING ENGINE:
00074 // Note: this routine should run in a thread - but in fact it is running in an interrupt routine for the time being.
00075 void laserSensingDisplay::laserDisplayThread()
00076 {
00077     // For tests:
00078     myLed1=!myLed1;
00079     // pc.printf("Point nb: %d\n", currentPoint);// does serial works in the interrupt?
00080 
00081     switch (stateLsd) {
00082         case NORMAL_POINT:
00083             if (currentPoint<currentTotalPoints+numOverlapPoints) { // Attention: use modulo currentTotalPoints when accessing trajectory index.
00084                 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!
00085                     uint8_t currentPointWrap=currentPoint%currentTotalPoints;
00086                     x= ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.lsdTrajectory[currentPointWrap].v2.x;
00087                     y= ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.lsdTrajectory[currentPointWrap].v2.y;
00088 
00089                     IO.writeOutX(x);
00090                     IO.writeOutY(y);
00091                     // for tests:
00092                     // pc.printf("%d - %d\n", x, y);// does serial works in the interrupt?
00093                 }
00094                 if (waitNormal<WAIT_NORMAL)  {
00095                     waitNormal++;// wait a little to correct for mirror delay (note: the mirror effective waiting time is WAIT_NORMAL + WAIT_LASER)
00096                 } else {   // if we got here, it means the mirrors are well positionned: activate laser:
00097                     if ((waitLaser==0)&&(currentPoint>numOverlapPoints)) { // change laser output the first time:
00098 #ifdef SENSING_LASER_BLANKING
00099                 IO.setLaserLockinPower(1);
00100 #endif  
00101 #ifndef debugDelayMirrors
00102                     IO.setRGBPower(currentColor); 
00103 #else               // TEST MODE for delay using blue laser:
00104                         uint8_t delayedPoint=(currentPoint+currentTotalPoints-ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.delayMirrorSamples)%currentTotalPoints;
00105                         if ( ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.lsdTrajectory[delayedPoint].lightZone<0) { // note: we use PREVIOUS sensing - so as not to wait again for
00106                             //IO.setRGBPower((currentColor&0x02)|0x04); // RED always on, BLUE OFF (and green whatever it was)
00107                             // Note: better not use complicated calls?
00108                             IO.setRGBPower(currentColor|0x02); // add blue (if blue was on, nothing happens...)
00109                         } else {
00110                             IO.setGreenPower(currentColor);
00111                         }
00112 #endif
00113                     }
00114                     if (waitLaser<WAIT_LASER) {
00115                         waitLaser++; // increment wait laser counter
00116                     } else { // If we got here, it means that mirrors and laser power are both properly set:
00117 
00118                         // READ the intensity and move to the next point:
00119                         uint8_t currentPointWrap=currentPoint%currentTotalPoints;
00120                         ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.lsdTrajectory[currentPointWrap].intensity=(unsigned char)(255.0*IO.lockInCorrectedValue(x,y));
00121                         ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.lsdTrajectory[currentPointWrap].intensity=(unsigned char)(255.0*IO.lockInCorrectedValue(x,y));
00122 
00123                         // Move to next point:
00124                         currentPoint++;
00125 
00126                         waitNormal=0;
00127                         waitLaser=0;
00128 
00129                         // Update the point display counter (meaning: this point has been properly acquired - we need (at least) configTotalPoints
00130                         // of those good acquisitions before updating and re-draw). But attention! this counter may OVERFLOW!
00131                         //pointDisplayCounter++;
00132                     }
00133                 }
00134             } else { // this means we ended rendering this blob, with or without partial duplication
00135             
00136 #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
00137                 ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.processSensedData();
00138 #endif
00139             
00140                 if (totalObjects>1) stateLsd=LAST_POINT;
00141                 else { // this means we are rendering a unique blob:
00142                     // currentObject does not change (equal to 0 always), stateLsd is always NORMAL_POINT
00143                     // The only thing we need to do is to reset "currentPoint" to 0, and eventually change the color of the blob:
00144                     currentPoint=0;
00145                     currentColor=ptSceneToDisplay->objectArray[currentObject]->myColor;
00146 
00147                     // Also, note that this means we ended displaying a whole "configuration", hence:
00148                     displayingFinished=true; // (whatever the previous state was).
00149                 }
00150             }
00151             break;
00152         case LAST_POINT:
00153             // pc.printf("LAST\n");// does serial works in the interrupt?
00154 
00155             // 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
00156             // we want a faster display, in which case we will need to adjust the mirrorDelay variable to something different from 0)
00157             if (waitLast<WAIT_LAST) waitLast++;
00158             else {
00159                 // switch off displaying lasers AND if required, the sensing laser (NOTE: there is no need to wait for switch off time)
00160                 IO.setRGBPower(0x00); 
00161 #ifdef SENSING_LASER_BLANKING
00162                 IO.setLaserLockinPower(0);
00163 #endif        
00164                 waitLast=0;
00165                 stateLsd=MOVE_NEXT_OBJECT;
00166             }
00167             break;
00168 
00169         case START_FIRST_OBJECT:
00170             // pc.printf("START NEW OBJECT\n");// does serial works in the interrupt?
00171 
00172             currentObject=0;
00173 
00174             // currentMirrorDelay=ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.delayMirrorSamples; // per blob delay!
00175             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)
00176             currentColor=ptSceneToDisplay->objectArray[currentObject]->myColor;
00177             currentPoint=0;
00178 
00179             if (totalObjects>1) stateLsd=START_POINT;
00180             else stateLsd=NORMAL_POINT; // in this case, we can skip the waiting for the last point (and first point too)
00181             break;
00182 
00183         case MOVE_NEXT_OBJECT:
00184             // TO DO: line and counter to avoid overshoot?
00185 
00186             // Start processing next blob:
00187             currentObject=(currentObject+1)%totalObjects;
00188 
00189             // NOTE: check if this was the last object:
00190             if (currentObject==0) {
00191                 displayingFinished=true; // that meant we cycle over the whole configuration
00192             }
00193 
00194             // currentMirrorDelay=ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.delayMirrorSamples; // per blob delay!
00195             currentTotalPoints=ptSceneToDisplay->objectArray[currentObject]->size();// displaySensingBuffer.lsdTrajectory.size();
00196             currentColor=ptSceneToDisplay->objectArray[currentObject]->myColor;
00197             currentPoint=0;
00198 
00199             if (totalObjects>1) stateLsd=START_POINT;
00200             else stateLsd=NORMAL_POINT; // in this case, we can skip the waiting for the last point (and first point too)
00201 
00202             break;
00203 
00204         case START_POINT:
00205             if (waitFirst==0) {
00206                 // Send command to position the mirrors on the first point of NEXT blob (laser is flying in between during this time... )
00207                 x= ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.lsdTrajectory[0].v2.x;
00208                 y= ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.lsdTrajectory[0].v2.y;
00209                 IO.writeOutX(x);
00210                 IO.writeOutY(y);
00211             }
00212             if (waitFirst<WAIT_FIRST) waitFirst++; // time for positioning of mirrors on next blob.
00213             else { //mirrors are positioned: activate laser and lock in (needs time):
00214                 if (waitFirstLaser==0) {
00215                     // activate laser - important in particular for giving time to the Lock-in to catch signal, then laser rouge:
00216                     IO.setRGBPower(currentColor);
00217 #ifdef SENSING_LASER_BLANKING     
00218                     IO.setLaserLockinPower(1);
00219 #endif
00220                 }
00221                 if (waitFirstLaser<WAIT_FIRST_LASER) waitFirstLaser++;
00222                 else  {
00223                     waitFirst=0;
00224                     waitFirstLaser=0;
00225                     stateLsd=NORMAL_POINT; // start normal point
00226                 }
00227             }
00228             break;
00229     }
00230 }
00231 
00232 /*
00233 void laserSensingDisplay::laserRenderThreadONEBLOBONLY() {
00234     // When we arrive here, we ASSUME the mirrors are well positioned at the currentPoint-1, so we need to process the currentPoint:
00235 
00236     // Current mirror position:
00237     x= ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.lsdTrajectory[currentPoint].v2.x;
00238     y= ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.lsdTrajectory[currentPoint].v2.y;
00239 
00240     // (2) Send command to position the mirrors to the next position:
00241     IO.writeOutX(x);
00242     IO.writeOutY(y);
00243 
00244 //   int delayedPoint=(currentPoint+currentMirrorDelay)%currentTotalPoints;
00245 
00246 #ifdef debugDelayMirrors
00247     if ( ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.lsdTrajectory[currentPoint].lightZone<0) {
00248         IO.setBluePower(0);
00249         // myled3=0;
00250     } else {
00251         IO.setBluePower(1);
00252         // myled3=1;
00253     }
00254     //IO.setRGBPower(0x04); else  IO.setRGBPower(0x07);
00255 #endif
00256 
00257     // (1) SENSING (on the current blob and particle index with mirror delay: )
00258     ptSceneToDisplay->objectArray[currentObject]->displaySensingBuffer.lsdTrajectory[currentPoint].intensity=(unsigned char)(255.0*IO.lockInCorrectedValue(x,y));
00259     //=lockin.getMedianValue(); //lockin.getLastValue();//
00260 
00261     // increment the current point index:
00262     currentPoint=(currentPoint+1)%currentTotalPoints;
00263 
00264 }
00265 */
00266