WebbotLib AVR library
WebbotLib It just does it
  C++ documentation  C documentation

servos.h

This is a servo driver for controlling servos that are plugged directly into your board and uses either software PWM, or hardware PWM, to send commands to each servo.
N.B. If you are using a commercial servo controller then refer to Servos.
The steps required to use any of the in-built servo functions of the library are:-
1. Define each servo using the MAKE_SERVO command
2. Put all of the servos into a list using SERVO_LIST
3. Create a driver for this list of servos using MAKE_SERVO_DRIVER
Finally we initialise the driver, and hence all of the servos, by calling the appropriate 'init' routine described below.
Sofware PWM
For software PWM each servo in the list can be attached to any I/O pin you like.
Initialise the driver by calling servosInit(SERVO_DRIVER* driver, const Timer* timer)
Hardware PWM
For hardware PWM each servo must be connected to a different 16 bit hardware PWM pin.
Initialise the driver by calling servoPWMInit(SERVO_DRIVER* driver)
Multiplexed Hardware PWM
This driver only uses only one 16 bit hardware PWM pin to drive up to 8 different servo - but it does require an extra hardware chip the 74HC238:-
muxServos.jpg
The hardware PWM pin from your controller is connected to pin 6 of the device. Pins 1,2 and 3 of the device are used to control which of the output servo pins receives each PWM pulse. By attaching pins 1, 2 and 3 to digital output pins on your micro-controller then WebbotLib automatically sets these pins to make sure that the PWM signal is sent to the correct servo.
The number of digital pins required depends on how many servos you want to control:-
1 servo - then MUX1, MUX2, MUX3 are not used
2 servos - connect MUX1 to a digital I/O pin on your micro-controller, MUX2 and MUX3 are not used
3 to 4 servos - connect MUX1 and MUX2 to digital I/O pins on your micro-controller, MUX3 is not used
5 to 8 servos - connect MUX1, MUX2 and MUX3 to digital I/O pins on your micro-controller
If any of the MUX pins aren't required then they should be made 'low' either by connecting them to ground or by using a pull-down resistor( say 100k but any value would do).
The driver can be initialised by calling servosMuxInit(SERVO_DRIVER* driver, const IOPin* pwm16, const IOPin* mux1, const IOPin* mux2, const IOPin* mux3)
Which method is best?
Hardware PWM tends to be 'more exact' than software PWM and requires less processing overhead. However: a processor only tends to have a handful of hardware PWM pins which may not be enough for your needs.
The next method would be the 'multiplexed' approach - as this allows more servos with fewer hardware PWM pins, plus a few digital I/O pins. It does require a bit more processing overhead, to manage the MUX lines, but it also depends on whether you want to add the extra hardware required.
The fall-back position is to use software PWM as this can be used to generate a PWM signal on any pin. However it does require more processing overhead and you are limited by the number of 16 bit timers available. Software PWM signals are not as accurate as hardware PWM signals because they are effected by other devices (ie interrupts from UARTs, timers etc) although with 'modified servos' this isn't noticeable.
Examples
It's very easy to use: you just give it a list of servos and an available 16 bit timer and that's it. Whether you are using hardware or software PWM then the pulses are sent out automatically in the background.
Once initialised then each servo can be controlled individually using the actuator commands. See actuators.h. As discussed in that section, a servo is handled the same way whether it is a modified servo or an unmodified servo. For an unmodified servo then just remember that the act_SetSpeed function sets the position of the servo rather than the speed.
Here is an example of the code in your application for hardware or software PWM:-
#include "servos.h"
// Define two servos
SERVO left = MAKE_SERVO(FALSE, D1,1500, 500);
SERVO right = MAKE_SERVO(TRUE , D2,1500, 500);
 
// Create the list - remember to place an & at the
// start of each servo name, and to use PROGMEM
SERVO_LIST PROGMEM servos[] = {&left,&right};
 
// Create a driver for the list of servos
SERVO_DRIVER bank1 = MAKE_SERVO_DRIVER(servos);
 
// In my initialising code - pass the list of servos to control
void appInitHardware(void){
    // Initialise the servo controller using software PWM
    servosInit(&bank1, TIMER1);
// OR use this line instead for hardware PWM
    servoPWMInit(&bank1);
}
// Initialise the servos
TICK_COUNT appInitSoftware(TICK_COUNT loopStart){
    // Give each servo a start value - make them go full ahead
    act_setSpeed(&left,DRIVE_SPEED_MAX);
    act_setSpeed(&right,DRIVE_SPEED_MAX);
    return 0;
}
 
