moving multiple servos through Arduino board

[frogit]

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 
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},          
};


int f, g, h, i, j, k, p;

void setup() 
{
  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)
{
 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]); /*[B]i[B] starts where it was left before and is incremented in steps of 1 degree to control the speed*/
   }
   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()
{
 for (p = 0; p < 5; p++)
 {
  moveServo(p);
 } 
}

[Adak]

Instead of using a number of arrays, I would use just one 2D array for the positions.

Servo 0 would have all it's positions in order, on row 0, cols 0 thru N
Servo 1 would have all it's positions in order, on row 1, cols 0 thru N
Servo 2 would have all it's positions in order, on row 2, cols 0 thru N
etc.

Now a nested pair of for loops

for(col = 0; col < N; col++) {
  for(row = 0; row < 5; row++) {
    //code to move a servo's in here using:
    mov servo[row] to position[row][col] at speed[VarForSpeedHere]; //kind of stuff
  }
}

See if that works. ;)

[frogit]

I'm not sure I understand your advice. Do you mean I should use the 2D positions array only? and not use the pos array?

The problem is that, to have speed control, I need the for loop shown here:

for(pos[i] = lastPos[i]; pos[i] <= positions[pp][i]; pos[i] ++)

I cannot simply point to a position and tell the servo to go there, like your proposal. It does work, but then I have no control over speed, the servos just zoom there.

So the pos array serves as a kind of bank where I store the next value the servos should move to, so in a way, it's a lot like the way you suggest but with a little detour to gain control over speed.

And to have small increments of 1 degree spaced by delays is the only way I found to do this.

But this creates the funky for loop shown above that I have some trouble taming.

Would there be another way of slowing a servo?

[Adak]

My experience is with stepper motors, not servo's. Check your spec doc.

Imo, you need to logically lay out your code and data, for better control. Get a "flow", going, for the servo's, and more clarity in your code, for you. Call your other functions from your main control loops, but don't stick the low level stuff, into the high level loop. That's for you.

What I used were a few high level loops which were the loops I worked with to adjust the programming. Then there were low level functions that were called by the high level functions, that handled most of the speed and other details.

This was for an auto-dialer to open safes (when the department goof-offs had lost the combo), so long sequences were generated, and reliability was a must. No "epileptic squirrels" allowed. ;)

These servos sound like fun, but prone to herky-jerky. Stepper motors are prone to inadequate torque and to lie about completing their move. ;)

[frogit]

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 evryone 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);
 } 
}