3 Servo Sweep

Good evening. Hope to get some help with a simple arduino uno rev3 project that has 3 small servos that will sweep between 0-90 degrees and I would like to have them sweeping continuously and at the same time, with some control over direction and angle as well as delay in between each of the three to offset the movements. What’s the best way to adapt the following sample sweep code so that I loop the three? Thank you!

// Sweep3

#include <Servo.h>

Servo servo1; // create servo object to control head
Servo servo2; // create servo object to control torso
Servo servo3; // create servo object to control legs

int pos = 0;    // variable to store the servo position

void setup() {
  servo1.attach(3);  // attaches the servo on pin 3 to the servo1
  servo2.attach(5);  // attaches the servo on pin 5 to the servo2
  servo3.attach(6); // attaches the servo on pin 6 to the servo3
}

void loop() {
  for (pos = 0; pos <= 90; pos += 1) { // goes from 0 degrees to 90 degrees
    // in steps of 1 degree
    servo1.write(pos);                 // tell servo to go to position in variable 'pos'   
    delay(8);                          // waits 8ms for the servo to reach the position
  }
  for (pos = 90; pos >= 0; pos -= 1) { // goes from 90 degrees to 0 degrees
    servo1.write(pos);                 // tell servo to go to position in variable 'pos'
    delay(8);                          // waits 8ms for the servo to reach the position
  }
}

One way is to use arrays and a structure.

This compiles but is not tested. Explanatory comments inline:

// Sweep3
#include <Servo.h>

#define NUM_SERVOS      3

Servo servo1; // create servo object to control head
Servo servo2; // create servo object to control torso
Servo servo3; // create servo object to control legs

typedef struct structServo
{
    Servo           *pServo;                //pointer to a servo object
    byte            cPin;                   //pin number associated with servo object
    bool            bDir;                   //true=increment, false=decrement
    int             nCurrPos;               //current stepper position
    int             nMinAngle;              //minimum angle
    int             nMaxAngle;              //maximum angle
    unsigned long   ulTimeStepServo;        //time delay between steps (uS)
    unsigned long   ulTimeServo;            //object-specfic time stamp
    
}sServo;

//initialize servo array
sServo grServos[] =
{
    {
        .pServo = (Servo *)&servo1,
        .cPin = 3,
        .bDir = false,
        .nCurrPos = 45,
        .nMinAngle = 0,
        .nMaxAngle = 90,
        .ulTimeStepServo = 8000,    //8mS per step
        .ulTimeServo = 0
                 
    },
    {
        .pServo = (Servo *)&servo2,
        .cPin = 5,
        .bDir = true,
        .nCurrPos = 0,
        .nMinAngle = 25,
        .nMaxAngle = 75,
        .ulTimeStepServo = 5000,  //5mS per step
        .ulTimeServo = 0
        
    },
    {
        .pServo = (Servo *)&servo3,
        .cPin = 6,
        .bDir = false,
        .nCurrPos = 65,
        .nMinAngle = 10,
        .nMaxAngle = 65,
        .ulTimeStepServo = 20000,    //20mS per step
        .ulTimeServo = 0
        
    }
};

unsigned long
    timeNow;
byte
    cIdx = 0;

void setup( void ) 
{
    //attach servos and set initial position
    for( int i=0; i<NUM_SERVOS; i++ )
    {
        grServos[i].pServo->attach( grServos[i].cPin );
        grServos[i].pServo->write( grServos[i].nCurrPos );
        
    }//for
    
}//setup