// This is the main loop
TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart){
    .. nothing to do. But in reality I would use act_speed to change the speed of each servo ...
    return 0; // no delay
}
Here is an example of the code in your application for multiplexed hardware PWM:-
#include "servos.h"
// Define two servos - they dont need I/O lines as they are connected to the multiplexer chip
SERVO left = MAKE_REMOTE_SERVO(FALSE, 1500, 500);
SERVO right = MAKE_REMOTE_SERVO(TRUE , 1500, 500);
 
// Create the list - remember to place an & at the
// start of each servo name, and to use PROGMEM
SERVO_LIST PROGMEM servos[] = {&left,&right};
 
// Create a driver for the list of servos
SERVO_DRIVER bank1 = MAKE_SERVO_DRIVER(servos);
 
// In my initialising code - pass the list of servos to control
void appInitHardware(void){
    // Initialise the servo controller using hardware PWM on D1
    // With only two servos we only need to specify 1 x MUX line - on B0
    servosMuxInit(&bank1, D1, B0, null, null);
}
// Initialise the servos
TICK_COUNT appInitSoftware(TICK_COUNT loopStart){
    // Give each servo a start value - make them go full ahead
    act_setSpeed(&left,DRIVE_SPEED_MAX);
    act_setSpeed(&right,DRIVE_SPEED_MAX);
    return 0;
}
 
// This is the main loop
TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart){
    .. nothing to do. But in reality I would use act_speed to change the speed of each servo ...
    return 0; // no delay
}
Note that if you have a lot of servos then you can break them down into separate lists, or 'banks' each with their own driver and timer.
When using software PWM to control a large number of servos, ie more than 10, then research suggests that performance is optimal when you split the servos into several banks with each bank containing the same number of servos.

 

Function

 


MAKE_SERVO(inverted, iopin, center, range)

Define a servo.
The parameters are:
inverted - TRUE or FALSE
iopin - The processor pin it is connected to. e.g. B5 or D2 etc
center - The center point of the servo. See below.
range - The range on either side of center. It should never be greater than 'center'. See below
For example: if you have a differential drive robot with servo motors on D2 and D3 then we could define them in our code as follows:-
SERVO left = MAKE_SERVO(FALSE, D2, 1500, 500);
SERVO right = MAKE_SERVO(TRUE, D3, 1500, 500);
Having defined the servos then we can set the speed etc with other functions in this file. However: the servo will not start moving until it has been attached to some other code that decides how to drive it - ie is it hardware PWM or software PWM or some other method.
Assuming that the servo is attached to a driver then what do those 'center' and 'range' parameters mean? The 'center' parameter is a number and indicates the pulse width (in microseconds) required to move the servo to its center position. For most servos this would be '1500' ie 1.5ms. The 'range' parameter says how much the pulse width changes either side of center'. For most servos this would be '500' ie 0.5ms - meaning the pulse width varies between 1ms and 2ms.
In practise you will need to fiddle with these values for each of your servos as they may not all be setup in the same way. Assuming you have modified a servo for continuous rotation then here's the best way to set these values.
1. Set the center=1500 and the range=0
2. Run the program
3. The servos should be stationary. If not then adjust the center value until they stop moving
4. Now change the 'range' value to 500 and change your program to set both motors to DRIVE_SPEED_MAX
5. The motors should now turn in the same direction. If not then you've probably forgotten to invert one of the servos.
6. If your servos dont rotate at the same speed then try decreasing the range value of the fastest servo until they do.

servosInit(SERVO_DRIVER* driver, const Timer* timer)

Drive the list of servos using software PWM.
The servos parameter is the list of servos that it controls.
The Timer parameter is ANY available 16 bit timer. Look at the 'sys/*.h' for your board and locate any 16 bit timer - it doesn't matter if it has an IO Pin output or not as the timer is only used to generate internal interrupts to drive the servos. The library will use the first two channels of the timer.
If in doubt then use TIMER1 as this works on most devices.

servoPWMInit(SERVO_DRIVER* driver)

Tells the servo driver what servos it controls.
Since this uses hardware PWM to drive each servo then there is a restriction as to what IO pins the servos can be connected to.
Look at the timer list in your 'sys/*.h'. Your servos must be connected to one of the 16 bit timer channels that has an IO pin to connect it to. Use of other pins will cause a runtime error.

servosMuxInit(SERVO_DRIVER* driver, const IOPin* pwm16, const IOPin* mux1, const IOPin* mux2, const IOPin* mux3)

