servos_serial.ino backup

servos_serial.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 moveSpeed = 10;

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);
  
  servosCenter();
  
}


void loop()
{     

  
  while (Serial.available() > 0) {
    // csv of left ankle, left hip, right ankle, right hip
    // 60,0,0,0-
    
    int la = Serial.parseInt();
    int lh = Serial.parseInt();
    int ra = Serial.parseInt();
    int rh = Serial.parseInt();
    
    byte in = Serial.read();

    if (in == 'w') { 
      walk();
    } else if (in == '-') { 
      Serial.println(la);
 
      if (la > 59) {
        constrain(la, 60, 120);
        LeftAnkle.write(la, moveSpeed);
      }
      if (ra > 59) {
        constrain(ra, 60, 120);
        RightAnkle.write(ra, moveSpeed);
      }
      if (lh > 59) {
        constrain(lh, 60, 120);
        LeftHip.write(lh, moveSpeed +5);
      }
      if (rh > 59) {
        constrain(rh, 60, 120);
        RightHip.write(rh, moveSpeed +5);
      }

      Serial.println(la);
    }
  }

}

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

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

  constrain(la, 60, 120);
  LeftAnkle.write(la, moveSpeed);
  
  constrain(ra, 60, 120);
  RightAnkle.write(ra, moveSpeed);
  
  constrain(lh, 60, 120);
  LeftHip.write(lh, moveSpeed +5);
  
  constrain(rh, 60, 120);
  RightHip.write(rh, moveSpeed +5);
      
      
}

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

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

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

  servosPosition(90,75,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); 

  
}