#include <Adafruit_PWMServoDriver.h>
#include <Wire.h>
Adafruit_PWMServoDriver servos = Adafruit_PWMServoDriver();
const int potPin = 0;
void setup() {
Serial.begin(9600);
servos.begin();
servos.setPWMFreq(60);
delay(10);
}
void loop() {
int potVal = analogRead(potPin);
moveLeg(potVal, 0, 4, 96, 54, 180, 120); // Leg 1
moveLeg(potVal, 1, 5, 83, 34, 180, 120); // Leg 2
moveLeg(potVal, 2, 6, 83, 37, 180, 120); // Leg 3
moveLeg(potVal, 3, 7, 95, 27, 180, 115); // Leg 4
Serial.println(potVal);
delay(20);
}
//----------------------------------------------------------------------------------------//
int angle2pulse(int angle){
int pulseLength = map(angle, 0, 180, 150, 600);
pulseLength = constrain(pulseLength, 150, 600);
return pulseLength;
}
void moveLeg(int potVal, uint8_t hip, uint8_t knee, int closedHipAngle, int closedKneeAngle,int expandedHipAngle,int expandedKneeAngle){
int angle_H = map(potVal, 0, 1024, closedHipAngle, expandedHipAngle);
int angle_K = map(potVal, 0, 1024, closedKneeAngle, expandedKneeAngle);
servos.setPWM(hip, 0, angle2pulse(angle_H));
servos.setPWM(knee, 0, angle2pulse(angle_K));
}
Comments