Jody McAdams avatar Jody McAdams committed b0a92d3

fixed animation library, using new ping for ultrasonic sensor

Comments (0)

Files changed (2)

Experimental/MicroHorse/MicroHorse.ino

 // by BARRAGAN <http://barraganstudio.com> 
 // This example code is in the public domain.
 
-
+#include <NewPing.h>
 #include <Servo.h> 
 #include <MathTypes.h>
 #include <MathUtil.h>
 #include <Animation.h>
 
-Servo servoPingSensor;
-int ultraSoundSignal = 3; // Ultrasound signal pin
-
-#define DEBUG_RESET 0
+#define DEBUG_RESET 1
  
 const float updateAnimSPF = 1.0f/60.0f;
   
 Animation* lowerLegServoAnimBL;
 Animation* pingAnim;
 
-const float overallMovementR = 1.0f;
-const float overallMovementL = 1.0f;
+float overallMovementR = 1.0f;
+float overallMovementL = 1.0f;
 
 const float animSpeed = 1.5f;
 
 const float rightSideOffset = 3.5f*0.45f;
 
 //Ping
+Servo servoPingSensor;
+int ultrasonicMaxDistanceCM = 400;
+int ultraSoundSignal = 3; // Ultrasound signal pin
+NewPing* sonar;
+
+float pingAnimVal;
 const Animation::AnimMode pingAnimMode = Animation::AnimMode_PingPong;
 const int pingAnimNumValues = 2;
-Animation::KeyFrame pingAnimValues[pingAnimNumValues] = {{0.0f,-90},{2.0f,90}};
+Animation::KeyFrame pingAnimValues[pingAnimNumValues] = {{0.0f,-70},{1.0f,70}};
 
 const Animation::AnimMode upperLegMode = Animation::AnimMode_Wrap;
 const Animation::AnimMode lowerLegMode = Animation::AnimMode_Wrap;
 Servo servoLowerBR;
 Servo servoLowerBL;
 
-enum PingState
-{
-  PingState_Idle,
-  PingState_Waiting1,
-  PingState_Waiting2,
-  PingState_Ready,
-};
-
-PingState pingState = PingState_Idle;
 unsigned long lastPingStateMicros;
 
 unsigned long echo = 0;
 
