Diff: rigidLoop.cpp

Revision:

7:0df17f3078bc

Parent:

5:73cd58b58f95

Child:

12:0de9cd2bced5

--- a/rigidLoop.cpp	Thu Apr 05 13:06:24 2012 +0000
+++ b/rigidLoop.cpp	Sat Apr 07 14:46:51 2012 +0000
@@ -43,7 +43,7 @@
             saccadeRadius=150;
 
             // default (initial) shape (the scafold belongs to the base class):
-            bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 16); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
+            bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 14); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
 
             // Note: We may assume NO MASS for the center of the contour following loop. Adding mass may be interesting though (smooth motion).
             massCenter=0.01;
@@ -58,8 +58,8 @@
             // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
             // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
             // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
-            displaySensingBuffer.setDelayMirrors(0);
-            angleCorrectionForceLoop=360.0/bluePrint.scafold.size()/2; // in DEGREES
+            displaySensingBuffer.setDelayMirrors(1);
+            angleCorrectionForceLoop=0;//360.0/bluePrint.scafold.size()/2; // in DEGREES
 
             break;
 
@@ -72,7 +72,8 @@
             setColor(0x04);
 
             // default (initial) shape (the scafold belongs to the base class):
-            bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 10); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
+            saccadeRadius=35;
+            bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
 
             // Note: We may assume NO MASS for the center of the contour following loop. Adding mass may be interesting though (smooth motion).
             massCenter=0.01;
@@ -85,13 +86,17 @@
             setRegionMotion(MIN_AD_MIRRORS+saccadeRadius, MIN_AD_MIRRORS+saccadeRadius, MAX_AD_MIRRORS-saccadeRadius, MAX_AD_MIRRORS-saccadeRadius);
 
             slidingDirection=true; //  For contour following (will change direction when touching wall)
-            speedContourFollowing=0.03;
+            speedContourFollowing=1.3*saccadeRadius;
+            justSearched=false;
 
-            // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
+            // per-blob mirror delay: ONLY USEFUL FOR ELASTIC BLOBS, because otherwise it can be corrected by "angleCorrection"
+            // (if things were well adjusted - in particular mirror waiting times, then this could be 0.
             // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
-            // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
-            displaySensingBuffer.setDelayMirrors(0);
-            angleCorrectionForceLoop=-29;//360.0/bluePrint.scafold.size()/2; // in DEGREES
+            // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop.
+            // 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 
+            // optimal value, and finish the correction (fine tunned) with the angle correction (only possible in the case of circular rigid blob).
+            displaySensingBuffer.setDelayMirrors(3); // this corresponds to an angular correction of -delayMirrors * 360/numPoints
+            angleCorrectionForceLoop=-5;//-65;//360.0/bluePrint.scafold.size()/2; // in DEGREES
 
             break;
 
@@ -102,14 +107,14 @@
             //setColor(0x07);//0x04+0x02>>i);
             setColor(0x04);
 
-            saccadeRadius=65;
+            saccadeRadius=85;
             // default (initial) shape (the scafold belongs to the base class):
-            bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 16); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
+            bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
 
             // Numeric parameters for the simulated mechanical system:
-            massCenter=0.012;
-            dampMotionCenterMass=0.0001;//0.00015;//00003;
-            factorBouncingForce=0.0025; // this is because we will use a force on the central mass
+            massCenter=0.0012;
+            dampMotionCenterMass=0.0005;//0.00015;//00003;
+            factorBouncingForce=0.0018; // this is because we will use a force on the central mass
 
             // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
             createLoopFromScafold();
@@ -120,11 +125,13 @@
             // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
             // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
             // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
-            displaySensingBuffer.setDelayMirrors(0);
-            angleCorrectionForceLoop=-28.5+180;//-29+180;//360.0/bluePrint.scafold.size()/2; // in DEGREES
+            displaySensingBuffer.setDelayMirrors(3);
+            angleCorrectionForceLoop=-5;// in degrees
 
             break;
     }
+    
+    saccadeRadius_initial=saccadeRadius; // this is for search mode for instance. 
 
 }
 
@@ -168,26 +175,37 @@
     // 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
     // sum in the circle is 0).
     vector2D momentVector(0,0);
