It's Working! But Need some help to avoid random results I am getting

Hi all,

Thank you to those who have previously helped, turning me on to states really helped.
I have my project “working” and can control my device with Alexa so I am pretty pleased, but could use some help on fine tuning 2 things driving me batty. Thought I would push my luck and post both at same time.

First, upon startup I cannot figure out to stop servo from full speeding to about 70 degrees before doing my slowmove to zero command during setup… assuming a servo initialization? It is a MG996R
(I have tried moving the code in different locations in the setup like before attach command)
Am hoping to have just slowmove at startup.

and Secondly…forgive my lack of proper terminology, but hopefully expresses the issue… I have one state case that simply moves servo to 180, and one state case that incrementally moves it by 60 degrees and automatically reverses it upon next move if at 0 or 180, if I continually activate the incremental case it keeps working as intended no matter how many times it is run, so it proves it knows to reverse at 180 and 0, awesome…BUT if I run the case that puts it at 180 and THEN trigger the incremental case it tries to go to 240 instead of 120? It “ignores” the minus anglestep it already proved it knew?? AND for the random part, if I trigger it enough times it will eventually do it correctly!? I need some fresh eyes please.

Still new to this so hopefully doing this correctly, :o
Here is code below and a shot of serial monitor is attached:

 #include <VarSpeedServo.h>

VarSpeedServo servo1;
VarSpeedServo servo2;

int angle =0;    // initial angle  for servos
int angleStep =60;

#define CLOSE 12   // pin 12 is connected to CLOSE relay channel
#define TILT  2  // pin 2 is connected to TILT relay channel

enum {ST_IDLE, ST_x, ST_y, ST_z} currentState = ST_IDLE;
// enum just allocates integers from the left so ST_STANDBY=0, ST_CW =1 etc
// find it easier to have human relatable names in the later

byte button1 = 2;
byte button2 = 3;

void setup()
  Serial.println(".... state machine with 2 inputs and 2 states ....");
  Serial.print("Compiler: ");
  Serial.print(", Arduino IDE: ");
  Serial.print("Created: ");
  Serial.print(", ");

  servo1.attach(9);  // attaches the servo on pin 9 to the servo object
  servo1.slowmove(angle,30);  // send servo to initial angle at startup
  pinMode(CLOSE, INPUT_PULLUP); // CLOSE button from pin 12 to ground
  pinMode(TILT, INPUT_PULLUP); // TILT button from pin 2 to ground
  Serial.println("Setup done...\n");
  Serial.println("Apply applicable input to move to its state");

void loop()

void doStates()
  switch (currentState)
    case ST_IDLE:
      if (!digitalRead(CLOSE)) currentState = ST_x;
      if (!digitalRead(TILT)) currentState = ST_y;

    case ST_x:
    // for CLOSE
      Serial.println("doing x");
      angle = (180); // to change whatever the ST_y Case may have it set to
      servo1.slowmove(angle,30);   // tell servo to go to 180 to CLOSE
      Serial.print("X Moving to: ");
      delay(2800); //wait for servo... zero to 180 needs time before detach
      Serial.println("  finished x, going back to idle to wait for a button");
      currentState = ST_IDLE;
      servo1.detach(); //so can manually move it when needed

//this below area works as intended if it sets it to 180,
//but if above Case ST_x sets it to 180 it fails to minus anglestep

      case ST_y:
      // for TILT
      // change the angle for next time through the case:
         angle = angle + angleStep;

    // reverse the direction of the servo at the ends of travel:
        if (angle <= 0 || angle >= 180) {
        angleStep = -angleStep;
    Serial.println("doing y");
    servo1.slowmove(angle,30); // move the servo to newly set angle TILT
      Serial.print("Y Moving to: ");
      Serial.print(angle);   // print the angle
      Serial.println(" degree"); 
      delay(2800); // waits for the servo to get there
      Serial.println("  finished y, going back to idle to wait for a button");
      currentState = ST_IDLE;
      servo1.detach(); //so can manually move it when needed


For the servo, try writing the angle that you want to start at before you attach the servo.


attach() will always set the servo to some base angle (the default is 90). If you are leaving the servo at random positions it will need to move and I imagine it will always be a full speed move.

It really can't do slow moves until after the attach() has completed. To start the slow move it needs to know what the current position is and there's no way to find that out from a normal hobby servo like an MG996R other than sending it to a specific position.


Thank you groundFungus and slipstick, I will tinker some more in that area...
Now hopefully some nibbles on the second issue.

Save the servo’s last position in eeprom. Then on boot-up read the eeprom attach the servo and immeditly give it that number?

-jim lee

Thank you jimLee, I will research that option.