erikzandboer

Servo Walking example

May 14th, 2022 (edited)
130
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. byte Servo1_Target = 90;
  2. byte Servo2_Target = 90;
  3. byte Servo1_Current = 0;
  4. byte Servo2_Current = 180;
  5.  
  6. void setup()
  7. {
  8.   // Init pins here
  9.  
  10.   // Init servos here
  11.  
  12. }
  13.  
  14. void loop()
  15. {
  16.   // --------------- HANDLE SERVO WALKING -------------------
  17.   if (Servo1_Current < Servo1_Target)
  18.   {
  19.     Servo1_Current++;
  20.     servo_1.write(Servo1_Current);
  21.   }
  22.   if (Servo1_Current > Servo1_Target)
  23.   {
  24.     Servo1_Current--;
  25.     servo_1.write(Servo1_Current);
  26.   }
  27.   if (Servo2_Current < Servo2_Target)
  28.   {
  29.     Servo2_Current++;
  30.     servo_2.write(Servo2_Current);
  31.   }
  32.   if (Servo2_Current > Servo2_Target)
  33.   {
  34.     Servo2_Current--;
  35.     servo_2.write(Servo2_Current);
  36.   }
  37.  
  38.   delay(1); // Make sure the code takes more than 1ms to execute (otherwise the while below might fire of more than 1 time per 25ms)
  39.  
  40.   while (millis()%25 !=0); // This will ensure proper timing of 25 ms for the loop independent of code length
  41. }
RAW Paste Data Copied