Servo Array for smooth not working

Projects Programming
1

Hello all I need help with this program I am making. Basically I am trying to make the servo move smoothly in different positions with delays. I used the Arduino example sweep example and implemented in my code so the servo moves swiftly and all positions are in the array. But that only works if the servo positions numbers are only increasing (ex. from 0 to 40 to 80 to 100, etc) but when a number in the array that is lower than the previous number (ex. 100 to 0) my code "or (smo1 = pos1*; smo1 <= pos1[i+1]; smo1 += 1) { "[/color] does not work because the code needs to be the "smo1 >= pos1[i+1]; smo1 -= 1)" so how do I put both so my servos would move smoothly when moving from 100 to 0 and then back to 140.*
```
*#include <Servo.h>
#define turn_size 7

Servo myservo;

int pos1[] = {0, 40, 80, 100, 0, 140, 180};
int delay1[] = {2000, 1000, 500, 100, 300, 2000, 180};
int i;
int smo1;

void setup()
{
myservo.attach(9);
}

void loop()
{
for (i = 0; i<turn_size; i++)
 {
   for (smo1 = pos1[i]; smo1 <= pos1[i+1]; smo1 += 1) {
   myservo.write(smo1);
   delay(15);
 }
delay(delay1[i]);
}
}*
```

2

You could have 2 functions. One to move in CW and one for CCW. Choose which to call based on the difference between the current position and the new desired position.

3

You need to introduce a new variable like 'incr' that can be +1 or -1 depending if you are going up or down

I also added in a bit of code so the compiler will figure out the size of your array. You don't have to. That way, if/when you add more positions, the code will continue to work.

#include <Servo.h>
//#define turn_size 7

Servo myservo;

const int pos1[] = {0, 40, 80, 100, 0, 140, 180};
const int turn_size = sizeof(pos1) / sizeof(pos1[0]);
const int delay1[turn_size] = {2000, 1000, 500, 100, 300, 2000, 180};

void setup()
{
  myservo.attach(9);
}

void loop()
{
  for (int i = 0; i < turn_size-1; i++)
  {
    int start = pos1[i];
    int end = pos1[i+1];
    int inc = 1;  // assume counting up
    if ( end < start )
    {
      inc = -1; // counting down
    }
    while( start != end )
    {
      myservo.write(start);
      delay(15);
      start += inc;
    }
    delay(delay1[i]);
  }
}
4

Thank you blh64 for the code. I appreciate you looking at my problem. Now how would I add multiple servos to this?

5

You would start by declaring more Servo objects associated to other pins.

Then, if you want them all moving at the same time, you can not have a while() loop for a given servo, you will have to single step each one and then advance.

6

I am still having a hard time configuring for two servos I don't know how to program after "void loop()"

#include <Servo.h>

Servo myservo1;
Servo myservo2;

const int pos1[] = {0, 40, 80, 100, 0, 140, 180, 100, 50, 20, 0};
const int pos2[] = {0, 10, 20, 30, 20, 10, 0, 10, 20, 10, 0};
const int turn_size = sizeof(pos1) / sizeof(pos1[0]);
const int delay1[turn_size] = {2000, 1000, 3000, 100, 300, 2000, 180, 100, 3000, 100, 100};

void setup()
{
  myservo1.attach(9);
  myservo2.attach(4);
}

void loop()
{
  for (int i = 0; i < turn_size-1; i++)
  {
    //---start
    int start = pos1[i];
    int end = pos1[i+1];
    int inc = 1;  // assume counting up
    if ( end < start )
    {
      inc = -1; // counting down
    }
    while( start != end )
    {
      myservo1.write(start);
      delay(15);
      start += inc;
    }
    delay(delay1[i]);
    //---end
  }
}
7

You basically have to duplicate your servo movement code for the second servo

#include <Servo.h>

Servo myservo1;
Servo myservo2;

const int pos1[] = {0, 40, 80, 100, 0, 140, 180, 100, 50, 20, 0};
const int pos2[] = {0, 10, 20, 30, 20, 10, 0, 10, 20, 10, 0};
const int turn_size = sizeof(pos1) / sizeof(pos1[0]);
const int delay1[turn_size] = {2000, 1000, 3000, 100, 300, 2000, 180, 100, 3000, 100, 100};

void setup() {
  myservo1.attach(9);
  myservo2.attach(4);
}


