From d2a008921c898358807c82894f575bf68a953779 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Thu, 2 May 2013 18:41:17 +0200 Subject: digital/beacon: calibration is done directly using reflectors on others beacons --- digital/beacon/src/calibration.c | 11 +++++++---- digital/beacon/src/laser.c | 25 +++++++++++++++++++++---- 2 files changed, 28 insertions(+), 8 deletions(-) (limited to 'digital/beacon/src') diff --git a/digital/beacon/src/calibration.c b/digital/beacon/src/calibration.c index 683fd5c1..8a153be4 100644 --- a/digital/beacon/src/calibration.c +++ b/digital/beacon/src/calibration.c @@ -31,6 +31,7 @@ #include "codewheel.h" #include "buttons.h" #include "uid.h" +#include "customisation.h" HAL_AppTimer_t calibrationTimer; // TIMER descripor used by the calibration task HAL_AppTimer_t ManualcalibrationTimer; // TIMER descripor used by the manual calibration task @@ -106,9 +107,9 @@ void calibration_task(void) /* Select which servo need to be calibrated */ if(servo_get_state(SERVO_1) == servo_get_state(SERVO_2)) - servo = SERVO_1; + servo = custom_get_servoID_order(1,get_uid()); else - servo = SERVO_2; + servo = custom_get_servoID_order(2,get_uid()); switch(calibration.state) { @@ -130,9 +131,10 @@ void calibration_task(void) /* 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_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 */ @@ -159,7 +161,7 @@ void calibration_task(void) 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)*9); + servo_set_value(servo,servo_get_value(servo) - servo_get_scanning_sense(servo)*13); servo_set_state(servo,SERVO_SCANNING_SLOW_FINISHED); uprintf("servo[%d] SLOW finished\r\n",servo); @@ -222,6 +224,7 @@ void calibration_scanning(TServo_ID servo_id) /* This function sets the laser flag according the given value SET or CLEAR */ void calibration_set_laser_flag(TLaser_flag_type value) { +// uprintf("&&&&&&&&&&&&&&&&& laser flag ok %d\r\n",(int)value); calibration.laser_flag = value; } diff --git a/digital/beacon/src/laser.c b/digital/beacon/src/laser.c index 447ddcbd..9f5cea81 100644 --- a/digital/beacon/src/laser.c +++ b/digital/beacon/src/laser.c @@ -31,6 +31,8 @@ #include "codewheel.h" #include "calibration.h" #include "network_send_commands.h" +#include "uid.h" +#include "customisation.h" laser_s laser; @@ -139,14 +141,29 @@ ISR(TIMER3_COMPB_vect) 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() <= custom_get_mire_angle(1,get_uid()) + SERVO_ANGLE_POSITION_TOLERANCE) || (laser_get_angle_degree() >= 360 - SERVO_ANGLE_POSITION_TOLERANCE)) && ((servo_get_state(custom_get_servoID_order(1,get_uid())) == SERVO_SCANNING_FAST_IN_PROGRESS) || (servo_get_state(custom_get_servoID_order(1,get_uid())) == SERVO_SCANNING_SLOW_IN_PROGRESS))) { - calibration_set_laser_flag(SET_SERVO_1); + if(custom_get_servoID_order(1,get_uid()) == SERVO_1) + { + calibration_set_laser_flag(SET_SERVO_1); + } + else + { + calibration_set_laser_flag(SET_SERVO_2); + } } + /* 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))) + else if(((laser_get_angle_degree() <= custom_get_mire_angle(2,get_uid()) + SERVO_ANGLE_POSITION_TOLERANCE) && (laser_get_angle_degree() >= custom_get_mire_angle(2,get_uid()) - SERVO_ANGLE_POSITION_TOLERANCE)) && ((servo_get_state(custom_get_servoID_order(2,get_uid())) == SERVO_SCANNING_FAST_IN_PROGRESS) || (servo_get_state(custom_get_servoID_order(2,get_uid())) == SERVO_SCANNING_SLOW_IN_PROGRESS))) { - calibration_set_laser_flag(SET_SERVO_2); + if(custom_get_servoID_order(2,get_uid()) == SERVO_1) + { + calibration_set_laser_flag(SET_SERVO_1); + } + else + { + calibration_set_laser_flag(SET_SERVO_2); + } } } } -- cgit v1.2.3