Commits

Jody McAdams committed 5834590

updated navigation code… still needs work

Comments (0)

Files changed (1)

Experimental/MicroHorse/MicroHorse.ino

 
 #define DEBUG_RESET 0
  
-const float updateAnimSPF = 1.0f/60.0f;
+const float updateAnimSPF = 1.0f/30.0f;
   
 Animation* upperLegServoAnimFR;
 Animation* upperLegServoAnimFL;
 Animation* lowerLegServoAnimBL;
 Animation* pingAnim;
 
-float movementMin = 0.0f;
-float movementMax = 1.0f;
+const float movementMin = 0.0f;
+const float movementMax = 1.0f;
 
-const float movementDistCheckMin = 0.0f;
-const float movementDistCheckMax = 50.0f;
-const float movementDistCheckDiff = movementDistCheckMax-movementDistCheckMin;
+const float closenessEffectMin = 0.5f;
+const float closenessEffectMax = 1.0f;
+
+const float animSpeedMin = 1.25f;
+const float animSpeedMax = 2.5f;
+
+const float movementDistCheckMax = 30.0f;
 
 float overallMovementR = 1.0f;
 float overallMovementL = 1.0f;
 
-const float animSpeed = 3.0f;
+float animSpeed = 3.0f;
 
 const float backLegOffset = 0.8f;
 
 float pingAnimVal;
 const Animation::AnimMode pingAnimMode = Animation::AnimMode_PingPong;
 const int pingAnimNumValues = 2;
-Animation::KeyFrame pingAnimValues[pingAnimNumValues] = {{0.0f,-70},{0.5f,70}};
+Animation::KeyFrame pingAnimValues[pingAnimNumValues] = {{0.0f,-70},{0.75f,70}};
 
 const Animation::AnimMode upperLegMode = Animation::AnimMode_Wrap;
 const Animation::AnimMode lowerLegMode = Animation::AnimMode_Wrap;
   lowerLegServoAnimBR->currT += rightSideOffset;
   
   //Left legs
-  upperLegServoAnimFL->animSpeed = animSpeed;
-  lowerLegServoAnimFL->animSpeed = animSpeed;
-  upperLegServoAnimBL->animSpeed = animSpeed;
-  lowerLegServoAnimBL->animSpeed = animSpeed;
-
-  //Right legs
-  upperLegServoAnimFR->animSpeed = animSpeed;
-  lowerLegServoAnimFR->animSpeed = animSpeed;
-  upperLegServoAnimBR->animSpeed = animSpeed;
-  lowerLegServoAnimBR->animSpeed = animSpeed;
-  
-  //Left legs
   upperLegServoAnimBL->currT += backLegOffset;
   lowerLegServoAnimBL->currT += backLegOffset;
 
 float distInRight = 0.0f;
 
 float distInAccum = 0.0f;