void loop() {
  for (int i = 0; i < turn_size - 1; i++) {
    //---start
    int start1 = pos1[i];
    int end1 = pos1[i + 1];
    int inc1 = 1;  // assume counting up
    if ( end1 < start1 ) {
      inc1 = -1; // counting down
    }

    int start2 = pos2[i];
    int end2 = pos2[i + 1];
    int inc2 = 1;  // assume counting up
    if ( end2 < start2 ) {
      inc2 = -1; // counting down
    }

    bool done1 = false;
    bool done2 = false;

    // move both servos at the same time until both are done
    while (done1 == false && done2 == false) {
      if ( start1 != end1 ) {
        myservo1.write(start1);
        delay(15);
        start1 += inc1;
      }
      else {
        done1 = true;
      }

      if ( start2 != end2 ) {
        myservo2.write(start2);
        delay(15);
        start2 += inc2;
      }
      else {
        done2 = true;
      }
    }

    // everybody is done, so pause
    delay(delay1[i]);
  }
}

If you are planning more servos, you might want to learn about arrays and how to do use them rather than keep duplicating code.

8

The code I made was this and was still having problems. Thanks alot of figure it out for me

#include <Servo.h>

Servo myservo1;
Servo myservo2;

const int pos1[] = {0, 40, 80, 100, 0, 140, 180, 100, 50, 20, 0};
const int pos2[] = {0, 10, 20, 30, 20, 10, 0, 10, 20, 10, 0};
const int turn_size = sizeof(pos1) / sizeof(pos1[0]);
const int delay1[turn_size] = {500, 1000, 500, 100, 300, 2000, 180, 100, 1000, 100, 100};

void setup()
{
  myservo1.attach(9);
  myservo2.attach(4);
}

void loop()
{
  for (int i = 0; i < turn_size-1; i++)
  {
    //---start1
    int start1 = pos1[i];
    int end1 = pos1[i+1];
    int start2 = pos2[i];
    int end2 = pos2[i+1];
    
    int inc1 = 1;  // assume counting up
    int inc2 = 1;
    if ( end1 < start1 && end2 < start2)
    {
      inc1 = -1; // counting down
      inc2 = -1;
    }
    else if ( end1 < start1 && end2 > start2)
    {
      inc1 = -1; // counting down
      inc2 = 1;
    }
    else if ( end1 > start1 && end2 < start2)
    {
      inc1 = 1; // counting down
      inc2 = -1;
    }
    while( start1 != end1 && start2 != end2)
    {
      myservo1.write(start1);
      delay(15);
      start1 += inc1;
      myservo2.write(start2);
      delay(15);
      start2 += inc2;
    }
    delay(delay1[i]);
    //---end1
  }
}
9

The servos jump around if I use my code or use yours. That is because if servo1 is completed its position movement and servo2 is still going they will both stop wait for the delay and then jump to the next position. This way servo1 will go to the next movement smoothly while servo2 has to jump and not be smooth. Example when servo1 goes from 80 to 100 and servo2 goes from 80 to 140 during the same time frame I see servo1 completing the movement from 80 to 100 quicker than servo2 which has a longer run. But once servo1 has completed its movement the program stops and waits for the delay and goes to the next position. Then Servo1 goes from 100 to 120 while servo2 has to jump to 140 which it hadn't completed and go to position 160. Look at the serial monitor after my code to see what I am talking about

#include <Servo.h>

Servo myservo1;
Servo myservo2;

const int pos1[] = {0, 40, 80, 100, [b]80, 100[/b], 120, 180, 100, 50, 0};
const int pos2[] = {0, 20, 80, 100, [b]80, 140[/b], 160, 100, 50, 20, 0};
const int turn_size = sizeof(pos1) / sizeof(pos1[0]);
const int delay1[turn_size] = {3000, 3000, 3000, 3000, 3000, 3000, 3000, 3000, 3000, 3000, 3000};

void setup()
{
  myservo1.attach(9);
  myservo2.attach(4);
  Serial.begin(9600);

}

void loop()
{
  for (int i = 0; i < turn_size-1; i++)
  {
    //---start1
    int start1 = pos1[i];
    int end1 = pos1[i+1];
    int start2 = pos2[i];
    int end2 = pos2[i+1];
    
    int inc1 = 1;  // assume counting up
    int inc2 = 1;
    if ( end1 < start1 && end2 < start2)
    {
      inc1 = -1; // counting down
      inc2 = -1;
    }
    else if ( end1 < start1 && end2 > start2)
    {
      inc1 = -1; // counting down
      inc2 = 1;
    }
    else if ( end1 > start1 && end2 < start2)
    {
      inc1 = 1; // counting down
      inc2 = -1;
    }
    while( start1 != end1 && start2 != end2)
    {
      myservo1.write(start1);
      delay(15);
      start1 += inc1;
      
      myservo2.write(start2);
      delay(15);
      start2 += inc2;

      Serial.print("S1:");
      Serial.print(start1);
      Serial.print(", S2:");
      Serial.println(start2);
       
    }
    delay(delay1[i]);
    //---end1
  }
}

