Servo Range of Motion - Code help please

Hello all. I'm trying to debug some code for a BD-1 Star Wars Droid that wasnt written by me. Essentially I set up a Switch so I can test the Min and Max values for each servo. The issue I am having is that no matter what I set the min and max values to, the servo only moves half that distance. So for example, I have a 180 degree servo, max out the Servo Max and it still will only move from the center/home position 90 degrees and then back.

Here is the main code:

// DS-72

int DelayStep = 1; //number of steps in each loop - this affects the speed of all servos - higher is faster

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwmMain = Adafruit_PWMServoDriver();
Adafruit_PWMServoDriver pwmBase = Adafruit_PWMServoDriver();

#include <Arduino.h>
#include <SoftwareSerial.h>
#include <DFPlayerMini_Fast.h>

int incomingByte = 9999;
uint16_t RunMode = 0;

long timer = 0;
#define lTiltHome 300
uint16_t lTiltPos = lTiltHome;
uint16_t lTiltPrevPos = lTiltHome;

#define rTiltHome 315
uint16_t rTiltPos = rTiltHome;
uint16_t rTiltPrevPos = rTiltHome;

#define turnHome 315 //
uint16_t turnPos = turnHome;
uint16_t turnPrevPos = turnHome;

#define earHome 300 //
uint16_t earPos = earHome;
uint16_t earPrevPos = earHome;


#define holoHome 320 //
uint16_t holoPos = holoHome;
uint16_t holoPrevPos = holoHome;

#define aperHome 300 //
uint16_t aperPos = aperHome;
uint16_t aperPrevPos = aperHome;

#define bodyHome 250 //
uint16_t bodyPos = bodyHome;
uint16_t bodyPrevPos = bodyHome;

#define neckHome 300
uint16_t neckPos = neckHome;
uint16_t neckPrevPos = neckHome;


//set delays
uint16_t earDelay;
uint16_t earDelayCount;
uint16_t holoDelay;
uint16_t holoDelayCount;
uint16_t aperDelay;
uint16_t aperDelayCount;
uint16_t bodyDelay;
uint16_t bodyDelayCount;
uint16_t neckDelay;
uint16_t neckDelayCount;
uint16_t turnDelay;
uint16_t turnDelayCount;
uint16_t rTiltDelay;
uint16_t rTiltDelayCount;
uint16_t lTiltDelay;
uint16_t lTiltDelayCount;
uint8_t servonum = 0;


bool bEars = false;
bool bHolo = false;
bool bAper = false;
bool bBody = false;
bool bNeck = false;
bool bTurn = false;
bool bRTilt = false;
bool bLTilt = false;
bool bLegs = false;
bool bProjector = false;
bool bDance = false;

int receivedInt; //prepare variable for input

void setup() {

  pwmMain.begin();
  pwmBase.begin();

  pwmMain.setOscillatorFrequency(26500000);
  pwmMain.setPWMFreq(50);  // Analog servos run at ~50 Hz updates
  pwmBase.setPWMFreq(50);
  delay(10);

  //start serial connection
  Serial.begin(9600);
  randomSeed(analogRead(7));

  //Begin pwmMain **********
#define pwmMainAddr 1
#define servoEar1 0
#define servoEar2 1
#define servoRTilt 2
#define servoLTilt 3
#define servoHeadTurn 5
#define servoNeck 6
#define servoBody 7
#define aperb 8
#define aperg 9
#define aperr 10
#define servoAperature 11
#define holob 12
#define holog 13
#define holor 14
#define servoHolo 15
  //End pwmMain **********

  //Begin pwmBase **********
#define pwmBaseAddr 0
#define servoLegL 0
#define servoLegR 1
  //End pwmBase **********

  // servo extents
#define earMin 200   
#define earMax 400
#define holoMin 285
#define holoMax 355
#define aperMin 160
#define aperMax 420
#define bodyMin 200
#define bodyMax 300
#define neckMin 150
#define neckMax 460
#define turnMin 215
#define turnMax 415
#define rTiltMin 215
#define rTiltMax 415
#define lTiltMin 160
#define lTiltMax 600


  //delay paramaters for servo speed - less delay and more steps is faster
#define DelayMin 1 //minumum delay between servo steps - lower is faster
#define DelayMed 4 //medium delay between servo steps - servos moving heavier parts should have a lower top speed
#define DelayMax 10 //maximum delay between servo steps - higher is slower
  int DelayStep = 1; //number of steps in each loop - this affects the speed of all servos - higher is faster

  delay (1500);

  CenterAll();
  printMenu();
}

// BEGIN LOOP ***************
void loop()
{
 recvInt();       //call function to read value from console
 PrintByInput();   //call function with SWITCH 
}

void recvInt() { 
 while(Serial.available() == 0) {  } //wait for input from console
 receivedInt = Serial.parseInt();   // parse input to INT, other values are ignored
  
}

