moving multiple servos through Arduino board

[cross-side]

Hello,

I am working on a project that involves 5 servomotors that need to move in a coordinated way. The servos are controlled via an Arduino micro-controller. I've been learning C through this project, so my coding might be a little off-convention.

What I have is 5 buttons (0 to 4). I want to associate a distinct series of positions to each button. So for every button, the servos will all move at the same time to a distinct set of positions at a definite speed.

Here are the 5 sets of postions

      int positions[5][5] =
     {
 
      {3, 45, 90, 135, 177}, //positions for button 0 (servo 0 goes to 3 degrees, servo 1 to 45 degrees etc.)
      {177, 3, 45, 90, 135}, //positions for button 1 etc
      {135, 177, 3, 45, 90},
      {90, 135, 177, 3, 45},
      {45, 90, 135, 177, 3},

     };

So, what I want is this:

when I push button 0;
servo 0 goes to position: array 0, position 0 (0,0)
servo 1 goes to position: array 0, position 1 (0,1)
servo 2 goes to position: array 0, position 2 (0,2)
servo 3 goes to position: array 0, position 3 (0,3)
servo 4 goes to position: array 0, position 4 (0,4)
when I push button 1;
servo 1 goes to position: array 1, position 0 (1,0)
servo 2 goes to position: array 1, position 0 (1,1)
servo 3 goes to position: array 1, position 0 (1,2)
servo 4 goes to position: array 1, position 0 (1,3)
servo 5 goes to position: array 1, position 0 (1,4)
and so on.

And what I get is this:

when I push button 0;
servo 0 goes to position: array 0, position 0 (0,0)
servo 1 goes to position: array 0, position 0 (0,0)
servo 2 goes to position: array 0, position 0 (0,0)
servo 3 goes to position: array 0, position 0 (0,0)
servo 4 goes to position: array 0, position 0 (0,0)
when I push button 1;
servo 1 goes to position: array 0, position 1 (0,1)
servo 2 goes to position: array 0, position 1 (0,1)
servo 3 goes to position: array 0, position 1 (0,1)
servo 4 goes to position: array 0, position 1 (0,1)
servo 5 goes to position: array 0, position 1 (0,1)
and so on.

This situation is something I can live with, (my project can work like that). But I want to understand how to do this since I plan to use this kind of pattern quite often, and the project would be so much nicer with the extra precision.

So, I see my code only accesses the first array. I've tried so many things to solve this that I'm getting confused now... In fact, I worked my way to this code through many many mistakes, and this code is also a mistake, because, as you will see, I have a variable i that would need to be incremented from 0 to 4, and it isn't. I know this is where my problem lies but every way I try to fix it, I get worse results than what this code does. Either the servos move one after the other or they jerk around like epileptic squirrels.

If anyone could guide me trough solving this puzzle, I would greatly appreciate,

thanks

oh yeah, here's the code

#include <Servo.h>

Servo myservo[5];                           
int buttonPin[] = {2, 3, 4, 5, 6};          //pin numbers on the board for the buttons
int buttonState[] = {0, 0, 0, 0, 0};        
int lastButtonState[] = {1, 1, 1, 1, 1};
int servospeeds[] = {10, 20, 30, 40, 50};   //in milliseconds
int pos[] = {0, 0, 0, 0, 0};                //stores the next positions the servos are heading to, this is needed for the speed control 
int lastPos[] = {0, 0, 0, 0, 0};
int positions[5][5] =                       //the ultimate servo destinations

{  
  {3, 45, 90, 135, 177},       
  {177, 3, 45, 90, 135},        
  {135, 177, 3, 45, 90},       
  {90, 135, 177, 3, 45},        
  {45, 90, 135, 177, 3},          
};


void setup() 
{
  int f, g;
  Serial.begin(9600); 
  int x = 11;
  for (f=0; f<5; f++)
  {                                   
   myservo[f].attach(x); x--;           //pin numbers on the board for the servos
  }
  for(g = 0; g < 5; g++)
  {                                                
   pinMode (buttonPin[g], INPUT);       //sets the button pins as inputs
  }
}


void moveServo(int pp)
{
 int i, j;
 buttonState[pp] = digitalRead(buttonPin[pp]);
 if ((buttonState[pp] != lastButtonState[pp])&&(buttonState[pp] == 0))  /*if button has changed state and is now depressed*/ 
 {
  for(pos[i] = lastPos[i]; pos[i] <= positions[pp][i]; pos[i] ++)
  {
   for(j = 0; j < 5; j++)
   {
    myservo[j].write(pos[i]); /*i starts where it was left before and is incremented in steps of 1 degree to control the speed. I know the problem is around here, I tried to nest another for loop to increment the rows of the positions array first and then incrementing the columns, but what I get is that the servos do their things one after the other while I want them to move altogether.*/
   }
   delay(servospeeds[3]);
   lastPos[i] = pos[i];
  }
  for(pos[i] = lastPos[i]; pos[i] >= positions[pp][i]; pos[i] --)
  {
   for(j = 0; j < 5; j++)
   {
    myservo[j].write(pos[i]);
   }
   delay(servospeeds[3]);
   lastPos[i] = pos[i];
  }
 }
 lastButtonState[pp] = buttonState[pp]; 
} 

void loop()
{
 int p;
 for (p = 0; p < 5; p++)
 {
  moveServo(p);
 } 
}

[stahta01]

Where do you use buttonState to select what you want to have happen.

Tim S.

[cross-side]

it's the first thing the moveServo function does. buttonState takes the value of a button reading, then checks if it has changed from pressed to not-pressed, comparing itself to lastbuttonState. So, all that happens afterwards is done accordingly to what button has been activated. (did I get your question right?)

[anduril462]

Are the buttons debounced? You could be getting some crazy readings of on, off, on if they aren't, which might be causing your squirrelly behavior.

Some notes on your program from a first glance:

• Don't use global variables.
• You never initialize or increment i anywhere, meaning it's always zero.
• I don't see a main where all this is tied together, so I don't know what else you may be doing that you shouldn't or what you might not be doing that you should.

[cross-side]

A trimmed down version of this program works fine, so the buttons should not be the problem. (they give their signal through the use of a pull-down resistor)

the Arduino board uses a simplified version of C, one of the things it simplifies is you don't need the main() function.

For th incrementation of i, as I noted in the post, this version of the code is an 'error' version, but it still gives me the closest results to what I'm looking for, so this is why I try to build on it.

for the Global variables, do you mean I should declare them for each function?

thanks

[CommonTater]

A trimmed down version of this program works fine, so the buttons should not be the problem. (they give their signal through the use of a pull-down resistor)

for the Global variables, do you mean I should declare them for each function?

Following up on the debounce suggestion... try bypassing that resistor to ground with about a .001uf ceramic capacitor.

Yes, variables should be declared in or passed into functions... global variables lead to globally variable stability problems.

[cross-side]

I'll try getting those caps next time I go to the store. Do you mean I should replace the resistor by a capacitor? What is the advantage of doing this?

For the variables, I've done the upgrade, thanks for the tip.

Now the problem is clearly in the syntax, There is no debugger in the Arduino environement so it's difficult to follow through the nested loops. When I added another for loop (this is where i used to be incremented) the problem became clear.