-    for (int i = 0; i < numPoints; i++) {
+    int counterDarkZone=0; // note: for a VERY strange reason, if I put this on the laserSensingtrajectory class, the program does not work anymore!!
+    for (int i = 0; i < numPoints; i++) { // note: numPoints should be EVEN
         if (displaySensingBuffer.lsdTrajectory[i].lightZone>0) { // this is, we are in a dark zone (better to integrate there, because it is normally smaller)
             momentVector+=bluePrint.scafold[i];// note: no need to do -centerMass.pos, because the scafold is "centered" around 0
+            counterDarkZone++;
         }
     }
+    momentVector=momentVector*(2*PI/numPoints);
+    float momentNorm=momentVector.length(); // = 2.R.sin(half_angle) in the direction of the dark zone
     
-//    if (displaySensingBuffer.lightRatio>0.5) momentVector*=-1.0;
-//    if (displaySensingBuffer.coko<numPoints/2) momentVector*=-1.0;
-    
-    float momentNorm=momentVector.length(); // = 2.R.sin(half_angle) in the direction of the dark zone
+    vector2D unitTowardsLight; // this is the normed vector, pointing towards the light zone
     if (momentNorm==0) {
+        unitTowardsLight.set(0,0);
         recenteringVectorLoop.set(0,0);
         normRecenteringVector=0;
         angleRecenteringVector=0;
     } else {
-        float aux=saccadeRadius/momentNorm;
-        if (aux>0.5) recenteringVectorLoop=momentVector*sqrt(aux*aux-0.25);
-        else recenteringVectorLoop=momentVector*sqrt(0.25-aux*aux);
-        //Rotate by angleCorrection (to correct delays, etc):
-        recenteringVectorLoop.rotateDeg(angleCorrectionForceLoop);
+         unitTowardsLight=momentVector/(-1.0*momentNorm);
+         // Apply correction angle (delay mirrors):
+        unitTowardsLight.rotateDeg(angleCorrectionForceLoop);
+         
+         // Compute "recenteringVectorLoop" (in fact, the vector making the spot goes completely away form the dark zone):
+         float aux=0.5*momentNorm/saccadeRadius; // note: in principle, we ALWAYS have momentNorm < 2.R, so aux < 1
+         if (aux>1) aux=1.0; // can happen because of the discrete integration!
+         if (counterDarkZone<=numPoints/2) { // note: numPoints HAS to be EVEN
+            recenteringVectorLoop=unitTowardsLight*saccadeRadius*(1.0-sqrt(1.0-aux*aux));
+            } else {
+             recenteringVectorLoop=unitTowardsLight*saccadeRadius*(1.0+sqrt(1.0-aux*aux));
+             }
+    
+      
         // Compute redundant quantities (if necessary, for sending through OSC, etc):
         normRecenteringVector=recenteringVectorLoop.length();
         angleRecenteringVector=recenteringVectorLoop.angleDegHoriz();
@@ -211,20 +229,32 @@
             if (momentNorm>0) {
                 //momentVector/=momentNorm;
                 // We can now compute the sliding vector as:
-                slidingVector=recenteringVectorLoop.getRotatedDeg(slidingDirection? 90 : -90) / normRecenteringVector * speedContourFollowing*saccadeRadius;
-                // if (displaySensingBuffer.coko<numPoints/2) slidingVector*=-1.0;
-                // Then the final correcting vector is the sum of sliding plus recentering vector (with a factor if one want some smothing)
+                slidingVector=unitTowardsLight.getRotatedDeg(slidingDirection? 90 : -90) * speedContourFollowing;
+                
+                // Then the final correcting vector is the sum of sliding plus a recentering vector (with a factor if one want some smothing)
                 // 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):
-              
-                centerMass.pos += slidingVector + recenteringVectorLoop * 0.2;
-                
-              //  centerMass.pos += recenteringVectorLoop*0.1; // ATTENTION!!! the REAL radius may be smaller if the mirrors are running fast!!!
+      
+                centerMass.pos +=slidingVector+ ( unitTowardsLight*(-1.0*saccadeRadius) + recenteringVectorLoop )* 0.6;
+               // ATTENTION!!! the REAL radius may be smaller if the mirrors are running fast!!! (hence the last factor, that is not only for "smoothing" the 
+               // re-entry and avoid oscillations). 
 
                 // The following function can help constraining the position "pos", but it also does too much. Do something simpler perhaps?
+                centerMass.posOld=centerMass.pos; // this is necessary to compute bouceOffWalls using Verlet method... (MAKE A new variable INTEGRATION METHOD?)
+                
                 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
+                
+                if (justSearched) {
+                    saccadeRadius=saccadeRadius_initial;
+                    bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), numPoints);
+                    justSearched=false;
+                    }
+                   
 
             } else {
-                // not on something. Do nothing for the time being
+                // not on something. SEARCH MODE (or go to spot_bouncing mode?)
+                saccadeRadius+=10; if (saccadeRadius>800) saccadeRadius=saccadeRadius_initial;
+                bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), numPoints);
+                justSearched=true;
             }
 
             break;
@@ -232,11 +262,18 @@
         case  SPOT_BOUNCING:
             // this is very simple: we need to give a force to the centralMass that is OPPOSITE to the recenteringVectorLoop vector
             centerMass.resetForce();
-           
 
  if (displaySensingBuffer.lightTouched) {
-            // add force:
+            // add force; MANY POSSIBILITIES:
+            // (1) Constant in norm:
+            //centerMass.addForce(unitTowardsLight*saccadeRadius*factorBouncingForce);
+            // Exactly what is needed to have an elastic bouncing:
+            
+            // Proportional to the penetration depth in the dark zone (spring):
              centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
+             // Or proportional to the square (or something else) of the penetration:
+             //centerMass.addForce(recenteringVectorLoop*normRecenteringVector*factorBouncingForce);
+             
             }
 
             // update dynamics for the central mass::
@@ -249,7 +286,7 @@
             
              if (displaySensingBuffer.lightTouched) {
             // do collision damping:
-             centerMass.setSpeed(centerMass.getSpeed()*0.99);
+               centerMass.setSpeed(centerMass.getSpeed()*0.99);
              }
 
 
@@ -260,7 +297,7 @@
     // OTHER PARTICULAR THINGS:
     // change sliding direction (for countour following):
     if (blobWallCollision) {
-        if (wallCounter>10) {
+        if (wallCounter>5) {
             slidingDirection=!slidingDirection;
             wallCounter=0;
         }