Alvaro Cassinelli / Mbed 2 deprecated skinGames_forktest

just a test

Dependencies:   mbed

Fork of scoreLight_Advanced by Alvaro Cassinelli

rigidLoop.cpp@7:0df17f3078bc, 2012-04-07 (annotated)

Committer:
mbedalvaro
Date:
Sat Apr 07 14:46:51 2012 +0000
Revision:
7:0df17f3078bc
Parent:
5:73cd58b58f95
Child:
12:0de9cd2bced5
working contour following and bouncing. The blob mode is not so nice now, but this is because the UPDATE TIME should be a funciton of the number of points and the complexity of update (very simple in case of bouncing/contour following for rigid loops, ...

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