Starting to understand this now- considering use of the goto command.

Ok, so I'm still a novice but making some progress modifying a servo smoothing program. This WAS for a halloween costume that animates an eyeball. You'll be happy to know it worked! It was actually pretty awesome- but that's a different story..

The previous program successfully tamed these servos so they move more naturally and aren't constantly jerking around. There was some strangeness but I think it was a physical servo issue.

I was controlling them with the standard arduino thumbstick and an additional potentiometer for eyelids. I ran an extra wire for the momentary button associated with that thumbstick but I never used it. Now that playtime is over I'm interested in following through with my program for the sake of learning this. What I want the button to do is change operational mode of the eye to automatic instead of controlled by input from the user.

To that end- I've so far created this frankenstein of a program that contains the code from that servo program as well as the toggle switch/debounce program and also some code to generate two random numbers from 0-1023 which I hope to substitute in for joystick X/Y coordinates.
To my amazement I have gotten all this to work (in serial monitor anyway). The code verifies and uploads and serial monitor prints out my X Y random numbers and toggles another (state)number with the pushbutton.

Great! So I think the next step is trickier and that is I want the program to either run the original code which uses coordinates from the joystick, or toggle to automatic where it will move to different (randomly generated) coordinates but still maintain the smooth motion that the previous code provides. I was thinking that I could use a goto command for this (from old school basic programming!) but I see that is "frowned" on in C++. It still may be the quickest way to adapt this code, however. So far I don't see a better way other than knowing what I'm doing and writing it from scratch- not an option (yet!).

Following that, I'll need to figure out a time variable in the secondary program where it will only decide to make an eyeball movement every 2 seconds or so instead of a deluge of continuously updating coordinates. I don't know that one yet either but it seems like it would be easier for me to figure out. I guess maybe just a delay command? Still- I basically need two separate programs and I need it to check what state the toggle button has it set it to choose which code gets run. Thanks for any help!

Code here:

OP's code so we don't have to leave this site

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

 //KD - my initials citing lines I've clipped from other code.
// constants won't change. They're used here to set pin numbers:
const int buttonPin = 7;    // KD the number of the pushbutton pin
const int ledPin = 13;      // KD the number of the LED pin

// Variables will change:
int ledState = HIGH;         // KD the current state of the output pin
int buttonState;             // KD the current reading from the input pin
int lastButtonState = LOW;   // KD the previous reading from the input pin

// the following variables are unsigned longs because the time, measured in
// milliseconds, will quickly become a bigger number than can be stored in an int.
unsigned long lastDebounceTime = 0;  // KD the last time the output pin was toggled
unsigned long debounceDelay = 50;    // KD the debounce time; increase if the output flickers

#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 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 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 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];

int Xrandom; //KD
int Yrandom; //KD

void setup()
  Serial.begin(9600);  //115200 too fast for serial monitor 9600 too slow for performance
   randomSeed(analogRead(5));  //KD random unconnected analog input

  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];
  pinMode(buttonPin, INPUT);  //KD
  pinMode(ledPin, OUTPUT);  //KD

  // set initial LED state  //KD
  digitalWrite(ledPin, ledState);  //KD

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

void loop()

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

    long joystickInput;

    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


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

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;
      servoPosition[i] = servoTarget;
    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.
  Xrandom = random(0,1023);
  Serial.print ("X= ");
  Serial.print (Xrandom);
  Yrandom = random(0,1023);
  Serial.print ("          ");
  Serial.print ("Y= ");
  Serial.print (Yrandom);
  Serial.println ();
  Serial.print (ledState);
  Serial.println ();

