Alvaro Cassinelli / Mbed 2 deprecated skinGames_forktest

just a test

Dependencies:   mbed

Fork of scoreLight_Advanced by Alvaro Cassinelli

rigidLoop.cpp@48:7633d8e7b0d3, 2014-12-02 (annotated)

Committer:
mbedalvaro
Date:
Tue Dec 02 04:28:42 2014 +0000
Revision:
48:7633d8e7b0d3
Parent:
47:2312a8dc9658 
this is the working version of the skin games sowtware (aka, scorelight but with pre-determined "games")

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