Robotic arm help

Hi All.

I’m currently in the process Of building a robot arm with a grapple on the end. I found Robin2’s code on here that I will try to use. The servos work well as is but I would like to have the ability to individually tune each servos start and stop angle. I also plan on using a continuous rotation servo for the main turret but that’s not a must have. There are 6 servos being controlled by 3 thumbsticks. Robin2’s sketch is perfect for this project because it holds the servos last position after the stick returns to center.

So my question is, Can I get some help with altering this sketch so that I can adjust each servos angles.

The #define lines are from the original sketch, I’m not sure if they are needed but I left them in there because I think I could use them as part of the alterations. I struggle with coding so please bare with me.

#include <Servo.h>

#define servo1 0
#define servo2 1
#define servo3 2
#define servo4 3
#define servo5 4
#define servo6 5

Servo servo[6];

byte angle[6] = {90, 90, 90, 90, 90, 90};

byte potPin[6] = {A0, A1, A2, A3, A4, A5};
byte servoPin[6] = {12, 11, 10, 9, 6, 5};

void setup() {

  Serial.begin(9600);
  Serial.println("Starting DiggerCode.ino");


  for (byte n = 0; n < 6; n++) {
    servo[n].attach(servoPin[n]);
  }
}

void loop() {
  readPotentiometers();
  moveServos();
  delay(10);
}

void readPotentiometers() {
  int potVal;
  for (byte n = 0; n < 6; n++) {
    potVal = analogRead(potPin[n]);
   
    if (potVal < 450) {
      angle[n] += 1;
      if (angle[n] > 170) {
        angle[n] = 170;
  }}
   
    if (potVal > 570) {
      angle[n] -= 1;
      if (angle[n] < 10) {
        angle[n] = 10;
  }}}}

void moveServos() {
  for (byte n = 0; n < 6; n++) {
    servo[n].write(angle[n]);
  }}

Just a couple more picturesIMG_0815.JPGIMG_0816.JPGIMG_0820.JPG

You can add minAngle[6] and maxAngle[6] arrays in addition to the angle[6] array, holding the angle limits, and compare against these values when incrementing/decrementing the angles.

You probably can "deconstruct" the code looping back into the individual servo instructions. If you have similar code for one servo, you usually can duplicate the code for each of the servos.

gyrojeremy:
so that I can adjust each servos angles.

What, exactly, do you mean by that ?

…R

Robin2:
What, exactly, do you mean by that ?

…R

