Project

General

Profile

Statistics
| Revision:

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

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