void moveServo(int pp)
{
 buttonState[pp] = digitalRead(buttonPin[pp]);
 if ((buttonState[pp] != lastButtonState[pp])&&(buttonState[pp] == 0))
 {
  for(i = 0; i < 5; i++)
  {
  for(pos[i] = lastPos[i]; pos[i] <= positions[pp][i]; pos[i] ++)
  {
   for(j = 0; j < 5; j++)
   {
    myservo[j].write(pos[i]);
   }
   delay(servospeeds[3]);
   lastPos[i] = pos[i];

I kind of managed to do a step by step approach on paper, and I get to this point where I see why the servos are activated one after the other. I just don't see how to get them started together, it makes me feel like I'm trying to catch raindrops with chopsticks...

[CommonTater]

I'll try getting those caps next time I go to the store. Do you mean I should replace the resistor by a capacitor? What is the advantage of doing this?

No not replace... add a cap from the junction of the resistor and switch to ground. The cap stores a small charge so that when the button is pressed, if the contact breaks or chatters, the cap can fill it in, bounceproofing the switch.

[anduril462]

I'll try getting those caps next time I go to the store. Do you mean I should replace the resistor by a capacitor? What is the advantage of doing this?

Here's an article on debouncing that might help explain the idea a bit.

I kind of managed to do a step by step approach on paper, and I get to this point where I see why the servos are activated one after the other. I just don't see how to get them started together, it makes me feel like I'm trying to catch raindrops with chopsticks...

Unless the Arduino environment gives you a way to write to several outputs simultaneously, it's probably not be possible. Your best bet is the approach you're taking now. You can try playing with different delay and degree settings. Lower your 30ms delay to 10ms. Try changing how much you move your servos by. Maybe 5 degrees instead of 1 will make the motion seem smoother.

void moveServo(int pp)
{
 buttonState[pp] = digitalRead(buttonPin[pp]);
 if ((buttonState[pp] != lastButtonState[pp])&&(buttonState[pp] == 0))
 {
  for(i = 0; i < 5; i++)
  {
  for(pos[i] = lastPos[i]; pos[i] <= positions[pp][i]; pos[i] ++)
  {
   for(j = 0; j < 5; j++)
   {
    myservo[j].write(pos[i]);
   }
   delay(servospeeds[3]);
   lastPos[i] = pos[i];

I'm a little confused with what you're doing in your loops here. It looks to me like the following:

for each servo i
    for every position pos[i] from the current to the goal position
        for each servo j
            write current position for servo[i]
        delay 30ms
        update the pos[i] to have the new position

So it seems you're setting all 5 servos to pos[i] each time through the loop, probably causing your squirrelly behavior, or at least making it not work. You probably want something more like:

do {
    done = TRUE
    for each servo, i
        if (pos[i] < positions[pp][i])
            pos[i]++
            done = FALSE;  // If we had to move a servo, we're not done yet
        else if pos[i] > positions[pp][i])
            // same except decrement
        myservo[i].write(pos[i]);  // always write a new position to the servo
    delay 30ms
} while (!done)

[cyberfish]

There are ways to send out the servo pulses at the same time.

Easiest is probably to sort the servos by pulse width, then wait for them one by one, incrementally.

For example, if you need to send 2ms (servo 1), 1ms (servo 2), 5ms (servo 3) to 3 servos.

Sort them to 1ms, 2ms, 5ms.
Then
1) Set all outputs to 1
2) wait 1ms, then turn servo 2 output off
3) wait 1ms, then turn servo 1 output off
4) wait 3ms, then turn servo 3 output off

Though even without that, since the servos only need ~20ms per update, if you do it right, they should still appear to be simultaneous.

Another way is to use timer interrupts, but that gets tricky.

Debouncing is a good suggestion. You basically need a RC low-pass filter on the button output. The cap size required would depend on the value of your pull-up of course. You may also want a Schmitt trigger.

[cross-side]

I got it!

All servos move together.
I have positioning control.
I have speed control.

And I learned that it's worth it to search the forums for similar problems. Turns out some genius named Korman had written a library to solve that exact problem! So I also learned how to download and install a library...

The code is so simple now, it's like, sleepwalking in the park compared to the previous one.

Thanks to cyberfish, I found the solution while investigating your idea, and as I understood, the library seems to function in way similar to your idea.

Thanks to anduril462, commonTater and stahta01, all your comments helped me understand C a little more. (and they make me see how vast it is!)


And to finish, about the debouncing, the buttons I use are in fact photosensors, the cheap kind you find in night-lights at the dollar-store. They work great, I set up the program so that when the light is blocked, it triggers the loop.

But since these photosensors are like variable resistors, can they and/or should they be debounced?

Thanks for everything !

here's the final code

#include <VarSpeedServo.h>

VarSpeedServo myservo[5];
int ledPin[] = {14, 15, 16, 17, 18};
int buttonPin[] = {2, 3, 4, 5, 6};
int buttonState[] = {0, 0, 0, 0, 0};
int lastButtonState[] = {1, 1, 1, 1, 1};
int servospeeds[] = {10, 20, 30, 40, 50};
int positions[5][5] =

{  
  {3, 45, 90, 135, 177},         // button0 - servo {0, 1, 2, 3, 4}
  {45, 90, 135, 177, 3},        // button1 - servo {0, 1, 2, 3, 4}
  {90, 135, 177, 3, 45},        // button2 - servo {0, 1, 2, 3, 4}
  {135, 177, 3, 45, 90},        // button3 - servo {0, 1, 2, 3, 4}
  {177, 3, 45, 90, 135},        // button4 - servo {0, 1, 2, 3, 4}   
};

void setup() 
{ 
  int f, g, h;
  Serial.begin(9600); 
  int x = 11;
  for (f=0; f<5; f++)
  {                                   
   myservo[f].attach(x); x--;
  }
  for(g = 0; g < 5; g++)
  {                                                
   pinMode (buttonPin[g], INPUT);                                                                       
  }
    for(h = 14; h < 19; h++)
  {                                                
   pinMode (ledPin[h], OUTPUT);                                                                       
  }
}

void moveServo(int pp)
{
 int j, m;
 for (m = 0; m < 5; m++)
 { 
  digitalWrite(ledPin[m], HIGH);
 } 
 buttonState[pp] = digitalRead(buttonPin[pp]);
 if ((buttonState[pp] != lastButtonState[pp])&&(buttonState[pp] == 0))
 {
  digitalWrite(ledPin[pp], LOW);
  delay(50); 
  for(j = 0; j < 5; j++)
  {
   myservo[j].slowmove(positions[pp][j], servospeeds[3]);
   delay(10);
  } 
 }
 lastButtonState[pp] = buttonState[pp];
} 

void loop()
{
 int p;
 for (p = 0; p < 5; p++)
 {
  moveServo(p);
 } 
}

[cyberfish]

But since these photosensors are like variable resistors, can they and/or should they be debounced?

Depends on how you use it. What does the circuit look like? Resistive divider?

They can't be "debounced", but they may need to be filtered.

[cross-side]

I'm not sure I get what resistive dividers are, it seems to be a way to create a ''reference'' voltage, so that if you read a voltage that is different from that reference, then it means something has happened, right/wrong?

If so, I think this is what my circuit does, heres's an image of it from the arduino site;

http://arduino.cc/en/uploads/Tutorial/button_schem.png.

[anduril462]

This site seems to have some good explinations: Volume I - DC : All About Circuits. Your divider circuits are explained in chapter 6, but you might want to read some of the earlier chapters since it seems you don't have a great grasp of Ohm's law.

Hint: when you get to chapter 6, think of your light sensor as a potentiometer in a divider circuit.

[CommonTater]

This site seems to have some good explinations: Volume I - DC : All About Circuits. Your divider circuits are explained in chapter 6, but you might want to read some of the earlier chapters since it seems you don't have a great grasp of Ohm's law.

Hint: when you get to chapter 6, think of your light sensor as a potentiometer in a divider circuit.

Yeah... All About Circuits ... one of the best electronics tutorials on the web...