Controlling Individual servos with individual buttons

I want to control 5 individuals servos with 5 individual buttons controlling a servo each, when i hold down a button the servo moves to 180 degrees, and when i release said button, the servo with return back to its original state, this code i have controls the servos with a button each but as soon as i release the button, nothing happens.

#include <Servo.h>

Servo myservo;  // pinky
Servo myservo1; // ring
Servo myservo2; // middle
Servo myservo3; // index
Servo myservo4; // thumb

//min value = finger is straight
//max value = finger is bend

int angle =170;
int angleStep =10;

int angle1 = 170;
int angleStep1= 10;

int angle2 = 170;
int angleStep2= 10;

int angle3 = 170;
int angleStep3= 10;

int angle4= 170;
int angleStep4= 10;

void setup() {
  // Servo buttons
  myservo.attach(3); 
  myservo1.attach(5);
  myservo2.attach(7);
  myservo3.attach(9);
  myservo4.attach(11);
  pinMode(2,INPUT_PULLUP);
  pinMode(4,INPUT_PULLUP);
  pinMode(6,INPUT_PULLUP);
  pinMode(8,INPUT_PULLUP);
  pinMode(10,INPUT_PULLUP);
}

void loop() {
  if(digitalRead(2) == LOW){
    angle = angle + angleStep;
    if (angle <= 0 || angle >= 180) {
      angleStep = -angleStep;
    }myservo.write(angle);
    delay(20); 
  }
 if(digitalRead(4) == LOW){
    angle1 = angle1 + angleStep1;
    if (angle1 <= 0 || angle1 >= 180) {
      angleStep1 = -angleStep1;
       }myservo1.write(angle1);    
  delay(20);
    }
 if(digitalRead(6) == LOW){
    angle2 = angle2 + angleStep2;
    if (angle2 <= 0 || angle2 >= 180) {
      angleStep2 = -angleStep2;
       }myservo2.write(angle2);    
  delay(20);
    }
  if(digitalRead(8) == LOW){
    angle3 = angle3 + angleStep3;
    if (angle3 <= 0 || angle3 >= 180) {
      angleStep3 = -angleStep3;
       }myservo3.write(angle3);    
  delay(20);
    }
  if (digitalRead(10) == LOW){
    angle4 = angle4 + angleStep4;
    if (angle4 <= 0 || angle4 >= 180) {
      angleStep4 = -angleStep4;
       }myservo4.write(angle4);    
  delay(20);
    }
   }

(Code tags added by Moderator)

Were you able to get one servo to work as a test ?

We need to see your wiring.


Show us a good schematic of your circuit.

Show us a good image of your ‘actual’ wiring.

Give links to components.


FYI


if(digitalRead(2) == LOW){
angle = angle + angleStep;
if (angle <= 0 || angle >= 180) {
angleStep = -angleStep;
}myservo.write(angle);
delay(20);
}

The servo moves when the switch is closed (LOW), not when you release the switch (HIGH).

Hi, @lbonde23
Welcome to the forum.

What do you want to happen if more than one button is pushed at or during the push?

If you just press and release the button, shorter time than to servo can go to 180, do you want it to complete the 0 -180 - 0 cycle, or mid sweep return back to 0?

Thanks.. Tom.. :grinning: :+1: :coffee: :australia:

if i were to just press the button and release straight away i want it to sweep back to 0

Here is a start:

//Version 1.01

#include <Servo.h>

//first servo stuff
Servo firstServo;  // pinky
const byte firstServoSwitch     = 2;
const byte firstServoAngle      = 170;
const byte firstServoAngleStep  = 10;
const byte firstServoPin        = 3;
byte lastSwitchStatefirstServo  = HIGH;



//*****************************************************
void setup()
{
  firstServo.attach(firstServoPin);

  pinMode(firstServoSwitch, INPUT_PULLUP);

} //END of setup()

//*****************************************************
void loop()
{
  checkSwitches();

} //END of loop()


//*****************************************************
void checkSwitches()
{
  byte state = digitalRead(firstServoSwitch);

  //******************************
  //first servo stuff
  if (lastSwitchStatefirstServo != state)
  {
    lastSwitchStatefirstServo = state;

    //***************
    //first servo switch is pushed ?
    if (state == LOW)
    {
      for (int pos = 0; pos <= firstServoAngle; pos = pos + firstServoAngleStep)
      {
        firstServo.write(pos);
        delay(20);
      }
    }

    //***************
    //first servo switch is released ?
    else
    {
      for (int pos = firstServoAngle; pos >= 0; pos = pos - firstServoAngleStep)
      {
        firstServo.write(pos);
        delay(20);
      }
    }
  }

  //******************************
  //second servo stuff

} //END of checkSwitches()

