Alvaro Cassinelli / Mbed 2 deprecated skinGames_forktest

just a test

Dependencies:   mbed

Fork of scoreLight_Advanced by Alvaro Cassinelli

rigidLoop.cpp@28:44b7b6e35548, 2012-06-19 (annotated)

Committer:
mbedalvaro
Date:
Tue Jun 19 11:22:46 2012 +0000
Revision:
28:44b7b6e35548
Parent:
27:1ce994629ffc
Child:
29:2fc8c12822eb
version almost ready for AsahiTV;

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