Alvaro Cassinelli / Mbed 2 deprecated skinGames_forktest

just a test

Dependencies:   mbed

Fork of scoreLight_Advanced by Alvaro Cassinelli

rigidLoop.cpp@46:90516893793a, 2014-04-01 (annotated)

Committer:
mbedalvaro
Date:
Tue Apr 01 11:34:37 2014 +0000
Revision:
46:90516893793a
Parent:
45:a3b984a79d5d
Child:
47:2312a8dc9658
added show parameters function to calibrate (including integration steps)

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 31:5f039cbddee8 6 rigidLoop::rigidLoop()
mbedalvaro 31:5f039cbddee8 7 {
mbedalvaro 1:a4050fee11f7 8 }
mbedalvaro 1:a4050fee11f7 9
mbedalvaro 31:5f039cbddee8 10 rigidLoop::~rigidLoop()
mbedalvaro 31:5f039cbddee8 11 {
mbedalvaro 1:a4050fee11f7 12
mbedalvaro 1:a4050fee11f7 13 }
mbedalvaro 1:a4050fee11f7 14
mbedalvaro 46:90516893793a 15 void rigidLoop::showParameters() {
mbedalvaro 46:90516893793a 16 //pc.printf("Blob Name:
mbedalvaro 46:90516893793a 17 pc.printf("Mirror delay :%d\n", displaySensingBuffer.delayMirrorSamples);
mbedalvaro 46:90516893793a 18 pc.printf("Angle correction force :%d\n", angleCorrectionForceLoop);
mbedalvaro 46:90516893793a 19 pc.printf("Integration Step :%d\n", angleCorrectionForceLoop);
mbedalvaro 46:90516893793a 20 }
mbedalvaro 1:a4050fee11f7 21
mbedalvaro 1:a4050fee11f7 22 // Note: this method is hidding the abstract method in the base class... and has DIFFERENT parameters than another child would have (enum type).
mbedalvaro 31:5f039cbddee8 23 void rigidLoop::createBlob(int _id, RigidLoopMode _elasticBlobMode, vector2Df _initPos, vector2Df _initSpeed)
mbedalvaro 31:5f039cbddee8 24 {
mbedalvaro 1:a4050fee11f7 25 // (1) set ID:
mbedalvaro 1:a4050fee11f7 26 identifier=_id;
mbedalvaro 1:a4050fee11f7 27
mbedalvaro 1:a4050fee11f7 28 updateMode=_elasticBlobMode;
mbedalvaro 31:5f039cbddee8 29
mbedalvaro 4:f9d364f10335 30 startCenter=_initPos;
mbedalvaro 4:f9d364f10335 31 startSpeed=_initSpeed;
mbedalvaro 46:90516893793a 32
mbedalvaro 46:90516893793a 33 integrationStep=0.23;
mbedalvaro 31:5f039cbddee8 34
mbedalvaro 1:a4050fee11f7 35 // (2) Initialize common variables of all blobs (base class):
mbedalvaro 45:a3b984a79d5d 36 //initCommonVariables();
mbedalvaro 1:a4050fee11f7 37
mbedalvaro 1:a4050fee11f7 38 // (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 39 // Sending data:
mbedalvaro 20:8e82b95180e7 40 periodSendingData=10; // in ms
mbedalvaro 18:d72935b13858 41 sendingRecenteringAngle=true;
mbedalvaro 18:d72935b13858 42 sendingAnchorPosition=true;
mbedalvaro 32:52273c3291fe 43 sendingBlobMaxMin=true;
mbedalvaro 32:52273c3291fe 44 // sending only on EVENT (which means the spot is touching something) in case of rigid loop:
mbedalvaro 18:d72935b13858 45 sendingOnlyWhenTouch=true;
mbedalvaro 32:52273c3291fe 46
mbedalvaro 31:5f039cbddee8 47
mbedalvaro 21:bc9b9383f4b6 48 // Gravity field:
mbedalvaro 31:5f039cbddee8 49 gravity.set(0,0);
mbedalvaro 3:b44ff6de81bd 50
mbedalvaro 1:a4050fee11f7 51 // (3) Initialize secondary variables depending on the behaviour mode (may be changed afterwards in real time)
mbedalvaro 1:a4050fee11f7 52
mbedalvaro 1:a4050fee11f7 53 switch (updateMode) {
mbedalvaro 31:5f039cbddee8 54
mbedalvaro 4:f9d364f10335 55 case SPOT_TEST:
mbedalvaro 4:f9d364f10335 56 // Name of this kind of spot:
mbedalvaro 4:f9d364f10335 57 sprintf(spotName,"spot_test");
mbedalvaro 4:f9d364f10335 58
mbedalvaro 4:f9d364f10335 59 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 44:46e25fa1669b 60 setColor(0x04); // only red?
mbedalvaro 45:a3b984a79d5d 61 blueTouch=true;
mbedalvaro 45:a3b984a79d5d 62
mbedalvaro 14:0fc33a3a7b4b 63 saccadeRadius=250;
mbedalvaro 4:f9d364f10335 64
mbedalvaro 4:f9d364f10335 65 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 14:0fc33a3a7b4b 66 // NOTE: number of points in the case of need to compute recentering vector needs to be EVEN
mbedalvaro 44:46e25fa1669b 67 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 20); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 4:f9d364f10335 68
mbedalvaro 4:f9d364f10335 69 massCenter=0.01;
mbedalvaro 4:f9d364f10335 70 dampMotionCenterMass=0.001;
mbedalvaro 31:5f039cbddee8 71
mbedalvaro 4:f9d364f10335 72 // 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 73 createLoopFromScafold();
mbedalvaro 4:f9d364f10335 74
mbedalvaro 5:73cd58b58f95 75 // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 5:73cd58b58f95 76 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 5:73cd58b58f95 77 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
mbedalvaro 7:0df17f3078bc 78 displaySensingBuffer.setDelayMirrors(1);
mbedalvaro 7:0df17f3078bc 79 angleCorrectionForceLoop=0;//360.0/bluePrint.scafold.size()/2; // in DEGREES
mbedalvaro 4:f9d364f10335 80
mbedalvaro 4:f9d364f10335 81 break;
mbedalvaro 31:5f039cbddee8 82
mbedalvaro 30:d8af03f01cd4 83 case SPOT_TRACK:
mbedalvaro 30:d8af03f01cd4 84 // Name of this kind of spot:
mbedalvaro 30:d8af03f01cd4 85 sprintf(spotName,"spot_track");
mbedalvaro 30:d8af03f01cd4 86
mbedalvaro 30:d8af03f01cd4 87 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 30:d8af03f01cd4 88 setColor(0x04);
mbedalvaro 45:a3b984a79d5d 89 blueTouch=false;
mbedalvaro 30:d8af03f01cd4 90
mbedalvaro 35:35af5086ab4f 91 saccadeRadius=45;//+rand()%20;
mbedalvaro 31:5f039cbddee8 92 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 31:5f039cbddee8 93 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 31:5f039cbddee8 94
mbedalvaro 31:5f039cbddee8 95 // Numeric parameters for the simulated mechanical system:
mbedalvaro 31:5f039cbddee8 96 massCenter=0.001;//+0.000005*(rand()%100);
mbedalvaro 31:5f039cbddee8 97 dampMotionCenterMass=0.001;//0.00015;//00003;
mbedalvaro 31:5f039cbddee8 98
mbedalvaro 31:5f039cbddee8 99 // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
mbedalvaro 31:5f039cbddee8 100 createLoopFromScafold();
mbedalvaro 31:5f039cbddee8 101
mbedalvaro 31:5f039cbddee8 102 // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 31:5f039cbddee8 103 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 31:5f039cbddee8 104 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
mbedalvaro 45:a3b984a79d5d 105 displaySensingBuffer.setDelayMirrors(2);
mbedalvaro 45:a3b984a79d5d 106 angleCorrectionForceLoop=-35;// in degrees
mbedalvaro 31:5f039cbddee8 107
mbedalvaro 31:5f039cbddee8 108 break;
mbedalvaro 31:5f039cbddee8 109
mbedalvaro 31:5f039cbddee8 110 case SPOT_TRACK_DOT:
mbedalvaro 31:5f039cbddee8 111 // Name of this kind of spot:
mbedalvaro 31:5f039cbddee8 112 sprintf(spotName,"spot_track");
mbedalvaro 31:5f039cbddee8 113
mbedalvaro 31:5f039cbddee8 114 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 31:5f039cbddee8 115 setColor(0x04);
mbedalvaro 45:a3b984a79d5d 116 blueTouch=false;
mbedalvaro 30:d8af03f01cd4 117
mbedalvaro 35:35af5086ab4f 118 saccadeRadius=45;//+rand()%20;
mbedalvaro 30:d8af03f01cd4 119 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 32:52273c3291fe 120 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 20); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 30:d8af03f01cd4 121
mbedalvaro 30:d8af03f01cd4 122 // Numeric parameters for the simulated mechanical system:
mbedalvaro 30:d8af03f01cd4 123 massCenter=0.001;//+0.000005*(rand()%100);
mbedalvaro 30:d8af03f01cd4 124 dampMotionCenterMass=0.001;//0.00015;//00003;
mbedalvaro 30:d8af03f01cd4 125
mbedalvaro 30:d8af03f01cd4 126 // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
mbedalvaro 30:d8af03f01cd4 127 createLoopFromScafold();
mbedalvaro 45:a3b984a79d5d 128
mbedalvaro 45:a3b984a79d5d 129 justSearched=false;
mbedalvaro 30:d8af03f01cd4 130
mbedalvaro 30:d8af03f01cd4 131 // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 30:d8af03f01cd4 132 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 30:d8af03f01cd4 133 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
mbedalvaro 30:d8af03f01cd4 134 displaySensingBuffer.setDelayMirrors(3);
mbedalvaro 30:d8af03f01cd4 135 angleCorrectionForceLoop=-5;// in degrees
mbedalvaro 30:d8af03f01cd4 136
mbedalvaro 30:d8af03f01cd4 137 break;
mbedalvaro 4:f9d364f10335 138
mbedalvaro 1:a4050fee11f7 139 case SPOT_FOLLOWING:
mbedalvaro 1:a4050fee11f7 140
mbedalvaro 1:a4050fee11f7 141 // Name of this kind of spot:
mbedalvaro 1:a4050fee11f7 142 sprintf(spotName,"rigid_following");
mbedalvaro 1:a4050fee11f7 143
mbedalvaro 3:b44ff6de81bd 144 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 35:35af5086ab4f 145 setColor(0x04); //only R
mbedalvaro 45:a3b984a79d5d 146 blueTouch=false;
mbedalvaro 45:a3b984a79d5d 147
mbedalvaro 1:a4050fee11f7 148 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 44:46e25fa1669b 149 saccadeRadius=30;//23;
mbedalvaro 32:52273c3291fe 150 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 20); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 1:a4050fee11f7 151
mbedalvaro 1:a4050fee11f7 152 // 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 153 massCenter=0.01;
mbedalvaro 1:a4050fee11f7 154 dampMotionCenterMass=0.001;
mbedalvaro 1:a4050fee11f7 155
mbedalvaro 4:f9d364f10335 156 // 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 157 createLoopFromScafold();
mbedalvaro 4:f9d364f10335 158
mbedalvaro 5:73cd58b58f95 159 slidingDirection=true; // For contour following (will change direction when touching wall)
mbedalvaro 31:5f039cbddee8 160 speedContourFollowing=startSpeed.length();//1.1*saccadeRadius;
mbedalvaro 7:0df17f3078bc 161 justSearched=false;
mbedalvaro 5:73cd58b58f95 162
mbedalvaro 7:0df17f3078bc 163 // per-blob mirror delay: ONLY USEFUL FOR ELASTIC BLOBS, because otherwise it can be corrected by "angleCorrection"
mbedalvaro 7:0df17f3078bc 164 // (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 5:73cd58b58f95 165 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 7:0df17f3078bc 166 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop.
mbedalvaro 31:5f039cbddee8 167 // 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 168 // optimal value, and finish the correction (fine tunned) with the angle correction (only possible in the case of circular rigid blob).
mbedalvaro 44:46e25fa1669b 169 displaySensingBuffer.setDelayMirrors(1); // this corresponds to an angular correction of -delayMirrors * 360/numPoints
mbedalvaro 15:56a0bf424e8d 170 angleCorrectionForceLoop= -5;// good for ONE spot: -5;// in DEGREES
mbedalvaro 4:f9d364f10335 171
mbedalvaro 1:a4050fee11f7 172 break;
mbedalvaro 1:a4050fee11f7 173
mbedalvaro 1:a4050fee11f7 174 case SPOT_BOUNCING:
mbedalvaro 1:a4050fee11f7 175 // Name of this kind of spot:
mbedalvaro 1:a4050fee11f7 176 sprintf(spotName,"rigid_bouncing");
mbedalvaro 1:a4050fee11f7 177
mbedalvaro 35:35af5086ab4f 178 setColor(0x05);// R+B
mbedalvaro 45:a3b984a79d5d 179 blueTouch=false;
mbedalvaro 45:a3b984a79d5d 180
mbedalvaro 31:5f039cbddee8 181 saccadeRadius=30;//+rand()%20;
mbedalvaro 1:a4050fee11f7 182 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 12:0de9cd2bced5 183 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 1:a4050fee11f7 184
mbedalvaro 1:a4050fee11f7 185 // Numeric parameters for the simulated mechanical system:
mbedalvaro 31:5f039cbddee8 186 massCenter=0.0007;//0.0008;//+0.000005*(rand()%100);
mbedalvaro 31:5f039cbddee8 187 dampMotionCenterMass=0.0002;//0.00015;//00003;
mbedalvaro 31:5f039cbddee8 188 factorBouncingForce=0.004; // this is because we will use a force on the central mass
mbedalvaro 28:44b7b6e35548 189
mbedalvaro 28:44b7b6e35548 190 // 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 191 createLoopFromScafold();
mbedalvaro 30:d8af03f01cd4 192
mbedalvaro 30:d8af03f01cd4 193 // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 30:d8af03f01cd4 194 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 30:d8af03f01cd4 195 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
mbedalvaro 30:d8af03f01cd4 196 displaySensingBuffer.setDelayMirrors(3);
mbedalvaro 44:46e25fa1669b 197 angleCorrectionForceLoop=-15;// in degrees
mbedalvaro 30:d8af03f01cd4 198
mbedalvaro 30:d8af03f01cd4 199 break;
mbedalvaro 31:5f039cbddee8 200
mbedalvaro 32:52273c3291fe 201 case SPOT_BOUNCING_FACTOR:
mbedalvaro 32:52273c3291fe 202 // Name of this kind of spot:
mbedalvaro 32:52273c3291fe 203 sprintf(spotName,"rigid_bouncing");
mbedalvaro 32:52273c3291fe 204
mbedalvaro 32:52273c3291fe 205 setColor(0x05);
mbedalvaro 45:a3b984a79d5d 206 blueTouch=false;
mbedalvaro 45:a3b984a79d5d 207
mbedalvaro 32:52273c3291fe 208 saccadeRadius=30;//+rand()%20;
mbedalvaro 32:52273c3291fe 209 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 32:52273c3291fe 210 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 32:52273c3291fe 211
mbedalvaro 32:52273c3291fe 212 // Numeric parameters for the simulated mechanical system:
mbedalvaro 32:52273c3291fe 213 massCenter=0.0007;//0.0008;//+0.000005*(rand()%100);
mbedalvaro 32:52273c3291fe 214 dampMotionCenterMass=0.0002;//0.00015;//00003;
mbedalvaro 32:52273c3291fe 215
mbedalvaro 32:52273c3291fe 216 factorBouncingForce=0.004; // this is because we will use a force on the central mass (not used in SPOT_BOUNCING_FACTOR)
mbedalvaro 32:52273c3291fe 217
mbedalvaro 32:52273c3291fe 218 factorAbsorptionShock=0.9; // coef elastic bouncing
mbedalvaro 32:52273c3291fe 219
mbedalvaro 32:52273c3291fe 220 // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
mbedalvaro 32:52273c3291fe 221 createLoopFromScafold();
mbedalvaro 32:52273c3291fe 222
mbedalvaro 32:52273c3291fe 223 // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 32:52273c3291fe 224 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 32:52273c3291fe 225 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
mbedalvaro 32:52273c3291fe 226 displaySensingBuffer.setDelayMirrors(3);
mbedalvaro 32:52273c3291fe 227 angleCorrectionForceLoop=-5;// in degrees
mbedalvaro 32:52273c3291fe 228
mbedalvaro 32:52273c3291fe 229 break;
mbedalvaro 32:52273c3291fe 230
mbedalvaro 31:5f039cbddee8 231 case SPOT_PACMAN:
mbedalvaro 31:5f039cbddee8 232 // this will define the behaviour of the "ghosts" in pacman. The spot just moves at a constant speed when there is no object.
mbedalvaro 31:5f039cbddee8 233 // When it collides, we use the position of the pacman (another spot) and the tangent vector to the blocking line to define the new direction of the
mbedalvaro 31:5f039cbddee8 234 // speed vector.
mbedalvaro 31:5f039cbddee8 235
mbedalvaro 31:5f039cbddee8 236 sprintf(spotName,"pacman");
mbedalvaro 30:d8af03f01cd4 237
mbedalvaro 30:d8af03f01cd4 238 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 35:35af5086ab4f 239 setColor(0x06); // red+green = yellowish
mbedalvaro 45:a3b984a79d5d 240 blueTouch=false;
mbedalvaro 45:a3b984a79d5d 241
mbedalvaro 30:d8af03f01cd4 242 saccadeRadius=35;//+rand()%20;
mbedalvaro 30:d8af03f01cd4 243 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 30:d8af03f01cd4 244 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 30:d8af03f01cd4 245
mbedalvaro 31:5f039cbddee8 246 massCenter=1.0;
mbedalvaro 31:5f039cbddee8 247 dampMotionCenterMass=0; // no motion damp (but there will be no forces: only constant uniform motion)
mbedalvaro 31:5f039cbddee8 248 factorBouncingForce=0; //actually not used.
mbedalvaro 31:5f039cbddee8 249 centerMass.dampBorder = 0; // no damping when hitting the mirror limits
mbedalvaro 30:d8af03f01cd4 250
mbedalvaro 30:d8af03f01cd4 251 // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
mbedalvaro 30:d8af03f01cd4 252 createLoopFromScafold();
mbedalvaro 30:d8af03f01cd4 253
mbedalvaro 30:d8af03f01cd4 254 slidingDirection=true; // For contour following (will change direction when touching wall)
mbedalvaro 30:d8af03f01cd4 255 speedContourFollowing=1.1*saccadeRadius;
mbedalvaro 30:d8af03f01cd4 256 justSearched=false;
mbedalvaro 30:d8af03f01cd4 257
mbedalvaro 30:d8af03f01cd4 258
mbedalvaro 30:d8af03f01cd4 259 // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 30:d8af03f01cd4 260 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 30:d8af03f01cd4 261 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
mbedalvaro 30:d8af03f01cd4 262 displaySensingBuffer.setDelayMirrors(3);
mbedalvaro 30:d8af03f01cd4 263 angleCorrectionForceLoop=-5;// in degrees
mbedalvaro 30:d8af03f01cd4 264
mbedalvaro 30:d8af03f01cd4 265 break;
mbedalvaro 31:5f039cbddee8 266
mbedalvaro 31:5f039cbddee8 267 case SPOT_GHOST:
mbedalvaro 31:5f039cbddee8 268 // this will define the behaviour of the "ghosts" in pacman. The spot just moves at a constant speed when there is no object.
mbedalvaro 31:5f039cbddee8 269 // When it collides, we use the position of the pacman (another spot) and the tangent vector to the blocking line to define the new direction of the
mbedalvaro 31:5f039cbddee8 270 // speed vector.
mbedalvaro 31:5f039cbddee8 271
mbedalvaro 31:5f039cbddee8 272 sprintf(spotName,"ghost");
mbedalvaro 30:d8af03f01cd4 273
mbedalvaro 35:35af5086ab4f 274 // Make it blueish by default:
mbedalvaro 30:d8af03f01cd4 275 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 35:35af5086ab4f 276 setColor(0x05); // red + blue
mbedalvaro 45:a3b984a79d5d 277 blueTouch=false;
mbedalvaro 45:a3b984a79d5d 278
mbedalvaro 31:5f039cbddee8 279 massCenter=1.0;
mbedalvaro 31:5f039cbddee8 280 dampMotionCenterMass=0; // no motion damp (but there will be no forces: only constant uniform motion)
mbedalvaro 31:5f039cbddee8 281 factorBouncingForce=0; //actually not used.
mbedalvaro 31:5f039cbddee8 282 centerMass.dampBorder = 0; // no damping when hitting the mirror limits
mbedalvaro 31:5f039cbddee8 283
mbedalvaro 30:d8af03f01cd4 284
mbedalvaro 30:d8af03f01cd4 285 saccadeRadius=35;//+rand()%20;
mbedalvaro 30:d8af03f01cd4 286 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 30:d8af03f01cd4 287 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 30:d8af03f01cd4 288
mbedalvaro 30:d8af03f01cd4 289
mbedalvaro 30:d8af03f01cd4 290 // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
mbedalvaro 30:d8af03f01cd4 291 createLoopFromScafold();
mbedalvaro 30:d8af03f01cd4 292
mbedalvaro 30:d8af03f01cd4 293 slidingDirection=true; // For contour following (will change direction when touching wall)
mbedalvaro 30:d8af03f01cd4 294 speedContourFollowing=1.1*saccadeRadius;
mbedalvaro 30:d8af03f01cd4 295 justSearched=false;
mbedalvaro 30:d8af03f01cd4 296
mbedalvaro 28:44b7b6e35548 297
mbedalvaro 28:44b7b6e35548 298 // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 28:44b7b6e35548 299 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 28:44b7b6e35548 300 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
mbedalvaro 28:44b7b6e35548 301 displaySensingBuffer.setDelayMirrors(3);
mbedalvaro 28:44b7b6e35548 302 angleCorrectionForceLoop=-5;// in degrees
mbedalvaro 28:44b7b6e35548 303
mbedalvaro 28:44b7b6e35548 304 break;
mbedalvaro 31:5f039cbddee8 305
mbedalvaro 31:5f039cbddee8 306 case SPOT_AIR_HOCKEY:
mbedalvaro 28:44b7b6e35548 307 // Name of this kind of spot:
mbedalvaro 28:44b7b6e35548 308 sprintf(spotName,"air_hockey");
mbedalvaro 28:44b7b6e35548 309
mbedalvaro 28:44b7b6e35548 310 //startCenter.set(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_Y);
mbedalvaro 31:5f039cbddee8 311 //startSpeed.set(0,0);
mbedalvaro 28:44b7b6e35548 312
mbedalvaro 28:44b7b6e35548 313 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 28:44b7b6e35548 314 setColor(0x04);
mbedalvaro 45:a3b984a79d5d 315 blueTouch=false;
mbedalvaro 45:a3b984a79d5d 316
mbedalvaro 28:44b7b6e35548 317 gravity.set(0,0);
mbedalvaro 28:44b7b6e35548 318
mbedalvaro 28:44b7b6e35548 319 saccadeRadius=50;//+rand()%20;
mbedalvaro 28:44b7b6e35548 320 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 28:44b7b6e35548 321 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 28:44b7b6e35548 322
mbedalvaro 28:44b7b6e35548 323 // Numeric parameters for the simulated mechanical system:
mbedalvaro 28:44b7b6e35548 324 massCenter=0.0008;//+0.000005*(rand()%100);
mbedalvaro 28:44b7b6e35548 325 dampMotionCenterMass=0.00065;//0.00015;//00003;
mbedalvaro 7:0df17f3078bc 326 factorBouncingForce=0.0018; // this is because we will use a force on the central mass
mbedalvaro 1:a4050fee11f7 327
mbedalvaro 4:f9d364f10335 328 // 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 329 createLoopFromScafold();
mbedalvaro 4:f9d364f10335 330
mbedalvaro 5:73cd58b58f95 331 // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 5:73cd58b58f95 332 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 5:73cd58b58f95 333 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
mbedalvaro 7:0df17f3078bc 334 displaySensingBuffer.setDelayMirrors(3);
mbedalvaro 7:0df17f3078bc 335 angleCorrectionForceLoop=-5;// in degrees
mbedalvaro 1:a4050fee11f7 336
mbedalvaro 1:a4050fee11f7 337 break;
mbedalvaro 31:5f039cbddee8 338
mbedalvaro 31:5f039cbddee8 339 case SPOT_LORENTZ_FORCE:
mbedalvaro 27:1ce994629ffc 340 // Name of this kind of spot:
mbedalvaro 27:1ce994629ffc 341 sprintf(spotName,"rigid_fountain");
mbedalvaro 27:1ce994629ffc 342
mbedalvaro 27:1ce994629ffc 343 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 27:1ce994629ffc 344 setColor(0x04);
mbedalvaro 45:a3b984a79d5d 345 blueTouch=false;
mbedalvaro 45:a3b984a79d5d 346
mbedalvaro 29:2fc8c12822eb 347 saccadeRadius=20;//+rand()%20;
mbedalvaro 27:1ce994629ffc 348 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 28:44b7b6e35548 349 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos, int _numScafoldPoints);
mbedalvaro 27:1ce994629ffc 350
mbedalvaro 27:1ce994629ffc 351 // Numeric parameters for the simulated mechanical system:
mbedalvaro 27:1ce994629ffc 352 massCenter=0.0005;//+0.000005*(rand()%100);
mbedalvaro 28:44b7b6e35548 353 dampMotionCenterMass=0.001;//0.00015;//00003;
mbedalvaro 28:44b7b6e35548 354 factorBouncingForce=0.0015; // this is because we will use a force on the central mass
mbedalvaro 27:1ce994629ffc 355
mbedalvaro 27:1ce994629ffc 356 // 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 357 createLoopFromScafold();
mbedalvaro 27:1ce994629ffc 358
mbedalvaro 27:1ce994629ffc 359 // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 27:1ce994629ffc 360 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 27:1ce994629ffc 361 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
mbedalvaro 27:1ce994629ffc 362 displaySensingBuffer.setDelayMirrors(3);
mbedalvaro 27:1ce994629ffc 363 angleCorrectionForceLoop=-5;// in degrees
mbedalvaro 27:1ce994629ffc 364
mbedalvaro 27:1ce994629ffc 365 break;
mbedalvaro 45:a3b984a79d5d 366 default:
mbedalvaro 45:a3b984a79d5d 367 break;
mbedalvaro 1:a4050fee11f7 368 }
mbedalvaro 31:5f039cbddee8 369
mbedalvaro 31:5f039cbddee8 370 saccadeRadius_initial=saccadeRadius; // this is for search mode for instance.
mbedalvaro 31:5f039cbddee8 371
mbedalvaro 12:0de9cd2bced5 372 // 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 373 setRegionMotion(MIN_AD_MIRRORS+saccadeRadius, MIN_AD_MIRRORS+saccadeRadius, MAX_AD_MIRRORS-saccadeRadius, MAX_AD_MIRRORS-saccadeRadius);
mbedalvaro 12:0de9cd2bced5 374
mbedalvaro 26:c9329c4fc20a 375 // draw it once on the display buffer for good initialization:
mbedalvaro 26:c9329c4fc20a 376 draw();
mbedalvaro 1:a4050fee11f7 377 }
mbedalvaro 1:a4050fee11f7 378
mbedalvaro 31:5f039cbddee8 379 void rigidLoop::createLoopFromScafold(void)
mbedalvaro 31:5f039cbddee8 380 {
mbedalvaro 1:a4050fee11f7 381
mbedalvaro 1:a4050fee11f7 382 initSizeBlob(bluePrint.scafold.size()); // very simple here (only need to set the size of the lsd buffer)
mbedalvaro 1:a4050fee11f7 383
mbedalvaro 1:a4050fee11f7 384 centerMass.mass=massCenter;
mbedalvaro 1:a4050fee11f7 385 centerMass.dampMotion = dampMotionCenterMass;
mbedalvaro 1:a4050fee11f7 386
mbedalvaro 1:a4050fee11f7 387 // note: the following may not be required in case of contour following:
mbedalvaro 46:90516893793a 388 centerMass.setIntegrationStep(integrationStep); //0.23// VERY IMPORTANT! in the case of verlet integration, we need to set dt BEFORE setting the initial speed.
mbedalvaro 4:f9d364f10335 389 centerMass.setInitialCondition(startCenter, startSpeed);
mbedalvaro 1:a4050fee11f7 390 // centerMass.setInitialCondition(2047.0, 2047.0,0.0,0.0);
mbedalvaro 31:5f039cbddee8 391
mbedalvaro 31:5f039cbddee8 392 }
mbedalvaro 31:5f039cbddee8 393
mbedalvaro 32:52273c3291fe 394 void rigidLoop::initSizeBlob(int _numPoints)
mbedalvaro 32:52273c3291fe 395 {
mbedalvaro 32:52273c3291fe 396 // Iinitialize blob size (number of points for the loop, as well as other structures such as lsdTrajectory)
mbedalvaro 32:52273c3291fe 397 numPoints=_numPoints;
mbedalvaro 32:52273c3291fe 398
mbedalvaro 32:52273c3291fe 399 // Sensing and Display trajectory:
mbedalvaro 32:52273c3291fe 400 displaySensingBuffer.lsdTrajectory.resize(numPoints); // the lsdTrajectory and the elastic loop will have the same number of points (this could be different - decimation?).
mbedalvaro 32:52273c3291fe 401 }
mbedalvaro 32:52273c3291fe 402
mbedalvaro 31:5f039cbddee8 403 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 31:5f039cbddee8 404 {
mbedalvaro 31:5f039cbddee8 405 // centerMass.setWallLimits(mmix+10, mmiy+10, mmax-10, mmay-10);
mbedalvaro 31:5f039cbddee8 406 // Use the static method of the pointMass class:
mbedalvaro 31:5f039cbddee8 407 pointMass::setWallLimits(mmix+10, mmiy+10, mmax-10, mmay-10);
mbedalvaro 1:a4050fee11f7 408 }
mbedalvaro 1:a4050fee11f7 409
mbedalvaro 33:43e8bc451ef0 410 void rigidLoop::setSize(float _newSize) {
mbedalvaro 33:43e8bc451ef0 411 saccadeRadius=_newSize;
mbedalvaro 33:43e8bc451ef0 412 saccadeRadius_initial=_newSize;
mbedalvaro 33:43e8bc451ef0 413 // rebuild the blueprint (that's all it's needed in the case of a rigid loop, IF we don't change the number of points in the scafold of course):
mbedalvaro 33:43e8bc451ef0 414 bluePrint.resizeDimensionScafold(_newSize);
mbedalvaro 33:43e8bc451ef0 415 }
mbedalvaro 33:43e8bc451ef0 416 void rigidLoop::sizeFactor(float sizeFactor) {
mbedalvaro 33:43e8bc451ef0 417 saccadeRadius*=sizeFactor;
mbedalvaro 33:43e8bc451ef0 418 saccadeRadius_initial*=sizeFactor;
mbedalvaro 33:43e8bc451ef0 419 // rebuild the blueprint (that's all it's needed in the case of a rigid loop, IF we don't change the number of points in the scafold of course):
mbedalvaro 33:43e8bc451ef0 420 bluePrint.resizeFactorDimensionScafold(sizeFactor);
mbedalvaro 33:43e8bc451ef0 421 }
mbedalvaro 33:43e8bc451ef0 422
mbedalvaro 33:43e8bc451ef0 423 void rigidLoop::setSpeed(float _newspeed) {
mbedalvaro 33:43e8bc451ef0 424 // in case of spot following:
mbedalvaro 33:43e8bc451ef0 425 speedContourFollowing=_newspeed;
mbedalvaro 33:43e8bc451ef0 426
mbedalvaro 33:43e8bc451ef0 427 // in case of bouncing, there are many ways to change the speed (play with the mass, damping or the bouncing force).
mbedalvaro 33:43e8bc451ef0 428 //centerMass.mass/=speedfactor;//0.0008;//+0.000005*(rand()%100);
mbedalvaro 33:43e8bc451ef0 429 centerMass.dampMotion/=1.0*_newspeed/7;//0.00045;//0.00015;//00003;
mbedalvaro 33:43e8bc451ef0 430 //factorBouncingForce=0.0018; // this is because we will use a force on the central mass
mbedalvaro 33:43e8bc451ef0 431 }
mbedalvaro 31:5f039cbddee8 432 void rigidLoop::speedFactor(float speedfactor)
mbedalvaro 31:5f039cbddee8 433 {
mbedalvaro 31:5f039cbddee8 434 // in case of spot following:
mbedalvaro 31:5f039cbddee8 435 speedContourFollowing*=speedfactor;
mbedalvaro 31:5f039cbddee8 436
mbedalvaro 31:5f039cbddee8 437 // in case of bouncing, there are many ways to change the speed (play with the mass, damping or the bouncing force).
mbedalvaro 31:5f039cbddee8 438 //centerMass.mass/=speedfactor;//0.0008;//+0.000005*(rand()%100);
mbedalvaro 31:5f039cbddee8 439 centerMass.dampMotion/=speedfactor;//0.00045;//0.00015;//00003;
mbedalvaro 31:5f039cbddee8 440 //factorBouncingForce=0.0018; // this is because we will use a force on the central mass
mbedalvaro 1:a4050fee11f7 441 }
mbedalvaro 1:a4050fee11f7 442
mbedalvaro 31:5f039cbddee8 443 void rigidLoop::explosion()
mbedalvaro 31:5f039cbddee8 444 {
mbedalvaro 31:5f039cbddee8 445 transientBlobColor=blobColor|0x02;
mbedalvaro 31:5f039cbddee8 446 for (saccadeRadius=30; saccadeRadius<900 ; saccadeRadius+=10) {
mbedalvaro 31:5f039cbddee8 447 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
mbedalvaro 31:5f039cbddee8 448 draw();
mbedalvaro 31:5f039cbddee8 449 }
mbedalvaro 31:5f039cbddee8 450 saccadeRadius=saccadeRadius_initial;
mbedalvaro 31:5f039cbddee8 451 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
mbedalvaro 31:5f039cbddee8 452 // reset to central position:
mbedalvaro 31:5f039cbddee8 453 centerMass.setInitialCondition(startCenter, startSpeed);
mbedalvaro 31:5f039cbddee8 454 transientBlobColor=blobColor;
mbedalvaro 24:4e52031a495b 455 }
mbedalvaro 1:a4050fee11f7 456
mbedalvaro 31:5f039cbddee8 457 vector2Df rigidLoop::getCenter()
mbedalvaro 31:5f039cbddee8 458 {
mbedalvaro 31:5f039cbddee8 459 return(centerMass.pos);
mbedalvaro 31:5f039cbddee8 460 }
mbedalvaro 31:5f039cbddee8 461
mbedalvaro 31:5f039cbddee8 462 void rigidLoop::update(vector2Df referencePos)
mbedalvaro 31:5f039cbddee8 463 {
mbedalvaro 1:a4050fee11f7 464
mbedalvaro 1:a4050fee11f7 465 // (I) process loop geometry: not needed (rigid)
mbedalvaro 1:a4050fee11f7 466 // Just check if the blob touched the borders (only need to do this with the central mass):
mbedalvaro 1:a4050fee11f7 467 blobWallCollision=centerMass.bWallCollision;
mbedalvaro 1:a4050fee11f7 468
mbedalvaro 1:a4050fee11f7 469 // (II) Process sensing buffer and compute light forces:
mbedalvaro 44:46e25fa1669b 470 // 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 471
mbedalvaro 1:a4050fee11f7 472 // (III) Compute recentering vector (the "penetration vector in fact"), using "first order moment":
mbedalvaro 3:b44ff6de81bd 473 // 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 474 // sum in the circle is 0).
mbedalvaro 12:0de9cd2bced5 475 vector2Df momentVector(0,0);
mbedalvaro 7:0df17f3078bc 476 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 477 for (int i = 0; i < numPoints; i++) { // note: numPoints should be EVEN
mbedalvaro 1:a4050fee11f7 478 if (displaySensingBuffer.lsdTrajectory[i].lightZone>0) { // this is, we are in a dark zone (better to integrate there, because it is normally smaller)
mbedalvaro 31:5f039cbddee8 479
mbedalvaro 31:5f039cbddee8 480 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 31:5f039cbddee8 481 momentVector.y+=(float)bluePrint.scafold[i].y;
mbedalvaro 31:5f039cbddee8 482
mbedalvaro 31:5f039cbddee8 483 // We can also do the following, but ATTENTION: momentVector is of type vector2Df, and scafold[i] of type vector2Dd...
mbedalvaro 12:0de9cd2bced5 484 // momentVector+=bluePrint.scafold[i];// note: no need to do -centerMass.pos, because the scafold is "centered" around 0
mbedalvaro 31:5f039cbddee8 485
mbedalvaro 7:0df17f3078bc 486 counterDarkZone++;
mbedalvaro 1:a4050fee11f7 487 }
mbedalvaro 1:a4050fee11f7 488 }
mbedalvaro 7:0df17f3078bc 489 momentVector=momentVector*(2*PI/numPoints);
mbedalvaro 7:0df17f3078bc 490 float momentNorm=momentVector.length(); // = 2.R.sin(half_angle) in the direction of the dark zone
mbedalvaro 31:5f039cbddee8 491
mbedalvaro 12:0de9cd2bced5 492 vector2Df unitTowardsLight; // this is the normed vector, pointing towards the light zone
mbedalvaro 2:34157ebbf56b 493 if (momentNorm==0) {
mbedalvaro 7:0df17f3078bc 494 unitTowardsLight.set(0,0);
mbedalvaro 3:b44ff6de81bd 495 recenteringVectorLoop.set(0,0);
mbedalvaro 3:b44ff6de81bd 496 normRecenteringVector=0;
mbedalvaro 3:b44ff6de81bd 497 angleRecenteringVector=0;
mbedalvaro 3:b44ff6de81bd 498 } else {
mbedalvaro 31:5f039cbddee8 499 unitTowardsLight=momentVector/(-1.0*momentNorm);
mbedalvaro 44:46e25fa1669b 500 // Apply correction angle (NOT delay mirrors):
mbedalvaro 7:0df17f3078bc 501 unitTowardsLight.rotateDeg(angleCorrectionForceLoop);
mbedalvaro 31:5f039cbddee8 502
mbedalvaro 31:5f039cbddee8 503 // Compute "recenteringVectorLoop": the vector making the spot goes completely AWAY form the dark zone
mbedalvaro 31:5f039cbddee8 504 float aux=0.5*momentNorm/saccadeRadius; // note: in principle, we ALWAYS have momentNorm < 2.R, so aux < 1
mbedalvaro 31:5f039cbddee8 505 if (aux>1) aux=1.0; // can happen because of the discrete integration!
mbedalvaro 31:5f039cbddee8 506 if (counterDarkZone<=numPoints/2) { // note: numPoints HAS to be EVEN
mbedalvaro 7:0df17f3078bc 507 recenteringVectorLoop=unitTowardsLight*saccadeRadius*(1.0-sqrt(1.0-aux*aux));
mbedalvaro 31:5f039cbddee8 508 } else {
mbedalvaro 31:5f039cbddee8 509 recenteringVectorLoop=unitTowardsLight*saccadeRadius*(1.0+sqrt(1.0-aux*aux));
mbedalvaro 31:5f039cbddee8 510 }
mbedalvaro 31:5f039cbddee8 511
mbedalvaro 31:5f039cbddee8 512
mbedalvaro 3:b44ff6de81bd 513 // Compute redundant quantities (if necessary, for sending through OSC, etc):
mbedalvaro 3:b44ff6de81bd 514 normRecenteringVector=recenteringVectorLoop.length();
mbedalvaro 3:b44ff6de81bd 515 angleRecenteringVector=recenteringVectorLoop.angleDegHoriz();
mbedalvaro 3:b44ff6de81bd 516 }
mbedalvaro 3:b44ff6de81bd 517
mbedalvaro 30:d8af03f01cd4 518 // ======================== Now, depending on the mode of operation, we have different types of behaviour ========================================
mbedalvaro 31:5f039cbddee8 519
mbedalvaro 12:0de9cd2bced5 520 vector2Df slidingVector; //( need to declare it here because a switch "jump" cannot bypass an initialization)
mbedalvaro 31:5f039cbddee8 521 vector2Df auxVector;
mbedalvaro 1:a4050fee11f7 522 switch (updateMode) {
mbedalvaro 31:5f039cbddee8 523 // ================================================================
mbedalvaro 31:5f039cbddee8 524 case SPOT_TEST: // this is just for adjusting mirror delays, checking recentering vector, etc:
mbedalvaro 31:5f039cbddee8 525 // do nothing for the time being
mbedalvaro 31:5f039cbddee8 526 // 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 31:5f039cbddee8 527 // 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 31:5f039cbddee8 528
mbedalvaro 31:5f039cbddee8 529 // (1) current color: change with touch? NO
mbedalvaro 31:5f039cbddee8 530 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 31:5f039cbddee8 531
mbedalvaro 31:5f039cbddee8 532 break;
mbedalvaro 31:5f039cbddee8 533 // ================================================================
mbedalvaro 30:d8af03f01cd4 534 case SPOT_TRACK:
mbedalvaro 45:a3b984a79d5d 535 if (displaySensingBuffer.lightTouched) {
mbedalvaro 31:5f039cbddee8 536 centerMass.pos +=recenteringVectorLoop;
mbedalvaro 31:5f039cbddee8 537 centerMass.posOld=centerMass.pos; // this is necessary to compute bouceOffWalls using Verlet method... (MAKE A new variable INTEGRATION METHOD?)
mbedalvaro 31:5f039cbddee8 538 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
mbedalvaro 45:a3b984a79d5d 539
mbedalvaro 45:a3b984a79d5d 540 if (justSearched) {
mbedalvaro 45:a3b984a79d5d 541 saccadeRadius=saccadeRadius_initial;
mbedalvaro 45:a3b984a79d5d 542 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
mbedalvaro 45:a3b984a79d5d 543 justSearched=false;
mbedalvaro 45:a3b984a79d5d 544 }
mbedalvaro 45:a3b984a79d5d 545
mbedalvaro 45:a3b984a79d5d 546 } else if (displaySensingBuffer.lightState==ALL_DARK) { // not touched nor on something white: SEARCH MODE
mbedalvaro 45:a3b984a79d5d 547 saccadeRadius+=20;
mbedalvaro 45:a3b984a79d5d 548 if (saccadeRadius>800) saccadeRadius=saccadeRadius_initial;
mbedalvaro 45:a3b984a79d5d 549 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
mbedalvaro 45:a3b984a79d5d 550 justSearched=true;
mbedalvaro 45:a3b984a79d5d 551 }
mbedalvaro 31:5f039cbddee8 552
mbedalvaro 30:d8af03f01cd4 553 // Change color with touch? YES
mbedalvaro 31:5f039cbddee8 554 if (displaySensingBuffer.lightTouched)
mbedalvaro 31:5f039cbddee8 555 transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
mbedalvaro 31:5f039cbddee8 556 else
mbedalvaro 31:5f039cbddee8 557 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 31:5f039cbddee8 558
mbedalvaro 30:d8af03f01cd4 559 break;
mbedalvaro 31:5f039cbddee8 560 // ================================================================
mbedalvaro 31:5f039cbddee8 561 case SPOT_TRACK_DOT: // here, a dot in the center of the saccade should remain in the center:
mbedalvaro 32:52273c3291fe 562 centerMass.pos -=recenteringVectorLoop*2.5;
mbedalvaro 31:5f039cbddee8 563 centerMass.posOld=centerMass.pos; // this is necessary to compute bouceOffWalls using Verlet method... (MAKE A new variable INTEGRATION METHOD?)
mbedalvaro 31:5f039cbddee8 564 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
mbedalvaro 31:5f039cbddee8 565
mbedalvaro 31:5f039cbddee8 566 // Change color with touch? YES
mbedalvaro 31:5f039cbddee8 567 if (displaySensingBuffer.lightTouched)
mbedalvaro 31:5f039cbddee8 568 transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
mbedalvaro 31:5f039cbddee8 569 else
mbedalvaro 31:5f039cbddee8 570 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 31:5f039cbddee8 571
mbedalvaro 31:5f039cbddee8 572 break;
mbedalvaro 31:5f039cbddee8 573 // ================================================================
mbedalvaro 1:a4050fee11f7 574 case SPOT_FOLLOWING:
mbedalvaro 1:a4050fee11f7 575 // we need to compute the tangencial "speed":
mbedalvaro 1:a4050fee11f7 576 // vector2D slidingVector;
mbedalvaro 2:34157ebbf56b 577 if (momentNorm>0) {
mbedalvaro 5:73cd58b58f95 578 //momentVector/=momentNorm;
mbedalvaro 3:b44ff6de81bd 579 // We can now compute the sliding vector as:
mbedalvaro 7:0df17f3078bc 580 slidingVector=unitTowardsLight.getRotatedDeg(slidingDirection? 90 : -90) * speedContourFollowing;
mbedalvaro 31:5f039cbddee8 581
mbedalvaro 7:0df17f3078bc 582 // 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 583 // 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 584 centerMass.pos +=slidingVector+ ( unitTowardsLight*(-1.0*saccadeRadius) + recenteringVectorLoop )* 0.6;
mbedalvaro 31:5f039cbddee8 585 // 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 31:5f039cbddee8 586 // re-entry and avoid oscillations).
mbedalvaro 3:b44ff6de81bd 587
mbedalvaro 3:b44ff6de81bd 588 // The following function can help constraining the position "pos", but it also does too much. Do something simpler perhaps?
mbedalvaro 7:0df17f3078bc 589 centerMass.posOld=centerMass.pos; // this is necessary to compute bouceOffWalls using Verlet method... (MAKE A new variable INTEGRATION METHOD?)
mbedalvaro 31:5f039cbddee8 590
mbedalvaro 3:b44ff6de81bd 591 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
mbedalvaro 31:5f039cbddee8 592
mbedalvaro 7:0df17f3078bc 593 if (justSearched) {
mbedalvaro 7:0df17f3078bc 594 saccadeRadius=saccadeRadius_initial;
mbedalvaro 12:0de9cd2bced5 595 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
mbedalvaro 7:0df17f3078bc 596 justSearched=false;
mbedalvaro 31:5f039cbddee8 597 }
mbedalvaro 31:5f039cbddee8 598
mbedalvaro 2:34157ebbf56b 599 } else {
mbedalvaro 7:0df17f3078bc 600 // not on something. SEARCH MODE (or go to spot_bouncing mode?)
mbedalvaro 31:5f039cbddee8 601 saccadeRadius+=30;
mbedalvaro 31:5f039cbddee8 602 if (saccadeRadius>800) saccadeRadius=saccadeRadius_initial;
mbedalvaro 12:0de9cd2bced5 603 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
mbedalvaro 7:0df17f3078bc 604 justSearched=true;
mbedalvaro 2:34157ebbf56b 605 }
mbedalvaro 1:a4050fee11f7 606
mbedalvaro 31:5f039cbddee8 607 // Change color with touch? NO
mbedalvaro 31:5f039cbddee8 608 // if (displaySensingBuffer.lightTouched)
mbedalvaro 31:5f039cbddee8 609 // transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
mbedalvaro 31:5f039cbddee8 610 // else
mbedalvaro 27:1ce994629ffc 611 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 31:5f039cbddee8 612
mbedalvaro 31:5f039cbddee8 613 // change sliding direction (for countour following):
mbedalvaro 31:5f039cbddee8 614 if (blobWallCollision) {
mbedalvaro 31:5f039cbddee8 615 if (wallCounter>5) {
mbedalvaro 31:5f039cbddee8 616 slidingDirection=!slidingDirection;
mbedalvaro 31:5f039cbddee8 617 wallCounter=0;
mbedalvaro 31:5f039cbddee8 618 }
mbedalvaro 28:44b7b6e35548 619 }
mbedalvaro 31:5f039cbddee8 620 wallCounter++;
mbedalvaro 27:1ce994629ffc 621
mbedalvaro 1:a4050fee11f7 622 break;
mbedalvaro 31:5f039cbddee8 623 // ================================================================
mbedalvaro 31:5f039cbddee8 624 case SPOT_GHOST:
mbedalvaro 31:5f039cbddee8 625 // This is not completely sliding nor bouncing, but a combination of both
mbedalvaro 31:5f039cbddee8 626 // Behaviour: - if the spot is NOT touching anything, just move with uniform speed (always constant speed in norm).
mbedalvaro 31:5f039cbddee8 627 // - if the spot touch something, then modify the speed so that it ALIGNS with the tangential vector, without changing its norm.
mbedalvaro 31:5f039cbddee8 628 // - also, choose the direction so as to APPROACH THE PACMAN (position of the pacman is in referencePos, a parameter to "update" method).
mbedalvaro 31:5f039cbddee8 629
mbedalvaro 31:5f039cbddee8 630 if (momentNorm>0) {
mbedalvaro 31:5f039cbddee8 631
mbedalvaro 31:5f039cbddee8 632 // first, get the current speed:
mbedalvaro 31:5f039cbddee8 633 auxVector=centerMass.getSpeed();
mbedalvaro 31:5f039cbddee8 634
mbedalvaro 31:5f039cbddee8 635 // Before recalculating the speed (that will recompute pos from posOld), set posOld well outside the dark zone (this is
mbedalvaro 31:5f039cbddee8 636 // necessary because if the printed pattern move and the speed is slow, then there is not enough bouncing!):
mbedalvaro 31:5f039cbddee8 637 centerMass.posOld=centerMass.pos+recenteringVectorLoop;
mbedalvaro 31:5f039cbddee8 638
mbedalvaro 31:5f039cbddee8 639 // then compute the new bounce speed vector:
mbedalvaro 31:5f039cbddee8 640 float aux=unitTowardsLight.dot(auxVector);
mbedalvaro 31:5f039cbddee8 641 float anglepac=unitTowardsLight.angleDeg(referencePos-centerMass.pos); // angle from unit vector to (pacman-center)
mbedalvaro 31:5f039cbddee8 642 if (abs(anglepac)<85)
mbedalvaro 31:5f039cbddee8 643 slidingVector= referencePos-centerMass.pos;
mbedalvaro 31:5f039cbddee8 644 //slidingVector= auxVector-unitTowardsLight*aux*2; // this is a normal bounce
mbedalvaro 31:5f039cbddee8 645 else
mbedalvaro 31:5f039cbddee8 646 slidingVector=unitTowardsLight.getRotatedDeg((anglepac>0)? 85 : -85);
mbedalvaro 31:5f039cbddee8 647
mbedalvaro 31:5f039cbddee8 648 slidingVector.scale(auxVector.length()); // do not forget to scale...
mbedalvaro 31:5f039cbddee8 649 // then reset speed:
mbedalvaro 31:5f039cbddee8 650 centerMass.setSpeed(slidingVector);
mbedalvaro 31:5f039cbddee8 651 }
mbedalvaro 5:73cd58b58f95 652
mbedalvaro 31:5f039cbddee8 653 // update dynamics for the central mass:
mbedalvaro 31:5f039cbddee8 654 #ifndef VERLET_METHOD
mbedalvaro 31:5f039cbddee8 655 centerMass.addDampingForce(); // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
mbedalvaro 31:5f039cbddee8 656 #endif
mbedalvaro 31:5f039cbddee8 657
mbedalvaro 31:5f039cbddee8 658 centerMass.update(); // unconstrained
mbedalvaro 31:5f039cbddee8 659 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
mbedalvaro 31:5f039cbddee8 660
mbedalvaro 31:5f039cbddee8 661 // Change color with touch? NO
mbedalvaro 31:5f039cbddee8 662 // if (displaySensingBuffer.lightTouched)
mbedalvaro 31:5f039cbddee8 663 // transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
mbedalvaro 31:5f039cbddee8 664 // else
mbedalvaro 31:5f039cbddee8 665 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 31:5f039cbddee8 666
mbedalvaro 31:5f039cbddee8 667
mbedalvaro 31:5f039cbddee8 668 break;
mbedalvaro 31:5f039cbddee8 669
mbedalvaro 31:5f039cbddee8 670 // ================================================================
mbedalvaro 31:5f039cbddee8 671 case SPOT_PACMAN:
mbedalvaro 31:5f039cbddee8 672 // This is not completely sliding nor bouncing, but a combination of both
mbedalvaro 31:5f039cbddee8 673 // Behaviour: - if the spot is NOT touching anything, just move with uniform speed (always constant speed in norm).
mbedalvaro 31:5f039cbddee8 674 // - if the spot touch something, then modify the speed so that it ALIGNS with the tangential vector, without changing its norm.
mbedalvaro 31:5f039cbddee8 675 // - also, choose the direction so that it minimizes the angle with the previous speed vector (a little like bouncing):
mbedalvaro 31:5f039cbddee8 676
mbedalvaro 31:5f039cbddee8 677 if (momentNorm>0) {
mbedalvaro 31:5f039cbddee8 678
mbedalvaro 31:5f039cbddee8 679 // (a) Compute the new speed (will use slidingVector as auxiliary vector2Df):
mbedalvaro 31:5f039cbddee8 680 auxVector=centerMass.getSpeed();
mbedalvaro 31:5f039cbddee8 681 float aux=unitTowardsLight.dot(auxVector);
mbedalvaro 31:5f039cbddee8 682 if (aux<0) {
mbedalvaro 32:52273c3291fe 683 slidingVector=auxVector-(unitTowardsLight*aux*2);
mbedalvaro 31:5f039cbddee8 684 // slidingVector.scale(auxVector.length()); // rescale to the size of the initial speed.
mbedalvaro 31:5f039cbddee8 685 centerMass.setSpeed(slidingVector);
mbedalvaro 31:5f039cbddee8 686 }
mbedalvaro 31:5f039cbddee8 687
mbedalvaro 5:73cd58b58f95 688 }
mbedalvaro 27:1ce994629ffc 689
mbedalvaro 27:1ce994629ffc 690 // update dynamics for the central mass:
mbedalvaro 27:1ce994629ffc 691 #ifndef VERLET_METHOD
mbedalvaro 27:1ce994629ffc 692 centerMass.addDampingForce(); // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
mbedalvaro 27:1ce994629ffc 693 #endif
mbedalvaro 27:1ce994629ffc 694
mbedalvaro 27:1ce994629ffc 695 centerMass.update(); // unconstrained
mbedalvaro 27:1ce994629ffc 696 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
mbedalvaro 31:5f039cbddee8 697
mbedalvaro 31:5f039cbddee8 698 // Change color with touch? NO
mbedalvaro 31:5f039cbddee8 699 // if (displaySensingBuffer.lightTouched)
mbedalvaro 31:5f039cbddee8 700 // transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
mbedalvaro 31:5f039cbddee8 701 // else
mbedalvaro 31:5f039cbddee8 702 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 31:5f039cbddee8 703
mbedalvaro 31:5f039cbddee8 704
mbedalvaro 31:5f039cbddee8 705 break;
mbedalvaro 31:5f039cbddee8 706
mbedalvaro 32:52273c3291fe 707 case SPOT_BOUNCING_FACTOR:
mbedalvaro 32:52273c3291fe 708
mbedalvaro 32:52273c3291fe 709 centerMass.resetForce();
mbedalvaro 32:52273c3291fe 710
mbedalvaro 32:52273c3291fe 711 if (momentNorm>0) {
mbedalvaro 32:52273c3291fe 712 // (a) Compute the new speed (will use slidingVector as auxiliary vector2Df):
mbedalvaro 32:52273c3291fe 713 auxVector=centerMass.getSpeed();
mbedalvaro 32:52273c3291fe 714 float aux=unitTowardsLight.dot(auxVector);
mbedalvaro 32:52273c3291fe 715 if (aux<0) {
mbedalvaro 32:52273c3291fe 716 slidingVector=auxVector-(unitTowardsLight*aux*2); // symmetric speed with respet to unitTowardsLight
mbedalvaro 32:52273c3291fe 717 slidingVector.scale(auxVector.length()*factorAbsorptionShock); // rescale to the size of the initial speed.
mbedalvaro 32:52273c3291fe 718 centerMass.setSpeed(slidingVector);
mbedalvaro 32:52273c3291fe 719 }
mbedalvaro 32:52273c3291fe 720
mbedalvaro 32:52273c3291fe 721 // This is a hack: let's ADD spring force if the penetration is de facto large:
mbedalvaro 32:52273c3291fe 722 if (recenteringVectorLoop.length()>(saccadeRadius/4)) centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
mbedalvaro 32:52273c3291fe 723
mbedalvaro 32:52273c3291fe 724 // Also, to avoid "tunneling" through dark zones, let's translate the spot:
mbedalvaro 32:52273c3291fe 725 centerMass.posOld+=recenteringVectorLoop*0.3;
mbedalvaro 32:52273c3291fe 726 centerMass.pos+=recenteringVectorLoop*0.3;
mbedalvaro 32:52273c3291fe 727 }
mbedalvaro 32:52273c3291fe 728
mbedalvaro 32:52273c3291fe 729 // Gravity? - side or central attraction?
mbedalvaro 32:52273c3291fe 730 centerMass.addForce(gravity*centerMass.mass);
mbedalvaro 32:52273c3291fe 731
mbedalvaro 32:52273c3291fe 732 // or central spring attraction;
mbedalvaro 32:52273c3291fe 733 //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
mbedalvaro 32:52273c3291fe 734 //vector2Df dist=centerMass.pos-centerAttraction;
mbedalvaro 32:52273c3291fe 735 //centerMass.addForce(-dist*centerMass.mass*0.0007);
mbedalvaro 32:52273c3291fe 736
mbedalvaro 32:52273c3291fe 737 // or "radial gravity":
mbedalvaro 32:52273c3291fe 738 //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
mbedalvaro 32:52273c3291fe 739 //vector2Df dist=centerMass.pos-centerAttraction;
mbedalvaro 32:52273c3291fe 740 //centerMass.addForce(dist.normalize()*centerMass.mass*0.5);
mbedalvaro 32:52273c3291fe 741
mbedalvaro 32:52273c3291fe 742
mbedalvaro 32:52273c3291fe 743 // update dynamics for the central mass:
mbedalvaro 32:52273c3291fe 744 #ifndef VERLET_METHOD
mbedalvaro 32:52273c3291fe 745 centerMass.addDampingForce(); // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
mbedalvaro 32:52273c3291fe 746 #endif
mbedalvaro 32:52273c3291fe 747
mbedalvaro 32:52273c3291fe 748 centerMass.update(); // unconstrained
mbedalvaro 32:52273c3291fe 749 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
mbedalvaro 32:52273c3291fe 750
mbedalvaro 32:52273c3291fe 751 // Change color with touch? NO
mbedalvaro 32:52273c3291fe 752 // if (displaySensingBuffer.lightTouched)
mbedalvaro 32:52273c3291fe 753 // transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
mbedalvaro 32:52273c3291fe 754 // else
mbedalvaro 32:52273c3291fe 755 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 32:52273c3291fe 756
mbedalvaro 32:52273c3291fe 757
mbedalvaro 32:52273c3291fe 758 break;
mbedalvaro 31:5f039cbddee8 759
mbedalvaro 31:5f039cbddee8 760
mbedalvaro 31:5f039cbddee8 761 // ================================================================
mbedalvaro 31:5f039cbddee8 762 case SPOT_BOUNCING:
mbedalvaro 31:5f039cbddee8 763 // this is very simple: we need to give a force to the centralMass that is OPPOSITE to the recenteringVectorLoop vector.
mbedalvaro 31:5f039cbddee8 764 // We can also MODIFY the position so as to avoid having completely or partially the spot inside the dark zone (because of inertia).
mbedalvaro 31:5f039cbddee8 765 centerMass.resetForce();
mbedalvaro 31:5f039cbddee8 766
mbedalvaro 31:5f039cbddee8 767 if (momentNorm>0) { //(displaySensingBuffer.lightTouched) {
mbedalvaro 31:5f039cbddee8 768 // add force; MANY POSSIBILITIES:
mbedalvaro 31:5f039cbddee8 769 // (1) Constant in norm:
mbedalvaro 31:5f039cbddee8 770 //centerMass.addForce(unitTowardsLight*saccadeRadius*factorBouncingForce);
mbedalvaro 31:5f039cbddee8 771
mbedalvaro 31:5f039cbddee8 772 // Proportional to the penetration depth in the dark zone (spring):
mbedalvaro 31:5f039cbddee8 773 centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
mbedalvaro 31:5f039cbddee8 774 // Or proportional to the square (or something else) of the penetration:
mbedalvaro 31:5f039cbddee8 775 //centerMass.addForce(recenteringVectorLoop*normRecenteringVector*factorBouncingForce);
mbedalvaro 31:5f039cbddee8 776
mbedalvaro 31:5f039cbddee8 777 // Also, translate to avoid penetration (need to do this with pos and oldPos, otherwise speed will change):
mbedalvaro 31:5f039cbddee8 778 centerMass.posOld+=recenteringVectorLoop*0.1;
mbedalvaro 31:5f039cbddee8 779 centerMass.pos+=recenteringVectorLoop*0.1;
mbedalvaro 31:5f039cbddee8 780 //note: we don't change the speed, this would be just like the pacman: not really math modelling:
mbedalvaro 31:5f039cbddee8 781 // centerMass.setSpeed(-centerMass.getSpeed());
mbedalvaro 31:5f039cbddee8 782 }
mbedalvaro 31:5f039cbddee8 783
mbedalvaro 31:5f039cbddee8 784 // Gravity? - side or central attraction?
mbedalvaro 31:5f039cbddee8 785 centerMass.addForce(gravity*centerMass.mass);
mbedalvaro 31:5f039cbddee8 786
mbedalvaro 31:5f039cbddee8 787 // or central spring attraction;
mbedalvaro 31:5f039cbddee8 788 //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
mbedalvaro 31:5f039cbddee8 789 //vector2Df dist=centerMass.pos-centerAttraction;
mbedalvaro 31:5f039cbddee8 790 //centerMass.addForce(-dist*centerMass.mass*0.0007);
mbedalvaro 31:5f039cbddee8 791
mbedalvaro 31:5f039cbddee8 792 // or "radial gravity":
mbedalvaro 31:5f039cbddee8 793 //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
mbedalvaro 31:5f039cbddee8 794 //vector2Df dist=centerMass.pos-centerAttraction;
mbedalvaro 31:5f039cbddee8 795 //centerMass.addForce(dist.normalize()*centerMass.mass*0.5);
mbedalvaro 31:5f039cbddee8 796
mbedalvaro 31:5f039cbddee8 797
mbedalvaro 31:5f039cbddee8 798 // update dynamics for the central mass:
mbedalvaro 31:5f039cbddee8 799 #ifndef VERLET_METHOD
mbedalvaro 31:5f039cbddee8 800 centerMass.addDampingForce(); // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
mbedalvaro 31:5f039cbddee8 801 #endif
mbedalvaro 31:5f039cbddee8 802
mbedalvaro 31:5f039cbddee8 803 centerMass.update(); // unconstrained
mbedalvaro 31:5f039cbddee8 804 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
mbedalvaro 31:5f039cbddee8 805
mbedalvaro 31:5f039cbddee8 806 if (displaySensingBuffer.lightTouched) {
mbedalvaro 31:5f039cbddee8 807 // do collision damping:
mbedalvaro 31:5f039cbddee8 808 centerMass.setSpeed(centerMass.getSpeed()*0.99);
mbedalvaro 31:5f039cbddee8 809 }
mbedalvaro 31:5f039cbddee8 810
mbedalvaro 27:1ce994629ffc 811 // Change color with touch? YES
mbedalvaro 31:5f039cbddee8 812 if (displaySensingBuffer.lightTouched)
mbedalvaro 31:5f039cbddee8 813 transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
mbedalvaro 31:5f039cbddee8 814 else
mbedalvaro 31:5f039cbddee8 815 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 27:1ce994629ffc 816 break;
mbedalvaro 31:5f039cbddee8 817
mbedalvaro 30:d8af03f01cd4 818 // ================================================================
mbedalvaro 31:5f039cbddee8 819 case SPOT_AIR_HOCKEY:
mbedalvaro 27:1ce994629ffc 820 // this is very simple: we need to give a force to the centralMass that is OPPOSITE to the recenteringVectorLoop vector
mbedalvaro 27:1ce994629ffc 821 centerMass.resetForce();
mbedalvaro 27:1ce994629ffc 822
mbedalvaro 31:5f039cbddee8 823 if (displaySensingBuffer.lightTouched) {
mbedalvaro 31:5f039cbddee8 824 // add force; MANY POSSIBILITIES:
mbedalvaro 31:5f039cbddee8 825 // (1) Constant in norm:
mbedalvaro 31:5f039cbddee8 826 //centerMass.addForce(unitTowardsLight*saccadeRadius*factorBouncingForce);
mbedalvaro 31:5f039cbddee8 827 // Exactly what is needed to have an elastic bouncing:
mbedalvaro 31:5f039cbddee8 828
mbedalvaro 31:5f039cbddee8 829 // Proportional to the penetration depth in the dark zone (spring):
mbedalvaro 31:5f039cbddee8 830 centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
mbedalvaro 31:5f039cbddee8 831 // Or proportional to the square (or something else) of the penetration:
mbedalvaro 31:5f039cbddee8 832 //centerMass.addForce(recenteringVectorLoop*normRecenteringVector*factorBouncingForce);
mbedalvaro 31:5f039cbddee8 833
mbedalvaro 27:1ce994629ffc 834 }
mbedalvaro 31:5f039cbddee8 835
mbedalvaro 28:44b7b6e35548 836 // Gravity? - side or central attraction?
mbedalvaro 31:5f039cbddee8 837 //centerMass.addForce(gravity*centerMass.mass);
mbedalvaro 31:5f039cbddee8 838
mbedalvaro 28:44b7b6e35548 839 // or central spring attraction;
mbedalvaro 31:5f039cbddee8 840 //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
mbedalvaro 28:44b7b6e35548 841 //vector2Df dist=centerMass.pos-centerAttraction;
mbedalvaro 28:44b7b6e35548 842 //centerMass.addForce(-dist*centerMass.mass*0.0007);
mbedalvaro 31:5f039cbddee8 843
mbedalvaro 28:44b7b6e35548 844 // or "radial gravity":
mbedalvaro 31:5f039cbddee8 845 //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
mbedalvaro 28:44b7b6e35548 846 //vector2Df dist=centerMass.pos-centerAttraction;
mbedalvaro 28:44b7b6e35548 847 //centerMass.addForce(dist.normalize()*centerMass.mass*0.5);
mbedalvaro 31:5f039cbddee8 848
mbedalvaro 1:a4050fee11f7 849
mbedalvaro 25:74cb85b85fd2 850 // update dynamics for the central mass:
mbedalvaro 1:a4050fee11f7 851 #ifndef VERLET_METHOD
mbedalvaro 1:a4050fee11f7 852 centerMass.addDampingForce(); // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
mbedalvaro 1:a4050fee11f7 853 #endif
mbedalvaro 1:a4050fee11f7 854
mbedalvaro 1:a4050fee11f7 855 centerMass.update(); // unconstrained
mbedalvaro 1:a4050fee11f7 856 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
mbedalvaro 31:5f039cbddee8 857
mbedalvaro 31:5f039cbddee8 858 if (displaySensingBuffer.lightTouched) {
mbedalvaro 31:5f039cbddee8 859 // do collision damping:
mbedalvaro 31:5f039cbddee8 860 centerMass.setSpeed(centerMass.getSpeed()*0.99);
mbedalvaro 31:5f039cbddee8 861 }
mbedalvaro 31:5f039cbddee8 862
mbedalvaro 27:1ce994629ffc 863 // Change color with touch? YES
mbedalvaro 31:5f039cbddee8 864 if (displaySensingBuffer.lightTouched)
mbedalvaro 31:5f039cbddee8 865 transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
mbedalvaro 31:5f039cbddee8 866 else
mbedalvaro 31:5f039cbddee8 867 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 31:5f039cbddee8 868
mbedalvaro 31:5f039cbddee8 869 // In case of "air hockey mode", reset position to initial positions and speeds when the spot touches any two opposites sides:
mbedalvaro 31:5f039cbddee8 870 if ((centerMass.innerCollitionDirection.x==1)||( centerMass.innerCollitionDirection.x==-1)) {
mbedalvaro 31:5f039cbddee8 871 transientBlobColor=blobColor|0x02;
mbedalvaro 30:d8af03f01cd4 872 for (saccadeRadius=30; saccadeRadius<900 ; saccadeRadius+=10) {
mbedalvaro 30:d8af03f01cd4 873 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
mbedalvaro 31:5f039cbddee8 874 draw();
mbedalvaro 30:d8af03f01cd4 875 }
mbedalvaro 30:d8af03f01cd4 876 saccadeRadius=saccadeRadius_initial;
mbedalvaro 30:d8af03f01cd4 877 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
mbedalvaro 30:d8af03f01cd4 878 // reset to central position:
mbedalvaro 31:5f039cbddee8 879 centerMass.setInitialCondition(startCenter, startSpeed);
mbedalvaro 30:d8af03f01cd4 880 transientBlobColor=blobColor;
mbedalvaro 31:5f039cbddee8 881 }
mbedalvaro 28:44b7b6e35548 882 break;
mbedalvaro 31:5f039cbddee8 883
mbedalvaro 31:5f039cbddee8 884
mbedalvaro 31:5f039cbddee8 885 // ================================================================
mbedalvaro 31:5f039cbddee8 886 case SPOT_LORENTZ_FORCE:
mbedalvaro 28:44b7b6e35548 887 // this is very simple: we need to give a force to the centralMass that is OPPOSITE to the recenteringVectorLoop vector
mbedalvaro 28:44b7b6e35548 888 centerMass.resetForce();
mbedalvaro 28:44b7b6e35548 889
mbedalvaro 31:5f039cbddee8 890 if (displaySensingBuffer.lightTouched) {
mbedalvaro 31:5f039cbddee8 891 // add force; MANY POSSIBILITIES:
mbedalvaro 31:5f039cbddee8 892 // (1) Constant in norm:
mbedalvaro 31:5f039cbddee8 893 //centerMass.addForce(unitTowardsLight*saccadeRadius*factorBouncingForce);
mbedalvaro 31:5f039cbddee8 894 // Exactly what is needed to have an elastic bouncing:
mbedalvaro 31:5f039cbddee8 895
mbedalvaro 31:5f039cbddee8 896 // Proportional to the penetration depth in the dark zone (spring):
mbedalvaro 31:5f039cbddee8 897 centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
mbedalvaro 31:5f039cbddee8 898 // Or proportional to the square (or something else) of the penetration:
mbedalvaro 31:5f039cbddee8 899 //centerMass.addForce(recenteringVectorLoop*normRecenteringVector*factorBouncingForce);
mbedalvaro 31:5f039cbddee8 900
mbedalvaro 28:44b7b6e35548 901 }
mbedalvaro 31:5f039cbddee8 902
mbedalvaro 28:44b7b6e35548 903 // RADIAL GRAVITY for the "fountain mode":
mbedalvaro 31:5f039cbddee8 904 // vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
mbedalvaro 31:5f039cbddee8 905 // vector2Df radialVector=centerMass.pos-centerAttraction;
mbedalvaro 31:5f039cbddee8 906 // radialVector.rotateDeg(slidingDirection? 80 : 260);
mbedalvaro 31:5f039cbddee8 907 // centerMass.addForce(radialVector.normalize()*centerMass.mass*0.5);
mbedalvaro 31:5f039cbddee8 908
mbedalvaro 28:44b7b6e35548 909 // bubble chamber? LORENTZ FORCE:
mbedalvaro 28:44b7b6e35548 910 vector2Df speedmass=centerMass.getSpeed();
mbedalvaro 28:44b7b6e35548 911 centerMass.addForce( speedmass.getRotatedDeg(90)*0.000002*speedContourFollowing);
mbedalvaro 31:5f039cbddee8 912
mbedalvaro 28:44b7b6e35548 913 // update dynamics for the central mass:
mbedalvaro 28:44b7b6e35548 914 #ifndef VERLET_METHOD
mbedalvaro 28:44b7b6e35548 915 centerMass.addDampingForce(); // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
mbedalvaro 28:44b7b6e35548 916 #endif
mbedalvaro 28:44b7b6e35548 917
mbedalvaro 28:44b7b6e35548 918 centerMass.update(); // unconstrained
mbedalvaro 28:44b7b6e35548 919 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
mbedalvaro 31:5f039cbddee8 920
mbedalvaro 31:5f039cbddee8 921 if (displaySensingBuffer.lightTouched) {
mbedalvaro 31:5f039cbddee8 922 // do collision damping:
mbedalvaro 31:5f039cbddee8 923 centerMass.setSpeed(centerMass.getSpeed()*0.99);
mbedalvaro 31:5f039cbddee8 924 }
mbedalvaro 31:5f039cbddee8 925
mbedalvaro 28:44b7b6e35548 926 // Change color with touch? YES
mbedalvaro 31:5f039cbddee8 927 if (displaySensingBuffer.lightTouched)
mbedalvaro 31:5f039cbddee8 928 transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
mbedalvaro 31:5f039cbddee8 929 else
mbedalvaro 31:5f039cbddee8 930 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 31:5f039cbddee8 931
mbedalvaro 28:44b7b6e35548 932 // In case of "fountain mode", reset position to initial positions and speeds, or change gravity sign:
mbedalvaro 31:5f039cbddee8 933 // if (blobWallCollision) centerMass.setInitialCondition(startCenter, startSpeed);
mbedalvaro 31:5f039cbddee8 934 if (blobWallCollision) slidingDirection=!slidingDirection;
mbedalvaro 1:a4050fee11f7 935 break;
mbedalvaro 30:d8af03f01cd4 936 // ================================================================
mbedalvaro 1:a4050fee11f7 937 }
mbedalvaro 31:5f039cbddee8 938
mbedalvaro 1:a4050fee11f7 939 }
mbedalvaro 1:a4050fee11f7 940
mbedalvaro 1:a4050fee11f7 941
mbedalvaro 1:a4050fee11f7 942 // Drawing the graphics - this will in fact use the graphic renderer - if any - and produce the trajectory to be displayed by the laser
mbedalvaro 31:5f039cbddee8 943 void rigidLoop::draw()
mbedalvaro 31:5f039cbddee8 944 {
mbedalvaro 1:a4050fee11f7 945 // for the time being, there is no "opengl" like renderer, so we just copy into the lsdTrajectory:
mbedalvaro 1:a4050fee11f7 946 float cx= centerMass.pos.x;
mbedalvaro 1:a4050fee11f7 947 float cy= centerMass.pos.y;
mbedalvaro 1:a4050fee11f7 948 for (int i = 0; i < numPoints; i++) {
mbedalvaro 1:a4050fee11f7 949 // The shape is drawn by translating the scafold shape (centered on centerMass):
mbedalvaro 12:0de9cd2bced5 950 displaySensingBuffer.lsdTrajectory[i].x= (unsigned short)(bluePrint.scafold[i].x + cx ); // note: it should be an unsigned short!!
mbedalvaro 12:0de9cd2bced5 951 displaySensingBuffer.lsdTrajectory[i].y= (unsigned short)(bluePrint.scafold[i].y + cy );
mbedalvaro 31:5f039cbddee8 952
mbedalvaro 12:0de9cd2bced5 953 // We can also do this, but ATTENTION: centerMass.pos is a vector2Df, and scafold[i] is a vector2Dd (typecasting?)
mbedalvaro 12:0de9cd2bced5 954 // displaySensingBuffer.lsdTrajectory[i]= bluePrint.scafold[i] + centerMass.pos;
mbedalvaro 31:5f039cbddee8 955
mbedalvaro 25:74cb85b85fd2 956 //displaySensingBuffer.displayColor=blobColor; // perhaps per point color is not a good idea for the time being...
mbedalvaro 1:a4050fee11f7 957 }
mbedalvaro 31:5f039cbddee8 958
mbedalvaro 25:74cb85b85fd2 959 // Global color for the whole loop:
mbedalvaro 27:1ce994629ffc 960 displaySensingBuffer.displayColor=transientBlobColor;
mbedalvaro 31:5f039cbddee8 961
mbedalvaro 1:a4050fee11f7 962 }
mbedalvaro 1:a4050fee11f7 963
mbedalvaro 31:5f039cbddee8 964 void rigidLoop::computeBoundingBox()
mbedalvaro 31:5f039cbddee8 965 {
mbedalvaro 1:a4050fee11f7 966 }
mbedalvaro 1:a4050fee11f7 967
mbedalvaro 1:a4050fee11f7 968
mbedalvaro 1:a4050fee11f7 969
mbedalvaro 31:5f039cbddee8 970 void rigidLoop::sendDataSpecific()
mbedalvaro 31:5f039cbddee8 971 {
mbedalvaro 1:a4050fee11f7 972 char auxstring[10];
mbedalvaro 1:a4050fee11f7 973 myled2=1; // for tests...
mbedalvaro 1:a4050fee11f7 974
mbedalvaro 1:a4050fee11f7 975 // First, set the top address of the message to the ID of the blob (not the name):
mbedalvaro 31:5f039cbddee8 976 // sprintf(auxstring, "%d", identifier);
mbedalvaro 31:5f039cbddee8 977 // sendMes.setTopAddress("0");//auxstring);
mbedalvaro 1:a4050fee11f7 978
mbedalvaro 1:a4050fee11f7 979 // ===================== OSC ======================
mbedalvaro 1:a4050fee11f7 980 if (sendOSC) {
mbedalvaro 1:a4050fee11f7 981
mbedalvaro 1:a4050fee11f7 982 // (a) Anchor mass:
mbedalvaro 1:a4050fee11f7 983 if (sendingAnchorPosition) {
mbedalvaro 31:5f039cbddee8 984 sprintf(auxstring, "/p %d",identifier);
mbedalvaro 18:d72935b13858 985 sendMes.setSubAddress(auxstring);
mbedalvaro 1:a4050fee11f7 986 long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 987 x=(long)(centerMass.pos.x);
mbedalvaro 1:a4050fee11f7 988 y=(long)(centerMass.pos.y);
mbedalvaro 1:a4050fee11f7 989 sendMes.setArgs( "ii", &x, &y);
mbedalvaro 1:a4050fee11f7 990 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 991 }
mbedalvaro 1:a4050fee11f7 992
mbedalvaro 1:a4050fee11f7 993 // (b) data from blob points (this is ONLY FOR TESTS, because the loop is rigid - sending the center is enough)
mbedalvaro 1:a4050fee11f7 994 if (sendingLoopPositions) {
mbedalvaro 1:a4050fee11f7 995 #ifdef SEND_AS_POINTS
mbedalvaro 1:a4050fee11f7 996 long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 997 float cx= centerMass.pos.x;
mbedalvaro 1:a4050fee11f7 998 float cy= centerMass.pos.y;
mbedalvaro 1:a4050fee11f7 999 for (int i = 0; i < numPoints; i++) {
mbedalvaro 2:34157ebbf56b 1000 sprintf(auxstring, "/p%d",identifier*10+ i);//20+ i+(identifier-1)*10); // auxstring read as "/p1", "/p2", ...
mbedalvaro 1:a4050fee11f7 1001 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 1002 x=(long)(bluePrint.scafold[i].x + cx);
mbedalvaro 1:a4050fee11f7 1003 y=(long)(bluePrint.scafold[i].y + cy);
mbedalvaro 1:a4050fee11f7 1004 sendMes.setArgs( "ii", &x, &y);
mbedalvaro 1:a4050fee11f7 1005 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 1006 }
mbedalvaro 1:a4050fee11f7 1007
mbedalvaro 1:a4050fee11f7 1008 #endif
mbedalvaro 1:a4050fee11f7 1009 #ifdef SEND_AS_BLOB
mbedalvaro 1:a4050fee11f7 1010 sendMes.clearArgs(); // no need, we won't use osc.sendOsc()...
mbedalvaro 1:a4050fee11f7 1011 uint8_t blobdata[4*numPoints]; // 2 bytes per coordinate, and 2 coordinates
mbedalvaro 1:a4050fee11f7 1012 float cx= centerMass.pos.x;
mbedalvaro 1:a4050fee11f7 1013 float cy= centerMass.pos.y;
mbedalvaro 1:a4050fee11f7 1014 for (int i = 0; i < numPoints; i++ ) {
mbedalvaro 1:a4050fee11f7 1015 // note: massesLoop[i].pos.x is a "float"
mbedalvaro 1:a4050fee11f7 1016 uint16_t x=(uint16_t)(bluePrint.scafold[i].x + cx);
mbedalvaro 1:a4050fee11f7 1017 blobdata[4*i]=(uint8_t)x>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
mbedalvaro 1:a4050fee11f7 1018 blobdata[4*i+1]=(uint8_t)x;
mbedalvaro 1:a4050fee11f7 1019
mbedalvaro 1:a4050fee11f7 1020 uint16_t y=(uint16_t)(bluePrint.scafold[i].y + cy);
mbedalvaro 1:a4050fee11f7 1021 blobdata[4*i+2]=(uint8_t)y>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
mbedalvaro 1:a4050fee11f7 1022 blobdata[4*i+3]=(uint8_t)y;
mbedalvaro 1:a4050fee11f7 1023 }
mbedalvaro 1:a4050fee11f7 1024 osc.sendOscBlob(&(blobdata[0]), 4*numPoints, &sendMes ); // second parameter is osc blob size in bytes
mbedalvaro 1:a4050fee11f7 1025 #endif
mbedalvaro 1:a4050fee11f7 1026 #ifdef SEND_AS_STRING
mbedalvaro 1:a4050fee11f7 1027 sendMes.clearArgs(); // no need, we won't use osc.sendOsc()...
mbedalvaro 1:a4050fee11f7 1028 uint8_t blobdata[4*numPoints]; // 2 bytes per coordinate, and 2 coordinates
mbedalvaro 1:a4050fee11f7 1029 float cx= centerMass.pos.x;
mbedalvaro 1:a4050fee11f7 1030 float cy= centerMass.pos.y;
mbedalvaro 1:a4050fee11f7 1031 for (int i = 0; i < numPoints; i++ ) {
mbedalvaro 1:a4050fee11f7 1032 // note: massesLoop[i].pos.x is a "float"
mbedalvaro 1:a4050fee11f7 1033 uint16_t x=(uint16_t)(bluePrint.scafold[i].x + cx );
mbedalvaro 1:a4050fee11f7 1034 blobdata[4*i]=(uint8_t)x>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
mbedalvaro 1:a4050fee11f7 1035 blobdata[4*i+1]=(uint8_t)x;
mbedalvaro 1:a4050fee11f7 1036
mbedalvaro 1:a4050fee11f7 1037 uint16_t y=(uint16_t)(bluePrint.scafold[i].y + cy);
mbedalvaro 1:a4050fee11f7 1038 blobdata[4*i+2]=(uint8_t)y>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
mbedalvaro 1:a4050fee11f7 1039 blobdata[4*i+3]=(uint8_t)y;
mbedalvaro 1:a4050fee11f7 1040 }
mbedalvaro 1:a4050fee11f7 1041 osc.sendOscString(blobdata, 4*numPoints, &sendMes ); // second parameter is osc blob size in bytes
mbedalvaro 1:a4050fee11f7 1042 #endif
mbedalvaro 1:a4050fee11f7 1043 }
mbedalvaro 1:a4050fee11f7 1044 if (sendingLoopRegions) {
mbedalvaro 1:a4050fee11f7 1045 long x; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 1046 for (int i = 0; i < numPoints; i++) {
mbedalvaro 1:a4050fee11f7 1047 sprintf(auxstring, "/r%d", i); // auxstring read as "/f1", "/f2", ...
mbedalvaro 1:a4050fee11f7 1048 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 1049 x=(long)(displaySensingBuffer.lsdTrajectory[i].lightZone>0? 1 : 0);
mbedalvaro 1:a4050fee11f7 1050 sendMes.setArgs( "i", &x);
mbedalvaro 1:a4050fee11f7 1051 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 1052 }
mbedalvaro 1:a4050fee11f7 1053 }
mbedalvaro 1:a4050fee11f7 1054 if (sendingLoopTouchWall) { // global touch wall for the loop (not per point)
mbedalvaro 1:a4050fee11f7 1055 long wall; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 1056 sprintf(auxstring, "/bWall");
mbedalvaro 1:a4050fee11f7 1057 sendMes.setSubAddress(auxstring);
mbedalvaro 1:a4050fee11f7 1058 wall=(long)(blobWallCollision? 1 : 0);
mbedalvaro 1:a4050fee11f7 1059 sendMes.setArgs( "i", &wall);
mbedalvaro 1:a4050fee11f7 1060 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 1061 }
mbedalvaro 1:a4050fee11f7 1062
mbedalvaro 1:a4050fee11f7 1063 // (d) Light sensing statistics:
mbedalvaro 1:a4050fee11f7 1064 if (sendingBlobMaxMin) {
mbedalvaro 1:a4050fee11f7 1065 sendMes.setSubAddress("/maxmin");
mbedalvaro 1:a4050fee11f7 1066 long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 1067 x=(long)(displaySensingBuffer.maxI);
mbedalvaro 1:a4050fee11f7 1068 y=(long)(displaySensingBuffer.minI);
mbedalvaro 1:a4050fee11f7 1069 sendMes.setArgs( "ii", &x, &y);
mbedalvaro 1:a4050fee11f7 1070 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 1071 }
mbedalvaro 1:a4050fee11f7 1072
mbedalvaro 1:a4050fee11f7 1073 // (e) Recentering vector: (note: redundant with sendingLightForce, IF the correction angle is known).
mbedalvaro 1:a4050fee11f7 1074 if (sendingRecenteringVector) {
mbedalvaro 1:a4050fee11f7 1075 sendMes.setSubAddress("/rvector");
mbedalvaro 1:a4050fee11f7 1076 long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 1077 x=(long)(recenteringVectorLoop.x);
mbedalvaro 1:a4050fee11f7 1078 y=(long)(recenteringVectorLoop.y);
mbedalvaro 1:a4050fee11f7 1079 sendMes.setArgs( "ii", &x, &y);
mbedalvaro 1:a4050fee11f7 1080 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 1081 }
mbedalvaro 1:a4050fee11f7 1082 if (sendingRecenteringAngle) {
mbedalvaro 18:d72935b13858 1083 sprintf(auxstring, "/v %d",identifier);
mbedalvaro 18:d72935b13858 1084 sendMes.setSubAddress(auxstring);
mbedalvaro 1:a4050fee11f7 1085 long x; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 1086 x=(long)(angleRecenteringVector);
mbedalvaro 1:a4050fee11f7 1087 sendMes.setArgs( "i", &x);
mbedalvaro 1:a4050fee11f7 1088 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 1089 }
mbedalvaro 1:a4050fee11f7 1090 if (sendingRecenteringNorm) {
mbedalvaro 1:a4050fee11f7 1091 sendMes.setSubAddress("/rnorm");
mbedalvaro 1:a4050fee11f7 1092 long x; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 1093 x=(long)(normRecenteringVector);
mbedalvaro 1:a4050fee11f7 1094 sendMes.setArgs( "i", &x);
mbedalvaro 1:a4050fee11f7 1095 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 1096 }
mbedalvaro 1:a4050fee11f7 1097
mbedalvaro 1:a4050fee11f7 1098 if (sendingTouched) {
mbedalvaro 1:a4050fee11f7 1099 if (displaySensingBuffer.lightTouched) {
mbedalvaro 1:a4050fee11f7 1100 sendMes.clearArgs(); // there are no arguments to send
mbedalvaro 1:a4050fee11f7 1101 sendMes.setSubAddress("/touched");
mbedalvaro 1:a4050fee11f7 1102 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 1103 }
mbedalvaro 1:a4050fee11f7 1104 }
mbedalvaro 1:a4050fee11f7 1105
mbedalvaro 1:a4050fee11f7 1106 } // end of OSC sending per-spot
mbedalvaro 1:a4050fee11f7 1107
mbedalvaro 1:a4050fee11f7 1108 // ===================== SERIAL ======================
mbedalvaro 1:a4050fee11f7 1109 if (sendSerial) {
mbedalvaro 1:a4050fee11f7 1110 //.. to do
mbedalvaro 1:a4050fee11f7 1111 }
mbedalvaro 1:a4050fee11f7 1112
mbedalvaro 1:a4050fee11f7 1113 myled2=0; // for tests...
mbedalvaro 1:a4050fee11f7 1114 }