From 9b2bc49c9ab3cee251b8d5b8205d055a23c7ab82 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Fri, 11 May 2012 00:04:05 +0200 Subject: digital/beacon: new state machine into Zigbee sending IRQ vector --- digital/beacon/src/laser.c | 27 ++++++++++++++++++++++++--- 1 file changed, 24 insertions(+), 3 deletions(-) (limited to 'digital/beacon/src/laser.c') 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 */ -- cgit v1.2.3