moving multiple servos through Arduino board

This is a discussion on moving multiple servos through Arduino board within the C Programming forums, part of the General Programming Boards category; Hello, I am working on a project that involves 5 servomotors that need to move in a coordinated way. The ...

  1. #1
    Registered User
    Join Date
    Feb 2011
    Posts
    11

    moving multiple servos through Arduino board

    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


    Code:
          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

    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);
     } 
    }
    Last edited by cross-side; 02-14-2011 at 02:34 PM.

  2. #2
    Registered User
    Join Date
    May 2009
    Posts
    2,765
    Where do you use buttonState to select what you want to have happen.

    Tim S.

  3. #3
    Registered User
    Join Date
    Feb 2011
    Posts
    11
    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?)

  4. #4
    Registered User
    Join Date
    Nov 2010
    Location
    Long Beach, CA
    Posts
    5,676
    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.

  5. #5
    Registered User
    Join Date
    Feb 2011
    Posts
    11
    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
    Last edited by cross-side; 02-14-2011 at 01:55 PM.

  6. #6
    Banned
    Join Date
    Aug 2010
    Location
    Ontario Canada
    Posts
    9,547
    Quote Originally Posted by cross-side View Post
    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.

  7. #7
    Registered User
    Join Date
    Feb 2011
    Posts
    11
    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.
    Code:
    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...

  8. #8
    Banned
    Join Date
    Aug 2010
    Location
    Ontario Canada
    Posts
    9,547
    Quote Originally Posted by cross-side View Post
    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.

  9. #9
    Registered User
    Join Date
    Nov 2010
    Location
    Long Beach, CA
    Posts
    5,676
    Quote Originally Posted by cross-side View Post
    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.

    Code:
    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:
    Code:
    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:
    Code:
    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)

  10. #10
    Registered User
    Join Date
    Dec 2006
    Location
    Canada
    Posts
    3,183
    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.

  11. #11
    Registered User
    Join Date
    Feb 2011
    Posts
    11

    problem solved! (almost)

    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

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

  12. #12
    Registered User
    Join Date
    Dec 2006
    Location
    Canada
    Posts
    3,183
    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.

  13. #13
    Registered User
    Join Date
    Feb 2011
    Posts
    11
    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.

  14. #14
    Registered User
    Join Date
    Nov 2010
    Location
    Long Beach, CA
    Posts
    5,676
    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.

  15. #15
    Banned
    Join Date
    Aug 2010
    Location
    Ontario Canada
    Posts
    9,547
    Quote Originally Posted by anduril462 View Post
    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...

Page 1 of 2 12 LastLast
Popular pages Recent additions subscribe to a feed

Similar Threads

  1. Replies: 16
    Last Post: 06-08-2009, 04:03 PM
  2. Phantom redefinition
    By CodeMonkey in forum C++ Programming
    Replies: 6
    Last Post: 06-12-2005, 06:42 PM
  3. Linker errors - Multiple Source files
    By nkhambal in forum C Programming
    Replies: 3
    Last Post: 04-24-2005, 03:41 AM
  4. Moving files from one directory to multiple dirs
    By csj561 in forum C Programming
    Replies: 7
    Last Post: 03-18-2005, 03:52 PM
  5. Replies: 1
    Last Post: 05-01-2003, 03:52 PM

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21