Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #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);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement