Alvaro Cassinelli / Mbed 2 deprecated skinGames_II

save loops

Dependencies:   mbed

rigidLoop.cpp@1:3be7b7d050f4, 2014-12-02 (annotated)

Committer:
mbedalvaro
Date:
Tue Dec 02 08:29:59 2014 +0000
Revision:
1:3be7b7d050f4
Parent:
0:df6fdd9b99f0 
updated

Who changed what in which revision?

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