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.
