From a64fb816764b34b7bee2dc886c7efaa2c8bf0539 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:28:12 +0200 Subject: digital/beacon: Improve calibration state machine --- digital/beacon/src/calibration.c | 76 ++++++++++++++++++++-------------------- digital/beacon/src/calibration.h | 11 +++--- digital/beacon/src/laser.c | 7 ++-- 3 files changed, 47 insertions(+), 47 deletions(-) (limited to 'digital/beacon') diff --git a/digital/beacon/src/calibration.c b/digital/beacon/src/calibration.c index 618fbcd3..f302992e 100644 --- a/digital/beacon/src/calibration.c +++ b/digital/beacon/src/calibration.c @@ -28,7 +28,7 @@ #include "calibration.h" #include "servo.h" #include "motor.h" - +#include "codewheel.h" HAL_AppTimer_t calibrationTimer; // TIMER descripor used by the DEBUG task calibration_s calibration; @@ -90,8 +90,6 @@ void calibration_change_task_frequency(uint32_t frequency) void calibration_task(void) { TServo_ID servo; - static uint16_t top = 0; - static uint16_t bottom = 0; /* Select which servo need to be calibrated */ if(servo_get_state(SERVO_1) == servo_get_state(SERVO_2)) @@ -110,21 +108,17 @@ void calibration_task(void) case CALIBRATION_FAST_SCANNING: /* Check if the laser catchs something */ - if(calibration_get_laser_flag() == CLEAR) - { - /* Nothing appended, update the state and continue scanning */ - servo_set_state(servo,SERVO_SCANNING_FAST_IN_PROGRESS); - calibration_scanning(servo); - } - else + if(calibration_get_laser_flag() == servo) { /* Clear the laser flag */ calibration_set_laser_flag(CLEAR); /* Laser detected the aim, reset the servo with the previous value and update the servo status */ - servo_set_value(servo,servo_get_value(servo)+servo_get_scanning_sense(servo)*FAST_SCANNING_OFFSET); + servo_set_value(servo,servo_get_value(servo) - servo_get_scanning_sense(servo)*FAST_SCANNING_OFFSET); servo_set_state(servo,SERVO_SCANNING_FAST_FINISHED); + uprintf("servo[%d] FAST finished\r\n",servo); + if((servo_get_state(SERVO_1) == SERVO_SCANNING_FAST_FINISHED) && (servo_get_state(SERVO_2) == SERVO_SCANNING_FAST_FINISHED)) { /* If both servo have finished FAST SCANNING go to SLOW SCANNING state */ @@ -134,45 +128,51 @@ void calibration_task(void) calibration_change_task_frequency(CALIBRATION_SLOW_TASK_PERIOD); } } + else + { + /* Nothing appended, update the state and continue scanning */ + servo_set_state(servo,SERVO_SCANNING_FAST_IN_PROGRESS); + calibration_scanning(servo); + } break; case CALIBRATION_SLOW_SCANNING: /* Check if the laser catched something */ - if(calibration_get_laser_flag() == CLEAR) - { - /* Nothing appended, update the state and continue scanning */ - calibration_scanning(servo); - servo_set_state(servo,SERVO_SCANNING_SLOW_IN_PROGRESS); - } - else + if(calibration_get_laser_flag() == servo) { /* Clear the laser flag */ calibration_set_laser_flag(CLEAR); - /* Check if it TOP AIM was already found */ - if(top == 0) - { - /* If no save the value */ - top = servo_get_value(servo); - } - else + /* Laser detected the aim, reset the servo with the previous value and update the servo status */ + servo_set_value(servo,servo_get_value(servo) + servo_get_scanning_sense(servo)*10); + servo_set_state(servo,SERVO_SCANNING_SLOW_FINISHED); + + uprintf("servo[%d] SLOW finished\r\n",servo); + + if((servo_get_state(SERVO_1) == SERVO_SCANNING_SLOW_FINISHED) && (servo_get_state(SERVO_2) == SERVO_SCANNING_SLOW_FINISHED)) { - /* If top TOP AIM was already found, avarage the TOP & BOTTOM value and update the servo status */ - bottom = servo_get_value(servo); - servo_set_value(servo,(top+bottom)/2); - servo_set_state(servo,SERVO_SCANNING_SLOW_FINISHED); - top = 0; - bottom = 0; - - /* If both servo have finished FAST SCANNING go to SLOW SCANNING*/ - if((servo_get_state(SERVO_1) == SERVO_SCANNING_SLOW_FINISHED) && (servo_get_state(SERVO_2) == SERVO_SCANNING_SLOW_FINISHED)) - { - calibration.state = SCANNING_STATE_CALIBRATED; - calibration_stop_task(); - } + /* If both servo have finished FAST SCANNING go to SLOW SCANNING state */ + calibration.state = SCANNING_STATE_CALIBRATED; +#ifdef LOL_NUMBER_1 + codewheel_set_rebase_offset(496); +#elif LOL_NUMBER_2 + codewheel_set_rebase_offset(3); +#elif LOL_NUMBER_3 + codewheel_set_rebase_offset(CODEWHEEL_CPR*3/4); +#endif + codewheel_set_state(CODEWHEEL_REQUEST_REBASE); + calibration_stop_task(); + servo_start_wave_task(); + uprintf("Calibration finished\r\n"); } } + else + { + /* Nothing appended, update the state and continue scanning */ + calibration_scanning(servo); + servo_set_state(servo,SERVO_SCANNING_SLOW_IN_PROGRESS); + } break; case SCANNING_STATE_CALIBRATED: calibration_stop_task(); diff --git a/digital/beacon/src/calibration.h b/digital/beacon/src/calibration.h index 3512eedf..cb6ef303 100644 --- a/digital/beacon/src/calibration.h +++ b/digital/beacon/src/calibration.h @@ -28,16 +28,17 @@ #include "servo.h" -#define CALIBRATION_FAST_TASK_PERIOD 11L -#define CALIBRATION_SLOW_TASK_PERIOD 110L +#define CALIBRATION_FAST_TASK_PERIOD 22L +#define CALIBRATION_SLOW_TASK_PERIOD 22L -#define FAST_SCANNING_OFFSET 10 +#define FAST_SCANNING_OFFSET 20 #define SCANNING_STEP 1 typedef enum { - CLEAR, - SET + SET_SERVO_1, + SET_SERVO_2, + CLEAR } TLaser_flag_type; typedef enum diff --git a/digital/beacon/src/laser.c b/digital/beacon/src/laser.c index bd8be64b..e73c1bd7 100644 --- a/digital/beacon/src/laser.c +++ b/digital/beacon/src/laser.c @@ -129,19 +129,18 @@ ISR(TIMER3_COMPB_vect) { codewheel_set_rebase_offset(laser_get_angle_raw()); codewheel_set_state(CODEWHEEL_REQUEST_REBASE); - calibration_set_laser_flag(SET); } else { /* If mire 1 is spotted */ - if(((laser_get_angle_degree() <= SERVO_1_ANGLE_POSITION + SERVO_ANGLE_POSITION_TOLERANCE) && (laser_get_angle_degree() >= 360 - SERVO_ANGLE_POSITION_TOLERANCE)) && ((servo_get_state(SERVO_1) == SERVO_SCANNING_FAST_IN_PROGRESS) || (servo_get_state(SERVO_1) == SERVO_SCANNING_SLOW_IN_PROGRESS))) + if(((laser_get_angle_degree() <= SERVO_1_ANGLE_POSITION + SERVO_ANGLE_POSITION_TOLERANCE) || (laser_get_angle_degree() >= 360 - SERVO_ANGLE_POSITION_TOLERANCE)) && ((servo_get_state(SERVO_1) == SERVO_SCANNING_FAST_IN_PROGRESS) || (servo_get_state(SERVO_1) == SERVO_SCANNING_SLOW_IN_PROGRESS))) { - calibration_set_laser_flag(SET); + calibration_set_laser_flag(SET_SERVO_1); } /* If mire 2 is spotted */ else if(((laser_get_angle_degree() <= SERVO_2_ANGLE_POSITION + SERVO_ANGLE_POSITION_TOLERANCE) && (laser_get_angle_degree() >= SERVO_2_ANGLE_POSITION - SERVO_ANGLE_POSITION_TOLERANCE)) && ((servo_get_state(SERVO_2) == SERVO_SCANNING_FAST_IN_PROGRESS) || (servo_get_state(SERVO_2) == SERVO_SCANNING_SLOW_IN_PROGRESS))) { - calibration_set_laser_flag(SET); + calibration_set_laser_flag(SET_SERVO_2); } } } -- cgit v1.2.3