Data hosted with ♥ by Pastebin.com - Download Raw - See Original
  1. #include <EEPROM.h>
  2. #include <Servo.h>
  3.  
  4. // SERVO PINS
  5. #define Lhippin         6          // digital pin  6 - Left  hip   servo
  6. #define Rhippin         5          // digital pin  5 - Right hip   servo
  7. #define Lkneepin        8          // digital pin  8 - Left  knee  servo
  8. #define Rkneepin        7          // digital pin  7 - Right knee  servo
  9.  
  10. #define LED13          13          // digital pin  2 - General purpose LED
  11.  
  12. // define global variables
  13. int Lkc;                           // Left  knee center position                                    
  14. int Rkc;                           // Right knee center position
  15. int Lhc;                           // Left  hip  center position
  16. int Rhc;                           // Right hip  center position
  17.  
  18. int Lkp;                           // Left  knee position
  19. int Rkp;                           // Right knee position
  20. int Lhp;                           // Left  hip  position
  21. int Rhp;                           // Right hip  position
  22.  
  23. int stride=400;
  24. int flift=150;                     // how much to tilt foot so toe grips or slides
  25. int steer=0;                       // controls direction of the robot
  26. int i=0;
  27. int timerj = 0;
  28.  
  29.  
  30. // define servos
  31. Servo Lknee;
  32. Servo Rknee;
  33. Servo Lhip;
  34. Servo Rhip;
  35.  
  36. void setup(){
  37.   //  Serial.begin(9600);
  38.   pinMode(LED13,OUTPUT);          // Back pack LED
  39.   if (Lkc<1000 || Lkc>2000 || Lhc<1000 || Lhc>2000 || Rkc<1000 || Rkc>2000 || Rhc<1000 || Rhc>2000){
  40.     // loads defaults if any value invalid
  41.     Lkc=1550;
  42.     Rkc=1450;
  43.     Lhc=1500;
  44.     Rhc=1500;
  45.     //    Store();                   // save defaults to EEPROM
  46.   }
  47.  
  48.   // set servos to center positions (legs straight)
  49.   Lkp=Lkc;
  50.   Rkp=Rkc;
  51.   Lhp=Lhc;
  52.   Rhp=Rhc;
  53.  
  54.   Lknee.attach(Lkneepin);
  55.   Rknee.attach(Rkneepin);
  56.   Lhip.attach(Lhippin);
  57.   Rhip.attach(Rhippin);
  58.  
  59.   Servopos();
  60.   delay(3000);                        //delay before start
  61. }
  62.  
  63. void loop(){
  64.  
  65.     //----------RIGHT------------
  66.     Rkp=Rkp+flift;                      // lift right toe so it does not have traction
  67.     for (i=stride;i>-stride;i-=6){
  68.       Servopos();                       // update servos
  69.       delayMicroseconds(3000);
  70.       digitalWrite(LED13,HIGH);
  71.       /*
  72.     delay(50);
  73.        Serial.print("R ");
  74.        Serial.println(i);
  75.        */
  76.     }
  77.     Rkp=Rkp-flift;                      // lower right toe to gain traction
  78.  
  79.  
  80.     //----------LEFT------------
  81.     Lkp=Lkp-flift;                      // lift left toe so it does not have traction
  82.     for (i=-stride;i<stride;i+=6){
  83.       Servopos();
  84.       delayMicroseconds(3000);
  85.       digitalWrite(LED13,LOW);
  86.       /*
  87.     delay(50);
  88.        Serial.print("L ");    
  89.        Serial.println(i);
  90.        */
  91.     }
  92.     Lkp=Lkp+flift;                       // lower right toe to gain traction
  93. }
  94.  
  95. // update servo positions
  96. void Servopos(){
  97.   Lknee.writeMicroseconds(Lkp+i);
  98.   Rknee.writeMicroseconds(Rkp+i);
  99.   Lhip.writeMicroseconds(Lhp+i);
  100.   Rhip.writeMicroseconds(Rhp+i);
  101. }