Alvaro Cassinelli / Mbed 2 deprecated skinGames_forktest

just a test

Dependencies:   mbed

Fork of scoreLight_Advanced by Alvaro Cassinelli

rigidLoop.cpp@3:b44ff6de81bd, 2012-04-02 (annotated)

Committer:
mbedalvaro
Date:
Mon Apr 02 05:33:44 2012 +0000
Revision:
3:b44ff6de81bd
Parent:
2:34157ebbf56b
Child:
4:f9d364f10335
working but needs a lot of calibration and optimization. In particular the laser renderer - I am going to modify it, that's why I commit now.

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