void PrintByInput() {
 if (receivedInt != NULL) {       //call SWITCH if input is available
  switch (receivedInt) {
case 1:
      Serial.println("Testing Ear 1 :");
      DelayStep = 1;
      // begin ears **************
      MoveServo(pwmMainAddr, servoEar1, bEars, earPrevPos, earPos, earMin, earMax, earDelay, earDelayCount, DelayMin, DelayMax, 0, 0);
      // end ears **************
      break;
      
    case 2:
      Serial.println("Testing Ear 2 :");
      // begin ears **************
      MoveServo(pwmMainAddr, servoEar2, bEars, earPrevPos, earPos, earMin, earMax, earDelay, earDelayCount, DelayMin, DelayMax, 0, 0);
      // end ears **************
      break;
      
    case 3:
      Serial.println("Testing Right Tilt :");
      // begin rTilt **************
      Serial.println("First Center Servo:");
      pwmMain.setPWM(servoRTilt, 0, rTiltHome);
      delay(100);
      Serial.println("Then home Servo: ");
      // begin rTilt **************
      MoveServo(pwmMainAddr, servoRTilt, bRTilt, rTiltPrevPos, rTiltPos, rTiltMin, rTiltMax, rTiltDelay, rTiltDelayCount, DelayMin, DelayMax, 0, rTiltHome);
      // end rTilt **************
      delay(100);
      Serial.println("Finally Move Servo:"); 
      pwmMain.setPWM(servoRTilt, 0, map(rTiltPrevPos, rTiltMin, rTiltMax, 0, 180));
      bRTilt = true;
      MoveServo(pwmMainAddr, servoRTilt, bRTilt, rTiltPrevPos, rTiltPos, rTiltMin, rTiltMax, rTiltDelay, rTiltDelayCount, DelayMin, DelayMax, 0, 0);
      // end rTilt **************
      Serial.println("Done!");
      break;
      
    case 4:
    Serial.println("Testing Left Tilt :");
      // begin lTilt **************
      MoveServo(pwmMainAddr, servoLTilt, bLTilt, lTiltPrevPos, lTiltPos, lTiltMin, lTiltMax, lTiltDelay, lTiltDelayCount, DelayMin, DelayMax, 0, 0);
      // end lTilt **************
      break;
      
    case 5:
    Serial.println("Testing Head Turn  :");
      // begin turn **************
      MoveServo(pwmMainAddr, servoHeadTurn, bTurn, turnPrevPos, turnPos, turnMin, turnMax, turnDelay, turnDelayCount, DelayMin, DelayMax, 0, 0);
      // end turn **************
      break;
      
    case 6:
    Serial.println("Testing Neck :");
      // begin neck **************
      MoveServo(pwmMainAddr, servoNeck, bNeck, neckPrevPos, neckPos, neckMin, neckMax, neckDelay, neckDelayCount, DelayMed, DelayMax, 0, 0);
      // end neck **************
      break;
      
    case 7:
    Serial.println("Testing Body :");
      // begin body **************
      MoveServo(pwmMainAddr, servoBody, bBody, bodyPrevPos, bodyPos, bodyMin, bodyMax, bodyDelay, bodyDelayCount, DelayMin, DelayMax, bodyHome + 50 , 0);
      // end body **************
      break;
      
    case 8:
    Serial.println("Testing Aperature :");
      // begin aperature **************
      MoveServo(pwmMainAddr, servoAperature, bAper, aperPrevPos, aperPos, aperMin, aperMax, aperDelay, aperDelayCount, DelayMin, DelayMax, 0, 0);
      // end aperature **************
      break;

    case 9:
    Serial.println("Testing Holoprojector :");
      // begin holoprojector **************
      MoveServo(pwmMainAddr, servoHolo, bHolo, holoPrevPos, holoPos, holoMin, holoMax, holoDelay, holoDelayCount, DelayMin, DelayMax, 0, 0);
      // end holoprojector **************
      break;

    case 10:
      Serial.println("Centering All Servos :");
      CenterAll();
      break;

    case 11:
      Serial.println("Homing All Servos :");
      void HomeAll();
      break;

    default:
     Serial.println("!WRONG INPUT!");
      break;
  }     
 receivedInt = NULL;
 }

}
// END LOOP ***********************

And here is the function being called:

