![](/media/cache/profiles/68a15b5278e4f7c4c056df9d5f1d3b1f.jpg.50x50_q85.jpg)
save loops
simpleLaserRenderer.cpp@0:df6fdd9b99f0, 2014-12-02 (annotated)
- Committer:
- mbedalvaro
- Date:
- Tue Dec 02 04:39:15 2014 +0000
- Revision:
- 0:df6fdd9b99f0
this new version of skinGames will have a function that stops scanning as the laser reaches the same position (i.e., loops) with a certain precision. It is for extracting contours for Takashita project. It can then save on a file or send on osc
Who changed what in which revision?
User | Revision | Line number | New 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 | } |