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