Developing in C for the ATmega328P: Servos

13 minute read
, , , ,

Where I describe how to use the servos() interface in AVR C.

Sources

Introduction

Servos are a powerful addition to the embedded programming toolkit as they enable motion. Not high-speed motion as in a electric motor, however, motion which can be easily controlled and typically in an arc or as angles. Servos are much more precise as to how they can move in comparison to electric motors. See Sources above for a few example tutorials as to how to use them.

In this entry, I’m going to demonstrate how to setup a timer to specifically drive two servos as well as demonstrate how to control up to 6 servos using the ATmega328P. This entry is designed to demonstrate how to write code to control servos, as compared to “how to write code to call servo functions”.

Servo Control

A servo is controlled by an signal pulse. There are two criteria for the pulse to be effective and cause the servo to move, a) the pulse must be in a specific frequency range and b) the width of the pulse, which serves to control where it moves, must be in a specific range as well. The best advice I’ve seen as to the frequency is from Elliot Williams:

“Control Pulse Frequency The pulse rate of one control pulse per 20 ms is fairly flexible. My servo works anywhere from 10 ms to about 25 ms. Delays shorter than 10ms seem to mess up the deadband circuitry, and because the motor only engages for a maximum of 25 ms per pulse, longer PWM periods allow the motor to disengage a tiny bit before it turns back on, but you might not even notice this in practice. In short, the inter-pulse timing isn’t critical.” Excerpt From Make: AVR Programming Elliot Williams

In my testing, it is relatively easy to create a 61kHz signal from Timer/Counter 0, and I found I was able to control my Adafruit Analog Feedback Micro Servo using this signal.

The second part is the width of the servo pulse, again, its best to listen to experts and Adafruit advised:

Note that the default servo pulse widths (usually 1ms to 2ms) may not give you a full 180 degrees of motion. In that case, check if you can set your servo controller to custom pulse lengths and try 0.75ms to 2.25ms. You can try shorter/longer pulses but be aware that if you go too far you could break your servo!

In my testing using the above servo, I needed a 500us (.5ms) pulse to move to 0 and a 2.3ms pulse to move to 180. In reality, I believe my servo was moving from 0 degrees to about 160 degrees.

The nice thing about the servo referenced, is that it includes a feedback line which allows you to read where the servo is positioned. While servos are intended to be “precise mechanical steppers”, given it’s $9.95 cost, this servo is not precise nor even accurate at times. It helps to have feedback to know specifically where the servo has moved.

Ultimately, you will want to see a waveform which looks like this:

Waveform of servo signal

Waveform of servo signal

Large Version to see detail

In the image above, the signal is 60.96kHz with a 1.922ms positive pulse width. Another way to think about the signal is it is a 60.96kHz waveform with a 11.7% duty cycle. See Developing in C for the ATmega328P: Function - analogWrite() for understanding PWM and the duty cycle of signals in greater detail.

This specific width and frequency drives the servo to approximate position 425.

Circuit

Here is the circuit for the software:

  1. The servo has four leads, power(red), ground(brown), control(orange) to pin 6 and feedback(white) to A0
  2. The pot has three leads, right(power), left(ground) and center to A2
  3. The Uno power(5V) and ground(GND) are connected to power and ground rails on the breadboard
Waveform of servo signal

Waveform of servo signal

Large Version to see detail

Servo Coding

I’ll walk through the entire servo_0 example, showing the specific line numbers from the file.

Step 1 Setup Timer 0

