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)

  • Participants
  • Parent commits 19e91bd

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;
+#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);