Alvaro Cassinelli / Mbed 2 deprecated skinGames_forktest

just a test

Dependencies:   mbed

Fork of scoreLight_Advanced by Alvaro Cassinelli

rigidLoop.cpp@26:c9329c4fc20a, 2012-06-18 (annotated)

Committer:
mbedalvaro
Date:
Mon Jun 18 12:39:28 2012 +0000
Revision:
26:c9329c4fc20a
Parent:
25:74cb85b85fd2
Child:
27:1ce994629ffc
still some color problems - cannot change the green from command

Who changed what in which revision?

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