void loop( void ) 
{
    //get the current micros counter
    timeNow = micros();

    //is it time to step the servo pointed to by cIdx
    if( (timeNow - grServos[cIdx].ulTimeServo) >= grServos[cIdx].ulTimeStepServo )
    {
        //yes; record time stamp
        grServos[cIdx].ulTimeServo = timeNow;

        //counting up?
        if( grServos[cIdx].bDir == true )
        {
            //yes; bump up current position
            grServos[cIdx].nCurrPos++;

            //and limit it to the max specified
            if( grServos[cIdx].nCurrPos >= grServos[cIdx].nMaxAngle )
            {
                //if we hit the limit, change direction
                grServos[cIdx].nCurrPos = grServos[cIdx].nMaxAngle;                
                grServos[cIdx].bDir = false;
                
            }//if
                                            
        }//if
        else
        {
            //decrementing
            grServos[cIdx].nCurrPos--;
            if( grServos[cIdx].nCurrPos <= grServos[cIdx].nMinAngle )
            {
                //same logic as increment but for min angle
                grServos[cIdx].nCurrPos = grServos[cIdx].nMinAngle;
                grServos[cIdx].bDir = true;
                
            }//if
            
        }//else

        //set servo position
        grServos[cIdx].pServo->write( grServos[cIdx].nCurrPos );
        
    }//if

    //each pass we deal with a different servo; bump index (cIdx)
    cIdx++;
    //when we've finished the last servo go back to beginning
    if( cIdx == NUM_SERVOS )
        cIdx = 0;
        
}//loop

Below is some simple servo test code using the serial monitor for two servos. You almost copy/paste a third servo if desired. Characters a,b,c, d, and r are sent to the servos from the sserial monitor to increase or decrease the servo positions. send a string of characters like aaaaaaaaaaabbbbbbbbbbbbcccccccccccccdddddddddddr (copy and paste in the serial monitor then send) to see how it works.

#include <Servo.h>

Servo myservo1;
Servo myservo2;

int pos1 = 90;
int pos2 = 90;
int delayPeriod = 50;  // increasing this slows down the servo movement
char c;


void setup()
{
  Serial.begin(9600);
  myservo1.attach(8);  // attaches the servo on pin 8 to the servo object
  myservo2.attach(9);  // attaches the servo on pin 9 to the servo object
  myservo1.write(pos1); // center the servo1
  myservo2.write(pos2); // center the servo2
}


void loop()
{
  if (Serial.available())  {

    char c = Serial.read();  //gets one byte from serial buffer
    Serial.print("received character is ");
    Serial.println(c);
    //}

    if (c == 'a')
    {
      // in steps of 1 degree
      if ( pos1 > 0)
        --pos1;
      myservo1.write(pos1);              // tell servo to go to position in variable 'pos'
      Serial.print("servo 1 is at ");
      Serial.println(pos1);
      //delay(delayPeriod);
    }
    if (c == 'b')
    {
      if ( pos1 < 180)
        ++pos1;
      myservo1.write(pos1);        // tell servo to go to position in variable 'pos'
      Serial.print("servo 1 is at ");
      Serial.println(pos1);
      //delay(delayPeriod);
    }

    //

    if (c == 'c')
    {
      // in steps of 1 degree
      if ( pos2 > 0)
        --pos2;
      myservo2.write(pos2);              // tell servo to go to position in variable 'pos'
      Serial.print("servo 2 is at ");
      Serial.println(pos2);
      //delay(delayPeriod);
    }
    if (c == 'd')
    {
      if ( pos2 < 180)
        ++pos2;
      myservo2.write(pos2);              // tell servo to go to position in variable 'pos'
      Serial.print("servo 2 is at ");
      Serial.println(pos2);
      //delay(delayPeriod);
    }

    //

    if (c == 'r')
    {
      myservo1.write(90);
      myservo2.write(90);
      Serial.println("servos reset at 90 deg.");
    }
  }
}

Thank you for your help. This produces some funky results with 3 servos, followed by them slowing down and eventually making individual steps and almost stalling. I was hoping for something a bit more simple, this code seems far more complex the original sweep example. I would like to just sweep 3 at the same time with individual control (concurrently). Thank you.

Blackfin:
One way is to use arrays and a structure.

This compiles but is not tested. Explanatory comments inline:

// Sweep3

#include <Servo.h>

#define NUM_SERVOS      3

Servo servo1; // create servo object to control head
Servo servo2; // create servo object to control torso
Servo servo3; // create servo object to control legs

