Changing code to work with Adafruit Servo Shield

Projects General Guidance
1

Hello, I am a beginner with Arduino and I am trying to build a 6 DOF Bipedal robot. I am trying to use a servo shield however the code was only adapted to work without a servo shield. How can I modify the code to work with the adafruit servo shield?

#define hipLOffset 105
#define kneeLOffset 155
#define ankleLOffset 85
#define hipROffset 82
#define kneeROffset 25
#define ankleROffset 85

#define l1 5
#define l2 5.7

#define stepClearance 1
#define stepHeight 10

#include <Servo.h>
#include "constants.h"
#include <Adafruit_PWMServoDriver.h>

Servo hipL;
Servo hipR;
Servo kneeL;
Servo kneeR;
Servo ankleL;
Servo ankleR;

void updateServoPos(int target1, int target2, int target3, char leg){
  if (leg == 'l'){
    hipL.write(hipLOffset - target1);
    kneeL.write(kneeLOffset - target2);
    ankleL.write(2*ankleLOffset - target3);
  }
  else if (leg == 'r'){
    hipR.write(hipROffset + target1);
    kneeR.write(kneeROffset + target2);
    ankleR.write(target3);
  }
}

void pos(float x, float z, char leg){
  float hipRad2 = atan(x/z);
  float hipDeg2 = hipRad2 * (180/PI);

  float z2 = z/cos(hipRad2);

  float hipRad1 = acos((sq(l1) + sq(z2) - sq(l2))/(2*l1*z2));
  float hipDeg1 = hipRad1 * (180/PI);
  
  float kneeRad = PI - acos((sq(l1) + sq(l2) - sq(z2))/(2*l1*l2));

  float ankleRad = PI/2 + hipRad2 - acos((sq(l2) + sq(z2) - sq(l1))/(2*l2*z2));
  
  float hipDeg = hipDeg1 + hipDeg2;
  float kneeDeg = kneeRad * (180/PI);
  float ankleDeg = ankleRad * (180/PI);

//  Serial.print(hipDeg);
//  Serial.print("\t");
//  Serial.print(kneeDeg);
//  Serial.print("\t");
//  Serial.println(ankleDeg);

  updateServoPos(hipDeg, kneeDeg, ankleDeg, leg);  
}

void takeStep(float stepLength, int stepVelocity){
  for (float i = stepLength; i >= -stepLength; i-=0.5){
    pos(i, stepHeight, 'r');
    pos(-i, stepHeight - stepClearance, 'l');
    delay(stepVelocity);
  }

  for (float i = stepLength; i >= -stepLength; i-=0.5){
    pos(-i, stepHeight - stepClearance, 'r');
    pos(i, stepHeight, 'l');
    delay(stepVelocity);
  }
}

void initialize(){
  for (float i = 10.7; i >= stepHeight; i-=0.1){
    pos(0, i, 'l');
    pos(0, i, 'r');
  }
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  hipL.attach(9);
  hipR.attach(8);
  kneeL.attach(7);
  kneeR.attach(6);
  ankleL.attach(5);
  ankleR.attach(4);

  hipL.write(hipLOffset);
  kneeL.write(kneeLOffset);
  ankleL.write(ankleLOffset);
  
  hipR.write(hipROffset);
  kneeR.write(kneeROffset);
  ankleR.write(ankleROffset);

  delay(5000);
  
  initialize();
}

void loop() {
  // put your main code here, to run repeatedly:
  takeStep(2, 0);
}
2

First off, an Adafruit motor shield can only drive 2 servo motors so you will need 3 shields. Luckily, they are stackable so they can have different addresses. Look at the servo library examples for using the adafruit shield

3

I'm fairly sure OP means a 16-channel PCA9685 shield.
A link to the shield could have stopped confusion.

Maybe wise to start a separate project with that shield first, with code from Adafruit, so you understand PCA9685 control. Then it might be easier to change your existing code.
Leo..

4

Take out the Servo library and put in this:

class Servo
{
  byte pin;
public:
  void attach(int _pin) {pin = _pin;}
  void write(int angle)
  {
    ServoDriver.setPWM(pin, 0, map(angle, 0, 180, SERVOMIN, SERVOMAX));
  }
};

That should provide a good portion of the translation between the Servo library and the Adafruit_PWMServoDriver library. You will probably want to use different 'pin' numbers.

Too bad you aren't using arrays for your servos, offsets, pin numbers...

5

This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.