Thumbstick (Joystick) and 2 Servos

Hi all, I’m working on a very popular project: controlling 2 servos with a thumbstick.
There are tons of sketches and guides all over the forum and the internet, but I have 2 very (for me) big problems.

Here’s the code I’m using, (author is Biomech75)

#include <Servo.h>

const int servo1 = 3;       // first servo
const int servo2 = 10;       // second servo
const int joyH = 3;        // L/R Parallax Thumbstick
const int joyV = 4;        // U/D Parallax Thumbstick

int servoVal;           // variable to read the value from the analog pin

Servo myservo1;  // create servo object to control a servo
Servo myservo2;  // create servo object to control a servo



void setup() {

  // Servo  
  myservo1.attach(servo1);  // attaches the servo
  myservo2.attach(servo2);  // attaches the servo

  // Inizialize Serial
  Serial.begin(9600);
}


void loop(){

    // Display Joystick values using the serial monitor
    outputJoystick();

    // Read the horizontal joystick value  (value between 0 and 1023)
    servoVal = analogRead(joyH);          
    servoVal = map(servoVal, 0, 1023, 0, 180);     // scale it to use it with the servo (result  between 0 and 180)

    myservo2.write(servoVal);                         // sets the servo position according to the scaled value    

    // Read the horizontal joystick value  (value between 0 and 1023)
    servoVal = analogRead(joyV);           
    servoVal = map(servoVal, 0, 1023, 70, 180);     // scale it to use it with the servo (result between 70 and 180)

    myservo1.write(servoVal);                           // sets the servo position according to the scaled value

    delay(15);                                       // waits for the servo to get there

}


/**
* Display joystick values
*/
void outputJoystick(){

    Serial.print(analogRead(joyH));
    Serial.print ("---"); 
    Serial.print(analogRead(joyV));
    Serial.println ("----------------");
}

It does actually work but:

  • The reading are very very imprecise and since they change by a little little bit on every reading (almost) the servos shake because they try to mainain the position readed by the Arduino, they are never (almost) completely stopped…sort of a continuous shake.
  • How can I make an “non-active” area? I would like to create a range in which (next to the center) the servos doesn’t move at all, in order to prevent accidental hit.
  • Third thing is: is it possible to define the center each time the arduino boot? Sort of a calibration of the center

Thank you very much

UPDATE:

I’m now running another code and the reading are waaaay better and it has a “dead center” so it does fit perfectly my needs.
I’m now trying to interface it with the servos, here’s the code:

#include <VarSpeedServoSam.h> 

VarSpeedServoSam HorizontalServo;
VarSpeedServoSam VerticalServo;

int ServoHorizontalPin = 2;
int ServoVerticalPin = 3;
int range = 1000;
int responseDelay = 5;
int threshold = range/4;
int center = range/2;


void setup() {
  Serial.begin(9600);
  HorizontalServo.attach(ServoHorizontalPin);
  VerticalServo.attach(ServoVerticalPin);
}

void loop() {

  int x1 = readAxis(A0);
  int y1 = readAxis(A1);
  
  String msg = "Orizzontale: ";
  msg += x1;
  msg += "   Verticale: ";
  msg += y1;

  Serial.println(msg);
HorizontalServo.write( WHICH VALUE? , 5);
  VerticalServo.write( WHICH VALUE? , 5);
}

int readAxis(int thisAxis) { 
  int reading = analogRead(thisAxis);
  reading = map(reading, 0, 1023, 70, range);
  int distance = reading - center;
  if (abs(distance) < threshold) {
    distance = 0;
  }
  return distance;
}

Any help is really appreciated

Servo/pot test code with a dead band.

//zoomkat dual pot/servo test 12-29-12
//view output using the serial monitor

#include <Servo.h> 
Servo myservoS1;
Servo myservoS2;

int potpinS1 = 0;  //analog input pin A0
int potpinS2 = 1;

int newvalS1, oldvalS1;
int newvalS2, oldvalS2;

void setup() 
{
  Serial.begin(9600);  
  myservoS1.attach(2);  
  myservoS2.attach(3);
  Serial.println("testing dual pot servo");  
}

void loop() 
{ 
  newvalS1 = analogRead(potpinS1);           
  newvalS1 = map(newvalS1, 0, 1023, 0, 179); 
  if (newvalS1 < (oldvalS1-2) || newvalS1 > (oldvalS1+2)){  
    myservoS1.write(newvalS1);
    Serial.print("1- ");
    Serial.println(newvalS1);
    oldvalS1=newvalS1;
  }

  newvalS2 = analogRead(potpinS2);
  newvalS2 = map(newvalS2, 0, 1023, 0, 179);
  if (newvalS2 < (oldvalS2-2) || newvalS2 > (oldvalS2+2)){  
    myservoS2.write(newvalS2);
    Serial.print("2- ");    
    Serial.println(newvalS2);
    oldvalS2=newvalS2;
  }
  delay(50); //slow down looping to better read serial monitor 
}

