Alvaro Cassinelli / Mbed 2 deprecated skinGames_forktest

just a test

Dependencies:   mbed

Fork of scoreLight_Advanced by Alvaro Cassinelli

rigidLoop.cpp@12:0de9cd2bced5, 2012-04-12 (annotated)

Committer:
mbedalvaro
Date:
Thu Apr 12 08:38:44 2012 +0000
Revision:
12:0de9cd2bced5
Parent:
7:0df17f3078bc
Child:
14:0fc33a3a7b4b
1) template class vector works fine. This way, I have more memory (by defining a rigid scafold using unsigned shorts). I can now make an elastic blob of at least 50 points. ; ; To do: double buffering (this will again take memory, but it may be ok becaus...

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