1 |
1 |
#include <avr/io.h>
|
2 |
|
#include "servo.h"
|
|
2 |
#include <avr/interrupt.h>
|
3 |
3 |
#include <stdlib.h>
|
|
4 |
#include "servo.h"
|
4 |
5 |
|
5 |
6 |
/*
|
6 |
7 |
|
7 |
|
servo controller. Timer0 is set to phase-correct PWM mode, with 32 prescaler.
|
|
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.
|
8 |
13 |
|
9 |
14 |
SERVO_PWM1: PD6 (Timer0)
|
10 |
15 |
|
... | ... | |
12 |
17 |
|
13 |
18 |
*/
|
14 |
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 |
|
15 |
31 |
int8_t servo1_angle;
|
16 |
32 |
int8_t servo2_angle;
|
17 |
33 |
|
18 |
34 |
void servo_init()
|
19 |
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);
|
20 |
47 |
|
21 |
|
// WGM0 0b001 (phase-correct PWM, 8-bit)
|
22 |
|
// COM0A 0b10 (clear OCR0A on compare match, set on BOTTOM)
|
23 |
|
// COM0B 0b10 (clear OCR0B on compare match, set on BOTTOM)
|
24 |
|
// CS0 0b011 (32 prescaler)
|
25 |
|
TCCR0A = _BV(WGM00) | _BV(COM0A1) |_BV(COM0B1);
|
26 |
|
TCCR0B = _BV(CS01) | _BV(CS00);
|
27 |
|
OCR0A = 0;
|
28 |
|
OCR0B = 0;
|
29 |
|
|
30 |
48 |
// set output pins
|
31 |
|
DDRD |= _BV(PD5) | _BV(PD6);
|
|
49 |
//DDRD |= _BV(PD5) | _BV(PD6); TODO temporary
|
|
50 |
DDRD |= _BV(PD6);
|
32 |
51 |
|
33 |
52 |
set_servo1(0);
|
34 |
53 |
set_servo2(0);
|
35 |
54 |
}
|
36 |
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 |
|
37 |
68 |
void set_servo1(int8_t angle)
|
38 |
69 |
/* angle must be between -128 and 127 */
|
39 |
70 |
{
|
40 |
71 |
servo1_angle = angle;
|
41 |
|
OCR0A = (uint8_t)(angle + 128);
|
42 |
72 |
}
|
43 |
73 |
|
44 |
74 |
void set_servo2(int8_t angle)
|
45 |
75 |
/* angle must be between -128 and 127 */
|
46 |
76 |
{
|
47 |
77 |
servo2_angle = angle;
|
48 |
|
OCR0B = (uint8_t)(angle + 128);
|
49 |
78 |
}
|
50 |
79 |
|
51 |
80 |
int8_t get_servo1()
|
... | ... | |
58 |
87 |
return servo2_angle;
|
59 |
88 |
}
|
60 |
89 |
|
|
90 |
ISR(TIMER0_OVF_vect)
|
|
91 |
{
|
|
92 |
// stop timer
|
|
93 |
TCCR0B = 0;
|
|
94 |
}
|