From 699c932c4a5e25d95b46095440c54dc6a274e387 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:05:59 +0200 Subject: digital/beacon: add servo wave scanning --- digital/beacon/src/servo.c | 48 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) (limited to 'digital/beacon/src/servo.c') diff --git a/digital/beacon/src/servo.c b/digital/beacon/src/servo.c index 8069f566..8aa76be0 100644 --- a/digital/beacon/src/servo.c +++ b/digital/beacon/src/servo.c @@ -30,6 +30,7 @@ servo_s servo1; servo_s servo2; +static HAL_AppTimer_t waveTimer; // TIMER descripor used by the DEBUG task /* This function initializes low and high level modules for servos */ void servo_init(void) @@ -244,6 +245,53 @@ void servo_inverse_scanning_sense(TServo_ID servo_id) } } +/* This function generates a wave scanning */ +void servo_waveform_scanning(TServo_ID servo_id, uint8_t average_value) +{ + + uint16_t next_value = 0; + uint16_t current_value = 0; + + /* Compute next value to set to the servo */ + next_value = servo_get_value(servo_id) + servo_get_scanning_sense(servo_id); + + /* Set it and check the return value in order to inverse the sense MIN or MAX is reached */ + current_value = servo_set_value(servo_id,next_value); + + if((current_value <= average_value - SERVO_WAVE_OFFSET) || (current_value >= average_value + SERVO_WAVE_OFFSET)) + { + servo_inverse_scanning_sense(servo_id); + } +} + +/* Wave Task */ +void servo_wave_task(void) +{ + static bool first_time = 1; + static uint8_t average_value[2] = {0}; + + /* Get and save the average value found with the calibration */ + if(first_time == 1) + { + average_value[SERVO_1] = servo_get_value(SERVO_1); + average_value[SERVO_2] = servo_get_value(SERVO_2); + first_time = 0; + } + + /* Scan each servos */ + servo_waveform_scanning(SERVO_1,average_value[SERVO_1]); + servo_waveform_scanning(SERVO_2,average_value[SERVO_2]); +} + +/* Start wave task */ +void servo_start_wave_task(void) +{ + waveTimer.interval = WAVE_TASK_PERIOD; + waveTimer.mode = TIMER_REPEAT_MODE; + waveTimer.callback = servo_wave_task; + HAL_StartAppTimer(&waveTimer); +} + SIGNAL (SIG_OVERFLOW1) { } -- cgit v1.2.3