Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- // include needed librarys
- #include <Adafruit_PWMServoDriver.h>
- #include <Wire.h>
- // create servo driver (defaults to addr 40)
- Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
- // give meaningful names to each servo channel
- int FRleg = 0;
- int FLleg = 1;
- int BRleg = 2;
- int BLleg = 3;
- int FRK = 4;
- int FLK = 5;
- int BRK = 6;
- int BLK = 7;
- float FR_init = 1350;
- float FL_init = 1350;
- float BR_init = 1440;
- float BL_init = 1440;
- float FRK_init = 1350;
- float FLK_init = 1350;
- float BRK_init = 1350;
- float BLK_init = 1350;
- float SERVOFREQ = 50;
- float pulseconstant;
- void setup() {
- pulseconstant = (1000000/SERVOFREQ)/4096;
- pwm.begin();
- pwm.setPWMFreq(SERVOFREQ);
- servoWrite(FRleg, FR_init);
- servoWrite(FLleg, FL_init);
- servoWrite(BRleg, BR_init);
- servoWrite(BLleg, BL_init);
- servoWrite(FRK, FRK_init);
- servoWrite(FLK, FLK_init);
- servoWrite(BRK, BRK_init);
- servoWrite(BLK, BLK_init);
- }
- void loop() {
- forward1();
- back();
- forward2();
- }
- void servoWrite(uint8_t n, float pulse) {
- float pulsetick = pulse/pulseconstant;
- pwm.setPWM(n, 0, pulsetick);
- }
- void forward1() {
- servoWrite(FRleg, 1600);
- servoWrite(BLleg, 1600);
- delay(500);
- }
- void back() {
- servoWrite(FRleg, 1200);
- servoWrite(BLleg, 1200);
- delay(500);
- }
- void forward2() {
- servoWrite(FLleg, 1600);
- servoWrite(BRleg, 1600);
- delay(500);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement