Alvaro Cassinelli / Mbed 2 deprecated skinGames_forktest

just a test

Dependencies:   mbed

Fork of scoreLight_Advanced by Alvaro Cassinelli

rigidLoop.cpp@45:a3b984a79d5d, 2014-04-01 (annotated)

Committer:
mbedalvaro
Date:
Tue Apr 01 10:08:49 2014 +0000
Revision:
45:a3b984a79d5d
Parent:
44:46e25fa1669b
Child:
46:90516893793a
working on tracking mode;

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