Statistics
| Revision:

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

 1 ```/* ``` ``` servo.c - Contains functions and interrupts necessary to activate servos ``` ``` author: Tom Lauwers ``` ``` ``` ``` 3/1/06 Iain ``` ``` Man, I don't know what a servo is. ``` ```*/ ``` ```#include ``` ```#include ``` ```#include ``` ```#include ``` ```#include ``` ```#include ``` ```//1677.7216 c/ms ``` ```//i dunno must be 2^k ``` ```//250 ``` ```//9C40 ~ 20ms ``` ```//#A000 ~ 20.48 but prolly wont work ``` ```//div by 256: 6.55078125 c/ms 80 ~ 19.5ms ``` ```#define SERVO_PERIOD 0x80 ``` ```//needs be 1ms ``` ```//is 1.22ms ``` ```#define SERVO_CONSTANT 0x8 ``` ```/* dirty dirty ``` ```unsigned int servo_vals[4] = {0,0,0,0}; // Stores the set servo values ``` ```int current_servo = 0; // Stores which servo is current in the interrupt routine ``` ```*/ ``` ```unsigned int servo_vals[8] = {0,0,0,0,0,0,0,0}; // Stores the set servo values ``` ```int phase_time=0; ``` ```int phase_base=0; ``` ```int servos_on = 0; // Stores which servos are enabled ``` ```int servos_enabled=0; //which are high ``` ```int servo_flag = 0; // Stores whether or not servos are enabled ``` ```/* Timer 1 output compare B interrupt. Does the following: ``` ``` Checks to see if the current servo is 4, in which case just need ``` ``` to set low servo 3's signal and return. ``` ``` Else, check to see if the current servo is enabled, and set it's signal high ``` ``` Then, set the previous servo's signal low ``` ``` Now, set the next output compare to occur for some time from now ``` ``` specified by the current servo's value ``` ```*/ ``` ```SIGNAL (SIG_OUTPUT_COMPARE3B) ``` ```{ ``` ``` lcd_putchar('i'); ``` ``` if(servo_flag == 0) ``` ``` return; ``` ``` ``` ``` int i; ``` ``` if(phase_time == 0){ ``` ``` lcd_putchar('p'); ``` ``` for(i=0;i<8;i++){ ``` ``` if(servos_on & _BV(i)) ``` ``` digital_output((_PORT_E << 3) +i,1); ``` ``` } ``` ``` servos_enabled = servos_on; ``` ``` phase_base = OCR3B; ``` ``` } ``` ``` unsigned int min=~0; ``` ``` for(i=0;i<8;i++){ ``` ``` if(_BV(i) & servos_enabled){ ``` ``` if(SERVO_CONSTANT*servo_vals[i] <= phase_time){ ``` ``` digital_output((_PORT_E << 3) + i,0); ``` ``` servos_enabled &= ~_BV(i); ``` ``` } ``` ``` else if(SERVO_CONSTANT*servo_vals[i] < min) { ``` ``` min = servo_vals[i]; ``` ``` } ``` ``` } ``` ``` } ``` ``` if(!servos_enabled){ ``` ``` OCR3B = phase_base + SERVO_PERIOD; ``` ``` phase_time = 0; ``` ``` } ``` ``` else { ``` ``` OCR3B = phase_base + min; ``` ``` phase_time = min; ``` ``` } ``` ``` ``` ```} ``` ```void init_servo() ``` ```{ ``` ``` //1010 1001 ``` ``` TCCR3A = 0; ``` ``` //0000 0100 ``` ``` TCCR3B = 0x4; //prescaler to 256 ``` ``` TCCR3C=0; ``` ``` lcd_init(); ``` ``` ETIMSK |= 0x08; ``` ``` servo_flag = 1; ``` ``` OCR3B = 0x0; ``` ``` lcd_putchar('i'); ``` ```} ``` ```// Enable a servo specified by config ``` ```void enable_servo(int config) ``` ```{ ``` ``` servos_on |= _BV(config); ``` ```} ``` ```// Disable a servo specified by config ``` ```void disable_servo(int config) ``` ```{ ``` ``` servos_on &= (0xFF-_BV(config)); ``` ```} ``` ```// Disables the timer1 interrupt, disabling the servos ``` ```void disable_servos() ``` ```{ ``` ``` // Disables interrupts ``` ``` ETIMSK &= ~0x08; ``` ``` servo_flag=0; ``` ```} ``` ```// Enables the timer1 interrupt, enabling the servos ``` ```void enable_servos() ``` ```{ ``` ``` // Enable interrupts on output compare B ``` ``` ETIMSK |= 0x08; ``` ``` servo_flag=1; ``` ```} ``` ```// Set a servo to a value. Automatically enables the servo. ``` ```void set_servo(int servo, unsigned int value) ``` ```{ ``` ``` enable_servo(servo); ``` ``` servo_vals[servo] = value; ``` ```} ```