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!