 # Servo: how to hold position at a certain degree and then go back to the original state?

What I EXACTLY (sorry for explaining badly yesterday) is:

Servo1 Move to 0 degrees
Servo2 Move to 0 degrees
Move servo1 slowly to 5 degrees
Move servo2 to 45 degrees
Hold that position for 3 seconds
Move servo1 slowly to 0 degrees
Move servo2 to 0 degrees
Hold that position for 3 seconds
Move servo1 slowly to 10 degrees
Move servo2 to 45 degrees
Hold that position for 3 seconds
Move servo1 slowly to 0 degrees
Move servo2 to 0 degrees
Hold that position for 3 seconds
Move servo1 slowly to 15 degrees
Move servo2 to 45 degrees
Hold that position for 3 seconds
Move servo1 slowly to 0 degrees
Move servo2 to 0 degrees
Hold that position for 3 seconds

Look, I tried adding JUST the part where de second servo moves to 0 degrees and 45 degrees and when I compile it, it doesn’t work, it gets stuck at only moving from 0 degrees to 5 degrees and viceversa.

``````
#include <Servo.h>

const byte ServoPin = 4;

Servo servo4;
Servo servo5;

const unsigned long MillisecondsPerStep = 250; // four degrees per second

void setup()

{

servo4.attach(ServoPin);
servo5.attach(5);

}

void loop()

{

ServoStateMachine();

// Other stuff goes here

}

void ServoStateMachine()

{

static byte state = 0;

static byte position = 72;    //In my servo 72º=0º, 60º=5º, 52º=10º and 44º=15º, that's why I changed it

static unsigned long timerStart;

switch (state)

{

case 0: // Put the servo at 0 degrees

position = 72;

servo4.write(position);

servo5.write(0);

timerStart = millis();

state++;

break;

case 1: // Move slowly the servo to 5 degrees

if (millis() - timerStart >= MillisecondsPerStep)

{

timerStart = millis();

position--;

servo4.write(position);

}

if (position == 60) // Arrived

{

timerStart = millis();

servo5.write(45);

state++;

}

break;

case 2: // and hold that position for 3 seconds

if (millis() - timerStart >= 3000)

{

// Go back to 0 degrees

position = 72;

servo4.write(position);

servo5.write(0);

timerStart = millis();

state++;

}

break;

case 3:  // Move slowly to 10 degrees...

if (millis() - timerStart >= MillisecondsPerStep)

{

timerStart = millis();

position--;

servo4.write(position);

}

if (position == 52) // Arrived

{

timerStart = millis();

servo5.write(45);

state++;

}

break;

case 4: // ...and hold for 3 seconds

if (millis() - timerStart >= 3000)

{

// Go back to 0 degrees

position = 72;

servo4.write(position);

servo5.write(0);

timerStart = millis();

state++;

}

break;

case 5:  // Move slowly to 15 degrees

if (millis() - timerStart >= MillisecondsPerStep)

{

timerStart = millis();

position--;

servo4.write(position);

}

if (position == 44) // Arrived

{

timerStart = millis();

servo5.write(45);

state = 0;  // back to the beginning

}

break;

}

}

``````

So this means there is a sequence of moving two servos sometimes slowly sometimes fast
If servos have reached the target position hold this position for a defined period of time.

and this basic pattern is repeated multiple times.

As soon as you have understood how to code

• moving a servo slowly

• making your code wait a defined interval and go on with the next step when this interval of time has passed by

• programming a state-machine (which realises this step by step-sequence)

you will have learned how to code it. With having this know-how it is just a matter or a question of diligence to code a sequence with hundreds or thousands of such steps.

Posting the sequence to me looks like a hidden request saying
"can you post the ready to use code that does this sequence?"

## NO !

I won't post such a code. I'm teaching how to fish and how to cook.
I don't serve ready to eat fish-meals.

As soon as you start posting an attempt how the code might look like and ask a specific question you have my support.

## EDIT:

well done posting your attempt trying to code it. Let me analyse it and I will post again...
best regards Stefan

So here is a code-version with some additional things that enable debugging
I have added two macros that print to the serial monitor which makes visible what the code is doing

I moved the variables state and position to be global

open the serial monitor adjust baud to 115200
activate the check box autoscroll
activate the check box show timestamp

If the programm has run down the state-machine two times
DE-activate checkbox autoscroll or unplug the arduino
then analyse the lines printed in the serial monitor to see which steps were
executed and which values variable state and position have.

Very old programmer-wisdom:

a program does always what you have programmed. If the program does something different than you expected you don't yet fully understand what you have programmed.