zoomkat:
Servo/pot test code with a dead band.

I'm not sure I'd call that dead band. It looks like you're not moving the servo when there are small changes but I don't think that's what the OP wants. Though it might work as well (or even better) as having a center dead band.

@c0rsa1r, I think you'd be better off using microsecond commands to the servos rather than degrees.

What's the raw center value of the two pots before you scale the values? Do the both pots produce a full range of output from the ADC?

You have a range of 1000 in the code for your servos but have you measured the upper and lower limits of your servos. If so what are they?

One way of making an input such as a pot more stable is to take multiple readings and average the readings. A rolling average might take care of your problems.

Edit: Here's a link to some information about a magnetic encoder project I did awhile back. The first video in the thread shows how there was a lot of fluctuation in the output from the sensor. This fluctuation showed up as unstable readings on the display. I added code to average multiple sensor readings and the second video in the thread shows how much averaging the readings improved the stability of the value displayed. I eventually modified the code again to use two different averaging techniques. When the sensor values didn't change much from one reading to the next, I assumed the knob wasn't being turned and I used lots of readings to compute the average. When the sensor was being turned, I reduced the number of readings averaged to prevent the response from being too sluggish.

Well first of all thank you both. I've uploaded the code provided by zoomkat and it does actually work as intended.

The objective is to have a pointer (a laser) that doesn't shake when "idle" or during movement, and in fact with this code it doesn't.

I loaded the VarSpeedServo lib (instead of the standard Servo lib) and it's moving slow and precise. I don't care about the readings (not really). I'll test it with the laser mounted and tell you if it's ok, but as far as I can see it actually is.

Thank you very very much

zoomkat:
Servo/pot test code with a dead band.

//zoomkat dual pot/servo test 12-29-12

//view output using the serial monitor

#include <Servo.h>
Servo myservoS1;
Servo myservoS2;

int potpinS1 = 0;  //analog input pin A0
int potpinS2 = 1;

int newvalS1, oldvalS1;
int newvalS2, oldvalS2;

void setup()
{
  Serial.begin(9600); 
  myservoS1.attach(2); 
  myservoS2.attach(3);
  Serial.println(“testing dual pot servo”); 
}

void loop()
{
  newvalS1 = analogRead(potpinS1);         
  newvalS1 = map(newvalS1, 0, 1023, 0, 179);
  if (newvalS1 < (oldvalS1-2) || newvalS1 > (oldvalS1+2)){ 
    myservoS1.write(newvalS1);
    Serial.print("1- ");
    Serial.println(newvalS1);
    oldvalS1=newvalS1;
  }

newvalS2 = analogRead(potpinS2);
  newvalS2 = map(newvalS2, 0, 1023, 0, 179);
  if (newvalS2 < (oldvalS2-2) || newvalS2 > (oldvalS2+2)){ 
    myservoS2.write(newvalS2);
    Serial.print("2- ");   
    Serial.println(newvalS2);
    oldvalS2=newvalS2;
  }
  delay(50); //slow down looping to better read serial monitor
}

thank you zoomkat, it is actually working exactly as intended. I just need to tune it a little bit.
It has to be a bit more “precise”, more “sensible”, because I have to reduce the max angle to +25 deg and -25deg, how do I change it? (I mean the sensibility, not the min-max angle)

You might try using microsecond values instead of degree values and change the mapping range similar to below. As thumb sticks them selves may be hard to control with just the thumb, you might consider hot gluing an extension (like a pencil or dowel) to the thumb stick button to make it into an easier to control joystick. The dead band values may need to be increased when using microseconds.

newvalS1 = map(newvalS1, 0, 1023, 1200, 1800);
myservoS1.writeMicroseconds(newvalS1);

I'm using a PSP Thumbstick (I love it, it's flat and it does fit perfectly my project)
Anyway that's what I've done so far

In void setup:
myservoS1.write(90);
myservoS2.write(90);

This way when I power the arduino both the servo are standing still (not moving at all)

In void loop:

newvalS1 = map(newvalS1, 0, 1023, 75, 105);
myservoS1.write(newvalS1);
newvalS2 = map(newvalS2, 0, 1023, 75, 105);
myservoS2.write(newvalS2);

So the min degree is 75 (90-15) and the max angle is 105 (90+15)
Will they become more accurate if I set the degrees in microseconds? How do I do the conversion?

