Alvaro Cassinelli
/
skinGames_forktest
just a test
Fork of scoreLight_Advanced by
rigidLoop.cpp@4:f9d364f10335, 2012-04-04 (annotated)
- Committer:
- mbedalvaro
- Date:
- Wed Apr 04 10:05:25 2012 +0000
- Revision:
- 4:f9d364f10335
- Parent:
- 3:b44ff6de81bd
- Child:
- 5:73cd58b58f95
- the new optimized laser output buffer has not been tested, but it compiles; - I am commiting here, because I am planning to change the structure of the classes: soundSpot will not contain an object of type classLaserSensingTrajectory, but be a CHILD o...
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mbedalvaro | 1:a4050fee11f7 | 1 | #include "rigidLoop.h" |
mbedalvaro | 1:a4050fee11f7 | 2 | |
mbedalvaro | 1:a4050fee11f7 | 3 | // SHOULD NOT BE HERE: (only because I am using AD_MIRRIOR... max and min in the set region function that should not be here) |
mbedalvaro | 1:a4050fee11f7 | 4 | #include "hardwareIO.h" |
mbedalvaro | 1:a4050fee11f7 | 5 | |
mbedalvaro | 1:a4050fee11f7 | 6 | rigidLoop::rigidLoop() { |
mbedalvaro | 1:a4050fee11f7 | 7 | } |
mbedalvaro | 1:a4050fee11f7 | 8 | |
mbedalvaro | 1:a4050fee11f7 | 9 | rigidLoop::~rigidLoop() { |
mbedalvaro | 1:a4050fee11f7 | 10 | |
mbedalvaro | 1:a4050fee11f7 | 11 | } |
mbedalvaro | 1:a4050fee11f7 | 12 | |
mbedalvaro | 1:a4050fee11f7 | 13 | |
mbedalvaro | 1:a4050fee11f7 | 14 | // Note: this method is hidding the abstract method in the base class... and has DIFFERENT parameters than another child would have (enum type). |
mbedalvaro | 4:f9d364f10335 | 15 | void rigidLoop::createBlob(int _id, RigidLoopMode _elasticBlobMode, vector2D _initPos, vector2D _initSpeed) { |
mbedalvaro | 1:a4050fee11f7 | 16 | // (1) set ID: |
mbedalvaro | 1:a4050fee11f7 | 17 | identifier=_id; |
mbedalvaro | 1:a4050fee11f7 | 18 | |
mbedalvaro | 1:a4050fee11f7 | 19 | updateMode=_elasticBlobMode; |
mbedalvaro | 4:f9d364f10335 | 20 | |
mbedalvaro | 4:f9d364f10335 | 21 | startCenter=_initPos; |
mbedalvaro | 4:f9d364f10335 | 22 | startSpeed=_initSpeed; |
mbedalvaro | 4:f9d364f10335 | 23 | |
mbedalvaro | 1:a4050fee11f7 | 24 | // (2) Initialize common variables of all blobs (base class): |
mbedalvaro | 1:a4050fee11f7 | 25 | initCommonVariables(); |
mbedalvaro | 1:a4050fee11f7 | 26 | |
mbedalvaro | 1:a4050fee11f7 | 27 | // (3) initialize common variables for the different modes of this rigid loop (even if some are not used, better not to have more subclasses...) |
mbedalvaro | 4:f9d364f10335 | 28 | |
mbedalvaro | 2:34157ebbf56b | 29 | angleCorrectionForceLoop=0; // this will depend on the size of the blob (in principle), and number of blobs... |
mbedalvaro | 3:b44ff6de81bd | 30 | |
mbedalvaro | 3:b44ff6de81bd | 31 | |
mbedalvaro | 1:a4050fee11f7 | 32 | // (3) Initialize secondary variables depending on the behaviour mode (may be changed afterwards in real time) |
mbedalvaro | 1:a4050fee11f7 | 33 | |
mbedalvaro | 1:a4050fee11f7 | 34 | switch (updateMode) { |
mbedalvaro | 4:f9d364f10335 | 35 | |
mbedalvaro | 4:f9d364f10335 | 36 | case SPOT_TEST: |
mbedalvaro | 4:f9d364f10335 | 37 | // Name of this kind of spot: |
mbedalvaro | 4:f9d364f10335 | 38 | sprintf(spotName,"spot_test"); |
mbedalvaro | 4:f9d364f10335 | 39 | |
mbedalvaro | 4:f9d364f10335 | 40 | //setColor(0x07);//0x04+0x02>>i); |
mbedalvaro | 4:f9d364f10335 | 41 | setColor(0x04); |
mbedalvaro | 4:f9d364f10335 | 42 | |
mbedalvaro | 4:f9d364f10335 | 43 | saccadeRadius=250; |
mbedalvaro | 4:f9d364f10335 | 44 | |
mbedalvaro | 4:f9d364f10335 | 45 | // default (initial) shape (the scafold belongs to the base class): |
mbedalvaro | 4:f9d364f10335 | 46 | bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 10); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints); |
mbedalvaro | 4:f9d364f10335 | 47 | |
mbedalvaro | 4:f9d364f10335 | 48 | // Note: We may assume NO MASS for the center of the contour following loop. Adding mass may be interesting though (smooth motion). |
mbedalvaro | 4:f9d364f10335 | 49 | massCenter=0.01; |
mbedalvaro | 4:f9d364f10335 | 50 | dampMotionCenterMass=0.001; |
mbedalvaro | 4:f9d364f10335 | 51 | |
mbedalvaro | 4:f9d364f10335 | 52 | // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things): |
mbedalvaro | 4:f9d364f10335 | 53 | createLoopFromScafold(); |
mbedalvaro | 4:f9d364f10335 | 54 | |
mbedalvaro | 4:f9d364f10335 | 55 | // Excursion limits (this will set the limits of motion for the rigid loop, which is given by it's central position, so we have to correct by the radius). |
mbedalvaro | 4:f9d364f10335 | 56 | setRegionMotion(MIN_AD_MIRRORS+saccadeRadius, MIN_AD_MIRRORS+saccadeRadius, MAX_AD_MIRRORS-saccadeRadius, MAX_AD_MIRRORS-saccadeRadius); |
mbedalvaro | 4:f9d364f10335 | 57 | |
mbedalvaro | 4:f9d364f10335 | 58 | // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0. But in case of unique blobs, it may be interesting to accelerate display |
mbedalvaro | 4:f9d364f10335 | 59 | // AND correct the delay by software): |
mbedalvaro | 4:f9d364f10335 | 60 | displaySensingBuffer.setDelayMirrors(0); |
mbedalvaro | 4:f9d364f10335 | 61 | |
mbedalvaro | 4:f9d364f10335 | 62 | break; |
mbedalvaro | 4:f9d364f10335 | 63 | |
mbedalvaro | 1:a4050fee11f7 | 64 | case SPOT_FOLLOWING: |
mbedalvaro | 1:a4050fee11f7 | 65 | |
mbedalvaro | 4:f9d364f10335 | 66 | |
mbedalvaro | 1:a4050fee11f7 | 67 | // Name of this kind of spot: |
mbedalvaro | 1:a4050fee11f7 | 68 | sprintf(spotName,"rigid_following"); |
mbedalvaro | 1:a4050fee11f7 | 69 | |
mbedalvaro | 3:b44ff6de81bd | 70 | //setColor(0x07);//0x04+0x02>>i); |
mbedalvaro | 3:b44ff6de81bd | 71 | setColor(0x04); |
mbedalvaro | 1:a4050fee11f7 | 72 | |
mbedalvaro | 1:a4050fee11f7 | 73 | // default (initial) shape (the scafold belongs to the base class): |
mbedalvaro | 4:f9d364f10335 | 74 | bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 10); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints); |
mbedalvaro | 1:a4050fee11f7 | 75 | |
mbedalvaro | 1:a4050fee11f7 | 76 | // Note: We may assume NO MASS for the center of the contour following loop. Adding mass may be interesting though (smooth motion). |
mbedalvaro | 1:a4050fee11f7 | 77 | massCenter=0.01; |
mbedalvaro | 1:a4050fee11f7 | 78 | dampMotionCenterMass=0.001; |
mbedalvaro | 1:a4050fee11f7 | 79 | |
mbedalvaro | 4:f9d364f10335 | 80 | // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things): |
mbedalvaro | 4:f9d364f10335 | 81 | createLoopFromScafold(); |
mbedalvaro | 4:f9d364f10335 | 82 | |
mbedalvaro | 4:f9d364f10335 | 83 | // Excursion limits (this will set the limits of motion for the rigid loop, which is given by it's central position, so we have to correct by the radius). |
mbedalvaro | 4:f9d364f10335 | 84 | setRegionMotion(MIN_AD_MIRRORS+saccadeRadius, MIN_AD_MIRRORS+saccadeRadius, MAX_AD_MIRRORS-saccadeRadius, MAX_AD_MIRRORS-saccadeRadius); |
mbedalvaro | 4:f9d364f10335 | 85 | |
mbedalvaro | 4:f9d364f10335 | 86 | // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0. But in case of unique blobs, it may be interesting to accelerate display |
mbedalvaro | 4:f9d364f10335 | 87 | // AND correct the delay by software): |
mbedalvaro | 4:f9d364f10335 | 88 | displaySensingBuffer.setDelayMirrors(4); |
mbedalvaro | 4:f9d364f10335 | 89 | |
mbedalvaro | 1:a4050fee11f7 | 90 | break; |
mbedalvaro | 1:a4050fee11f7 | 91 | |
mbedalvaro | 1:a4050fee11f7 | 92 | case SPOT_BOUNCING: |
mbedalvaro | 1:a4050fee11f7 | 93 | // Name of this kind of spot: |
mbedalvaro | 1:a4050fee11f7 | 94 | sprintf(spotName,"rigid_bouncing"); |
mbedalvaro | 1:a4050fee11f7 | 95 | |
mbedalvaro | 3:b44ff6de81bd | 96 | //setColor(0x07);//0x04+0x02>>i); |
mbedalvaro | 3:b44ff6de81bd | 97 | setColor(0x04); |
mbedalvaro | 1:a4050fee11f7 | 98 | |
mbedalvaro | 4:f9d364f10335 | 99 | saccadeRadius=250; |
mbedalvaro | 1:a4050fee11f7 | 100 | // default (initial) shape (the scafold belongs to the base class): |
mbedalvaro | 4:f9d364f10335 | 101 | bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 40); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints); |
mbedalvaro | 1:a4050fee11f7 | 102 | |
mbedalvaro | 1:a4050fee11f7 | 103 | // Numeric parameters for the simulated mechanical system: |
mbedalvaro | 1:a4050fee11f7 | 104 | massCenter=0.1; |
mbedalvaro | 2:34157ebbf56b | 105 | dampMotionCenterMass=0.00015;//00003; |
mbedalvaro | 1:a4050fee11f7 | 106 | factorBouncingForce=0.0001; // this is because we will use a force on the central mass |
mbedalvaro | 1:a4050fee11f7 | 107 | |
mbedalvaro | 2:34157ebbf56b | 108 | slidingDirection=true; // For contour following (will change direction when touching wall) |
mbedalvaro | 2:34157ebbf56b | 109 | speedContourFollowing=0.0003; |
mbedalvaro | 1:a4050fee11f7 | 110 | |
mbedalvaro | 4:f9d364f10335 | 111 | // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things): |
mbedalvaro | 4:f9d364f10335 | 112 | createLoopFromScafold(); |
mbedalvaro | 4:f9d364f10335 | 113 | |
mbedalvaro | 4:f9d364f10335 | 114 | // Excursion limits (this will set the limits of motion for the rigid loop, which is given by it's central position, so we have to correct by the radius). |
mbedalvaro | 4:f9d364f10335 | 115 | setRegionMotion(MIN_AD_MIRRORS+saccadeRadius, MIN_AD_MIRRORS+saccadeRadius, MAX_AD_MIRRORS-saccadeRadius, MAX_AD_MIRRORS-saccadeRadius); |
mbedalvaro | 4:f9d364f10335 | 116 | |
mbedalvaro | 4:f9d364f10335 | 117 | // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0. But in case of unique blobs, it may be interesting to accelerate display |
mbedalvaro | 4:f9d364f10335 | 118 | // AND correct the delay by software): |
mbedalvaro | 4:f9d364f10335 | 119 | displaySensingBuffer.setDelayMirrors(4); |
mbedalvaro | 1:a4050fee11f7 | 120 | |
mbedalvaro | 1:a4050fee11f7 | 121 | break; |
mbedalvaro | 1:a4050fee11f7 | 122 | } |
mbedalvaro | 3:b44ff6de81bd | 123 | |
mbedalvaro | 1:a4050fee11f7 | 124 | } |
mbedalvaro | 1:a4050fee11f7 | 125 | |
mbedalvaro | 1:a4050fee11f7 | 126 | |
mbedalvaro | 1:a4050fee11f7 | 127 | void rigidLoop::initSizeBlob(int _numPoints) { |
mbedalvaro | 1:a4050fee11f7 | 128 | // Iinitialize blob size (number of points for the loop, as well as other structures such as lsdTrajectory) |
mbedalvaro | 1:a4050fee11f7 | 129 | numPoints=_numPoints; |
mbedalvaro | 1:a4050fee11f7 | 130 | |
mbedalvaro | 1:a4050fee11f7 | 131 | // Sensing and Display trajectory: |
mbedalvaro | 1:a4050fee11f7 | 132 | displaySensingBuffer.lsdTrajectory.resize(numPoints); // the lsdTrajectory and the elastic loop will have the same number of points (this could be different - decimation?). |
mbedalvaro | 1:a4050fee11f7 | 133 | } |
mbedalvaro | 1:a4050fee11f7 | 134 | |
mbedalvaro | 1:a4050fee11f7 | 135 | void rigidLoop::createLoopFromScafold(void) { |
mbedalvaro | 1:a4050fee11f7 | 136 | |
mbedalvaro | 1:a4050fee11f7 | 137 | initSizeBlob(bluePrint.scafold.size()); // very simple here (only need to set the size of the lsd buffer) |
mbedalvaro | 1:a4050fee11f7 | 138 | |
mbedalvaro | 1:a4050fee11f7 | 139 | centerMass.mass=massCenter; |
mbedalvaro | 1:a4050fee11f7 | 140 | centerMass.dampMotion = dampMotionCenterMass; |
mbedalvaro | 1:a4050fee11f7 | 141 | |
mbedalvaro | 1:a4050fee11f7 | 142 | // note: the following may not be required in case of contour following: |
mbedalvaro | 2:34157ebbf56b | 143 | centerMass.setIntegrationStep(0.23); // VERY IMPORTANT! in the case of verlet integration, we need to set dt BEFORE setting the initial speed. |
mbedalvaro | 4:f9d364f10335 | 144 | centerMass.setInitialCondition(startCenter, startSpeed); |
mbedalvaro | 1:a4050fee11f7 | 145 | // centerMass.setInitialCondition(2047.0, 2047.0,0.0,0.0); |
mbedalvaro | 1:a4050fee11f7 | 146 | } |
mbedalvaro | 1:a4050fee11f7 | 147 | |
mbedalvaro | 1:a4050fee11f7 | 148 | void rigidLoop::setRegionMotion(int mmix, int mmiy, int mmax, int mmay) { // wrapper for setWallLimits, because there is no more things to do than set this for a unique mass |
mbedalvaro | 1:a4050fee11f7 | 149 | centerMass.setWallLimits(mmix+10, mmiy+10, mmax-10, mmay-10); |
mbedalvaro | 1:a4050fee11f7 | 150 | } |
mbedalvaro | 1:a4050fee11f7 | 151 | |
mbedalvaro | 1:a4050fee11f7 | 152 | |
mbedalvaro | 1:a4050fee11f7 | 153 | void rigidLoop::update() { |
mbedalvaro | 1:a4050fee11f7 | 154 | |
mbedalvaro | 1:a4050fee11f7 | 155 | // (I) process loop geometry: not needed (rigid) |
mbedalvaro | 1:a4050fee11f7 | 156 | // Just check if the blob touched the borders (only need to do this with the central mass): |
mbedalvaro | 1:a4050fee11f7 | 157 | blobWallCollision=centerMass.bWallCollision; |
mbedalvaro | 1:a4050fee11f7 | 158 | |
mbedalvaro | 1:a4050fee11f7 | 159 | // (II) Process sensing buffer and compute light forces: |
mbedalvaro | 1:a4050fee11f7 | 160 | displaySensingBuffer.processSensedData(); // note: region with light is -1, and without is 2 (TO CHANGE!!! then we don't need to do "if" in the moment computation, but just a product) |
mbedalvaro | 1:a4050fee11f7 | 161 | |
mbedalvaro | 1:a4050fee11f7 | 162 | // (III) Compute recentering vector (the "penetration vector in fact"), using "first order moment": |
mbedalvaro | 3:b44ff6de81bd | 163 | // ATTENTION!! for this simple method (of "first order moment") to work, we have either to have numPoints very large, OR an EVEN quantity - so the |
mbedalvaro | 3:b44ff6de81bd | 164 | // sum in the circle is 0). |
mbedalvaro | 1:a4050fee11f7 | 165 | vector2D momentVector(0,0); |
mbedalvaro | 1:a4050fee11f7 | 166 | for (int i = 0; i < numPoints; i++) { |
mbedalvaro | 1:a4050fee11f7 | 167 | if (displaySensingBuffer.lsdTrajectory[i].lightZone>0) { // this is, we are in a dark zone (better to integrate there, because it is normally smaller) |
mbedalvaro | 4:f9d364f10335 | 168 | // momentVector+=bluePrint.scafold[i];// note: no need to do -centerMass.pos, because the scafold is "centered" around 0 |
mbedalvaro | 1:a4050fee11f7 | 169 | } |
mbedalvaro | 1:a4050fee11f7 | 170 | } |
mbedalvaro | 1:a4050fee11f7 | 171 | float momentNorm=momentVector.length(); |
mbedalvaro | 2:34157ebbf56b | 172 | if (momentNorm==0) { |
mbedalvaro | 3:b44ff6de81bd | 173 | recenteringVectorLoop.set(0,0); |
mbedalvaro | 3:b44ff6de81bd | 174 | normRecenteringVector=0; |
mbedalvaro | 3:b44ff6de81bd | 175 | angleRecenteringVector=0; |
mbedalvaro | 3:b44ff6de81bd | 176 | } else { |
mbedalvaro | 3:b44ff6de81bd | 177 | float aux=saccadeRadius/momentNorm; |
mbedalvaro | 3:b44ff6de81bd | 178 | if (aux>0.5) recenteringVectorLoop=momentVector*sqrt(aux*aux-0.25); |
mbedalvaro | 3:b44ff6de81bd | 179 | else recenteringVectorLoop=momentVector*sqrt(0.25-aux*aux); |
mbedalvaro | 3:b44ff6de81bd | 180 | //Rotate by angleCorrection (to correct delays, etc): |
mbedalvaro | 3:b44ff6de81bd | 181 | recenteringVectorLoop.rotateDeg(angleCorrectionForceLoop); |
mbedalvaro | 3:b44ff6de81bd | 182 | // Compute redundant quantities (if necessary, for sending through OSC, etc): |
mbedalvaro | 3:b44ff6de81bd | 183 | normRecenteringVector=recenteringVectorLoop.length(); |
mbedalvaro | 3:b44ff6de81bd | 184 | angleRecenteringVector=recenteringVectorLoop.angleDegHoriz(); |
mbedalvaro | 3:b44ff6de81bd | 185 | } |
mbedalvaro | 3:b44ff6de81bd | 186 | |
mbedalvaro | 1:a4050fee11f7 | 187 | |
mbedalvaro | 1:a4050fee11f7 | 188 | // Now, depending on the mode of operation, we have two types of behaviour: |
mbedalvaro | 1:a4050fee11f7 | 189 | vector2D slidingVector; //( need to declare it here because a switch "jump" cannot bypass an initialization) |
mbedalvaro | 1:a4050fee11f7 | 190 | switch (updateMode) { |
mbedalvaro | 4:f9d364f10335 | 191 | |
mbedalvaro | 4:f9d364f10335 | 192 | case SPOT_TEST: // this is just for adjusting mirror delays, checking recentering vector, etc: |
mbedalvaro | 4:f9d364f10335 | 193 | // do nothing for the time being |
mbedalvaro | 4:f9d364f10335 | 194 | // NOTE: it is not so easy to show the recentering vector without affecting the mirror delay BECAUSE I AM USING THE INTERRUPT METHOD for display. |
mbedalvaro | 4:f9d364f10335 | 195 | // A possible solution is to instantiate ANOTHER blob with a shape just equal to a line, and rotate it using data from the this blob. Make a new class? Seems a good idea. |
mbedalvaro | 4:f9d364f10335 | 196 | |
mbedalvaro | 4:f9d364f10335 | 197 | break; |
mbedalvaro | 1:a4050fee11f7 | 198 | |
mbedalvaro | 1:a4050fee11f7 | 199 | case SPOT_FOLLOWING: |
mbedalvaro | 1:a4050fee11f7 | 200 | // we need to compute the tangencial "speed": |
mbedalvaro | 1:a4050fee11f7 | 201 | // vector2D slidingVector; |
mbedalvaro | 2:34157ebbf56b | 202 | // let's normalize the moment vector (this simplifies the calculations for the sliding vector - need some calculations, but this is the result): |
mbedalvaro | 2:34157ebbf56b | 203 | if (momentNorm>0) { |
mbedalvaro | 3:b44ff6de81bd | 204 | momentVector/=momentNorm; |
mbedalvaro | 3:b44ff6de81bd | 205 | // We can now compute the sliding vector as: |
mbedalvaro | 3:b44ff6de81bd | 206 | slidingVector=momentVector.getRotatedDeg(slidingDirection? 90 : -90) * speedContourFollowing*saccadeRadius; |
mbedalvaro | 3:b44ff6de81bd | 207 | // Then the final correcting vector is the sum of sliding plus recentering vector (with a factor if one want some smothing) |
mbedalvaro | 3:b44ff6de81bd | 208 | // This is used to update the position of the central mass - WITHOUT INTEGRATION (or with it, but for the time being, we don't do that): |
mbedalvaro | 3:b44ff6de81bd | 209 | centerMass.pos += slidingVector + recenteringVectorLoop * 0.9; |
mbedalvaro | 3:b44ff6de81bd | 210 | |
mbedalvaro | 3:b44ff6de81bd | 211 | // The following function can help constraining the position "pos", but it also does too much. Do something simpler perhaps? |
mbedalvaro | 3:b44ff6de81bd | 212 | centerMass.bounceOffWalls(); // constrain position (and compute wall "hit") |
mbedalvaro | 3:b44ff6de81bd | 213 | |
mbedalvaro | 2:34157ebbf56b | 214 | } else { |
mbedalvaro | 3:b44ff6de81bd | 215 | // not on something. Do nothing for the time being |
mbedalvaro | 2:34157ebbf56b | 216 | } |
mbedalvaro | 1:a4050fee11f7 | 217 | |
mbedalvaro | 1:a4050fee11f7 | 218 | break; |
mbedalvaro | 1:a4050fee11f7 | 219 | |
mbedalvaro | 1:a4050fee11f7 | 220 | case SPOT_BOUNCING: |
mbedalvaro | 1:a4050fee11f7 | 221 | // this is very simple: we need to give a force to the centralMass that is OPPOSITE to the recenteringVectorLoop vector |
mbedalvaro | 1:a4050fee11f7 | 222 | centerMass.resetForce(); |
mbedalvaro | 2:34157ebbf56b | 223 | centerMass.addForce(recenteringVectorLoop*factorBouncingForce); |
mbedalvaro | 1:a4050fee11f7 | 224 | |
mbedalvaro | 1:a4050fee11f7 | 225 | // update dynamics for the central mass:: |
mbedalvaro | 1:a4050fee11f7 | 226 | #ifndef VERLET_METHOD |
mbedalvaro | 1:a4050fee11f7 | 227 | centerMass.addDampingForce(); // // only in case of EULER method (damping in VERLET mode is done automatically when updating) |
mbedalvaro | 1:a4050fee11f7 | 228 | #endif |
mbedalvaro | 1:a4050fee11f7 | 229 | |
mbedalvaro | 1:a4050fee11f7 | 230 | centerMass.update(); // unconstrained |
mbedalvaro | 1:a4050fee11f7 | 231 | centerMass.bounceOffWalls(); // constrain position (and compute wall "hit") |
mbedalvaro | 1:a4050fee11f7 | 232 | |
mbedalvaro | 1:a4050fee11f7 | 233 | break; |
mbedalvaro | 1:a4050fee11f7 | 234 | |
mbedalvaro | 1:a4050fee11f7 | 235 | } |
mbedalvaro | 1:a4050fee11f7 | 236 | |
mbedalvaro | 1:a4050fee11f7 | 237 | // OTHER PARTICULAR THINGS: |
mbedalvaro | 1:a4050fee11f7 | 238 | // change sliding direction (for countour following): |
mbedalvaro | 1:a4050fee11f7 | 239 | if (blobWallCollision) { |
mbedalvaro | 1:a4050fee11f7 | 240 | if (wallCounter>10) { |
mbedalvaro | 1:a4050fee11f7 | 241 | slidingDirection=!slidingDirection; |
mbedalvaro | 1:a4050fee11f7 | 242 | wallCounter=0; |
mbedalvaro | 1:a4050fee11f7 | 243 | } |
mbedalvaro | 1:a4050fee11f7 | 244 | } |
mbedalvaro | 1:a4050fee11f7 | 245 | wallCounter++; |
mbedalvaro | 1:a4050fee11f7 | 246 | } |
mbedalvaro | 1:a4050fee11f7 | 247 | |
mbedalvaro | 1:a4050fee11f7 | 248 | |
mbedalvaro | 1:a4050fee11f7 | 249 | // Drawing the graphics - this will in fact use the graphic renderer - if any - and produce the trajectory to be displayed by the laser |
mbedalvaro | 1:a4050fee11f7 | 250 | void rigidLoop::draw() { |
mbedalvaro | 1:a4050fee11f7 | 251 | // for the time being, there is no "opengl" like renderer, so we just copy into the lsdTrajectory: |
mbedalvaro | 1:a4050fee11f7 | 252 | float cx= centerMass.pos.x; |
mbedalvaro | 1:a4050fee11f7 | 253 | float cy= centerMass.pos.y; |
mbedalvaro | 1:a4050fee11f7 | 254 | for (int i = 0; i < numPoints; i++) { |
mbedalvaro | 1:a4050fee11f7 | 255 | // The shape is drawn by translating the scafold shape (centered on centerMass): |
mbedalvaro | 1:a4050fee11f7 | 256 | displaySensingBuffer.lsdTrajectory[i].x= int(bluePrint.scafold[i].x + cx ); // note: it should be an unsigned short!! |
mbedalvaro | 1:a4050fee11f7 | 257 | displaySensingBuffer.lsdTrajectory[i].y= int(bluePrint.scafold[i].y + cy ); |
mbedalvaro | 1:a4050fee11f7 | 258 | //displaySensingBuffer.lsdTrajectory[i].color=blobColor; // perhaps per point color is not a good idea for the time being... |
mbedalvaro | 1:a4050fee11f7 | 259 | } |
mbedalvaro | 1:a4050fee11f7 | 260 | // global color for the whole loop: |
mbedalvaro | 1:a4050fee11f7 | 261 | displaySensingBuffer.displayColor=blobColor; |
mbedalvaro | 1:a4050fee11f7 | 262 | } |
mbedalvaro | 1:a4050fee11f7 | 263 | |
mbedalvaro | 1:a4050fee11f7 | 264 | void rigidLoop::computeBoundingBox() { |
mbedalvaro | 1:a4050fee11f7 | 265 | } |
mbedalvaro | 1:a4050fee11f7 | 266 | |
mbedalvaro | 1:a4050fee11f7 | 267 | |
mbedalvaro | 1:a4050fee11f7 | 268 | |
mbedalvaro | 1:a4050fee11f7 | 269 | void rigidLoop::sendDataSpecific() { |
mbedalvaro | 1:a4050fee11f7 | 270 | char auxstring[10]; |
mbedalvaro | 1:a4050fee11f7 | 271 | myled2=1; // for tests... |
mbedalvaro | 1:a4050fee11f7 | 272 | |
mbedalvaro | 1:a4050fee11f7 | 273 | // First, set the top address of the message to the ID of the blob (not the name): |
mbedalvaro | 1:a4050fee11f7 | 274 | sprintf(auxstring, "%d", identifier); |
mbedalvaro | 1:a4050fee11f7 | 275 | sendMes.setTopAddress("0");//auxstring); |
mbedalvaro | 1:a4050fee11f7 | 276 | |
mbedalvaro | 1:a4050fee11f7 | 277 | // ===================== OSC ====================== |
mbedalvaro | 1:a4050fee11f7 | 278 | if (sendOSC) { |
mbedalvaro | 1:a4050fee11f7 | 279 | |
mbedalvaro | 1:a4050fee11f7 | 280 | // (a) Anchor mass: |
mbedalvaro | 1:a4050fee11f7 | 281 | if (sendingAnchorPosition) { |
mbedalvaro | 1:a4050fee11f7 | 282 | sendMes.setSubAddress("/apos"); |
mbedalvaro | 1:a4050fee11f7 | 283 | long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) |
mbedalvaro | 1:a4050fee11f7 | 284 | x=(long)(centerMass.pos.x); |
mbedalvaro | 1:a4050fee11f7 | 285 | y=(long)(centerMass.pos.y); |
mbedalvaro | 1:a4050fee11f7 | 286 | sendMes.setArgs( "ii", &x, &y); |
mbedalvaro | 1:a4050fee11f7 | 287 | osc.sendOsc( &sendMes ); |
mbedalvaro | 1:a4050fee11f7 | 288 | } |
mbedalvaro | 1:a4050fee11f7 | 289 | |
mbedalvaro | 1:a4050fee11f7 | 290 | // (b) data from blob points (this is ONLY FOR TESTS, because the loop is rigid - sending the center is enough) |
mbedalvaro | 1:a4050fee11f7 | 291 | if (sendingLoopPositions) { |
mbedalvaro | 1:a4050fee11f7 | 292 | #ifdef SEND_AS_POINTS |
mbedalvaro | 1:a4050fee11f7 | 293 | long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) |
mbedalvaro | 1:a4050fee11f7 | 294 | float cx= centerMass.pos.x; |
mbedalvaro | 1:a4050fee11f7 | 295 | float cy= centerMass.pos.y; |
mbedalvaro | 1:a4050fee11f7 | 296 | for (int i = 0; i < numPoints; i++) { |
mbedalvaro | 2:34157ebbf56b | 297 | sprintf(auxstring, "/p%d",identifier*10+ i);//20+ i+(identifier-1)*10); // auxstring read as "/p1", "/p2", ... |
mbedalvaro | 1:a4050fee11f7 | 298 | sendMes.setSubAddress(auxstring); // ATTENTION: the host computer needs to know in advance how many points are in the loop (I did not implement "bundle" messages yet...) |
mbedalvaro | 1:a4050fee11f7 | 299 | x=(long)(bluePrint.scafold[i].x + cx); |
mbedalvaro | 1:a4050fee11f7 | 300 | y=(long)(bluePrint.scafold[i].y + cy); |
mbedalvaro | 1:a4050fee11f7 | 301 | sendMes.setArgs( "ii", &x, &y); |
mbedalvaro | 1:a4050fee11f7 | 302 | osc.sendOsc( &sendMes ); |
mbedalvaro | 1:a4050fee11f7 | 303 | } |
mbedalvaro | 1:a4050fee11f7 | 304 | |
mbedalvaro | 1:a4050fee11f7 | 305 | #endif |
mbedalvaro | 1:a4050fee11f7 | 306 | #ifdef SEND_AS_BLOB |
mbedalvaro | 1:a4050fee11f7 | 307 | sendMes.clearArgs(); // no need, we won't use osc.sendOsc()... |
mbedalvaro | 1:a4050fee11f7 | 308 | uint8_t blobdata[4*numPoints]; // 2 bytes per coordinate, and 2 coordinates |
mbedalvaro | 1:a4050fee11f7 | 309 | float cx= centerMass.pos.x; |
mbedalvaro | 1:a4050fee11f7 | 310 | float cy= centerMass.pos.y; |
mbedalvaro | 1:a4050fee11f7 | 311 | for (int i = 0; i < numPoints; i++ ) { |
mbedalvaro | 1:a4050fee11f7 | 312 | // note: massesLoop[i].pos.x is a "float" |
mbedalvaro | 1:a4050fee11f7 | 313 | uint16_t x=(uint16_t)(bluePrint.scafold[i].x + cx); |
mbedalvaro | 1:a4050fee11f7 | 314 | blobdata[4*i]=(uint8_t)x>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE) |
mbedalvaro | 1:a4050fee11f7 | 315 | blobdata[4*i+1]=(uint8_t)x; |
mbedalvaro | 1:a4050fee11f7 | 316 | |
mbedalvaro | 1:a4050fee11f7 | 317 | uint16_t y=(uint16_t)(bluePrint.scafold[i].y + cy); |
mbedalvaro | 1:a4050fee11f7 | 318 | blobdata[4*i+2]=(uint8_t)y>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE) |
mbedalvaro | 1:a4050fee11f7 | 319 | blobdata[4*i+3]=(uint8_t)y; |
mbedalvaro | 1:a4050fee11f7 | 320 | } |
mbedalvaro | 1:a4050fee11f7 | 321 | osc.sendOscBlob(&(blobdata[0]), 4*numPoints, &sendMes ); // second parameter is osc blob size in bytes |
mbedalvaro | 1:a4050fee11f7 | 322 | #endif |
mbedalvaro | 1:a4050fee11f7 | 323 | #ifdef SEND_AS_STRING |
mbedalvaro | 1:a4050fee11f7 | 324 | sendMes.clearArgs(); // no need, we won't use osc.sendOsc()... |
mbedalvaro | 1:a4050fee11f7 | 325 | uint8_t blobdata[4*numPoints]; // 2 bytes per coordinate, and 2 coordinates |
mbedalvaro | 1:a4050fee11f7 | 326 | float cx= centerMass.pos.x; |
mbedalvaro | 1:a4050fee11f7 | 327 | float cy= centerMass.pos.y; |
mbedalvaro | 1:a4050fee11f7 | 328 | for (int i = 0; i < numPoints; i++ ) { |
mbedalvaro | 1:a4050fee11f7 | 329 | // note: massesLoop[i].pos.x is a "float" |
mbedalvaro | 1:a4050fee11f7 | 330 | uint16_t x=(uint16_t)(bluePrint.scafold[i].x + cx ); |
mbedalvaro | 1:a4050fee11f7 | 331 | blobdata[4*i]=(uint8_t)x>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE) |
mbedalvaro | 1:a4050fee11f7 | 332 | blobdata[4*i+1]=(uint8_t)x; |
mbedalvaro | 1:a4050fee11f7 | 333 | |
mbedalvaro | 1:a4050fee11f7 | 334 | uint16_t y=(uint16_t)(bluePrint.scafold[i].y + cy); |
mbedalvaro | 1:a4050fee11f7 | 335 | blobdata[4*i+2]=(uint8_t)y>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE) |
mbedalvaro | 1:a4050fee11f7 | 336 | blobdata[4*i+3]=(uint8_t)y; |
mbedalvaro | 1:a4050fee11f7 | 337 | } |
mbedalvaro | 1:a4050fee11f7 | 338 | osc.sendOscString(blobdata, 4*numPoints, &sendMes ); // second parameter is osc blob size in bytes |
mbedalvaro | 1:a4050fee11f7 | 339 | #endif |
mbedalvaro | 1:a4050fee11f7 | 340 | } |
mbedalvaro | 1:a4050fee11f7 | 341 | if (sendingLoopRegions) { |
mbedalvaro | 1:a4050fee11f7 | 342 | long x; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) |
mbedalvaro | 1:a4050fee11f7 | 343 | for (int i = 0; i < numPoints; i++) { |
mbedalvaro | 1:a4050fee11f7 | 344 | sprintf(auxstring, "/r%d", i); // auxstring read as "/f1", "/f2", ... |
mbedalvaro | 1:a4050fee11f7 | 345 | sendMes.setSubAddress(auxstring); // ATTENTION: the host computer needs to know in advance how many points are in the loop (I did not implement "bundle" messages yet...) |
mbedalvaro | 1:a4050fee11f7 | 346 | x=(long)(displaySensingBuffer.lsdTrajectory[i].lightZone>0? 1 : 0); |
mbedalvaro | 1:a4050fee11f7 | 347 | sendMes.setArgs( "i", &x); |
mbedalvaro | 1:a4050fee11f7 | 348 | osc.sendOsc( &sendMes ); |
mbedalvaro | 1:a4050fee11f7 | 349 | } |
mbedalvaro | 1:a4050fee11f7 | 350 | } |
mbedalvaro | 1:a4050fee11f7 | 351 | if (sendingLoopTouchWall) { // global touch wall for the loop (not per point) |
mbedalvaro | 1:a4050fee11f7 | 352 | long wall; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) |
mbedalvaro | 1:a4050fee11f7 | 353 | sprintf(auxstring, "/bWall"); |
mbedalvaro | 1:a4050fee11f7 | 354 | sendMes.setSubAddress(auxstring); |
mbedalvaro | 1:a4050fee11f7 | 355 | wall=(long)(blobWallCollision? 1 : 0); |
mbedalvaro | 1:a4050fee11f7 | 356 | sendMes.setArgs( "i", &wall); |
mbedalvaro | 1:a4050fee11f7 | 357 | osc.sendOsc( &sendMes ); |
mbedalvaro | 1:a4050fee11f7 | 358 | } |
mbedalvaro | 1:a4050fee11f7 | 359 | |
mbedalvaro | 1:a4050fee11f7 | 360 | // (d) Light sensing statistics: |
mbedalvaro | 1:a4050fee11f7 | 361 | if (sendingBlobMaxMin) { |
mbedalvaro | 1:a4050fee11f7 | 362 | sendMes.setSubAddress("/maxmin"); |
mbedalvaro | 1:a4050fee11f7 | 363 | long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) |
mbedalvaro | 1:a4050fee11f7 | 364 | x=(long)(displaySensingBuffer.maxI); |
mbedalvaro | 1:a4050fee11f7 | 365 | y=(long)(displaySensingBuffer.minI); |
mbedalvaro | 1:a4050fee11f7 | 366 | sendMes.setArgs( "ii", &x, &y); |
mbedalvaro | 1:a4050fee11f7 | 367 | osc.sendOsc( &sendMes ); |
mbedalvaro | 1:a4050fee11f7 | 368 | } |
mbedalvaro | 1:a4050fee11f7 | 369 | |
mbedalvaro | 1:a4050fee11f7 | 370 | // (e) Recentering vector: (note: redundant with sendingLightForce, IF the correction angle is known). |
mbedalvaro | 1:a4050fee11f7 | 371 | if (sendingRecenteringVector) { |
mbedalvaro | 1:a4050fee11f7 | 372 | sendMes.setSubAddress("/rvector"); |
mbedalvaro | 1:a4050fee11f7 | 373 | long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) |
mbedalvaro | 1:a4050fee11f7 | 374 | x=(long)(recenteringVectorLoop.x); |
mbedalvaro | 1:a4050fee11f7 | 375 | y=(long)(recenteringVectorLoop.y); |
mbedalvaro | 1:a4050fee11f7 | 376 | sendMes.setArgs( "ii", &x, &y); |
mbedalvaro | 1:a4050fee11f7 | 377 | osc.sendOsc( &sendMes ); |
mbedalvaro | 1:a4050fee11f7 | 378 | } |
mbedalvaro | 1:a4050fee11f7 | 379 | if (sendingRecenteringAngle) { |
mbedalvaro | 1:a4050fee11f7 | 380 | sendMes.setSubAddress("/rangle"); |
mbedalvaro | 1:a4050fee11f7 | 381 | long x; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) |
mbedalvaro | 1:a4050fee11f7 | 382 | x=(long)(angleRecenteringVector); |
mbedalvaro | 1:a4050fee11f7 | 383 | sendMes.setArgs( "i", &x); |
mbedalvaro | 1:a4050fee11f7 | 384 | osc.sendOsc( &sendMes ); |
mbedalvaro | 1:a4050fee11f7 | 385 | } |
mbedalvaro | 1:a4050fee11f7 | 386 | if (sendingRecenteringNorm) { |
mbedalvaro | 1:a4050fee11f7 | 387 | sendMes.setSubAddress("/rnorm"); |
mbedalvaro | 1:a4050fee11f7 | 388 | long x; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) |
mbedalvaro | 1:a4050fee11f7 | 389 | x=(long)(normRecenteringVector); |
mbedalvaro | 1:a4050fee11f7 | 390 | sendMes.setArgs( "i", &x); |
mbedalvaro | 1:a4050fee11f7 | 391 | osc.sendOsc( &sendMes ); |
mbedalvaro | 1:a4050fee11f7 | 392 | } |
mbedalvaro | 1:a4050fee11f7 | 393 | |
mbedalvaro | 1:a4050fee11f7 | 394 | if (sendingTouched) { |
mbedalvaro | 1:a4050fee11f7 | 395 | if (displaySensingBuffer.lightTouched) { |
mbedalvaro | 1:a4050fee11f7 | 396 | sendMes.clearArgs(); // there are no arguments to send |
mbedalvaro | 1:a4050fee11f7 | 397 | sendMes.setSubAddress("/touched"); |
mbedalvaro | 1:a4050fee11f7 | 398 | osc.sendOsc( &sendMes ); |
mbedalvaro | 1:a4050fee11f7 | 399 | } |
mbedalvaro | 1:a4050fee11f7 | 400 | } |
mbedalvaro | 1:a4050fee11f7 | 401 | |
mbedalvaro | 1:a4050fee11f7 | 402 | } // end of OSC sending per-spot |
mbedalvaro | 1:a4050fee11f7 | 403 | |
mbedalvaro | 1:a4050fee11f7 | 404 | // ===================== SERIAL ====================== |
mbedalvaro | 1:a4050fee11f7 | 405 | if (sendSerial) { |
mbedalvaro | 1:a4050fee11f7 | 406 | //.. to do |
mbedalvaro | 1:a4050fee11f7 | 407 | } |
mbedalvaro | 1:a4050fee11f7 | 408 | |
mbedalvaro | 1:a4050fee11f7 | 409 | myled2=0; // for tests... |
mbedalvaro | 1:a4050fee11f7 | 410 | } |