Commits

Jody McAdams committed 19e91bd

improved navigation

  • Participants
  • Parent commits 3464d60

Comments (0)

Files changed (1)

File Experimental/MicroHorse/MicroHorse.ino

 #include <Animation.h>
 
 #define DEBUG_RESET 0
+#define DEBUG_PRINT 0
  
 const float updateAnimSPF = 1.0f/30.0f;
   
 Animation* lowerLegServoAnimBL;
 Animation* pingAnim;
 
-const float movementMin = 0.1f;
+const float movementMin = 0.0f;
 const float movementMax = 1.0f;
 
-const float animSpeedMin = 1.25f;
-const float animSpeedMax = 2.5f;
-
-const float movementDistCheckMax = 70.0f;
+const float movementDistCheckMax = 50.0f;
 
 float overallMovementR = 1.0f;
 float overallMovementL = 1.0f;
 
-float animSpeed = 3.0f;
+float animSpeed = 2.5f;
 
 const float backLegOffset = 0.8f;
 
 float lastTimeMillis = 0.0f;
 void setup() 
 { 
-  //Serial.begin(9600);
-  
+#if DEBUG_PRINT
+  Serial.begin(9600);
+#endif
+
   sonar = new NewPing(ultraSoundSignal,ultraSoundSignal,ultrasonicMaxDistanceCM);
   
   //Create animations
 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);
         
         break;
       }
       case PingState_Center:
       { 
         distInCenter = distInAverage;
-        //distInCenter = minDist;
-        //Serial.print("Center:");
-        //Serial.println(distInCenter);
         
         break;
       }
       case PingState_Right:
       {
         distInRight = distInAverage;
-        //distInRight = minDist;
-        //Serial.print("Right:");
-        //Serial.println(distInRight);
-        
+
         break;
       }
     }
 
-    //minDist = movementDistCheckMax;
     distInAccum = 0.0f;
     numPings = 0;
     
     pingState = newState;
 }
 
-void loop() 
-{ 
-  //Get current time in seconds
-  float currTimeMillis = millis();
-  float timeDelta = (currTimeMillis-lastTimeMillis)/1000.0f;
-  lastTimeMillis = currTimeMillis;
-  
+void UpdatePing(float timeDelta)
+{
   pingUpdateTimer -= timeDelta;
   if(pingUpdateTimer < 0.0f)
   {
     
     distInAccum += distInches;
     
-    /*if(distInches < minDist)
-    {
-      minDist = distInches;
-    }*/
-    
     static const float maxAngle = 35.0f;
     
     switch(pingState)
       }
     }
   }
+}
+
+
+void loop() 
+{ 
+  //Get current time in seconds
+  const float currTimeMillis = millis();
+  const float timeDelta = (currTimeMillis-lastTimeMillis)/1000.0f;
+  lastTimeMillis = currTimeMillis;
   
-  float centerDistT = distInCenter/movementDistCheckMax;
-  //float closeness = 1.0f-centerDistT;
+  UpdatePing(timeDelta);
   
-  /*const float distDiff = distInRight-distInLeft;
-  overallMovementL = (distDiff/movementDistCheckMax)*0.5f+0.5f;
-  overallMovementR = 1.0f-overallMovementL;*/
-  
-  overallMovementL = MinF(1.0f,distInRight/movementDistCheckMax);
-  overallMovementR = MinF(1.0f,distInLeft/movementDistCheckMax);
+  overallMovementL = distInRight/movementDistCheckMax;
+  overallMovementR = distInLeft/movementDistCheckMax;
   
   const float moveDiff = 1.0f-abs(overallMovementL-overallMovementR);
-  //Serial.print("moveDiffT: ");
-  //Serial.print(moveDiffT);
+  const float maxMove = MaxF(overallMovementL,overallMovementR);
   
-  const float moveDiffAdd = moveDiff*0.5f;
-  //Serial.print(moveDiffAdd);
-  
-  overallMovementL += moveDiffAdd;
-  overallMovementR += moveDiffAdd;
-  
-  //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 addToMove = (1.0f-maxMove);
-  
-  //overallMovementL += addToMove;
-  //overallMovementR += addToMove;
+  overallMovementL /= maxMove;
+  overallMovementR /= maxMove;
 
   overallMovementL = Lerp(movementMin,movementMax,overallMovementL);
   overallMovementR = Lerp(movementMin,movementMax,overallMovementR);
-  
-  //animSpeed = Lerp(animSpeedMin,animSpeedMax,centerDistT);
-  
-  /*Serial.print("Left: ");
+
+#if DEBUG_PRINT
+  Serial.print("Left: ");
   Serial.print(overallMovementL);
   Serial.print(", Right: ");
   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;
+  Serial.println(animSpeed);
+ #endif
   
   animUpdateTimer -= timeDelta;
   if(animUpdateTimer < 0.0f)
   topBackLeftAnimVal *= overallMovementL;
   bottomBackLeftAnimVal *= overallMovementL;
   
+  //TODO: bake these into the animations at startup
+  
   //Right legs
   topFrontRightAnimVal += 90.0f;
   bottomFrontRightAnimVal += 90.0f;