Quadruped Pumpkin

1#include <Adafruit_PWMServoDriver.h>
2#include <Wire.h>
3
4Adafruit_PWMServoDriver  servos = Adafruit_PWMServoDriver();
5const int potPin = 0;
6
7void setup()  {
8  Serial.begin(9600);
9  servos.begin();
10  servos.setPWMFreq(60);
11  delay(10);
12}
13
14void loop() {
15  int potVal = analogRead(potPin);
16  moveLeg(potVal, 0, 4, 96, 54, 180, 120);   // Leg 1
17  moveLeg(potVal, 1, 5,  83, 34, 180, 120);   // Leg 2
18  moveLeg(potVal, 2, 6, 83, 37, 180, 120);   //  Leg 3
19  moveLeg(potVal, 3, 7, 95, 27, 180, 115);   // Leg 4
20  Serial.println(potVal);
21  delay(20);
22}
23
24//----------------------------------------------------------------------------------------//
25int  angle2pulse(int angle){
26  int pulseLength = map(angle, 0, 180, 150, 600);
27  pulseLength = constrain(pulseLength, 150, 600);
28  return pulseLength;
29}
30
31void  moveLeg(int potVal, uint8_t hip, uint8_t knee, int closedHipAngle, int closedKneeAngle,int  expandedHipAngle,int expandedKneeAngle){
32  int angle_H = map(potVal, 0, 1024,  closedHipAngle, expandedHipAngle);
33  int angle_K = map(potVal, 0, 1024, closedKneeAngle,  expandedKneeAngle);
34  servos.setPWM(hip, 0, angle2pulse(angle_H));
35  servos.setPWM(knee,  0, angle2pulse(angle_K));
36}