Say I wanted servo1 to move between 40° and 150° and servo2 to move between 10° and 180°, servo3 between 20° and 180° and so on. Basically to prevent the servos from trying to drive past their stops on the arm. When I run the sketch the way it is, the servos will drive all the way to the servo limits (about 0° to 190° controlled by joystick inputs). If I try to adjust the limits in the sketch,

      if (angle[n] > 170) {
        angle[n] = 170;
 
      if (angle[n] < 10) {
        angle[n] = 10;

the servos will go haywire and will move to 190° (without joystick input) and stay there no matter what joystick inputs I make.

Does that make sense or have I just created more confusion?

DrDiettrich: You can add minAngle[6] and maxAngle[6] arrays in addition to the angle[6] array, holding the angle limits, and compare against these values when incrementing/decrementing the angles.

Could you provide an example, I feel dyslexic when it comes to C++

You can set the servo limits when you attach the servos.

  myServo[0].writeMicroseconds(START_PULSE_X);
  myServo[1].writeMicroseconds(START_PULSE_Y);
  myServo[0].attach(SERVO_X_PIN, MIN_PULSE, MAX_PULSE);
  myServo[1].attach(SERVO_Y_PIN, MIN_PULSE, MAX_PULSE);

As I recently learned on the forum, it's a good idea to write to the servo before attaching the servo.

Here's how I defined my man and min values.

const long MIN_PULSE = 900;                      // User changeable.
const long MAX_PULSE = 2100;                     // User changeable.

I'm not sure I really need to use longs but there have been times when calculations have required long values in order to prevent an overflow error.

Contrary to what is often said, there's not a memory penalty for using larger variable types when setting a constant's value. The memory penalty only applies to variables. (I compared saving constants as various types and found the variable type didn't affect the amount of program memory or the amount of flash used (assuming I was reading the numbers correctly).)

Depending on how you're using the arm, you might want to use the joystick to control the servos' speed rather than the servo's position. Yesterday I posted some code to control servo with speed rather than position. You might want to consider using it for your project.

IMO, using "write" to control servos is a mistake. You'll have much better control of the servos if you use "writeMicroseconds". The units of "write" very rarely actually equal angles anyway so you might as well control the servos the right way instead of pretending to use angles as arguments.

You can edit your earlier posts and insert the images you uploaded inline with your post. Here's a link to the instructions how to do this. I often insert others' images into my reply but I my replies are too far down in the thread for the images to be noticed. (I haven't seen them myself. I don't like to have to download an image to view it.)

Edit: Thanks for editing your first two posts in this thread. It's much more fun to see the pictures inline with the post than to have to download them to view. The pictures look great.

Hi Duane,

I've been trying your sketch out and it runs the servos very nicely. I like the ability to control the speed without delay. I managed to add a third servo and second joystick so shouldn't be difficult to add in the other 3 servos and joystick. Is there a way to set the limits on an individual servo using these

const long MIN_PULSE = 850; 
const long MAX_PULSE = 2100;

I used a single set of servo limits but it wouldn’t be hard to make an array of limits.

If you converted the code I posted to run on six servos, I’d think you could handle making an array of servo limits.

Let us know if you have trouble adapting the code.

Edit: Here’s a bit to get you started. The following uses the same limit values on all but the last two servos. You can of course modify these values.

const long MIN_PULSE[] = {850, 850, 850, 850, 1000, 1000};  
const long MAX_PULSE[] = {2100, 2100, 2100, 2100, 2000, 2000};

Here’s how you attach the servos.

  myServo[0].attach(SERVO_X_PIN, MIN_PULSE[0], MAX_PULSE[0]);
  myServo[1].attach(SERVO_Y_PIN, MIN_PULSE[1], MAX_PULSE[1]);

If you made an array to hold the pin numbers, you could attach the servos with a “for” loop.

  for(int i = 0; i < 6; i++)
  {
    myServo[i].attach(SERVO_PIN[i], MIN_PULSE[i], MAX_PULSE[i]);
  }

There are other areas where you’ll need to modify the code. Let know if you need help with these other changes.

Edit again: I’m not sure where these last couple of posts belong myself. I agree, it seems like it’s best to post them here. (We had posted these replies in the thread I linked to earlier.)

Yeah sorry about that, I got mixed up on what thread I was in. I will endeavor to mod the sketch to the best of my abilities. No doubt I'll need your help after I've hacked it to pieces. Thanks for your input, I really appreciate it.

Ok I changed it to accommodate 6 servos and added the arrays. It compiles with a few errors though?

/* JoystickServoSpeed151116a
 *
 *  by Duane Degn
 *  December 17, 2015
 *
 *  A ring buffers are used to average the
 *  ADC readings from two poteniometers.
 *  This average is used to control
 *  speed of two hobby servos.
 *
 */

#include <Servo.h>

// User changeable.
#define SERVO_X_PIN 12                           // User changeable.
#define SERVO_Y_PIN 11                           // User changeable.
#define SERVO_Z_PIN 10
#define SERVO_A_PIN 9
#define SERVO_B_PIN 6
#define SERVO_C_PIN 5

// "JOYSTICK_X_PIN" and "JOYSTICK_Y_PIN" need to be assigned to analog pins.
#define JOYSTICK_X_PIN A0                       // User changeable.
#define JOYSTICK_Y_PIN A1                       // User changeable.
#define JOYSTICK_Z_PIN A2
#define JOYSTICK_A_PIN A3
#define JOYSTICK_B_PIN A4
#define JOYSTICK_C_PIN A5

const long MIN_PULSE[] = {850, 850, 850, 850, 1000, 1000};  
const long MAX_PULSE[] = {2100, 2100, 2100, 2100, 2000, 2000};                      // User changeable.
const long MIN_POT = 0;                          // User changeable.
const long MAX_POT = 1023;                       // User changeable.
const long POWER_OF_TWO_TO_AVERAGE = 4;          // User changeable.
// Changing "POWER_OF_TWO_TO_AVERAGE" changes several other constants.
// The constants "BUFFER_SIZE" and "BUFFER_LIMIT" are calculated based on "POWER_OF_TWO_TO_AVERAGE".


const long SERVO_PULSE_RANGE = MAX_PULSE - MIN_PULSE; // This needs to be a long for the equations to work correctly.
const long START_PULSE_X = MIN_PULSE + (SERVO_PULSE_RANGE / 2);  // User changeable.
const long START_PULSE_Y = MIN_PULSE + (SERVO_PULSE_RANGE / 2);  // User changeable.
const long START_PULSE_Z = MIN_PULSE + (SERVO_PULSE_RANGE / 2);
const long START_PULSE_A = MIN_PULSE + (SERVO_PULSE_RANGE / 2);
const long START_PULSE_B = MIN_PULSE + (SERVO_PULSE_RANGE / 2);
const long START_PULSE_C = MIN_PULSE + (SERVO_PULSE_RANGE / 2);

const long POT_RANGE = MAX_POT - MIN_POT;

const int BUFFER_SIZE = 1 << POWER_OF_TWO_TO_AVERAGE; // Do not change.
const int BUFFER_LIMIT = BUFFER_SIZE - 1;             // Do not change.

// Time constants and variables should be unsigned longs.
const unsigned long ANALOG_READ_PERIOD = 5000;  // read pots at 200Hz "ANALOG_READ_PERIOD" must be <= "DEBUG_PERIOD"
const unsigned long DEBUG_PERIOD = 100000;  // update serial at 4Hz "DEBUG_PERIOD" must be >= "SERVO_PERIOD"
const unsigned long SERVO_PERIOD = 20000;  // update servo at 50Hz

const long LOW_CENTER_THRESHOLD = 477;     // User changeable.
const long HIGH_CENTER_THRESHOLD = 515;    // User changeable.
const long POT_TO_SPEED_CONSTANT = 25;     // User changeable. Larger values for slower speeds.

long averagingBufferX[BUFFER_SIZE];
long averagingBufferY[BUFFER_SIZE];
long averagingBufferZ[BUFFER_SIZE];
long averagingBufferA[BUFFER_SIZE];
long averagingBufferB[BUFFER_SIZE];
long averagingBufferC[BUFFER_SIZE];
int bufferIndex = 0;
long servoPosition[] = {START_PULSE_X, START_PULSE_Y, START_PULSE_Z, START_PULSE_A, START_PULSE_B, START_PULSE_C};

long bufferTotalX = 0; //START_PULSE_X * BUFFER_SIZE;
long bufferTotalY = 0; //START_PULSE_Y * BUFFER_SIZE;
long bufferTotalZ = 0;
long bufferTotalA = 0;
long bufferTotalB = 0;
long bufferTotalC = 0;

unsigned long lastDebug;
unsigned long lastServo;
unsigned long lastAnalogRead;

Servo myServo[6];
//Servo servoY;

void setup()
{
  Serial.begin(115200);
  myServo[0].writeMicroseconds(START_PULSE_X);
  myServo[1].writeMicroseconds(START_PULSE_Y);
  myServo[2].writeMicroseconds(START_PULSE_Z);
  myServo[3].writeMicroseconds(START_PULSE_A);
  myServo[4].writeMicroseconds(START_PULSE_B);
  myServo[5].writeMicroseconds(START_PULSE_C);
  myServo[0].attach(SERVO_X_PIN, MIN_PULSE[0], MAX_PULSE[0]);
  myServo[1].attach(SERVO_Y_PIN, MIN_PULSE[1], MAX_PULSE[1]);
  myServo[2].attach(SERVO_Z_PIN, MIN_PULSE[2], MAX_PULSE[2]);
  myServo[3].attach(SERVO_A_PIN, MIN_PULSE[3], MAX_PULSE[3]);
  myServo[4].attach(SERVO_B_PIN, MIN_PULSE[4], MAX_PULSE[4]);
  myServo[5].attach(SERVO_C_PIN, MIN_PULSE[5], MAX_PULSE[5]);

  for (byte i; i < BUFFER_SIZE; i++) // Fill buffer with start position.
  {
    averagingBufferX[i] = (MAX_POT - MIN_POT) / 2;
    averagingBufferY[i] = (MAX_POT - MIN_POT) / 2;
    averagingBufferZ[i] = (MAX_POT - MIN_POT) / 2;
    averagingBufferA[i] = (MAX_POT - MIN_POT) / 2;
    averagingBufferB[i] = (MAX_POT - MIN_POT) / 2;
    averagingBufferC[i] = (MAX_POT - MIN_POT) / 2;
    bufferTotalX += averagingBufferX[i];
    bufferTotalY += averagingBufferY[i];
    bufferTotalZ += averagingBufferZ[i];
    bufferTotalA += averagingBufferA[i];
    bufferTotalB += averagingBufferB[i];
    bufferTotalC += averagingBufferC[i];
  }
 
  lastDebug = micros();
  lastServo = lastDebug;
  lastAnalogRead = lastDebug;
}

void loop()
{
  checkAnalogReadTime();
}

void checkAnalogReadTime()
{
  if (micros() - lastAnalogRead > ANALOG_READ_PERIOD)
  {
    lastAnalogRead += ANALOG_READ_PERIOD;
    //long joystickInputX = analogRead(JOYSTICK_X_PIN);
    long joystickInputX = MAX_POT - analogRead(JOYSTICK_X_PIN); // Use this line to reverse direction of X.
    long joystickInputY = analogRead(JOYSTICK_Y_PIN);
    //long joystickInputY = MAX_POT - analogRead(JOYSTICK_Y_PIN); // Use this line to reverse direction of Y.
    long joystickInputZ = analogRead(JOYSTICK_Z_PIN);
    long joystickInputA = analogRead(JOYSTICK_A_PIN);
    long joystickInputB = analogRead(JOYSTICK_B_PIN);
    long joystickInputC = analogRead(JOYSTICK_C_PIN);
   
    bufferIndex++;
    bufferIndex &= BUFFER_LIMIT;
   
    bufferTotalX -= averagingBufferX[bufferIndex]; // out with the old
    bufferTotalY -= averagingBufferY[bufferIndex];
    bufferTotalZ -= averagingBufferZ[bufferIndex];
    bufferTotalA -= averagingBufferA[bufferIndex];
    bufferTotalB -= averagingBufferB[bufferIndex];
    bufferTotalC -= averagingBufferC[bufferIndex];
    averagingBufferX[bufferIndex] = joystickInputX;
    averagingBufferY[bufferIndex] = joystickInputY;
    averagingBufferZ[bufferIndex] = joystickInputZ;
    averagingBufferA[bufferIndex] = joystickInputA;
    averagingBufferB[bufferIndex] = joystickInputB;
    averagingBufferC[bufferIndex] = joystickInputC;
    bufferTotalX += averagingBufferX[bufferIndex]; // in with the new
    bufferTotalY += averagingBufferY[bufferIndex];
    bufferTotalZ += averagingBufferZ[bufferIndex];
    bufferTotalA += averagingBufferA[bufferIndex]; // in with the new
    bufferTotalB += averagingBufferB[bufferIndex];
    bufferTotalC += averagingBufferC[bufferIndex];
   
    checkServoTime();
  }
}


void checkServoTime()
// Called from "checkAnalogReadTime" function.
{
  if (micros() - lastServo > SERVO_PERIOD)
  {
    lastServo += SERVO_PERIOD;
    controlServo();
  }
}

Had to post code in 2 parts

void controlServo()
// Called from "checkServoTime" function.
{

  long average[6];
  long servoSpeed[6];
 
  average[0] = bufferTotalX >> POWER_OF_TWO_TO_AVERAGE; // it might be a good idea to make averageX global so it can be used elsewhere in program
  average[1] = bufferTotalY >> POWER_OF_TWO_TO_AVERAGE;
  average[2] = bufferTotalZ >> POWER_OF_TWO_TO_AVERAGE;
  average[3] = bufferTotalA >> POWER_OF_TWO_TO_AVERAGE; // it might be a good idea to make averageX global so it can be used elsewhere in program
  average[4] = bufferTotalB >> POWER_OF_TWO_TO_AVERAGE;
  average[5] = bufferTotalC >> POWER_OF_TWO_TO_AVERAGE;

  for (int i = 0; i < 6; i++)
  {
    if (average[i] < LOW_CENTER_THRESHOLD)
    {
      servoSpeed[i] = (average[i] - LOW_CENTER_THRESHOLD) / POT_TO_SPEED_CONSTANT;
      // negative speed proportional to distance from center pot
    }
    else if  (average[i] > HIGH_CENTER_THRESHOLD)
    {
      servoSpeed[i] = (average[i] - HIGH_CENTER_THRESHOLD) / POT_TO_SPEED_CONSTANT;
                    // positive speed
    }
    else // pot in dead zone
    {
      servoSpeed[i] = 0;
    }
    servoPosition[i] += servoSpeed[i]; // but you should make sure "servoPosition" stays within endpoints
    if (servoPosition[i] > MAX_PULSE)
    {
      servoPosition[i] = MAX_PULSE;
    }
    else if (servoPosition[i] < MIN_PULSE)
    {
      servoPosition[i] = MIN_PULSE;
    }
    myServo[i].writeMicroseconds(servoPosition[i]);
  }

  checkDebugTime(average[0], average[1], average[2], average[3], average[4], average[4], servoPosition[0], servoPosition[1], servoPosition[2], servoPosition[3], servoPosition[4], servoPosition[5], servoSpeed[0], servoSpeed[1], servoSpeed[2], servoSpeed[3], servoSpeed[4], servoSpeed[5]);
}

void checkDebugTime(long averageX, long averageY, long averageZ, long averageA, long averageB, long averageC, long servoOutputX, long servoOutputY, long servoOutputZ, long servoOutputA, long servoOutputB, long servoOutputC, long speedX, long speedY, long speedZ, long speedA, long speedB, long speedC)
// Called from "checkServoTime" function.
// Serial output slows down code execution.
// This method checks to see if it's time to
// display data.
// It would probably be a good idea to remove this section of code
// once the program is working as hoped and when serial
// output is now longer desired.
{
  if (micros() - lastDebug > DEBUG_PERIOD)
  {
    lastDebug += DEBUG_PERIOD;

    Serial.print(F("average = "));
    Serial.print(averageX, DEC);
    Serial.print(F(", "));
    Serial.print(averageY, DEC);
    Serial.print(F(", Servo = "));
    Serial.print(servoOutputX, DEC);
    Serial.print(F(", "));
    Serial.print(servoOutputY, DEC);
    Serial.print(F(", Spead = "));
    Serial.print(speedX, DEC);
    Serial.print(F(", "));
    Serial.println(speedY, DEC);
  }
}

I don't have time to look at the code right now, but I will when I have some time.

DuaneDegn: I don't have time to look at the code right now, but I will when I have some time.

Thank you. I appreciate it.

I haven’t tested this code on all the channels but the first two channels appear to work as expected.

/* Joystick6ServoSpeed151118a
 *
 *  by Duane Degn
 *  December 18, 2015
 *
 *  A ring buffers are used to average the
 *  ADC readings from six potentiometers.
 *  This average is used to control
 *  speed of six hobby servos.
 *
 */

#include <Servo.h>

// User changeable.
#define SERVO_X_PIN 12                           // User changeable.
#define SERVO_Y_PIN 11                           // User changeable.
#define SERVO_Z_PIN 10
#define SERVO_A_PIN 9
#define SERVO_B_PIN 6
#define SERVO_C_PIN 5

// "JOYSTICK_X_PIN" and "JOYSTICK_Y_PIN" need to be assigned to analog pins.
/*#define JOYSTICK_X_PIN A0                       // User changeable.
#define JOYSTICK_Y_PIN A1                       // User changeable.
#define JOYSTICK_Z_PIN A2
#define JOYSTICK_A_PIN A3
#define JOYSTICK_B_PIN A4
#define JOYSTICK_C_PIN A5*/

const byte JOYSTICK_X_PIN = A0;                       // User changeable.
const byte JOYSTICK_Y_PIN = A1;                       // User changeable.
const byte JOYSTICK_Z_PIN = A2;
const byte JOYSTICK_A_PIN = A3;
const byte JOYSTICK_B_PIN = A4;
const byte JOYSTICK_C_PIN = A5;

const byte SERVOS_IN_USE = 6;
const boolean SERVO_REVERSE_FLAG[] = {1, 0, 0, 0, 0, 0}; // set to one if servo should be reversed

const long MIN_PULSE[] = {850, 850, 850, 850, 1000, 1000};
const long MAX_PULSE[] = {2100, 2100, 2100, 2100, 2000, 2000};                      // User changeable.
const long MIN_POT = 0;                          // User changeable.
const long MAX_POT = 1023;                       // User changeable.
const long POWER_OF_TWO_TO_AVERAGE = 4;          // User changeable.
// Changing "POWER_OF_TWO_TO_AVERAGE" changes several other constants.
// The constants "BUFFER_SIZE" and "BUFFER_LIMIT" are calculated based on "POWER_OF_TWO_TO_AVERAGE".


//long SERVO_PULSE_RANGE[SERVOS_IN_USE]; // This isn't really a consstant but we only set the value once.



const byte SERVO_PIN[] = {SERVO_X_PIN, SERVO_Y_PIN, SERVO_Z_PIN, SERVO_A_PIN, SERVO_B_PIN, SERVO_C_PIN};
const byte JOYSTICK_PIN[] = {JOYSTICK_X_PIN, JOYSTICK_Y_PIN, JOYSTICK_Z_PIN, JOYSTICK_A_PIN, JOYSTICK_B_PIN, JOYSTICK_C_PIN};

//const long POT_RANGE = MAX_POT - MIN_POT;

const int BUFFER_SIZE = 1 << POWER_OF_TWO_TO_AVERAGE; // Do not change.
const int BUFFER_LIMIT = BUFFER_SIZE - 1;             // Do not change.

// Time constants and variables should be unsigned longs.
const unsigned long ANALOG_READ_PERIOD = 5000;  // read pots at 200Hz "ANALOG_READ_PERIOD" must be <= "DEBUG_PERIOD"
const unsigned long DEBUG_PERIOD = 100000;  // update serial at 4Hz "DEBUG_PERIOD" must be >= "SERVO_PERIOD"
const unsigned long SERVO_PERIOD = 20000;  // update servo at 50Hz

const long LOW_CENTER_THRESHOLD = 477;     // User changeable.
const long HIGH_CENTER_THRESHOLD = 515;    // User changeable.
const long POT_TO_SPEED_CONSTANT = 8;     // User changeable. Larger values for slower speeds.

long averagingBuffer[SERVOS_IN_USE][BUFFER_SIZE];
int bufferIndex = 0;
long servoPosition[SERVOS_IN_USE];

long bufferTotal[SERVOS_IN_USE];

unsigned long lastDebug;
unsigned long lastServo;
unsigned long lastAnalogRead;

Servo myServo[SERVOS_IN_USE];
//Servo servoY;

void setup()
{
  Serial.begin(115200);
  for (int i = 0; i < SERVOS_IN_USE; i++)
  {
    servoPosition[i] = MIN_PULSE[i] + ((MAX_PULSE[i] - MIN_PULSE[i]) / 2);
    myServo[i].writeMicroseconds(servoPosition[i]); // start servo in center position
    myServo[i].attach(SERVO_PIN[i], MIN_PULSE[i], MAX_PULSE[i]);
    bufferTotal[i] = 0;
    for (int j; j < BUFFER_SIZE; j++) // Fill buffer with start position.
    {
      averagingBuffer[i][j] = (MAX_POT - MIN_POT) / 2;
      bufferTotal[i] += averagingBuffer[i][j];
    }
  }

  lastDebug = micros();
  lastServo = lastDebug;
  lastAnalogRead = lastDebug;
}

void loop()
{
  checkAnalogReadTime();
}

void checkAnalogReadTime()
{
  if (micros() - lastAnalogRead > ANALOG_READ_PERIOD)
  {
    lastAnalogRead += ANALOG_READ_PERIOD;
    long joystickInput;

    bufferIndex++;
    bufferIndex &= BUFFER_LIMIT;

    for (int i = 0; i < SERVOS_IN_USE; i++)
    {
      joystickInput = analogRead(JOYSTICK_PIN[i]);
      if (SERVO_REVERSE_FLAG[i])
      {
        joystickInput = MAX_POT - joystickInput;
      }
      bufferTotal[i] -= averagingBuffer[i][bufferIndex]; // out with the old
      averagingBuffer[i][bufferIndex] = joystickInput;
      bufferTotal[i] += averagingBuffer[i][bufferIndex]; // in with the new
    }
    checkServoTime();
  }
}

void checkServoTime()
// Called from "checkAnalogReadTime" function.
{
  if (micros() - lastServo > SERVO_PERIOD)
  {
    lastServo += SERVO_PERIOD;
    controlServo();
  }
}

void controlServo()
// Called from "checkServoTime" function.
{
  long average;
  long servoSpeed;
  boolean debugFlag = checkDebugTime();
  
  for (int i = 0; i < SERVOS_IN_USE; i++)
  {
    average = bufferTotal[i] >> POWER_OF_TWO_TO_AVERAGE;
    if (average < LOW_CENTER_THRESHOLD)
    {
      servoSpeed = (average - LOW_CENTER_THRESHOLD) / POT_TO_SPEED_CONSTANT;
      // negative speed proportional to distance from center pot
    }
    else if  (average > HIGH_CENTER_THRESHOLD)
    {
      servoSpeed = (average - HIGH_CENTER_THRESHOLD) / POT_TO_SPEED_CONSTANT;
      // positive speed
    }
    else // pot in dead zone
    {
      servoSpeed = 0;
    }
    servoPosition[i] += servoSpeed;
    if (servoPosition[i] > MAX_PULSE[i])
    {
      servoPosition[i] = MAX_PULSE[i];
    }
    else if (servoPosition[i] < MIN_PULSE[i])
    {
      servoPosition[i] = MIN_PULSE[i];
    }
    myServo[i].writeMicroseconds(servoPosition[i]);
    if (debugFlag)
    {
      //debugServo(i, average, servoPosition[i], servoSpeed);
    }
  }
}

boolean checkDebugTime()
// Called from "controlServo" function.
// This method checks to see if it's time to
// display data.
{
  boolean debugFlag = 0;
  if (micros() - lastDebug > DEBUG_PERIOD)
  {
    lastDebug += DEBUG_PERIOD;
    debugFlag = 1;
  }
  return debugFlag;
}

void debugServo(int servoIndex, long average, long servoOutput, long servoSpeed)
// Called from "controlServo" function.
// Serial output slows down code execution.
// It would probably be a good idea to remove this section of code
// once the program is working as hoped and when serial
// output is now longer desired.
{
    Serial.print(F("servo # "));
    Serial.print(servoIndex, DEC);
    Serial.print(F(": average = "));
    Serial.print(average, DEC);
    Serial.print(F(", position = "));
    Serial.print(servoOutput, DEC);
    Serial.print(F(", spead = "));
    Serial.println(servoSpeed, DEC);
}

I commented out the line which calls the debug function.

    if (debugFlag)
    {
      //debugServo(i, average, servoPosition[i], servoSpeed);
    }

The debug statements introduces a stutter in the servos’ movement.

You can add the debug call back if you need to figure out what parameter you should use for the servos’ max and min limits.

I added several arrays to the program. The “REVERSE_FLAG” array is used to indicate if the servo’s direction should be reversed.

const boolean SERVO_REVERSE_FLAG[] = {1, 0, 0, 0, 0, 0}; // set to one if servo should be reversed

As you can see, only the first servo’s direction is reversed.

One of the advantages of using arrays is now the code fits in one post.

Thank you so much Duane, That works great. I had some hardware issues to start with that through me off.

1st, Turns out there was 3 broken wires inside the joystick wiring loom about 100mm inside the outer sheath.

2nd, My Arduino uno is toast, lucky I had a mega to trouble shoot that one.

3rd, Nether the uno or the mega will run the code on my external p/s. It has to be connected to the laptop to work? Not sure why that is, has anyone come across that before??

gyrojeremy: 3rd, Nether the uno or the mega will run the code on my external p/s.

What does "my external p/s" mean? Do you mean an external power supply?

If so, you need to make sure you have a ground connection between the external power and the Arduino.

Yes, p/s (power supply). The p/s ground is connected directly to the arduino ground and the vcc to the servos.
It seems as it not just the power supply. I just hooked it up through the USB and it went nuts.(servos locked at full rotation and madly jittering).
Tried disconnecting and reconnecting (p/s and usb) 3 or 4 times before it came right and worked perfect.
I’ve tried uploading the sketch at lest a dozen times.

The photo of the p/s shows connections (the clear insulated wires) on ground for both servos and adruino, 5v for servos and 12v for arduino. At the other end I have 12v on the center pin of the barrel jack, ground around the outside. 5v to servo + and ground to arduino ground pins.IMG_0825.JPGIMG_0827.JPGIMG_0828.JPG