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