summaryrefslogtreecommitdiffhomepage
path: root/digital/beacon/src/laser.c
diff options
context:
space:
mode:
authorFlorent Duchon2012-05-11 00:04:05 +0200
committerFlorent Duchon2012-05-11 00:16:45 +0200
commit9b2bc49c9ab3cee251b8d5b8205d055a23c7ab82 (patch)
tree58c76cea25f83ba3259a8cfcf594d50e95964fd4 /digital/beacon/src/laser.c
parenta7ec22d4a46b124e70cbd93f64a449862ce99933 (diff)
digital/beacon: new state machine into Zigbee sending IRQ vector
Diffstat (limited to 'digital/beacon/src/laser.c')
-rw-r--r--digital/beacon/src/laser.c27
1 files changed, 24 insertions, 3 deletions
diff --git a/digital/beacon/src/laser.c b/digital/beacon/src/laser.c
index 73c85910..27638678 100644
--- a/digital/beacon/src/laser.c
+++ b/digital/beacon/src/laser.c
@@ -115,10 +115,31 @@ void laser_set_angle(uint16_t angle)
/* Zigbee sending IRQ vector */
ISR(TIMER3_COMPB_vect)
{
- if(servo_get_state(SERVO_1) == SERVO_SCANNING_FAST_IN_PROGRESS)
+ if(calibration_get_state() != SCANNING_STATE_CALIBRATED)
{
- codewheel_set_rebase_offset(laser_get_angle_raw());
- codewheel_set_state(CODEWHEEL_REQUEST_REBASE);
+ if(codewheel_get_state() == CODEWHEEL_INIT)
+ {
+ 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)))
+ {
+ calibration_set_laser_flag(SET);
+ }
+ /* 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);
+ }
+ }
+ }
+ else
+ {
+ // TODO: Send angle
}
/* Disable the interrupt */