+//float minDist = 0.0f;
 
 enum PingState
 {
       case PingState_Left:
       { 
         distInLeft = distInAverage;
+        //distInLeft = minDist;
         //Serial.print("Left:");
         //Serial.println(distInLeft);
         
       case PingState_Center:
       { 
         distInCenter = distInAverage;
+        //distInCenter = minDist;
         //Serial.print("Center:");
         //Serial.println(distInCenter);
         
       case PingState_Right:
       {
         distInRight = distInAverage;
+        //distInRight = minDist;
         //Serial.print("Right:");
         //Serial.println(distInRight);
         
         break;
       }
     }
-  
-    //Init
-    switch(newState)
-    {
-      case PingState_Left:
-      { 
-        //Serial.println("Left");
-        
-        break;
-      }
-      case PingState_Center:
-      { 
-        //Serial.println("Center");
-        
-        break;
-      }
-      case PingState_Right:
-      {
-        //Serial.println("Right");
-        
-        break;
-      }
-    }
-    
+
+    //minDist = movementDistCheckMax;
     distInAccum = 0.0f;
     numPings = 0;
     
     
     distInAccum += distInches;
     
-    static const float maxAngle = 20.0f;
+    /*if(distInches < minDist)
+    {
+      minDist = distInches;
+    }*/
+    
+    static const float maxAngle = 35.0f;
     
     switch(pingState)
     {
     }
   }
   
-  float pingDistLeft = distInLeft;//(distInLeft+distInCenter)*0.5f;
-  float pingDistRight = distInRight;//(distInRight+distInCenter)*0.5f;
-
-  float movementLeftT = ClampF((pingDistLeft-movementDistCheckMin)/(movementDistCheckDiff),0.0f,1.0f);
-  float movementRightT = ClampF((pingDistRight-movementDistCheckMin)/(movementDistCheckDiff),0.0f,1.0f);
+  float centerDistT = distInCenter/movementDistCheckMax;
+  //float closeness = 1.0f-centerDistT;
   
-  overallMovementR = Lerp(movementMin,movementMax,movementLeftT);
-  overallMovementL = Lerp(movementMin,movementMax,movementRightT);
+  /*const float distDiff = distInRight-distInLeft;
+  overallMovementL = (distDiff/movementDistCheckMax)*0.5f+0.5f;
+  overallMovementR = 1.0f-overallMovementL;*/
   
-  float maxMove = MaxF(overallMovementL,overallMovementR);
-  float addToMove = (1.0f-maxMove)*0.5f;
-
+  overallMovementL = MinF(1.0f,distInRight/movementDistCheckMax);
+  overallMovementR = MinF(1.0f,distInLeft/movementDistCheckMax);
+  
+  //overallMovementL += centerDistT*0.5f;
+  //overallMovementR += centerDistT*0.5f;
+  
+  overallMovementL = MinF(1.0f,overallMovementL);
+  overallMovementR = MinF(1.0f,overallMovementR);
+  
+  const float maxMove = MaxF(overallMovementL,overallMovementR);
+  const float addToMoveT = Lerp(closenessEffectMin,closenessEffectMax,centerDistT);
+  const float addToMove = (1.0f-maxMove)*addToMoveT;
+  
   overallMovementL += addToMove;
   overallMovementR += addToMove;
 
+  overallMovementL = Lerp(movementMin,movementMax,overallMovementL);
+  overallMovementR = Lerp(movementMin,movementMax,overallMovementR);
+  
+  animSpeed = Lerp(animSpeedMin,animSpeedMax,centerDistT);
+  
   Serial.print("Left: ");
   Serial.print(overallMovementL);
   Serial.print(", Right: ");
-  Serial.println(overallMovementR);
+  Serial.print(overallMovementR);
+  Serial.print(", Anim Speed: ");
+  Serial.println(animSpeed);
   
-  
+  //Left legs
+  upperLegServoAnimFL->animSpeed = animSpeed;
+  lowerLegServoAnimFL->animSpeed = animSpeed;
+  upperLegServoAnimBL->animSpeed = animSpeed;
+  lowerLegServoAnimBL->animSpeed = animSpeed;
+
+  //Right legs
+  upperLegServoAnimFR->animSpeed = animSpeed;
+  lowerLegServoAnimFR->animSpeed = animSpeed;
+  upperLegServoAnimBR->animSpeed = animSpeed;
+  lowerLegServoAnimBR->animSpeed = animSpeed;
   
   animUpdateTimer -= timeDelta;
   if(animUpdateTimer < 0.0f)
   
   //Left legs
   topFrontLeftAnimVal *= overallMovementL;
-  //bottomFrontLeftAnimVal *= overallMovementL;
+  bottomFrontLeftAnimVal *= overallMovementL;
   topBackLeftAnimVal *= overallMovementL;
-  //bottomBackLeftAnimVal *= overallMovementL;
+  bottomBackLeftAnimVal *= overallMovementL;
   
   //Right legs
   topFrontRightAnimVal += 90.0f;
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.