//    Serial.print(F("servo # "));
//    Serial.print(servoIndex, DEC);
//    Serial.print(F(": average = "));
//    Serial.print(average, DEC);
//    Serial.print(F(", position = "));
//    Serial.println(servoOutput, DEC);
  // KD  read the state of the switch into a local variable:
  int reading = digitalRead(buttonPin);

  // check to see if you just pressed the button
  // (i.e. the input went from LOW to HIGH), and you've waited long enough
  // since the last press to ignore any noise:

  // If the switch changed, due to noise or pressing:
  if (reading != lastButtonState) {
    // reset the debouncing timer
    lastDebounceTime = millis();

  if ((millis() - lastDebounceTime) > debounceDelay) {
    // whatever the reading is at, it's been there for longer than the debounce
    // delay, so take it as the actual current state:

    // if the button state has changed:
    if (reading != buttonState) {
      buttonState = reading;

      // only toggle the LED if the new button state is HIGH
      if (buttonState == HIGH) {
        ledState = !ledState;

  // set the LED:
  digitalWrite(ledPin, ledState);

  // KD  save the reading. Next time through the loop, it'll be the lastButtonState:
  lastButtonState = reading;

thanks for that- I did that because the forum page said I'd reached my 9000 (iirc) character limit. Evidently there's a cure for that!

I guess maybe just a delay command?

Using delay() will certainly do what its name suggests but whilst the delay happens then nothing else can run. Luckily there are alternative, non blocking ways to implement delays

Have a look at Using millis() for timing. A beginners guide, Several things at the same time and look at the BlinkWithoutDelay example in the IDE.

Evidently there's a cure for that!

It is the total character count that matters so your code alone was small enough to fit in a post

Can you post your original program? Did it already use the debugServo() function in the way it's used now? Because in my view it does not make sense to name a function with debug in the name and next put non-debug stuff in there; especially if the comment at the beginning states that it's a good idea to remove that function :slight_smile:

Just dump it in a post on its own (don't forget code tags).

Yes- I don't know how to use debugServo at all. The parts I added so far are the random #'s, some Serial.print's and the button toggle. Also, I initially modified it to work with 3 servos instead of 2.
I actually had posted the code in this post before getting it to work somehow. I'm still not quite sure how/why it started working correctly so I'm sticking to my blame on that one particular servo. Anyhow- here is that post.

Don't think about a goto. Think about how to slice Frankenstein into smaller modules which have a definite purpose. Then instead of loading new code that does different stuff, just call the modules that you need right now.

In C, those modules are called "functions". You can have a function that takes care of driving the motors. Another function does the joystick input. Another function does the automatic motions.

Something like this...

void loop() {
  inputValue = readInput();
  modeIsAutomatic = readAutoSwitch();
  if(modeIsAutomatic) outputValue = auto(); else outputValue = manual(inputValue);

Now you have several values, so this simple method of passing a value to the function won't work. You would really put that if(modeIsAutomatic) inside another function and set the global X/Y/Z output values there. motorDrive() would take no parameters, but it would read the global variables to find out what to output.

This is surely good advice- but I'm confident at least for now that it's above my experience level. I'll give it a shot now that I don't have a deadline but I have to say it's a bit intimidating to try to sort out in my head at present. thanks!

I see that you still haven't fixed the bug that @johnwasser indicated in your other thread :wink:

    for (int j; j < BUFFER_SIZE; j++) // Fill buffer with start position.

The variable j is not initialised so does not always start at 0; compare it with what you used earlier as the outer for-loop

for (int i = 0; i < SERVOS_IN_USE; i++)

where i is initialised with 0.

this might be the cause of your quirk with the servos

Although I agree that the approach by @MorganS is preferred, the easiest place for you to use the value of the switch and assign some random values is in the checkAnalogReadTime() function.

The assigning of a value to joystickInput will either be a random() value or a value based on the analogRead(), depending on your switch position.

I think that you can keep the averaging in with this approach.

Thanks all. Clearly I have some work to do. This will take a while.
The reason I didn't update the code on Johnwasser's suggestion is twofold:

  1. I wasn't sure how- what you suggested is what I would have guessed but I also didn't want to screw it up.
  2. It started working as intended after changing a servo without my changing the code, and so I concluded the servo must have been at fault. I also assumed that johnwasser, not finding any other fault may have speculated that that detail was the cause. I see that he was right following your explanation. I should have replied with a follow up though- in my defense I was distracted by Halloween and successful deployment of this costume prop.

Anyway- thanks!