/*
   servoName - Name of servo to move  eg. servoLegs
   bEnableServo - Boolean for the servo eg. bLegs
   prevPos - Servo's previous position eg. legsPrevPos
   currPos- Servo's current position eg. legsPos
   servoMin - Servo's minimum extent eg. legsMin
   servoMax - Servo's maximum extent eg. legsMax
   servoDelay - Servo's delay random number between delMin and delMax eg. legsDelay
   delayCount - Counter to keep track of where we are eg. legsDelayCount
   delMin - Minimum delay (fastest speed) for servoDelay eg. DelayMed or DelayMin
   delMax - Maximum delay (slowest speed) for servoDelay eg. DelayMin or DelayMed
   returnPos - If not zero, then return to this position after movement eg. legsHome or legsMin
*/
void MoveServo(uint16_t pwmAddress, uint16_t servoName, bool& bEnableServo, uint16_t& prevPos, uint16_t& currPos, uint16_t servoMin, uint16_t servoMax, uint16_t& servoDelay, uint16_t& delayCount, uint16_t delMin, uint16_t delMax, uint16_t returnPos, uint16_t destPos )
{
  Serial.println("Testing Servo :");
  Serial.println("Servo PWM Address :");
  Serial.println(pwmAddress);
  Serial.println("Servo Name :");
  Serial.println(servoName);
  Serial.println("Servo Enabled? :");
  Serial.println(bEnableServo);
  Serial.println("Servo Previous Position :");
  Serial.println(prevPos);
  Serial.println("Servo Current Position :");
  Serial.println(currPos);  
  Serial.println("Servo Min :");
  Serial.println(servoMin);  
  Serial.println("Servo Max :");
  Serial.println(servoMax);  
  Serial.println("Servo Delay :");
  Serial.println(servoDelay);  
  Serial.println("Servo Delay Count :");
  Serial.println(delayCount);  
  Serial.println("Servo Delay Min :");
  Serial.println(delMin);  
  Serial.println("Servo Delay Max :");
  Serial.println(delMax);  
  Serial.println("Servo Return Position :");
  Serial.println(returnPos);  
  Serial.println("Servo Destination Position :");
  Serial.println(destPos);  
 
  if (bEnableServo) {
    Serial.println("Servo is Enabled!");
    if (delayCount >= servoDelay) {
      Serial.println("delayCount >= servoDelay!");
      if (prevPos <= currPos) {
        prevPos = prevPos + DelayStep;
        if (prevPos > currPos) {
          prevPos = currPos;
        }
      }

      if (prevPos > currPos) {
        prevPos = prevPos - DelayStep;
        if (prevPos < currPos) {
          prevPos = currPos;
        }
      }
      //check to see which PWM board the device is on.
      if (pwmAddress == pwmMainAddr) {
        pwmMain.setPWM(servoName, 0, prevPos);
      }
      else {
        pwmBase.setPWM(servoName, 0, prevPos);
      }

      delayCount = 0;
      if (prevPos == currPos) {
        bEnableServo = false;
      }

      /*
      //some servos have an extra function, like LEDS etc.
      switch (servoName) {
        case servoAperature:
          pwmMain.setPWM(aperr, 0, map(aperPrevPos, aperMin, aperMax, 0, 4095)); // brighten the aperature LED as the aperature opens
          break;
        case servoHolo:
          if (bEnableServo) {
            // a little flicker before setting to a value relative to the holoprojector position
            pwmMain.setPWM(holob, 0, 4095);
            pwmMain.setPWM(holob, 0, holoPos * 17);
          }
          else {
            pwmMain.setPWM(holob, 0, 400);
          }
          break;
        case servoEar2:
          pwmMain.setPWM(servoEar1, 0, map(earPrevPos, earMin, earMax, earMax, earMin));
          break;
      }
      */
    }
    delayCount++;
  }
  else {
    if (destPos != 0 && currPos != destPos) { //move to specific position
      servoDelay = 1;
      prevPos = currPos;
      currPos = destPos;
      bEnableServo = true;
    }


    if (returnPos != 0 && bEnableServo == false && currPos != returnPos) { //after moving, return
      servoDelay = DelayMax;
      currPos = returnPos;
      bEnableServo = true;
    }
  }
}

Can someone help me figure out why its not going the full range? What I want is for whatever start position, to be able to go +90 and -90 degrees from the original start position.

Thanks for helping!

Quick answer: servos don’t do that. If you start towards the end of the range, you can’t go 90 degrees further, but you might be able to go 120 degrees the other way.

Or is that something you understand or I have misunderstood about you problem?

a7

Are you talking about how every servo behaves or just one or two? If so which ones?

The only way you get a servo to move + and - is to first initialise it in a central position which I'm guessing are the xxxxxHome values. How did you calculate/discover all the various min and max values for the servos? When you say "max out the Servo Max" what actual value does that mean? What exact type of servo(s) do you have?

If you test them with a basic IDE example like Sweep or Knob do they move over the full range?

Steve

I understand so maybe I'm not stating my problem well enough.

Lets say I have a servo with a horn and a pointer. The pointer is pointing down at 6 o'clock.

If I set the variables like this:
ServoMin = 0
ServoMax = 1400
ServoHome = 300

When the program starts, the Servo moves from 6 o'clock position to 3 o'clock positon. Then if I run the MoveServo function, it will move to the 6 o'clock position. Fine.

Problem is if I set the ServoMax to 5000 it still only moves from 6 o'clock to 3 o'clock.

If I change the ServoHome to say then the servo will start out pointing at 12 o'clock.

Does this make sense?

Steve,

Yes, if I test them out with Knob or Sweep they move over the full range. The code has a ServoHome value that it uses to center between 0 and ServoHome.

For the min and max I'm manually updating those variables and then uploading the code to test.

I'm testing this with a Longrunner LKY62 Servo. In the end I'll want to test it with MG90S servos but wanted to understand how to test the limits first.

Try looking at the tutorial for the Adafruit library you're using. Also the servo example program installed with that library. 0 and 5000 are illegal values when you are trying to use servos with the PCA9685. The maximum practical range of values is roughly 150 min, 600 max.

Steve