root / branches / rbom / code / projects / colonet / utilities / robot_wireless_relay / servo.c @ 1390
History | View | Annotate | Download (2.97 KB)
1 |
/*
|
---|---|
2 |
servo.c - Contains functions and interrupts necessary to activate servos
|
3 |
author: Tom Lauwers
|
4 |
|
5 |
3/1/06 Iain
|
6 |
Man, I don't know what a servo is.
|
7 |
*/
|
8 |
#include <avr/io.h> |
9 |
#include <avr/interrupt.h> |
10 |
#include <servo.h> |
11 |
#include <dio.h> |
12 |
#include <lights.h> |
13 |
#include <lcd.h> |
14 |
//1677.7216 c/ms
|
15 |
//i dunno must be 2^k
|
16 |
//250
|
17 |
//9C40 ~ 20ms
|
18 |
//#A000 ~ 20.48 but prolly wont work
|
19 |
//div by 256: 6.55078125 c/ms 80 ~ 19.5ms
|
20 |
#define SERVO_PERIOD 0x80 |
21 |
//needs be 1ms
|
22 |
//is 1.22ms
|
23 |
#define SERVO_CONSTANT 0x8 |
24 |
|
25 |
/* dirty dirty
|
26 |
unsigned int servo_vals[4] = {0,0,0,0}; // Stores the set servo values
|
27 |
int current_servo = 0; // Stores which servo is current in the interrupt routine
|
28 |
*/
|
29 |
unsigned int servo_vals[8] = {0,0,0,0,0,0,0,0}; // Stores the set servo values |
30 |
int phase_time=0; |
31 |
int phase_base=0; |
32 |
int servos_on = 0; // Stores which servos are enabled |
33 |
int servos_enabled=0; //which are high |
34 |
int servo_flag = 0; // Stores whether or not servos are enabled |
35 |
|
36 |
/* Timer 1 output compare B interrupt. Does the following:
|
37 |
Checks to see if the current servo is 4, in which case just need
|
38 |
to set low servo 3's signal and return.
|
39 |
Else, check to see if the current servo is enabled, and set it's signal high
|
40 |
Then, set the previous servo's signal low
|
41 |
Now, set the next output compare to occur for some time from now
|
42 |
specified by the current servo's value
|
43 |
*/
|
44 |
SIGNAL (SIG_OUTPUT_COMPARE3B) |
45 |
{ |
46 |
lcd_putchar('i');
|
47 |
if(servo_flag == 0) |
48 |
return;
|
49 |
|
50 |
int i;
|
51 |
if(phase_time == 0){ |
52 |
lcd_putchar('p');
|
53 |
for(i=0;i<8;i++){ |
54 |
if(servos_on & _BV(i))
|
55 |
digital_output((_PORT_E << 3) +i,1); |
56 |
} |
57 |
servos_enabled = servos_on; |
58 |
phase_base = OCR3B; |
59 |
} |
60 |
|
61 |
unsigned int min=~0; |
62 |
for(i=0;i<8;i++){ |
63 |
if(_BV(i) & servos_enabled){
|
64 |
if(SERVO_CONSTANT*servo_vals[i] <= phase_time){
|
65 |
digital_output((_PORT_E << 3) + i,0); |
66 |
servos_enabled &= ~_BV(i); |
67 |
} |
68 |
else if(SERVO_CONSTANT*servo_vals[i] < min) { |
69 |
min = servo_vals[i]; |
70 |
} |
71 |
} |
72 |
} |
73 |
if(!servos_enabled){
|
74 |
OCR3B = phase_base + SERVO_PERIOD; |
75 |
phase_time = 0;
|
76 |
} |
77 |
else {
|
78 |
OCR3B = phase_base + min; |
79 |
phase_time = min; |
80 |
} |
81 |
|
82 |
} |
83 |
|
84 |
void init_servo()
|
85 |
{ |
86 |
//1010 1001
|
87 |
TCCR3A = 0;
|
88 |
//0000 0100
|
89 |
TCCR3B = 0x4; //prescaler to 256 |
90 |
TCCR3C=0;
|
91 |
lcd_init(); |
92 |
ETIMSK |= 0x08;
|
93 |
servo_flag = 1;
|
94 |
OCR3B = 0x0;
|
95 |
lcd_putchar('i');
|
96 |
} |
97 |
|
98 |
// Enable a servo specified by config
|
99 |
void enable_servo(int config) |
100 |
{ |
101 |
servos_on |= _BV(config); |
102 |
} |
103 |
|
104 |
// Disable a servo specified by config
|
105 |
void disable_servo(int config) |
106 |
{ |
107 |
servos_on &= (0xFF-_BV(config));
|
108 |
} |
109 |
|
110 |
// Disables the timer1 interrupt, disabling the servos
|
111 |
void disable_servos()
|
112 |
{ |
113 |
// Disables interrupts
|
114 |
ETIMSK &= ~0x08;
|
115 |
servo_flag=0;
|
116 |
|
117 |
} |
118 |
|
119 |
// Enables the timer1 interrupt, enabling the servos
|
120 |
void enable_servos()
|
121 |
{ |
122 |
// Enable interrupts on output compare B
|
123 |
ETIMSK |= 0x08;
|
124 |
servo_flag=1;
|
125 |
} |
126 |
// Set a servo to a value. Automatically enables the servo.
|
127 |
void set_servo(int servo, unsigned int value) |
128 |
{ |
129 |
enable_servo(servo); |
130 |
servo_vals[servo] = value; |
131 |
} |
132 |
|