typedef struct structServo
{
    Servo          *pServo;                //pointer to a servo object
    byte            cPin;                  //pin number associated with servo object
    bool            bDir;                  //true=increment, false=decrement
    int            nCurrPos;              //current stepper position
    int            nMinAngle;              //minimum angle
    int            nMaxAngle;              //maximum angle
    unsigned long  ulTimeStepServo;        //time delay between steps (uS)
    unsigned long  ulTimeServo;            //object-specfic time stamp
   
}sServo;

//initialize servo array
sServo grServos =
{
    {
        .pServo = (Servo *)&servo1,
        .cPin = 3,
        .bDir = false,
        .nCurrPos = 45,
        .nMinAngle = 0,
        .nMaxAngle = 90,
        .ulTimeStepServo = 8000,    //8mS per step
        .ulTimeServo = 0
               
    },
    {
        .pServo = (Servo *)&servo2,
        .cPin = 5,
        .bDir = true,
        .nCurrPos = 0,
        .nMinAngle = 25,
        .nMaxAngle = 75,
        .ulTimeStepServo = 5000,  //5mS per step
        .ulTimeServo = 0
       
    },
    {
        .pServo = (Servo *)&servo3,
        .cPin = 6,
        .bDir = false,
        .nCurrPos = 65,
        .nMinAngle = 10,
        .nMaxAngle = 65,
        .ulTimeStepServo = 20000,    //20mS per step
        .ulTimeServo = 0
       
    }
};

unsigned long
    timeNow;
byte
    cIdx = 0;

void setup( void )
{
    //attach servos and set initial position
    for( int i=0; i<NUM_SERVOS; i++ )
    {
        grServos[i].pServo->attach( grServos[i].cPin );
        grServos[i].pServo->write( grServos[i].nCurrPos );
       
    }//for
   
}//setup

void loop( void )
{
    //get the current micros counter
    timeNow = micros();

//is it time to step the servo pointed to by cIdx
    if( (timeNow - grServos[cIdx].ulTimeServo) >= grServos[cIdx].ulTimeStepServo )
    {
        //yes; record time stamp
        grServos[cIdx].ulTimeServo = timeNow;

//counting up?
        if( grServos[cIdx].bDir == true )
        {
            //yes; bump up current position
            grServos[cIdx].nCurrPos++;

//and limit it to the max specified
            if( grServos[cIdx].nCurrPos >= grServos[cIdx].nMaxAngle )
            {
                //if we hit the limit, change direction
                grServos[cIdx].nCurrPos = grServos[cIdx].nMaxAngle;               
                grServos[cIdx].bDir = false;
               
            }//if
                                           
        }//if
        else
        {
            //decrementing
            grServos[cIdx].nCurrPos–;
            if( grServos[cIdx].nCurrPos <= grServos[cIdx].nMinAngle )
            {
                //same logic as increment but for min angle
                grServos[cIdx].nCurrPos = grServos[cIdx].nMinAngle;
                grServos[cIdx].bDir = true;
               
            }//if
           
        }//else

//set servo position
        grServos[cIdx].pServo->write( grServos[cIdx].nCurrPos );
       
    }//if

//each pass we deal with a different servo; bump index (cIdx)
    cIdx++;
    //when we’ve finished the last servo go back to beginning
    if( cIdx == NUM_SERVOS )
        cIdx = 0;
       
}//loop

Give yourself a break.

Servo headServo;
Servo torsoServo;
Servo legServo;

briancnc:
Thank you for your help. This produces some funky results with 3 servos, followed by them slowing down and eventually making individual steps and almost stalling. I was hoping for something a bit more simple, this code seems far more complex the original sweep example. I would like to just sweep 3 at the same time with individual control (concurrently). Thank you.

Tested it with a scope on my Mega2560; works fine here.

What Arduino are you using?

Note that you will need to power the servos from an adequate power supply similar to below.

servo power.jpg

servo power.jpg