1. How a servo works?
The servo's structure is simpler than you think. A DC motor whose shaft connected to a potentiometer via a group of gears. So the shaft's position represents the resistance value of the potentiometer. For sure the resistance can be measured by a voltage, let's call it Vout.
The servo has an input line, whose voltage is compared to Vout, if the two voltages don't equal, then the motor runs, which results in the resistance change of the potentiometer, and finally changes the Vout. When Vout equals the input voltage, the DC motor stops.
2. SG90
SG90 servos are popular in the hobby world due to its lower price and decent performance. The input line accepts PWM to emulate analog voltage input, which is common in DC motor control.
Basic parameters:- VCC: +5V
- PWM frequency: 50Hz (so T = 20ms = 20,000 us)
- duty cycle range: 500 us ~ 2400 us (2.5% ~ 12%)
- Rotation range: 0° ~ 180°.
The confusing thing is that most datasheets of SG90 on the internet are wrong about the duty cycle range.
From the parameters above, we can see that the duty cycle's range is (2.4-0.5=1.9ms). So in order to get precise positioning, we need accurate control of this time frame. E.g if we set a 1ms timer, for sure it's not good enough to get any control. If we set a 100 us(0.1 ms) timer, then we have 1.9/0.1 = 19 positions in control. If we set a 1 us (0.001ms) timer, we have 1.9/0.001 = 1900 positions, which means (180°/1900) accuracy.
However, a 1 us timer would affect CPU's performance, so maybe 100us is good enough.
3. Three different ways of PWM
To generate PWM output, there are 3 different ways.
3.1 _delay_us( )
void main()
{
DDRB |= 1; // pin8 as output
while(1){
PORTB |= 1; // output 5V
_delay_us(1450);
PORTB &= ~1; // output 0V
_delay_us(20000-1450);
}
}
This will generate a PWM output, but the CPU cannot do anything else. Certainly, it's not a good idea.
- 100% CPU waste
- not accurate (needs to take account calculate instructions cycles)
3.2 timer-based PWM
This method utilizes the timer interrupt to output PWM.
- more accurate
- less CPU time consumption
- can control many servos at the same time (any GPIO port works)
- The ISR must be done within the timer interval (here 100 us)
#include <avr/io.h>
#define F_CPU 16000000UL
#include <util/delay.h>
#include <avr/interrupt.h>
static void on_10us();
static unsigned int duty_clicks = 0;
/* 100 us timer */
/*
* * Make sure the handler can be done within 100us, about
* * 100*16 clk = 1600 instructions
* * avoid float calculations here.
* */
ISR(TIMER0_OVF_vect)
{
TCNT0 = 231;
on_100us();
}
void start_timer()
{
TCNT0 = 231;
TCCR0A = 0;
TIMSK0 = (1<<TOIE0);
TCCR0B = 0x03;
sei();
}
static void on_100us()
{
static unsigned int _100us = 0;
if(_100us == 200){
_100us = 0;
}
if(_100us == 0){
PORTB |= 1;
}
if(_100us == duty_clicks){
PORTB &= ~1;
}
_100us ++;
}
void servo_go(float angle)
{
float duty_us = 500 + (angle/180.0)*(2400-500);
duty_clicks = (unsigned int)(duty_us/100);
}
int main()
{
DDRB |= 1;
start_timer();
while(1){
servo_go(0);
_delay_ms(1000);
servo_go(90);
_delay_ms(1000);
servo_go(180);
_delay_ms(1000);
}
}
3.3 hardware-based PWM
This method depends completely on hardware to output PWM, so it doesn't consume any CPU time!
- Fully hardware implementation, no CPU time needed
- Only limited specific PINs work ( Pin 10 / OC1B / PB2 )
As SG90 needs 50Hz PWM, this is not possible with 8-bits timer (Timer0, Timer2). So we go to the only 16bits timer, Timer1. The PWM output pin is pin 10/PB2/OC1B.
Only WGM mode 15 can generate such a low 50Hz PWM. Let's calculate the configuration.
50Hz = 16MHz / N*(OCR1A+1)
so,
N*(OCR1A+1) = 16M/50 = 320,000
As we want finer graduality, the OCR1A should be as big as possible while the N is as small as possible.
Let N = 1, then OCR1A = 319999 > 65535 (biggest 16bits number), so it's not acceptable.
Let N = 8, then OCR1A = 39999 < 65535, good!
source code:
#include <avr/io.h>
#define F_CPU 16000000UL
#include <util/delay.h>
#define CYCLE_US 20000 /* 20 ms */
#define DUTY_MIN_US 500 /* min duty 500 us */
#define DUTY_MAX_US 2400 /* min duty 2400 us */
#define CYCLE_CLICKS 40000 /* clicks within 20 ms */
void servo_go(float angle)
{
float duty_us = (angle/180.0) * (2400-500) + 500;
float us_per_click = (float)CYCLE_US / (float)CYCLE_CLICKS;
unsigned int duty_clicks = (unsigned int)(duty_us / us_per_click);
OCR1BH = duty_clicks >> 8;
OCR1BL = duty_clicks & 0x00FF;
}
int main()
{
DDRB |= (1<<2); // pin 10
OCR1AH = (CYCLE_CLICKS-1) >> 8;
OCR1AL = (CYCLE_CLICKS-1) & 0x00FF;
OCR1BH = 2000 >> 8;
OCR1BL = 2000 & 0x00FF;
TCCR1A = 0x23;
TCCR1B = 0x1A; // Mode 15, clock=8
while(1){
servo_go(0);
_delay_ms(1000);
servo_go(90);
_delay_ms(1000);
servo_go(180);
_delay_ms(1000);
}
}
No comments:
Post a Comment