that worked well, pictured is the setup i am using, where the button is connected to ground and pin 2

Looks like you are using the 5v pin on the Arduino to power the servo(s).

If you do this your computer on the other side of the USB cable can be overloaded, possibly damaged.

You should use an external 5v (6v) power supply to power servos; this power supply should share its GND with the Arduino GND.

The external power supply must be able to supply the maximum current that all the servos will draw.

Here is a different version of the sketch offered to you in post #7.

This version is designed without blocking code, i.e. delay() is not used.

//Forum https://forum.arduino.cc/t/controlling-individual-servos-with-individual-buttons/861547?u=larryd
//Non blocking servo control

//Version  YYMMDD   Description
// 1.02    210513   made non-blocking

#include <Servo.h>

#define doNothing                 0
#define goingToMax                1
#define goingToMin                2

const byte heartbeatLED         = 13;

//****************************************
//first servo stuff
Servo firstServo;  // pinky
const byte firstServoSwitch     = 2;
const byte firstServoAngle      = 170;
const byte firstServoAngleStep  = 10;
const byte firstServoPin        = 3;
byte lastSwitchStatefirstServo  = HIGH;
int firstPosition               = 0;
byte firstFSM                   = doNothing;
unsigned long firstMillis;

//****************************************
//second servo stuff




//timing stuff
unsigned long heartbeatMillis;
unsigned long switchMillis;


//*******************************************************************
void setup()
{
  pinMode(heartbeatLED, OUTPUT);

  firstServo.attach(firstServoPin);
  firstServo.write(0);
  pinMode(firstServoSwitch, INPUT_PULLUP);

} //END of setup()

//*******************************************************************
void loop()
{
  //*******************************
  //time to toggle the heartbeatLED ?
  if (millis() - heartbeatMillis >= 250)
  {
    //retart the Timer
    heartbeatMillis = millis();

    digitalWrite(heartbeatLED, !digitalRead(heartbeatLED));
  }

  //*******************************
  //time to check the switches ?
  if (millis() - switchMillis >= 50)
  {
    //retart the Timer
    switchMillis = millis();

    checkSwitches();
  }

  //*******************************
  //check the FSM
  checkFSM();

} //END of loop()


//*******************************************************************
//Finite State Machine (FSM)
void checkFSM()
{
  //**********************************************
  //first state machine
  switch (firstFSM)
  {
    //***********
    case doNothing:
      {
        //do nothing
      }
      break;

    //***********
    case goingToMax:
      if (millis() - firstMillis >= 20)
      {
        //restart the TIMER
        firstMillis = millis();

        //move servo
        firstServo.write(firstPosition);

        //next position
        firstPosition = firstPosition + firstServoAngleStep;

        //at maximum ?
        if (firstPosition > firstServoAngle)
        {
          firstPosition = firstServoAngle;
          firstServo.write(firstPosition);

          //we are finished
          firstFSM = doNothing;
        }

      }
      break;

    //***********
    case goingToMin:
      if (millis() - firstMillis >= 20)
      {
        //restart the TIMER
        firstMillis = millis();

        //move servo
        firstServo.write(firstPosition);

        //next position
        firstPosition = firstPosition - firstServoAngleStep;

        //at minimum ?
        if (firstPosition <= 0)
        {
          firstPosition = 0;
          firstServo.write(firstPosition);

          //we are finished
          firstFSM = doNothing;
        }

      }
      break;
  }

  //**********************************************
  //second state machine

  //**********************************************

} //END of checkFSM()


//*******************************************************************
void checkSwitches()
{
  byte state = digitalRead(firstServoSwitch);

  //******************************
  //first servo switch 
  if (lastSwitchStatefirstServo != state)
  {
    lastSwitchStatefirstServo = state;

    //***************
    //first servo switch is pushed ?
    if (state == LOW)
    {
      //restart the TIMER
      firstMillis = millis();

      firstFSM = goingToMax;
    }

    //***************
    //first servo switch is released ?
    else
    {
      //restart the TIMER
      firstMillis = millis();

      firstFSM = goingToMin;
    }
  }

  //******************************
  //second servo switch



} //END of checkSwitches()


//*******************************************************************

Hello
I will start with a little bit of pseudo code that help to think about the needed button handler:

The notation 01 or 10 means the button transition.
if (button_01) servoWrite(destination_1);
if (button_10) servoWrite(destination_2);

that works really well and just how i want it to, how would i go about changing it to add 4 other servos (5 servos in total)??

You should be able to find the 3 areas in the sketch for the first servo.

. . . means you need to copy the code that follows, however, after pasting what you copied, you must change the naming accordingly.

