Alvaro Cassinelli / Mbed 2 deprecated skinGames_forktest

just a test

Dependencies:   mbed

Fork of scoreLight_Advanced by Alvaro Cassinelli

rigidLoop.cpp@27:1ce994629ffc, 2012-06-18 (annotated)

Committer:
mbedalvaro
Date:
Mon Jun 18 15:09:25 2012 +0000
Revision:
27:1ce994629ffc
Parent:
26:c9329c4fc20a
Child:
28:44b7b6e35548
new weird mode ad hoc, with wrong time delay so it produces blue spots along the contour (for a reason I still don't understand)

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 12:0de9cd2bced5 15 void rigidLoop::createBlob(int _id, RigidLoopMode _elasticBlobMode, vector2Df _initPos, vector2Df _initSpeed) {
mbedalvaro 1:a4050fee11f7 16 // (1) set ID:
mbedalvaro 1:a4050fee11f7 17 identifier=_id;
mbedalvaro 1:a4050fee11f7 18
mbedalvaro 1:a4050fee11f7 19 updateMode=_elasticBlobMode;
mbedalvaro 4:f9d364f10335 20
mbedalvaro 4:f9d364f10335 21 startCenter=_initPos;
mbedalvaro 4:f9d364f10335 22 startSpeed=_initSpeed;
mbedalvaro 4:f9d364f10335 23
mbedalvaro 1:a4050fee11f7 24 // (2) Initialize common variables of all blobs (base class):
mbedalvaro 1:a4050fee11f7 25 initCommonVariables();
mbedalvaro 1:a4050fee11f7 26
mbedalvaro 1:a4050fee11f7 27 // (3) initialize common variables for the different modes of this rigid loop (even if some are not used, better not to have more subclasses...)
mbedalvaro 18:d72935b13858 28 // Sending data:
mbedalvaro 20:8e82b95180e7 29 periodSendingData=10; // in ms
mbedalvaro 18:d72935b13858 30 sendingRecenteringAngle=true;
mbedalvaro 18:d72935b13858 31 sendingAnchorPosition=true;
mbedalvaro 18:d72935b13858 32 sendingOnlyWhenTouch=true;
mbedalvaro 21:bc9b9383f4b6 33
mbedalvaro 21:bc9b9383f4b6 34 // Gravity field:
mbedalvaro 21:bc9b9383f4b6 35 gravity.set(.5,0);
mbedalvaro 3:b44ff6de81bd 36
mbedalvaro 1:a4050fee11f7 37 // (3) Initialize secondary variables depending on the behaviour mode (may be changed afterwards in real time)
mbedalvaro 1:a4050fee11f7 38
mbedalvaro 1:a4050fee11f7 39 switch (updateMode) {
mbedalvaro 4:f9d364f10335 40
mbedalvaro 4:f9d364f10335 41 case SPOT_TEST:
mbedalvaro 4:f9d364f10335 42 // Name of this kind of spot:
mbedalvaro 4:f9d364f10335 43 sprintf(spotName,"spot_test");
mbedalvaro 4:f9d364f10335 44
mbedalvaro 4:f9d364f10335 45 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 4:f9d364f10335 46 setColor(0x04);
mbedalvaro 4:f9d364f10335 47
mbedalvaro 14:0fc33a3a7b4b 48 saccadeRadius=250;
mbedalvaro 4:f9d364f10335 49
mbedalvaro 4:f9d364f10335 50 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 14:0fc33a3a7b4b 51 // NOTE: number of points in the case of need to compute recentering vector needs to be EVEN
mbedalvaro 14:0fc33a3a7b4b 52 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 16); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 4:f9d364f10335 53
mbedalvaro 4:f9d364f10335 54 // Note: We may assume NO MASS for the center of the contour following loop. Adding mass may be interesting though (smooth motion).
mbedalvaro 4:f9d364f10335 55 massCenter=0.01;
mbedalvaro 4:f9d364f10335 56 dampMotionCenterMass=0.001;
mbedalvaro 4:f9d364f10335 57
mbedalvaro 4:f9d364f10335 58 // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
mbedalvaro 4:f9d364f10335 59 createLoopFromScafold();
mbedalvaro 4:f9d364f10335 60
mbedalvaro 5:73cd58b58f95 61 // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 5:73cd58b58f95 62 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 5:73cd58b58f95 63 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
mbedalvaro 7:0df17f3078bc 64 displaySensingBuffer.setDelayMirrors(1);
mbedalvaro 7:0df17f3078bc 65 angleCorrectionForceLoop=0;//360.0/bluePrint.scafold.size()/2; // in DEGREES
mbedalvaro 4:f9d364f10335 66
mbedalvaro 4:f9d364f10335 67 break;
mbedalvaro 4:f9d364f10335 68
mbedalvaro 1:a4050fee11f7 69 case SPOT_FOLLOWING:
mbedalvaro 1:a4050fee11f7 70
mbedalvaro 1:a4050fee11f7 71 // Name of this kind of spot:
mbedalvaro 1:a4050fee11f7 72 sprintf(spotName,"rigid_following");
mbedalvaro 1:a4050fee11f7 73
mbedalvaro 3:b44ff6de81bd 74 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 3:b44ff6de81bd 75 setColor(0x04);
mbedalvaro 1:a4050fee11f7 76
mbedalvaro 1:a4050fee11f7 77 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 24:4e52031a495b 78 saccadeRadius=40;
mbedalvaro 12:0de9cd2bced5 79 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 1:a4050fee11f7 80
mbedalvaro 1:a4050fee11f7 81 // 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 82 massCenter=0.01;
mbedalvaro 1:a4050fee11f7 83 dampMotionCenterMass=0.001;
mbedalvaro 1:a4050fee11f7 84
mbedalvaro 4:f9d364f10335 85 // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
mbedalvaro 4:f9d364f10335 86 createLoopFromScafold();
mbedalvaro 4:f9d364f10335 87
mbedalvaro 5:73cd58b58f95 88 slidingDirection=true; // For contour following (will change direction when touching wall)
mbedalvaro 22:d87317d7ca91 89 speedContourFollowing=1.1*saccadeRadius;
mbedalvaro 7:0df17f3078bc 90 justSearched=false;
mbedalvaro 5:73cd58b58f95 91
mbedalvaro 7:0df17f3078bc 92 // per-blob mirror delay: ONLY USEFUL FOR ELASTIC BLOBS, because otherwise it can be corrected by "angleCorrection"
mbedalvaro 7:0df17f3078bc 93 // (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 5:73cd58b58f95 94 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 7:0df17f3078bc 95 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop.
mbedalvaro 7:0df17f3078bc 96 // BUT because we may want to see the blue laser where there is dark zone, then we would try to adjust mirror delay as close as possible to the
mbedalvaro 7:0df17f3078bc 97 // optimal value, and finish the correction (fine tunned) with the angle correction (only possible in the case of circular rigid blob).
mbedalvaro 7:0df17f3078bc 98 displaySensingBuffer.setDelayMirrors(3); // this corresponds to an angular correction of -delayMirrors * 360/numPoints
mbedalvaro 15:56a0bf424e8d 99 angleCorrectionForceLoop= -5;// good for ONE spot: -5;// in DEGREES
mbedalvaro 4:f9d364f10335 100
mbedalvaro 1:a4050fee11f7 101 break;
mbedalvaro 1:a4050fee11f7 102
mbedalvaro 1:a4050fee11f7 103 case SPOT_BOUNCING:
mbedalvaro 1:a4050fee11f7 104 // Name of this kind of spot:
mbedalvaro 1:a4050fee11f7 105 sprintf(spotName,"rigid_bouncing");
mbedalvaro 1:a4050fee11f7 106
mbedalvaro 3:b44ff6de81bd 107 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 3:b44ff6de81bd 108 setColor(0x04);
mbedalvaro 27:1ce994629ffc 109
mbedalvaro 27:1ce994629ffc 110 gravity.set(0,0);
mbedalvaro 1:a4050fee11f7 111
mbedalvaro 24:4e52031a495b 112 saccadeRadius=65;//+rand()%20;
mbedalvaro 1:a4050fee11f7 113 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 12:0de9cd2bced5 114 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 1:a4050fee11f7 115
mbedalvaro 1:a4050fee11f7 116 // Numeric parameters for the simulated mechanical system:
mbedalvaro 24:4e52031a495b 117 massCenter=0.0008;//+0.000005*(rand()%100);
mbedalvaro 22:d87317d7ca91 118 dampMotionCenterMass=0.00045;//0.00015;//00003;
mbedalvaro 7:0df17f3078bc 119 factorBouncingForce=0.0018; // this is because we will use a force on the central mass
mbedalvaro 1:a4050fee11f7 120
mbedalvaro 4:f9d364f10335 121 // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
mbedalvaro 4:f9d364f10335 122 createLoopFromScafold();
mbedalvaro 4:f9d364f10335 123
mbedalvaro 5:73cd58b58f95 124 // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 5:73cd58b58f95 125 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 5:73cd58b58f95 126 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
mbedalvaro 7:0df17f3078bc 127 displaySensingBuffer.setDelayMirrors(3);
mbedalvaro 7:0df17f3078bc 128 angleCorrectionForceLoop=-5;// in degrees
mbedalvaro 1:a4050fee11f7 129
mbedalvaro 1:a4050fee11f7 130 break;
mbedalvaro 27:1ce994629ffc 131
mbedalvaro 27:1ce994629ffc 132 case SPOT_FOUNTAIN:
mbedalvaro 27:1ce994629ffc 133 // Name of this kind of spot:
mbedalvaro 27:1ce994629ffc 134 sprintf(spotName,"rigid_fountain");
mbedalvaro 27:1ce994629ffc 135
mbedalvaro 27:1ce994629ffc 136 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 27:1ce994629ffc 137 setColor(0x04);
mbedalvaro 27:1ce994629ffc 138
mbedalvaro 27:1ce994629ffc 139 saccadeRadius=40;//+rand()%20;
mbedalvaro 27:1ce994629ffc 140 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 27:1ce994629ffc 141 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 27:1ce994629ffc 142
mbedalvaro 27:1ce994629ffc 143 // Numeric parameters for the simulated mechanical system:
mbedalvaro 27:1ce994629ffc 144 massCenter=0.0005;//+0.000005*(rand()%100);
mbedalvaro 27:1ce994629ffc 145 dampMotionCenterMass=0.00045;//0.00015;//00003;
mbedalvaro 27:1ce994629ffc 146 factorBouncingForce=0.0018; // this is because we will use a force on the central mass
mbedalvaro 27:1ce994629ffc 147
mbedalvaro 27:1ce994629ffc 148 // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
mbedalvaro 27:1ce994629ffc 149 createLoopFromScafold();
mbedalvaro 27:1ce994629ffc 150
mbedalvaro 27:1ce994629ffc 151 // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 27:1ce994629ffc 152 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 27:1ce994629ffc 153 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
mbedalvaro 27:1ce994629ffc 154 displaySensingBuffer.setDelayMirrors(3);
mbedalvaro 27:1ce994629ffc 155 angleCorrectionForceLoop=-5;// in degrees
mbedalvaro 27:1ce994629ffc 156
mbedalvaro 27:1ce994629ffc 157 break;
mbedalvaro 1:a4050fee11f7 158 }
mbedalvaro 7:0df17f3078bc 159
mbedalvaro 7:0df17f3078bc 160 saccadeRadius_initial=saccadeRadius; // this is for search mode for instance.
mbedalvaro 12:0de9cd2bced5 161
mbedalvaro 12:0de9cd2bced5 162 // Excursion limits (for all points). Tthis 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 12:0de9cd2bced5 163 setRegionMotion(MIN_AD_MIRRORS+saccadeRadius, MIN_AD_MIRRORS+saccadeRadius, MAX_AD_MIRRORS-saccadeRadius, MAX_AD_MIRRORS-saccadeRadius);
mbedalvaro 12:0de9cd2bced5 164
mbedalvaro 26:c9329c4fc20a 165 // draw it once on the display buffer for good initialization:
mbedalvaro 26:c9329c4fc20a 166 draw();
mbedalvaro 1:a4050fee11f7 167 }
mbedalvaro 1:a4050fee11f7 168
mbedalvaro 1:a4050fee11f7 169
mbedalvaro 1:a4050fee11f7 170 void rigidLoop::initSizeBlob(int _numPoints) {
mbedalvaro 1:a4050fee11f7 171 // Iinitialize blob size (number of points for the loop, as well as other structures such as lsdTrajectory)
mbedalvaro 1:a4050fee11f7 172 numPoints=_numPoints;
mbedalvaro 1:a4050fee11f7 173
mbedalvaro 1:a4050fee11f7 174 // Sensing and Display trajectory:
mbedalvaro 1:a4050fee11f7 175 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 176 }
mbedalvaro 1:a4050fee11f7 177
mbedalvaro 1:a4050fee11f7 178 void rigidLoop::createLoopFromScafold(void) {
mbedalvaro 1:a4050fee11f7 179
mbedalvaro 1:a4050fee11f7 180 initSizeBlob(bluePrint.scafold.size()); // very simple here (only need to set the size of the lsd buffer)
mbedalvaro 1:a4050fee11f7 181
mbedalvaro 1:a4050fee11f7 182 centerMass.mass=massCenter;
mbedalvaro 1:a4050fee11f7 183 centerMass.dampMotion = dampMotionCenterMass;
mbedalvaro 1:a4050fee11f7 184
mbedalvaro 1:a4050fee11f7 185 // note: the following may not be required in case of contour following:
mbedalvaro 2:34157ebbf56b 186 centerMass.setIntegrationStep(0.23); // VERY IMPORTANT! in the case of verlet integration, we need to set dt BEFORE setting the initial speed.
mbedalvaro 4:f9d364f10335 187 centerMass.setInitialCondition(startCenter, startSpeed);
mbedalvaro 1:a4050fee11f7 188 // centerMass.setInitialCondition(2047.0, 2047.0,0.0,0.0);
mbedalvaro 26:c9329c4fc20a 189
mbedalvaro 1:a4050fee11f7 190 }
mbedalvaro 1:a4050fee11f7 191
mbedalvaro 12:0de9cd2bced5 192 void rigidLoop::setRegionMotion(float mmix, float mmiy, float mmax, float mmay) { // wrapper for setWallLimits, because there is no more things to do than set this for a unique mass
mbedalvaro 12:0de9cd2bced5 193 // centerMass.setWallLimits(mmix+10, mmiy+10, mmax-10, mmay-10);
mbedalvaro 12:0de9cd2bced5 194 // Use the static method of the pointMass class:
mbedalvaro 12:0de9cd2bced5 195 pointMass::setWallLimits(mmix+10, mmiy+10, mmax-10, mmay-10);
mbedalvaro 1:a4050fee11f7 196 }
mbedalvaro 1:a4050fee11f7 197
mbedalvaro 24:4e52031a495b 198 void rigidLoop::speedFactor(float speedfactor) {
mbedalvaro 24:4e52031a495b 199 // in case of spot following:
mbedalvaro 24:4e52031a495b 200 speedContourFollowing*=speedfactor;
mbedalvaro 24:4e52031a495b 201
mbedalvaro 24:4e52031a495b 202 // in case of bouncing, there are many ways to change the speed (play with the mass, damping or the bouncing force).
mbedalvaro 24:4e52031a495b 203 //centerMass.mass/=speedfactor;//0.0008;//+0.000005*(rand()%100);
mbedalvaro 24:4e52031a495b 204 centerMass.dampMotion/=speedfactor;//0.00045;//0.00015;//00003;
mbedalvaro 24:4e52031a495b 205 //factorBouncingForce=0.0018; // this is because we will use a force on the central mass
mbedalvaro 24:4e52031a495b 206
mbedalvaro 24:4e52031a495b 207
mbedalvaro 24:4e52031a495b 208 }
mbedalvaro 1:a4050fee11f7 209
mbedalvaro 1:a4050fee11f7 210 void rigidLoop::update() {
mbedalvaro 1:a4050fee11f7 211
mbedalvaro 1:a4050fee11f7 212 // (I) process loop geometry: not needed (rigid)
mbedalvaro 1:a4050fee11f7 213 // Just check if the blob touched the borders (only need to do this with the central mass):
mbedalvaro 1:a4050fee11f7 214 blobWallCollision=centerMass.bWallCollision;
mbedalvaro 1:a4050fee11f7 215
mbedalvaro 1:a4050fee11f7 216 // (II) Process sensing buffer and compute light forces:
mbedalvaro 1:a4050fee11f7 217 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 218
mbedalvaro 1:a4050fee11f7 219 // (III) Compute recentering vector (the "penetration vector in fact"), using "first order moment":
mbedalvaro 3:b44ff6de81bd 220 // 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 221 // sum in the circle is 0).
mbedalvaro 12:0de9cd2bced5 222 vector2Df momentVector(0,0);
mbedalvaro 7:0df17f3078bc 223 int counterDarkZone=0; // note: for a VERY strange reason, if I put this on the laserSensingtrajectory class, the program does not work anymore!!
mbedalvaro 7:0df17f3078bc 224 for (int i = 0; i < numPoints; i++) { // note: numPoints should be EVEN
mbedalvaro 1:a4050fee11f7 225 if (displaySensingBuffer.lsdTrajectory[i].lightZone>0) { // this is, we are in a dark zone (better to integrate there, because it is normally smaller)
mbedalvaro 12:0de9cd2bced5 226
mbedalvaro 12:0de9cd2bced5 227 momentVector.x+=(float)bluePrint.scafold[i].x; // note: casting is happening here automatically (unsigned short to float), but I put (float) to remember that types are different
mbedalvaro 12:0de9cd2bced5 228 momentVector.y+=(float)bluePrint.scafold[i].y;
mbedalvaro 12:0de9cd2bced5 229
mbedalvaro 12:0de9cd2bced5 230 // We can also do the following, but ATTENTION: momentVector is of type vector2Df, and scafold[i] of type vector2Dd...
mbedalvaro 12:0de9cd2bced5 231 // momentVector+=bluePrint.scafold[i];// note: no need to do -centerMass.pos, because the scafold is "centered" around 0
mbedalvaro 12:0de9cd2bced5 232
mbedalvaro 7:0df17f3078bc 233 counterDarkZone++;
mbedalvaro 1:a4050fee11f7 234 }
mbedalvaro 1:a4050fee11f7 235 }
mbedalvaro 7:0df17f3078bc 236 momentVector=momentVector*(2*PI/numPoints);
mbedalvaro 7:0df17f3078bc 237 float momentNorm=momentVector.length(); // = 2.R.sin(half_angle) in the direction of the dark zone
mbedalvaro 5:73cd58b58f95 238
mbedalvaro 12:0de9cd2bced5 239 vector2Df unitTowardsLight; // this is the normed vector, pointing towards the light zone
mbedalvaro 2:34157ebbf56b 240 if (momentNorm==0) {
mbedalvaro 7:0df17f3078bc 241 unitTowardsLight.set(0,0);
mbedalvaro 3:b44ff6de81bd 242 recenteringVectorLoop.set(0,0);
mbedalvaro 3:b44ff6de81bd 243 normRecenteringVector=0;
mbedalvaro 3:b44ff6de81bd 244 angleRecenteringVector=0;
mbedalvaro 3:b44ff6de81bd 245 } else {
mbedalvaro 7:0df17f3078bc 246 unitTowardsLight=momentVector/(-1.0*momentNorm);
mbedalvaro 7:0df17f3078bc 247 // Apply correction angle (delay mirrors):
mbedalvaro 7:0df17f3078bc 248 unitTowardsLight.rotateDeg(angleCorrectionForceLoop);
mbedalvaro 7:0df17f3078bc 249
mbedalvaro 7:0df17f3078bc 250 // Compute "recenteringVectorLoop" (in fact, the vector making the spot goes completely away form the dark zone):
mbedalvaro 7:0df17f3078bc 251 float aux=0.5*momentNorm/saccadeRadius; // note: in principle, we ALWAYS have momentNorm < 2.R, so aux < 1
mbedalvaro 7:0df17f3078bc 252 if (aux>1) aux=1.0; // can happen because of the discrete integration!
mbedalvaro 7:0df17f3078bc 253 if (counterDarkZone<=numPoints/2) { // note: numPoints HAS to be EVEN
mbedalvaro 7:0df17f3078bc 254 recenteringVectorLoop=unitTowardsLight*saccadeRadius*(1.0-sqrt(1.0-aux*aux));
mbedalvaro 7:0df17f3078bc 255 } else {
mbedalvaro 7:0df17f3078bc 256 recenteringVectorLoop=unitTowardsLight*saccadeRadius*(1.0+sqrt(1.0-aux*aux));
mbedalvaro 7:0df17f3078bc 257 }
mbedalvaro 7:0df17f3078bc 258
mbedalvaro 7:0df17f3078bc 259
mbedalvaro 3:b44ff6de81bd 260 // Compute redundant quantities (if necessary, for sending through OSC, etc):
mbedalvaro 3:b44ff6de81bd 261 normRecenteringVector=recenteringVectorLoop.length();
mbedalvaro 3:b44ff6de81bd 262 angleRecenteringVector=recenteringVectorLoop.angleDegHoriz();
mbedalvaro 3:b44ff6de81bd 263 }
mbedalvaro 3:b44ff6de81bd 264
mbedalvaro 1:a4050fee11f7 265 // Now, depending on the mode of operation, we have two types of behaviour:
mbedalvaro 12:0de9cd2bced5 266 vector2Df slidingVector; //( need to declare it here because a switch "jump" cannot bypass an initialization)
mbedalvaro 1:a4050fee11f7 267 switch (updateMode) {
mbedalvaro 4:f9d364f10335 268
mbedalvaro 4:f9d364f10335 269 case SPOT_TEST: // this is just for adjusting mirror delays, checking recentering vector, etc:
mbedalvaro 4:f9d364f10335 270 // do nothing for the time being
mbedalvaro 4:f9d364f10335 271 // NOTE: it is not so easy to show the recentering vector without affecting the mirror delay BECAUSE I AM USING THE INTERRUPT METHOD for display.
mbedalvaro 4:f9d364f10335 272 // A possible solution is to instantiate ANOTHER blob with a shape just equal to a line, and rotate it using data from the this blob. Make a new class? Seems a good idea.
mbedalvaro 4:f9d364f10335 273
mbedalvaro 27:1ce994629ffc 274 // (1) current color: change with touch? NO
mbedalvaro 27:1ce994629ffc 275 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 27:1ce994629ffc 276
mbedalvaro 4:f9d364f10335 277 break;
mbedalvaro 1:a4050fee11f7 278
mbedalvaro 1:a4050fee11f7 279 case SPOT_FOLLOWING:
mbedalvaro 1:a4050fee11f7 280 // we need to compute the tangencial "speed":
mbedalvaro 1:a4050fee11f7 281 // vector2D slidingVector;
mbedalvaro 2:34157ebbf56b 282 if (momentNorm>0) {
mbedalvaro 5:73cd58b58f95 283 //momentVector/=momentNorm;
mbedalvaro 3:b44ff6de81bd 284 // We can now compute the sliding vector as:
mbedalvaro 7:0df17f3078bc 285 slidingVector=unitTowardsLight.getRotatedDeg(slidingDirection? 90 : -90) * speedContourFollowing;
mbedalvaro 7:0df17f3078bc 286
mbedalvaro 7:0df17f3078bc 287 // Then the final correcting vector is the sum of sliding plus a recentering vector (with a factor if one want some smothing)
mbedalvaro 3:b44ff6de81bd 288 // 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 7:0df17f3078bc 289
mbedalvaro 7:0df17f3078bc 290 centerMass.pos +=slidingVector+ ( unitTowardsLight*(-1.0*saccadeRadius) + recenteringVectorLoop )* 0.6;
mbedalvaro 7:0df17f3078bc 291 // ATTENTION!!! the REAL radius may be smaller if the mirrors are running fast!!! (hence the last factor, that is not only for "smoothing" the
mbedalvaro 7:0df17f3078bc 292 // re-entry and avoid oscillations).
mbedalvaro 3:b44ff6de81bd 293
mbedalvaro 3:b44ff6de81bd 294 // The following function can help constraining the position "pos", but it also does too much. Do something simpler perhaps?
mbedalvaro 7:0df17f3078bc 295 centerMass.posOld=centerMass.pos; // this is necessary to compute bouceOffWalls using Verlet method... (MAKE A new variable INTEGRATION METHOD?)
mbedalvaro 7:0df17f3078bc 296
mbedalvaro 3:b44ff6de81bd 297 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
mbedalvaro 7:0df17f3078bc 298
mbedalvaro 7:0df17f3078bc 299 if (justSearched) {
mbedalvaro 7:0df17f3078bc 300 saccadeRadius=saccadeRadius_initial;
mbedalvaro 12:0de9cd2bced5 301 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
mbedalvaro 7:0df17f3078bc 302 justSearched=false;
mbedalvaro 7:0df17f3078bc 303 }
mbedalvaro 7:0df17f3078bc 304
mbedalvaro 3:b44ff6de81bd 305
mbedalvaro 2:34157ebbf56b 306 } else {
mbedalvaro 7:0df17f3078bc 307 // not on something. SEARCH MODE (or go to spot_bouncing mode?)
mbedalvaro 15:56a0bf424e8d 308 saccadeRadius+=30; if (saccadeRadius>800) saccadeRadius=saccadeRadius_initial;
mbedalvaro 12:0de9cd2bced5 309 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
mbedalvaro 7:0df17f3078bc 310 justSearched=true;
mbedalvaro 2:34157ebbf56b 311 }
mbedalvaro 1:a4050fee11f7 312
mbedalvaro 27:1ce994629ffc 313 // Change color with touch? NO
mbedalvaro 27:1ce994629ffc 314 // if (displaySensingBuffer.lightTouched)
mbedalvaro 27:1ce994629ffc 315 // transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
mbedalvaro 27:1ce994629ffc 316 // else
mbedalvaro 27:1ce994629ffc 317 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 27:1ce994629ffc 318
mbedalvaro 1:a4050fee11f7 319 break;
mbedalvaro 1:a4050fee11f7 320
mbedalvaro 1:a4050fee11f7 321 case SPOT_BOUNCING:
mbedalvaro 1:a4050fee11f7 322 // this is very simple: we need to give a force to the centralMass that is OPPOSITE to the recenteringVectorLoop vector
mbedalvaro 1:a4050fee11f7 323 centerMass.resetForce();
mbedalvaro 5:73cd58b58f95 324
mbedalvaro 21:bc9b9383f4b6 325 if (displaySensingBuffer.lightTouched) {
mbedalvaro 7:0df17f3078bc 326 // add force; MANY POSSIBILITIES:
mbedalvaro 7:0df17f3078bc 327 // (1) Constant in norm:
mbedalvaro 7:0df17f3078bc 328 //centerMass.addForce(unitTowardsLight*saccadeRadius*factorBouncingForce);
mbedalvaro 7:0df17f3078bc 329 // Exactly what is needed to have an elastic bouncing:
mbedalvaro 7:0df17f3078bc 330
mbedalvaro 7:0df17f3078bc 331 // Proportional to the penetration depth in the dark zone (spring):
mbedalvaro 5:73cd58b58f95 332 centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
mbedalvaro 7:0df17f3078bc 333 // Or proportional to the square (or something else) of the penetration:
mbedalvaro 7:0df17f3078bc 334 //centerMass.addForce(recenteringVectorLoop*normRecenteringVector*factorBouncingForce);
mbedalvaro 7:0df17f3078bc 335
mbedalvaro 5:73cd58b58f95 336 }
mbedalvaro 21:bc9b9383f4b6 337
mbedalvaro 22:d87317d7ca91 338 // Gravity? - side or central attraction?
mbedalvaro 27:1ce994629ffc 339 centerMass.addForce(gravity*centerMass.mass);
mbedalvaro 24:4e52031a495b 340
mbedalvaro 22:d87317d7ca91 341 // or central spring attraction;
mbedalvaro 24:4e52031a495b 342 //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
mbedalvaro 24:4e52031a495b 343 //vector2Df dist=centerMass.pos-centerAttraction;
mbedalvaro 24:4e52031a495b 344 //centerMass.addForce(-dist*centerMass.mass*0.0007);
mbedalvaro 24:4e52031a495b 345
mbedalvaro 24:4e52031a495b 346 // or "radial gravity":
mbedalvaro 27:1ce994629ffc 347 //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
mbedalvaro 27:1ce994629ffc 348 //vector2Df dist=centerMass.pos-centerAttraction;
mbedalvaro 27:1ce994629ffc 349 //centerMass.addForce(dist.normalize()*centerMass.mass*0.5);
mbedalvaro 27:1ce994629ffc 350
mbedalvaro 27:1ce994629ffc 351
mbedalvaro 27:1ce994629ffc 352 // update dynamics for the central mass:
mbedalvaro 27:1ce994629ffc 353 #ifndef VERLET_METHOD
mbedalvaro 27:1ce994629ffc 354 centerMass.addDampingForce(); // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
mbedalvaro 27:1ce994629ffc 355 #endif
mbedalvaro 27:1ce994629ffc 356
mbedalvaro 27:1ce994629ffc 357 centerMass.update(); // unconstrained
mbedalvaro 27:1ce994629ffc 358 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
mbedalvaro 27:1ce994629ffc 359
mbedalvaro 27:1ce994629ffc 360 if (displaySensingBuffer.lightTouched) {
mbedalvaro 27:1ce994629ffc 361 // do collision damping:
mbedalvaro 27:1ce994629ffc 362 centerMass.setSpeed(centerMass.getSpeed()*0.99);
mbedalvaro 27:1ce994629ffc 363 }
mbedalvaro 27:1ce994629ffc 364
mbedalvaro 27:1ce994629ffc 365 // Change color with touch? YES
mbedalvaro 27:1ce994629ffc 366 if (displaySensingBuffer.lightTouched)
mbedalvaro 27:1ce994629ffc 367 transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
mbedalvaro 27:1ce994629ffc 368 else
mbedalvaro 27:1ce994629ffc 369 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 27:1ce994629ffc 370
mbedalvaro 27:1ce994629ffc 371 break;
mbedalvaro 27:1ce994629ffc 372
mbedalvaro 27:1ce994629ffc 373 case SPOT_FOUNTAIN:
mbedalvaro 27:1ce994629ffc 374 // this is very simple: we need to give a force to the centralMass that is OPPOSITE to the recenteringVectorLoop vector
mbedalvaro 27:1ce994629ffc 375 centerMass.resetForce();
mbedalvaro 27:1ce994629ffc 376
mbedalvaro 27:1ce994629ffc 377 if (displaySensingBuffer.lightTouched) {
mbedalvaro 27:1ce994629ffc 378 // add force; MANY POSSIBILITIES:
mbedalvaro 27:1ce994629ffc 379 // (1) Constant in norm:
mbedalvaro 27:1ce994629ffc 380 //centerMass.addForce(unitTowardsLight*saccadeRadius*factorBouncingForce);
mbedalvaro 27:1ce994629ffc 381 // Exactly what is needed to have an elastic bouncing:
mbedalvaro 27:1ce994629ffc 382
mbedalvaro 27:1ce994629ffc 383 // Proportional to the penetration depth in the dark zone (spring):
mbedalvaro 27:1ce994629ffc 384 centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
mbedalvaro 27:1ce994629ffc 385 // Or proportional to the square (or something else) of the penetration:
mbedalvaro 27:1ce994629ffc 386 //centerMass.addForce(recenteringVectorLoop*normRecenteringVector*factorBouncingForce);
mbedalvaro 27:1ce994629ffc 387
mbedalvaro 27:1ce994629ffc 388 }
mbedalvaro 27:1ce994629ffc 389
mbedalvaro 27:1ce994629ffc 390 // RADIAL GRAVITY for the "fountain mode":
mbedalvaro 22:d87317d7ca91 391 vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
mbedalvaro 22:d87317d7ca91 392 vector2Df dist=centerMass.pos-centerAttraction;
mbedalvaro 24:4e52031a495b 393 centerMass.addForce(dist.normalize()*centerMass.mass*0.5);
mbedalvaro 22:d87317d7ca91 394
mbedalvaro 1:a4050fee11f7 395
mbedalvaro 25:74cb85b85fd2 396 // update dynamics for the central mass:
mbedalvaro 1:a4050fee11f7 397 #ifndef VERLET_METHOD
mbedalvaro 1:a4050fee11f7 398 centerMass.addDampingForce(); // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
mbedalvaro 1:a4050fee11f7 399 #endif
mbedalvaro 1:a4050fee11f7 400
mbedalvaro 1:a4050fee11f7 401 centerMass.update(); // unconstrained
mbedalvaro 1:a4050fee11f7 402 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
mbedalvaro 5:73cd58b58f95 403
mbedalvaro 5:73cd58b58f95 404 if (displaySensingBuffer.lightTouched) {
mbedalvaro 5:73cd58b58f95 405 // do collision damping:
mbedalvaro 7:0df17f3078bc 406 centerMass.setSpeed(centerMass.getSpeed()*0.99);
mbedalvaro 5:73cd58b58f95 407 }
mbedalvaro 22:d87317d7ca91 408
mbedalvaro 27:1ce994629ffc 409 // Change color with touch? YES
mbedalvaro 27:1ce994629ffc 410 if (displaySensingBuffer.lightTouched)
mbedalvaro 27:1ce994629ffc 411 transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
mbedalvaro 27:1ce994629ffc 412 else
mbedalvaro 27:1ce994629ffc 413 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 27:1ce994629ffc 414
mbedalvaro 27:1ce994629ffc 415 // In case of "fountain mode", reset position to initial positions and speeds:
mbedalvaro 27:1ce994629ffc 416 if (blobWallCollision) centerMass.setInitialCondition(startCenter, startSpeed);
mbedalvaro 27:1ce994629ffc 417
mbedalvaro 1:a4050fee11f7 418 break;
mbedalvaro 1:a4050fee11f7 419 }
mbedalvaro 17:356ca5690a59 420
mbedalvaro 17:356ca5690a59 421
mbedalvaro 1:a4050fee11f7 422 // change sliding direction (for countour following):
mbedalvaro 1:a4050fee11f7 423 if (blobWallCollision) {
mbedalvaro 7:0df17f3078bc 424 if (wallCounter>5) {
mbedalvaro 1:a4050fee11f7 425 slidingDirection=!slidingDirection;
mbedalvaro 1:a4050fee11f7 426 wallCounter=0;
mbedalvaro 1:a4050fee11f7 427 }
mbedalvaro 1:a4050fee11f7 428 }
mbedalvaro 1:a4050fee11f7 429 wallCounter++;
mbedalvaro 1:a4050fee11f7 430 }
mbedalvaro 1:a4050fee11f7 431
mbedalvaro 1:a4050fee11f7 432
mbedalvaro 1:a4050fee11f7 433 // 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 434 void rigidLoop::draw() {
mbedalvaro 1:a4050fee11f7 435 // for the time being, there is no "opengl" like renderer, so we just copy into the lsdTrajectory:
mbedalvaro 1:a4050fee11f7 436 float cx= centerMass.pos.x;
mbedalvaro 1:a4050fee11f7 437 float cy= centerMass.pos.y;
mbedalvaro 1:a4050fee11f7 438 for (int i = 0; i < numPoints; i++) {
mbedalvaro 1:a4050fee11f7 439 // The shape is drawn by translating the scafold shape (centered on centerMass):
mbedalvaro 12:0de9cd2bced5 440 displaySensingBuffer.lsdTrajectory[i].x= (unsigned short)(bluePrint.scafold[i].x + cx ); // note: it should be an unsigned short!!
mbedalvaro 12:0de9cd2bced5 441 displaySensingBuffer.lsdTrajectory[i].y= (unsigned short)(bluePrint.scafold[i].y + cy );
mbedalvaro 12:0de9cd2bced5 442
mbedalvaro 12:0de9cd2bced5 443 // We can also do this, but ATTENTION: centerMass.pos is a vector2Df, and scafold[i] is a vector2Dd (typecasting?)
mbedalvaro 12:0de9cd2bced5 444 // displaySensingBuffer.lsdTrajectory[i]= bluePrint.scafold[i] + centerMass.pos;
mbedalvaro 12:0de9cd2bced5 445
mbedalvaro 25:74cb85b85fd2 446 //displaySensingBuffer.displayColor=blobColor; // perhaps per point color is not a good idea for the time being...
mbedalvaro 1:a4050fee11f7 447 }
mbedalvaro 25:74cb85b85fd2 448
mbedalvaro 25:74cb85b85fd2 449 // Global color for the whole loop:
mbedalvaro 27:1ce994629ffc 450 displaySensingBuffer.displayColor=transientBlobColor;
mbedalvaro 27:1ce994629ffc 451
mbedalvaro 1:a4050fee11f7 452 }
mbedalvaro 1:a4050fee11f7 453
mbedalvaro 1:a4050fee11f7 454 void rigidLoop::computeBoundingBox() {
mbedalvaro 1:a4050fee11f7 455 }
mbedalvaro 1:a4050fee11f7 456
mbedalvaro 1:a4050fee11f7 457
mbedalvaro 1:a4050fee11f7 458
mbedalvaro 1:a4050fee11f7 459 void rigidLoop::sendDataSpecific() {
mbedalvaro 1:a4050fee11f7 460 char auxstring[10];
mbedalvaro 1:a4050fee11f7 461 myled2=1; // for tests...
mbedalvaro 1:a4050fee11f7 462
mbedalvaro 1:a4050fee11f7 463 // First, set the top address of the message to the ID of the blob (not the name):
mbedalvaro 18:d72935b13858 464 // sprintf(auxstring, "%d", identifier);
mbedalvaro 18:d72935b13858 465 // sendMes.setTopAddress("0");//auxstring);
mbedalvaro 1:a4050fee11f7 466
mbedalvaro 1:a4050fee11f7 467 // ===================== OSC ======================
mbedalvaro 1:a4050fee11f7 468 if (sendOSC) {
mbedalvaro 1:a4050fee11f7 469
mbedalvaro 1:a4050fee11f7 470 // (a) Anchor mass:
mbedalvaro 1:a4050fee11f7 471 if (sendingAnchorPosition) {
mbedalvaro 18:d72935b13858 472 sprintf(auxstring, "/p %d",identifier);
mbedalvaro 18:d72935b13858 473 sendMes.setSubAddress(auxstring);
mbedalvaro 1:a4050fee11f7 474 long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 475 x=(long)(centerMass.pos.x);
mbedalvaro 1:a4050fee11f7 476 y=(long)(centerMass.pos.y);
mbedalvaro 1:a4050fee11f7 477 sendMes.setArgs( "ii", &x, &y);
mbedalvaro 1:a4050fee11f7 478 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 479 }
mbedalvaro 1:a4050fee11f7 480
mbedalvaro 1:a4050fee11f7 481 // (b) data from blob points (this is ONLY FOR TESTS, because the loop is rigid - sending the center is enough)
mbedalvaro 1:a4050fee11f7 482 if (sendingLoopPositions) {
mbedalvaro 1:a4050fee11f7 483 #ifdef SEND_AS_POINTS
mbedalvaro 1:a4050fee11f7 484 long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 485 float cx= centerMass.pos.x;
mbedalvaro 1:a4050fee11f7 486 float cy= centerMass.pos.y;
mbedalvaro 1:a4050fee11f7 487 for (int i = 0; i < numPoints; i++) {
mbedalvaro 2:34157ebbf56b 488 sprintf(auxstring, "/p%d",identifier*10+ i);//20+ i+(identifier-1)*10); // auxstring read as "/p1", "/p2", ...
mbedalvaro 1:a4050fee11f7 489 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 490 x=(long)(bluePrint.scafold[i].x + cx);
mbedalvaro 1:a4050fee11f7 491 y=(long)(bluePrint.scafold[i].y + cy);
mbedalvaro 1:a4050fee11f7 492 sendMes.setArgs( "ii", &x, &y);
mbedalvaro 1:a4050fee11f7 493 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 494 }
mbedalvaro 1:a4050fee11f7 495
mbedalvaro 1:a4050fee11f7 496 #endif
mbedalvaro 1:a4050fee11f7 497 #ifdef SEND_AS_BLOB
mbedalvaro 1:a4050fee11f7 498 sendMes.clearArgs(); // no need, we won't use osc.sendOsc()...
mbedalvaro 1:a4050fee11f7 499 uint8_t blobdata[4*numPoints]; // 2 bytes per coordinate, and 2 coordinates
mbedalvaro 1:a4050fee11f7 500 float cx= centerMass.pos.x;
mbedalvaro 1:a4050fee11f7 501 float cy= centerMass.pos.y;
mbedalvaro 1:a4050fee11f7 502 for (int i = 0; i < numPoints; i++ ) {
mbedalvaro 1:a4050fee11f7 503 // note: massesLoop[i].pos.x is a "float"
mbedalvaro 1:a4050fee11f7 504 uint16_t x=(uint16_t)(bluePrint.scafold[i].x + cx);
mbedalvaro 1:a4050fee11f7 505 blobdata[4*i]=(uint8_t)x>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
mbedalvaro 1:a4050fee11f7 506 blobdata[4*i+1]=(uint8_t)x;
mbedalvaro 1:a4050fee11f7 507
mbedalvaro 1:a4050fee11f7 508 uint16_t y=(uint16_t)(bluePrint.scafold[i].y + cy);
mbedalvaro 1:a4050fee11f7 509 blobdata[4*i+2]=(uint8_t)y>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
mbedalvaro 1:a4050fee11f7 510 blobdata[4*i+3]=(uint8_t)y;
mbedalvaro 1:a4050fee11f7 511 }
mbedalvaro 1:a4050fee11f7 512 osc.sendOscBlob(&(blobdata[0]), 4*numPoints, &sendMes ); // second parameter is osc blob size in bytes
mbedalvaro 1:a4050fee11f7 513 #endif
mbedalvaro 1:a4050fee11f7 514 #ifdef SEND_AS_STRING
mbedalvaro 1:a4050fee11f7 515 sendMes.clearArgs(); // no need, we won't use osc.sendOsc()...
mbedalvaro 1:a4050fee11f7 516 uint8_t blobdata[4*numPoints]; // 2 bytes per coordinate, and 2 coordinates
mbedalvaro 1:a4050fee11f7 517 float cx= centerMass.pos.x;
mbedalvaro 1:a4050fee11f7 518 float cy= centerMass.pos.y;
mbedalvaro 1:a4050fee11f7 519 for (int i = 0; i < numPoints; i++ ) {
mbedalvaro 1:a4050fee11f7 520 // note: massesLoop[i].pos.x is a "float"
mbedalvaro 1:a4050fee11f7 521 uint16_t x=(uint16_t)(bluePrint.scafold[i].x + cx );
mbedalvaro 1:a4050fee11f7 522 blobdata[4*i]=(uint8_t)x>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
mbedalvaro 1:a4050fee11f7 523 blobdata[4*i+1]=(uint8_t)x;
mbedalvaro 1:a4050fee11f7 524
mbedalvaro 1:a4050fee11f7 525 uint16_t y=(uint16_t)(bluePrint.scafold[i].y + cy);
mbedalvaro 1:a4050fee11f7 526 blobdata[4*i+2]=(uint8_t)y>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
mbedalvaro 1:a4050fee11f7 527 blobdata[4*i+3]=(uint8_t)y;
mbedalvaro 1:a4050fee11f7 528 }
mbedalvaro 1:a4050fee11f7 529 osc.sendOscString(blobdata, 4*numPoints, &sendMes ); // second parameter is osc blob size in bytes
mbedalvaro 1:a4050fee11f7 530 #endif
mbedalvaro 1:a4050fee11f7 531 }
mbedalvaro 1:a4050fee11f7 532 if (sendingLoopRegions) {
mbedalvaro 1:a4050fee11f7 533 long x; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 534 for (int i = 0; i < numPoints; i++) {
mbedalvaro 1:a4050fee11f7 535 sprintf(auxstring, "/r%d", i); // auxstring read as "/f1", "/f2", ...
mbedalvaro 1:a4050fee11f7 536 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 537 x=(long)(displaySensingBuffer.lsdTrajectory[i].lightZone>0? 1 : 0);
mbedalvaro 1:a4050fee11f7 538 sendMes.setArgs( "i", &x);
mbedalvaro 1:a4050fee11f7 539 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 540 }
mbedalvaro 1:a4050fee11f7 541 }
mbedalvaro 1:a4050fee11f7 542 if (sendingLoopTouchWall) { // global touch wall for the loop (not per point)
mbedalvaro 1:a4050fee11f7 543 long wall; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 544 sprintf(auxstring, "/bWall");
mbedalvaro 1:a4050fee11f7 545 sendMes.setSubAddress(auxstring);
mbedalvaro 1:a4050fee11f7 546 wall=(long)(blobWallCollision? 1 : 0);
mbedalvaro 1:a4050fee11f7 547 sendMes.setArgs( "i", &wall);
mbedalvaro 1:a4050fee11f7 548 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 549 }
mbedalvaro 1:a4050fee11f7 550
mbedalvaro 1:a4050fee11f7 551 // (d) Light sensing statistics:
mbedalvaro 1:a4050fee11f7 552 if (sendingBlobMaxMin) {
mbedalvaro 1:a4050fee11f7 553 sendMes.setSubAddress("/maxmin");
mbedalvaro 1:a4050fee11f7 554 long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 555 x=(long)(displaySensingBuffer.maxI);
mbedalvaro 1:a4050fee11f7 556 y=(long)(displaySensingBuffer.minI);
mbedalvaro 1:a4050fee11f7 557 sendMes.setArgs( "ii", &x, &y);
mbedalvaro 1:a4050fee11f7 558 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 559 }
mbedalvaro 1:a4050fee11f7 560
mbedalvaro 1:a4050fee11f7 561 // (e) Recentering vector: (note: redundant with sendingLightForce, IF the correction angle is known).
mbedalvaro 1:a4050fee11f7 562 if (sendingRecenteringVector) {
mbedalvaro 1:a4050fee11f7 563 sendMes.setSubAddress("/rvector");
mbedalvaro 1:a4050fee11f7 564 long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 565 x=(long)(recenteringVectorLoop.x);
mbedalvaro 1:a4050fee11f7 566 y=(long)(recenteringVectorLoop.y);
mbedalvaro 1:a4050fee11f7 567 sendMes.setArgs( "ii", &x, &y);
mbedalvaro 1:a4050fee11f7 568 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 569 }
mbedalvaro 1:a4050fee11f7 570 if (sendingRecenteringAngle) {
mbedalvaro 18:d72935b13858 571 sprintf(auxstring, "/v %d",identifier);
mbedalvaro 18:d72935b13858 572 sendMes.setSubAddress(auxstring);
mbedalvaro 1:a4050fee11f7 573 long x; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 574 x=(long)(angleRecenteringVector);
mbedalvaro 1:a4050fee11f7 575 sendMes.setArgs( "i", &x);
mbedalvaro 1:a4050fee11f7 576 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 577 }
mbedalvaro 1:a4050fee11f7 578 if (sendingRecenteringNorm) {
mbedalvaro 1:a4050fee11f7 579 sendMes.setSubAddress("/rnorm");
mbedalvaro 1:a4050fee11f7 580 long x; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 581 x=(long)(normRecenteringVector);
mbedalvaro 1:a4050fee11f7 582 sendMes.setArgs( "i", &x);
mbedalvaro 1:a4050fee11f7 583 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 584 }
mbedalvaro 1:a4050fee11f7 585
mbedalvaro 1:a4050fee11f7 586 if (sendingTouched) {
mbedalvaro 1:a4050fee11f7 587 if (displaySensingBuffer.lightTouched) {
mbedalvaro 1:a4050fee11f7 588 sendMes.clearArgs(); // there are no arguments to send
mbedalvaro 1:a4050fee11f7 589 sendMes.setSubAddress("/touched");
mbedalvaro 1:a4050fee11f7 590 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 591 }
mbedalvaro 1:a4050fee11f7 592 }
mbedalvaro 1:a4050fee11f7 593
mbedalvaro 1:a4050fee11f7 594 } // end of OSC sending per-spot
mbedalvaro 1:a4050fee11f7 595
mbedalvaro 1:a4050fee11f7 596 // ===================== SERIAL ======================
mbedalvaro 1:a4050fee11f7 597 if (sendSerial) {
mbedalvaro 1:a4050fee11f7 598 //.. to do
mbedalvaro 1:a4050fee11f7 599 }
mbedalvaro 1:a4050fee11f7 600
mbedalvaro 1:a4050fee11f7 601 myled2=0; // for tests...
mbedalvaro 1:a4050fee11f7 602 }