Commits

Jody McAdams committed 0096d73

best navigation code so far.. and with visualization!

  • Participants
  • Parent commits a107755

Comments (0)

Files changed (1)

Experimental/MicroHorse/MicroHorse.ino

 #include <Animation.h>
 
 #define DEBUG_RESET 0
-#define DEBUG_PRINT 1
+#define DEBUG_PRINT 0
 #define ALLOW_PING 1
 
-const float obstacleDistanceInches = 36.0f;
+const float obstacleDistanceInches = 28.0f;
 
 float steeringValue = 0.0f;
-const float calibrationAngle = -10.0f;
-const float pingAngleMax = 65.0f;
-const float maxTurnAngle = pingAngleMax;
+const float calibrationAngle = -20.0f;
+const float pingAngleMax = 90.0f;
 float pingCurrAngle = -pingAngleMax;
 int pingCurrIndex = 0;
-const float pingEstimatedServoRotationSpeed = 200.0f; //deg per second
+const float pingEstimatedServoRotationSpeed = 250.0f; //deg per second
 float pingAngleMoveTime = pingAngleMax/pingEstimatedServoRotationSpeed;
 
 const int pingNumDistArrayEntries = 32;
+const int robotFatness = 3;
 float pingDistArray[pingNumDistArrayEntries];
 float pingDistArrayAngles[pingNumDistArrayEntries];  //Should be fine since memory seems to not be an issue yet
 
 
 float animSpeedMin = 2.0f;
 float animSpeedMax = 4.0f;
-float animSpeed = 3.0f;
+float animSpeed = 2.0f;
 
 const float backLegOffset = 0.8f;
 
     pingDistArray[i] = 0.0f;
     
     const float t = (float)i/(float)(pingNumDistArrayEntries-1);
-    pingDistArrayAngles[i] = Lerp(-pingAngleMax,pingAngleMax,t);
+    pingDistArrayAngles[i] = Lerp(pingAngleMax,-pingAngleMax,t);
    
 #if DEBUG_PRINT
     Serial.print(i);
   
   //If we should be at the next angle, change to the next index and do a ping
   pingAngleMoveTime -= timeDelta;
-  if(pingAngleMoveTime < 0.0f)
+  if(pingAngleMoveTime <= 0.0f)
   {
     //Do a ping at the current angle
     const float distance = sonar->ping_in();
     {
       //Increment index
       ++pingCurrIndex;
-      
-      //Get next angle
-      pingCurrAngle = pingDistArrayAngles[pingCurrIndex];
     }
+    
+    //Get next angle
+    pingCurrAngle = pingDistArrayAngles[pingCurrIndex];
 
     //Set timer based on the amount of turning to do
     pingAngleMoveTime = abs(pingCurrAngle-lastAngle)/pingEstimatedServoRotationSpeed;
     
     //Write to servo
-    servoPingSensor.write(-pingCurrAngle+calibrationAngle+90.0f);
+    servoPingSensor.write(pingCurrAngle+calibrationAngle+90.0f);
     
     //Finished a scan so update movement!
     if(reachedEnd)
     {
       pingCurrIndex = 0;
       
-      int bestNonObstacleCount = 0;
+      int bestNonObstacleCount = -1;
       int countIndexStart = 0;
       int currNonObstacleCount = 0;
-      float desiredAngle = 0.0f;
+      int selectedIndex = 0;
+      float bestAccumulatedDist = 0.0f;
+      float currAccumulatedDist = 0.0f;
       
       for(int i=0; i<pingNumDistArrayEntries; ++i)
       {
+        const float currDist = pingDistArray[i];
+        
         //If we find an obstacle or we reach the end of the array...
-        if(pingDistArray[i] < obstacleDistanceInches
+        if(currDist < obstacleDistanceInches
           || i == (pingNumDistArrayEntries-1))
         { 
+#if DEBUG_PRINT
+    Serial.print('X');
+#endif
           //If this is the widest open area we've found
           //so far...
-          if(currNonObstacleCount > bestNonObstacleCount)
+          if(currNonObstacleCount >= robotFatness
+            && currNonObstacleCount >= bestNonObstacleCount
+            && currAccumulatedDist > bestAccumulatedDist)
           {
             //Save the best!
              bestNonObstacleCount = currNonObstacleCount;
+             bestAccumulatedDist = currAccumulatedDist;
              
              //Save the angle in the center of this hopefully huge open area
-             const int desiredIndex = (countIndexStart+i)/2;
-             desiredAngle = pingDistArrayAngles[desiredIndex];
+             selectedIndex = (countIndexStart + i)/2;
           }
           
           //Set up for net run
           countIndexStart = i+1;        
           currNonObstacleCount = 0;
+          currAccumulatedDist = 0.0f;
         }
+        //No obstacles.  Yay!
         else
         {
-          //No obstacles.  Yay!
+#if DEBUG_PRINT
+    Serial.print('.');
+#endif
+          currAccumulatedDist += currDist;
           ++currNonObstacleCount;
         }
       }
-      
+
+    if(bestNonObstacleCount == -1)
+    {
+      selectedIndex = (pingNumDistArrayEntries-1)/2;
+    }
+    
 #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);
+    Serial.println(' ');
+    for(int i=0; i<pingNumDistArrayEntries; ++i)
+    {
+      if(i == selectedIndex)
+      {
+        Serial.print('^');
+      }
+      else
+      {
+        Serial.print(' ');
+      }
+    }  
+    Serial.println(' ');
 #endif
+      steeringValue = (float)selectedIndex/(float)(pingNumDistArrayEntries-1);
   
-      overallMovementL = steeringValue*0.5f+0.5f;
+      overallMovementL = steeringValue;
       overallMovementR = 1.0f-overallMovementL;
       
       const float moveDiff = 1.0f-abs(overallMovementL-overallMovementR);