Thank you again zoomkat you are very very helpful

c0rsa1r:
Will they become more accurate if I set the degrees in microseconds? How do I do the conversion?

According to the "Attach" page, the default servo extremes are 544 and 2400. This sure seems odd to me. This places the center at 1472us rather than 1500us.

To convert your angle to pulse you'd multiple by the normal full range of microseconds and divide by the normal full range of angles.

So 15 degrees becomes 15 * 1856 / 180 = 155

So you want to map your microseconds from 1472 - 155 to 1472 + 155 or 1317 - 1627. This sure seems strange to me so I hope zoomkat or someone else confirms this is correct of lets me know what I got wrong.

If the above is correct, then zoomkat's earlier code could be modified to:

newvalS1 = map(newvalS1, 0, 1023, 1317, 1627);
myservoS1.writeMicroseconds(newvalS1);

Does your joystick output the full range from 0 to 1023? I'm using a different joystick and the pot values do not move to their full extremes. One pot gives values from 371 to 632. I think the little thumbsticks I've used did output a full range so I'm betting the above modified code should work.

Below is some servo test code that might be of interest in setting up a servo. This allows the command value to be determined as the servo is incrementally moved to a desired position. The actual command values for the servo mechanical movements can also be determined. Note that in the myservo.attach line the servo microsecond limits are changed from the default limits to ensure the servo can be tested to its mechanical limits.

// zoomkat 3-28-14 serial servo incremental test code
// using serial monitor type a character (s to increase or a 
// to decrease) and enter to change servo position 
// (two hands required, one for letter entry and one for enter key)
// use strings like 90x or 1500x for new servo position 
// for IDE 1.0.5 and later
// Powering a servo from the arduino usually *DOES NOT WORK*.

#include<Servo.h>
String readString;
Servo myservo;
int pos=1500; //~neutral value for continous rotation servo
//int pos=90;

void setup()
{
  myservo.attach(7, 400, 2600); //servo control pin, and range if desired
  Serial.begin(9600);
  Serial.println("serial servo incremental test code");
  Serial.println("type a character (s to increase or a to decrease)");
  Serial.println("and enter to change servo position");
  Serial.println("use strings like 90x or 1500x for new servo position");
  Serial.println();
}

void loop()
{
  while (Serial.available()) {
    char c = Serial.read();  //gets one byte from serial buffer
    readString += c; //makes the string readString
    delay(2);  //slow looping to allow buffer to fill with next character
  }
  if (readString.length() >0) {
    if(readString.indexOf('x') >0) { 
      pos = readString.toInt();
    }

    if(readString =="a"){
      (pos=pos-1); //use larger numbers for larger increments
      if(pos<0) (pos=0); //prevent negative number
    }
    if (readString =="s"){
      (pos=pos+1);
    }

    if(pos >= 400) //determine servo write method
    {
      Serial.println(pos);
      myservo.writeMicroseconds(pos);
    }
    else
    {   
      Serial.println(pos);
      myservo.write(pos); 
    }
  }
  readString=""; //empty for next input
}

I've been experimenting with reading pots from joysticks and controlling servos.

I've sure there are much simpler ways to smooth out the motion of a servo, but this my attempting at using a ring buffer to average 16 readings.

/* JoystickWithAverageing151113b
 *  
 *  by Duane Degn
 *  November 13, 2015 (Modified Dec 18)
 *  
 *  A ring buffers are used to average the
 *  ADC readings from two potentiometers. 
 *  This average is used to control two
 *  hobby servos.
 *
 */

#include <Servo.h>

// User changeable.
#define SERVO_X_PIN 2                           // User changeable.
#define SERVO_Y_PIN 3                           // User changeable.

// "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.

const int MIN_PULSE = 900;                      // User changeable.
const int MAX_PULSE = 2100;                     // User changeable.
const int MIN_POT_X = 0;                      // User changeable.
const int MAX_POT_X = 1023;                      // User changeable.
const int MIN_POT_Y = 0;                      // User changeable.
const int MAX_POT_Y = 1023;                      // User changeable.
const int 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 int START_PULSE_X = MIN_PULSE + (SERVO_PULSE_RANGE) / 2;  // User changeable.
const int START_PULSE_Y = MIN_PULSE + (SERVO_PULSE_RANGE) / 2;  // User changeable.

const int POT_RANGE_X = MAX_POT_X - MIN_POT_X;
const int POT_RANGE_Y = MAX_POT_Y - MIN_POT_Y;

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

int averagingBufferX[BUFFER_SIZE];
int averagingBufferY[BUFFER_SIZE];
int bufferIndex = 0;

long bufferTotalX = 0;
long bufferTotalY = 0;

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

