Alvaro Cassinelli / Mbed 2 deprecated skinGames_forktest

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of scoreLight_Advanced by Alvaro Cassinelli

simpleLaserRenderer.cpp@46:90516893793a, 2014-04-01 (annotated)

Committer:
mbedalvaro
Date:
Tue Apr 01 11:34:37 2014 +0000
Revision:
46:90516893793a
Parent:
45:a3b984a79d5d 
added show parameters function to calibrate (including integration steps)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbedalvaro 30:d8af03f01cd4 1 #include "simpleLaserRenderer.h"
mbedalvaro 30:d8af03f01cd4 2
mbedalvaro 30:d8af03f01cd4 3 void simpleLaserSensingRenderer::setConfigToRender(blobConfig* ptBlobCf) {
mbedalvaro 30:d8af03f01cd4 4 //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.
mbedalvaro 30:d8af03f01cd4 5 ptBlobCfToRender=ptBlobCf;
mbedalvaro 30:d8af03f01cd4 6 // totalBlobs=ptBlobCfToRender->numBlobs; // equal in fact to blobArray.size()
mbedalvaro 30:d8af03f01cd4 7 totalBlobs=ptBlobCfToRender->blobArray.size(); // equal in fact to blobArray.size()
mbedalvaro 30:d8af03f01cd4 8
mbedalvaro 30:d8af03f01cd4 9 // NOTE: the wait times (normal, start and end point) can be BLOB dependent. This may be a nice future (TO DO?).
mbedalvaro 30:d8af03f01cd4 10
mbedalvaro 30:d8af03f01cd4 11 // SET STATE MACHINE TO INITIAL VALUES:
mbedalvaro 30:d8af03f01cd4 12 waitFirst=0;
mbedalvaro 30:d8af03f01cd4 13 waitFirstLaser=0;
mbedalvaro 30:d8af03f01cd4 14 waitNormal=0;
mbedalvaro 30:d8af03f01cd4 15 waitLaser=0;
mbedalvaro 30:d8af03f01cd4 16 waitLast=0;
mbedalvaro 30:d8af03f01cd4 17 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)
mbedalvaro 30:d8af03f01cd4 18 stateLsd=MOVE_NEXT_BLOB;
mbedalvaro 30:d8af03f01cd4 19
mbedalvaro 30:d8af03f01cd4 20 /*
mbedalvaro 30:d8af03f01cd4 21 // For tests: case of unique blob:
mbedalvaro 30:d8af03f01cd4 22 currentBlob=0;// in case of unique blob (for tests)
mbedalvaro 30:d8af03f01cd4 23 // currentMirrorDelay=ptBlobCfToRender->blobArray[currentBlob]->delayMirrorSamples; // per blob delay!
mbedalvaro 30:d8af03f01cd4 24 currentTotalPoints=ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory.size();
mbedalvaro 30:d8af03f01cd4 25 currentColor=tBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.displayColor;
mbedalvaro 30:d8af03f01cd4 26 IO.setRGBPower(currentColor|0x04); // Note: RED always on...
mbedalvaro 30:d8af03f01cd4 27 */
mbedalvaro 30:d8af03f01cd4 28
mbedalvaro 30:d8af03f01cd4 29 // overlap display to avoid deformed saccade and give time to the mirrors to be well in the saccade trajectory
mbedalvaro 30:d8af03f01cd4 30 // NOTE: ideally, numOverlapPoints depends on the number of points of EACH blob, as well as the distance between the spots.
mbedalvaro 30:d8af03f01cd4 31 // But for the time being, this will be a fixed quantity (DEFAULT_OVERLAP_POINTS).
mbedalvaro 30:d8af03f01cd4 32 if (totalBlobs>1) numOverlapPoints=DEFAULT_OVERLAP_POINTS;
mbedalvaro 30:d8af03f01cd4 33 else numOverlapPoints=0;
mbedalvaro 30:d8af03f01cd4 34
mbedalvaro 30:d8af03f01cd4 35 configTotalPoints=0;
mbedalvaro 30:d8af03f01cd4 36 for (int i=0; i<totalBlobs ; i++) {
mbedalvaro 30:d8af03f01cd4 37 configTotalPoints+=ptBlobCfToRender->blobArray[i]->displaySensingBuffer.lsdTrajectory.size();
mbedalvaro 30:d8af03f01cd4 38 }
mbedalvaro 30:d8af03f01cd4 39 // 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,
mbedalvaro 30:d8af03f01cd4 40 // wherever the current point being displayed when we start the update/draw.
mbedalvaro 30:d8af03f01cd4 41 pointDisplayCounter=0;
mbedalvaro 44:46e25fa1669b 42
mbedalvaro 30:d8af03f01cd4 43 }
mbedalvaro 30:d8af03f01cd4 44
mbedalvaro 30:d8af03f01cd4 45 bool simpleLaserSensingRenderer::endedFullDisplay() {
mbedalvaro 30:d8af03f01cd4 46 return(pointDisplayCounter>configTotalPoints);
mbedalvaro 30:d8af03f01cd4 47 }
mbedalvaro 30:d8af03f01cd4 48
mbedalvaro 44:46e25fa1669b 49 bool simpleLaserSensingRenderer::endedFractionDisplay(int frac) {
mbedalvaro 44:46e25fa1669b 50 if (frac==0) return(true);
mbedalvaro 44:46e25fa1669b 51 else return(pointDisplayCounter>configTotalPoints/frac);
mbedalvaro 44:46e25fa1669b 52 }
mbedalvaro 44:46e25fa1669b 53
mbedalvaro 30:d8af03f01cd4 54 void simpleLaserSensingRenderer::startFullDisplay() {
mbedalvaro 30:d8af03f01cd4 55 pointDisplayCounter=0;
mbedalvaro 30:d8af03f01cd4 56 }
mbedalvaro 30:d8af03f01cd4 57
mbedalvaro 30:d8af03f01cd4 58
mbedalvaro 30:d8af03f01cd4 59 void simpleLaserSensingRenderer::laserRenderThread() {
mbedalvaro 30:d8af03f01cd4 60
mbedalvaro 30:d8af03f01cd4 61 switch (stateLsd) {
mbedalvaro 30:d8af03f01cd4 62 case NORMAL_POINT:
mbedalvaro 30:d8af03f01cd4 63 if (currentPoint<currentTotalPoints+numOverlapPoints) { // Attention: use modulo currentTotalPoints when accessing trajectory index.
mbedalvaro 30:d8af03f01cd4 64 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!
mbedalvaro 30:d8af03f01cd4 65
mbedalvaro 30:d8af03f01cd4 66 uint8_t currentPointWrap=currentPoint%currentTotalPoints;
mbedalvaro 30:d8af03f01cd4 67 x= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].x;
mbedalvaro 30:d8af03f01cd4 68 y= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].y;
mbedalvaro 30:d8af03f01cd4 69
mbedalvaro 30:d8af03f01cd4 70 IO.writeOutX(x);
mbedalvaro 30:d8af03f01cd4 71 IO.writeOutY(y);
mbedalvaro 30:d8af03f01cd4 72 }
mbedalvaro 30:d8af03f01cd4 73 if (waitNormal<WAIT_NORMAL) {
mbedalvaro 30:d8af03f01cd4 74 waitNormal++;// wait a little to correct for mirror delay (note: the mirror effective waiting time is WAIT_NORMAL + WAIT_LASER)
mbedalvaro 30:d8af03f01cd4 75 } else { // if we got here, it means the mirrors are well positionned: activate laser:
mbedalvaro 30:d8af03f01cd4 76 if ((waitLaser==0)&&(currentPoint>numOverlapPoints)) { // change laser output the first time:
mbedalvaro 30:d8af03f01cd4 77 #ifndef debugDelayMirrors
mbedalvaro 44:46e25fa1669b 78 IO.setRGBPower(currentColor|0x04); // Note: the "RED" here also affects the lockin laser (now red, in the future IR).
mbedalvaro 45:a3b984a79d5d 79 // BUT enable the blue activation if one wants (in particular elastic blobs...)
mbedalvaro 45:a3b984a79d5d 80 if (ptBlobCfToRender->blobArray[currentBlob]->blueTouch) {
mbedalvaro 45:a3b984a79d5d 81 uint8_t delayedPoint=currentPoint;
mbedalvaro 45:a3b984a79d5d 82 if ( ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[delayedPoint].lightZone<0) { // note: we use PREVIOUS sensing - so as not to wait again for
mbedalvaro 45:a3b984a79d5d 83 //IO.setRGBPower((currentColor&0x02)|0x04); // RED always on, BLUE OFF (and green whatever it was)
mbedalvaro 45:a3b984a79d5d 84 // Note: better not use complicated calls?
mbedalvaro 45:a3b984a79d5d 85 IO.setBluePower(0);
mbedalvaro 45:a3b984a79d5d 86 IO.setGreenPower(currentColor&0x02);
mbedalvaro 45:a3b984a79d5d 87 } else {
mbedalvaro 45:a3b984a79d5d 88 //IO.setRGBPower((currentColor|0x01)|0x04); // RED always ON, BLUE ON (and green whatever it was)
mbedalvaro 45:a3b984a79d5d 89 IO.setBluePower(1);
mbedalvaro 45:a3b984a79d5d 90 IO.setGreenPower(currentColor&0x02);
mbedalvaro 45:a3b984a79d5d 91 }
mbedalvaro 45:a3b984a79d5d 92 }
mbedalvaro 30:d8af03f01cd4 93 #else // TEST MODE for delay using blue laser:
mbedalvaro 44:46e25fa1669b 94 // 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
mbedalvaro 44:46e25fa1669b 95 // compute delayedPoint, but exactly the reverse as the calculation made in the classLaserSensingTrajectory...
mbedalvaro 44:46e25fa1669b 96 // (a) "raw":
mbedalvaro 44:46e25fa1669b 97 //uint8_t delayedPoint=(currentPoint+currentTotalPoints-ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.delayMirrorSamples)%currentTotalPoints;
mbedalvaro 44:46e25fa1669b 98 // (b) corrected delay:
mbedalvaro 44:46e25fa1669b 99 uint8_t delayedPoint=currentPoint;
mbedalvaro 30:d8af03f01cd4 100 if ( ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[delayedPoint].lightZone<0) { // note: we use PREVIOUS sensing - so as not to wait again for
mbedalvaro 30:d8af03f01cd4 101 //IO.setRGBPower((currentColor&0x02)|0x04); // RED always on, BLUE OFF (and green whatever it was)
mbedalvaro 30:d8af03f01cd4 102 // Note: better not use complicated calls?
mbedalvaro 30:d8af03f01cd4 103 IO.setBluePower(0);
mbedalvaro 30:d8af03f01cd4 104 IO.setGreenPower(currentColor&0x02);
mbedalvaro 30:d8af03f01cd4 105 } else {
mbedalvaro 44:46e25fa1669b 106 //IO.setRGBPower((currentColor|0x01)|0x04); // RED always ON, BLUE ON (and green whatever it was)
mbedalvaro 30:d8af03f01cd4 107 IO.setBluePower(1);
mbedalvaro 30:d8af03f01cd4 108 IO.setGreenPower(currentColor&0x02);
mbedalvaro 30:d8af03f01cd4 109 }
mbedalvaro 30:d8af03f01cd4 110 #endif
mbedalvaro 30:d8af03f01cd4 111 }
mbedalvaro 30:d8af03f01cd4 112 if (waitLaser<WAIT_LASER) {
mbedalvaro 30:d8af03f01cd4 113 waitLaser++; // increment wait laser counter
mbedalvaro 30:d8af03f01cd4 114 } else { // If we got here, it means that mirrors and laser power are both properly set:
mbedalvaro 30:d8af03f01cd4 115 // Read the intensity and move to the next point:
mbedalvaro 30:d8af03f01cd4 116
mbedalvaro 30:d8af03f01cd4 117 uint8_t currentPointWrap=currentPoint%currentTotalPoints;
mbedalvaro 30:d8af03f01cd4 118 ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].intensity=(unsigned char)(255.0*IO.lockInCorrectedValue(x,y));
mbedalvaro 30:d8af03f01cd4 119 ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].intensity=(unsigned char)(255.0*IO.lockInCorrectedValue(x,y));
mbedalvaro 30:d8af03f01cd4 120
mbedalvaro 30:d8af03f01cd4 121 // Move to next point:
mbedalvaro 30:d8af03f01cd4 122 currentPoint++;
mbedalvaro 30:d8af03f01cd4 123
mbedalvaro 30:d8af03f01cd4 124 waitNormal=0;
mbedalvaro 30:d8af03f01cd4 125 waitLaser=0;
mbedalvaro 30:d8af03f01cd4 126
mbedalvaro 30:d8af03f01cd4 127 // 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)
mbedalvaro 30:d8af03f01cd4 128 pointDisplayCounter++;
mbedalvaro 30:d8af03f01cd4 129 }
mbedalvaro 30:d8af03f01cd4 130 }
mbedalvaro 30:d8af03f01cd4 131 } else { // this means we ended rendering this blob, with or without partial duplication
mbedalvaro 30:d8af03f01cd4 132 if (totalBlobs>1) stateLsd=LAST_POINT;
mbedalvaro 30:d8af03f01cd4 133 else { // this means we are rendering a unique blob:
mbedalvaro 30:d8af03f01cd4 134 // currentBlob does not change (equal to 0 always), stateLsd is always NORMAL_POINT
mbedalvaro 44:46e25fa1669b 135 // The only thing we need to do is to reset "currentPoint" to 0, and eventually change the color of the blob.
mbedalvaro 44:46e25fa1669b 136 // 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
mbedalvaro 44:46e25fa1669b 137 // currentPoint to 1:
mbedalvaro 44:46e25fa1669b 138 currentPoint=0; // and we copy the code in the NORMAL mode (this will increment currentPoint):
mbedalvaro 44:46e25fa1669b 139
mbedalvaro 44:46e25fa1669b 140 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!
mbedalvaro 44:46e25fa1669b 141
mbedalvaro 44:46e25fa1669b 142 uint8_t currentPointWrap=currentPoint%currentTotalPoints;
mbedalvaro 44:46e25fa1669b 143 x= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].x;
mbedalvaro 44:46e25fa1669b 144 y= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].y;
mbedalvaro 44:46e25fa1669b 145
mbedalvaro 44:46e25fa1669b 146 IO.writeOutX(x);
mbedalvaro 44:46e25fa1669b 147 IO.writeOutY(y);
mbedalvaro 44:46e25fa1669b 148 }
mbedalvaro 44:46e25fa1669b 149 if (waitNormal<WAIT_NORMAL) {
mbedalvaro 44:46e25fa1669b 150 waitNormal++;// wait a little to correct for mirror delay (note: the mirror effective waiting time is WAIT_NORMAL + WAIT_LASER)
mbedalvaro 44:46e25fa1669b 151 } else { // if we got here, it means the mirrors are well positionned: activate laser:
mbedalvaro 44:46e25fa1669b 152 if ((waitLaser==0)&&(currentPoint>numOverlapPoints)) { // change laser output the first time:
mbedalvaro 44:46e25fa1669b 153 #ifndef debugDelayMirrors
mbedalvaro 44:46e25fa1669b 154 IO.setRGBPower(currentColor|0x04); // Note: the "RED" here also affects the lockin laser (now red, in the future IR).
mbedalvaro 44:46e25fa1669b 155 #else // TEST MODE for delay using blue laser:
mbedalvaro 44:46e25fa1669b 156 // 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
mbedalvaro 46:90516893793a 157 // compute delayedPoint, but exactly as the reverse of the calculation made in the classLaserSensingTrajectory...
mbedalvaro 44:46e25fa1669b 158 // (a) "raw":
mbedalvaro 44:46e25fa1669b 159 //uint8_t delayedPoint=(currentPoint+currentTotalPoints-ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.delayMirrorSamples)%currentTotalPoints;
mbedalvaro 44:46e25fa1669b 160 // (b) corrected delay:
mbedalvaro 44:46e25fa1669b 161 uint8_t delayedPoint=currentPoint;
mbedalvaro 44:46e25fa1669b 162 if ( ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[delayedPoint].lightZone<0) { // note: we use PREVIOUS sensing - so as not to wait again for
mbedalvaro 44:46e25fa1669b 163 //IO.setRGBPower((currentColor&0x02)|0x04); // RED always on, BLUE OFF (and green whatever it was)
mbedalvaro 44:46e25fa1669b 164 // Note: better not use complicated calls?
mbedalvaro 44:46e25fa1669b 165 IO.setBluePower(0);
mbedalvaro 44:46e25fa1669b 166 IO.setGreenPower(currentColor&0x02);
mbedalvaro 44:46e25fa1669b 167 } else {
mbedalvaro 44:46e25fa1669b 168 //IO.setRGBPower((currentColor|0x01)|0x04); // RED always ON, BLUE ON (and green whatever it was)
mbedalvaro 44:46e25fa1669b 169 IO.setBluePower(1);
mbedalvaro 44:46e25fa1669b 170 IO.setGreenPower(currentColor&0x02);
mbedalvaro 44:46e25fa1669b 171 }
mbedalvaro 44:46e25fa1669b 172 #endif
mbedalvaro 44:46e25fa1669b 173 }
mbedalvaro 44:46e25fa1669b 174 if (waitLaser<WAIT_LASER) {
mbedalvaro 44:46e25fa1669b 175 waitLaser++; // increment wait laser counter
mbedalvaro 44:46e25fa1669b 176 } else { // If we got here, it means that mirrors and laser power are both properly set:
mbedalvaro 44:46e25fa1669b 177 // Read the intensity and move to the next point:
mbedalvaro 44:46e25fa1669b 178
mbedalvaro 44:46e25fa1669b 179 uint8_t currentPointWrap=currentPoint%currentTotalPoints;
mbedalvaro 44:46e25fa1669b 180 ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].intensity=(unsigned char)(255.0*IO.lockInCorrectedValue(x,y));
mbedalvaro 44:46e25fa1669b 181 ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPointWrap].intensity=(unsigned char)(255.0*IO.lockInCorrectedValue(x,y));
mbedalvaro 44:46e25fa1669b 182
mbedalvaro 44:46e25fa1669b 183 // Move to next point:
mbedalvaro 44:46e25fa1669b 184 currentPoint++;
mbedalvaro 44:46e25fa1669b 185
mbedalvaro 44:46e25fa1669b 186 waitNormal=0;
mbedalvaro 44:46e25fa1669b 187 waitLaser=0;
mbedalvaro 44:46e25fa1669b 188
mbedalvaro 44:46e25fa1669b 189 // 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)
mbedalvaro 44:46e25fa1669b 190 pointDisplayCounter++;
mbedalvaro 44:46e25fa1669b 191 }
mbedalvaro 44:46e25fa1669b 192 }
mbedalvaro 44:46e25fa1669b 193
mbedalvaro 30:d8af03f01cd4 194 currentColor=ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.displayColor;
mbedalvaro 30:d8af03f01cd4 195 }
mbedalvaro 30:d8af03f01cd4 196 }
mbedalvaro 30:d8af03f01cd4 197 break;
mbedalvaro 30:d8af03f01cd4 198 case LAST_POINT:
mbedalvaro 30:d8af03f01cd4 199 // 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
mbedalvaro 30:d8af03f01cd4 200 // we want a faster display, in which case we will need to adjust the mirrorDelay variable to something different from 0)
mbedalvaro 30:d8af03f01cd4 201 if (waitLast<WAIT_LAST) waitLast++;
mbedalvaro 30:d8af03f01cd4 202 else {
mbedalvaro 30:d8af03f01cd4 203 // switch off laser (NOTE: there is no need to wait for switch off time)
mbedalvaro 30:d8af03f01cd4 204 #ifdef RED_BLANKING
mbedalvaro 30:d8af03f01cd4 205 IO.setRGBPower(0x00);
mbedalvaro 30:d8af03f01cd4 206 #else
mbedalvaro 30:d8af03f01cd4 207 IO.setRGBPower(0x04); // Note: RED always on, or really 0
mbedalvaro 30:d8af03f01cd4 208 #endif
mbedalvaro 30:d8af03f01cd4 209 waitLast=0;
mbedalvaro 30:d8af03f01cd4 210 stateLsd=MOVE_NEXT_BLOB;
mbedalvaro 30:d8af03f01cd4 211 }
mbedalvaro 30:d8af03f01cd4 212 break;
mbedalvaro 30:d8af03f01cd4 213 case MOVE_NEXT_BLOB:
mbedalvaro 30:d8af03f01cd4 214 // TO DO: line and counter to avoid overshoot?
mbedalvaro 30:d8af03f01cd4 215
mbedalvaro 30:d8af03f01cd4 216 // Start processing next blob:
mbedalvaro 30:d8af03f01cd4 217 currentBlob=(currentBlob+1)%totalBlobs;
mbedalvaro 30:d8af03f01cd4 218
mbedalvaro 30:d8af03f01cd4 219 // currentMirrorDelay=ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.delayMirrorSamples; // per blob delay!
mbedalvaro 30:d8af03f01cd4 220 currentTotalPoints=ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory.size();
mbedalvaro 30:d8af03f01cd4 221 currentColor=ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.displayColor;
mbedalvaro 30:d8af03f01cd4 222 currentPoint=0;
mbedalvaro 30:d8af03f01cd4 223
mbedalvaro 30:d8af03f01cd4 224 if (totalBlobs>1) stateLsd=START_POINT;
mbedalvaro 30:d8af03f01cd4 225 else stateLsd=NORMAL_POINT; // in this case, we can skip the waiting for the last point (and first point too)
mbedalvaro 30:d8af03f01cd4 226
mbedalvaro 30:d8af03f01cd4 227 break;
mbedalvaro 30:d8af03f01cd4 228
mbedalvaro 30:d8af03f01cd4 229 case START_POINT:
mbedalvaro 30:d8af03f01cd4 230 if (waitFirst==0) {
mbedalvaro 30:d8af03f01cd4 231 // Send command to position the mirrors on the first point of NEXT blob (laser is flying in between during this time... )
mbedalvaro 30:d8af03f01cd4 232 x= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[0].x;
mbedalvaro 30:d8af03f01cd4 233 y= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[0].y;
mbedalvaro 30:d8af03f01cd4 234 IO.writeOutX(x);
mbedalvaro 30:d8af03f01cd4 235 IO.writeOutY(y);
mbedalvaro 30:d8af03f01cd4 236 }
mbedalvaro 30:d8af03f01cd4 237 if (waitFirst<WAIT_FIRST) waitFirst++; // time for positioning of mirrors on next blob.
mbedalvaro 30:d8af03f01cd4 238 else { //mirrors are positioned: activate laser and lock in (needs time):
mbedalvaro 30:d8af03f01cd4 239 if (waitFirstLaser==0) {
mbedalvaro 30:d8af03f01cd4 240 // activate laser - important in particular for giving time to the Lock-in to catch signal, then laser rouge:
mbedalvaro 30:d8af03f01cd4 241 IO.setRGBPower(currentColor|0x04); // Note: RED always on...
mbedalvaro 30:d8af03f01cd4 242 }
mbedalvaro 30:d8af03f01cd4 243 if (waitFirstLaser<WAIT_FIRST_LASER) waitFirstLaser++;
mbedalvaro 30:d8af03f01cd4 244 else {
mbedalvaro 30:d8af03f01cd4 245 waitFirst=0;
mbedalvaro 30:d8af03f01cd4 246 waitFirstLaser=0;
mbedalvaro 30:d8af03f01cd4 247 stateLsd=NORMAL_POINT; // start normal point
mbedalvaro 30:d8af03f01cd4 248 }
mbedalvaro 30:d8af03f01cd4 249 }
mbedalvaro 30:d8af03f01cd4 250 break;
mbedalvaro 30:d8af03f01cd4 251 }
mbedalvaro 30:d8af03f01cd4 252 }
mbedalvaro 30:d8af03f01cd4 253
mbedalvaro 30:d8af03f01cd4 254 void simpleLaserSensingRenderer::laserRenderThreadONEBLOBONLY() {
mbedalvaro 30:d8af03f01cd4 255 // When we arrive here, we ASSUME the mirrors are well positioned at the currentPoint-1, so we need to process the currentPoint:
mbedalvaro 30:d8af03f01cd4 256
mbedalvaro 30:d8af03f01cd4 257 // Current mirror position:
mbedalvaro 30:d8af03f01cd4 258 x= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPoint].x;
mbedalvaro 30:d8af03f01cd4 259 y= ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPoint].y;
mbedalvaro 30:d8af03f01cd4 260
mbedalvaro 30:d8af03f01cd4 261 // (2) Send command to position the mirrors to the next position:
mbedalvaro 30:d8af03f01cd4 262 IO.writeOutX(x);
mbedalvaro 30:d8af03f01cd4 263 IO.writeOutY(y);
mbedalvaro 30:d8af03f01cd4 264
mbedalvaro 30:d8af03f01cd4 265 // int delayedPoint=(currentPoint+currentMirrorDelay)%currentTotalPoints;
mbedalvaro 30:d8af03f01cd4 266
mbedalvaro 30:d8af03f01cd4 267 #ifdef debugDelayMirrors
mbedalvaro 30:d8af03f01cd4 268 if ( ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPoint].lightZone<0) {
mbedalvaro 30:d8af03f01cd4 269 IO.setBluePower(0);
mbedalvaro 30:d8af03f01cd4 270 // myled3=0;
mbedalvaro 30:d8af03f01cd4 271 } else {
mbedalvaro 30:d8af03f01cd4 272 IO.setBluePower(1);
mbedalvaro 30:d8af03f01cd4 273 // myled3=1;
mbedalvaro 30:d8af03f01cd4 274 }
mbedalvaro 30:d8af03f01cd4 275 //IO.setRGBPower(0x04); else IO.setRGBPower(0x07);
mbedalvaro 30:d8af03f01cd4 276 #endif
mbedalvaro 30:d8af03f01cd4 277
mbedalvaro 30:d8af03f01cd4 278 // (1) SENSING (on the current blob and particle index with mirror delay: )
mbedalvaro 30:d8af03f01cd4 279 ptBlobCfToRender->blobArray[currentBlob]->displaySensingBuffer.lsdTrajectory[currentPoint].intensity=(unsigned char)(255.0*IO.lockInCorrectedValue(x,y));
mbedalvaro 30:d8af03f01cd4 280 //=lockin.getMedianValue(); //lockin.getLastValue();//
mbedalvaro 30:d8af03f01cd4 281
mbedalvaro 30:d8af03f01cd4 282 // increment the current point index:
mbedalvaro 30:d8af03f01cd4 283 currentPoint=(currentPoint+1)%currentTotalPoints;
mbedalvaro 30:d8af03f01cd4 284
mbedalvaro 30:d8af03f01cd4 285 }