From b8a2122276a91deba4f9cc74e994815fb3a4bf17 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Tue, 17 Apr 2012 22:38:40 +0200 Subject: digital/beacon: import calibration files --- digital/beacon/src/calibration.c | 216 +++++++++++++++++++++++++++++++++++++++ digital/beacon/src/calibration.h | 93 +++++++++++++++++ digital/beacon/src/debug.c | 9 ++ digital/beacon/src/main_avr.c | 3 +- 4 files changed, 320 insertions(+), 1 deletion(-) create mode 100644 digital/beacon/src/calibration.c create mode 100644 digital/beacon/src/calibration.h diff --git a/digital/beacon/src/calibration.c b/digital/beacon/src/calibration.c new file mode 100644 index 00000000..75e33fba --- /dev/null +++ b/digital/beacon/src/calibration.c @@ -0,0 +1,216 @@ +/* calibration.c */ +/* Beacon servomotor calibration. {{{ + * + * Copyright (C) 2012 Florent Duchon + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +#include +#include "debug.h" +#include "servo.h" +#include "calibration.h" + +HAL_AppTimer_t calibrationTimer; // TIMER descripor used by the DEBUG task +calibration_s calibration; + + +/* This function iniatializes the calibration structure*/ +void calibration_init_structure(void) +{ + /* Calibration initial values */ + calibration.state = CALIBRATION_INIT; + calibration.laser_flag = CLEAR; +} + +/* This function starts the calibration task */ +void calibration_start_task(void) +{ + calibrationTimer.interval = CALIBRATION_FAST_TASK_PERIOD; + calibrationTimer.mode = TIMER_REPEAT_MODE; + calibrationTimer.callback = calibration_task; + calibration_init_structure(); + servo_structure_init(); + HAL_StartAppTimer(&calibrationTimer); +} + +/* This function stops the calibration task */ +void calibration_stop_task(void) +{ + HAL_StopAppTimer(&calibrationTimer); +} + +/* This function starts or stops the calibration task depending on the current state */ +void calibration_start_stop_task(void) +{ + static bool task_step = 0; + if(task_step == 0) + { + calibration_start_task(); + task_step = 1; + } + else + { + calibration_stop_task(); + task_step = 0; + } +} + +/* This function restarts the calibration task changing its frequency */ +void calibration_change_task_frequency(uint32_t frequency) +{ + HAL_StopAppTimer(&calibrationTimer); + calibrationTimer.interval = frequency; + calibrationTimer.mode = TIMER_REPEAT_MODE; + calibrationTimer.callback = calibration_task; + HAL_StartAppTimer(&calibrationTimer); +} + + +/* Calibration task is used to calibrated the servo motors in order to run the laser parallel to the table */ +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)) + servo = SERVO_1; + else + servo = SERVO_2; + + switch(calibration.state) + { + case CALIBRATION_INIT: + + /* Go directly to next step */ + calibration.state = CALIBRATION_FAST_SCANNING; + break; + + 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 + { + /* 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)+FAST_SCANNING_OFFSET); + servo_set_state(servo,SERVO_SCANNING_FAST_FINISHED); + + 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 */ + calibration.state = CALIBRATION_SLOW_SCANNING; + + /* For better accuracy change task frequency for SLOW SCANNING */ + calibration_change_task_frequency(CALIBRATION_SLOW_TASK_PERIOD); + } + } + 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_FAST_IN_PROGRESS); + } + else + { + /* 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 + { + /* 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(); + } + } + } + break; + case SCANNING_STATE_CALIBRATED: + break; + default: + break; + } +} + +/* This function drives the defined servo motor in order to scan verticaly */ +void calibration_scanning(TServo_ID servo_id) +{ + uint16_t next_value = 0; + uint16_t current_value = 0; + + /* Compute next value to set to the servo */ + next_value = servo_get_value(servo_id) + servo_get_scanning_sense(servo_id)*SCANNING_STEP; + + /* Set it and check the return value in order to inverse the sense MIN or MAX is reached */ + current_value = servo_set_value(servo_id,next_value); + + if((current_value <= SERVO_ANGLE_MIN) || (current_value >= SERVO_ANGLE_MAX)) + { + servo_inverse_scanning_sense(servo_id); + } +} + +/* This function sets the laser flag according the given value SET or CLEAR */ +void calibration_set_laser_flag(TLaser_flag_type value) +{ + calibration.laser_flag = value; +} + +/* This function returns the laser flag */ +TLaser_flag_type calibration_get_laser_flag(void) +{ + return calibration.laser_flag; +} + +/* This function returns the calibration state */ +TCalibration_state calibration_get_state(void) +{ + return calibration.state; +} diff --git a/digital/beacon/src/calibration.h b/digital/beacon/src/calibration.h new file mode 100644 index 00000000..3512eedf --- /dev/null +++ b/digital/beacon/src/calibration.h @@ -0,0 +1,93 @@ +/* calibration.h */ +/* Beacon servomotor calibration. {{{ + * + * Copyright (C) 2012 Florent Duchon + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +#ifndef _CALIBRATION_H +#define _CALIBRATION_H + +#include "servo.h" + +#define CALIBRATION_FAST_TASK_PERIOD 11L +#define CALIBRATION_SLOW_TASK_PERIOD 110L + +#define FAST_SCANNING_OFFSET 10 +#define SCANNING_STEP 1 + +typedef enum +{ + CLEAR, + SET +} TLaser_flag_type; + +typedef enum +{ + FAST_SCANNING, + SLOW_SCANNING +} TScanning_mode; + +typedef enum +{ + CALIBRATION_INIT, + CALIBRATION_FAST_SCANNING, + CALIBRATION_SLOW_SCANNING, + SCANNING_STATE_CALIBRATED +} TCalibration_state; + +typedef struct +{ + TCalibration_state state; + TLaser_flag_type laser_flag; +} calibration_s; + +/* This function iniatializes the calibration structure*/ +void calibration_init_structure(void); + +/* This function starts the calibration task */ +void calibration_start_task(void); + +/* This function stops the calibration task */ +void calibration_stop_task(void); + +/* This function starts or stops the calibration task depending on the current state */ +void calibration_start_stop_task(void); + +/* This function restarts the calibration task changing its frequency */ +void calibration_change_task_frequency(uint32_t frequency); + +/* Calibration task is used to calibrated the servo motors in order to run the laser parallel to the table */ +void calibration_task(void); + +/* This function drives the defined servo motor in order to scan verticaly */ +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); + +/* This function returns the laser flag */ +TLaser_flag_type calibration_get_laser_flag(void); + +/* This function returns the calibration state */ +TCalibration_state calibration_get_state(void); + +#endif diff --git a/digital/beacon/src/debug.c b/digital/beacon/src/debug.c index dc3e412e..221f606c 100644 --- a/digital/beacon/src/debug.c +++ b/digital/beacon/src/debug.c @@ -26,6 +26,7 @@ #include #include #include "configuration.h" +#include "calibration.h" #include "debug.h" #include "servo.h" #include "sensors.h" @@ -33,6 +34,7 @@ HAL_UsartDescriptor_t appUsartDescriptor; // USART descriptor (required by stack) HAL_AppTimer_t debugTimer; // TIMER descripor used by the DEBUG task + uint8_t usartRxBuffer[APP_USART_RX_BUFFER_SIZE]; // USART Rx buffer uint8_t usartTxBuffer[APP_USART_TX_BUFFER_SIZE]; // USART Tx buffer @@ -117,6 +119,12 @@ void usartRXCallback(uint16_t bytesToRead) case 'z': sensors_codewheel_reset(); break; + case 'c': + calibration_start_stop_task(); + break; + case 'q': + calibration_set_laser_flag(SET); + break; /* Default */ default : uprintf(" ?? Unknown command ??\r\n"); @@ -175,5 +183,6 @@ void debug_task(void) uprintf("NWK : status = 0x%x\r\n",network_get_status()); #ifdef TYPE_END uprintf("CodeWheel = %d\r\n",sensors_codewheel_get_value()); + uprintf("Calibration state = %d\r\n",calibration_get_state()); #endif } diff --git a/digital/beacon/src/main_avr.c b/digital/beacon/src/main_avr.c index 9da8356d..70819ace 100644 --- a/digital/beacon/src/main_avr.c +++ b/digital/beacon/src/main_avr.c @@ -26,6 +26,7 @@ #include #include #include "configuration.h" +#include "calibration.h" #include "network.h" #include "sensors.h" #include "servo.h" @@ -74,7 +75,7 @@ void APL_TaskHandler(void) uprintf("DEVICE_TYPE_COORDINATOR init OK\n\r"); break; case DEVICE_TYPE_END_DEVICE: - servo_timer1_init(); + servo_init(); sensors_laser_init(); sensors_codewheel_init(); sensors_laser_init(); -- cgit v1.2.3