Alvaro Cassinelli
save loops
Diff: simpleLaserRenderer.cpp
- Revision:
- 0:df6fdd9b99f0
--- /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;
+
+}