Statistics
| Branch: | Revision:

scoutece / 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
}