simpleLaserRenderer.cpp

00001 #include "simpleLaserRenderer.h"
00002 
00003 void simpleLaserSensingRenderer::setConfigToRender(blobConfig* ptBlobCf) {
00004     //Note: when setting the config to render, we assume the number of blobs is fixed, and the number of points per blob is also fixed.
00005     ptBlobCfToRender=ptBlobCf;
00006     // totalBlobs=ptBlobCfToRender->numBlobs; // equal in fact to blobArray.size()
00007     totalBlobs=ptBlobCfToRender->blobArray.size(); // equal in fact to blobArray.size()
00008 
00009     // NOTE: the wait times (normal, start and end point) can be BLOB dependent. This may be a nice future (TO DO?).
00010 
00011 // SET STATE MACHINE TO INITIAL VALUES:
00012     waitFirst=0;
00013     waitFirstLaser=0;
00014     waitNormal=0;
00015     waitLaser=0;
00016     waitLast=0;
00017     currentBlob=-1; // this is only for the very first time we initialize the state machine (we could have another initial state, but this would be inefficient)
00018     stateLsd=MOVE_NEXT_BLOB;
00019 
00020     /*
00021      // For tests: case of unique blob:
00022       currentBlob=0;// in case of unique blob (for tests)
00023     // currentMirrorDelay=ptBlobCfToRender->blobArray[currentBlob]->delayMirrorSamples; // per blob delay!
00024      currentTotalPoints=ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory.size();
00025      currentColor=tBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.displayColor;
00026      IO.setRGBPower(currentColor|0x04); // Note: RED always on...
00027      */
00028 
00029     // overlap display to avoid deformed saccade and give time to the mirrors to be well in the saccade trajectory
00030     // NOTE: ideally, numOverlapPoints depends on the number of points of EACH blob, as well as the distance between the spots. 
00031     //       But for the time being, this will be a fixed quantity (DEFAULT_OVERLAP_POINTS).
00032     if (totalBlobs>1) numOverlapPoints=DEFAULT_OVERLAP_POINTS; 
00033     else numOverlapPoints=0;
00034 
00035     configTotalPoints=0;
00036     for (int i=0; i<totalBlobs ; i++) {
00037         configTotalPoints+=ptBlobCfToRender->blobArray[i]->displaySensingBuffer.lsdTrajectory.size();
00038     }
00039     // 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,
00040     // wherever the current point being displayed when we start the update/draw.
00041     pointDisplayCounter=0;
00042   
00043 }
00044 
00045 bool simpleLaserSensingRenderer::endedFullDisplay() {
00046     return(pointDisplayCounter>configTotalPoints);
00047 }
00048 
00049 bool simpleLaserSensingRenderer::endedFractionDisplay(int frac) {
00050     if (frac==0) return(true); 
00051     else  return(pointDisplayCounter>configTotalPoints/frac);
00052 }
00053 
00054 void simpleLaserSensingRenderer::startFullDisplay() {
00055     pointDisplayCounter=0;
00056 }
00057 
00058 
00059 void simpleLaserSensingRenderer::laserRenderThread() {
00060 
00061     switch (stateLsd) {
00062         case NORMAL_POINT:
00063             if (currentPoint<currentTotalPoints+numOverlapPoints) { // Attention: use modulo currentTotalPoints when accessing trajectory index.
00064                 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!
00065                 
00066                     uint8_t currentPointWrap=currentPoint%currentTotalPoints; 
00067                     x= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].x;
00068                     y= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].y;
00069 
00070                     IO.writeOutX(x);
00071                     IO.writeOutY(y);
00072                 }
00073                 if (waitNormal<WAIT_NORMAL)  {
00074                     waitNormal++;// wait a little to correct for mirror delay (note: the mirror effective waiting time is WAIT_NORMAL + WAIT_LASER)
00075                 } else {   // if we got here, it means the mirrors are well positionned: activate laser:
00076                     if ((waitLaser==0)&&(currentPoint>numOverlapPoints)) { // change laser output the first time:
00077 #ifndef debugDelayMirrors
00078                         IO.setRGBPower(currentColor|0x04); // Note: the "RED" here also affects the lockin laser (now red, in the future IR). 
00079                         // BUT enable the blue activation if one wants (in particular elastic blobs...)
00080                         if (ptBlobCfToRender->blobArray[currentBlob]->blueTouch) {
00081                             uint8_t delayedPoint=currentPoint; 
00082                         if ( ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[delayedPoint].lightZone<0) { // note: we use PREVIOUS sensing - so as not to wait again for
00083                             //IO.setRGBPower((currentColor&0x02)|0x04); // RED always on, BLUE OFF (and green whatever it was)
00084                             // Note: better not use complicated calls?
00085                             IO.setBluePower(0);
00086                             IO.setGreenPower(currentColor&0x02);
00087                         } else {
00088                             //IO.setRGBPower((currentColor|0x01)|0x04); // RED always ON, BLUE ON (and green whatever it was)
00089                             IO.setBluePower(1);
00090                             IO.setGreenPower(currentColor&0x02);
00091                         }
00092                             }
00093 #else               // TEST MODE for delay using blue laser:
00094                     // 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 
00095                     // compute delayedPoint, but exactly the reverse as the calculation made in the classLaserSensingTrajectory...
00096                         // (a) "raw":
00097                         //uint8_t delayedPoint=(currentPoint+currentTotalPoints-ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.delayMirrorSamples)%currentTotalPoints;
00098                         // (b) corrected delay:
00099                         uint8_t delayedPoint=currentPoint; 
00100                         if ( ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[delayedPoint].lightZone<0) { // note: we use PREVIOUS sensing - so as not to wait again for
00101                             //IO.setRGBPower((currentColor&0x02)|0x04); // RED always on, BLUE OFF (and green whatever it was)
00102                             // Note: better not use complicated calls?
00103                             IO.setBluePower(0);
00104                             IO.setGreenPower(currentColor&0x02);
00105                         } else {
00106                             //IO.setRGBPower((currentColor|0x01)|0x04); // RED always ON, BLUE ON (and green whatever it was)
00107                             IO.setBluePower(1);
00108                             IO.setGreenPower(currentColor&0x02);
00109                         }
00110 #endif
00111                     }
00112                     if (waitLaser<WAIT_LASER) {
00113                         waitLaser++; // increment wait laser counter
00114                     } else { // If we got here, it means that mirrors and laser power are both properly set:
00115                         // Read the intensity and move to the next point:
00116 
00117                         uint8_t currentPointWrap=currentPoint%currentTotalPoints;
00118                         ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].intensity=(unsigned char)(255.0*IO.lockInCorrectedValue(x,y));
00119                         ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].intensity=(unsigned char)(255.0*IO.lockInCorrectedValue(x,y));
00120 
00121                         // Move to next point:
00122                         currentPoint++;
00123 
00124                         waitNormal=0;
00125                         waitLaser=0;
00126 
00127                         // 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)
00128                         pointDisplayCounter++;
00129                     }
00130                 }
00131             } else { // this means we ended rendering this blob, with or without partial duplication
00132                 if (totalBlobs>1) stateLsd=LAST_POINT;
00133                 else { // this means we are rendering a unique blob:
00134                     // currentBlob does not change (equal to 0 always), stateLsd is always NORMAL_POINT
00135                     // The only thing we need to do is to reset "currentPoint" to 0, and eventually change the color of the blob.
00136                     // 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
00137                     // currentPoint to 1:
00138                     currentPoint=0; // and we copy the code in the NORMAL mode (this will increment currentPoint):
00139                     
00140                     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!
00141                 
00142                     uint8_t currentPointWrap=currentPoint%currentTotalPoints; 
00143                     x= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].x;
00144                     y= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].y;
00145 
00146                     IO.writeOutX(x);
00147                     IO.writeOutY(y);
00148                 }
00149                 if (waitNormal<WAIT_NORMAL)  {
00150                     waitNormal++;// wait a little to correct for mirror delay (note: the mirror effective waiting time is WAIT_NORMAL + WAIT_LASER)
00151                 } else {   // if we got here, it means the mirrors are well positionned: activate laser:
00152                     if ((waitLaser==0)&&(currentPoint>numOverlapPoints)) { // change laser output the first time:
00153 #ifndef debugDelayMirrors
00154                         IO.setRGBPower(currentColor|0x04); // Note: the "RED" here also affects the lockin laser (now red, in the future IR). 
00155 #else               // TEST MODE for delay using blue laser:
00156                     // 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 
00157                     // compute delayedPoint, but exactly as the reverse of the calculation made in the classLaserSensingTrajectory...
00158                         // (a) "raw":
00159                         //uint8_t delayedPoint=(currentPoint+currentTotalPoints-ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.delayMirrorSamples)%currentTotalPoints;
00160                         // (b) corrected delay:
00161                         uint8_t delayedPoint=currentPoint; 
00162                         if ( ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[delayedPoint].lightZone<0) { // note: we use PREVIOUS sensing - so as not to wait again for
00163                             //IO.setRGBPower((currentColor&0x02)|0x04); // RED always on, BLUE OFF (and green whatever it was)
00164                             // Note: better not use complicated calls?
00165                             IO.setBluePower(0);
00166                             IO.setGreenPower(currentColor&0x02);
00167                         } else {
00168                             //IO.setRGBPower((currentColor|0x01)|0x04); // RED always ON, BLUE ON (and green whatever it was)
00169                             IO.setBluePower(1);
00170                             IO.setGreenPower(currentColor&0x02);
00171                         }
00172 #endif
00173                     }
00174                     if (waitLaser<WAIT_LASER) {
00175                         waitLaser++; // increment wait laser counter
00176                     } else { // If we got here, it means that mirrors and laser power are both properly set:
00177                         // Read the intensity and move to the next point:
00178 
00179                         uint8_t currentPointWrap=currentPoint%currentTotalPoints;
00180                         ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].intensity=(unsigned char)(255.0*IO.lockInCorrectedValue(x,y));
00181                         ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].intensity=(unsigned char)(255.0*IO.lockInCorrectedValue(x,y));
00182 
00183                         // Move to next point:
00184                         currentPoint++;
00185 
00186                         waitNormal=0;
00187                         waitLaser=0;
00188 
00189                         // 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)
00190                         pointDisplayCounter++;
00191                     }
00192                 }
00193                     
00194                     currentColor=ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.displayColor;
00195                 }
00196             }
00197             break;
00198         case LAST_POINT:
00199             // 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
00200             // we want a faster display, in which case we will need to adjust the mirrorDelay variable to something different from 0)
00201             if (waitLast<WAIT_LAST) waitLast++;
00202             else {
00203                 // switch off laser (NOTE: there is no need to wait for switch off time)
00204 #ifdef RED_BLANKING
00205                IO.setRGBPower(0x00);
00206 #else
00207                IO.setRGBPower(0x04); // Note: RED always on, or really 0
00208 #endif
00209                 waitLast=0;
00210                 stateLsd=MOVE_NEXT_BLOB;
00211             }
00212             break;
00213         case MOVE_NEXT_BLOB:
00214             // TO DO: line and counter to avoid overshoot?
00215 
00216             // Start processing next blob:
00217             currentBlob=(currentBlob+1)%totalBlobs;
00218 
00219             // currentMirrorDelay=ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.delayMirrorSamples; // per blob delay!
00220             currentTotalPoints=ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory.size();
00221             currentColor=ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.displayColor;
00222             currentPoint=0;
00223 
00224             if (totalBlobs>1) stateLsd=START_POINT;
00225             else stateLsd=NORMAL_POINT; // in this case, we can skip the waiting for the last point (and first point too)
00226 
00227             break;
00228 
00229         case START_POINT:
00230             if (waitFirst==0) {
00231                 // Send command to position the mirrors on the first point of NEXT blob (laser is flying in between during this time... )
00232                 x= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[0].x;
00233                 y= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[0].y;
00234                 IO.writeOutX(x);
00235                 IO.writeOutY(y);
00236             }
00237             if (waitFirst<WAIT_FIRST) waitFirst++; // time for positioning of mirrors on next blob.
00238             else { //mirrors are positioned: activate laser and lock in (needs time):
00239                 if (waitFirstLaser==0) {
00240                     // activate laser - important in particular for giving time to the Lock-in to catch signal, then laser rouge:
00241                     IO.setRGBPower(currentColor|0x04); // Note: RED always on...
00242                 }
00243                 if (waitFirstLaser<WAIT_FIRST_LASER) waitFirstLaser++;
00244                 else  {
00245                     waitFirst=0;
00246                     waitFirstLaser=0;
00247                     stateLsd=NORMAL_POINT; // start normal point
00248                 }
00249             }
00250             break;
00251     }
00252 }
00253 
00254 void simpleLaserSensingRenderer::laserRenderThreadONEBLOBONLY() {
00255     // When we arrive here, we ASSUME the mirrors are well positioned at the currentPoint-1, so we need to process the currentPoint:
00256 
00257     // Current mirror position:
00258     x= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPoint].x;
00259     y= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPoint].y;
00260 
00261     // (2) Send command to position the mirrors to the next position:
00262     IO.writeOutX(x);
00263     IO.writeOutY(y);
00264 
00265 //   int delayedPoint=(currentPoint+currentMirrorDelay)%currentTotalPoints;
00266 
00267 #ifdef debugDelayMirrors
00268     if ( ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPoint].lightZone<0) {
00269         IO.setBluePower(0);
00270         // myled3=0;
00271     } else {
00272         IO.setBluePower(1);
00273         // myled3=1;
00274     }
00275     //IO.setRGBPower(0x04); else  IO.setRGBPower(0x07);
00276 #endif
00277 
00278     // (1) SENSING (on the current blob and particle index with mirror delay: )
00279     ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPoint].intensity=(unsigned char)(255.0*IO.lockInCorrectedValue(x,y));
00280     //=lockin.getMedianValue(); //lockin.getLastValue();//
00281 
00282     // increment the current point index:
00283     currentPoint=(currentPoint+1)%currentTotalPoints;
00284 
00285 }