Alvaro Cassinelli / Mbed 2 deprecated skinGames_forktest

just a test

Dependencies:   mbed

Fork of scoreLight_Advanced by Alvaro Cassinelli

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?

UserRevisionLine numberNew 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 }