Serial Monitor

16:13:44.803 -> S1:1, S2:1
16:13:44.803 -> S1:2, S2:2
16:13:44.844 -> S1:3, S2:3
16:13:44.844 -> S1:4, S2:4
16:13:44.877 -> S1:5, S2:5
16:13:44.913 -> S1:6, S2:6
16:13:44.947 -> S1:7, S2:7
16:13:44.982 -> S1:8, S2:8
16:13:45.020 -> S1:9, S2:9
16:13:45.054 -> S1:10, S2:10
16:13:45.091 -> S1:11, S2:11
16:13:45.091 -> S1:12, S2:12
16:13:45.127 -> S1:13, S2:13
16:13:45.162 -> S1:14, S2:14
16:13:45.196 -> S1:15, S2:15
16:13:45.232 -> S1:16, S2:16
16:13:45.266 -> S1:17, S2:17
16:13:45.299 -> S1:18, S2:18
16:13:45.299 -> S1:19, S2:19
16:13:45.336 -> S1:20, S2:20
16:13:48.385 -> S1:41, S2:21
16:13:48.419 -> S1:42, S2:22
16:13:48.419 -> S1:43, S2:23
16:13:48.453 -> S1:44, S2:24
16:13:48.486 -> S1:45, S2:25
16:13:48.520 -> S1:46, S2:26
16:13:48.554 -> S1:47, S2:27
16:13:48.588 -> S1:48, S2:28
16:13:48.625 -> S1:49, S2:29
16:13:48.658 -> S1:50, S2:30
16:13:48.695 -> S1:51, S2:31
16:13:48.695 -> S1:52, S2:32
16:13:48.730 -> S1:53, S2:33
16:13:48.766 -> S1:54, S2:34
16:13:48.802 -> S1:55, S2:35
16:13:48.836 -> S1:56, S2:36
16:13:48.871 -> S1:57, S2:37
16:13:48.908 -> S1:58, S2:38
16:13:48.908 -> S1:59, S2:39
16:13:48.945 -> S1:60, S2:40
16:13:48.978 -> S1:61, S2:41
16:13:49.015 -> S1:62, S2:42
16:13:49.048 -> S1:63, S2:43
16:13:49.082 -> S1:64, S2:44
16:13:49.118 -> S1:65, S2:45
16:13:49.155 -> S1:66, S2:46
16:13:49.155 -> S1:67, S2:47
16:13:49.188 -> S1:68, S2:48
16:13:49.222 -> S1:69, S2:49
16:13:49.257 -> S1:70, S2:50
16:13:49.295 -> S1:71, S2:51
16:13:49.330 -> S1:72, S2:52
16:13:49.330 -> S1:73, S2:53
16:13:49.368 -> S1:74, S2:54
16:13:49.402 -> S1:75, S2:55
16:13:49.436 -> S1:76, S2:56
16:13:49.472 -> S1:77, S2:57
16:13:49.506 -> S1:78, S2:58
16:13:49.539 -> S1:79, S2:59
16:13:49.574 -> S1:80, S2:60
16:13:52.591 -> S1:81, S2:81
16:13:52.625 -> S1:82, S2:82
16:13:52.662 -> S1:83, S2:83
16:13:52.697 -> S1:84, S2:84
16:13:52.697 -> S1:85, S2:85
16:13:52.732 -> S1:86, S2:86
16:13:52.766 -> S1:87, S2:87
16:13:52.802 -> S1:88, S2:88
16:13:52.837 -> S1:89, S2:89
16:13:52.875 -> S1:90, S2:90
16:13:52.909 -> S1:91, S2:91
16:13:52.909 -> S1:92, S2:92
16:13:52.946 -> S1:93, S2:93
16:13:52.984 -> S1:94, S2:94
16:13:53.017 -> S1:95, S2:95
16:13:53.055 -> S1:96, S2:96
16:13:53.093 -> S1:97, S2:97
16:13:53.093 -> S1:98, S2:98
16:13:53.128 -> S1:99, S2:99
16:13:53.170 -> S1:100, S2:100
16:13:56.196 -> S1:99, S2:99
16:13:56.230 -> S1:98, S2:98
16:13:56.264 -> S1:97, S2:97
16:13:56.303 -> S1:96, S2:96
16:13:56.303 -> S1:95, S2:95
16:13:56.337 -> S1:94, S2:94
16:13:56.370 -> S1:93, S2:93
16:13:56.404 -> S1:92, S2:92
16:13:56.437 -> S1:91, S2:91
16:13:56.470 -> S1:90, S2:90
16:13:56.504 -> S1:89, S2:89
16:13:56.538 -> S1:88, S2:88
16:13:56.575 -> S1:87, S2:87
16:13:56.575 -> S1:86, S2:86
16:13:56.613 -> S1:85, S2:85
16:13:56.648 -> S1:84, S2:84
16:13:56.685 -> S1:83, S2:83
16:13:56.722 -> S1:82, S2:82
16:13:56.757 -> S1:81, S2:81
16:13:56.790 -> S1:80, S2:80
16:13:59.815 -> S1:81, S2:81
16:13:59.849 -> S1:82, S2:82
16:13:59.849 -> S1:83, S2:83
16:13:59.885 -> S1:84, S2:84
16:13:59.920 -> S1:85, S2:85
16:13:59.953 -> S1:86, S2:86
16:13:59.990 -> S1:87, S2:87
16:14:00.023 -> S1:88, S2:88
16:14:00.056 -> S1:89, S2:89
16:14:00.090 -> S1:90, S2:90
16:14:00.124 -> S1:91, S2:91
16:14:00.124 -> S1:92, S2:92
16:14:00.157 -> S1:93, S2:93
16:14:00.190 -> S1:94, S2:94
16:14:00.224 -> S1:95, S2:95
16:14:00.262 -> S1:96, S2:96
16:14:00.298 -> S1:97, S2:97
16:14:00.331 -> S1:98, S2:98
16:14:00.368 -> S1:99, S2:99
[b]16:14:00.368 -> S1:100, S2:100
16:14:03.395 -> S1:101, S2:141[/b]
16:14:03.429 -> S1:102, S2:142
16:14:03.462 -> S1:103, S2:143
16:14:03.498 -> S1:104, S2:144
16:14:03.532 -> S1:105, S2:145
16:14:03.565 -> S1:106, S2:146
16:14:03.598 -> S1:107, S2:147
16:14:03.635 -> S1:108, S2:148
16:14:03.668 -> S1:109, S2:149
16:14:03.668 -> S1:110, S2:150
16:14:03.704 -> S1:111, S2:151
16:14:03.742 -> S1:112, S2:152
16:14:03.779 -> S1:113, S2:153
16:14:03.815 -> S1:114, S2:154
16:14:03.848 -> S1:115, S2:155
16:14:03.882 -> S1:116, S2:156
16:14:03.882 -> S1:117, S2:157
16:14:03.916 -> S1:118, S2:158
16:14:03.949 -> S1:119, S2:159
16:14:03.985 -> S1:120, S2:160
10

