Statistics
| Branch: | Revision:

scoutece / paintboard / code / servo.c @ d9b50611

History | View | Annotate | Download (1.94 KB)

1 1226c007 AnsonLinux
#include <avr/io.h>
2 d9b50611 Tom Mullins
#include <avr/interrupt.h>
3 1226c007 AnsonLinux
#include <stdlib.h>
4 d9b50611 Tom Mullins
#include "servo.h"
5 1226c007 AnsonLinux
6
/*
7

8 d9b50611 Tom Mullins
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 1226c007 AnsonLinux

14
SERVO_PWM1: PD6 (Timer0)
15

16
SERVO_PWM2: PD5 (Timer0)
17

18
*/
19
20 d9b50611 Tom Mullins
#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 1226c007 AnsonLinux
int8_t servo1_angle;
32
int8_t servo2_angle;
33
34
void servo_init()
35
{
36 d9b50611 Tom Mullins
  // 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 1226c007 AnsonLinux
48
  // set output pins
49 d9b50611 Tom Mullins
  //DDRD |= _BV(PD5) | _BV(PD6); TODO temporary
50
  DDRD |= _BV(PD6);
51 1226c007 AnsonLinux
52
  set_servo1(0);
53
  set_servo2(0);
54
}
55
56 d9b50611 Tom Mullins
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 1226c007 AnsonLinux
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 d9b50611 Tom Mullins
ISR(TIMER0_OVF_vect)
91
{
92
  // stop timer
93
  TCCR0B = 0;
94
}