I found this code written a few years back by Duane- I guess he's not active here anymore, and the code in general is WAY above my pay grade- so I'm trying to be careful about modifying it as a novice. However it does a pretty great job at controlling 2 (now 3) hobby servos.
I was previously posting to get it to work with 3 servos- I have that sorted but the only quirk now is that one of them ("Servo Y") is very sketchy about returning. Seems like it only returns to center in one direction. The other two servos seem to be good. Also the terminal monitor is delivering gibberish- I don't mind cutting that out except that it might otherwise help troubleshoot this. Anyway I made this video to illustrate the problem
Here is a link to the previous thread where it was discussed and composed.
Here is the code that I attempted to modify:
/* 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 9 // User changeable.
#define SERVO_Y_PIN 10 // User changeable.
#define SERVO_Z_PIN 11 // 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.
#define JOYSTICK_Z_PIN A2 // 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 = 3;
const int MAX_SPEED = 16;
const byte SERVO_PIN[] = {SERVO_X_PIN, SERVO_Y_PIN, SERVO_Z_PIN};
const byte JOYSTICK_PIN[] = {JOYSTICK_X_PIN, JOYSTICK_Y_PIN, JOYSTICK_Z_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_Z = MIN_PULSE + (SERVO_PULSE_RANGE) / 2; // User changeable.
const int START_PULSE[] = {START_PULSE_X, START_PULSE_Y, START_PULSE_Z};
const int POT_RANGE_X = MAX_POT - MIN_POT;
const int POT_RANGE_Y = MAX_POT - MIN_POT;
const int POT_RANGE_Z = MAX_POT - MIN_POT;
const int POT_RANGE[] = {POT_RANGE_X, POT_RANGE_Y, POT_RANGE_Z};
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, START_PULSE_Z};
unsigned long lastDebug;
unsigned long lastServo;
unsigned long lastAnalogRead;
Servo myServo[3];
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);
}
Thanks for any help sorting out this last detail!