-void updatePing(float timeDelta)
-{
-  switch(pingState)
-  {
-    case PingState_Idle:
-    {
-      pinMode(ultraSoundSignal, OUTPUT); // Switch signalpin to output
-      digitalWrite(ultraSoundSignal, LOW); // Send low pulse
-      
-      lastPingStateMicros = micros();
-      
-      pingState = PingState_Waiting1;
-      
-      break;
-    }
-    case PingState_Waiting1:
-    {
-      unsigned long currMicros = micros();
-      
-      if(currMicros-lastPingStateMicros >= 2)
-      {
-        digitalWrite(ultraSoundSignal, HIGH); // Send high pulse
-        
-        lastPingStateMicros = micros();
-        pingState = PingState_Waiting2;
-      }
-      
-      break;
-    }
-    case PingState_Waiting2:
-    {
-      unsigned long currMicros = micros();
-      
-      if(currMicros-lastPingStateMicros >= 5)
-      {
-        digitalWrite(ultraSoundSignal, LOW); // Holdoff
-        pinMode(ultraSoundSignal, INPUT); // Switch signalpin to input
-        digitalWrite(ultraSoundSignal, HIGH); // Turn on pullup resistor
-        // please note that pulseIn has a 1sec timeout, which may
-        // not be desirable. Depending on your sensor specs, you
-        // can likely bound the time like this -- marcmerlin
-        // echo = pulseIn(ultraSoundSignal, HIGH, 38000)
-        echo = pulseIn(ultraSoundSignal, HIGH); //Listen for echo
-        //ultrasoundValue = (echo / 58.138f) * .39f; //convert to CM then to inches
-        
-        pingState = PingState_Ready;
-      }
-      
-      break;
-    }
-    case PingState_Ready:
-    {
-      //Serial.println(echo);
-      
-      pingState = PingState_Idle;
-      
-      break;
-    }
-  }
-}
- 
 float lastTimeMillis = 0.0f;
 void setup() 
 { 
   Serial.begin(9600);
   
+  sonar = new NewPing(ultraSoundSignal,ultraSoundSignal,ultrasonicMaxDistanceCM);
+  
   //Create animations
   pingAnim = new Animation(pingAnimValues,pingAnimNumValues,pingAnimMode);
   
 float pingUpdateTimer = 0.0f;
 float animUpdateTimer = 0.0f;
 
+float distInLeft = 0.0f;
+float distInCenter = 0.0f;
+float distInRight = 0.0f;
+
+enum PingState
+{
+  PingState_Left,
+  PingState_Center,
+  PingState_Right,
+};
+
+PingState pingState = PingState_Center;
+float maxPingIn = 0.0f;
+
+void ChangePingState(PingState newState);
+void ChangePingState(PingState newState)
+{
+  switch(newState)
+    {
+      case PingState_Left:
+      { 
+        Serial.println("Left");
+        
+        break;
+      }
+      case PingState_Center:
+      { 
+        Serial.println("Center");
+        
+        break;
+      }
+      case PingState_Right:
+      {
+        Serial.println("Right");
+        
+        break;
+      }
+    }
+    
+    pingState = newState;
+}
+
 void loop() 
 { 
   //Get current time in seconds
   pingUpdateTimer -= timeDelta;
   if(pingUpdateTimer < 0.0f)
   {
-    pingUpdateTimer += 1.0f;
+    pingUpdateTimer += 0.1f;
     
-    updatePing(timeDelta);
+    //pingState = PingState_Start;
+    float distInches = sonar->ping_in();
+    
+    switch(pingState)
+    {
+      case PingState_Left:
+      {
+        if(pingAnimVal > -30.0f)
+        {
+          ChangePingState(PingState_Center);
+        }
+        
+        break;
+      }
+      case PingState_Center:
+      {
+        if(pingAnimVal > 30.0f)
+        {
+          ChangePingState(PingState_Right);
+        }
+        else if(pingAnimVal < -30.0f)
+        {
+          ChangePingState(PingState_Left);
+        }
+        
+        break;
+      }
+      case PingState_Right:
+      {
+        if(pingAnimVal < 30.0f)
+        {
+           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);*/
   }
   
   animUpdateTimer -= timeDelta;
   lowerLegServoAnimBL->Update(timeDelta);  
   
   pingAnim->Update(timeDelta);
-  float pingAnimVal = pingAnim->CalcAnimVal();
-  pingAnimVal += 90.0f;
+  pingAnimVal = pingAnim->CalcAnimVal();
+  
+  const float finalPingAnimVal = pingAnimVal+90.0f;
   
   float topFrontRightAnimVal =  upperLegServoAnimFR->CalcAnimVal();
   float topFrontLeftAnimVal = -upperLegServoAnimFL->CalcAnimVal();
   topBackLeftAnimVal += 90.0f;
   bottomBackLeftAnimVal += 90.0f;  
   
+  servoPingSensor.write(finalPingAnimVal); 
+  
  #if !DEBUG_RESET
-  servoPingSensor.write(pingAnimVal); 
    //Set servo values from animations
   servoUpperFR.write(topFrontRightAnimVal);
   servoLowerFR.write(bottomFrontRightAnimVal);

Libraries/Animation/Animation.cpp

     animMode = mode;
 	endPauseTime = 0.0f;
 	pauseTimer = 0.0f;
-	  
-	if(mode == AnimMode_PingPong)
-	{
-		totalAnimTime *= 0.5f;
-	}
     
     SetValues(valueArray,numValues);
   }
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.