Allows you to use one 16 bit hardware PWM channel to control up to 16 servos.
This requires the use of a de-multiplexer chip like the 74HC238. This chip has several inputs: the main one being the hardware PWM input and the remainder being up to three digital I/O select pins. These three I/O pins dictate which of the eight 74HC238 output pins the servo PWM signal will appear on.
Each output pin can send a PWM signal of up to 2.5ms and given that there are 8 outputs (whether used or not) then the cycle is repeated every 8 * 2.5ms = 20ms.

servoSerialInit(SERVO_DRIVER* driver, UART* uart, BAUD_RATE baud, SERVO_PROTOCOL protocol)

Initialise a bank of servos being driven by serial servo controller.
See Generic/Serial Servo Controller for an example of how to use it.

servosSetConnected(const SERVO_DRIVER* driver, boolean connect)

Connect or disconnect a group of servos.
This is the equivalent of calling act_isConnected(const __ACTUATOR_COMMON* act) for each servo that is attached to the driver.
The second parameter should to TRUE to 'connect' the servos and FALSE to 'disconnect' the servos.
Two shorthand forms exist:
servosConnect(driver); // To start send pulses to the servos
servosDisconnect(driver); // To stop sending pulses to the servos

servosSetSpeed(const SERVO_DRIVER* driver, DRIVE_SPEED speed)

Set the speed for a group of servos.
This is the equivalent of calling act_setSpeed(__ACTUATOR_COMMON* act, DRIVE_SPEED speed) for each servo that is attached to the driver so that all the servos have the same speed.

servoSetConfig(SERVO* servo, uint16_t center, uint16_t range)

Changes the servo centre position and range at runtime.
Allows you to change the configuration of your servo at any time. You may want to do this, for example, because you have stored the servo settings in EEPROM and want to revert to those values rather than the values specified in your MAKE_SERVO constructor.
Example. Assume we have already created a servo called 'servo1' then we can modify its centre position to 1450 with a range of 475 by calling:
servoSetConfig(&servo1, 1450, 475);

servosCenter(SERVO_LIST* const servos, UART* uart)

A 'robot-side' function that talks over a UART to a terminal program that helps you to configure the centre and range of any servos you are using.
All of the servos should be set up and configured as described elsewhere in this manual. You should then call this function from your appControl main loop.
Here is an example of a program for the AxonII that uses this function:-
#include <sys/Axon2.h>
 
// Connect to terminal program via this uart at 115200 baud
#define THE_UART UART3
 
#include <servos.h>
#include <_uart_common.h>
 
// Define servos for the left side of my Brat humanoid
SERVO servo1 = MAKE_SERVO(false,B5,1500,650);
SERVO servo2 = MAKE_SERVO(false,B6,1500,650);
SERVO servo3 = MAKE_SERVO(false,B7,1500,650);
static SERVO_LIST PROGMEM bank1_list[] = {&servo1,&servo2,&servo3};
SERVO_DRIVER bank1 = MAKE_SERVO_DRIVER(bank1_list);
 
// Define servos for the right side of my Brat humanoid
SERVO servo4 = MAKE_SERVO(true,E3,1500,650);
SERVO servo5 = MAKE_SERVO(true,E4,1500,650);
SERVO servo6 = MAKE_SERVO(true,E5,1500,650);
static SERVO_LIST PROGMEM bank2_list[] = {&servo4,&servo5,&servo6};
SERVO_DRIVER bank2 = MAKE_SERVO_DRIVER(bank2_list);
 
// Create a list of ALL servos across ALL banks
// The ordering in this list is the ordering used by the terminal program
SERVO_LIST PROGMEM all[] = {&servo1,&servo2,&servo3,&servo4,&servo5,&servo6};
 
//initialize hardware, ie servos, UART, etc.
void appInitHardware(void){
    servosInit(&bank1, TIMER1);
    servosInit(&bank2, TIMER3);
 
    //setup UART on USB at the required baud rate
    uartInit(THE_UART, 115200);
}
 
TICK_COUNT appInitSoftware(TICK_COUNT loopStart){
    return 0;
}
 
// This is the main loop
TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart){
    servosCenter(all,THE_UART); // Talk to the terminal program
    return 0;
}
From your terminal program (eg HyperTerminal) the following commands are available:
L - This will list all of the servos with their current centre and range values
N - Select the next servo
P - Select the previous servo
C - Centring mode
R - Ranging mode
+ - Add 1 to the current centre or range value
* - Add 10 to the current centre or range value
- - Subtract 1 from the current centre or range value
/ - Subtract 10 from the current centre or range value
Any other key will display the above list
Once you have finished setting up the servos you should issue the L command and note down the centre and range values for each servo. Go back to your code and enter these values in the servo definitions.

Valid XHTML 1.0 Transitional