Servo servoX;
Servo servoY;

void setup()
{
  Serial.begin(115200);
  servoX.attach(SERVO_X_PIN, MIN_PULSE, MAX_PULSE);
  servoY.attach(SERVO_Y_PIN, MIN_PULSE, MAX_PULSE);

  for (byte i; i < BUFFER_SIZE; i++) // Fill buffer with start position.
  {
    averagingBufferX[i] = START_PULSE_X;
    averagingBufferY[i] = START_PULSE_Y;
    bufferTotalX += averagingBufferX[i];
    bufferTotalY += averagingBufferY[i];
  }
  
  lastDebug = micros();
  lastServo = lastDebug;
  lastAnalogRead = lastDebug;
}

void loop()
{
  checkAnalogReadTime();
}

void checkAnalogReadTime()
{
  if (micros() - lastAnalogRead > ANALOG_READ_PERIOD)
  {
    lastAnalogRead += ANALOG_READ_PERIOD;
    int joystickInputX = analogRead(JOYSTICK_X_PIN);
    int joystickInputY = analogRead(JOYSTICK_Y_PIN);
  
    bufferIndex++;
    bufferIndex &= BUFFER_LIMIT;
  
    bufferTotalX -= averagingBufferX[bufferIndex]; // out with the old
    bufferTotalY -= averagingBufferY[bufferIndex];
    averagingBufferX[bufferIndex] = joystickInputX;
    averagingBufferY[bufferIndex] = joystickInputY;
    bufferTotalX += averagingBufferX[bufferIndex]; // in with the new
    bufferTotalY += averagingBufferY[bufferIndex];
  
    checkServoTime();
  }
}


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

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

  int averageX = bufferTotalX >> POWER_OF_TWO_TO_AVERAGE; // it might be a good idea to make averageX global so it can be used elsewhere in program
  int averageY = bufferTotalY >> POWER_OF_TWO_TO_AVERAGE;

  int servoOutputX = MIN_PULSE + ((averageX - MIN_POT_X) * SERVO_PULSE_RANGE / POT_RANGE_X);
  int servoOutputY = MIN_PULSE + ((averageY - MIN_POT_Y) * SERVO_PULSE_RANGE / POT_RANGE_Y);

  servoX.writeMicroseconds(servoOutputX);
  servoY.writeMicroseconds(servoOutputY);
  checkDebugTime(averageX, averageY, servoOutputX, servoOutputY);
}

void checkDebugTime(int averageX, int averageY, int servoOutputX, int servoOutputY)
// 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("average = ");
    Serial.print(averageX, DEC);
    Serial.print(", ");
    Serial.print(averageY, DEC);
    Serial.print(", Servo = ");
    Serial.print(servoOutputX, DEC);
    Serial.print(", ");
    Serial.println(servoOutputY, DEC);
  }
}

It doesn't do much good to update the position of most hobby servos more than once every 20ms so this program uses micros() to monitor the time to determine when to read the ADCs, write the servos and output debug statements. There aren't any delays in the code so it could be expanded to perform other tasks without too much trouble.

The constant "POWER_OF_TWO_TO_AVERAGE" is used to set the size of the ring buffer.

I have a version of the code which allows the buffer size to be changed on the fly in case anyone is interested.

Make sure and adjust the constant values at the top of the program to match your pots and servos. The joystick I'm using don't provide full scale output from the ADC.

Edit: I know there are people who think constants should be defined with "#DEFINE" but I advise against doing so with the above code. #DEFINE can produce some very unexpected behaviour when used with constants defined in terms of other constants.

As an example of this strange behaviour, if "BUFFER_SIZE" were defined with "#DEFINE" then "BUFFER_LIMIT" ends up equaling 8 instead of the desired value 15. (I learned once again, computers/microcontrollers do what you tell them to do, not what you want them to do.)

Thank you all, at the moment I'm using the code below based on the code provided by zoomkat, but I noticed that if I set writeMicroseconds instead of write it is far more precise. However, writeMicroseconds does not accept the speed variable provided by varspeedservosam (I'm using a DUE) library.

How can I solve that? (using another lib such as ServoEaser?)

#include <VarSpeedServoSam.h> 
VarSpeedServoSam myservoS1;
VarSpeedServoSam myservoS2;

int potpinS1 = 0;
int potpinS2 = 1;

int newvalS1, oldvalS1;
int newvalS2, oldvalS2;

void setup() 
{
  myservoS1.attach(2);  
  myservoS2.attach(3);
  myservoS1.write(90);  
  myservoS2.write(90);
}

