baby_steps.ino backup

baby_steps.ino

// https://github.com/netlabtoolkit/VarSpeedServo
#include <VarSpeedServo.h>

#define LEFT_ANKLE_CENTRE 90


//Servo servo;
VarSpeedServo LeftHip;
VarSpeedServo LeftAnkle;

VarSpeedServo RightHip;
VarSpeedServo RightAnkle;

byte LeftHipPin          = 9;  
byte RightHipPin         = 10;
byte LeftAnklePin        = 11;
byte RightAnklePin        = 12;

byte lastLh = 0;
byte lastLa = 0;
byte lastRh = 0;
byte lastRa = 0;

byte moveSpeed = 20;

void setup()
{
  //Serial.begin(9600);
  

  
  /*
  LeftHip.attach(LeftHipPin);		// config the left/right/center leg servos
  LeftAnkle.attach(LeftAnklePin);

  RightHip.attach(RightHipPin);         // config the right leg
  RightAnkle.attach(RightAnklePin);
  */
//XXservosCenter();
  
  
}


void loop()
{     

  walk();

}

void servosCenter() {
  LeftHip.write(90);
  LeftAnkle.write(LEFT_ANKLE_CENTRE);
  RightHip.write(90);
  RightAnkle.write(90);  
  
  /*
  LeftHip.detach();
  LeftAnkle.detach();
  RightHip.detach();
  RightAnkle.detach();
  */
}

void XXservosCenter() {
  
    LeftHip.attach(LeftHipPin);
    LeftHip.write(90);
    //LeftHip.detach();
    
  LeftAnkle.attach(LeftAnklePin);
  LeftAnkle.write(LEFT_ANKLE_CENTRE);
  //LeftAnkle.detach();
  
  RightHip.attach(RightHipPin);  
  RightHip.write(90);
  //RightHip.detach();
  
  RightAnkle.attach(RightAnklePin);
  RightAnkle.write(90);  
  //RightAnkle.detach();
  
  delay(100);
  LeftHip.detach();
  LeftAnkle.detach();
  RightHip.detach();
  RightAnkle.detach();
}

/**
 * csv of left ankle, left hip, right ankle, right hip
 */
void servosPosition(int la, int lh, int ra, int rh) {

  // Ankles first, one servo at a time.
  
  if (la > 59 && lastLa != la) {
    constrain(la, 60, 120);
    LeftAnkle.attach(LeftAnklePin);
    LeftAnkle.write(la, moveSpeed, true);
    LeftAnkle.detach();
    lastLa = la;
  }
  
  if (ra > 59 && lastRa != ra) {
    constrain(ra, 60, 120);
    RightAnkle.attach(RightAnklePin);
    RightAnkle.write(ra, moveSpeed, true);
    RightAnkle.detach();
    lastRa = ra;
  }
  
  if (lh > 59 && lastLh != lh) {
    constrain(lh, 60, 120);
    LeftHip.attach(LeftHipPin);
    LeftHip.write(lh, moveSpeed +20, true);
    LeftHip.detach();
    lastLh = lh;
  }
  
  if (rh > 59 && lastRh != rh) {
    constrain(rh, 60, 120);
    RightHip.attach(RightHipPin); 
    RightHip.write(rh, moveSpeed +20, true);
    RightHip.detach();
    lastRh = rh;
  }
   
   /*
  //delay(1000);
  LeftHip.detach();
  LeftAnkle.detach();
  RightHip.detach();
  RightAnkle.detach();  
      */
}

void walk() {
  // left ankle, left hip, right ankle, right hip
  

  servosPosition(90,105,90,100); 
  //delay(1000);

  servosPosition(90,105,105,70);

  servosPosition(100,80,105,70);
  //delay(1000);

  servosPosition(90,80,90,70);
  //delay(1000);

  servosPosition(75,105,80,100);
  //delay(1000);

  servosPosition(90,105,90,100);
  //delay(1000);
 
  // back to the start
  servosPosition(90,105,90,100); 

  
}