Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / colonet / testing / robot_routine_reg_test / servo.c @ 13

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