Source

ArduinoProjects / Experimental / MicroHorse / MicroHorse.ino

Diff from to

File Experimental/MicroHorse/MicroHorse.ino

 Animation* pingAnim;
 
 const float overallMovementR = 1.0f;
-const float overallMovementL = 0.25f;
+const float overallMovementL = 1.0f;
 
-const float animSpeed = 3.0f;
+const float animSpeed = 4.0f;
 
 const float backLegOffset = 0.8f;
 
-const float rightSideOffset = 3.5f*0.5f;
+const float rightSideOffset = 3.5f*0.45f;
 
 //Ping
 const Animation::AnimMode pingAnimMode = Animation::AnimMode_PingPong;
 
 //Front
 const int numUpperLegAnimValuesF = 4;
-Animation::KeyFrame upperLegAnimValuesF[numUpperLegAnimValuesF] = {{0.0f,30},{1.0f,-55},{2.1f,0},{3.5f,30}};
+Animation::KeyFrame upperLegAnimValuesF[numUpperLegAnimValuesF] = {{0.0f,20},{1.0f,-80},{2.1f,-20},{3.5f,20}};
+
 //Back
 const int numUpperLegAnimValuesB = 4;
-Animation::KeyFrame upperLegAnimValuesB[numUpperLegAnimValuesB] = {{0.0f,30},{1.0f,-55},{2.1f,0},{3.5f,30}};
+Animation::KeyFrame upperLegAnimValuesB[numUpperLegAnimValuesB] = {{0.0f,30},{1.0f,-50},{2.1f,10},{3.5f,30}};
+
 
 //Bottom
 
 //Front
 const int numLowerLegAnimValuesF = 5;
-Animation::KeyFrame lowerLegAnimValuesF[numLowerLegAnimValuesF] = {{0.0f,0},{1.0f,90},{1.25f,-30},{2.0f,0},{3.5f,0}};
+Animation::KeyFrame lowerLegAnimValuesF[numLowerLegAnimValuesF] = {{0.0f,-5},{0.9f,90},{1.25f,-40},{2.0f,0},{3.5f,-5}};
+
 
 //Back
 const int numLowerLegAnimValuesB = 5;
-Animation::KeyFrame lowerLegAnimValuesB[numLowerLegAnimValuesB] = {{0.0f,0},{1.0f,90},{1.25f,-30},{2.0f,0},{3.5f,0}};
+Animation::KeyFrame lowerLegAnimValuesB[numLowerLegAnimValuesB] = {{0.0f,15},{0.6f,90},{1.7f,-40},{2.0f,0},{3.5f,15}};
+
 
 Servo servoUpperFR;
 Servo servoUpperFL;
 Servo servoLowerBL;
 
 Servo servoPingSensor;
+int ultraSoundSignal = 3; // Ultrasound signal pin
 
+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);
+  
   //Create animations
   pingAnim = new Animation(pingAnimValues,pingAnimNumValues,pingAnimMode);
   
   
   servoPingSensor.attach(12);
   
+  pinMode(ultraSoundSignal,OUTPUT);
+
   //Init time
   lastTimeMillis = millis();
 } 
 
+float pingUpdateTimer = 0.0f;
+float animUpdateTimer = 0.0f;
+
 void loop() 
 { 
   //Get current time in seconds
   float timeDelta = (currTimeMillis-lastTimeMillis)/1000.0f;
   lastTimeMillis = currTimeMillis;
   
+  pingUpdateTimer -= timeDelta;
+  
+  if(timeDelta < 0.0f)
+  {
+    timeDelta += 1.0f;
+    
+    updatePing(timeDelta);
+  }
+  
+  animUpdateTimer -= timeDelta;
+  
+  if(animUpdateTimer < 0.0f)
+  {
+    animUpdateTimer += 1.0f/60.0f;
+    
+    UpdateAnims(1.0f/60.0f);
+  }
+} 
+
+void UpdateAnims(float timeDelta)
+{
   //Update animations
   //Right legs
   upperLegServoAnimFR->Update(timeDelta); 
   servoUpperBL.write(topBackLeftAnimVal);
   servoLowerBL.write(bottomBackLeftAnimVal);
  #endif
-  
-} 
+}