just a test

Dependencies:   mbed

Fork of scoreLight_Advanced by Alvaro Cassinelli

Revision:
27:1ce994629ffc
Parent:
26:c9329c4fc20a
Child:
28:44b7b6e35548
--- a/rigidLoop.cpp	Mon Jun 18 12:39:28 2012 +0000
+++ b/rigidLoop.cpp	Mon Jun 18 15:09:25 2012 +0000
@@ -106,6 +106,8 @@
 
             //setColor(0x07);//0x04+0x02>>i);
             setColor(0x04);
+            
+            gravity.set(0,0);
 
             saccadeRadius=65;//+rand()%20;
             // default (initial) shape (the scafold belongs to the base class):
@@ -126,6 +128,33 @@
             angleCorrectionForceLoop=-5;// in degrees
 
             break;
+            
+             case  SPOT_FOUNTAIN:
+            // Name of this kind of spot:
+            sprintf(spotName,"rigid_fountain");
+
+            //setColor(0x07);//0x04+0x02>>i);
+            setColor(0x04);
+
+            saccadeRadius=40;//+rand()%20;
+            // default (initial) shape (the scafold belongs to the base class):
+            bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
+
+            // Numeric parameters for the simulated mechanical system:
+            massCenter=0.0005;//+0.000005*(rand()%100);
+            dampMotionCenterMass=0.00045;//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();
+
+            // 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(3);
+            angleCorrectionForceLoop=-5;// in degrees
+
+            break;
     }
     
     saccadeRadius_initial=saccadeRadius; // this is for search mode for instance. 
@@ -242,6 +271,9 @@
         // 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.
         // 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. 
         
+        // (1) current color: change with touch? NO
+        transientBlobColor=blobColor; // just the original blob color
+        
         break;
 
         case SPOT_FOLLOWING:
@@ -278,6 +310,12 @@
                 justSearched=true;
             }
 
+              // Change color with touch? NO
+           // if (displaySensingBuffer.lightTouched)  
+           //      transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
+           // else 
+            transientBlobColor=blobColor; // just the original blob color
+
             break;
 
         case  SPOT_BOUNCING:
@@ -298,7 +336,7 @@
             }
             
             // Gravity? - side or central attraction?
-           //  centerMass.addForce(gravity*centerMass.mass); 
+            centerMass.addForce(gravity*centerMass.mass); 
             
             // or central spring attraction;
             //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X); 
@@ -306,6 +344,50 @@
             //centerMass.addForce(-dist*centerMass.mass*0.0007);
             
             // or "radial gravity":
+            //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X); 
+            //vector2Df dist=centerMass.pos-centerAttraction;
+            //centerMass.addForce(dist.normalize()*centerMass.mass*0.5);
+          
+
+            // update dynamics for the central mass:
+#ifndef VERLET_METHOD
+            centerMass.addDampingForce();  // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
+#endif
+
+            centerMass.update();  // unconstrained
+            centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
+            
+             if (displaySensingBuffer.lightTouched) {
+            // do collision damping:
+               centerMass.setSpeed(centerMass.getSpeed()*0.99);
+             }
+             
+            // Change color with touch? YES
+            if (displaySensingBuffer.lightTouched)  
+                 transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
+            else 
+                 transientBlobColor=blobColor; // just the original blob color
+             
+            break;
+            
+            case  SPOT_FOUNTAIN:
+            // 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; 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);
+             
+            }
+            
+            // RADIAL GRAVITY for the "fountain mode":
             vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X); 
             vector2Df dist=centerMass.pos-centerAttraction;
             centerMass.addForce(dist.normalize()*centerMass.mass*0.5);
@@ -324,6 +406,15 @@
                centerMass.setSpeed(centerMass.getSpeed()*0.99);
              }
              
+            // Change color with touch? YES
+            if (displaySensingBuffer.lightTouched)  
+                 transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
+            else 
+                 transientBlobColor=blobColor; // just the original blob color
+            
+            // In case of "fountain mode", reset position to initial positions and speeds:
+             if (blobWallCollision) centerMass.setInitialCondition(startCenter, startSpeed);  
+             
             break;
     }
     
@@ -356,10 +447,8 @@
     }
     
     // Global color for the whole loop:
-    if (displaySensingBuffer.lightTouched)  
-        displaySensingBuffer.displayColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
-     else 
-         displaySensingBuffer.displayColor=blobColor;
+    displaySensingBuffer.displayColor=transientBlobColor;
+ 
 }
 
 void rigidLoop::computeBoundingBox() {