From a64fb816764b34b7bee2dc886c7efaa2c8bf0539 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:28:12 +0200 Subject: digital/beacon: Improve calibration state machine --- digital/beacon/src/laser.c | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) (limited to 'digital/beacon/src/laser.c') 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); } } } -- cgit v1.2.3 From 61738d34f10046b667846d474724294ff159a320 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:29:49 +0200 Subject: digital/beacon: improve laser angle confirmation --- digital/beacon/src/laser.c | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) (limited to 'digital/beacon/src/laser.c') diff --git a/digital/beacon/src/laser.c b/digital/beacon/src/laser.c index e73c1bd7..888589ad 100644 --- a/digital/beacon/src/laser.c +++ b/digital/beacon/src/laser.c @@ -85,14 +85,7 @@ void laser_inhibit_angle_confirmation(void) /* This function configures the AVR OC3B interrupt that will send the angle LASER_SENDING_OFFSET after the latest rising edge */ void laser_engage_angle_confirmation(uint16_t value) { - if(value > CODEWHEEL_CPR - LASER_CONFIRMATION_OFFSET) - { - OCR3B = LASER_CONFIRMATION_OFFSET - (CODEWHEEL_CPR - value); - } - else - { - OCR3B = value + LASER_CONFIRMATION_OFFSET; - } + OCR3B = (value + LASER_CONFIRMATION_OFFSET)%CODEWHEEL_CPR; /* Enable interrupt */ TIMSK3 |= (1< #include "debug_avr.h" #include "codewheel.h" +#include "laser.h" +#include "network.h" codewheel_s codewheel; @@ -111,4 +113,5 @@ ISR(TIMER3_COMPA_vect) { OCR3A = CODEWHEEL_CPR; } + laser_reset_angle_id(); } diff --git a/digital/beacon/src/laser.c b/digital/beacon/src/laser.c index 888589ad..bf77cdd9 100644 --- a/digital/beacon/src/laser.c +++ b/digital/beacon/src/laser.c @@ -28,7 +28,9 @@ #include "debug_avr.h" #include "laser.h" #include "servo.h" +#include "network.h" #include "codewheel.h" +#include "calibration.h" laser_s laser; @@ -37,6 +39,7 @@ void laser_init(void) { /* Init laser structiure */ laser_set_angle(0); + laser_reset_angle_id(); /* Configure Input compare interrupts for Laser Interrupt*/ TCCR3B |= (1< 30) && (laser_get_angle_degree() < 70)) + { + uprintf("angle[%d] = %f\r\n",laser.angle_id,laser_get_angle_degree()); + laser.angle_id++; + } } /* Disable the interrupt */ diff --git a/digital/beacon/src/laser.h b/digital/beacon/src/laser.h index f50c5b07..f0d32d93 100644 --- a/digital/beacon/src/laser.h +++ b/digital/beacon/src/laser.h @@ -64,6 +64,9 @@ uint16_t laser_get_angle_raw(void); float laser_get_angle_degrees(void); /* This function sets the angle value in raw format */ -void laser_set_angle_raw(uint16_t angle); +void laser_set_angle(uint16_t angle); + +/* This function resets the angle id variable */ +void laser_reset_angle_id(void); #endif -- cgit v1.2.3 From f8b59805b7d25107a492d927983b2b6bd1eab14a Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:32:53 +0200 Subject: digital/beacon: read ICR3 directly when enterring in the interruption --- digital/beacon/src/laser.c | 28 ++++++++++++++++++++++------ 1 file changed, 22 insertions(+), 6 deletions(-) (limited to 'digital/beacon/src/laser.c') diff --git a/digital/beacon/src/laser.c b/digital/beacon/src/laser.c index bf77cdd9..f5fca7ff 100644 --- a/digital/beacon/src/laser.c +++ b/digital/beacon/src/laser.c @@ -173,6 +173,7 @@ ISR(TIMER3_COMPB_vect) ISR(TIMER3_CAPT_vect) { static uint16_t virtual_angle = 0; + uint16_t icr3_temp = ICR3; TLaser_edge_type current_edge; /* Check which kind of edge triggered the interrupt */ @@ -185,29 +186,44 @@ ISR(TIMER3_CAPT_vect) { /* First rising edge of a reflector */ case LASER_FIRST_RISING_EDGE: - virtual_angle = ICR3; + virtual_angle = icr3_temp; break; /* Common rising edge of a reflector */ case LASER_RISING_EDGE: /* Recompute the angle value */ - virtual_angle = (virtual_angle + ICR3) / 2; + if(ICR3 < virtual_angle) + { + virtual_angle = ((virtual_angle + icr3_temp + CODEWHEEL_CPR) / 2)%(CODEWHEEL_CPR+1); + } + else + { + virtual_angle = (virtual_angle + icr3_temp) / 2; + } break; /* Falling edge detected */ case LASER_FALLING_EDGE: /* Recompute the angle value */ - virtual_angle = (virtual_angle + ICR3) / 2; - /* UseI ICR3 for now*/ - virtual_angle = ICR3; + if(ICR3 < virtual_angle) + { + virtual_angle = ((virtual_angle + icr3_temp + CODEWHEEL_CPR) / 2)%(CODEWHEEL_CPR+1); + } + else + { + virtual_angle = (virtual_angle + icr3_temp) / 2; + } + + /* For now we use directly ICR3 */ + virtual_angle = icr3_temp; /* It's a falling edge so potentially current_angle could be a real one */ laser_set_angle(virtual_angle); /* Start virtual angle confirmation */ - laser_engage_angle_confirmation(ICR3); + laser_engage_angle_confirmation(icr3_temp); break; default: -- cgit v1.2.3