Try mode code, but change this line

    //while (done1 == false && done2 == false) {
    while (done1 == false || done2 == false) {

You want the while loop to continue until both servos are done (OR, not AND)

11

If you want both servos to operate completely independently of each other, this code does that (untested)

#include <Servo.h>

Servo myservo1;
Servo myservo2;

const int pos1[] = {0, 40, 80, 100, 0, 140, 180, 100, 50, 20, 0};
const int positionCount1 = sizeof(pos1) / sizeof(pos1[0]);
const unsigned int delayTime1[positionCount1] = {2000, 1000, 3000, 100, 300, 2000, 180, 100, 3000, 100, 100};

const int pos2[] = {0, 10, 20, 30, 20, 10, 0, 10, 20, 10, 0};
const int positionCount2 = sizeof(pos2) / sizeof(pos2[0]);
const unsigned int delayTime2[positionCount2] = {2000, 1000, 3000, 100, 300, 2000, 180, 100, 3000, 100, 100};

void setup() {
  myservo1.attach(9);
  myservo2.attach(4);
}


void loop() {

  checkServo1();
  checkServo2();
}

void checkServo1() {
  const unsigned long pauseTime = 15;
  static int startPos = pos1[0];
  static int endPos = pos1[1];
  static int incSize = (startPos < endPos) ? 1 : -1;
  static bool isStepping = true;  // true = stepping, false = pausing
  static unsigned long lastTime;

  static int idx = 0;

  if ( isStepping == true ) {
    // check if previous step is complate and repeat if needed
    if ( millis() - lastTime >= pauseTime ) {
      // stepping time
      if ( startPos != endPos ) {
        myservo1.write(startPos);
        lastTime = millis();
        startPos += incSize;
      }
      else {
        // done stepping so pause
        isStepping = false;
        lastTime = millis();
      }
    }
  } else {
    // we are pausing so see if we are done
    if ( millis() - lastTime >= delayTime1[idx] ) {
      // done waiting so start the next step sequence
      idx++;
      if ( idx >= positionCount1 - 1 ) idx = 0;
      startPos = pos1[idx];
      endPos = pos1[idx + 1];
      incSize = (startPos < endPos) ? 1 : -1;
      isStepping = true;
    }
  }
}


void checkServo2() {
  const unsigned long pauseTime = 15;
  static int startPos = pos2[0];
  static int endPos = pos2[1];
  static int incSize = (startPos < endPos) ? 1 : -1;
  static bool isStepping = true;  // true = stepping, false = pausing
  static unsigned long lastTime;

  static int idx = 0;

  if ( isStepping == true ) {
    // check if previous step is complate and repeat if needed
    if ( millis() - lastTime >= pauseTime ) {
      // stepping time
      if ( startPos != endPos ) {
        myservo1.write(startPos);
        lastTime = millis();
        startPos += incSize;
      }
      else {
        // done stepping so pause
        isStepping = false;
        lastTime = millis();
      }
    }
  } else {
    // we are pausing so see if we are done
    if ( millis() - lastTime >= delayTime2[idx] ) {
      // done waiting so start the next step sequence
      idx++;
      if ( idx >= positionCount2 - 1 ) idx = 0;
      startPos = pos2[idx];
      endPos = pos2[idx + 1];
      incSize = (startPos < endPos) ? 1 : -1;
      isStepping = true;
    }
  }
}
12

Your new code did not work. And I also tried the using || instead of && in the while loop and that didnt work either.

13

driveralot:
Your new code did not work. And I also tried the using || instead of && in the while loop and that didnt work either.

Did not work is not a helpful description of any problem.
I added some debug statements into the first code and it seems to step through every servo step

#include <Servo.h>

Servo myservo1;
Servo myservo2;

const int pos1[] = {0, 40, 80, 100, 0, 140, 180, 100, 50, 20, 0};
const int pos2[] = {0, 10, 20, 30, 20, 10, 0, 10, 20, 10, 0};
const int turn_size = sizeof(pos1) / sizeof(pos1[0]);
const int delay1[turn_size] = {2000, 1000, 3000, 100, 300, 2000, 180, 100, 3000, 100, 100};

void setup() {
  Serial.begin(9600);
  myservo1.attach(9);
  myservo2.attach(4);
}


void loop() {

  Serial.println("Starting loop");
  for (int i = 0; i < turn_size - 1; i++) {
    //---start
    int start1 = pos1[i];
    int end1 = pos1[i + 1];
    int inc1 = 1;  // assume counting up
    if ( end1 < start1 ) {
      inc1 = -1; // counting down
    }

    int start2 = pos2[i];
    int end2 = pos2[i + 1];
    int inc2 = 1;  // assume counting up
    if ( end2 < start2 ) {
      inc2 = -1; // counting down
    }

    bool done1 = false;
    bool done2 = false;

    Tell( "S1", start1, end1, inc1 );
    Tell( "S2", start2, end2, inc2 );
    // move both servos at the same time until both are done
    while (done1 == false || done2 == false) {
      if ( start1 != end1 ) {
        start1 += inc1;
        Serial.print( "S1: step to "); Serial.println(start1);
        myservo1.write(start1);
        delay(15);
      }
      else {
        if( !done1 ) Serial.println( "S1: done" );
        done1 = true;
      }

      if ( start2 != end2 ) {
        start2 += inc2;
        Serial.print( "S2: step to "); Serial.println(start2);
        myservo2.write(start2);
        delay(15);
      }
      else {
        if( !done2 ) Serial.println( "S2: done" );
        done2 = true;
      }
    }

    // everybody is done, so pause
    Serial.print("Pausing for "); Serial.println(delay1[i]);
    delay(delay1[i]);
  }

  // just 1 time through loop for testing
  while(1);
}

void Tell(const char* msg, int start, int end, int step) {

  Serial.print( msg ); Serial.print( ": stepping from ");
  Serial.print( start ); Serial.print( " to " );
  Serial.print( end ); Serial.print( " step " );
  Serial.println( step );
}

What are you seeing?