root / paintboard / code / servo.c @ d9b50611
History | View | Annotate | Download (1.94 KB)
1 |
#include <avr/io.h> |
---|---|
2 |
#include <avr/interrupt.h> |
3 |
#include <stdlib.h> |
4 |
#include "servo.h" |
5 |
|
6 |
/*
|
7 |
|
8 |
Servo controller. Timer0 is set to fast PWM mode, with 64 prescaler. Outputs
|
9 |
are cleared on BOTTOM and set on compare match, and the compare values are set
|
10 |
to 255 minus the regular value. This way the timer can be stopped when it
|
11 |
overflows and the output will end up off, but will have been on for the correct
|
12 |
amount of time.
|
13 |
|
14 |
SERVO_PWM1: PD6 (Timer0)
|
15 |
|
16 |
SERVO_PWM2: PD5 (Timer0)
|
17 |
|
18 |
*/
|
19 |
|
20 |
#define PRESCALER 64 |
21 |
#define MS_TO_TICKS(x) ((uint16_t)((x) * (F_CPU / 1000 / PRESCALER))) |
22 |
|
23 |
#define MIN_TICKS MS_TO_TICKS(1) |
24 |
#define MAX_TICKS MS_TO_TICKS(2) |
25 |
|
26 |
static uint8_t angle_to_ticks(int8_t angle)
|
27 |
{ |
28 |
return ((int16_t)angle + 128) * (MAX_TICKS - MIN_TICKS) / 256 + MIN_TICKS; |
29 |
} |
30 |
|
31 |
int8_t servo1_angle; |
32 |
int8_t servo2_angle; |
33 |
|
34 |
void servo_init()
|
35 |
{ |
36 |
// WGM0 0b011 (fast pwm)
|
37 |
// COM0A 0b11 (set OCR0A on compare match, clear on BOTTOM)
|
38 |
// COM0B 0b11 (set OCR0B on compare match, clear on BOTTOM)
|
39 |
// B is temporarily disabled grrrrr
|
40 |
//TCCR0A = _BV(WGM01) | _BV(WGM00) | _BV(COM0A1) | _BV(COM0A0) | _BV(COM0B1)
|
41 |
// | _BV(COM0B0); TODO temporary
|
42 |
TCCR0A = _BV(WGM01) | _BV(WGM00) | _BV(COM0A1) | _BV(COM0A0); |
43 |
TCCR0B = 0;
|
44 |
|
45 |
// enable overflow interrupt
|
46 |
TIMSK0 = _BV(TOIE0); |
47 |
|
48 |
// set output pins
|
49 |
//DDRD |= _BV(PD5) | _BV(PD6); TODO temporary
|
50 |
DDRD |= _BV(PD6); |
51 |
|
52 |
set_servo1(0);
|
53 |
set_servo2(0);
|
54 |
} |
55 |
|
56 |
void servo_pulse()
|
57 |
{ |
58 |
// set limits, turn on outputs, and start timer
|
59 |
|
60 |
OCR0A = 255 - angle_to_ticks(servo1_angle);
|
61 |
OCR0B = 255 - angle_to_ticks(servo2_angle);
|
62 |
TCNT0 = 0;
|
63 |
|
64 |
// CS0 0b011 (64 prescaler)
|
65 |
TCCR0B = _BV(CS01) | _BV(CS00); |
66 |
} |
67 |
|
68 |
void set_servo1(int8_t angle)
|
69 |
/* angle must be between -128 and 127 */
|
70 |
{ |
71 |
servo1_angle = angle; |
72 |
} |
73 |
|
74 |
void set_servo2(int8_t angle)
|
75 |
/* angle must be between -128 and 127 */
|
76 |
{ |
77 |
servo2_angle = angle; |
78 |
} |
79 |
|
80 |
int8_t get_servo1() |
81 |
{ |
82 |
return servo1_angle;
|
83 |
} |
84 |
|
85 |
int8_t get_servo2() |
86 |
{ |
87 |
return servo2_angle;
|
88 |
} |
89 |
|
90 |
ISR(TIMER0_OVF_vect) |
91 |
{ |
92 |
// stop timer
|
93 |
TCCR0B = 0;
|
94 |
} |