Example:
//****************************************
//first servo stuff
. . .
//**********************************************
//first state machine
. . .
//******************************
//first servo switch
. . .


The above 3 sections have to be repeated for each servo you add.
example:
//****************************************
//second servo stuff
. . .
//**********************************************
//second state machine
. . .
//******************************
//second servo switch
. . .

In the checkSwitches() function, for each additional servo, you will need:

state = digitalRead(secondServoSwitch);


state = digitalRead(thirdServoSwitch);


state = digitalRead(fourthServoSwitch);


state = digitalRead(fifthServoSwitch);


Appropriately located of course. :wink:

Oh, one more thing.


In setup() you will need to a these lines for each servo added:

firstServo.attach(firstServoPin);
firstServo.write(0);
pinMode(firstServoSwitch, INPUT_PULLUP);


example:

secondServo.attach(secondServoPin);
secondServo.write(0);
pinMode(secondServoSwitch, INPUT_PULLUP);


etc.

so i put in everything you said but i am still having trouble with it, here it says that i can't have '}' at the end of input??

#include <Servo.h>

#define doNothing                 0
#define goingToMax                1
#define goingToMin                2

const byte heartbeatLED         = 13;

//****************************************
//first servo stuff
Servo firstServo;  // pinky
const byte firstServoSwitch     = 2;
const byte firstServoAngle      = 170;
const byte firstServoAngleStep  = 10;
const byte firstServoPin        = 3;
byte lastSwitchStatefirstServo  = HIGH;
int firstPosition               = 0;
byte firstFSM                   = doNothing;
unsigned long firstMillis;

//****************************************
//second servo stuff
Servo secondServo;  // ring
const byte secondServoSwitch     = 4;
const byte secondServoAngle      = 170;
const byte secondServoAngleStep  = 10;
const byte secondServoPin        = 5;
byte lastSwitchStatesecondServo  = HIGH;
int secondPosition               = 0;
byte secondFSM                   = doNothing;
unsigned long secondMillis;

//*********************************************
//third servo stuff
Servo thirdServo;  // middle
const byte thirdServoSwitch     = 6;
const byte thirdServoAngle      = 170;
const byte thirdServoAngleStep  = 10;
const byte thirdServoPin        = 7;
byte lastSwitchStatethirdServo  = HIGH;
int thirdPosition               = 0;
byte thirdFSM                   = doNothing;
unsigned long thirdMillis;

//**********************************************
//forth servo stuff
Servo forthServo;  // index
const byte forthServoSwitch     = 8;
const byte forthServoAngle      = 170;
const byte forthServoAngleStep  = 10;
const byte forthServoPin        = 9;
byte lastSwitchStateforthServo  = HIGH;
int forthPosition               = 0;
byte forthFSM                   = doNothing;
unsigned long forthMillis;

//***********************************************
//fifth servo stuff
Servo fifthServo;  // thumb
const byte fifthServoSwitch     = 10;
const byte fifthServoAngle      = 170;
const byte fifthServoAngleStep  = 10;
const byte fifthServoPin        = 11;
byte lastSwitchStatefifthServo  = HIGH;
int fifthPosition               = 0;
byte fifthFSM                   = doNothing;
unsigned long fifthMillis;

//timing stuff
unsigned long heartbeatMillis;
unsigned long switchMillis;


//*******************************************************************
void setup()
{
  pinMode(heartbeatLED, OUTPUT);

  firstServo.attach(firstServoPin);
  firstServo.write(0);
  pinMode(firstServoSwitch, INPUT_PULLUP);

  secondServo.attach(secondServoPin);
  secondServo.write(0);
  pinMode(secondServoSwitch, INPUT_PULLUP);

  thirdServo.attach(thirdServoPin);
  thirdServo.write(0);
  pinMode(thirdServoSwitch, INPUT_PULLUP);

  forthServo.attach(forthServoPin);
  forthServo.write(0);
  pinMode(forthServoSwitch, INPUT_PULLUP);

  fifthServo.attach(fifthServoPin);
  fifthServo.write(0);
  pinMode(fifthServoSwitch, INPUT_PULLUP);

} //END of setup()

//*******************************************************************
void loop()
{
  //*******************************
  //time to toggle the heartbeatLED ?
  if (millis() - heartbeatMillis >= 250)
  {
    //retart the Timer
    heartbeatMillis = millis();

    digitalWrite(heartbeatLED, !digitalRead(heartbeatLED));
  }

  //*******************************
  //time to check the switches ?
  if (millis() - switchMillis >= 50)
  {
    //retart the Timer
    switchMillis = millis();

    checkSwitches();
  }

  //*******************************
  //check the FSM
  checkFSM();

} //END of loop()