The first step is to setup Timer 0 and ensure you can get a proper frequency (between 10ms and 25s periods or 40Hz to 100Hz frequency). I looked at the code for setting up Timer 0 for the millis() clock then adjusted from there. Its a simple setup, however, I went for Fast PWM on Timer 0. Once I set it up and the 61kHz signal appeared, I was finished. (For more information as to how to do this in Linux, using bloom and gdb, see this entry.

73
74
75
76
77
78
79
80

// Generate a 61kHz pulse on 6 and 5 using Fast PWM
// OCR0A/B control width of pulse on 6 and 5
// TCCR0A [ COM0A1 COM0A0 COM0B1 COM0B0 0 0 WGM01 WGM00 ] = 0b10100011
TCCR0A = _BV(COM0A1) | _BV(COM0B1) | _BV(WGM01) | _BV(WGM00);
// TCCR0B [ FOC2A FOC2B 0 0 WGM02 CS02 CS01 CS00 ] = 0b00000101
TCCR0B =  _BV(CS02) | _BV(CS00);
OCR0A = PULSE_MIN;
OCR0B = PULSE_MAX;

The next step is to ensure I can control the width of the pulse, which will determine where the servo will move. The easiest method to do this is to simply view the pulse on the scope (Waveforms) and determine which values will give me a range of .5ms to 2.3ms.

Another method to determine if your signal is correct is to use a digital multimeter with a frequency measurement and duty cycle capability. If so, follow these steps:

  1. Measure the frequency, it needs to be in the range 40Hz to 100Hz.
  2. To obtain the period of the frequency use 1/frequency * 1000 to get the period in milliseconds.
  3. Press your DMM mode button to obtain the duty cycle, it will probably be in the range of 2-5%, multiply it times your period in step 2 and this will be the pulse width. The pulse width needs to be between .5ms and 2.3ms.

The variable I need to affect this change is either OCR0A (for pin 6) or OCR0B (for pin 5). For additional details as to how I knew this, see this entry.

I wrote a function which would check the value to ensure it is in the proper range, then it would set OCR0A. I also check the position of the servo by reading an analog port connected to the white wire from the servo. The function returns with this value, after I have delayed sufficiently to allow the servo to move to the position based on the pulse width.

83
84
85
86
87
88
89

uint16_t set_pwm_0(uint8_t angle_value)
{
   uint8_t angle = constrain(angle_value, PULSE_MIN, PULSE_MAX);
   OCR0A = angle;
   delay(SERVO_DELAY);
   return analogRead(SERVO_POS_PIN_0);
}

Control Code

The main function initializes then controls the servo. The servo position is controlled by a value from the potentiometer. As the potentiometer is turned from right to left, the servo moves from 0 degrees to 180 degrees. The values from the pot are 10-bits ranging from 0-1023 and are mapped to values appropriate to the servo from 6 (minimum pulse width of .5ms) to 36 (maximum pulse width of 2.3ms). The delay(1) served as a way for me to have a nop in my code for a breakpoint.

 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103

int main (void)
{   
init_pwms();

while(TRUE)
{
    control_pos = analogRead(POT_PIN);
    servo_pulse = map(control_pos, POT_MIN, POT_MAX, PULSE_MIN, PULSE_MAX);
    servo_pos = set_pwm_0(servo_pulse);
    delay(1);
}
return 0;
}

Debugging with avr-gdb

Programming Gotcha

While I was debugging the program using avr-gdb, I ran across some optimization issues. From time to time, the gcc compiler would optimize out one of my variables which were required for controlling the timer. Maddeningly, it wouldn’t be consistent, I would simply see a message as to a variable and the program wouldn’t work.

Using a breakpoint

There is a delay(1); in the code at line 121, this line was used to allow me to catch a breakpoint after the servo had moved and check the values for the servo position (servo_pos), the pulse width (servo_pulse) and the control pot (control_pos) value. Using these values, I could determine if the program was working properly. My process (in Linux) would be to start bloom as my gdb server, then avr-gdb as my debugger. I would set the breakpoint at 121, display my 3 variables and let the debugger (c)ontinue. It would move the servo then stop and display the values, and example of several iterations of this is below:

(gdb) br 121
Breakpoint 1 at 0x1a8: file main.c, line 121.
Note: automatically using hardware breakpoints for read-only addresses.
(gdb) disp control_pos
1: control_pos = 1023
(gdb) disp servo_pulse
2: servo_pulse = 36 '$'
(gdb) disp servo_pos
3: servo_pos = 499
(gdb) c
Continuing.

Breakpoint 1, main () at main.c:121
121         delay(1);
1: control_pos = 1023
2: servo_pulse = 36 '$'
3: servo_pos = 499
(gdb) c
Continuing.

Breakpoint 1, main () at main.c:121
121         delay(1);
1: control_pos = 0
2: servo_pulse = 6 '\006'
3: servo_pos = 127
(gdb) c
Continuing.

Breakpoint 1, main () at main.c:121
121         delay(1);
1: control_pos = 437
2: servo_pulse = 18 '\022'
3: servo_pos = 273
(gdb)

Complete Program

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124

// servo_0: Demo how to control two servos using Timer/Counter 0

// Circuit setup:
// 1. The servo has four leads, power(red), ground(brown), control(orange)
// to pin 6 and position pot feedback(white) to A0
// 2. The control pot has three leads, right(power), left(ground) and 
// center to A2
// 3. The Uno power(5V) and ground(GND) are connected to power and 
// ground rails on the breadboard

// Overview:
// Setup servo clock to 60.96kHz signal, variable pulse width pins 6(A) and 5(B)
// Read control pot and use map() to go from 10bit ADC value to 8bit OCR0n value
// Set OCR0n with new value and get new position of servo

// Timer 0 Configuration, 8bit Fast PWM p113-7
// TCCR0A - Timer/Counter0 Control Register                 
// [ COM0A1 COM0A0 COM0B1 COM0B0 0 0 WGM01 WGM00 ]          
// TCCR0B - Timer/Counter0 Control Register                 
// [ FOC0A FOC0B 0 0 WGM02 CS02 CS01 CS20 ]                 

// COM0A1:0 = 10 => Clear OC0A on compare match (non-inverting)             
// COM0B1:0 = 10 => Clear OC0B on compare match (non-inverting)
// WGM02:0 = 011 => Fast PWM, Update at Bottom, TOV=MAX   
// CS02:0 = 101 => clkio / (256 * 1024), 101 => 1/1024                          

// PWM Pins p97   
// ARD5 PD5 OC2B 8bit Timer/Counter 0 OCR0B setup but not implemented                       
// ARD6 PD6 OC2A 8bit Timer/Counter 0 OCR0A controls servo 0

// Example parameters                             
// Frequency: 16MHz / 256 / 1024 = 61.35Hz(calc) or 60.96Hz(measured)                  
// Pin 5/PD5/OCR0B=15 has a 5.8% duty cycle or 15/255 => 1.025ms (measured)                    
// Pin 6/PD6/OCR0A=30 has a 11.6% duty cycle or 30/255 => 1.987ms (measured)
#include <avr/io.h>
#include "pinMode.h"
#include "delay.h"
#include "analogRead.h"
#include "sysclock.h"
#include "map.h"

// Parameters for Adafruit 1449 Microservo with feedback
// Two can be controlled, code only controls one at this time
const uint8_t PULSE_MIN = 6;            // min width pulse (.5ms)
const uint8_t PULSE_MAX = 36;           // max width pulse (2.3ms)
const uint16_t SERVO_DELAY = 250;       // allow servo to move 
const uint8_t SERVO_CONTROL_0 = 6;      // servo 0 control pin
const uint8_t SERVO_POS_PIN_0 = A0;     // servo 0 position pin (pot feedback)
const uint8_t SERVO_CONTROL_1 = 5;      // servo 1 control pin
const uint8_t SERVO_POS_PIN_1 = A1;     // servo 0 position pin (pot feedback)
const uint8_t POT_PIN = A2;             // external pot to control servo pos
const uint16_t POT_MAX = 1023;          // max value for control pot
const uint16_t POT_MIN = 0;             // min value for control pot

// In rare situations, variables have been optimized out (determined using gdb)
// Made volatile to ensure they don't
volatile uint16_t control_pos = POT_MIN;
volatile uint16_t servo_pos = 0;
volatile uint8_t servo_pulse = PULSE_MIN;

// use to ensure servo min/max are not exceeded, breaking the servo
uint8_t constrain(uint8_t value, uint8_t min, uint8_t max) {
    if (value < min)
    {
        return min;
    }
    else if (value > max)
    {
        return max;
    }
    else
    {
        return value;
    }
}

void init_servo_clock(void)
{
    pinMode(SERVO_CONTROL_0, OUTPUT);
    pinMode(SERVO_CONTROL_1, OUTPUT);
    pinMode(SERVO_POS_PIN_0, INPUT);
    pinMode(SERVO_POS_PIN_1, INPUT);
    pinMode(POT_PIN, INPUT);

    // reset Timer 0 control registers to ensure clean slate for new timer
    TCCR0A = 0;
    TCCR0B = 0; 

    // Generate a 60.96kHz pulse on 6 and 5 using Fast PWM
    // OCR0A/B control width of pulse on 6 and 5
    // TCCR0A [ COM0A1 COM0A0 COM0B1 COM0B0 0 0 WGM01 WGM00 ] = 0b1010 0011
    TCCR0A = _BV(COM0A1) | _BV(COM0B1) | _BV(WGM01) | _BV(WGM00);
    // TCCR0B [ FOC2A FOC2B 0 0 WGM02 CS02 CS01 CS00 ] = 0b0000 0101
    TCCR0B =  _BV(CS02) | _BV(CS00);
    OCR0A = PULSE_MIN;              // start with servo 0 at 0 degrees
    OCR0B = PULSE_MAX;              // start with servo 1 at 180 degrees
}

// controls servo 0 angle, duplicate to control servo 1
// delay to ensure servo has completed move, before reading position
uint16_t set_servo_pos_0(uint8_t angle_value)
{
   uint8_t angle = constrain(angle_value, PULSE_MIN, PULSE_MAX);
   OCR0A = angle;
   delay(SERVO_DELAY);
   return analogRead(SERVO_POS_PIN_0);
}

int main (void)
{   
    init_servo_clock();

    // get control pot value and convert to pulse width using map()
    // set servo position, function returns new position
    // _NOP() exists as a debugger aid, ensures a line to BP to check values
    while(TRUE)
    {
        control_pos = analogRead(POT_PIN);
        servo_pulse = map(control_pos, POT_MIN, POT_MAX, PULSE_MIN, PULSE_MAX);
        servo_pos = set_servo_pos_0(servo_pulse);
        delay(1);
    }
    return 0;
}

Controlling Upto 6 Servos

This version of the servo control code will control up to 5 servos on any of the digital pins. The control outcome is the same as above, an approximately 50Hz signal with a positive pulse width of .5ms to 2.3ms. To deliver this waveform, one must adjust the following parameters:

  • servo.h:MAX_SERVOS - set the total number of servos between 2-6
  • servo.c:SERVO_PULSE_WIDTH - count required for a frequency of ~52Hz
  • servo.c:set_servo:high_count - count required for desired pulse of .5ms to 2.3ms.

Constants are defined in servo.h with required values. The constants are shown below, 5 servos are being configured:

// Uncomment block of defines based on number of servos
// Parameters create a 52.1Hz frequency with pulse range .5ms to 2.3ms

// Using 2 servos
// #define MAX_SERVOS 2
// #define SERVO_PULSE_WIDTH 600
// #define HIGH_COUNT_MAX 72
// #define HIGH_COUNT_MIN 16

// Using 3 servos
// #define MAX_SERVOS 3
// #define SERVO_PULSE_WIDTH 400
// #define HIGH_COUNT_MAX 50
// #define HIGH_COUNT_MIN 12

// Using 4 servos
// #define MAX_SERVOS 4
// #define SERVO_PULSE_WIDTH 300
// #define HIGH_COUNT_MAX 36
// #define HIGH_COUNT_MIN 8

// Using 5 servos
#define MAX_SERVOS 5
#define SERVO_PULSE_WIDTH 240
#define HIGH_COUNT_MAX 29
#define HIGH_COUNT_MIN 6

// Using 6 servos
// #define MAX_SERVOS 6
// #define SERVO_PULSE_WIDTH 200
// #define HIGH_COUNT_MAX 24
// #define HIGH_COUNT_MIN 5

The process is ensure MAX_SERVOS and SERVO_PULSE_WIDTH are set appropriately then use set_servo() to set high_count for desired pulse width or position of servo. Subsequent calls to set_servo() will move servo to new position based on pulse width. To determine position of servo, it will be important to setup a function which polls the feedback wire and translates the value to a position. This is demonstrated in the first servo code on this page.

Required for examples/multi_servo

  1. In Makefile, on CPPFLAG line add: -DSERVO=$(SERVO)
  2. In env.make add (when not using servo code, change to 0): SERVO = 1
  3. Setup the hardware servo using wiring similar to the Circuit above
  4. Compile-link-load examples/multi_servo
  5. Open a serial terminal program (i.e, CoolTerm) and follow the prompts:
Enter servo and angle: EX:2 120 Servo 0 to angle   0    Pulse width: 15 Servo Pos: 108
Enter servo and angle: EX:2 120 Servo 0 to angle 180    Pulse width: 72 Servo Pos: 491
Enter servo and angle: EX:2 120 Servo 0 to angle   0    Pulse width: 15 Servo Pos: 108
Enter servo and angle: EX:2 120 Servo 0 to angle  90    Pulse width: 43 Servo Pos: 307
Enter servo and angle: EX:2 120 Servo 0 to angle  45    Pulse width: 29 Servo Pos: 211
Enter servo and angle: EX:2 120 Servo 0 to angle 112    Pulse width: 50 Servo Pos: 353

This code uses Timer/Counter 0 which conflicts with using tone().

Comments powered by Talkyard.