Commits

Jody McAdams committed 6fbaebb

naive obstacle avoidance

  • Participants
  • Parent commits b0a92d3

Comments (0)

Files changed (1)

Experimental/MicroHorse/MicroHorse.ino

 #include <MathUtil.h>
 #include <Animation.h>
 
-#define DEBUG_RESET 1
+#define DEBUG_RESET 0
  
 const float updateAnimSPF = 1.0f/60.0f;
   
 Animation* lowerLegServoAnimBL;
 Animation* pingAnim;
 
+float movementMin = 0.0f;
+float movementMax = 1.0f;
+
+const float movementDistCheckMin = 0.0f;
+const float movementDistCheckMax = 50.0f;
+const float movementDistCheckDiff = movementDistCheckMax-movementDistCheckMin;
+
 float overallMovementR = 1.0f;
 float overallMovementL = 1.0f;
 
-const float animSpeed = 1.5f;
+const 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},{1.0f,70}};
+Animation::KeyFrame pingAnimValues[pingAnimNumValues] = {{0.0f,-70},{0.5f,70}};
 
 const Animation::AnimMode upperLegMode = Animation::AnimMode_Wrap;
 const Animation::AnimMode lowerLegMode = Animation::AnimMode_Wrap;
 float distInCenter = 0.0f;
 float distInRight = 0.0f;
 
+float distInAccum = 0.0f;
+
 enum PingState
 {
   PingState_Left,
 
 PingState pingState = PingState_Center;
 float maxPingIn = 0.0f;
+int numPings = 0;
 
 void ChangePingState(PingState newState);
 void ChangePingState(PingState newState)
 {
-  switch(newState)
+    float distInAverage = distInAccum/(float)numPings;
+    //Uninit
+    switch(pingState)
     {
       case PingState_Left:
       { 
-        Serial.println("Left");
+        distInLeft = distInAverage;
+        //Serial.print("Left:");
+        //Serial.println(distInLeft);
         
         break;
       }
       case PingState_Center:
       { 
-        Serial.println("Center");
+        distInCenter = distInAverage;
+        //Serial.print("Center:");
+        //Serial.println(distInCenter);
         
         break;
       }
       case PingState_Right:
       {
-        Serial.println("Right");
+        distInRight = distInAverage;
+        //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;
+      }
+    }
+    
+    distInAccum = 0.0f;
+    numPings = 0;
     
     pingState = newState;
 }
   pingUpdateTimer -= timeDelta;
   if(pingUpdateTimer < 0.0f)
   {
-    pingUpdateTimer += 0.1f;
+    pingUpdateTimer += 0.01f;
+
+    float distInches = sonar->ping_in();
+    ++numPings;
     
-    //pingState = PingState_Start;
-    float distInches = sonar->ping_in();
+    distInAccum += distInches;
+    
+    static const float maxAngle = 20.0f;
     
     switch(pingState)
     {
       case PingState_Left:
       {
-        if(pingAnimVal > -30.0f)
-        {
+        if(pingAnimVal < maxAngle)
+        {   
           ChangePingState(PingState_Center);
         }
         
       }
       case PingState_Center:
       {
-        if(pingAnimVal > 30.0f)
+        if(pingAnimVal > maxAngle)
+        {
+          ChangePingState(PingState_Left);
+        }
+        else if(pingAnimVal < -maxAngle)
         {
           ChangePingState(PingState_Right);
         }
-        else if(pingAnimVal < -30.0f)
-        {
-          ChangePingState(PingState_Left);
-        }
         
         break;
       }
       case PingState_Right:
       {
-        if(pingAnimVal < 30.0f)
+        if(pingAnimVal > -maxAngle)
         {
            ChangePingState(PingState_Center);
         }
         break;
       }
     }
-    
-    /*if(pingAnimVal < -30.0f)
-    {
-      distInLeft = distInches;
-    }
-    else if(pingAnimVal > 30.0f)
-    {
-      distInRight = distInches;
-    }
-    else
-    {
-      distInCenter = distInches;
-    }
-    
-    Serial.print("Left: ");
-    Serial.print(distInLeft);
-    Serial.print(", ");
-    
-    Serial.print("Center: ");
-    Serial.print(distInCenter);
-    Serial.print(", ");
-    
-    Serial.print("Left: ");
-    Serial.println(distInRight);*/
   }
   
+  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);
+  
+  overallMovementR = Lerp(movementMin,movementMax,movementLeftT);
+  overallMovementL = Lerp(movementMin,movementMax,movementRightT);
+  
+  float maxMove = MaxF(overallMovementL,overallMovementR);
+  float addToMove = (1.0f-maxMove)*0.5f;
+
+  overallMovementL += addToMove;
+  overallMovementR += addToMove;
+
+  Serial.print("Left: ");
+  Serial.print(overallMovementL);
+  Serial.print(", Right: ");
+  Serial.println(overallMovementR);
+  
+  
+  
   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;