Alvaro Cassinelli / Mbed 2 deprecated skinGames_forktest

just a test

Dependencies:   mbed

Fork of scoreLight_Advanced by Alvaro Cassinelli

rigidLoop.cpp@32:52273c3291fe, 2012-10-29 (annotated)

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