Commits

Jody McAdams committed a107755

new code that finds the most open area to walk to (sucks when there is no open area at all)

Comments (0)

Files changed (1)

Experimental/MicroHorse/MicroHorse.ino

 #include <Animation.h>
 
 #define DEBUG_RESET 0
-#define DEBUG_PRINT 0
- 
-const float updateAnimSPF = 1.0f/30.0f;
+#define DEBUG_PRINT 1
+#define ALLOW_PING 1
+
+const float obstacleDistanceInches = 36.0f;
+
+float steeringValue = 0.0f;
+const float calibrationAngle = -10.0f;
+const float pingAngleMax = 65.0f;
+const float maxTurnAngle = pingAngleMax;
+float pingCurrAngle = -pingAngleMax;
+int pingCurrIndex = 0;
+const float pingEstimatedServoRotationSpeed = 200.0f; //deg per second
+float pingAngleMoveTime = pingAngleMax/pingEstimatedServoRotationSpeed;
+
+const int pingNumDistArrayEntries = 32;
+float pingDistArray[pingNumDistArrayEntries];
+float pingDistArrayAngles[pingNumDistArrayEntries];  //Should be fine since memory seems to not be an issue yet
+
+const float updateAnimSPF = 1.0f/60.0f;
   
 Animation* upperLegServoAnimFR;
 Animation* upperLegServoAnimFL;
 const float movementMin = 0.0f;
 const float movementMax = 1.0f;
 
-const float movementDistCheckMax = 50.0f;
+//const float movementDistCheckMax = 40.0f;
 
 float overallMovementR = 1.0f;
 float overallMovementL = 1.0f;
 
-float animSpeed = 2.5f;
+float animSpeedMin = 2.0f;
+float animSpeedMax = 4.0f;
+float animSpeed = 3.0f;
 
 const float backLegOffset = 0.8f;
 
 Servo servoLowerBR;
 Servo servoLowerBL;
 
-unsigned long lastPingStateMicros;
-
-unsigned long echo = 0;
-
 float lastTimeMillis = 0.0f;
 void setup() 
 { 
   Serial.begin(9600);
 #endif
 
+  for(int i=0; i<pingNumDistArrayEntries; ++i)
+  {
+    pingDistArray[i] = 0.0f;
+    
+    const float t = (float)i/(float)(pingNumDistArrayEntries-1);
+    pingDistArrayAngles[i] = Lerp(-pingAngleMax,pingAngleMax,t);
+   
+#if DEBUG_PRINT
+    Serial.print(i);
+    Serial.print(": ");
+    Serial.println(pingDistArrayAngles[i]);
+#endif
+  }
+
   sonar = new NewPing(ultraSoundSignal,ultraSoundSignal,ultrasonicMaxDistanceCM);
   
   //Create animations
   
   servoPingSensor.attach(12);
   
-  pinMode(ultraSoundSignal,OUTPUT);
+  //pinMode(ultraSoundSignal,OUTPUT);
 
   //Init time
   lastTimeMillis = millis();
 } 
 
-float pingUpdateTimer = 0.0f;
 float animUpdateTimer = 0.0f;
 
