Alvaro Cassinelli / Mbed 2 deprecated skinGames_II

save loops

Dependencies:   mbed

simpleLaserRenderer.cpp@1:3be7b7d050f4, 2014-12-02 (annotated)

Committer:
mbedalvaro
Date:
Tue Dec 02 08:29:59 2014 +0000
Revision:
1:3be7b7d050f4
Parent:
0:df6fdd9b99f0 
updated

Who changed what in which revision?

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