![](/media/cache/profiles/68a15b5278e4f7c4c056df9d5f1d3b1f.jpg.50x50_q85.jpg)
just a test
Fork of scoreLight_Advanced by
rigidLoop.cpp@2:34157ebbf56b, 2012-03-31 (annotated)
- Committer:
- mbedalvaro
- Date:
- Sat Mar 31 12:50:32 2012 +0000
- Revision:
- 2:34157ebbf56b
- Parent:
- 1:a4050fee11f7
- Child:
- 3:b44ff6de81bd
- Debugged some subtle problem that made the motion of the rigidLoops strange (when damping factor was too small, the blobs would start moving faster and faster sometimes, and oscillate - the reason being the VERLET integration and the fact that the up...
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 | 1:a4050fee11f7 | 15 | void rigidLoop::createBlob(int _id, RigidLoopMode _elasticBlobMode, vector2D _initPos) { |
mbedalvaro | 1:a4050fee11f7 | 16 | // (1) set ID: |
mbedalvaro | 1:a4050fee11f7 | 17 | identifier=_id; |
mbedalvaro | 1:a4050fee11f7 | 18 | |
mbedalvaro | 1:a4050fee11f7 | 19 | updateMode=_elasticBlobMode; |
mbedalvaro | 1:a4050fee11f7 | 20 | |
mbedalvaro | 1:a4050fee11f7 | 21 | // (2) Initialize common variables of all blobs (base class): |
mbedalvaro | 1:a4050fee11f7 | 22 | initCommonVariables(); |
mbedalvaro | 1:a4050fee11f7 | 23 | |
mbedalvaro | 1:a4050fee11f7 | 24 | // (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 | 1:a4050fee11f7 | 25 | saccadeRadius=50; |
mbedalvaro | 2:34157ebbf56b | 26 | angleCorrectionForceLoop=0; // this will depend on the size of the blob (in principle), and number of blobs... |
mbedalvaro | 2:34157ebbf56b | 27 | |
mbedalvaro | 2:34157ebbf56b | 28 | |
mbedalvaro | 1:a4050fee11f7 | 29 | // (3) Initialize secondary variables depending on the behaviour mode (may be changed afterwards in real time) |
mbedalvaro | 1:a4050fee11f7 | 30 | |
mbedalvaro | 1:a4050fee11f7 | 31 | switch (updateMode) { |
mbedalvaro | 1:a4050fee11f7 | 32 | case SPOT_FOLLOWING: |
mbedalvaro | 1:a4050fee11f7 | 33 | |
mbedalvaro | 1:a4050fee11f7 | 34 | // Name of this kind of spot: |
mbedalvaro | 1:a4050fee11f7 | 35 | sprintf(spotName,"rigid_following"); |
mbedalvaro | 1:a4050fee11f7 | 36 | |
mbedalvaro | 1:a4050fee11f7 | 37 | // Color: (use parameter in the future): |
mbedalvaro | 1:a4050fee11f7 | 38 | setColor(0x07);//0x04+0x02>>i); |
mbedalvaro | 1:a4050fee11f7 | 39 | |
mbedalvaro | 1:a4050fee11f7 | 40 | // default (initial) shape (the scafold belongs to the base class): |
mbedalvaro | 2:34157ebbf56b | 41 | bluePrint.buildCircularScafold(saccadeRadius, _initPos, vector2D(-2,5), 10); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints); |
mbedalvaro | 1:a4050fee11f7 | 42 | |
mbedalvaro | 1:a4050fee11f7 | 43 | // 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 | 44 | massCenter=0.01; |
mbedalvaro | 1:a4050fee11f7 | 45 | dampMotionCenterMass=0.001; |
mbedalvaro | 1:a4050fee11f7 | 46 | |
mbedalvaro | 1:a4050fee11f7 | 47 | break; |
mbedalvaro | 1:a4050fee11f7 | 48 | |
mbedalvaro | 1:a4050fee11f7 | 49 | case SPOT_BOUNCING: |
mbedalvaro | 1:a4050fee11f7 | 50 | // Name of this kind of spot: |
mbedalvaro | 1:a4050fee11f7 | 51 | sprintf(spotName,"rigid_bouncing"); |
mbedalvaro | 1:a4050fee11f7 | 52 | |
mbedalvaro | 1:a4050fee11f7 | 53 | |
mbedalvaro | 1:a4050fee11f7 | 54 | // default (initial) shape (the scafold belongs to the base class): |
mbedalvaro | 2:34157ebbf56b | 55 | bluePrint.buildCircularScafold(saccadeRadius, _initPos, vector2D(1,5), 10); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints); |
mbedalvaro | 1:a4050fee11f7 | 56 | |
mbedalvaro | 1:a4050fee11f7 | 57 | // Numeric parameters for the simulated mechanical system: |
mbedalvaro | 1:a4050fee11f7 | 58 | massCenter=0.1; |
mbedalvaro | 2:34157ebbf56b | 59 | dampMotionCenterMass=0.00015;//00003; |
mbedalvaro | 1:a4050fee11f7 | 60 | factorBouncingForce=0.0001; // this is because we will use a force on the central mass |
mbedalvaro | 1:a4050fee11f7 | 61 | |
mbedalvaro | 2:34157ebbf56b | 62 | slidingDirection=true; // For contour following (will change direction when touching wall) |
mbedalvaro | 2:34157ebbf56b | 63 | speedContourFollowing=0.0003; |
mbedalvaro | 1:a4050fee11f7 | 64 | |
mbedalvaro | 1:a4050fee11f7 | 65 | |
mbedalvaro | 1:a4050fee11f7 | 66 | break; |
mbedalvaro | 1:a4050fee11f7 | 67 | } |
mbedalvaro | 1:a4050fee11f7 | 68 | |
mbedalvaro | 1:a4050fee11f7 | 69 | |
mbedalvaro | 1:a4050fee11f7 | 70 | // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things): |
mbedalvaro | 1:a4050fee11f7 | 71 | createLoopFromScafold(); |
mbedalvaro | 1:a4050fee11f7 | 72 | |
mbedalvaro | 1:a4050fee11f7 | 73 | // 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 | 1:a4050fee11f7 | 74 | setRegionMotion(MIN_AD_MIRRORS+saccadeRadius, MIN_AD_MIRRORS+saccadeRadius, MAX_AD_MIRRORS-saccadeRadius, MAX_AD_MIRRORS-saccadeRadius); |
mbedalvaro | 1:a4050fee11f7 | 75 | |
mbedalvaro | 1:a4050fee11f7 | 76 | // !!!!!!!!!!!!!!!!!!!!! :: |
mbedalvaro | 1:a4050fee11f7 | 77 | // The following is not nice here (should be in the classLaserSensingTrajectory, not even on the simpleLaserRenderer), but for the time being let's leave it here |
mbedalvaro | 1:a4050fee11f7 | 78 | setDelayMirrors(4); // ATTN!!! needs to be called AFTER setting the number of points!!! (note: 5 seemed good) |
mbedalvaro | 1:a4050fee11f7 | 79 | |
mbedalvaro | 1:a4050fee11f7 | 80 | } |
mbedalvaro | 1:a4050fee11f7 | 81 | |
mbedalvaro | 1:a4050fee11f7 | 82 | |
mbedalvaro | 1:a4050fee11f7 | 83 | void rigidLoop::initSizeBlob(int _numPoints) { |
mbedalvaro | 1:a4050fee11f7 | 84 | // Iinitialize blob size (number of points for the loop, as well as other structures such as lsdTrajectory) |
mbedalvaro | 1:a4050fee11f7 | 85 | numPoints=_numPoints; |
mbedalvaro | 1:a4050fee11f7 | 86 | |
mbedalvaro | 1:a4050fee11f7 | 87 | // Sensing and Display trajectory: |
mbedalvaro | 1:a4050fee11f7 | 88 | 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 | 89 | } |
mbedalvaro | 1:a4050fee11f7 | 90 | |
mbedalvaro | 1:a4050fee11f7 | 91 | void rigidLoop::createLoopFromScafold(void) { |
mbedalvaro | 1:a4050fee11f7 | 92 | |
mbedalvaro | 1:a4050fee11f7 | 93 | initSizeBlob(bluePrint.scafold.size()); // very simple here (only need to set the size of the lsd buffer) |
mbedalvaro | 1:a4050fee11f7 | 94 | |
mbedalvaro | 1:a4050fee11f7 | 95 | centerMass.mass=massCenter; |
mbedalvaro | 1:a4050fee11f7 | 96 | centerMass.dampMotion = dampMotionCenterMass; |
mbedalvaro | 1:a4050fee11f7 | 97 | |
mbedalvaro | 1:a4050fee11f7 | 98 | // note: the following may not be required in case of contour following: |
mbedalvaro | 2:34157ebbf56b | 99 | centerMass.setIntegrationStep(0.23); // VERY IMPORTANT! in the case of verlet integration, we need to set dt BEFORE setting the initial speed. |
mbedalvaro | 1:a4050fee11f7 | 100 | centerMass.setInitialCondition(bluePrint.center.x, bluePrint.center.y, bluePrint.speed.x, bluePrint.speed.y); |
mbedalvaro | 1:a4050fee11f7 | 101 | // centerMass.setInitialCondition(2047.0, 2047.0,0.0,0.0); |
mbedalvaro | 1:a4050fee11f7 | 102 | } |
mbedalvaro | 1:a4050fee11f7 | 103 | |
mbedalvaro | 1:a4050fee11f7 | 104 | 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 | 105 | centerMass.setWallLimits(mmix+10, mmiy+10, mmax-10, mmay-10); |
mbedalvaro | 1:a4050fee11f7 | 106 | } |
mbedalvaro | 1:a4050fee11f7 | 107 | |
mbedalvaro | 1:a4050fee11f7 | 108 | |
mbedalvaro | 1:a4050fee11f7 | 109 | void rigidLoop::update() { |
mbedalvaro | 1:a4050fee11f7 | 110 | |
mbedalvaro | 1:a4050fee11f7 | 111 | // (I) process loop geometry: not needed (rigid) |
mbedalvaro | 1:a4050fee11f7 | 112 | // Just check if the blob touched the borders (only need to do this with the central mass): |
mbedalvaro | 1:a4050fee11f7 | 113 | blobWallCollision=centerMass.bWallCollision; |
mbedalvaro | 1:a4050fee11f7 | 114 | |
mbedalvaro | 1:a4050fee11f7 | 115 | // (II) Process sensing buffer and compute light forces: |
mbedalvaro | 1:a4050fee11f7 | 116 | 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 | 117 | |
mbedalvaro | 1:a4050fee11f7 | 118 | // (III) Compute recentering vector (the "penetration vector in fact"), using "first order moment": |
mbedalvaro | 1:a4050fee11f7 | 119 | vector2D momentVector(0,0); |
mbedalvaro | 1:a4050fee11f7 | 120 | for (int i = 0; i < numPoints; i++) { |
mbedalvaro | 1:a4050fee11f7 | 121 | if (displaySensingBuffer.lsdTrajectory[i].lightZone>0) { // this is, we are in a dark zone (better to integrate there, because it is normally smaller) |
mbedalvaro | 1:a4050fee11f7 | 122 | momentVector+=bluePrint.scafold[i]-centerMass.pos; |
mbedalvaro | 1:a4050fee11f7 | 123 | } |
mbedalvaro | 1:a4050fee11f7 | 124 | } |
mbedalvaro | 1:a4050fee11f7 | 125 | float momentNorm=momentVector.length(); |
mbedalvaro | 2:34157ebbf56b | 126 | if (momentNorm==0) { |
mbedalvaro | 2:34157ebbf56b | 127 | recenteringVectorLoop.set(0,0); |
mbedalvaro | 2:34157ebbf56b | 128 | normRecenteringVector=0; |
mbedalvaro | 2:34157ebbf56b | 129 | angleRecenteringVector=0; |
mbedalvaro | 2:34157ebbf56b | 130 | } |
mbedalvaro | 2:34157ebbf56b | 131 | else { |
mbedalvaro | 1:a4050fee11f7 | 132 | float aux=saccadeRadius/momentNorm; |
mbedalvaro | 1:a4050fee11f7 | 133 | if (aux>0.5) recenteringVectorLoop=momentVector*sqrt(aux*aux-0.25); |
mbedalvaro | 1:a4050fee11f7 | 134 | else recenteringVectorLoop=momentVector*sqrt(0.25-aux*aux); |
mbedalvaro | 1:a4050fee11f7 | 135 | //Rotate by angleCorrection (to correct delays, etc): |
mbedalvaro | 1:a4050fee11f7 | 136 | recenteringVectorLoop.rotateDeg(angleCorrectionForceLoop); |
mbedalvaro | 2:34157ebbf56b | 137 | // Compute redundant quantities (if necessary, for sending through OSC, etc): |
mbedalvaro | 1:a4050fee11f7 | 138 | normRecenteringVector=recenteringVectorLoop.length(); |
mbedalvaro | 1:a4050fee11f7 | 139 | angleRecenteringVector=recenteringVectorLoop.angleDegHoriz(); |
mbedalvaro | 2:34157ebbf56b | 140 | } |
mbedalvaro | 2:34157ebbf56b | 141 | |
mbedalvaro | 1:a4050fee11f7 | 142 | |
mbedalvaro | 1:a4050fee11f7 | 143 | // Now, depending on the mode of operation, we have two types of behaviour: |
mbedalvaro | 1:a4050fee11f7 | 144 | vector2D slidingVector; //( need to declare it here because a switch "jump" cannot bypass an initialization) |
mbedalvaro | 1:a4050fee11f7 | 145 | switch (updateMode) { |
mbedalvaro | 1:a4050fee11f7 | 146 | |
mbedalvaro | 1:a4050fee11f7 | 147 | case SPOT_FOLLOWING: |
mbedalvaro | 1:a4050fee11f7 | 148 | // we need to compute the tangencial "speed": |
mbedalvaro | 1:a4050fee11f7 | 149 | // vector2D slidingVector; |
mbedalvaro | 2:34157ebbf56b | 150 | // 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 | 151 | if (momentNorm>0) { |
mbedalvaro | 2:34157ebbf56b | 152 | momentVector/=momentNorm; |
mbedalvaro | 1:a4050fee11f7 | 153 | // We can now compute the sliding vector as: |
mbedalvaro | 1:a4050fee11f7 | 154 | slidingVector=momentVector.getRotatedDeg(slidingDirection? 90 : -90) * speedContourFollowing*saccadeRadius; |
mbedalvaro | 1:a4050fee11f7 | 155 | // Then the final correcting vector is the sum of sliding plus recentering vector (with a factor if one want some smothing) |
mbedalvaro | 1:a4050fee11f7 | 156 | // 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 | 1:a4050fee11f7 | 157 | centerMass.pos += slidingVector + recenteringVectorLoop * 0.9; |
mbedalvaro | 2:34157ebbf56b | 158 | |
mbedalvaro | 2:34157ebbf56b | 159 | // The following function can help constraining the position "pos", but it also does too much. Do something simpler perhaps? |
mbedalvaro | 2:34157ebbf56b | 160 | centerMass.bounceOffWalls(); // constrain position (and compute wall "hit") |
mbedalvaro | 2:34157ebbf56b | 161 | |
mbedalvaro | 2:34157ebbf56b | 162 | } else { |
mbedalvaro | 2:34157ebbf56b | 163 | // not on something. Do nothing for the time being |
mbedalvaro | 2:34157ebbf56b | 164 | } |
mbedalvaro | 1:a4050fee11f7 | 165 | |
mbedalvaro | 1:a4050fee11f7 | 166 | break; |
mbedalvaro | 1:a4050fee11f7 | 167 | |
mbedalvaro | 1:a4050fee11f7 | 168 | case SPOT_BOUNCING: |
mbedalvaro | 1:a4050fee11f7 | 169 | // this is very simple: we need to give a force to the centralMass that is OPPOSITE to the recenteringVectorLoop vector |
mbedalvaro | 1:a4050fee11f7 | 170 | centerMass.resetForce(); |
mbedalvaro | 2:34157ebbf56b | 171 | centerMass.addForce(recenteringVectorLoop*factorBouncingForce); |
mbedalvaro | 1:a4050fee11f7 | 172 | |
mbedalvaro | 1:a4050fee11f7 | 173 | // update dynamics for the central mass:: |
mbedalvaro | 1:a4050fee11f7 | 174 | #ifndef VERLET_METHOD |
mbedalvaro | 1:a4050fee11f7 | 175 | centerMass.addDampingForce(); // // only in case of EULER method (damping in VERLET mode is done automatically when updating) |
mbedalvaro | 1:a4050fee11f7 | 176 | #endif |
mbedalvaro | 1:a4050fee11f7 | 177 | |
mbedalvaro | 1:a4050fee11f7 | 178 | centerMass.update(); // unconstrained |
mbedalvaro | 1:a4050fee11f7 | 179 | centerMass.bounceOffWalls(); // constrain position (and compute wall "hit") |
mbedalvaro | 1:a4050fee11f7 | 180 | |
mbedalvaro | 1:a4050fee11f7 | 181 | break; |
mbedalvaro | 1:a4050fee11f7 | 182 | |
mbedalvaro | 1:a4050fee11f7 | 183 | } |
mbedalvaro | 1:a4050fee11f7 | 184 | |
mbedalvaro | 1:a4050fee11f7 | 185 | // OTHER PARTICULAR THINGS: |
mbedalvaro | 1:a4050fee11f7 | 186 | // change sliding direction (for countour following): |
mbedalvaro | 1:a4050fee11f7 | 187 | if (blobWallCollision) { |
mbedalvaro | 1:a4050fee11f7 | 188 | if (wallCounter>10) { |
mbedalvaro | 1:a4050fee11f7 | 189 | slidingDirection=!slidingDirection; |
mbedalvaro | 1:a4050fee11f7 | 190 | wallCounter=0; |
mbedalvaro | 1:a4050fee11f7 | 191 | } |
mbedalvaro | 1:a4050fee11f7 | 192 | } |
mbedalvaro | 1:a4050fee11f7 | 193 | wallCounter++; |
mbedalvaro | 1:a4050fee11f7 | 194 | } |
mbedalvaro | 1:a4050fee11f7 | 195 | |
mbedalvaro | 1:a4050fee11f7 | 196 | |
mbedalvaro | 1:a4050fee11f7 | 197 | // 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 | 198 | void rigidLoop::draw() { |
mbedalvaro | 1:a4050fee11f7 | 199 | // for the time being, there is no "opengl" like renderer, so we just copy into the lsdTrajectory: |
mbedalvaro | 1:a4050fee11f7 | 200 | float cx= centerMass.pos.x; |
mbedalvaro | 1:a4050fee11f7 | 201 | float cy= centerMass.pos.y; |
mbedalvaro | 1:a4050fee11f7 | 202 | for (int i = 0; i < numPoints; i++) { |
mbedalvaro | 1:a4050fee11f7 | 203 | // The shape is drawn by translating the scafold shape (centered on centerMass): |
mbedalvaro | 1:a4050fee11f7 | 204 | displaySensingBuffer.lsdTrajectory[i].x= int(bluePrint.scafold[i].x + cx ); // note: it should be an unsigned short!! |
mbedalvaro | 1:a4050fee11f7 | 205 | displaySensingBuffer.lsdTrajectory[i].y= int(bluePrint.scafold[i].y + cy ); |
mbedalvaro | 1:a4050fee11f7 | 206 | //displaySensingBuffer.lsdTrajectory[i].color=blobColor; // perhaps per point color is not a good idea for the time being... |
mbedalvaro | 1:a4050fee11f7 | 207 | } |
mbedalvaro | 1:a4050fee11f7 | 208 | // global color for the whole loop: |
mbedalvaro | 1:a4050fee11f7 | 209 | displaySensingBuffer.displayColor=blobColor; |
mbedalvaro | 1:a4050fee11f7 | 210 | } |
mbedalvaro | 1:a4050fee11f7 | 211 | |
mbedalvaro | 1:a4050fee11f7 | 212 | void rigidLoop::computeBoundingBox() { |
mbedalvaro | 1:a4050fee11f7 | 213 | } |
mbedalvaro | 1:a4050fee11f7 | 214 | |
mbedalvaro | 1:a4050fee11f7 | 215 | |
mbedalvaro | 1:a4050fee11f7 | 216 | |
mbedalvaro | 1:a4050fee11f7 | 217 | void rigidLoop::sendDataSpecific() { |
mbedalvaro | 1:a4050fee11f7 | 218 | char auxstring[10]; |
mbedalvaro | 1:a4050fee11f7 | 219 | myled2=1; // for tests... |
mbedalvaro | 1:a4050fee11f7 | 220 | |
mbedalvaro | 1:a4050fee11f7 | 221 | // First, set the top address of the message to the ID of the blob (not the name): |
mbedalvaro | 1:a4050fee11f7 | 222 | sprintf(auxstring, "%d", identifier); |
mbedalvaro | 1:a4050fee11f7 | 223 | sendMes.setTopAddress("0");//auxstring); |
mbedalvaro | 1:a4050fee11f7 | 224 | |
mbedalvaro | 1:a4050fee11f7 | 225 | // ===================== OSC ====================== |
mbedalvaro | 1:a4050fee11f7 | 226 | if (sendOSC) { |
mbedalvaro | 1:a4050fee11f7 | 227 | |
mbedalvaro | 1:a4050fee11f7 | 228 | // (a) Anchor mass: |
mbedalvaro | 1:a4050fee11f7 | 229 | if (sendingAnchorPosition) { |
mbedalvaro | 1:a4050fee11f7 | 230 | sendMes.setSubAddress("/apos"); |
mbedalvaro | 1:a4050fee11f7 | 231 | long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) |
mbedalvaro | 1:a4050fee11f7 | 232 | x=(long)(centerMass.pos.x); |
mbedalvaro | 1:a4050fee11f7 | 233 | y=(long)(centerMass.pos.y); |
mbedalvaro | 1:a4050fee11f7 | 234 | sendMes.setArgs( "ii", &x, &y); |
mbedalvaro | 1:a4050fee11f7 | 235 | osc.sendOsc( &sendMes ); |
mbedalvaro | 1:a4050fee11f7 | 236 | } |
mbedalvaro | 1:a4050fee11f7 | 237 | |
mbedalvaro | 1:a4050fee11f7 | 238 | // (b) data from blob points (this is ONLY FOR TESTS, because the loop is rigid - sending the center is enough) |
mbedalvaro | 1:a4050fee11f7 | 239 | if (sendingLoopPositions) { |
mbedalvaro | 1:a4050fee11f7 | 240 | #ifdef SEND_AS_POINTS |
mbedalvaro | 1:a4050fee11f7 | 241 | long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) |
mbedalvaro | 1:a4050fee11f7 | 242 | float cx= centerMass.pos.x; |
mbedalvaro | 1:a4050fee11f7 | 243 | float cy= centerMass.pos.y; |
mbedalvaro | 1:a4050fee11f7 | 244 | for (int i = 0; i < numPoints; i++) { |
mbedalvaro | 2:34157ebbf56b | 245 | sprintf(auxstring, "/p%d",identifier*10+ i);//20+ i+(identifier-1)*10); // auxstring read as "/p1", "/p2", ... |
mbedalvaro | 1:a4050fee11f7 | 246 | 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 | 247 | x=(long)(bluePrint.scafold[i].x + cx); |
mbedalvaro | 1:a4050fee11f7 | 248 | y=(long)(bluePrint.scafold[i].y + cy); |
mbedalvaro | 1:a4050fee11f7 | 249 | sendMes.setArgs( "ii", &x, &y); |
mbedalvaro | 1:a4050fee11f7 | 250 | osc.sendOsc( &sendMes ); |
mbedalvaro | 1:a4050fee11f7 | 251 | } |
mbedalvaro | 1:a4050fee11f7 | 252 | |
mbedalvaro | 1:a4050fee11f7 | 253 | #endif |
mbedalvaro | 1:a4050fee11f7 | 254 | #ifdef SEND_AS_BLOB |
mbedalvaro | 1:a4050fee11f7 | 255 | sendMes.clearArgs(); // no need, we won't use osc.sendOsc()... |
mbedalvaro | 1:a4050fee11f7 | 256 | uint8_t blobdata[4*numPoints]; // 2 bytes per coordinate, and 2 coordinates |
mbedalvaro | 1:a4050fee11f7 | 257 | float cx= centerMass.pos.x; |
mbedalvaro | 1:a4050fee11f7 | 258 | float cy= centerMass.pos.y; |
mbedalvaro | 1:a4050fee11f7 | 259 | for (int i = 0; i < numPoints; i++ ) { |
mbedalvaro | 1:a4050fee11f7 | 260 | // note: massesLoop[i].pos.x is a "float" |
mbedalvaro | 1:a4050fee11f7 | 261 | uint16_t x=(uint16_t)(bluePrint.scafold[i].x + cx); |
mbedalvaro | 1:a4050fee11f7 | 262 | blobdata[4*i]=(uint8_t)x>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE) |
mbedalvaro | 1:a4050fee11f7 | 263 | blobdata[4*i+1]=(uint8_t)x; |
mbedalvaro | 1:a4050fee11f7 | 264 | |
mbedalvaro | 1:a4050fee11f7 | 265 | uint16_t y=(uint16_t)(bluePrint.scafold[i].y + cy); |
mbedalvaro | 1:a4050fee11f7 | 266 | blobdata[4*i+2]=(uint8_t)y>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE) |
mbedalvaro | 1:a4050fee11f7 | 267 | blobdata[4*i+3]=(uint8_t)y; |
mbedalvaro | 1:a4050fee11f7 | 268 | } |
mbedalvaro | 1:a4050fee11f7 | 269 | osc.sendOscBlob(&(blobdata[0]), 4*numPoints, &sendMes ); // second parameter is osc blob size in bytes |
mbedalvaro | 1:a4050fee11f7 | 270 | #endif |
mbedalvaro | 1:a4050fee11f7 | 271 | #ifdef SEND_AS_STRING |
mbedalvaro | 1:a4050fee11f7 | 272 | sendMes.clearArgs(); // no need, we won't use osc.sendOsc()... |
mbedalvaro | 1:a4050fee11f7 | 273 | uint8_t blobdata[4*numPoints]; // 2 bytes per coordinate, and 2 coordinates |
mbedalvaro | 1:a4050fee11f7 | 274 | float cx= centerMass.pos.x; |
mbedalvaro | 1:a4050fee11f7 | 275 | float cy= centerMass.pos.y; |
mbedalvaro | 1:a4050fee11f7 | 276 | for (int i = 0; i < numPoints; i++ ) { |
mbedalvaro | 1:a4050fee11f7 | 277 | // note: massesLoop[i].pos.x is a "float" |
mbedalvaro | 1:a4050fee11f7 | 278 | uint16_t x=(uint16_t)(bluePrint.scafold[i].x + cx ); |
mbedalvaro | 1:a4050fee11f7 | 279 | blobdata[4*i]=(uint8_t)x>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE) |
mbedalvaro | 1:a4050fee11f7 | 280 | blobdata[4*i+1]=(uint8_t)x; |
mbedalvaro | 1:a4050fee11f7 | 281 | |
mbedalvaro | 1:a4050fee11f7 | 282 | uint16_t y=(uint16_t)(bluePrint.scafold[i].y + cy); |
mbedalvaro | 1:a4050fee11f7 | 283 | blobdata[4*i+2]=(uint8_t)y>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE) |
mbedalvaro | 1:a4050fee11f7 | 284 | blobdata[4*i+3]=(uint8_t)y; |
mbedalvaro | 1:a4050fee11f7 | 285 | } |
mbedalvaro | 1:a4050fee11f7 | 286 | osc.sendOscString(blobdata, 4*numPoints, &sendMes ); // second parameter is osc blob size in bytes |
mbedalvaro | 1:a4050fee11f7 | 287 | #endif |
mbedalvaro | 1:a4050fee11f7 | 288 | } |
mbedalvaro | 1:a4050fee11f7 | 289 | if (sendingLoopRegions) { |
mbedalvaro | 1:a4050fee11f7 | 290 | long x; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) |
mbedalvaro | 1:a4050fee11f7 | 291 | for (int i = 0; i < numPoints; i++) { |
mbedalvaro | 1:a4050fee11f7 | 292 | sprintf(auxstring, "/r%d", i); // auxstring read as "/f1", "/f2", ... |
mbedalvaro | 1:a4050fee11f7 | 293 | 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 | 294 | x=(long)(displaySensingBuffer.lsdTrajectory[i].lightZone>0? 1 : 0); |
mbedalvaro | 1:a4050fee11f7 | 295 | sendMes.setArgs( "i", &x); |
mbedalvaro | 1:a4050fee11f7 | 296 | osc.sendOsc( &sendMes ); |
mbedalvaro | 1:a4050fee11f7 | 297 | } |
mbedalvaro | 1:a4050fee11f7 | 298 | } |
mbedalvaro | 1:a4050fee11f7 | 299 | if (sendingLoopTouchWall) { // global touch wall for the loop (not per point) |
mbedalvaro | 1:a4050fee11f7 | 300 | long wall; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) |
mbedalvaro | 1:a4050fee11f7 | 301 | sprintf(auxstring, "/bWall"); |
mbedalvaro | 1:a4050fee11f7 | 302 | sendMes.setSubAddress(auxstring); |
mbedalvaro | 1:a4050fee11f7 | 303 | wall=(long)(blobWallCollision? 1 : 0); |
mbedalvaro | 1:a4050fee11f7 | 304 | sendMes.setArgs( "i", &wall); |
mbedalvaro | 1:a4050fee11f7 | 305 | osc.sendOsc( &sendMes ); |
mbedalvaro | 1:a4050fee11f7 | 306 | } |
mbedalvaro | 1:a4050fee11f7 | 307 | |
mbedalvaro | 1:a4050fee11f7 | 308 | // (d) Light sensing statistics: |
mbedalvaro | 1:a4050fee11f7 | 309 | if (sendingBlobMaxMin) { |
mbedalvaro | 1:a4050fee11f7 | 310 | sendMes.setSubAddress("/maxmin"); |
mbedalvaro | 1:a4050fee11f7 | 311 | long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) |
mbedalvaro | 1:a4050fee11f7 | 312 | x=(long)(displaySensingBuffer.maxI); |
mbedalvaro | 1:a4050fee11f7 | 313 | y=(long)(displaySensingBuffer.minI); |
mbedalvaro | 1:a4050fee11f7 | 314 | sendMes.setArgs( "ii", &x, &y); |
mbedalvaro | 1:a4050fee11f7 | 315 | osc.sendOsc( &sendMes ); |
mbedalvaro | 1:a4050fee11f7 | 316 | } |
mbedalvaro | 1:a4050fee11f7 | 317 | |
mbedalvaro | 1:a4050fee11f7 | 318 | // (e) Recentering vector: (note: redundant with sendingLightForce, IF the correction angle is known). |
mbedalvaro | 1:a4050fee11f7 | 319 | if (sendingRecenteringVector) { |
mbedalvaro | 1:a4050fee11f7 | 320 | sendMes.setSubAddress("/rvector"); |
mbedalvaro | 1:a4050fee11f7 | 321 | long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) |
mbedalvaro | 1:a4050fee11f7 | 322 | x=(long)(recenteringVectorLoop.x); |
mbedalvaro | 1:a4050fee11f7 | 323 | y=(long)(recenteringVectorLoop.y); |
mbedalvaro | 1:a4050fee11f7 | 324 | sendMes.setArgs( "ii", &x, &y); |
mbedalvaro | 1:a4050fee11f7 | 325 | osc.sendOsc( &sendMes ); |
mbedalvaro | 1:a4050fee11f7 | 326 | } |
mbedalvaro | 1:a4050fee11f7 | 327 | if (sendingRecenteringAngle) { |
mbedalvaro | 1:a4050fee11f7 | 328 | sendMes.setSubAddress("/rangle"); |
mbedalvaro | 1:a4050fee11f7 | 329 | long x; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) |
mbedalvaro | 1:a4050fee11f7 | 330 | x=(long)(angleRecenteringVector); |
mbedalvaro | 1:a4050fee11f7 | 331 | sendMes.setArgs( "i", &x); |
mbedalvaro | 1:a4050fee11f7 | 332 | osc.sendOsc( &sendMes ); |
mbedalvaro | 1:a4050fee11f7 | 333 | } |
mbedalvaro | 1:a4050fee11f7 | 334 | if (sendingRecenteringNorm) { |
mbedalvaro | 1:a4050fee11f7 | 335 | sendMes.setSubAddress("/rnorm"); |
mbedalvaro | 1:a4050fee11f7 | 336 | long x; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) |
mbedalvaro | 1:a4050fee11f7 | 337 | x=(long)(normRecenteringVector); |
mbedalvaro | 1:a4050fee11f7 | 338 | sendMes.setArgs( "i", &x); |
mbedalvaro | 1:a4050fee11f7 | 339 | osc.sendOsc( &sendMes ); |
mbedalvaro | 1:a4050fee11f7 | 340 | } |
mbedalvaro | 1:a4050fee11f7 | 341 | |
mbedalvaro | 1:a4050fee11f7 | 342 | if (sendingTouched) { |
mbedalvaro | 1:a4050fee11f7 | 343 | if (displaySensingBuffer.lightTouched) { |
mbedalvaro | 1:a4050fee11f7 | 344 | sendMes.clearArgs(); // there are no arguments to send |
mbedalvaro | 1:a4050fee11f7 | 345 | sendMes.setSubAddress("/touched"); |
mbedalvaro | 1:a4050fee11f7 | 346 | osc.sendOsc( &sendMes ); |
mbedalvaro | 1:a4050fee11f7 | 347 | } |
mbedalvaro | 1:a4050fee11f7 | 348 | } |
mbedalvaro | 1:a4050fee11f7 | 349 | |
mbedalvaro | 1:a4050fee11f7 | 350 | } // end of OSC sending per-spot |
mbedalvaro | 1:a4050fee11f7 | 351 | |
mbedalvaro | 1:a4050fee11f7 | 352 | // ===================== SERIAL ====================== |
mbedalvaro | 1:a4050fee11f7 | 353 | if (sendSerial) { |
mbedalvaro | 1:a4050fee11f7 | 354 | //.. to do |
mbedalvaro | 1:a4050fee11f7 | 355 | } |
mbedalvaro | 1:a4050fee11f7 | 356 | |
mbedalvaro | 1:a4050fee11f7 | 357 | myled2=0; // for tests... |
mbedalvaro | 1:a4050fee11f7 | 358 | } |