Source

ArduinoProjects / Experimental / MicroHorse / MicroHorse.ino

Full commit
// Sweep
// 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>

#define DEBUG_RESET 0
 
const float updateAnimSPF = 1.0f/30.0f;
  
Animation* upperLegServoAnimFR;
Animation* upperLegServoAnimFL;
Animation* upperLegServoAnimBR;
Animation* upperLegServoAnimBL;
Animation* lowerLegServoAnimFR;
Animation* lowerLegServoAnimFL;
Animation* lowerLegServoAnimBR;
Animation* lowerLegServoAnimBL;
Animation* pingAnim;

const float movementMin = 0.0f;
const float movementMax = 1.0f;

const float closenessEffectMin = 0.5f;
const float closenessEffectMax = 1.0f;

const float animSpeedMin = 1.25f;
const float animSpeedMax = 2.5f;

const float movementDistCheckMax = 30.0f;

float overallMovementR = 1.0f;
float overallMovementL = 1.0f;

float animSpeed = 3.0f;

const float backLegOffset = 0.8f;

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,-70},{0.75f,70}};

const Animation::AnimMode upperLegMode = Animation::AnimMode_Wrap;
const Animation::AnimMode lowerLegMode = Animation::AnimMode_Wrap;

//Top

//Front
const int numUpperLegAnimValuesF = 4;
Animation::KeyFrame upperLegAnimValuesF[numUpperLegAnimValuesF] = {{0.0f,20},{1.0f,-60},{2.1f,-20},{3.5f,20}};

//Back
const int numUpperLegAnimValuesB = 4;
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,-5},{0.9f,90},{1.25f,-20},{2.0f,0},{3.5f,-5}};


//Back
const int numLowerLegAnimValuesB = 5;
Animation::KeyFrame lowerLegAnimValuesB[numLowerLegAnimValuesB] = {{0.0f,15},{0.6f,90},{1.7f,-20},{2.0f,0},{3.5f,15}};


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

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

unsigned long lastPingStateMicros;

unsigned long echo = 0;

float lastTimeMillis = 0.0f;
void setup() 
{ 
  Serial.begin(9600);
  
  sonar = new NewPing(ultraSoundSignal,ultraSoundSignal,ultrasonicMaxDistanceCM);
  
  //Create animations
  pingAnim = new Animation(pingAnimValues,pingAnimNumValues,pingAnimMode);
  
  //Left legs
  upperLegServoAnimFL = new Animation(upperLegAnimValuesF,numUpperLegAnimValuesF,upperLegMode);
  lowerLegServoAnimFL = new Animation(lowerLegAnimValuesF,numLowerLegAnimValuesF,lowerLegMode);
  upperLegServoAnimBL = new Animation(upperLegAnimValuesB,numUpperLegAnimValuesB,upperLegMode);
  lowerLegServoAnimBL = new Animation(lowerLegAnimValuesB,numLowerLegAnimValuesB,lowerLegMode);
  
  //Right legs
  upperLegServoAnimFR = new Animation(upperLegAnimValuesF,numUpperLegAnimValuesF,upperLegMode);
  lowerLegServoAnimFR = new Animation(lowerLegAnimValuesF,numLowerLegAnimValuesF,lowerLegMode);
  upperLegServoAnimBR = new Animation(upperLegAnimValuesB,numUpperLegAnimValuesB,upperLegMode);
  lowerLegServoAnimBR = new Animation(lowerLegAnimValuesB,numLowerLegAnimValuesB,lowerLegMode);

  //Right legs
  upperLegServoAnimFR->currT += rightSideOffset;
  lowerLegServoAnimFR->currT += rightSideOffset;
  upperLegServoAnimBR->currT += rightSideOffset;
  lowerLegServoAnimBR->currT += rightSideOffset;
  
  //Left legs
  upperLegServoAnimBL->currT += backLegOffset;
  lowerLegServoAnimBL->currT += backLegOffset;

  //Right legs
  upperLegServoAnimBR->currT += backLegOffset;
  lowerLegServoAnimBR->currT += backLegOffset;
  
  servoUpperFR.attach(4);
  servoUpperFL.attach(5);
  servoUpperBR.attach(6);
  servoUpperBL.attach(7);
  
  servoLowerFR.attach(8);
  servoLowerFL.attach(9);
  servoLowerBR.attach(10);
  servoLowerBL.attach(11);
  
  servoPingSensor.attach(12);
  
  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;
//float minDist = 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;
        //distInLeft = minDist;
        //Serial.print("Left:");
        //Serial.println(distInLeft);
        
        break;
      }
      case PingState_Center:
      { 
        distInCenter = distInAverage;
        //distInCenter = minDist;
        //Serial.print("Center:");
        //Serial.println(distInCenter);
        
        break;
      }
      case PingState_Right:
      {
        distInRight = distInAverage;
        //distInRight = minDist;
        //Serial.print("Right:");
        //Serial.println(distInRight);
        
        break;
      }
    }

    //minDist = movementDistCheckMax;
    distInAccum = 0.0f;
    numPings = 0;
    
    pingState = newState;
}

