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.)