void loop() 
{ analogReadResolution(12);
  newvalS1 = analogRead(potpinS1);           
  newvalS1 = map(newvalS1, 0, 4095, 81, 101); 
  if (newvalS1 < (oldvalS1-1) || newvalS1 > (oldvalS1+1)){  
    myservoS1.write(newvalS1, 5);
    oldvalS1=newvalS1;
  }
  analogReadResolution(12);
  newvalS2 = analogRead(potpinS2);
  newvalS2 = map(newvalS2, 0, 4095, 81, 101);
  if (newvalS2 < (oldvalS2-1) || newvalS2 > (oldvalS2+1)){  
    myservoS2.write(newvalS2, 5);
    oldvalS2=newvalS2;
  }
}

thank you very much

I don't know the answer to your question but I wanted to let you know I modified the code I posted to use pot extremes which are likely to match the joystick you're using. I encourage you to try the program.

Thank you I'll try as soon as possible.

Is there a way in this code to slow down the movements? (going from a reading to another slower and not at full speed)

varspeedservo does that so well but I don't know how to set this with microseconds

I think code has been posted that uses millis to slow the movement speed of servos. You might incorporate this into your code, but it probably add another level of complexity. You might also be able to modify the varspeedservo library files to accommodate us values instead of degree values.

I posted some code which uses a joystick to set the speed a servo is moving rather than setting the servo's position. You might want to give it a try to see which method of control you prefer.

c0rsa1r:
Is there a way in this code to slow down the movements?

Yes, this will be relatively easy to add. I'll probably work on this later today.

The code I linked to above has a speed setting of sorts. The smaller the constant "POT_TO_SPEED_CONSTANT", the faster the servo will move.

Slowing the speed will likely improve how well your servos move but what you really need is an algorithm which accelerates the servo motion up to some max allowed speed and then decelerates as it reaches the target position.

I've done this sort of constant acceleration servo control in other projects but I haven't done this sort of servo control using an Arduino yet.

I'll likely try writing some constant acceleration code in the near future. I'll post a note here when I do.

I'm not sure how fast you want the servos to move but you can easily change the speed by changing the value of "MAX_SPEED". To increase the speed of the servos, increase the value of "MAX_SPEED".

Here's the speed limited joystick control code.

/* JoystickWithAveraging151219a
 *
 *  by Duane Degn
 *  December 19, 2015
 *
 *  A ring buffers are used to average the
 *  ADC readings from two potentiometers.
 *  This average is used to control two
 *  hobby servos.
 *  The speed of the servos is limited
 *  by the constant "MAX_SPEED".
 *
 */

#include <Servo.h>

// User changeable.
#define SERVO_X_PIN 2                           // User changeable.
#define SERVO_Y_PIN 3                           // User changeable.

// "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.

const int MIN_PULSE = 900;                      // User changeable.
const int MAX_PULSE = 2100;                     // User changeable.
const int MIN_POT = 0;                      // User changeable.
const int MAX_POT = 1023;                      // User changeable.

const int 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 byte SERVOS_IN_USE = 2;
const int MAX_SPEED = 16;
const byte SERVO_PIN[] = {SERVO_X_PIN, SERVO_Y_PIN};
const byte JOYSTICK_PIN[] = {JOYSTICK_X_PIN, JOYSTICK_Y_PIN};

const long SERVO_PULSE_RANGE = MAX_PULSE - MIN_PULSE; // This needs to be a long for the equations to work correctly.
const int START_PULSE_X = MIN_PULSE + (SERVO_PULSE_RANGE) / 2;  // User changeable.
const int START_PULSE_Y = MIN_PULSE + (SERVO_PULSE_RANGE) / 2;  // User changeable.
const int START_PULSE[] = {START_PULSE_X, START_PULSE_Y};
const int POT_RANGE_X = MAX_POT - MIN_POT;
const int POT_RANGE_Y = MAX_POT - MIN_POT;
const int POT_RANGE[] = {POT_RANGE_X, POT_RANGE_Y};

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

int averagingBuffer[SERVOS_IN_USE][BUFFER_SIZE];
int averagingBufferY[BUFFER_SIZE];
int bufferIndex = 0;

long bufferTotal[SERVOS_IN_USE];

int servoPosition[] = {START_PULSE_X, START_PULSE_Y};
unsigned long lastDebug;
unsigned long lastServo;
unsigned long lastAnalogRead;

Servo myServo[2];


