Source

ArduinoProjects / Experimental / MicroHorse / MicroHorse.ino

Full commit
// Sweep
// by BARRAGAN <http://barraganstudio.com> 
// This example code is in the public domain.


#include <Servo.h> 
#include <MathTypes.h>
#include <MathUtil.h>
#include <Animation.h>

Animation* upperLegServoAnimFR;
Animation* upperLegServoAnimFL;
Animation* upperLegServoAnimBR;
Animation* upperLegServoAnimBL;
Animation* lowerLegServoAnimFR;
Animation* lowerLegServoAnimFL;
Animation* lowerLegServoAnimBR;
Animation* lowerLegServoAnimBL;

const Animation::AnimMode upperLegMode = Animation::AnimMode_PingPong;
const int numUpperLegAnimValues = 2;
float upperLegAnimValues[numUpperLegAnimValues] = {-60,30};

const Animation::AnimMode lowerLegMode = Animation::AnimMode_Wrap;
const int numLowerLegAnimValues = 2;
float lowerLegAnimValues[numLowerLegAnimValues] = {-30,90};

Servo servoUpperFR;
Servo servoUpperFL;
Servo servoUpperBR;
Servo servoUpperBL;

Servo servoLowerFR;
Servo servoLowerFL;
Servo servoLowerBR;
Servo servoLowerBL;

float lastTimeMillis = 0.0f;
void setup() 
{ 
  //Create animations
  const float animTimeSharedTop = 0.25f;
  const float animTimeSharedBottom = animTimeSharedTop*2.0f;
  
  upperLegServoAnimFR = new Animation(upperLegAnimValues,numUpperLegAnimValues,upperLegMode,animTimeSharedTop);
  upperLegServoAnimFL = new Animation(upperLegAnimValues,numUpperLegAnimValues,upperLegMode,animTimeSharedTop);
  upperLegServoAnimBR = new Animation(upperLegAnimValues,numUpperLegAnimValues,upperLegMode,animTimeSharedTop);
  upperLegServoAnimBL = new Animation(upperLegAnimValues,numUpperLegAnimValues,upperLegMode,animTimeSharedTop);
  
  lowerLegServoAnimFR = new Animation(lowerLegAnimValues,numLowerLegAnimValues,lowerLegMode,animTimeSharedBottom);
  lowerLegServoAnimFL = new Animation(lowerLegAnimValues,numLowerLegAnimValues,lowerLegMode,animTimeSharedBottom);
  lowerLegServoAnimBR = new Animation(lowerLegAnimValues,numLowerLegAnimValues,lowerLegMode,animTimeSharedBottom);
  lowerLegServoAnimBL = new Animation(lowerLegAnimValues,numLowerLegAnimValues,lowerLegMode,animTimeSharedBottom);
  
  upperLegServoAnimFL->currT += 0.8f;
  lowerLegServoAnimFL->currT += 0.8f;
  upperLegServoAnimBL->currT += 0.6f;
  lowerLegServoAnimBL->currT += 0.6f;
  
  upperLegServoAnimFR->currT += 0.2f;
  lowerLegServoAnimFR->currT += 0.2f;
  upperLegServoAnimBR->currT += 0.0f;
  lowerLegServoAnimBR->currT += 0.0f;
  
  servoUpperFR.attach(4);
  servoUpperFL.attach(5);
  servoUpperBR.attach(6);
  servoUpperBL.attach(7);
  
  servoLowerFR.attach(8);
  servoLowerFL.attach(9);
  servoLowerBR.attach(10);
  servoLowerBL.attach(11);
  
  //Init time
  lastTimeMillis = millis();
} 

void loop() 
{ 
  //Get current time in seconds
  float currTimeMillis = millis();
  float timeDelta = (currTimeMillis-lastTimeMillis)/1000.0f;
  lastTimeMillis = currTimeMillis;
  
  //Update animations
  upperLegServoAnimFR->Update(timeDelta); 
  upperLegServoAnimFL->Update(timeDelta);
  upperLegServoAnimBR->Update(timeDelta); 
  upperLegServoAnimBL->Update(timeDelta);    
  
  lowerLegServoAnimFR->Update(timeDelta);
  lowerLegServoAnimFL->Update(timeDelta);
  lowerLegServoAnimBR->Update(timeDelta);
  lowerLegServoAnimBL->Update(timeDelta);
  
  float topFrontRightAnimVal =  upperLegServoAnimFR->CalcAnimVal();
  float topFrontLeftAnimVal = -upperLegServoAnimFL->CalcAnimVal();
  float topBackRightAnimVal =  upperLegServoAnimBR->CalcAnimVal();
  float topBackLeftAnimVal = -upperLegServoAnimBL->CalcAnimVal();
  
  float bottomFrontRightAnimVal =  lowerLegServoAnimFR->CalcAnimVal();
  float bottomFrontLeftAnimVal = -lowerLegServoAnimFL->CalcAnimVal();
  float bottomBackRightAnimVal =  lowerLegServoAnimBR->CalcAnimVal();
  float bottomBackLeftAnimVal = -lowerLegServoAnimBL->CalcAnimVal();
  
  topFrontRightAnimVal += 90.0f;
  topFrontLeftAnimVal += 90.0f;
  topBackRightAnimVal += 90.0f;
  topBackLeftAnimVal += 90.0f;
  
  bottomFrontRightAnimVal += 90.0f;
  bottomFrontLeftAnimVal += 90.0f;
  bottomBackRightAnimVal += 90.0f;
  bottomBackLeftAnimVal += 90.0f;
  
  //Set servo values from animations
  servoUpperFR.write(topFrontRightAnimVal);
  servoUpperFL.write(topFrontLeftAnimVal);
  servoUpperBR.write(topBackRightAnimVal);
  servoUpperBL.write(topBackLeftAnimVal);
  
  servoLowerFR.write(bottomFrontRightAnimVal);
  servoLowerFL.write(bottomFrontLeftAnimVal);
  servoLowerBR.write(bottomBackRightAnimVal);
  servoLowerBL.write(bottomBackLeftAnimVal);
  
}