void loop() 
{ 
  //Get current time in seconds
  float currTimeMillis = millis();
  float timeDelta = (currTimeMillis-lastTimeMillis)/1000.0f;
  lastTimeMillis = currTimeMillis;
  
  pingUpdateTimer -= timeDelta;
  if(pingUpdateTimer < 0.0f)
  {
    pingUpdateTimer += 0.01f;

    float distInches = sonar->ping_in();
    ++numPings;
    
    distInAccum += distInches;
    
    /*if(distInches < minDist)
    {
      minDist = distInches;
    }*/
    
    static const float maxAngle = 35.0f;
    
    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;
      }
    }
  }
  
  float centerDistT = distInCenter/movementDistCheckMax;
  //float closeness = 1.0f-centerDistT;
  
  /*const float distDiff = distInRight-distInLeft;
  overallMovementL = (distDiff/movementDistCheckMax)*0.5f+0.5f;
  overallMovementR = 1.0f-overallMovementL;*/
  
  overallMovementL = MinF(1.0f,distInRight/movementDistCheckMax);
  overallMovementR = MinF(1.0f,distInLeft/movementDistCheckMax);
  
  //overallMovementL += centerDistT*0.5f;
  //overallMovementR += centerDistT*0.5f;
  
  overallMovementL = MinF(1.0f,overallMovementL);
  overallMovementR = MinF(1.0f,overallMovementR);
  
  const float maxMove = MaxF(overallMovementL,overallMovementR);
  const float addToMoveT = Lerp(closenessEffectMin,closenessEffectMax,centerDistT);
  const float addToMove = (1.0f-maxMove)*addToMoveT;
  
  overallMovementL += addToMove;
  overallMovementR += addToMove;

  overallMovementL = Lerp(movementMin,movementMax,overallMovementL);
  overallMovementR = Lerp(movementMin,movementMax,overallMovementR);
  
  animSpeed = Lerp(animSpeedMin,animSpeedMax,centerDistT);
  
  Serial.print("Left: ");
  Serial.print(overallMovementL);
  Serial.print(", Right: ");
  Serial.print(overallMovementR);
  Serial.print(", Anim Speed: ");
  Serial.println(animSpeed);
  
  //Left legs
  upperLegServoAnimFL->animSpeed = animSpeed;
  lowerLegServoAnimFL->animSpeed = animSpeed;
  upperLegServoAnimBL->animSpeed = animSpeed;
  lowerLegServoAnimBL->animSpeed = animSpeed;

  //Right legs
  upperLegServoAnimFR->animSpeed = animSpeed;
  lowerLegServoAnimFR->animSpeed = animSpeed;
  upperLegServoAnimBR->animSpeed = animSpeed;
  lowerLegServoAnimBR->animSpeed = animSpeed;
  
  animUpdateTimer -= timeDelta;
  if(animUpdateTimer < 0.0f)
  {
    animUpdateTimer += updateAnimSPF;
    
    UpdateAnims(updateAnimSPF);
  }
} 

void UpdateAnims(float timeDelta)
{
  //Update animations
  //Right legs
  upperLegServoAnimFR->Update(timeDelta); 
  lowerLegServoAnimFR->Update(timeDelta);
  upperLegServoAnimBR->Update(timeDelta); 
  lowerLegServoAnimBR->Update(timeDelta);
  
  //Left legs
  upperLegServoAnimFL->Update(timeDelta);
  lowerLegServoAnimFL->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();
  float topBackLeftAnimVal = -upperLegServoAnimBL->CalcAnimVal();
  
  float bottomFrontRightAnimVal =  lowerLegServoAnimFR->CalcAnimVal();
  float bottomFrontLeftAnimVal = -lowerLegServoAnimFL->CalcAnimVal();
  float bottomBackRightAnimVal =  lowerLegServoAnimBR->CalcAnimVal();
  float bottomBackLeftAnimVal = -lowerLegServoAnimBL->CalcAnimVal();
  
   //Right legs
  topFrontRightAnimVal *= overallMovementR;
  bottomFrontRightAnimVal *= overallMovementR;
  topBackRightAnimVal *= overallMovementR;
  bottomBackRightAnimVal *= overallMovementR;
  
  //Left legs
  topFrontLeftAnimVal *= overallMovementL;
  bottomFrontLeftAnimVal *= overallMovementL;
  topBackLeftAnimVal *= overallMovementL;
  bottomBackLeftAnimVal *= overallMovementL;
  
  //Right legs
  topFrontRightAnimVal += 90.0f;
  bottomFrontRightAnimVal += 90.0f;
  topBackRightAnimVal += 90.0f;
  bottomBackRightAnimVal += 90.0f;
  
  //Left legs
  topFrontLeftAnimVal += 90.0f;
  bottomFrontLeftAnimVal += 90.0f;
  topBackLeftAnimVal += 90.0f;
  bottomBackLeftAnimVal += 90.0f;  
  
  servoPingSensor.write(finalPingAnimVal); 
  
 #if !DEBUG_RESET
   //Set servo values from animations
  servoUpperFR.write(topFrontRightAnimVal);
  servoLowerFR.write(bottomFrontRightAnimVal);
  servoUpperBR.write(topBackRightAnimVal);
  servoLowerBR.write(bottomBackRightAnimVal);
  
  servoUpperFL.write(topFrontLeftAnimVal);
  servoLowerFL.write(bottomFrontLeftAnimVal);
  servoUpperBL.write(topBackLeftAnimVal);
  servoLowerBL.write(bottomBackLeftAnimVal);
 #endif
}