Revision d9b50611
Fixed servo code to output correct signal
| paintboard/code/servo.c | ||
|---|---|---|
| 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 |
} |
|
Also available in: Unified diff