-float distInLeft = 0.0f;
-float distInCenter = 0.0f;
-float distInRight = 0.0f;
-
-float distInAccum = 0.0f;
-
-enum PingState
-{
-  PingState_Left,
-  PingState_Center,
-  PingState_Right,
-};
-
-PingState pingState = PingState_Center;
-float maxPingIn = 0.0f;
-int numPings = 0;
-
-void ChangePingState(PingState newState);
-void ChangePingState(PingState newState)
-{
-    float distInAverage = distInAccum/(float)numPings;
-    //Uninit
-    switch(pingState)
-    {
-      case PingState_Left:
-      { 
-        distInLeft = distInAverage;
-        
-        break;
-      }
-      case PingState_Center:
-      { 
-        distInCenter = distInAverage;
-        
-        break;
-      }
-      case PingState_Right:
-      {
-        distInRight = distInAverage;
-
-        break;
-      }
-    }
-
-    distInAccum = 0.0f;
-    numPings = 0;
-    
-    pingState = newState;
-}
-
 void UpdatePing(float timeDelta)
 {
-  pingUpdateTimer -= timeDelta;
-  if(pingUpdateTimer < 0.0f)
+  bool reachedEnd = false;
+  
+  //If we should be at the next angle, change to the next index and do a ping
+  pingAngleMoveTime -= timeDelta;
+  if(pingAngleMoveTime < 0.0f)
   {
-    pingUpdateTimer += 0.01f;
+    //Do a ping at the current angle
+    const float distance = sonar->ping_in();
+    
+    pingDistArray[pingCurrIndex] = distance;
+    
+/*#if DEBUG_PRINT
+    Serial.print("Angle: ");
+    Serial.print(pingCurrAngle);
+    Serial.print("Distance (inches): ");
+    Serial.println(distance);
+#endif*/
+    
+    const float lastAngle = pingCurrAngle;
+    
+    //Switch to next angle
+    if(pingCurrIndex == pingNumDistArrayEntries-1)
+    {
+      reachedEnd = true;
+      
+      //Get next angle
+      pingCurrAngle = -pingAngleMax;
+    }
+    else
+    {
+      //Increment index
+      ++pingCurrIndex;
+      
+      //Get next angle
+      pingCurrAngle = pingDistArrayAngles[pingCurrIndex];
+    }
 
-    float distInches = sonar->ping_in();
-    ++numPings;
+    //Set timer based on the amount of turning to do
+    pingAngleMoveTime = abs(pingCurrAngle-lastAngle)/pingEstimatedServoRotationSpeed;
     
-    distInAccum += distInches;
+    //Write to servo
+    servoPingSensor.write(-pingCurrAngle+calibrationAngle+90.0f);
     
-    static const float maxAngle = 35.0f;
+    //Finished a scan so update movement!
+    if(reachedEnd)
+    {
+      pingCurrIndex = 0;
+      
+      int bestNonObstacleCount = 0;
+      int countIndexStart = 0;
+      int currNonObstacleCount = 0;
+      float desiredAngle = 0.0f;
+      
+      for(int i=0; i<pingNumDistArrayEntries; ++i)
+      {
+        //If we find an obstacle or we reach the end of the array...
+        if(pingDistArray[i] < obstacleDistanceInches
+          || i == (pingNumDistArrayEntries-1))
+        { 
+          //If this is the widest open area we've found
+          //so far...
+          if(currNonObstacleCount > bestNonObstacleCount)
+          {
+            //Save the best!
+             bestNonObstacleCount = currNonObstacleCount;
+             
+             //Save the angle in the center of this hopefully huge open area
+             const int desiredIndex = (countIndexStart+i)/2;
+             desiredAngle = pingDistArrayAngles[desiredIndex];
+          }
+          
+          //Set up for net run
+          countIndexStart = i+1;        
+          currNonObstacleCount = 0;
+        }
+        else
+        {
+          //No obstacles.  Yay!
+          ++currNonObstacleCount;
+        }
+      }
+      
+#if DEBUG_PRINT
+      Serial.print("Turn to this angle: ");
+      Serial.print(desiredAngle);
+      Serial.print(", Steering value: ");
+      steeringValue = ClampF(desiredAngle/maxTurnAngle,-1.0f,1.0f);
+      Serial.println(steeringValue);
+#endif
+  
+      overallMovementL = steeringValue*0.5f+0.5f;
+      overallMovementR = 1.0f-overallMovementL;
+      
+      const float moveDiff = 1.0f-abs(overallMovementL-overallMovementR);
+      const float maxMove = MaxF(overallMovementL,overallMovementR);
+      
+      //animSpeed = Lerp(animSpeedMin,animSpeedMax,moveDiff);
+      
+      overallMovementL /= maxMove;
+      overallMovementR /= maxMove;
     
-    switch(pingState)
-    {
-      case PingState_Left:
-      {
-        if(pingAnimVal < maxAngle)
-        {   
-          ChangePingState(PingState_Center);
-        }
-        
-        break;
-      }
-      case PingState_Center:
-      {
-        if(pingAnimVal > maxAngle)
-        {
-          ChangePingState(PingState_Left);
-        }
-        else if(pingAnimVal < -maxAngle)
-        {
-          ChangePingState(PingState_Right);
-        }
-        
-        break;
-      }
-      case PingState_Right:
-      {
-        if(pingAnimVal > -maxAngle)
-        {
-           ChangePingState(PingState_Center);
-        }
-        
-        break;
-      }
+      overallMovementL = Lerp(movementMin,movementMax,overallMovementL);
+      overallMovementR = Lerp(movementMin,movementMax,overallMovementR);
+  
+/*#if DEBUG_PRINT
+      Serial.print("Left: ");
+      Serial.print(overallMovementL);
+      Serial.print(", Right: ");
+      Serial.print(overallMovementR);
+      Serial.print(", Anim Speed: ");
+      Serial.println(animSpeed);
+#endif*/
     }
   }
 }
   const float timeDelta = (currTimeMillis-lastTimeMillis)/1000.0f;
   lastTimeMillis = currTimeMillis;
   
+#if ALLOW_PING
   UpdatePing(timeDelta);
-  
-  overallMovementL = distInRight/movementDistCheckMax;
-  overallMovementR = distInLeft/movementDistCheckMax;
-  
-  const float moveDiff = 1.0f-abs(overallMovementL-overallMovementR);
-  const float maxMove = MaxF(overallMovementL,overallMovementR);
-  
-  overallMovementL /= maxMove;
-  overallMovementR /= maxMove;
-
-  overallMovementL = Lerp(movementMin,movementMax,overallMovementL);
-  overallMovementR = Lerp(movementMin,movementMax,overallMovementR);
-
-#if DEBUG_PRINT
-  Serial.print("Left: ");
-  Serial.print(overallMovementL);
-  Serial.print(", Right: ");
-  Serial.print(overallMovementR);
-  Serial.print(", Anim Speed: ");
-  Serial.println(animSpeed);
- #endif
+#endif
   
   animUpdateTimer -= timeDelta;
   if(animUpdateTimer < 0.0f)
   }
 } 
 