void setup()
{
  Serial.begin(115200);

  for (int i = 0; i < SERVOS_IN_USE; i++)
  {
    myServo[i].writeMicroseconds(servoPosition[i]); // start servo in center position
    myServo[i].attach(SERVO_PIN[i], MIN_PULSE, MAX_PULSE);
    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]);

      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.
{
  int average;
  int servoTarget;
  boolean debugFlag = checkDebugTime();
  for (int i = 0; i < SERVOS_IN_USE; i++)
  {
    average = bufferTotal[i] >> POWER_OF_TWO_TO_AVERAGE;
    servoTarget = MIN_PULSE + ((average - MIN_POT) * SERVO_PULSE_RANGE / POT_RANGE[i]);
    if (servoTarget > servoPosition[i] + MAX_SPEED)
    {
      servoPosition[i] += MAX_SPEED;
    }
    else if (servoTarget < servoPosition[i] - MAX_SPEED)
    {
      servoPosition[i] -= MAX_SPEED;
    }
    else
    {
      servoPosition[i] = servoTarget;
    }
    myServo[i].writeMicroseconds(servoPosition[i]);
    if (debugFlag)
    {
      debugServo(i, average, servoPosition[i]);
    }
  }
}

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)
// 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.println(servoOutput, DEC);
}

I'm speechless DuaneDegn, this is really awesome. I'm trying some settings such as speed = 1 and it does indeed slow down as intented.

