Alvaro Cassinelli
/
skinGames_forktest
just a test
Fork of scoreLight_Advanced by
simpleLaserRenderer.cpp
- Committer:
- mbedalvaro
- Date:
- 2014-12-02
- Revision:
- 48:7633d8e7b0d3
- Parent:
- 46:90516893793a
File content as of revision 48:7633d8e7b0d3:
#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; }