summaryrefslogtreecommitdiffhomepage
path: root/digital/beacon
diff options
context:
space:
mode:
authorFlorent Duchon2012-05-16 17:28:12 +0200
committerFlorent Duchon2012-05-16 17:43:11 +0200
commita64fb816764b34b7bee2dc886c7efaa2c8bf0539 (patch)
treec6e011df7eebb7859d261c29e2c64e1d2bb906cf /digital/beacon
parent36d1bbfc275e6ecb2ff267e224bac7a11f8b245e (diff)
digital/beacon: Improve calibration state machine
Diffstat (limited to 'digital/beacon')
-rw-r--r--digital/beacon/src/calibration.c76
-rw-r--r--digital/beacon/src/calibration.h11
-rw-r--r--digital/beacon/src/laser.c7
3 files changed, 47 insertions, 47 deletions
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);
}
}
}