summaryrefslogtreecommitdiffhomepage
path: root/digital/beacon/src
diff options
context:
space:
mode:
authorFlorent Duchon2013-05-02 18:41:17 +0200
committerFlorent Duchon2013-05-02 22:52:49 +0200
commitd2a008921c898358807c82894f575bf68a953779 (patch)
treee146b1a04c39411a81cf3a882d53baa3118b48ef /digital/beacon/src
parent6ab36ad8c87371733ab8c94ff9eee173b72f3980 (diff)
digital/beacon: calibration is done directly using reflectors on others beacons
Diffstat (limited to 'digital/beacon/src')
-rw-r--r--digital/beacon/src/calibration.c11
-rw-r--r--digital/beacon/src/laser.c25
2 files changed, 28 insertions, 8 deletions
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);
+ }
}
}
}