//*******************************************************************
//Finite State Machine (FSM)
void checkFSM()
{
  //***********************************************************************************************************1
  //first state machine
  switch (firstFSM)
  {
    //***********
    case doNothing:
      {
        //do nothing
      }
      break;

    //***********
    case goingToMax:
      if (millis() - firstMillis >= 20)
      {
        //restart the TIMER
        firstMillis = millis();

        //move servo
        firstServo.write(firstPosition);

        //next position
        firstPosition = firstPosition + firstServoAngleStep;

        //at maximum ?
        if (firstPosition > firstServoAngle)
        {
          firstPosition = firstServoAngle;
          firstServo.write(firstPosition);

          //we are finished
          firstFSM = doNothing;
        }

      }
      break;

    //***********
    case goingToMin:
      if (millis() - firstMillis >= 20)
      {
        //restart the TIMER
        firstMillis = millis();

        //move servo
        firstServo.write(firstPosition);

        //next position
        firstPosition = firstPosition - firstServoAngleStep;

        //at minimum ?
        if (firstPosition <= 0)
        {
          firstPosition = 0;
          firstServo.write(firstPosition);

          //we are finished
          firstFSM = doNothing;
        }

      }
      break;
  }

  //*******************************************************************************************************2
  //second state machine
switch (secondFSM)
  {
    //***********
    case doNothing:
      {
        //do nothing
      }
      break;

    //***********
    case goingToMax:
      if (millis() - secondMillis >= 20)
      {
        //restart the TIMER
        secondMillis = millis();

        //move servo
        secondServo.write(secondPosition);

        //next position
        secondPosition = secondPosition + secondServoAngleStep;

        //at maximum ?
        if (secondPosition > secondServoAngle)
        {
          secondPosition = secondServoAngle;
          secondServo.write(secondPosition);

          //we are finished
          secondFSM = doNothing;
        }

      }
      break;

    //***********
    case goingToMin:
      if (millis() - secondMillis >= 20)
      {
        //restart the TIMER
        secondMillis = millis();

        //move servo
        secondServo.write(secondPosition);

        //next position
        secondPosition = secondPosition - secondServoAngleStep;

        //at minimum ?
        if (secondPosition <= 0)
        {
          secondPosition = 0;
          secondServo.write(secondPosition);

          //we are finished
          secondFSM = doNothing;
        }

      }
      break;
  }
  //********************************************************************************************* 3
  // third state machine
switch (thirdFSM)
  {
    //***********
    case doNothing:
      {
        //do nothing
      }
      break;

    //***********
    case goingToMax:
      if (millis() - thirdMillis >= 20)
      {
        //restart the TIMER
        thirdMillis = millis();

        //move servo
        thirdServo.write(thirdPosition);

        //next position
        thirdPosition = thirdPosition + thirdServoAngleStep;

        //at maximum ?
        if (thirdPosition > thirdServoAngle)
        {
          thirdPosition = thirdServoAngle;
          thirdServo.write(thirdPosition);

          //we are finished
          thirdFSM = doNothing;
        }

      }
      break;

    //***********
    case goingToMin:
      if (millis() - thirdMillis >= 20)
      {
        //restart the TIMER
        thirdMillis = millis();

        //move servo
        thirdServo.write(thirdPosition);

        //next position
        thirdPosition = thirdPosition - thirdServoAngleStep;

        //at minimum ?
        if (thirdPosition <= 0)
        {
          thirdPosition = 0;
          thirdServo.write(thirdPosition);

          //we are finished
          thirdFSM = doNothing;
        }

      }
      break;
  }

//************************************************************************************************************** 4
// forth state machine
switch (forthFSM)
  {
    //***********
    case doNothing:
      {
        //do nothing
      }
      break;

    //***********
    case goingToMax:
      if (millis() - forthMillis >= 20)
      {
        //restart the TIMER
        forthMillis = millis();

        //move servo
        forthServo.write(forthPosition);

        //next position
        forthPosition = forthPosition + forthServoAngleStep;

        //at maximum ?
        if (forthPosition > forthServoAngle)
        {
          forthPosition = forthServoAngle;
          forthServo.write(forthPosition);

          //we are finished
          forthFSM = doNothing;
        }

      }
      break;

    //***********
    case goingToMin:
      if (millis() - forthMillis >= 20)
      {
        //restart the TIMER
        forthMillis = millis();

        //move servo
        forthServo.write(forthPosition);

        //next position
        forthPosition = forthPosition - forthServoAngleStep;

        //at minimum ?
        if (forthPosition <= 0)
        {
          forthPosition = 0;
          forthServo.write(forthPosition);

          //we are finished
          forthFSM = doNothing;
        }

      }
      break;
  }

//*************************************************************************************************************5
// fifth state machine
switch (fifthFSM)
  {
    //***********
    case doNothing:
      {
        //do nothing
      }
      break;

    //***********
    case goingToMax:
      if (millis() - fifthMillis >= 20)
      {
        //restart the TIMER
        fifthMillis = millis();

        //move servo
        fifthServo.write(fifthPosition);

        //next position
        fifthPosition = fifthPosition + fifthServoAngleStep;

        //at maximum ?
        if (fifthPosition > fifthServoAngle)
        {
          fifthPosition = fifthServoAngle;
          fifthServo.write(fifthPosition);

          //we are finished
          fifthFSM = doNothing;
        }

      }
      break;

    //***********
    case goingToMin:
      if (millis() - fifthMillis >= 20)
      {
        //restart the TIMER
        fifthMillis = millis();

        //move servo
        fifthServo.write(fifthPosition);

        //next position
        fifthPosition = fifthPosition - fifthServoAngleStep;

        //at minimum ?
        if (fifthPosition <= 0)
        {
          fifthPosition = 0;
          fifthServo.write(fifthPosition);

          //we are finished
          fifthFSM = doNothing;
        }

      }
      break;
  }

//*************************************************************************************************************DONE

} //END of checkFSM()


