#include <EEPROM.h>
#include <Servo.h>
// SERVO PINS
#define Lhippin 6 // digital pin 6 - Left hip servo
#define Rhippin 5 // digital pin 5 - Right hip servo
#define Lkneepin 8 // digital pin 8 - Left knee servo
#define Rkneepin 7 // digital pin 7 - Right knee servo
#define LED13 13 // digital pin 2 - General purpose LED
// define global variables
int Lkc; // Left knee center position
int Rkc; // Right knee center position
int Lhc; // Left hip center position
int Rhc; // Right hip center position
int Lkp; // Left knee position
int Rkp; // Right knee position
int Lhp; // Left hip position
int Rhp; // Right hip position
int stride=400;
int flift=150; // how much to tilt foot so toe grips or slides
int steer=0; // controls direction of the robot
int i=0;
int timerj = 0;
// define servos
Servo Lknee;
Servo Rknee;
Servo Lhip;
Servo Rhip;
void setup(){
// Serial.begin(9600);
pinMode(LED13,OUTPUT); // Back pack LED
if (Lkc<1000 || Lkc>2000 || Lhc<1000 || Lhc>2000 || Rkc<1000 || Rkc>2000 || Rhc<1000 || Rhc>2000){
// loads defaults if any value invalid
Lkc=1550;
Rkc=1450;
Lhc=1500;
Rhc=1500;
// Store(); // save defaults to EEPROM
}
// set servos to center positions (legs straight)
Lkp=Lkc;
Rkp=Rkc;
Lhp=Lhc;
Rhp=Rhc;
Lknee.attach(Lkneepin);
Rknee.attach(Rkneepin);
Lhip.attach(Lhippin);
Rhip.attach(Rhippin);
Servopos();
delay(3000); //delay before start
}
void loop(){
//----------RIGHT------------
Rkp=Rkp+flift; // lift right toe so it does not have traction
for (i=stride;i>-stride;i-=6){
Servopos(); // update servos
delayMicroseconds(3000);
digitalWrite(LED13,HIGH);
/*
delay(50);
Serial.print("R ");
Serial.println(i);
*/
}
Rkp=Rkp-flift; // lower right toe to gain traction
//----------LEFT------------
Lkp=Lkp-flift; // lift left toe so it does not have traction
for (i=-stride;i<stride;i+=6){
Servopos();
delayMicroseconds(3000);
digitalWrite(LED13,LOW);
/*
delay(50);
Serial.print("L ");
Serial.println(i);
*/
}
Lkp=Lkp+flift; // lower right toe to gain traction
}
// update servo positions
void Servopos(){
Lknee.writeMicroseconds(Lkp+i);
Rknee.writeMicroseconds(Rkp+i);
Lhip.writeMicroseconds(Lhp+i);
Rhip.writeMicroseconds(Rhp+i);
}