From 2a6a2fce72211170c27d5d36e0cdcf96fcfd09c7 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Tue, 17 Apr 2012 22:08:16 +0200 Subject: digital/beacon: new servomotor implementation --- digital/beacon/src/servo.c | 161 ++++++++++++++++++++++++++++++++++++++++----- digital/beacon/src/servo.h | 67 +++++++++++++++---- 2 files changed, 201 insertions(+), 27 deletions(-) (limited to 'digital') diff --git a/digital/beacon/src/servo.c b/digital/beacon/src/servo.c index 96a5b0d4..2d590c3d 100644 --- a/digital/beacon/src/servo.c +++ b/digital/beacon/src/servo.c @@ -28,13 +28,23 @@ #include "servo.h" #include "debug.h" +servo_s servo1; +servo_s servo2; + +/* This function initializes low and high level modules for servos */ +void servo_init(void) +{ + servo_timer1_init(); + servo_structure_init(); +} + /* This function initializes the timer used for servomotor signal generation */ void servo_timer1_init(void) { //Fpwm = f_IO / (prescaler * (1 + TOP)) = 7200 Hz. */ - OCR1B = SERVO_1_ANGLE_INIT; - OCR1A = SERVO_2_ANGLE_INIT; + OCR1B = SERVO_ANGLE_INIT; + OCR1A = SERVO_ANGLE_INIT; /* Fast PWM 10bits with TOP=0x03FF */ @@ -59,53 +69,158 @@ void servo_timer1_init(void) /* Enable Interrupts */ sei(); +} + +/* This function initializes the servo structures */ +void servo_structure_init(void) +{ + /* Servo 1 init values */ + servo1.id = SERVO_1; + servo1.state = SERVO_SCANNING_OFF; + servo1.top = 0; + servo1.bottom = 0; + servo1.scanning_sense = FALLING; + + /* Servo 2 init values */ + servo2.id = SERVO_2; + servo2.state = SERVO_SCANNING_OFF; + servo2.top = 0; + servo2.bottom = 0; + servo2.scanning_sense = FALLING; } -/* This function increase by one unit the angle of the defined servo and returns "angle" value */ -int servo_angle_increase(TServo_ID servo_id) +/* This function increases by one unit the angle of the defined servo and returns "angle" value */ +int16_t servo_angle_increase(TServo_ID servo_id) { switch(servo_id) { case SERVO_1: - if(OCR1A < SERVO_1_ANGLE_MAX) + if(OCR1A < SERVO_ANGLE_MAX) { OCR1A++; - return OCR1A; } + return OCR1A; break; case SERVO_2: - if(OCR1B < SERVO_2_ANGLE_MAX) + if(OCR1B < SERVO_ANGLE_MAX) { OCR1B++; - return OCR1B; } + return OCR1B; break; default: break; } - return 0; + return -1; } -/* This function decrease by one unit the angle of the defined servo and returns "angle" value*/ -int servo_angle_decrease(TServo_ID servo_id) +/* This function decreases by one unit the angle of the defined servo and returns "angle" value*/ +int16_t servo_angle_decrease(TServo_ID servo_id) { switch(servo_id) { case SERVO_1: - if(OCR1A > SERVO_1_ANGLE_MIN) + if(OCR1A > SERVO_ANGLE_MIN) { OCR1A--; - return OCR1A; } + return OCR1A; break; case SERVO_2: - if(OCR1B > SERVO_2_ANGLE_MIN) + if(OCR1B > SERVO_ANGLE_MIN) { OCR1B--; - return OCR1B; } + return OCR1B; + break; + default: + break; + } + return -1; +} + +/* This function returns the "angle" value of a defined servo */ +int16_t servo_get_value(TServo_ID servo_id) +{ + switch(servo_id) + { + case SERVO_1: + return OCR1A; + break; + case SERVO_2: + return OCR1B; + break; + default: + break; + } + return -1; +} + +/* This function sets the "angle" value of a defined servo */ +int16_t servo_set_value(TServo_ID servo_id,int16_t value) +{ + if(value < SERVO_ANGLE_MIN) + value = SERVO_ANGLE_MIN; + else if(value > SERVO_ANGLE_MAX) + value = SERVO_ANGLE_MAX; + + switch(servo_id) + { + case SERVO_1: + OCR1A = value; + break; + case SERVO_2: + OCR1B= value; + break; + default: + break; + } + return value; +} + +/* This function returns the current state of a defined servo */ +TServo_state servo_get_state(TServo_ID servo_id) +{ + switch(servo_id) + { + case SERVO_1: + return servo1.state; + case SERVO_2: + return servo2.state; + default: + break; + } + return -1; +} + +/* This function updates the state of a defined servo */ +void servo_set_state(TServo_ID servo_id,TServo_state state) +{ + switch(servo_id) + { + case SERVO_1: + servo1.state = state; + break; + case SERVO_2: + servo2.state = state; + break; + default: + break; + } +} + +/* This function returns the scanning sens of the defined servo */ +int8_t servo_get_scanning_sense(TServo_ID servo_id) +{ + switch(servo_id) + { + case SERVO_1: + return servo1.scanning_sense; + break; + case SERVO_2: + return servo2.scanning_sense; break; default: break; @@ -113,6 +228,22 @@ int servo_angle_decrease(TServo_ID servo_id) return 0; } +/* This function inverses the scanning sense of the servo */ +void servo_inverse_scanning_sense(TServo_ID servo_id) +{ + switch(servo_id) + { + case SERVO_1: + servo1.scanning_sense *= -1; + break; + case SERVO_2: + servo1.scanning_sense *= -1; + break; + default: + break; + } +} + SIGNAL (SIG_OVERFLOW1) { } diff --git a/digital/beacon/src/servo.h b/digital/beacon/src/servo.h index eb3ab0fd..6343a2f3 100644 --- a/digital/beacon/src/servo.h +++ b/digital/beacon/src/servo.h @@ -26,27 +26,70 @@ #ifndef _SERVO_H #define _SERVO_H -#define SERVO_1_ANGLE_INIT 150 -#define SERVO_1_ANGLE_MIN 69 -#define SERVO_1_ANGLE_MAX 300 -#define SERVO_2_ANGLE_INIT 150 -#define SERVO_2_ANGLE_MIN 69 -#define SERVO_2_ANGLE_MAX 300 + +#define SERVO_ANGLE_INIT 200 +#define SERVO_ANGLE_MIN 69 +#define SERVO_ANGLE_MAX 200 + +#define RISING 1 +#define FALLING -1 /* SERVO ID */ typedef enum { - SERVO_1=1, + SERVO_1, SERVO_2 } TServo_ID; +typedef enum +{ + SERVO_SCANNING_OFF, + SERVO_SCANNING_FAST_IN_PROGRESS, + SERVO_SCANNING_FAST_FINISHED, + SERVO_SCANNING_SLOW_IN_PROGRESS, + SERVO_SCANNING_SLOW_FINISHED +} TServo_state; + +typedef struct +{ + TServo_ID id; + int16_t top; + int16_t bottom; + int scanning_sense; + TServo_state state; +} servo_s; + +/* This function initializes the servo structures */ +void servo_structure_init(void); + +/* This function initializes low and high level modules for servos */ +void servo_init(void); + /* This function initializes the timer used for servomotor signal generation */ void servo_timer1_init(void); -/* This function increase by one unit the angle of the defined servo and returns "angle" value */ -int servo_angle_increase(TServo_ID servo_id); +/* This function increases by one unit the angle of the defined servo and returns "angle" value */ +int16_t servo_angle_increase(TServo_ID servo_id); + +/* This function decreases by one unit the angle of the defined servo and returns "angle" value */ +int16_t servo_angle_decrease(TServo_ID servo_id); + +/* This function returns the "angle" value of a defined servo */ +int16_t servo_get_value(TServo_ID servo_id); + +/* This function sets the "angle" value of a defined servo */ +int16_t servo_set_value(TServo_ID servo_id,int16_t value); + +/* This function returns the current state of a defined servo */ +TServo_state servo_get_state(TServo_ID servo_id); + +/* This function updates the state of a defined servo */ +void servo_set_state(TServo_ID servo_id,TServo_state state); + +/* This function returns the scanning sens of the defined servo */ +int8_t servo_get_scanning_sense(TServo_ID servo_id); -/* This function decrease by one unit the angle of the defined servo and returns "angle" value*/ -int servo_angle_decrease(TServo_ID servo_id); +/* This function inverses the scanning sense of the servo */ +void servo_inverse_scanning_sense(TServo_ID servo_id); -#endif \ No newline at end of file +#endif -- cgit v1.2.3