//*******************************************************************1
// first servo switch?
void checkSwitches()
{
  byte state = digitalRead(firstServoSwitch);

  //******************************
  //first servo switch 
  if (lastSwitchStatefirstServo != state)
  {
    lastSwitchStatefirstServo = state;

    //***************
    //first servo switch is pushed ?
    if (state == LOW)
    {
      //restart the TIMER
      firstMillis = millis();

      firstFSM = goingToMax;
    }

    //***************
    //first servo switch is released ?
    else
    {
      //restart the TIMER
      firstMillis = millis();

      firstFSM = goingToMin;
    }
  }

  //***********************************************************2
  //second servo switch?
{
  byte state = digitalRead(secondServoSwitch);

  //******************************
  //second servo switch 
  if (lastSwitchStatesecondServo != state)
  {
    lastSwitchStatesecondServo = state;

    //***************
    //second servo switch is pushed ?
    if (state == LOW)
    {
      //restart the TIMER
      secondMillis = millis();

      secondFSM = goingToMax;
    }

    //***************
    //second servo switch is released ?
    else
    {
      //restart the TIMER
      secondMillis = millis();

      secondFSM = goingToMin;
    }
  }

  //***********************************************************3
    //third servo switch
{
  byte state = digitalRead(thirdServoSwitch);

  //******************************
  //third servo switch 
  if (lastSwitchStatethirdServo != state)
  {
    lastSwitchStatethirdServo = state;

    //***************
    //third servo switch is pushed ?
    if (state == LOW)
    {
      //restart the TIMER
      thirdMillis = millis();

      thirdFSM = goingToMax;
    }

    //***************
    //third servo switch is released ?
    else
    {
      //restart the TIMER
      thirdMillis = millis();

      thirdFSM = goingToMin;
    }
  }

  //***********************************************************4
    //forth servo switch
{
  byte state = digitalRead(forthServoSwitch);

  //******************************
  //first servo switch 
  if (lastSwitchStateforthServo != state)
  {
    lastSwitchStateforthServo = state;

    //***************
    //forth servo switch is pushed ?
    if (state == LOW)
    {
      //restart the TIMER
      forthMillis = millis();

      forthFSM = goingToMax;
    }

    //***************
    //forth servo switch is released ?
    else
    {
      //restart the TIMER
      forthMillis = millis();

      forthFSM = goingToMin;
    }
  }

  //************************************************************5
    //fifth servo switch
{
  byte state = digitalRead(fifthServoSwitch);

  //******************************
  //fifth servo switch 
  if (lastSwitchStatefifthServo != state)
  {
    lastSwitchStatefifthServo = state;

    //***************
    //fifth servo switch is pushed ?
    if (state == LOW)
    {
      //restart the TIMER
      fifthMillis = millis();

      fifthFSM = goingToMax;
    }

    //***************
    //fifth servo switch is released ?
    else
    {
      //restart the TIMER
      fifthMillis = millis();

      fifthFSM = goingToMin;
    }
  }

  //******************************


}//END of checkSwitches()


//*******************************************************************

(Code tags added by Moderator - again!)

Servo_buttons_code_.ino (13.7 KB)

here is the file

1 Like

Hello,
take a look in the usage of the struct{} instruction and combine a essential information in this data structure. This data structure should contain the pin information and the destination infomation for the servo.
Now you need a button handler and an operation part to do operation on this information, that´s all.

You had several misplaced braces }

You take directions very well, congratulations.

Now you do realize that it is up to you to study the workings of the sketch and learn how it functions.

As mentioned, there are ways to make things easier on yourself when writing a program i.e. using a struct{ } and perhaps a class.



BTW
Always use CTRL T to format your sketches as this will make them easier to read.

//Forum https://forum.arduino.cc/t/controlling-individual-servos-with-individual-buttons/861547?u=larryd
//
//Non blocking servo control of 5 servos

//Version  YYMMDD   Description
// 1.00    210513   functional sketch
// 1.02    210513   made non-blocking
// 1.03    210514   added 4 more servos, total of 5

#include <Servo.h>

#define doNothing                 0
#define goingToMax                1
#define goingToMin                2

const byte heartbeatLED         = 13;

//****************************************
//first servo stuff
Servo firstServo;                 // pinky
const byte firstServoSwitch     = 2;
const byte firstServoAngle      = 170;
const byte firstServoAngleStep  = 10;
const byte firstServoPin        = 3;
byte lastSwitchStatefirstServo  = HIGH;
int firstPosition               = 0;
byte firstFSM                   = doNothing;
unsigned long firstMillis;

//****************************************
//second servo stuff
Servo secondServo;                 // ring
const byte secondServoSwitch     = 4;
const byte secondServoAngle      = 170;
const byte secondServoAngleStep  = 10;
const byte secondServoPin        = 5;
byte lastSwitchStatesecondServo  = HIGH;
int secondPosition               = 0;
byte secondFSM                   = doNothing;
unsigned long secondMillis;

//*********************************************
//third servo stuff
Servo thirdServo;                 // middle
const byte thirdServoSwitch     = 6;
const byte thirdServoAngle      = 170;
const byte thirdServoAngleStep  = 10;
const byte thirdServoPin        = 7;
byte lastSwitchStatethirdServo  = HIGH;
int thirdPosition               = 0;
byte thirdFSM                   = doNothing;
unsigned long thirdMillis;

//**********************************************
//fourth servo stuff
Servo fourthServo;                 // index
const byte fourthServoSwitch     = 8;
const byte fourthServoAngle      = 170;
const byte fourthServoAngleStep  = 10;
const byte fourthServoPin        = 9;
byte lastSwitchStatefourthServo  = HIGH;
int fourthPosition               = 0;
byte fourthFSM                   = doNothing;
unsigned long fourthMillis;

//***********************************************
//fifth servo stuff
Servo fifthServo;                 // thumb
const byte fifthServoSwitch     = 10;
const byte fifthServoAngle      = 170;
const byte fifthServoAngleStep  = 10;
const byte fifthServoPin        = 11;
byte lastSwitchStatefifthServo  = HIGH;
int fifthPosition               = 0;
byte fifthFSM                   = doNothing;
unsigned long fifthMillis;

//***********************************************
//timing stuff
unsigned long heartbeatMillis;
unsigned long switchMillis;


//*******************************************************************
void setup()
{
  pinMode(heartbeatLED, OUTPUT);

  //first servo
  firstServo.attach(firstServoPin);
  firstServo.write(0);
  pinMode(firstServoSwitch, INPUT_PULLUP);

  //second servo
  secondServo.attach(secondServoPin);
  secondServo.write(0);
  pinMode(secondServoSwitch, INPUT_PULLUP);

  //third servo
  thirdServo.attach(thirdServoPin);
  thirdServo.write(0);
  pinMode(thirdServoSwitch, INPUT_PULLUP);

  //fourth servo
  fourthServo.attach(fourthServoPin);
  fourthServo.write(0);
  pinMode(fourthServoSwitch, INPUT_PULLUP);

  //fifth servo
  fifthServo.attach(fifthServoPin);
  fifthServo.write(0);
  pinMode(fifthServoSwitch, INPUT_PULLUP);

} //END of setup()

//*******************************************************************
void loop()
{
  //*******************************
  //time to toggle the heartbeatLED ?
  if (millis() - heartbeatMillis >= 250)
  {
    //retart the Timer
    heartbeatMillis = millis();

    //toggle the LED
    digitalWrite(heartbeatLED, !digitalRead(heartbeatLED));
  }

  //*******************************
  //time to check the switches ?
  if (millis() - switchMillis >= 50)
  {
    //retart the Timer
    switchMillis = millis();

    checkSwitches();
  }

  //*******************************
  //check the FSM
  checkFSM();

} //END of loop()


//*******************************************************************
//Finite State Machine (FSM)
void checkFSM()
{
  //******************************************                          1
  //first state machine
  switch (firstFSM)
  {
    //***********
    case doNothing:
      {
        //do nothing
      }
      break;

    //***********
    case goingToMax:
      if (millis() - firstMillis >= 20)
      {
        //restart the TIMER
        firstMillis = millis();

        //move servo
        firstServo.write(firstPosition);

        //next position
        firstPosition = firstPosition + firstServoAngleStep;

        //at maximum ?
        if (firstPosition > firstServoAngle)
        {
          firstPosition = firstServoAngle;
          firstServo.write(firstPosition);

          //we are finished
          firstFSM = doNothing;
        }
      }
      break;

    //***********
    case goingToMin:
      if (millis() - firstMillis >= 20)
      {
        //restart the TIMER
        firstMillis = millis();

        //move servo
        firstServo.write(firstPosition);

        //next position
        firstPosition = firstPosition - firstServoAngleStep;

        //at minimum ?
        if (firstPosition <= 0)
        {
          firstPosition = 0;
          firstServo.write(firstPosition);

          //we are finished
          firstFSM = doNothing;
        }
      }
      break;
      
  } //END of switch/case

  //******************************************                          2
  //second state machine
  switch (secondFSM)
  {
    //***********
    case doNothing:
      {
        //do nothing
      }
      break;

    //***********
    case goingToMax:
      if (millis() - secondMillis >= 20)
      {
        //restart the TIMER
        secondMillis = millis();

        //move servo
        secondServo.write(secondPosition);

        //next position
        secondPosition = secondPosition + secondServoAngleStep;

        //at maximum ?
        if (secondPosition > secondServoAngle)
        {
          secondPosition = secondServoAngle;
          secondServo.write(secondPosition);

          //we are finished
          secondFSM = doNothing;
        }
      }
      break;

    //***********
    case goingToMin:
      if (millis() - secondMillis >= 20)
      {
        //restart the TIMER
        secondMillis = millis();

        //move servo
        secondServo.write(secondPosition);

        //next position
        secondPosition = secondPosition - secondServoAngleStep;

        //at minimum ?
        if (secondPosition <= 0)
        {
          secondPosition = 0;
          secondServo.write(secondPosition);

          //we are finished
          secondFSM = doNothing;
        }
      }
      break;
      
  } //END of switch/case

  //******************************************                          3
  // third state machine
  switch (thirdFSM)
  {
    //***********
    case doNothing:
      {
        //do nothing
      }
      break;

    //***********
    case goingToMax:
      if (millis() - thirdMillis >= 20)
      {
        //restart the TIMER
        thirdMillis = millis();

        //move servo
        thirdServo.write(thirdPosition);

        //next position
        thirdPosition = thirdPosition + thirdServoAngleStep;

        //at maximum ?
        if (thirdPosition > thirdServoAngle)
        {
          thirdPosition = thirdServoAngle;
          thirdServo.write(thirdPosition);

          //we are finished
          thirdFSM = doNothing;
        }
      }
      break;

    //***********
    case goingToMin:
      if (millis() - thirdMillis >= 20)
      {
        //restart the TIMER
        thirdMillis = millis();

        //move servo
        thirdServo.write(thirdPosition);

        //next position
        thirdPosition = thirdPosition - thirdServoAngleStep;

        //at minimum ?
        if (thirdPosition <= 0)
        {
          thirdPosition = 0;
          thirdServo.write(thirdPosition);

          //we are finished
          thirdFSM = doNothing;
        }
      }
      break;
      
  } //END of switch/case

  //******************************************                          4
  // fourth state machine
  switch (fourthFSM)
  {
    //***********
    case doNothing:
      {
        //do nothing
      }
      break;

    //***********
    case goingToMax:
      if (millis() - fourthMillis >= 20)
      {
        //restart the TIMER
        fourthMillis = millis();

        //move servo
        fourthServo.write(fourthPosition);

        //next position
        fourthPosition = fourthPosition + fourthServoAngleStep;

        //at maximum ?
        if (fourthPosition > fourthServoAngle)
        {
          fourthPosition = fourthServoAngle;
          fourthServo.write(fourthPosition);

          //we are finished
          fourthFSM = doNothing;
        }
      }
      break;

    //***********
    case goingToMin:
      if (millis() - fourthMillis >= 20)
      {
        //restart the TIMER
        fourthMillis = millis();

        //move servo
        fourthServo.write(fourthPosition);

        //next position
        fourthPosition = fourthPosition - fourthServoAngleStep;

        //at minimum ?
        if (fourthPosition <= 0)
        {
          fourthPosition = 0;
          fourthServo.write(fourthPosition);

          //we are finished
          fourthFSM = doNothing;
        }
      }
      break;
      
  } //END of switch/case

  //******************************************                          5
  // fifth state machine
  switch (fifthFSM)
  {
    //***********
    case doNothing:
      {
        //do nothing
      }
      break;

    //***********
    case goingToMax:
      if (millis() - fifthMillis >= 20)
      {
        //restart the TIMER
        fifthMillis = millis();

        //move servo
        fifthServo.write(fifthPosition);

        //next position
        fifthPosition = fifthPosition + fifthServoAngleStep;

        //at maximum ?
        if (fifthPosition > fifthServoAngle)
        {
          fifthPosition = fifthServoAngle;
          fifthServo.write(fifthPosition);

          //we are finished
          fifthFSM = doNothing;
        }
      }
      break;

    //***********
    case goingToMin:
      if (millis() - fifthMillis >= 20)
      {
        //restart the TIMER
        fifthMillis = millis();

        //move servo
        fifthServo.write(fifthPosition);

        //next position
        fifthPosition = fifthPosition - fifthServoAngleStep;

        //at minimum ?
        if (fifthPosition <= 0)
        {
          fifthPosition = 0;
          fifthServo.write(fifthPosition);

          //we are finished
          fifthFSM = doNothing;
        }
      }
      break;
      
  } //END of switch/case
  
  //******************************************                      DONE

} //END of checkFSM()


//*******************************************************************
void checkSwitches()
{
  //reusable variable
  byte state;
  
  //******************************************                          1
  //change state first servo switch ?
  state = digitalRead(firstServoSwitch);

  //******************************
  if (lastSwitchStatefirstServo != state)
  {
    //update to the new state
    lastSwitchStatefirstServo = state;

    //***************
    //first servo switch is pushed ?
    if (state == LOW)
    {
      //restart the TIMER
      firstMillis = millis();

      firstFSM = goingToMax;
    }

    //***************
    //first servo switch is released ?
    else
    {
      //restart the TIMER
      firstMillis = millis();

      firstFSM = goingToMin;
    }
  }

  //******************************************                          2
  //change state second servo switch ?
  state = digitalRead(secondServoSwitch);

  //******************************
  if (lastSwitchStatesecondServo != state)
  {
    //update to the new state
    lastSwitchStatesecondServo = state;

    //***************
    //second servo switch is pushed ?
    if (state == LOW)
    {
      //restart the TIMER
      secondMillis = millis();

      secondFSM = goingToMax;
    }

    //***************
    //second servo switch is released ?
    else
    {
      //restart the TIMER
      secondMillis = millis();

      secondFSM = goingToMin;
    }
  }

  //******************************************                          3
  //change state third servo switch ?
  state = digitalRead(thirdServoSwitch);

  //******************************
  if (lastSwitchStatethirdServo != state)
  {
    //update to the new state
    lastSwitchStatethirdServo = state;

    //***************
    //third servo switch is pushed ?
    if (state == LOW)
    {
      //restart the TIMER
      thirdMillis = millis();

      thirdFSM = goingToMax;
    }

    //***************
    //third servo switch is released ?
    else
    {
      //restart the TIMER
      thirdMillis = millis();

      thirdFSM = goingToMin;
    }
  }

  //******************************************                          4
  //change state fourth servo switch ?
  state = digitalRead(fourthServoSwitch);

  //******************************
  if (lastSwitchStatefourthServo != state)
  {
    //update to the new state
    lastSwitchStatefourthServo = state;

    //***************
    //fourth servo switch is pushed ?
    if (state == LOW)
    {
      //restart the TIMER
      fourthMillis = millis();

      fourthFSM = goingToMax;
    }

    //***************
    //fourth servo switch is released ?
    else
    {
      //restart the TIMER
      fourthMillis = millis();

      fourthFSM = goingToMin;
    }
  }

  //******************************************                          5
  //change state fifth servo switch ?
  state = digitalRead(fifthServoSwitch);

  //******************************
  if (lastSwitchStatefifthServo != state)
  {
    //update to the new state
    lastSwitchStatefifthServo = state;

    //***************
    //fifth servo switch is pushed ?
    if (state == LOW)
    {
      //restart the TIMER
      fifthMillis = millis();

      fifthFSM = goingToMax;
    }

    //***************
    //fifth servo switch is released ?
    else
    {
      //restart the TIMER
      fifthMillis = millis();

      fifthFSM = goingToMin;
    }
  }

  //******************************


}//END of checkSwitches()


//*******************************************************************



And

You cannot power your 5 servos from the USB cable or the Arduino 5v pin.

You must use an external 5v (6v) power supply; the GND must be common with the Arduino GND.



thank you very much, is it okay to use a 6v power supply to power the servos or will it harm them? would i just have to use a separate 5v power supply?