``````#define dbg(myFixedText, variableName) \
Serial.print( F(#myFixedText " "  #variableName"=") ); \
Serial.println(variableName);
// usage: dbg("1:my fixed text",myVariable);
// myVariable can be any variable or expression that is defined in scope

#define dbgi(myFixedText, variableName,timeInterval) \
do { \
static unsigned long intervalStartTime; \
if ( millis() - intervalStartTime >= timeInterval ){ \
intervalStartTime = millis(); \
Serial.print( F(#myFixedText " "  #variableName"=") ); \
Serial.println(variableName); \
} \
} while (false);
// usage: dbgi("2:my fixed text",myVar,myInterval);
// myVar can be any variable or expression that is defined in scope
// myInterval is the time-interval which must pass by before the next
// print is executed

#include <Servo.h>

const byte ServoPin = 4;

Servo servo4;
Servo servo5;

const unsigned long MillisecondsPerStep = 250; // four degrees per second

// defining variables as GLOBAL
byte state;
byte position;    //In my servo 72º=0º, 60º=5º, 52º=10º and 44º=15º, that's why I changed it

void setup() {
Serial.begin(115200);
Serial.println( F("Setup-Start") );
// initialise variable state with value for start
state = 0;

servo4.attach(ServoPin);
servo5.attach(5);
}

void loop() {
ServoStateMachine();
// Other stuff goes here
}

void ServoStateMachine() {

static unsigned long timerStart;

dbgi("above switch",state,500);
switch (state)  {

case 0: // Put the servo at 0 degrees
position = 72;    //In my servo 72º=0º, 60º=5º, 52º=10º and 44º=15º, that's why I changed it

servo4.write(position);
servo5.write(0);
dbg("case 0 servo4",position);
dbg("case 0 servo5 0",0);

timerStart = millis();
state++; // change to next state with the next call of the function
break;

case 1: // Move slowly the servo to 5 degrees

if (millis() - timerStart >= MillisecondsPerStep) {
timerStart = millis();
position--;

servo4.write(position);
dbg("case 1 servo4",position);
}

if (position == 60) { // Arrived
dbg("case 1 servo4",position);
dbg("case 1 servo5 45",0);
timerStart = millis();
servo5.write(45);
state++;
}
break;

case 2: // and hold that position for 3 seconds
if (millis() - timerStart >= 3000)    {
// Go back to 0 degrees
position = 72;
servo4.write(position);
servo5.write(0);
dbg("case 2 servo4",position);
dbg("case 2 servo5 0",0);

timerStart = millis();
state++;
}
break;

case 3:  // Move slowly to 10 degrees...

if (millis() - timerStart >= MillisecondsPerStep) {
timerStart = millis();
position--;
servo4.write(position);
dbg("case 3 servo4",position);
}

if (position == 52) { // Arrived
timerStart = millis();
servo5.write(45);
dbg("case 3 servo4",position);
dbg("case 3 servo4 45",0);
state++;
}
break;

case 4: // ...and hold for 3 seconds

if (millis() - timerStart >= 3000)    {
// Go back to 0 degrees
position = 72;
servo4.write(position);
servo5.write(0);
dbg("case 4 servo4",position);
dbg("case 4 servo5 0",0);
timerStart = millis();
state++;
}
break;

case 5:  // Move slowly to 15 degrees

if (millis() - timerStart >= MillisecondsPerStep) {
timerStart = millis();
position--;
servo4.write(position);
dbg("case 5 servo4",position);
}

if (position == 44) { // Arrived
timerStart = millis();
servo5.write(45);
dbg("case 5 servo4",position);
dbg("case 5 servo5 45",0);
state = 0;  // back to the beginning
}
break;

}

}``````

best regards Stefan

1 Like

I did what you said and the program works like the other, I opened the serial monitor and it just stops in case 1, I’ve been looking at it for some time and I don’t understand why it stops

Well if you click in the upper right corner of the code-section of my to copy the entire code into the clipboard and you paste that into a empty *.ino-file let it compile and upload you should see this serial output

``````22:27:38.269 -> Setup-Start
22:27:38.269 -> "case 0 servo4" position=72
22:27:38.269 -> "case 0 servo5 0" 0=0
22:27:38.507 -> "case 1 servo4" position=71
22:27:38.752 -> "case 1 servo4" position=70
22:27:38.752 -> "above switch" state=1
22:27:39.007 -> "case 1 servo4" position=69
22:27:39.263 -> "above switch" state=1
22:27:39.263 -> "case 1 servo4" position=68
22:27:39.539 -> "case 1 servo4" position=67
22:27:39.755 -> "above switch" state=1
22:27:39.755 -> "case 1 servo4" position=66
22:27:40.008 -> "case 1 servo4" position=65
22:27:40.255 -> "case 1 servo4" position=64
22:27:40.255 -> "above switch" state=1
22:27:40.509 -> "case 1 servo4" position=63
22:27:40.756 -> "above switch" state=1
22:27:40.756 -> "case 1 servo4" position=62
22:27:41.010 -> "case 1 servo4" position=61
22:27:41.252 -> "above switch" state=1
22:27:41.252 -> "case 1 servo4" position=60
22:27:41.252 -> "case 1 servo4" position=60
22:27:41.252 -> "case 1 servo5 45" 0=0
22:27:41.757 -> "above switch" state=2
22:27:42.258 -> "above switch" state=2
22:27:42.774 -> "above switch" state=2
22:27:43.249 -> "above switch" state=2
22:27:43.778 -> "above switch" state=2
22:27:44.260 -> "above switch" state=2
22:27:44.260 -> "case 2 servo4" position=72
22:27:44.260 -> "case 2 servo5 0" 0=0
22:27:44.498 -> "case 3 servo4" position=71
22:27:44.777 -> "above switch" state=3
22:27:44.777 -> "case 3 servo4" position=70
22:27:45.014 -> "case 3 servo4" position=69
22:27:45.258 -> "above switch" state=3
22:27:45.258 -> "case 3 servo4" position=68
22:27:45.493 -> "case 3 servo4" position=67
22:27:45.768 -> "above switch" state=3
22:27:45.768 -> "case 3 servo4" position=66
22:27:46.016 -> "case 3 servo4" position=65
22:27:46.263 -> "above switch" state=3
22:27:46.263 -> "case 3 servo4" position=64
22:27:46.516 -> "case 3 servo4" position=63
22:27:46.763 -> "above switch" state=3
22:27:46.763 -> "case 3 servo4" position=62
22:27:47.017 -> "case 3 servo4" position=61
22:27:47.264 -> "above switch" state=3
22:27:47.264 -> "case 3 servo4" position=60
22:27:47.517 -> "case 3 servo4" position=59
22:27:47.756 -> "above switch" state=3
22:27:47.756 -> "case 3 servo4" position=58
22:27:48.000 -> "case 3 servo4" position=57
22:27:48.264 -> "above switch" state=3
22:27:48.264 -> "case 3 servo4" position=56
22:27:48.518 -> "case 3 servo4" position=55
22:27:48.756 -> "above switch" state=3
22:27:48.756 -> "case 3 servo4" position=54
22:27:49.022 -> "case 3 servo4" position=53
22:27:49.249 -> "above switch" state=3
22:27:49.249 -> "case 3 servo4" position=52
22:27:49.249 -> "case 3 servo4" position=52
22:27:49.249 -> "case 3 servo4 45" 0=0
22:27:49.759 -> "above switch" state=4
22:27:50.236 -> "above switch" state=4
22:27:50.768 -> "above switch" state=4
22:27:51.270 -> "above switch" state=4
22:27:51.732 -> "above switch" state=4
22:27:52.272 -> "above switch" state=4
22:27:52.272 -> "case 4 servo4" position=72
22:27:52.272 -> "case 4 servo5 0" 0=0
22:27:52.525 -> "case 5 servo4" position=71
22:27:52.772 -> "above switch" state=5
22:27:52.772 -> "case 5 servo4" position=70
22:27:52.997 -> "case 5 servo4" position=69
22:27:53.273 -> "above switch" state=5
22:27:53.273 -> "case 5 servo4" position=68
22:27:53.524 -> "case 5 servo4" position=67
22:27:53.773 -> "above switch" state=5
22:27:53.773 -> "case 5 servo4" position=66
22:27:54.026 -> "case 5 servo4" position=65
22:27:54.226 -> "above switch" state=5
22:27:54.263 -> "case 5 servo4" position=64
22:27:54.490 -> "case 5 servo4" position=63
22:27:54.727 -> "above switch" state=5
22:27:54.774 -> "case 5 servo4" position=62
22:27:54.986 -> "case 5 servo4" position=61
22:27:55.228 -> "above switch" state=5
22:27:55.274 -> "case 5 servo4" position=60
22:27:55.490 -> "case 5 servo4" position=59
22:27:55.728 -> "above switch" state=5
22:27:55.728 -> "case 5 servo4" position=58
22:27:56.028 -> "case 5 servo4" position=57
22:27:56.228 -> "above switch" state=5
22:27:56.267 -> "case 5 servo4" position=56
22:27:56.479 -> "case 5 servo4" position=55
22:27:56.745 -> "above switch" state=5
22:27:56.745 -> "case 5 servo4" position=54
22:27:57.029 -> "case 5 servo4" position=53
22:27:57.229 -> "above switch" state=5
22:27:57.229 -> "case 5 servo4" position=52
22:27:57.514 -> "case 5 servo4" position=51
22:27:57.730 -> "above switch" state=5
22:27:57.730 -> "case 5 servo4" position=50
22:27:57.977 -> "case 5 servo4" position=49
22:27:58.236 -> "above switch" state=5
22:27:58.236 -> "case 5 servo4" position=48
22:27:58.509 -> "case 5 servo4" position=47
22:27:58.731 -> "above switch" state=5
22:27:58.731 -> "case 5 servo4" position=46
22:27:59.032 -> "case 5 servo4" position=45
22:27:59.232 -> "above switch" state=5
22:27:59.279 -> "case 5 servo4" position=44
22:27:59.279 -> "case 5 servo4" position=44
22:27:59.279 -> "case 5 servo5 45" 0=0
22:27:59.279 -> "case 0 servo4" position=72
22:27:59.279 -> "case 0 servo5 0" 0=0
22:27:59.502 -> "case 1 servo4" position=71
22:27:59.733 -> "above switch" state=1
22:27:59.733 -> "case 1 servo4" position=70
22:27:59.980 -> "case 1 servo4" position=69
22:28:00.264 -> "above switch" state=1
22:28:00.264 -> "case 1 servo4" position=68
22:28:00.496 -> "case 1 servo4" position=67...
``````

This shows the statemachine is running through all 5 states setting the servos to the positions as the serial output prints

If your code does something different you aren't using an arduino Uno
or you did not copy the entire code
best regards Stefan

1 Like

Oh sorry, I don’t know why the first time I compiled it it didn’t work, now works well but for some reason, when the servo4 is at 44 (15 degrees), the servo5 doesn’t move to 45 degrees, it just remains at 0 degrees

I did copy & paste the code you have posted
This is the serial output I get

``````22:47:19.876 -> Setup-Start
22:47:19.876 -> "case 0 servo4" position=72
22:47:19.876 -> "case 0 servo5 0" 0=0
22:47:20.151 -> "case 1 servo4" position=71
22:47:20.367 -> "case 1 servo4" position=70
22:47:20.367 -> "above switch" state=1
22:47:20.620 -> "case 1 servo4" position=69
22:47:20.867 -> "above switch" state=1
22:47:20.867 -> "case 1 servo4" position=68
22:47:21.122 -> "case 1 servo4" position=67
22:47:21.388 -> "above switch" state=1
22:47:21.388 -> "case 1 servo4" position=66
22:47:21.638 -> "case 1 servo4" position=65
22:47:21.865 -> "case 1 servo4" position=64
22:47:21.865 -> "above switch" state=1
22:47:22.107 -> "case 1 servo4" position=63
22:47:22.371 -> "above switch" state=1
22:47:22.371 -> "case 1 servo4" position=62
22:47:22.625 -> "case 1 servo4" position=61
22:47:22.872 -> "above switch" state=1
22:47:22.872 -> "case 1 servo4" position=60
22:47:22.872 -> "case 1 servo4" position=60
22:47:22.872 -> "case 1 servo5 45" 0=0
22:47:23.357 -> "above switch" state=2
22:47:23.870 -> "above switch" state=2
22:47:24.395 -> "above switch" state=2
22:47:24.876 -> "above switch" state=2
22:47:25.392 -> "above switch" state=2
22:47:25.885 -> "above switch" state=2
22:47:25.885 -> "case 2 servo4" position=72
22:47:25.885 -> "case 2 servo5 0" 0=0
22:47:26.109 -> "case 3 servo4" position=71
22:47:26.379 -> "above switch" state=3
22:47:26.379 -> "case 3 servo4" position=70
22:47:26.631 -> "case 3 servo4" position=69
22:47:26.879 -> "above switch" state=3
22:47:26.879 -> "case 3 servo4" position=68
22:47:27.149 -> "case 3 servo4" position=67
22:47:27.380 -> "above switch" state=3
22:47:27.380 -> "case 3 servo4" position=66
22:47:27.642 -> "case 3 servo4" position=65
22:47:27.866 -> "above switch" state=3
22:47:27.866 -> "case 3 servo4" position=64
22:47:28.135 -> "case 3 servo4" position=63
22:47:28.382 -> "above switch" state=3
22:47:28.382 -> "case 3 servo4" position=62
22:47:28.636 -> "case 3 servo4" position=61
22:47:28.853 -> "above switch" state=3
22:47:28.908 -> "case 3 servo4" position=60
22:47:29.137 -> "case 3 servo4" position=59
22:47:29.384 -> "above switch" state=3
22:47:29.384 -> "case 3 servo4" position=58
22:47:29.638 -> "case 3 servo4" position=57
22:47:29.885 -> "above switch" state=3
22:47:29.885 -> "case 3 servo4" position=56
22:47:30.139 -> "case 3 servo4" position=55
22:47:30.358 -> "above switch" state=3
22:47:30.358 -> "case 3 servo4" position=54
22:47:30.640 -> "case 3 servo4" position=53
22:47:30.901 -> "above switch" state=3
22:47:30.901 -> "case 3 servo4" position=52
22:47:30.901 -> "case 3 servo4" position=52
22:47:30.901 -> "case 3 servo4 45" 0=0
22:47:31.358 -> "above switch" state=4
22:47:31.890 -> "above switch" state=4
22:47:32.344 -> "above switch" state=4
22:47:32.845 -> "above switch" state=4
22:47:33.362 -> "above switch" state=4
22:47:33.847 -> "above switch" state=4
22:47:33.893 -> "case 4 servo4" position=72
22:47:33.893 -> "case 4 servo5 0" 0=0
22:47:34.154 -> "case 5 servo4" position=71
22:47:34.379 -> "above switch" state=5
22:47:34.379 -> "case 5 servo4" position=70
22:47:34.649 -> "case 5 servo4" position=69
22:47:34.849 -> "above switch" state=5
22:47:34.849 -> "case 5 servo4" position=68
22:47:35.103 -> "case 5 servo4" position=67
22:47:35.382 -> "above switch" state=5
22:47:35.382 -> "case 5 servo4" position=66
22:47:35.599 -> "case 5 servo4" position=65
22:47:35.852 -> "above switch" state=5
22:47:35.852 -> "case 5 servo4" position=64
22:47:36.099 -> "case 5 servo4" position=63
22:47:36.384 -> "above switch" state=5
22:47:36.384 -> "case 5 servo4" position=62
22:47:36.601 -> "case 5 servo4" position=61
22:47:36.848 -> "above switch" state=5
22:47:36.848 -> "case 5 servo4" position=60
22:47:37.133 -> "case 5 servo4" position=59
22:47:37.388 -> "above switch" state=5
22:47:37.388 -> "case 5 servo4" position=58
22:47:37.601 -> "case 5 servo4" position=57
22:47:37.857 -> "above switch" state=5
22:47:37.857 -> "case 5 servo4" position=56
22:47:38.126 -> "case 5 servo4" position=55
22:47:38.374 -> "above switch" state=5
22:47:38.374 -> "case 5 servo4" position=54
22:47:38.605 -> "case 5 servo4" position=53
22:47:38.843 -> "above switch" state=5
22:47:38.843 -> "case 5 servo4" position=52
22:47:39.122 -> "case 5 servo4" position=51
22:47:39.359 -> "above switch" state=5
22:47:39.359 -> "case 5 servo4" position=50
22:47:39.623 -> "case 5 servo4" position=49
22:47:39.860 -> "above switch" state=5
22:47:39.860 -> "case 5 servo4" position=48
22:47:40.118 -> "case 5 servo4" position=47
22:47:40.345 -> "above switch" state=5
22:47:40.345 -> "case 5 servo4" position=46
22:47:40.608 -> "case 5 servo4" position=45
22:47:40.862 -> "above switch" state=5
22:47:40.862 -> "case 5 servo4" position=44
22:47:40.862 -> "case 5 servo4" position=44
22:47:40.862 -> "case 5 servo5 45" 0=0
22:47:40.862 -> "case 0 servo4" position=72
22:47:40.862 -> "case 0 servo5 0" 0=0
22:47:41.109 -> "case 1 servo4" position=71
22:47:41.341 -> "above switch" state=1
22:47:41.380 -> "case 1 servo4" position=70
22:47:41.610 -> "case 1 servo4" position=69
22:47:41.864 -> "above switch" state=1
22:47:41.864 -> "case 1 servo4" position=68
22:47:42.101 -> "case 1 servo4" position=67
22:47:42.396 -> "above switch" state=1
22:47:42.396 -> "case 1 servo4" position=66
22:47:42.612 -> "case 1 servo4" position=65
22:47:42.866 -> "above switch" state=1
22:47:42.866 -> "case 1 servo4" position=64
22:47:43.113 -> "case 1 servo4" position=63
22:47:43.367 -> "above switch" state=1
22:47:43.367 -> "case 1 servo4" position=62
22:47:43.614 -> "case 1 servo4" position=61
22:47:43.868 -> "above switch" state=1
22:47:43.868 -> "case 1 servo4" position=60
22:47:43.868 -> "case 1 servo4" position=60
22:47:43.868 -> "case 1 servo5 45" 0=0
22:47:44.400 -> "above switch" state=2
22:47:44.870 -> "above switch" state=2
22:47:45.352 -> "above switch" state=2
22:47:45.872 -> "above switch" state=2
22:47:46.335 -> "above switch" state=2
22:47:46.874 -> "above switch" state=2
22:47:46.874 -> "case 2 servo4" position=72
22:47:46.874 -> "case 2 servo5 0" 0=0
22:47:47.122 -> "case 3 servo4" position=71
22:47:47.340 -> "above switch" state=3
22:47:47.407 -> "case 3 servo4" position=70
22:47:47.623 -> "case 3 servo4" position=69
22:47:47.882 -> "above switch" state=3
22:47:47.882 -> "case 3 servo4" position=68
22:47:48.108 -> "case 3 servo4" position=67
22:47:48.408 -> "above switch" state=3
22:47:48.408 -> "case 3 servo4" position=66
22:47:48.619 -> "case 3 servo4" position=65
22:47:48.833 -> "above switch" state=3
22:47:48.880 -> "case 3 servo4" position=64
22:47:49.096 -> "case 3 servo4" position=63
22:47:49.329 -> "above switch" state=3
22:47:49.397 -> "case 3 servo4" position=62
22:47:49.628 -> "case 3 servo4" position=61
22:47:49.829 -> "above switch" state=3
22:47:49.867 -> "case 3 servo4" position=60
22:47:50.099 -> "case 3 servo4" position=59
22:47:50.331 -> "above switch" state=3
22:47:50.398 -> "case 3 servo4" position=58
22:47:50.627 -> "case 3 servo4" position=57
22:47:50.832 -> "above switch" state=3
22:47:50.885 -> "case 3 servo4" position=56
22:47:51.141 -> "case 3 servo4" position=55
22:47:51.368 -> "above switch" state=3
22:47:51.368 -> "case 3 servo4" position=54
22:47:51.633 -> "case 3 servo4" position=53
22:47:51.834 -> "above switch" state=3
22:47:51.887 -> "case 3 servo4" position=52
22:47:51.887 -> "case 3 servo4" position=52
22:47:51.887 -> "case 3 servo4 45" 0=0
22:47:52.351 -> "above switch" state=4
22:47:52.836 -> "above switch" state=4
22:47:53.370 -> "above switch" state=4
22:47:53.851 -> "above switch" state=4
22:47:54.369 -> "above switch" state=4
22:47:54.838 -> "above switch" state=4
22:47:54.838 -> "case 4 servo4" position=72
22:47:54.838 -> "case 4 servo5 0" 0=0
22:47:55.092 -> "case 5 servo4" position=71
22:47:55.343 -> "above switch" state=5
22:47:55.343 -> "case 5 servo4" position=70
22:47:55.608 -> "case 5 servo4" position=69
22:47:55.837 -> "above switch" state=5
22:47:55.837 -> "case 5 servo4" position=68
22:47:56.093 -> "case 5 servo4" position=67
22:47:56.341 -> "above switch" state=5
22:47:56.341 -> "case 5 servo4" position=66
22:47:56.594 -> "case 5 servo4" position=65
22:47:56.841 -> "above switch" state=5
22:47:56.841 -> "case 5 servo4" position=64
22:47:57.088 -> "case 5 servo4" position=63
22:47:57.326 -> "above switch" state=5
22:47:57.373 -> "case 5 servo4" position=62
22:47:57.596 -> "case 5 servo4" position=61
22:47:57.843 -> "above switch" state=5
22:47:57.843 -> "case 5 servo4" position=60
22:47:58.096 -> "case 5 servo4" position=59
22:47:58.344 -> "above switch" state=5
22:47:58.344 -> "case 5 servo4" position=58
22:47:58.597 -> "case 5 servo4" position=57
22:47:58.844 -> "above switch" state=5
22:47:58.844 -> "case 5 servo4" position=56
22:47:59.082 -> "case 5 servo4" position=55
22:47:59.345 -> "above switch" state=5
22:47:59.345 -> "case 5 servo4" position=54
22:47:59.598 -> "case 5 servo4" position=53
22:47:59.844 -> "above switch" state=5
22:47:59.844 -> "case 5 servo4" position=52
22:48:00.099 -> "case 5 servo4" position=51
22:48:00.356 -> "above switch" state=5
22:48:00.356 -> "case 5 servo4" position=50
22:48:00.584 -> "case 5 servo4" position=49
22:48:00.847 -> "above switch" state=5
22:48:00.847 -> "case 5 servo4" position=48
22:48:01.100 -> "case 5 servo4" position=47
22:48:01.348 -> "above switch" state=5
22:48:01.348 -> "case 5 servo4" position=46
22:48:01.601 -> "case 5 servo4" position=45
22:48:01.848 -> "above switch" state=5
22:48:01.848 -> "case 5 servo4" position=44
22:48:01.848 -> "case 5 servo4" position=44
22:48:01.848 -> "case 5 servo5 45" 0=0
22:48:01.848 -> "case 0 servo4" position=72
22:48:01.848 -> "case 0 servo5 0" 0=0
22:48:02.102 -> "case 1 servo4" position=71
22:48:02.340 -> "above switch" state=1
22:48:02.340 -> "case 1 servo4" position=70
22:48:02.603 -> "case 1 servo4" position=69
22:48:02.851 -> "above switch" state=1
22:48:02.851 -> "case 1 servo4" position=68
22:48:03.105 -> "case 1 servo4" position=67
22:48:03.352 -> "above switch" state=1
22:48:03.352 -> "case 1 servo4" position=66
22:48:03.584 -> "case 1 servo4" position=65
22:48:03.822 -> "above switch" state=1
22:48:03.869 -> "case 1 servo4" position=64
22:48:04.107 -> "case 1 servo4" position=63
22:48:04.349 -> "above switch" state=1
22:48:04.349 -> "case 1 servo4" position=62
22:48:04.608 -> "case 1 servo4" position=61
22:48:04.824 -> "above switch" state=1
22:48:04.874 -> "case 1 servo4" position=60
22:48:04.874 -> "case 1 servo4" position=60
22:48:04.874 -> "case 1 servo5 45" 0=0
22:48:05.357 -> "above switch" state=2
22:48:05.858 -> "above switch" state=2
22:48:06.318 -> "above switch" state=2
22:48:06.813 -> "above switch" state=2
22:48:07.314 -> "above switch" state=2
22:48:07.816 -> "above switch" state=2
22:48:07.863 -> "case 2 servo4" position=72
22:48:07.863 -> "case 2 servo5 0" 0=0
22:48:08.123 -> "case 3 servo4" position=71
22:48:08.349 -> "above switch" state=3
22:48:08.349 -> "case 3 servo4" position=70
22:48:08.618 -> "case 3 servo4" position=69
22:48:08.818 -> "above switch" state=3
22:48:08.861 -> "case 3 servo4" position=68
22:48:09.119 -> "case 3 servo4" position=67
22:48:09.335 -> "above switch" state=3
22:48:09.335 -> "case 3 servo4" position=66
``````

works as expected.

I decided to see you as a low-level troll. This means search for the bug on your own or with the help of others.

If you really would be careful the code you posted would work the same way as on my Arduino-Uno.
Good luck in finding what lazyness caused the different behaviour

Sorry for not posting it, I’m new to all this😅 But it’s the same code you posted, this one:

``````
#define dbg(myFixedText, variableName) \
Serial.print( F(#myFixedText " "  #variableName"=") ); \
Serial.println(variableName);
// usage: dbg("1:my fixed text",myVariable);
// myVariable can be any variable or expression that is defined in scope

#define dbgi(myFixedText, variableName,timeInterval) \
do { \
static unsigned long intervalStartTime; \
if ( millis() - intervalStartTime >= timeInterval ){ \
intervalStartTime = millis(); \
Serial.print( F(#myFixedText " "  #variableName"=") ); \
Serial.println(variableName); \
} \
} while (false);
// usage: dbgi("2:my fixed text",myVar,myInterval);
// myVar can be any variable or expression that is defined in scope
// myInterval is the time-interval which must pass by before the next
// print is executed

#include <Servo.h>

const byte ServoPin = 4;

Servo servo4;
Servo servo5;

const unsigned long MillisecondsPerStep = 250; // four degrees per second

// defining variables as GLOBAL
byte state;
byte position;    //In my servo 72º=0º, 60º=5º, 52º=10º and 44º=15º, that's why I changed it

void setup() {
Serial.begin(115200);
Serial.println( F("Setup-Start") );
// initialise variable state with value for start
state = 0;

servo4.attach(ServoPin);
servo5.attach(5);
}

void loop() {
ServoStateMachine();
// Other stuff goes here
}

void ServoStateMachine() {

static unsigned long timerStart;

dbgi("above switch",state,500);
switch (state)  {

case 0: // Put the servo at 0 degrees
position = 72;    //In my servo 72º=0º, 60º=5º, 52º=10º and 44º=15º, that's why I changed it

servo4.write(position);
servo5.write(0);
dbg("case 0 servo4",position);
dbg("case 0 servo5 0",0);

timerStart = millis();
state++; // change to next state with the next call of the function
break;

case 1: // Move slowly the servo to 5 degrees

if (millis() - timerStart >= MillisecondsPerStep) {
timerStart = millis();
position--;

servo4.write(position);
dbg("case 1 servo4",position);
}

if (position == 60) { // Arrived
dbg("case 1 servo4",position);
dbg("case 1 servo5 45",0);
timerStart = millis();
servo5.write(45);
state++;
}
break;

case 2: // and hold that position for 3 seconds
if (millis() - timerStart >= 3000)    {
// Go back to 0 degrees
position = 72;
servo4.write(position);
servo5.write(0);
dbg("case 2 servo4",position);
dbg("case 2 servo5 0",0);

timerStart = millis();
state++;
}
break;

case 3:  // Move slowly to 10 degrees...

if (millis() - timerStart >= MillisecondsPerStep) {
timerStart = millis();
position--;
servo4.write(position);
dbg("case 3 servo4",position);
}

if (position == 52) { // Arrived
timerStart = millis();
servo5.write(45);
dbg("case 3 servo4",position);
dbg("case 3 servo4 45",0);
state++;
}
break;

case 4: // ...and hold for 3 seconds

if (millis() - timerStart >= 3000)    {
// Go back to 0 degrees
position = 72;
servo4.write(position);
servo5.write(0);
dbg("case 4 servo4",position);
dbg("case 4 servo5 0",0);
timerStart = millis();
state++;
}
break;

case 5:  // Move slowly to 15 degrees

if (millis() - timerStart >= MillisecondsPerStep) {
timerStart = millis();
position--;
servo4.write(position);
dbg("case 5 servo4",position);
}

if (position == 44) { // Arrived
timerStart = millis();
servo5.write(45);
dbg("case 5 servo4",position);
dbg("case 5 servo5 45",0);
state = 0;  // back to the beginning
}
break;

}

}

``````

I copied & pasted the code you have posted
This is the serial output I get

``````22:47:19.876 -> Setup-Start
22:47:19.876 -> "case 0 servo4" position=72
22:47:19.876 -> "case 0 servo5 0" 0=0
22:47:20.151 -> "case 1 servo4" position=71
22:47:20.367 -> "case 1 servo4" position=70
22:47:20.367 -> "above switch" state=1
22:47:20.620 -> "case 1 servo4" position=69
22:47:20.867 -> "above switch" state=1
22:47:20.867 -> "case 1 servo4" position=68
22:47:21.122 -> "case 1 servo4" position=67
22:47:21.388 -> "above switch" state=1
22:47:21.388 -> "case 1 servo4" position=66
22:47:21.638 -> "case 1 servo4" position=65
22:47:21.865 -> "case 1 servo4" position=64
22:47:21.865 -> "above switch" state=1
22:47:22.107 -> "case 1 servo4" position=63
22:47:22.371 -> "above switch" state=1
22:47:22.371 -> "case 1 servo4" position=62
22:47:22.625 -> "case 1 servo4" position=61
22:47:22.872 -> "above switch" state=1
22:47:22.872 -> "case 1 servo4" position=60
22:47:22.872 -> "case 1 servo4" position=60
22:47:22.872 -> "case 1 servo5 45" 0=0
22:47:23.357 -> "above switch" state=2
22:47:23.870 -> "above switch" state=2
22:47:24.395 -> "above switch" state=2
22:47:24.876 -> "above switch" state=2
22:47:25.392 -> "above switch" state=2
22:47:25.885 -> "above switch" state=2
22:47:25.885 -> "case 2 servo4" position=72
22:47:25.885 -> "case 2 servo5 0" 0=0
22:47:26.109 -> "case 3 servo4" position=71
22:47:26.379 -> "above switch" state=3
22:47:26.379 -> "case 3 servo4" position=70
22:47:26.631 -> "case 3 servo4" position=69
22:47:26.879 -> "above switch" state=3
22:47:26.879 -> "case 3 servo4" position=68
22:47:27.149 -> "case 3 servo4" position=67
22:47:27.380 -> "above switch" state=3
22:47:27.380 -> "case 3 servo4" position=66
22:47:27.642 -> "case 3 servo4" position=65
22:47:27.866 -> "above switch" state=3
22:47:27.866 -> "case 3 servo4" position=64
22:47:28.135 -> "case 3 servo4" position=63
22:47:28.382 -> "above switch" state=3
22:47:28.382 -> "case 3 servo4" position=62
22:47:28.636 -> "case 3 servo4" position=61
22:47:28.853 -> "above switch" state=3
22:47:28.908 -> "case 3 servo4" position=60
22:47:29.137 -> "case 3 servo4" position=59
22:47:29.384 -> "above switch" state=3
22:47:29.384 -> "case 3 servo4" position=58
22:47:29.638 -> "case 3 servo4" position=57
22:47:29.885 -> "above switch" state=3
22:47:29.885 -> "case 3 servo4" position=56
22:47:30.139 -> "case 3 servo4" position=55
22:47:30.358 -> "above switch" state=3
22:47:30.358 -> "case 3 servo4" position=54
22:47:30.640 -> "case 3 servo4" position=53
22:47:30.901 -> "above switch" state=3
22:47:30.901 -> "case 3 servo4" position=52
22:47:30.901 -> "case 3 servo4" position=52
22:47:30.901 -> "case 3 servo4 45" 0=0
22:47:31.358 -> "above switch" state=4
22:47:31.890 -> "above switch" state=4
22:47:32.344 -> "above switch" state=4
22:47:32.845 -> "above switch" state=4
22:47:33.362 -> "above switch" state=4
22:47:33.847 -> "above switch" state=4
22:47:33.893 -> "case 4 servo4" position=72
22:47:33.893 -> "case 4 servo5 0" 0=0
22:47:34.154 -> "case 5 servo4" position=71
22:47:34.379 -> "above switch" state=5
22:47:34.379 -> "case 5 servo4" position=70
22:47:34.649 -> "case 5 servo4" position=69
22:47:34.849 -> "above switch" state=5
22:47:34.849 -> "case 5 servo4" position=68
22:47:35.103 -> "case 5 servo4" position=67
22:47:35.382 -> "above switch" state=5
22:47:35.382 -> "case 5 servo4" position=66
22:47:35.599 -> "case 5 servo4" position=65
22:47:35.852 -> "above switch" state=5
22:47:35.852 -> "case 5 servo4" position=64
22:47:36.099 -> "case 5 servo4" position=63
22:47:36.384 -> "above switch" state=5
22:47:36.384 -> "case 5 servo4" position=62
22:47:36.601 -> "case 5 servo4" position=61
22:47:36.848 -> "above switch" state=5
22:47:36.848 -> "case 5 servo4" position=60
22:47:37.133 -> "case 5 servo4" position=59
22:47:37.388 -> "above switch" state=5
22:47:37.388 -> "case 5 servo4" position=58
22:47:37.601 -> "case 5 servo4" position=57
22:47:37.857 -> "above switch" state=5
22:47:37.857 -> "case 5 servo4" position=56
22:47:38.126 -> "case 5 servo4" position=55
22:47:38.374 -> "above switch" state=5
22:47:38.374 -> "case 5 servo4" position=54
22:47:38.605 -> "case 5 servo4" position=53
22:47:38.843 -> "above switch" state=5
22:47:38.843 -> "case 5 servo4" position=52
22:47:39.122 -> "case 5 servo4" position=51
22:47:39.359 -> "above switch" state=5
22:47:39.359 -> "case 5 servo4" position=50
22:47:39.623 -> "case 5 servo4" position=49
22:47:39.860 -> "above switch" state=5
22:47:39.860 -> "case 5 servo4" position=48
22:47:40.118 -> "case 5 servo4" position=47
22:47:40.345 -> "above switch" state=5
22:47:40.345 -> "case 5 servo4" position=46
22:47:40.608 -> "case 5 servo4" position=45
22:47:40.862 -> "above switch" state=5
22:47:40.862 -> "case 5 servo4" position=44
22:47:40.862 -> "case 5 servo4" position=44
22:47:40.862 -> "case 5 servo5 45" 0=0
22:47:40.862 -> "case 0 servo4" position=72
22:47:40.862 -> "case 0 servo5 0" 0=0
22:47:41.109 -> "case 1 servo4" position=71
22:47:41.341 -> "above switch" state=1
22:47:41.380 -> "case 1 servo4" position=70
22:47:41.610 -> "case 1 servo4" position=69
22:47:41.864 -> "above switch" state=1
22:47:41.864 -> "case 1 servo4" position=68
22:47:42.101 -> "case 1 servo4" position=67
22:47:42.396 -> "above switch" state=1
22:47:42.396 -> "case 1 servo4" position=66
22:47:42.612 -> "case 1 servo4" position=65
22:47:42.866 -> "above switch" state=1
22:47:42.866 -> "case 1 servo4" position=64
22:47:43.113 -> "case 1 servo4" position=63
22:47:43.367 -> "above switch" state=1
22:47:43.367 -> "case 1 servo4" position=62
22:47:43.614 -> "case 1 servo4" position=61
22:47:43.868 -> "above switch" state=1
22:47:43.868 -> "case 1 servo4" position=60
22:47:43.868 -> "case 1 servo4" position=60
22:47:43.868 -> "case 1 servo5 45" 0=0
22:47:44.400 -> "above switch" state=2
22:47:44.870 -> "above switch" state=2
22:47:45.352 -> "above switch" state=2
22:47:45.872 -> "above switch" state=2
22:47:46.335 -> "above switch" state=2
22:47:46.874 -> "above switch" state=2
22:47:46.874 -> "case 2 servo4" position=72
22:47:46.874 -> "case 2 servo5 0" 0=0
22:47:47.122 -> "case 3 servo4" position=71
22:47:47.340 -> "above switch" state=3
22:47:47.407 -> "case 3 servo4" position=70
22:47:47.623 -> "case 3 servo4" position=69
22:47:47.882 -> "above switch" state=3
22:47:47.882 -> "case 3 servo4" position=68
22:47:48.108 -> "case 3 servo4" position=67
22:47:48.408 -> "above switch" state=3
22:47:48.408 -> "case 3 servo4" position=66
22:47:48.619 -> "case 3 servo4" position=65
22:47:48.833 -> "above switch" state=3
22:47:48.880 -> "case 3 servo4" position=64
22:47:49.096 -> "case 3 servo4" position=63
22:47:49.329 -> "above switch" state=3
22:47:49.397 -> "case 3 servo4" position=62
22:47:49.628 -> "case 3 servo4" position=61
22:47:49.829 -> "above switch" state=3
22:47:49.867 -> "case 3 servo4" position=60
22:47:50.099 -> "case 3 servo4" position=59
22:47:50.331 -> "above switch" state=3
22:47:50.398 -> "case 3 servo4" position=58
22:47:50.627 -> "case 3 servo4" position=57
22:47:50.832 -> "above switch" state=3
22:47:50.885 -> "case 3 servo4" position=56
22:47:51.141 -> "case 3 servo4" position=55
22:47:51.368 -> "above switch" state=3
22:47:51.368 -> "case 3 servo4" position=54
22:47:51.633 -> "case 3 servo4" position=53
22:47:51.834 -> "above switch" state=3
22:47:51.887 -> "case 3 servo4" position=52
22:47:51.887 -> "case 3 servo4" position=52
22:47:51.887 -> "case 3 servo4 45" 0=0
22:47:52.351 -> "above switch" state=4
22:47:52.836 -> "above switch" state=4
22:47:53.370 -> "above switch" state=4
22:47:53.851 -> "above switch" state=4
22:47:54.369 -> "above switch" state=4
22:47:54.838 -> "above switch" state=4
22:47:54.838 -> "case 4 servo4" position=72
22:47:54.838 -> "case 4 servo5 0" 0=0
22:47:55.092 -> "case 5 servo4" position=71
22:47:55.343 -> "above switch" state=5
22:47:55.343 -> "case 5 servo4" position=70
22:47:55.608 -> "case 5 servo4" position=69
22:47:55.837 -> "above switch" state=5
22:47:55.837 -> "case 5 servo4" position=68
22:47:56.093 -> "case 5 servo4" position=67
22:47:56.341 -> "above switch" state=5
22:47:56.341 -> "case 5 servo4" position=66
22:47:56.594 -> "case 5 servo4" position=65
22:47:56.841 -> "above switch" state=5
22:47:56.841 -> "case 5 servo4" position=64
22:47:57.088 -> "case 5 servo4" position=63
22:47:57.326 -> "above switch" state=5
22:47:57.373 -> "case 5 servo4" position=62
22:47:57.596 -> "case 5 servo4" position=61
22:47:57.843 -> "above switch" state=5
22:47:57.843 -> "case 5 servo4" position=60
22:47:58.096 -> "case 5 servo4" position=59
22:47:58.344 -> "above switch" state=5
22:47:58.344 -> "case 5 servo4" position=58
22:47:58.597 -> "case 5 servo4" position=57
22:47:58.844 -> "above switch" state=5
22:47:58.844 -> "case 5 servo4" position=56
22:47:59.082 -> "case 5 servo4" position=55
22:47:59.345 -> "above switch" state=5
22:47:59.345 -> "case 5 servo4" position=54
22:47:59.598 -> "case 5 servo4" position=53
22:47:59.844 -> "above switch" state=5
22:47:59.844 -> "case 5 servo4" position=52
22:48:00.099 -> "case 5 servo4" position=51
22:48:00.356 -> "above switch" state=5
22:48:00.356 -> "case 5 servo4" position=50
22:48:00.584 -> "case 5 servo4" position=49
22:48:00.847 -> "above switch" state=5
22:48:00.847 -> "case 5 servo4" position=48
22:48:01.100 -> "case 5 servo4" position=47
22:48:01.348 -> "above switch" state=5
22:48:01.348 -> "case 5 servo4" position=46
22:48:01.601 -> "case 5 servo4" position=45
22:48:01.848 -> "above switch" state=5
22:48:01.848 -> "case 5 servo4" position=44
22:48:01.848 -> "case 5 servo4" position=44
22:48:01.848 -> "case 5 servo5 45" 0=0
22:48:01.848 -> "case 0 servo4" position=72
22:48:01.848 -> "case 0 servo5 0" 0=0
22:48:02.102 -> "case 1 servo4" position=71
22:48:02.340 -> "above switch" state=1
22:48:02.340 -> "case 1 servo4" position=70
22:48:02.603 -> "case 1 servo4" position=69
22:48:02.851 -> "above switch" state=1
22:48:02.851 -> "case 1 servo4" position=68
22:48:03.105 -> "case 1 servo4" position=67
22:48:03.352 -> "above switch" state=1
22:48:03.352 -> "case 1 servo4" position=66
22:48:03.584 -> "case 1 servo4" position=65
22:48:03.822 -> "above switch" state=1
22:48:03.869 -> "case 1 servo4" position=64
22:48:04.107 -> "case 1 servo4" position=63
22:48:04.349 -> "above switch" state=1
22:48:04.349 -> "case 1 servo4" position=62
22:48:04.608 -> "case 1 servo4" position=61
22:48:04.824 -> "above switch" state=1
22:48:04.874 -> "case 1 servo4" position=60
22:48:04.874 -> "case 1 servo4" position=60
22:48:04.874 -> "case 1 servo5 45" 0=0
22:48:05.357 -> "above switch" state=2
22:48:05.858 -> "above switch" state=2
22:48:06.318 -> "above switch" state=2
22:48:06.813 -> "above switch" state=2
22:48:07.314 -> "above switch" state=2
22:48:07.816 -> "above switch" state=2
22:48:07.863 -> "case 2 servo4" position=72
22:48:07.863 -> "case 2 servo5 0" 0=0
22:48:08.123 -> "case 3 servo4" position=71
22:48:08.349 -> "above switch" state=3
22:48:08.349 -> "case 3 servo4" position=70
22:48:08.618 -> "case 3 servo4" position=69
22:48:08.818 -> "above switch" state=3
22:48:08.861 -> "case 3 servo4" position=68
22:48:09.119 -> "case 3 servo4" position=67
22:48:09.335 -> "above switch" state=3
22:48:09.335 -> "case 3 servo4" position=66
``````

works as expected.
I decided to see you as a low-level troll
I will mute you as a user and this thread.
find the lazyiness and difference that causes the different behaviour of your code on your own or with the help of others.

What? I know you won’t see this but I was talking seriously. I’m really confused. I will never be trolling anyone who is trying so much to help me. Honestly I feel bad, I didn’t intend to make fun of anyone.
Sorry from the bottom of my heart.

So everywhere you said '0' you meant '72' and where you said '5' you meant '60' and where you said '10' you meant '52' and where you said '15' you meant '44'? I'll try to keep that in mind.

Becuse right after telling servo5 to go to 45"

``````      if (position == 44) // Arrived
{
timerStart = millis();
servo5.write(45);
state = 0;  // back to the beginning
}
``````

You tell it to go to 0:

``````    case 0: // Put the servo at 0 degrees
position = 72;
servo4.write(position);
servo5.write(0);
timerStart = millis();
state++;
break;
``````

Remember, your original description of what you wanted the sketch to do did not include "and hold for 3 secs" between "Move slowly to 15 degrees" and "Go back to 0 degrees".

Oh thanks for showing me what was wrong, I’ll try to change it myself, I don’t know if I will do it well but I’ll try

My guess is that your power supply can't move both servos at the same time and your Arduino is resetting. Figure 1 Amp per servo.

If my guess is correct, your sketch may be way more correct than you think. It looks correct so-far to me. You would just need some more "hold 3 seconds" states to make it do everything you wanted.

Well this sketch:

``````
#define dbg(myFixedText, variableName) \
Serial.print( F(#myFixedText " "  #variableName"=") ); \
Serial.println(variableName);
// usage: dbg("1:my fixed text",myVariable);
// myVariable can be any variable or expression that is defined in scope

#define dbgi(myFixedText, variableName,timeInterval) \
do { \
static unsigned long intervalStartTime; \
if ( millis() - intervalStartTime >= timeInterval ){ \
intervalStartTime = millis(); \
Serial.print( F(#myFixedText " "  #variableName"=") ); \
Serial.println(variableName); \
} \
} while (false);
// usage: dbgi("2:my fixed text",myVar,myInterval);
// myVar can be any variable or expression that is defined in scope
// myInterval is the time-interval which must pass by before the next
// print is executed

#include <Servo.h>

const byte ServoPin = 4;

Servo servo4;
Servo servo5;

const unsigned long MillisecondsPerStep = 250; // four degrees per second

// defining variables as GLOBAL
byte state;
byte position;    //In my servo 72º=0º, 60º=5º, 52º=10º and 44º=15º, that's why I changed it

void setup() {
Serial.begin(115200);
Serial.println( F("Setup-Start") );
// initialise variable state with value for start
state = 0;

servo4.attach(ServoPin);
servo5.attach(5);
}

void loop() {
ServoStateMachine();
// Other stuff goes here
}

void ServoStateMachine() {

static unsigned long timerStart;

dbgi("above switch",state,500);
switch (state)  {

case 0: // Put the servo at 0 degrees
position = 72;    //In my servo 72º=0º, 60º=5º, 52º=10º and 44º=15º, that's why I changed it

servo4.write(position);
servo5.write(0);
dbg("case 0 servo4",position);
dbg("case 0 servo5 0",0);

timerStart = millis();
state++; // change to next state with the next call of the function
break;

case 1: // Move slowly the servo to 5 degrees

if (millis() - timerStart >= MillisecondsPerStep) {
timerStart = millis();
position--;

servo4.write(position);
dbg("case 1 servo4",position);
}

if (position == 60) { // Arrived
dbg("case 1 servo4",position);
dbg("case 1 servo5 45",0);
timerStart = millis();
servo5.write(45);
state++;
}
break;

case 2: // and hold that position for 3 seconds
if (millis() - timerStart >= 3000)    {
// Go back to 0 degrees
position = 72;
servo4.write(position);
servo5.write(0);
dbg("case 2 servo4",position);
dbg("case 2 servo5 0",0);

timerStart = millis();
state++;
}
break;

case 3:  // Move slowly to 10 degrees...

if (millis() - timerStart >= MillisecondsPerStep) {
timerStart = millis();
position--;
servo4.write(position);
dbg("case 3 servo4",position);
}

if (position == 52) { // Arrived
timerStart = millis();
servo5.write(45);
dbg("case 3 servo4",position);
dbg("case 3 servo4 45",0);
state++;
}
break;

case 4: // ...and hold for 3 seconds

if (millis() - timerStart >= 3000)    {
// Go back to 0 degrees
position = 72;
servo4.write(position);
servo5.write(0);
dbg("case 4 servo4",position);
dbg("case 4 servo5 0",0);
timerStart = millis();
state++;
}
break;

case 5:  // Move slowly to 15 degrees

if (millis() - timerStart >= MillisecondsPerStep) {
timerStart = millis();
position--;
servo4.write(position);
dbg("case 5 servo4",position);
}

if (position == 44) { // Arrived
timerStart = millis();
servo5.write(45);
dbg("case 5 servo4",position);
dbg("case 5 servo5 45",0);
state = 0;  // back to the beginning
}
break;

}

}

``````

Does everything I wanted but in the last case, when the servo4 is at 44 degrees, the servo5 doesn’t move, and I don’t think that it’s because my laptop doesn’t got power for doing that because in the other cases the two servos work perfectly fine

You should go back and look at reply 33 where I explain why it is doing that. It will get fixed automatically when you add the new "Hold that position for 3 seconds" states you mentioned in reply 21.