-void UpdateAnims(float timeDelta)
+inline void UpdateAnims(float timeDelta)
 {
+  upperLegServoAnimFR->animSpeed = animSpeed;
+  lowerLegServoAnimFR->animSpeed = animSpeed;
+  upperLegServoAnimBR->animSpeed = animSpeed;
+  lowerLegServoAnimBR->animSpeed = animSpeed;
+  upperLegServoAnimFL->animSpeed = animSpeed;
+  lowerLegServoAnimFL->animSpeed = animSpeed;
+  upperLegServoAnimBL->animSpeed = animSpeed;
+  lowerLegServoAnimBL->animSpeed = animSpeed; 
+  
   //Update animations
   //Right legs
   upperLegServoAnimFR->Update(timeDelta); 
   upperLegServoAnimBL->Update(timeDelta);
   lowerLegServoAnimBL->Update(timeDelta);  
   
-  pingAnim->Update(timeDelta);
-  pingAnimVal = pingAnim->CalcAnimVal();
-  
-  const float finalPingAnimVal = pingAnimVal+90.0f;
-  
   float topFrontRightAnimVal =  upperLegServoAnimFR->CalcAnimVal();
   float topFrontLeftAnimVal = -upperLegServoAnimFL->CalcAnimVal();
   float topBackRightAnimVal =  upperLegServoAnimBR->CalcAnimVal();
   topBackLeftAnimVal += 90.0f;
   bottomBackLeftAnimVal += 90.0f;  
   
-  servoPingSensor.write(finalPingAnimVal); 
-  
  #if !DEBUG_RESET
    //Set servo values from animations
   servoUpperFR.write(topFrontRightAnimVal);