Diff: rigidLoop.cpp

Revision:

5:73cd58b58f95

Parent:

4:f9d364f10335

Child:

7:0df17f3078bc

--- a/rigidLoop.cpp	Wed Apr 04 10:05:25 2012 +0000
+++ b/rigidLoop.cpp	Thu Apr 05 12:29:14 2012 +0000
@@ -26,7 +26,7 @@
 
     // (3) initialize common variables for the different modes of this rigid loop (even if some are not used, better not to have more subclasses...)
 
-    angleCorrectionForceLoop=0; // this will depend on the size of the blob (in principle), and number of blobs...
+  //  angleCorrectionForceLoop=0; // this will depend on the size of the blob (in principle), and number of blobs...
 
 
 // (3) Initialize secondary variables depending on the behaviour mode (may be changed afterwards in real time)
@@ -40,10 +40,10 @@
             //setColor(0x07);//0x04+0x02>>i);
             setColor(0x04);
 
-            saccadeRadius=250;
+            saccadeRadius=150;
 
             // 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);
+            bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 16); //(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;
@@ -55,15 +55,16 @@
             // Excursion limits (this 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).
             setRegionMotion(MIN_AD_MIRRORS+saccadeRadius, MIN_AD_MIRRORS+saccadeRadius, MAX_AD_MIRRORS-saccadeRadius, MAX_AD_MIRRORS-saccadeRadius);
 
-            // 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):
+            // 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
 
             break;
 
         case SPOT_FOLLOWING:
 
-
             // Name of this kind of spot:
             sprintf(spotName,"rigid_following");
 
@@ -83,9 +84,14 @@
             // Excursion limits (this 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).
             setRegionMotion(MIN_AD_MIRRORS+saccadeRadius, MIN_AD_MIRRORS+saccadeRadius, MAX_AD_MIRRORS-saccadeRadius, MAX_AD_MIRRORS-saccadeRadius);
 
-            // 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):
-            displaySensingBuffer.setDelayMirrors(4);
+            slidingDirection=true; //  For contour following (will change direction when touching wall)
+            speedContourFollowing=0.03;
+
+            // 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=-29;//360.0/bluePrint.scafold.size()/2; // in DEGREES
 
             break;
 
@@ -96,17 +102,14 @@
             //setColor(0x07);//0x04+0x02>>i);
             setColor(0x04);
 
-            saccadeRadius=250;
+            saccadeRadius=65;
             // default (initial) shape (the scafold belongs to the base class):
-            bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 40); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
+            bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 16); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
 
             // Numeric parameters for the simulated mechanical system:
-            massCenter=0.1;
-            dampMotionCenterMass=0.00015;//00003;
-            factorBouncingForce=0.0001; // this is because we will use a force on the central mass
-
-            slidingDirection=true; //  For contour following (will change direction when touching wall)
-            speedContourFollowing=0.0003;
+            massCenter=0.012;
+            dampMotionCenterMass=0.0001;//0.00015;//00003;
+            factorBouncingForce=0.0025; // 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();
@@ -114,9 +117,11 @@
             // Excursion limits (this 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).
             setRegionMotion(MIN_AD_MIRRORS+saccadeRadius, MIN_AD_MIRRORS+saccadeRadius, MAX_AD_MIRRORS-saccadeRadius, MAX_AD_MIRRORS-saccadeRadius);
 
-            // 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):
-            displaySensingBuffer.setDelayMirrors(4);
+            // 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
 
             break;
     }
@@ -165,10 +170,14 @@
     vector2D momentVector(0,0);
     for (int i = 0; i < numPoints; i++) {
         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
+            momentVector+=bluePrint.scafold[i];// note: no need to do -centerMass.pos, because the scafold is "centered" around 0
         }
     }
-    float momentNorm=momentVector.length();
+    
+//    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
     if (momentNorm==0) {
         recenteringVectorLoop.set(0,0);
         normRecenteringVector=0;
@@ -199,14 +208,17 @@
         case SPOT_FOLLOWING:
             // we need to compute the tangencial "speed":
             // vector2D slidingVector;
-            // let's normalize the moment vector (this simplifies the calculations for the sliding vector - need some calculations, but this is the result):
             if (momentNorm>0) {
-                momentVector/=momentNorm;
+                //momentVector/=momentNorm;
                 // We can now compute the sliding vector as:
-                slidingVector=momentVector.getRotatedDeg(slidingDirection? 90 : -90) * speedContourFollowing*saccadeRadius;
+                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)
                 // 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.9;
+              
+                centerMass.pos += slidingVector + recenteringVectorLoop * 0.2;
+                
+              //  centerMass.pos += recenteringVectorLoop*0.1; // ATTENTION!!! the REAL radius may be smaller if the mirrors are running fast!!!
 
                 // The following function can help constraining the position "pos", but it also does too much. Do something simpler perhaps?
                 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
@@ -220,7 +232,12 @@
         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();
-            centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
+           
+
+ if (displaySensingBuffer.lightTouched) {
+            // add force:
+             centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
+            }
 
             // update dynamics for the central mass::
 #ifndef VERLET_METHOD
@@ -229,6 +246,12 @@
 
             centerMass.update();  // unconstrained
             centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
+            
+             if (displaySensingBuffer.lightTouched) {
+            // do collision damping:
+             centerMass.setSpeed(centerMass.getSpeed()*0.99);
+             }
+
 
             break;