What if I set the adc to 12 bit (since I'm a DUE) instead of the standard 10 bit? Is it an advantage?

I'll run a lot of tests today and let you know if something doesn't work as supposed to.

thank you very very much

I'm glad it's working as expected.

I didn't want the servos I'm using in a project to abruptly change speeds so I added an acceleration component to the control algorithm.

The code didn't fit. I'll add it to the next reply.

There are now lots of parameters to set. I started adding instructions on how to set these various parameters but I've run out of time. I'll have to finish the instructions some other time.

The above code should control servos nice and smooth.

The code above will control servos' position or the servos' speed based on the position of the joystick(s). The program can be modified to use as many analog inputs as the Arduino being used supports.

I added a note in the comments about using the code with a DUE.

Let me know if you have any questions or if the program does something unexpected.

As I said, I haven't finished documenting all the parameters but hopefully the variable names will offer a clue on the parameter's purpose.

Here’s the first part of the program.

/* Joystick2ServoAcceleration160117a
 *
 *  by Duane Degn
 *  December 22, 2015
 *  modified January 17, 2016
 *
 *
 *  Program to control servos with either
 *  potentiometer or joystick input.
 *  
 *  A ring buffers are used to average the
 *  ADC readings from two potentiometers.
 *  This average is used to control
 *  speed of two hobby servos.
 *  The servos will now use a constant
 *  acceleration algorithm.
 *  
 *  To increase the speed of the program,
 *  comment out calls to "debugServo".
 * 
 *  Watch for /****** Instructions ******/ /*
 *  notices about how to configure the program.
 *  
 *  Version 22a has a bunch of extra debug statements.
 *  Version 160117a added indicator for which
 *  constants arrays need to be adjusted when
 *  the value of "SERVOS_IN_USE" is changed.
 *  The comment below will be used to mark 
 *  these arrays.
 *  **** Number of elements should match "SERVOS_IN_USE" ****
 */

  // Note about DUE analog inputs.
  // If the DUE's 12-bit inputs are used, the following
  // arrays should be modified.
  // "LOW_CENTER_THRESHOLD", "HIGH_CENTER_THRESHOLD"
  // "MIN_POT" (if non-zero), "MAX_POT" and the
  // value of "POT_TO_SPEED_CONSTANT" should be
  // adjusted (usually multiplied by a factor of four).
  
#include <Servo.h>

// ****** Start of Constants ******

  /****** Instructions ******/
  // The various parameter arrays will need to be
  // modified is additional servos are used.               
const byte SERVOS_IN_USE = 2;   // User changeable.

  /****** Instructions ******/
  // Add pins for servos and joysticks (or pots) as needed.
const byte SERVO_PIN[] = {2, 3}; //**** Number of elements should match "SERVOS_IN_USE" ****
const byte JOYSTICK_PIN[] = {A0, A1}; //**** Number of elements should match "SERVOS_IN_USE" ****

  /****** Instructions ******/
  // Add additional zeros (or ones) as needed.
  // set to one if servo should be reversed
const boolean SERVO_REVERSE_FLAG[] = {0, 0}; //**** Number of elements should match "SERVOS_IN_USE" ****

  /****** Instructions ******/
  // To use a joystick to set a servo's position, set the corresponding element to zero. 
  // An element set to one will indicate the servo's speed will be controlled by the joystick.
  // set to one if joystick provides speed input rather than position input
const boolean SERVO_SPEED_CONTROL_FLAG[] = {0, 0}; //**** Number of elements should match "SERVOS_IN_USE" **** 

  /****** Instructions ******/
  // "LOW_CENTER_THRESHOLD" and "HIGH_CENTER_THRESHOLD" are only used with the speed control
  // algorithm. Pot values between these thresholds will not change a speed controlled
  // servo's position.
  // These values need to be determined experimentally.
const long LOW_CENTER_THRESHOLD[] = {477, 477}; //**** Number of elements should match "SERVOS_IN_USE" ****     
const long HIGH_CENTER_THRESHOLD[] = {515, 515}; //**** Number of elements should match "SERVOS_IN_USE" ****   

  /****** Instructions ******/
  // "POT_TO_SPEED_CONSTANT" are only used with the speed control algorithm.
  // The minimum value of "POT_TO_SPEED_CONSTANT" is 1.
  // Larger values for slower speeds.
const long POT_TO_SPEED_CONSTANT[] = {8, 8}; //**** Number of elements should match "SERVOS_IN_USE" ****     

  /****** Instructions ******/
  // The values below should be determined experimentally.
  // Servo endpoints, the default endpoints used by the Arduino
  // servo library are 544 and 2400. The 544 and 2400
  // setting can damage some servos. The "900" and "2100" are much
  // more conservative endpoint setting.
  // Again these values need to be determined experimentally. 
const long MIN_PULSE[] = {900, 900}; //**** Number of elements should match "SERVOS_IN_USE" ****
const long MAX_PULSE[] = {2100, 2100}; //**** Number of elements should match "SERVOS_IN_USE" ****   

  /****** Instructions ******/
  // The values below should be determined experimentally.
  // Some joysticks do not provide a full range of analog values.
  // 0 and 1023 are common values for "thumb sticks". 
const long MIN_POT[] = {0, 0}; //**** Number of elements should match "SERVOS_IN_USE" ****                          
const long MAX_POT[] = {1023, 1023}; //**** Number of elements should match "SERVOS_IN_USE" ****      

  /****** Instructions ******/
  // The "MAX_SPEED" of each servo may be individually set.
  // This is the amount the pulse length will
  // change each refresh cycle when the servo
  // is moving at its top speed.
const long MAX_SPEED[] = {32, 32}; //**** Number of elements should match "SERVOS_IN_USE" ****

  /****** Instructions ******/
  // The "ACCELERATION" of each servo may be individually set.
  // The minimum value of this parameter is 1.
  // This is the amount the speed of the servo
  // will change with each refresh cycle.
  // If this value is set to the same value 
  // as "MAX_SPEED", the servo will not accelerate
  // and decelerate but it will move with
  // constant speed.
const long ACCELERATION[] = {2, 2}; //**** Number of elements should match "SERVOS_IN_USE" ****

  /****** Instructions ******/
  // It is important some time constants are larger than
  // other time constants.
  // "ANALOG_READ_PERIOD" must be smaller than "SERVO_PERIOD".
  // "SERVO_PERIOD" must be smaller than "DEBUG_PERIOD".
  // To reduce the stutter caused from multiple
  // debug stateements at once, the debug data
  // is staggered. Data from a single servo is
  // displayted
const unsigned long ANALOG_READ_PERIOD = 5000;  // read pots at 200Hz "ANALOG_READ_PERIOD" must be <= "DEBUG_PERIOD"
const unsigned long SERVO_PERIOD = 20000;  // update servo at 50Hz
const unsigned long DEBUG_PERIOD = 5000000;  // update serial at 4Hz "DEBUG_PERIOD" must be >= "SERVO_PERIOD"

  /****** Instructions ******/
  // The constant "POWER_OF_TWO_TO_AVERAGE" sets the size of
  // the ring buffer used to average the pot values.
  // Large buffers will provide smoother motion but large
  // buffers also slow down response times.
  // The minimum value of "POWER_OF_TWO_TO_AVERAGE"
  // is one. If zero is used, the code will need to be
  // modified.
  // 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 POWER_OF_TWO_TO_AVERAGE = 4;          // User changeable.

// ****** End of User Changeable Constants ******

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

// ****** End of Constants ******

// The value of "servoPulseRange" will be calculated
// in the "setup" section of the program. 
// The value of "servoPulseRange" will not change 
// outside the "setup" function.
long servoPulseRange[SERVOS_IN_USE]; 


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

long bufferTotal[SERVOS_IN_USE];
byte servoToDebug = SERVOS_IN_USE - 1;
unsigned long lastDebug;
unsigned long lastServo;
unsigned long lastAnalogRead;

Servo myServo[SERVOS_IN_USE];

Edit (17 January 2016): The latest version with extra instructions won’t fit in this reply. I’ve only included the text of the constant and variable declaration sections.

To see the full code, you need to download the attached file.

Joystick2ServoAcceleration160117a.ino (13.2 KB)

Here’s the rest.

long getSpeedBasedPosition(byte servoIndex, long averageJoystickInput)
{
  long targetSpeed;
  long targetPosition;
  long distanceToStop = 0;
  long stopSpeed;
  
  if (averageJoystickInput < LOW_CENTER_THRESHOLD[servoIndex])
  {
    targetSpeed = (averageJoystickInput - LOW_CENTER_THRESHOLD[servoIndex]) / POT_TO_SPEED_CONSTANT[servoIndex];
    // negative speed proportional to distance from center pot
  }
  else if  (averageJoystickInput > HIGH_CENTER_THRESHOLD[servoIndex])
  {
    targetSpeed = (averageJoystickInput - HIGH_CENTER_THRESHOLD[servoIndex]) / POT_TO_SPEED_CONSTANT[servoIndex];
    // positive speed
  }
  else // pot in dead zone
  {
    targetSpeed = 0;
  }
    
  targetSpeed = constrain(targetSpeed, servoSpeed[servoIndex] - ACCELERATION[servoIndex], servoSpeed[servoIndex] + ACCELERATION[servoIndex]);
  
  if (targetSpeed > 0)
  {
    distanceToStop = MAX_PULSE[servoIndex] - servoPosition[servoIndex];
  }
  else if (targetSpeed < 0)
  {
    distanceToStop = MIN_PULSE[servoIndex] - servoPosition[servoIndex];
  }
  
  stopSpeed = computeMaxSpeedWhileStillStopping(ACCELERATION[servoIndex], distanceToStop);
  
  if (abs(targetSpeed) > abs(stopSpeed))
  {
    targetSpeed = stopSpeed;
  }
  
  servoSpeed[servoIndex] = constrain(targetSpeed, -MAX_SPEED[servoIndex], MAX_SPEED[servoIndex]);
 
  targetPosition = servoPosition[servoIndex] + servoSpeed[servoIndex];

  servoPosition[servoIndex] = constrain(targetPosition, MIN_PULSE[servoIndex], MAX_PULSE[servoIndex]);

}
 
long computeStopDistance(long acceleration, long currentSpeed)
{
  long stopDistance;
  long timeToStop = abs(currentSpeed / acceleration);
 
  stopDistance = currentSpeed * timeToStop / 2.0;
  return stopDistance;
}

long computeMaxSpeedWhileStillStopping(long acceleration, long distance)
{
  long maxSpeed = sqrt(2 * acceleration * abs(distance));
  if (distance < 0)
  {
    maxSpeed *= -1;
  }
  return maxSpeed;
}

long getJoystickBasedPosition(byte servoIndex, long averageJoystickInput)
{
  long targetSpeed;
  long targetPosition = MIN_PULSE[servoIndex] + ((averageJoystickInput - MIN_POT[servoIndex]) * SERVO_PULSE_RANGE[servoIndex] / (MAX_POT[servoIndex] - MIN_POT[servoIndex]));
  long distanceToGo = targetPosition - servoPosition[servoIndex];

  targetSpeed = computeMaxSpeedWhileStillStopping(ACCELERATION[servoIndex], distanceToGo);
  targetSpeed = constrain(targetSpeed, servoSpeed[servoIndex] - ACCELERATION[servoIndex], servoSpeed[servoIndex] + ACCELERATION[servoIndex]);
  servoSpeed[servoIndex] = constrain(targetSpeed, -MAX_SPEED[servoIndex], MAX_SPEED[servoIndex]);  
  servoPosition[servoIndex] = constrain(servoPosition[servoIndex] + servoSpeed[servoIndex], MIN_PULSE[servoIndex], MAX_PULSE[servoIndex]);
}

byte checkDebugTime()
// Called from "controlServo" function.
// This method checks to see if it's time to
// display data and returns the servo
// id number of the servo to debug.
{
  byte debugFlag = SERVOS_IN_USE;
  if (micros() - lastDebug > DEBUG_PERIOD)
  {
    lastDebug += DEBUG_PERIOD;
    servoToDebug++;
    if (servoToDebug == SERVOS_IN_USE)
    {
      servoToDebug = 0;
    }
    debugFlag = servoToDebug;
  }
  return debugFlag;
}

void debugServo(int servoIndex, long average)
// 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(servoPosition[servoIndex], DEC);
  Serial.print(F(", speed = "));
  Serial.println(servoSpeed[servoIndex], DEC);
}

I had to deleted a bunch of commented out code in order to get this to fit in two posts. Hopefully I didn’t delete anything important.

I’ve attach a full version as a file in an earlier reply. The code embedded in these two replies is the same as the attached code.