Alvaro Cassinelli / Mbed 2 deprecated skinGames_forktest

just a test

Dependencies:   mbed

Fork of scoreLight_Advanced by Alvaro Cassinelli

rigidLoop.cpp@20:8e82b95180e7, 2012-04-28 (annotated)

Committer:
mbedalvaro
Date:
Sat Apr 28 15:08:27 2012 +0000
Revision:
20:8e82b95180e7
Parent:
18:d72935b13858
Child:
21:bc9b9383f4b6
Version working at the end of the working day with Daito. Need to:; 1) control speed, size spot, color stifness and number of spots from Processing or Max patch.; 2) check why sometimes bouncing does not send message. ; 3) simplify protocol sending TOP AD...

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 3:b44ff6de81bd 33
mbedalvaro 1:a4050fee11f7 34 // (3) Initialize secondary variables depending on the behaviour mode (may be changed afterwards in real time)
mbedalvaro 1:a4050fee11f7 35
mbedalvaro 1:a4050fee11f7 36 switch (updateMode) {
mbedalvaro 4:f9d364f10335 37
mbedalvaro 4:f9d364f10335 38 case SPOT_TEST:
mbedalvaro 4:f9d364f10335 39 // Name of this kind of spot:
mbedalvaro 4:f9d364f10335 40 sprintf(spotName,"spot_test");
mbedalvaro 4:f9d364f10335 41
mbedalvaro 4:f9d364f10335 42 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 4:f9d364f10335 43 setColor(0x04);
mbedalvaro 4:f9d364f10335 44
mbedalvaro 14:0fc33a3a7b4b 45 saccadeRadius=250;
mbedalvaro 4:f9d364f10335 46
mbedalvaro 4:f9d364f10335 47 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 14:0fc33a3a7b4b 48 // NOTE: number of points in the case of need to compute recentering vector needs to be EVEN
mbedalvaro 14:0fc33a3a7b4b 49 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 16); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 4:f9d364f10335 50
mbedalvaro 4:f9d364f10335 51 // 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 52 massCenter=0.01;
mbedalvaro 4:f9d364f10335 53 dampMotionCenterMass=0.001;
mbedalvaro 4:f9d364f10335 54
mbedalvaro 4:f9d364f10335 55 // 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 56 createLoopFromScafold();
mbedalvaro 4:f9d364f10335 57
mbedalvaro 5:73cd58b58f95 58 // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 5:73cd58b58f95 59 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 5:73cd58b58f95 60 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
mbedalvaro 7:0df17f3078bc 61 displaySensingBuffer.setDelayMirrors(1);
mbedalvaro 7:0df17f3078bc 62 angleCorrectionForceLoop=0;//360.0/bluePrint.scafold.size()/2; // in DEGREES
mbedalvaro 4:f9d364f10335 63
mbedalvaro 4:f9d364f10335 64 break;
mbedalvaro 4:f9d364f10335 65
mbedalvaro 1:a4050fee11f7 66 case SPOT_FOLLOWING:
mbedalvaro 1:a4050fee11f7 67
mbedalvaro 1:a4050fee11f7 68 // Name of this kind of spot:
mbedalvaro 1:a4050fee11f7 69 sprintf(spotName,"rigid_following");
mbedalvaro 1:a4050fee11f7 70
mbedalvaro 3:b44ff6de81bd 71 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 3:b44ff6de81bd 72 setColor(0x04);
mbedalvaro 1:a4050fee11f7 73
mbedalvaro 1:a4050fee11f7 74 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 17:356ca5690a59 75 saccadeRadius=25;
mbedalvaro 12:0de9cd2bced5 76 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 1:a4050fee11f7 77
mbedalvaro 1:a4050fee11f7 78 // 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 79 massCenter=0.01;
mbedalvaro 1:a4050fee11f7 80 dampMotionCenterMass=0.001;
mbedalvaro 1:a4050fee11f7 81
mbedalvaro 4:f9d364f10335 82 // 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 83 createLoopFromScafold();
mbedalvaro 4:f9d364f10335 84
mbedalvaro 5:73cd58b58f95 85 slidingDirection=true; // For contour following (will change direction when touching wall)
mbedalvaro 7:0df17f3078bc 86 speedContourFollowing=1.3*saccadeRadius;
mbedalvaro 7:0df17f3078bc 87 justSearched=false;
mbedalvaro 5:73cd58b58f95 88
mbedalvaro 7:0df17f3078bc 89 // per-blob mirror delay: ONLY USEFUL FOR ELASTIC BLOBS, because otherwise it can be corrected by "angleCorrection"
mbedalvaro 7:0df17f3078bc 90 // (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 5:73cd58b58f95 91 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 7:0df17f3078bc 92 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop.
mbedalvaro 7:0df17f3078bc 93 // 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 94 // optimal value, and finish the correction (fine tunned) with the angle correction (only possible in the case of circular rigid blob).
mbedalvaro 7:0df17f3078bc 95 displaySensingBuffer.setDelayMirrors(3); // this corresponds to an angular correction of -delayMirrors * 360/numPoints
mbedalvaro 15:56a0bf424e8d 96 angleCorrectionForceLoop= -5;// good for ONE spot: -5;// in DEGREES
mbedalvaro 4:f9d364f10335 97
mbedalvaro 1:a4050fee11f7 98 break;
mbedalvaro 1:a4050fee11f7 99
mbedalvaro 1:a4050fee11f7 100 case SPOT_BOUNCING:
mbedalvaro 1:a4050fee11f7 101 // Name of this kind of spot:
mbedalvaro 1:a4050fee11f7 102 sprintf(spotName,"rigid_bouncing");
mbedalvaro 1:a4050fee11f7 103
mbedalvaro 3:b44ff6de81bd 104 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 3:b44ff6de81bd 105 setColor(0x04);
mbedalvaro 1:a4050fee11f7 106
mbedalvaro 7:0df17f3078bc 107 saccadeRadius=85;
mbedalvaro 1:a4050fee11f7 108 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 12:0de9cd2bced5 109 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 1:a4050fee11f7 110
mbedalvaro 1:a4050fee11f7 111 // Numeric parameters for the simulated mechanical system:
mbedalvaro 7:0df17f3078bc 112 massCenter=0.0012;
mbedalvaro 7:0df17f3078bc 113 dampMotionCenterMass=0.0005;//0.00015;//00003;
mbedalvaro 7:0df17f3078bc 114 factorBouncingForce=0.0018; // this is because we will use a force on the central mass
mbedalvaro 1:a4050fee11f7 115
mbedalvaro 4:f9d364f10335 116 // 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 117 createLoopFromScafold();
mbedalvaro 4:f9d364f10335 118
mbedalvaro 5:73cd58b58f95 119 // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 5:73cd58b58f95 120 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 5:73cd58b58f95 121 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
mbedalvaro 7:0df17f3078bc 122 displaySensingBuffer.setDelayMirrors(3);
mbedalvaro 7:0df17f3078bc 123 angleCorrectionForceLoop=-5;// in degrees
mbedalvaro 1:a4050fee11f7 124
mbedalvaro 1:a4050fee11f7 125 break;
mbedalvaro 1:a4050fee11f7 126 }
mbedalvaro 7:0df17f3078bc 127
mbedalvaro 7:0df17f3078bc 128 saccadeRadius_initial=saccadeRadius; // this is for search mode for instance.
mbedalvaro 12:0de9cd2bced5 129
mbedalvaro 12:0de9cd2bced5 130 // 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 131 setRegionMotion(MIN_AD_MIRRORS+saccadeRadius, MIN_AD_MIRRORS+saccadeRadius, MAX_AD_MIRRORS-saccadeRadius, MAX_AD_MIRRORS-saccadeRadius);
mbedalvaro 12:0de9cd2bced5 132
mbedalvaro 3:b44ff6de81bd 133
mbedalvaro 1:a4050fee11f7 134 }
mbedalvaro 1:a4050fee11f7 135
mbedalvaro 1:a4050fee11f7 136
mbedalvaro 1:a4050fee11f7 137 void rigidLoop::initSizeBlob(int _numPoints) {
mbedalvaro 1:a4050fee11f7 138 // Iinitialize blob size (number of points for the loop, as well as other structures such as lsdTrajectory)
mbedalvaro 1:a4050fee11f7 139 numPoints=_numPoints;
mbedalvaro 1:a4050fee11f7 140
mbedalvaro 1:a4050fee11f7 141 // Sensing and Display trajectory:
mbedalvaro 1:a4050fee11f7 142 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 143 }
mbedalvaro 1:a4050fee11f7 144
mbedalvaro 1:a4050fee11f7 145 void rigidLoop::createLoopFromScafold(void) {
mbedalvaro 1:a4050fee11f7 146
mbedalvaro 1:a4050fee11f7 147 initSizeBlob(bluePrint.scafold.size()); // very simple here (only need to set the size of the lsd buffer)
mbedalvaro 1:a4050fee11f7 148
mbedalvaro 1:a4050fee11f7 149 centerMass.mass=massCenter;
mbedalvaro 1:a4050fee11f7 150 centerMass.dampMotion = dampMotionCenterMass;
mbedalvaro 1:a4050fee11f7 151
mbedalvaro 1:a4050fee11f7 152 // note: the following may not be required in case of contour following:
mbedalvaro 2:34157ebbf56b 153 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 154 centerMass.setInitialCondition(startCenter, startSpeed);
mbedalvaro 1:a4050fee11f7 155 // centerMass.setInitialCondition(2047.0, 2047.0,0.0,0.0);
mbedalvaro 1:a4050fee11f7 156 }
mbedalvaro 1:a4050fee11f7 157
mbedalvaro 12:0de9cd2bced5 158 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 159 // centerMass.setWallLimits(mmix+10, mmiy+10, mmax-10, mmay-10);
mbedalvaro 12:0de9cd2bced5 160 // Use the static method of the pointMass class:
mbedalvaro 12:0de9cd2bced5 161 pointMass::setWallLimits(mmix+10, mmiy+10, mmax-10, mmay-10);
mbedalvaro 1:a4050fee11f7 162 }
mbedalvaro 1:a4050fee11f7 163
mbedalvaro 1:a4050fee11f7 164
mbedalvaro 1:a4050fee11f7 165 void rigidLoop::update() {
mbedalvaro 1:a4050fee11f7 166
mbedalvaro 1:a4050fee11f7 167 // (I) process loop geometry: not needed (rigid)
mbedalvaro 1:a4050fee11f7 168 // Just check if the blob touched the borders (only need to do this with the central mass):
mbedalvaro 1:a4050fee11f7 169 blobWallCollision=centerMass.bWallCollision;
mbedalvaro 1:a4050fee11f7 170
mbedalvaro 1:a4050fee11f7 171 // (II) Process sensing buffer and compute light forces:
mbedalvaro 1:a4050fee11f7 172 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 173
mbedalvaro 1:a4050fee11f7 174 // (III) Compute recentering vector (the "penetration vector in fact"), using "first order moment":
mbedalvaro 3:b44ff6de81bd 175 // 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 176 // sum in the circle is 0).
mbedalvaro 12:0de9cd2bced5 177 vector2Df momentVector(0,0);
mbedalvaro 7:0df17f3078bc 178 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 179 for (int i = 0; i < numPoints; i++) { // note: numPoints should be EVEN
mbedalvaro 1:a4050fee11f7 180 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 181
mbedalvaro 12:0de9cd2bced5 182 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 183 momentVector.y+=(float)bluePrint.scafold[i].y;
mbedalvaro 12:0de9cd2bced5 184
mbedalvaro 12:0de9cd2bced5 185 // We can also do the following, but ATTENTION: momentVector is of type vector2Df, and scafold[i] of type vector2Dd...
mbedalvaro 12:0de9cd2bced5 186 // momentVector+=bluePrint.scafold[i];// note: no need to do -centerMass.pos, because the scafold is "centered" around 0
mbedalvaro 12:0de9cd2bced5 187
mbedalvaro 7:0df17f3078bc 188 counterDarkZone++;
mbedalvaro 1:a4050fee11f7 189 }
mbedalvaro 1:a4050fee11f7 190 }
mbedalvaro 7:0df17f3078bc 191 momentVector=momentVector*(2*PI/numPoints);
mbedalvaro 7:0df17f3078bc 192 float momentNorm=momentVector.length(); // = 2.R.sin(half_angle) in the direction of the dark zone
mbedalvaro 5:73cd58b58f95 193
mbedalvaro 12:0de9cd2bced5 194 vector2Df unitTowardsLight; // this is the normed vector, pointing towards the light zone
mbedalvaro 2:34157ebbf56b 195 if (momentNorm==0) {
mbedalvaro 7:0df17f3078bc 196 unitTowardsLight.set(0,0);
mbedalvaro 3:b44ff6de81bd 197 recenteringVectorLoop.set(0,0);
mbedalvaro 3:b44ff6de81bd 198 normRecenteringVector=0;
mbedalvaro 3:b44ff6de81bd 199 angleRecenteringVector=0;
mbedalvaro 3:b44ff6de81bd 200 } else {
mbedalvaro 7:0df17f3078bc 201 unitTowardsLight=momentVector/(-1.0*momentNorm);
mbedalvaro 7:0df17f3078bc 202 // Apply correction angle (delay mirrors):
mbedalvaro 7:0df17f3078bc 203 unitTowardsLight.rotateDeg(angleCorrectionForceLoop);
mbedalvaro 7:0df17f3078bc 204
mbedalvaro 7:0df17f3078bc 205 // Compute "recenteringVectorLoop" (in fact, the vector making the spot goes completely away form the dark zone):
mbedalvaro 7:0df17f3078bc 206 float aux=0.5*momentNorm/saccadeRadius; // note: in principle, we ALWAYS have momentNorm < 2.R, so aux < 1
mbedalvaro 7:0df17f3078bc 207 if (aux>1) aux=1.0; // can happen because of the discrete integration!
mbedalvaro 7:0df17f3078bc 208 if (counterDarkZone<=numPoints/2) { // note: numPoints HAS to be EVEN
mbedalvaro 7:0df17f3078bc 209 recenteringVectorLoop=unitTowardsLight*saccadeRadius*(1.0-sqrt(1.0-aux*aux));
mbedalvaro 7:0df17f3078bc 210 } else {
mbedalvaro 7:0df17f3078bc 211 recenteringVectorLoop=unitTowardsLight*saccadeRadius*(1.0+sqrt(1.0-aux*aux));
mbedalvaro 7:0df17f3078bc 212 }
mbedalvaro 7:0df17f3078bc 213
mbedalvaro 7:0df17f3078bc 214
mbedalvaro 3:b44ff6de81bd 215 // Compute redundant quantities (if necessary, for sending through OSC, etc):
mbedalvaro 3:b44ff6de81bd 216 normRecenteringVector=recenteringVectorLoop.length();
mbedalvaro 3:b44ff6de81bd 217 angleRecenteringVector=recenteringVectorLoop.angleDegHoriz();
mbedalvaro 3:b44ff6de81bd 218 }
mbedalvaro 3:b44ff6de81bd 219
mbedalvaro 1:a4050fee11f7 220 // Now, depending on the mode of operation, we have two types of behaviour:
mbedalvaro 12:0de9cd2bced5 221 vector2Df slidingVector; //( need to declare it here because a switch "jump" cannot bypass an initialization)
mbedalvaro 1:a4050fee11f7 222 switch (updateMode) {
mbedalvaro 4:f9d364f10335 223
mbedalvaro 4:f9d364f10335 224 case SPOT_TEST: // this is just for adjusting mirror delays, checking recentering vector, etc:
mbedalvaro 4:f9d364f10335 225 // do nothing for the time being
mbedalvaro 4:f9d364f10335 226 // 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 227 // 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 228
mbedalvaro 4:f9d364f10335 229 break;
mbedalvaro 1:a4050fee11f7 230
mbedalvaro 1:a4050fee11f7 231 case SPOT_FOLLOWING:
mbedalvaro 1:a4050fee11f7 232 // we need to compute the tangencial "speed":
mbedalvaro 1:a4050fee11f7 233 // vector2D slidingVector;
mbedalvaro 2:34157ebbf56b 234 if (momentNorm>0) {
mbedalvaro 5:73cd58b58f95 235 //momentVector/=momentNorm;
mbedalvaro 3:b44ff6de81bd 236 // We can now compute the sliding vector as:
mbedalvaro 7:0df17f3078bc 237 slidingVector=unitTowardsLight.getRotatedDeg(slidingDirection? 90 : -90) * speedContourFollowing;
mbedalvaro 7:0df17f3078bc 238
mbedalvaro 7:0df17f3078bc 239 // 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 240 // 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 241
mbedalvaro 7:0df17f3078bc 242 centerMass.pos +=slidingVector+ ( unitTowardsLight*(-1.0*saccadeRadius) + recenteringVectorLoop )* 0.6;
mbedalvaro 7:0df17f3078bc 243 // 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 244 // re-entry and avoid oscillations).
mbedalvaro 3:b44ff6de81bd 245
mbedalvaro 3:b44ff6de81bd 246 // The following function can help constraining the position "pos", but it also does too much. Do something simpler perhaps?
mbedalvaro 7:0df17f3078bc 247 centerMass.posOld=centerMass.pos; // this is necessary to compute bouceOffWalls using Verlet method... (MAKE A new variable INTEGRATION METHOD?)
mbedalvaro 7:0df17f3078bc 248
mbedalvaro 3:b44ff6de81bd 249 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
mbedalvaro 7:0df17f3078bc 250
mbedalvaro 7:0df17f3078bc 251 if (justSearched) {
mbedalvaro 7:0df17f3078bc 252 saccadeRadius=saccadeRadius_initial;
mbedalvaro 12:0de9cd2bced5 253 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
mbedalvaro 7:0df17f3078bc 254 justSearched=false;
mbedalvaro 7:0df17f3078bc 255 }
mbedalvaro 7:0df17f3078bc 256
mbedalvaro 3:b44ff6de81bd 257
mbedalvaro 2:34157ebbf56b 258 } else {
mbedalvaro 7:0df17f3078bc 259 // not on something. SEARCH MODE (or go to spot_bouncing mode?)
mbedalvaro 15:56a0bf424e8d 260 saccadeRadius+=30; if (saccadeRadius>800) saccadeRadius=saccadeRadius_initial;
mbedalvaro 12:0de9cd2bced5 261 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
mbedalvaro 7:0df17f3078bc 262 justSearched=true;
mbedalvaro 2:34157ebbf56b 263 }
mbedalvaro 1:a4050fee11f7 264
mbedalvaro 1:a4050fee11f7 265 break;
mbedalvaro 1:a4050fee11f7 266
mbedalvaro 1:a4050fee11f7 267 case SPOT_BOUNCING:
mbedalvaro 1:a4050fee11f7 268 // this is very simple: we need to give a force to the centralMass that is OPPOSITE to the recenteringVectorLoop vector
mbedalvaro 1:a4050fee11f7 269 centerMass.resetForce();
mbedalvaro 5:73cd58b58f95 270
mbedalvaro 5:73cd58b58f95 271 if (displaySensingBuffer.lightTouched) {
mbedalvaro 7:0df17f3078bc 272 // add force; MANY POSSIBILITIES:
mbedalvaro 7:0df17f3078bc 273 // (1) Constant in norm:
mbedalvaro 7:0df17f3078bc 274 //centerMass.addForce(unitTowardsLight*saccadeRadius*factorBouncingForce);
mbedalvaro 7:0df17f3078bc 275 // Exactly what is needed to have an elastic bouncing:
mbedalvaro 7:0df17f3078bc 276
mbedalvaro 7:0df17f3078bc 277 // Proportional to the penetration depth in the dark zone (spring):
mbedalvaro 5:73cd58b58f95 278 centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
mbedalvaro 7:0df17f3078bc 279 // Or proportional to the square (or something else) of the penetration:
mbedalvaro 7:0df17f3078bc 280 //centerMass.addForce(recenteringVectorLoop*normRecenteringVector*factorBouncingForce);
mbedalvaro 7:0df17f3078bc 281
mbedalvaro 5:73cd58b58f95 282 }
mbedalvaro 1:a4050fee11f7 283
mbedalvaro 1:a4050fee11f7 284 // update dynamics for the central mass::
mbedalvaro 1:a4050fee11f7 285 #ifndef VERLET_METHOD
mbedalvaro 1:a4050fee11f7 286 centerMass.addDampingForce(); // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
mbedalvaro 1:a4050fee11f7 287 #endif
mbedalvaro 1:a4050fee11f7 288
mbedalvaro 1:a4050fee11f7 289 centerMass.update(); // unconstrained
mbedalvaro 1:a4050fee11f7 290 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
mbedalvaro 5:73cd58b58f95 291
mbedalvaro 5:73cd58b58f95 292 if (displaySensingBuffer.lightTouched) {
mbedalvaro 5:73cd58b58f95 293 // do collision damping:
mbedalvaro 7:0df17f3078bc 294 centerMass.setSpeed(centerMass.getSpeed()*0.99);
mbedalvaro 5:73cd58b58f95 295 }
mbedalvaro 5:73cd58b58f95 296
mbedalvaro 1:a4050fee11f7 297
mbedalvaro 1:a4050fee11f7 298 break;
mbedalvaro 1:a4050fee11f7 299
mbedalvaro 1:a4050fee11f7 300 }
mbedalvaro 1:a4050fee11f7 301
mbedalvaro 1:a4050fee11f7 302 // OTHER PARTICULAR THINGS:
mbedalvaro 17:356ca5690a59 303 if (displaySensingBuffer.lightTouched) setColor(0x02); // green on
mbedalvaro 17:356ca5690a59 304 else setColor(0x00); // green off
mbedalvaro 17:356ca5690a59 305
mbedalvaro 17:356ca5690a59 306
mbedalvaro 1:a4050fee11f7 307 // change sliding direction (for countour following):
mbedalvaro 1:a4050fee11f7 308 if (blobWallCollision) {
mbedalvaro 7:0df17f3078bc 309 if (wallCounter>5) {
mbedalvaro 1:a4050fee11f7 310 slidingDirection=!slidingDirection;
mbedalvaro 1:a4050fee11f7 311 wallCounter=0;
mbedalvaro 1:a4050fee11f7 312 }
mbedalvaro 1:a4050fee11f7 313 }
mbedalvaro 1:a4050fee11f7 314 wallCounter++;
mbedalvaro 1:a4050fee11f7 315 }
mbedalvaro 1:a4050fee11f7 316
mbedalvaro 1:a4050fee11f7 317
mbedalvaro 1:a4050fee11f7 318 // 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 319 void rigidLoop::draw() {
mbedalvaro 1:a4050fee11f7 320 // for the time being, there is no "opengl" like renderer, so we just copy into the lsdTrajectory:
mbedalvaro 1:a4050fee11f7 321 float cx= centerMass.pos.x;
mbedalvaro 1:a4050fee11f7 322 float cy= centerMass.pos.y;
mbedalvaro 1:a4050fee11f7 323 for (int i = 0; i < numPoints; i++) {
mbedalvaro 1:a4050fee11f7 324 // The shape is drawn by translating the scafold shape (centered on centerMass):
mbedalvaro 12:0de9cd2bced5 325 displaySensingBuffer.lsdTrajectory[i].x= (unsigned short)(bluePrint.scafold[i].x + cx ); // note: it should be an unsigned short!!
mbedalvaro 12:0de9cd2bced5 326 displaySensingBuffer.lsdTrajectory[i].y= (unsigned short)(bluePrint.scafold[i].y + cy );
mbedalvaro 12:0de9cd2bced5 327
mbedalvaro 12:0de9cd2bced5 328 // We can also do this, but ATTENTION: centerMass.pos is a vector2Df, and scafold[i] is a vector2Dd (typecasting?)
mbedalvaro 12:0de9cd2bced5 329 // displaySensingBuffer.lsdTrajectory[i]= bluePrint.scafold[i] + centerMass.pos;
mbedalvaro 12:0de9cd2bced5 330
mbedalvaro 17:356ca5690a59 331 displaySensingBuffer.displayColor=blobColor; // perhaps per point color is not a good idea for the time being...
mbedalvaro 1:a4050fee11f7 332 }
mbedalvaro 1:a4050fee11f7 333 // global color for the whole loop:
mbedalvaro 1:a4050fee11f7 334 displaySensingBuffer.displayColor=blobColor;
mbedalvaro 1:a4050fee11f7 335 }
mbedalvaro 1:a4050fee11f7 336
mbedalvaro 1:a4050fee11f7 337 void rigidLoop::computeBoundingBox() {
mbedalvaro 1:a4050fee11f7 338 }
mbedalvaro 1:a4050fee11f7 339
mbedalvaro 1:a4050fee11f7 340
mbedalvaro 1:a4050fee11f7 341
mbedalvaro 1:a4050fee11f7 342 void rigidLoop::sendDataSpecific() {
mbedalvaro 1:a4050fee11f7 343 char auxstring[10];
mbedalvaro 1:a4050fee11f7 344 myled2=1; // for tests...
mbedalvaro 1:a4050fee11f7 345
mbedalvaro 1:a4050fee11f7 346 // First, set the top address of the message to the ID of the blob (not the name):
mbedalvaro 18:d72935b13858 347 // sprintf(auxstring, "%d", identifier);
mbedalvaro 18:d72935b13858 348 // sendMes.setTopAddress("0");//auxstring);
mbedalvaro 1:a4050fee11f7 349
mbedalvaro 1:a4050fee11f7 350 // ===================== OSC ======================
mbedalvaro 1:a4050fee11f7 351 if (sendOSC) {
mbedalvaro 1:a4050fee11f7 352
mbedalvaro 1:a4050fee11f7 353 // (a) Anchor mass:
mbedalvaro 1:a4050fee11f7 354 if (sendingAnchorPosition) {
mbedalvaro 18:d72935b13858 355 sprintf(auxstring, "/p %d",identifier);
mbedalvaro 18:d72935b13858 356 sendMes.setSubAddress(auxstring);
mbedalvaro 1:a4050fee11f7 357 long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 358 x=(long)(centerMass.pos.x);
mbedalvaro 1:a4050fee11f7 359 y=(long)(centerMass.pos.y);
mbedalvaro 1:a4050fee11f7 360 sendMes.setArgs( "ii", &x, &y);
mbedalvaro 1:a4050fee11f7 361 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 362 }
mbedalvaro 1:a4050fee11f7 363
mbedalvaro 1:a4050fee11f7 364 // (b) data from blob points (this is ONLY FOR TESTS, because the loop is rigid - sending the center is enough)
mbedalvaro 1:a4050fee11f7 365 if (sendingLoopPositions) {
mbedalvaro 1:a4050fee11f7 366 #ifdef SEND_AS_POINTS
mbedalvaro 1:a4050fee11f7 367 long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 368 float cx= centerMass.pos.x;
mbedalvaro 1:a4050fee11f7 369 float cy= centerMass.pos.y;
mbedalvaro 1:a4050fee11f7 370 for (int i = 0; i < numPoints; i++) {
mbedalvaro 2:34157ebbf56b 371 sprintf(auxstring, "/p%d",identifier*10+ i);//20+ i+(identifier-1)*10); // auxstring read as "/p1", "/p2", ...
mbedalvaro 1:a4050fee11f7 372 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 373 x=(long)(bluePrint.scafold[i].x + cx);
mbedalvaro 1:a4050fee11f7 374 y=(long)(bluePrint.scafold[i].y + cy);
mbedalvaro 1:a4050fee11f7 375 sendMes.setArgs( "ii", &x, &y);
mbedalvaro 1:a4050fee11f7 376 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 377 }
mbedalvaro 1:a4050fee11f7 378
mbedalvaro 1:a4050fee11f7 379 #endif
mbedalvaro 1:a4050fee11f7 380 #ifdef SEND_AS_BLOB
mbedalvaro 1:a4050fee11f7 381 sendMes.clearArgs(); // no need, we won't use osc.sendOsc()...
mbedalvaro 1:a4050fee11f7 382 uint8_t blobdata[4*numPoints]; // 2 bytes per coordinate, and 2 coordinates
mbedalvaro 1:a4050fee11f7 383 float cx= centerMass.pos.x;
mbedalvaro 1:a4050fee11f7 384 float cy= centerMass.pos.y;
mbedalvaro 1:a4050fee11f7 385 for (int i = 0; i < numPoints; i++ ) {
mbedalvaro 1:a4050fee11f7 386 // note: massesLoop[i].pos.x is a "float"
mbedalvaro 1:a4050fee11f7 387 uint16_t x=(uint16_t)(bluePrint.scafold[i].x + cx);
mbedalvaro 1:a4050fee11f7 388 blobdata[4*i]=(uint8_t)x>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
mbedalvaro 1:a4050fee11f7 389 blobdata[4*i+1]=(uint8_t)x;
mbedalvaro 1:a4050fee11f7 390
mbedalvaro 1:a4050fee11f7 391 uint16_t y=(uint16_t)(bluePrint.scafold[i].y + cy);
mbedalvaro 1:a4050fee11f7 392 blobdata[4*i+2]=(uint8_t)y>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
mbedalvaro 1:a4050fee11f7 393 blobdata[4*i+3]=(uint8_t)y;
mbedalvaro 1:a4050fee11f7 394 }
mbedalvaro 1:a4050fee11f7 395 osc.sendOscBlob(&(blobdata[0]), 4*numPoints, &sendMes ); // second parameter is osc blob size in bytes
mbedalvaro 1:a4050fee11f7 396 #endif
mbedalvaro 1:a4050fee11f7 397 #ifdef SEND_AS_STRING
mbedalvaro 1:a4050fee11f7 398 sendMes.clearArgs(); // no need, we won't use osc.sendOsc()...
mbedalvaro 1:a4050fee11f7 399 uint8_t blobdata[4*numPoints]; // 2 bytes per coordinate, and 2 coordinates
mbedalvaro 1:a4050fee11f7 400 float cx= centerMass.pos.x;
mbedalvaro 1:a4050fee11f7 401 float cy= centerMass.pos.y;
mbedalvaro 1:a4050fee11f7 402 for (int i = 0; i < numPoints; i++ ) {
mbedalvaro 1:a4050fee11f7 403 // note: massesLoop[i].pos.x is a "float"
mbedalvaro 1:a4050fee11f7 404 uint16_t x=(uint16_t)(bluePrint.scafold[i].x + cx );
mbedalvaro 1:a4050fee11f7 405 blobdata[4*i]=(uint8_t)x>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
mbedalvaro 1:a4050fee11f7 406 blobdata[4*i+1]=(uint8_t)x;
mbedalvaro 1:a4050fee11f7 407
mbedalvaro 1:a4050fee11f7 408 uint16_t y=(uint16_t)(bluePrint.scafold[i].y + cy);
mbedalvaro 1:a4050fee11f7 409 blobdata[4*i+2]=(uint8_t)y>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
mbedalvaro 1:a4050fee11f7 410 blobdata[4*i+3]=(uint8_t)y;
mbedalvaro 1:a4050fee11f7 411 }
mbedalvaro 1:a4050fee11f7 412 osc.sendOscString(blobdata, 4*numPoints, &sendMes ); // second parameter is osc blob size in bytes
mbedalvaro 1:a4050fee11f7 413 #endif
mbedalvaro 1:a4050fee11f7 414 }
mbedalvaro 1:a4050fee11f7 415 if (sendingLoopRegions) {
mbedalvaro 1:a4050fee11f7 416 long x; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 417 for (int i = 0; i < numPoints; i++) {
mbedalvaro 1:a4050fee11f7 418 sprintf(auxstring, "/r%d", i); // auxstring read as "/f1", "/f2", ...
mbedalvaro 1:a4050fee11f7 419 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 420 x=(long)(displaySensingBuffer.lsdTrajectory[i].lightZone>0? 1 : 0);
mbedalvaro 1:a4050fee11f7 421 sendMes.setArgs( "i", &x);
mbedalvaro 1:a4050fee11f7 422 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 423 }
mbedalvaro 1:a4050fee11f7 424 }
mbedalvaro 1:a4050fee11f7 425 if (sendingLoopTouchWall) { // global touch wall for the loop (not per point)
mbedalvaro 1:a4050fee11f7 426 long wall; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 427 sprintf(auxstring, "/bWall");
mbedalvaro 1:a4050fee11f7 428 sendMes.setSubAddress(auxstring);
mbedalvaro 1:a4050fee11f7 429 wall=(long)(blobWallCollision? 1 : 0);
mbedalvaro 1:a4050fee11f7 430 sendMes.setArgs( "i", &wall);
mbedalvaro 1:a4050fee11f7 431 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 432 }
mbedalvaro 1:a4050fee11f7 433
mbedalvaro 1:a4050fee11f7 434 // (d) Light sensing statistics:
mbedalvaro 1:a4050fee11f7 435 if (sendingBlobMaxMin) {
mbedalvaro 1:a4050fee11f7 436 sendMes.setSubAddress("/maxmin");
mbedalvaro 1:a4050fee11f7 437 long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 438 x=(long)(displaySensingBuffer.maxI);
mbedalvaro 1:a4050fee11f7 439 y=(long)(displaySensingBuffer.minI);
mbedalvaro 1:a4050fee11f7 440 sendMes.setArgs( "ii", &x, &y);
mbedalvaro 1:a4050fee11f7 441 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 442 }
mbedalvaro 1:a4050fee11f7 443
mbedalvaro 1:a4050fee11f7 444 // (e) Recentering vector: (note: redundant with sendingLightForce, IF the correction angle is known).
mbedalvaro 1:a4050fee11f7 445 if (sendingRecenteringVector) {
mbedalvaro 1:a4050fee11f7 446 sendMes.setSubAddress("/rvector");
mbedalvaro 1:a4050fee11f7 447 long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 448 x=(long)(recenteringVectorLoop.x);
mbedalvaro 1:a4050fee11f7 449 y=(long)(recenteringVectorLoop.y);
mbedalvaro 1:a4050fee11f7 450 sendMes.setArgs( "ii", &x, &y);
mbedalvaro 1:a4050fee11f7 451 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 452 }
mbedalvaro 1:a4050fee11f7 453 if (sendingRecenteringAngle) {
mbedalvaro 18:d72935b13858 454 sprintf(auxstring, "/v %d",identifier);
mbedalvaro 18:d72935b13858 455 sendMes.setSubAddress(auxstring);
mbedalvaro 1:a4050fee11f7 456 long x; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 457 x=(long)(angleRecenteringVector);
mbedalvaro 1:a4050fee11f7 458 sendMes.setArgs( "i", &x);
mbedalvaro 1:a4050fee11f7 459 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 460 }
mbedalvaro 1:a4050fee11f7 461 if (sendingRecenteringNorm) {
mbedalvaro 1:a4050fee11f7 462 sendMes.setSubAddress("/rnorm");
mbedalvaro 1:a4050fee11f7 463 long x; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 464 x=(long)(normRecenteringVector);
mbedalvaro 1:a4050fee11f7 465 sendMes.setArgs( "i", &x);
mbedalvaro 1:a4050fee11f7 466 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 467 }
mbedalvaro 1:a4050fee11f7 468
mbedalvaro 1:a4050fee11f7 469 if (sendingTouched) {
mbedalvaro 1:a4050fee11f7 470 if (displaySensingBuffer.lightTouched) {
mbedalvaro 1:a4050fee11f7 471 sendMes.clearArgs(); // there are no arguments to send
mbedalvaro 1:a4050fee11f7 472 sendMes.setSubAddress("/touched");
mbedalvaro 1:a4050fee11f7 473 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 474 }
mbedalvaro 1:a4050fee11f7 475 }
mbedalvaro 1:a4050fee11f7 476
mbedalvaro 1:a4050fee11f7 477 } // end of OSC sending per-spot
mbedalvaro 1:a4050fee11f7 478
mbedalvaro 1:a4050fee11f7 479 // ===================== SERIAL ======================
mbedalvaro 1:a4050fee11f7 480 if (sendSerial) {
mbedalvaro 1:a4050fee11f7 481 //.. to do
mbedalvaro 1:a4050fee11f7 482 }
mbedalvaro 1:a4050fee11f7 483
mbedalvaro 1:a4050fee11f7 484 myled2=0; // for tests...
mbedalvaro 1:a4050fee11f7 485 }