Alvaro Cassinelli
/
skinGames_II
save loops
Diff: simpleLaserRenderer.cpp
- Revision:
- 0:df6fdd9b99f0
diff -r 000000000000 -r df6fdd9b99f0 simpleLaserRenderer.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/simpleLaserRenderer.cpp Tue Dec 02 04:39:15 2014 +0000 @@ -0,0 +1,285 @@ +#include "simpleLaserRenderer.h" + +void simpleLaserSensingRenderer::setConfigToRender(blobConfig* ptBlobCf) { + //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. + ptBlobCfToRender=ptBlobCf; + // totalBlobs=ptBlobCfToRender->numBlobs; // equal in fact to blobArray.size() + totalBlobs=ptBlobCfToRender->blobArray.size(); // equal in fact to blobArray.size() + + // NOTE: the wait times (normal, start and end point) can be BLOB dependent. This may be a nice future (TO DO?). + +// SET STATE MACHINE TO INITIAL VALUES: + waitFirst=0; + waitFirstLaser=0; + waitNormal=0; + waitLaser=0; + waitLast=0; + 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) + stateLsd=MOVE_NEXT_BLOB; + + /* + // For tests: case of unique blob: + currentBlob=0;// in case of unique blob (for tests) + // currentMirrorDelay=ptBlobCfToRender->blobArray[currentBlob]->delayMirrorSamples; // per blob delay! + currentTotalPoints=ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory.size(); + currentColor=tBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.displayColor; + IO.setRGBPower(currentColor|0x04); // Note: RED always on... + */ + + // 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 (totalBlobs>1) numOverlapPoints=DEFAULT_OVERLAP_POINTS; + else numOverlapPoints=0; + + configTotalPoints=0; + for (int i=0; i<totalBlobs ; i++) { + configTotalPoints+=ptBlobCfToRender->blobArray[i]->displaySensingBuffer.lsdTrajectory.size(); + } + // 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; +} + + +void simpleLaserSensingRenderer::laserRenderThread() { + + 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= 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). + // BUT enable the blue activation if one wants (in particular elastic blobs...) + if (ptBlobCfToRender->blobArray[currentBlob]->blueTouch) { + 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); + } + } +#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++; + } + } + } else { // this means we ended rendering this blob, with or without partial duplication + 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. + // 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 as the reverse of 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; + } + } + break; + case LAST_POINT: + // 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 laser (NOTE: there is no need to wait for switch off time) +#ifdef RED_BLANKING + IO.setRGBPower(0x00); +#else + IO.setRGBPower(0x04); // Note: RED always on, or really 0 +#endif + waitLast=0; + stateLsd=MOVE_NEXT_BLOB; + } + break; + case MOVE_NEXT_BLOB: + // TO DO: line and counter to avoid overshoot? + + // Start processing next blob: + currentBlob=(currentBlob+1)%totalBlobs; + + // currentMirrorDelay=ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.delayMirrorSamples; // per blob delay! + currentTotalPoints=ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory.size(); + currentColor=ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.displayColor; + currentPoint=0; + + if (totalBlobs>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= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[0].x; + y= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[0].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|0x04); // Note: RED always on... + } + if (waitFirstLaser<WAIT_FIRST_LASER) waitFirstLaser++; + else { + waitFirst=0; + waitFirstLaser=0; + stateLsd=NORMAL_POINT; // start normal point + } + } + break; + } +} + +void simpleLaserSensingRenderer::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= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPoint].x; + y= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPoint].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 ( ptBlobCfToRender->blobArray[currentBlob]->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: ) + ptBlobCfToRender->blobArray[currentBlob]->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; + +}