From 0e853d3c6fe3a1701ca977dfa0f155e87b93a66c Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 16:57:28 +0200 Subject: digital/beacon: remove unessecary wheelTimer task --- digital/beacon/src/debug_avr.c | 1 - 1 file changed, 1 deletion(-) diff --git a/digital/beacon/src/debug_avr.c b/digital/beacon/src/debug_avr.c index b195a4f2..a3eec0f3 100644 --- a/digital/beacon/src/debug_avr.c +++ b/digital/beacon/src/debug_avr.c @@ -37,7 +37,6 @@ HAL_UsartDescriptor_t appUsartDescriptor; // USART descriptor (required by stack) HAL_AppTimer_t debugTimer; // TIMER descripor used by the DEBUG task -HAL_AppTimer_t wheelTimer; // 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 -- cgit v1.2.3 From 57e774eee922780d6a584be72a8984d2a3711987 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:00:33 +0200 Subject: digital/beacon: broadcast reset API --- digital/beacon/src/debug_avr.c | 3 +++ digital/beacon/src/network.c | 3 +++ digital/beacon/src/network.h | 3 ++- 3 files changed, 8 insertions(+), 1 deletion(-) diff --git a/digital/beacon/src/debug_avr.c b/digital/beacon/src/debug_avr.c index a3eec0f3..c99286aa 100644 --- a/digital/beacon/src/debug_avr.c +++ b/digital/beacon/src/debug_avr.c @@ -137,6 +137,9 @@ void usartRXCallback(uint16_t bytesToRead) case 'j': jack_on_off(); break; + case '0': + network_send_data(NETWORK_RESET,0x1); + break; /* Default */ default : uprintf(" ?? Unknown command ??\r\n"); diff --git a/digital/beacon/src/network.c b/digital/beacon/src/network.c index fe413314..51cb1ea2 100644 --- a/digital/beacon/src/network.c +++ b/digital/beacon/src/network.c @@ -299,6 +299,9 @@ void APS_DataIndication(APS_DataInd_t* indData) /* New angle is avaiiable, update position */ // update_position(appMessage->data[NETWORK_MSG_ADDR_FIELD],appMessage->data[NETWORK_MSG_DATA_FIELD]); break; + case NETWORK_RESET: + reset_avr(); + break; default: uprintf("Unknown data type received = %x\r\n",appMessage->data[NETWORK_MSG_TYPE_FIELD]); break; diff --git a/digital/beacon/src/network.h b/digital/beacon/src/network.h index 88c875f2..a29a24d2 100644 --- a/digital/beacon/src/network.h +++ b/digital/beacon/src/network.h @@ -47,7 +47,8 @@ typedef enum { NETWORK_JACK_STATE, NETWORK_OPPONENT_NUMBER, - NETWORK_ANGLE_RAW + NETWORK_ANGLE_RAW, + NETWORK_RESET } TMessage_type; -- cgit v1.2.3 From a9618ac60931d7f052b694de72ba79f43353f081 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:01:17 +0200 Subject: digital/beacon: remove WHEEL_TASK_PERIOD --- digital/beacon/src/debug_avr.h | 1 - 1 file changed, 1 deletion(-) diff --git a/digital/beacon/src/debug_avr.h b/digital/beacon/src/debug_avr.h index 78639d74..bbf9a370 100644 --- a/digital/beacon/src/debug_avr.h +++ b/digital/beacon/src/debug_avr.h @@ -35,7 +35,6 @@ #define READ_USART HAL_ReadUsart #define USART_CHANNEL APP_USART_CHANNEL #define DEBUG_TASK_PERIOD 700L -#define WHEEL_TASK_PERIOD 1000L typedef enum { -- cgit v1.2.3 From dba137dc4ec11f695f853d643fd8621500b7d9b0 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:03:14 +0200 Subject: digital/beacon: import jack_update_status API --- digital/beacon/src/misc.c | 11 +++++++++++ digital/beacon/src/misc.h | 3 +++ 2 files changed, 14 insertions(+) diff --git a/digital/beacon/src/misc.c b/digital/beacon/src/misc.c index 04db1de5..c7bef9d2 100644 --- a/digital/beacon/src/misc.c +++ b/digital/beacon/src/misc.c @@ -45,3 +45,14 @@ void jack_on_off(void) } +/* This function sends the jack status to the slave beacons*/ +void jack_update_status(uint8_t value) +{ + uint8_t old_jack = 0; + if(value != old_jack) + { + network_send_data(NETWORK_JACK_STATE,value); + old_jack = value; + } +} + diff --git a/digital/beacon/src/misc.h b/digital/beacon/src/misc.h index 95bba519..faa6ca7a 100644 --- a/digital/beacon/src/misc.h +++ b/digital/beacon/src/misc.h @@ -32,4 +32,7 @@ void reset_avr(void); /* This function simulates the jack's state and send it over the air */ void jack_on_off(void); +/* This function sends the jack status to the slave beacons*/ +void jack_update_status(uint8_t value); + #endif -- cgit v1.2.3 From 640ae537ae3536361b7401de740e3c2be94a8a37 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:04:00 +0200 Subject: digital/beacon: control motor via PWM with TIMER0 --- digital/beacon/src/motor.c | 31 ++++++++++++++++++++++++++----- 1 file changed, 26 insertions(+), 5 deletions(-) diff --git a/digital/beacon/src/motor.c b/digital/beacon/src/motor.c index c487c22a..56682b48 100644 --- a/digital/beacon/src/motor.c +++ b/digital/beacon/src/motor.c @@ -28,24 +28,41 @@ /* This function initializes the motor control output */ void motor_init(void) { - DDRB |= (1<= average_value + SERVO_WAVE_OFFSET)) + { + servo_inverse_scanning_sense(servo_id); + } +} + +/* Wave Task */ +void servo_wave_task(void) +{ + static bool first_time = 1; + static uint8_t average_value[2] = {0}; + + /* Get and save the average value found with the calibration */ + if(first_time == 1) + { + average_value[SERVO_1] = servo_get_value(SERVO_1); + average_value[SERVO_2] = servo_get_value(SERVO_2); + first_time = 0; + } + + /* Scan each servos */ + servo_waveform_scanning(SERVO_1,average_value[SERVO_1]); + servo_waveform_scanning(SERVO_2,average_value[SERVO_2]); +} + +/* Start wave task */ +void servo_start_wave_task(void) +{ + waveTimer.interval = WAVE_TASK_PERIOD; + waveTimer.mode = TIMER_REPEAT_MODE; + waveTimer.callback = servo_wave_task; + HAL_StartAppTimer(&waveTimer); +} + SIGNAL (SIG_OVERFLOW1) { } diff --git a/digital/beacon/src/servo.h b/digital/beacon/src/servo.h index f36e1742..131f3075 100644 --- a/digital/beacon/src/servo.h +++ b/digital/beacon/src/servo.h @@ -95,4 +95,13 @@ int8_t servo_get_scanning_sense(TServo_ID servo_id); /* This function inverses the scanning sense of the servo */ void servo_inverse_scanning_sense(TServo_ID servo_id); +/* This function generates a wave scanning */ +void servo_waveform_scanning(TServo_ID servo_id, uint8_t average_value); + +/* Wave Task */ +void servo_wave_task(void); + +/* Start wave task */ +void servo_start_wave_task(void); + #endif -- cgit v1.2.3 From 8b1d462ca24afa1cda50ed994856fcc644c4cc00 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:06:24 +0200 Subject: digital/beacon: add defines for wave scanning --- digital/beacon/src/servo.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/digital/beacon/src/servo.h b/digital/beacon/src/servo.h index 131f3075..0eb5228b 100644 --- a/digital/beacon/src/servo.h +++ b/digital/beacon/src/servo.h @@ -29,6 +29,8 @@ #define SERVO_ANGLE_MIN 69 #define SERVO_ANGLE_MAX 254 +#define WAVE_TASK_PERIOD 15L +#define SERVO_WAVE_OFFSET (uint16_t)6 #define SERVO_1_ANGLE_POSITION 0 #define SERVO_2_ANGLE_POSITION 270 #define SERVO_ANGLE_POSITION_TOLERANCE 20 -- cgit v1.2.3 From 101b15e4721f6a6579771445ee4ade7647f1d5b8 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:06:57 +0200 Subject: digital/beacon: decrease calibration servo angle tolerance --- digital/beacon/src/servo.h | 1 + 1 file changed, 1 insertion(+) diff --git a/digital/beacon/src/servo.h b/digital/beacon/src/servo.h index 0eb5228b..fa437c36 100644 --- a/digital/beacon/src/servo.h +++ b/digital/beacon/src/servo.h @@ -29,6 +29,7 @@ #define SERVO_ANGLE_MIN 69 #define SERVO_ANGLE_MAX 254 +#define SERVO_ANGLE_POSITION_TOLERANCE 5 #define WAVE_TASK_PERIOD 15L #define SERVO_WAVE_OFFSET (uint16_t)6 #define SERVO_1_ANGLE_POSITION 0 -- cgit v1.2.3 From 63cb73a6ee91deb2ca0f1458595eb45bc172632e Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:07:37 +0200 Subject: digital/beacon: modify SERVO_2 angle position according to the beacon number (and fix double define) --- digital/beacon/src/servo.h | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/digital/beacon/src/servo.h b/digital/beacon/src/servo.h index fa437c36..7066643c 100644 --- a/digital/beacon/src/servo.h +++ b/digital/beacon/src/servo.h @@ -30,11 +30,17 @@ #define SERVO_ANGLE_MAX 254 #define SERVO_ANGLE_POSITION_TOLERANCE 5 + #define WAVE_TASK_PERIOD 15L #define SERVO_WAVE_OFFSET (uint16_t)6 + + #define SERVO_1_ANGLE_POSITION 0 +#ifdef LOL_NUMBER_3 #define SERVO_2_ANGLE_POSITION 270 -#define SERVO_ANGLE_POSITION_TOLERANCE 20 +#else +#define SERVO_2_ANGLE_POSITION 90 +#endif #define RISING 1 -- cgit v1.2.3 From 1d36a77e506bb5beb5ac4d0a33d72be7faef7d47 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:08:48 +0200 Subject: digital/beacon: add twi specific --- digital/beacon/src/main_avr.c | 26 +---------- digital/beacon/src/twi_specific.c | 96 +++++++++++++++++++++++++++++++++++++++ digital/beacon/src/twi_specific.h | 68 +++++++++++++++++++++++++++ 3 files changed, 166 insertions(+), 24 deletions(-) create mode 100644 digital/beacon/src/twi_specific.c create mode 100644 digital/beacon/src/twi_specific.h diff --git a/digital/beacon/src/main_avr.c b/digital/beacon/src/main_avr.c index 4f5cc5eb..180c2060 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 "twi_specific.h" #include "calibration.h" #include "codewheel.h" #include "debug_avr.h" @@ -45,32 +46,9 @@ AppState_t appState = APP_INITIAL_STATE; // application state #else DeviceType_t deviceType = DEVICE_TYPE_END_DEVICE; #endif - - -void twi_RXTX_update(void) -{ - uint8_t TXbuffer[AC_TWI_SLAVE_SEND_BUFFER_SIZE]; - uint8_t RXbuffer[AC_TWI_SLAVE_RECV_BUFFER_SIZE]; - uint8_t RXlen; - - /* data to be communicated to the master */ - twi_slave_update (TXbuffer, sizeof (TXbuffer)); - - /* Check for data. */ - RXlen = twi_slave_poll (RXbuffer, AC_TWI_SLAVE_RECV_BUFFER_SIZE); - - /* data availlable */ - if(RXlen != 0) - { - } -} void APL_TaskHandler(void) { - if(deviceType == DEVICE_TYPE_COORDINATOR) - { - twi_RXTX_update(); - } switch (appState) { case APP_INITIAL_STATE: @@ -85,7 +63,7 @@ void APL_TaskHandler(void) { case DEVICE_TYPE_COORDINATOR: network_init(); - twi_init(AC_BEACON_TWI_ADDRESS); + twi_init_specific(); uprintf("COORDINATOR initialisation OK !\n\r"); break; case DEVICE_TYPE_END_DEVICE: diff --git a/digital/beacon/src/twi_specific.c b/digital/beacon/src/twi_specific.c new file mode 100644 index 00000000..5def1a2b --- /dev/null +++ b/digital/beacon/src/twi_specific.c @@ -0,0 +1,96 @@ +/* twi_specific.c */ +/* twi specific funtion. {{{ + * + * 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 "modules/utils/byte.h" +#include "modules/utils/crc.h" +#include "twi_specific.h" +#include "configuration.h" +#include "position.h" +#include "misc.h" +#include "twi.h" +#include "network.h" + + +static HAL_AppTimer_t twiTimer; + +/* This function manages the TWI RX/RX transferts */ +void twi_task(void) +{ + uint8_t TXbuffer[AC_TWI_SLAVE_SEND_BUFFER_SIZE]; + uint8_t RXbuffer[AC_TWI_SLAVE_RECV_BUFFER_SIZE]; + uint8_t RXlen; + static uint8_t seq = 0; + + TXbuffer[TWI_TX_RFU1_FIELD] = 0; + TXbuffer[TWI_TX_RFU2_FIELD] = 0; + TXbuffer[TWI_TX_SEQ_FIELD] = seq; + + /* Opponent 1 */ + TXbuffer[TWI_TX_X1_MSB_FIELD] = v16_to_v8 (position_get_coord(OPPONENT_1,X), 1); + TXbuffer[TWI_TX_X1_LSB_FIELD] = v16_to_v8 (position_get_coord(OPPONENT_1,X), 0); + TXbuffer[TWI_TX_Y1_MSB_FIELD] = v16_to_v8 (position_get_coord(OPPONENT_1,Y), 1); + TXbuffer[TWI_TX_Y1_LSB_FIELD] = v16_to_v8 (position_get_coord(OPPONENT_1,Y), 0); + TXbuffer[TWI_TX_TRUST1_FIELD] = position_get_trust(OPPONENT_1); + + /* Opponent 2 */ + TXbuffer[TWI_TX_X2_MSB_FIELD] = v16_to_v8 (position_get_coord(OPPONENT_2,X), 1); + TXbuffer[TWI_TX_X2_LSB_FIELD] = v16_to_v8 (position_get_coord(OPPONENT_2,X), 0); + TXbuffer[TWI_TX_Y2_MSB_FIELD] = v16_to_v8 (position_get_coord(OPPONENT_2,Y), 1); + TXbuffer[TWI_TX_Y2_LSB_FIELD] = v16_to_v8 (position_get_coord(OPPONENT_2,Y), 0); + TXbuffer[TWI_TX_TRUST2_FIELD] = position_get_trust(OPPONENT_2); + + /* Compute CRC */ + TXbuffer[TWI_TX_CRC_FIELD] = crc_compute (&TXbuffer[1], sizeof (TXbuffer) - 1); + + /* data to be communicated to the master */ + twi_slave_update (TXbuffer, sizeof (TXbuffer)); + + /* Check for data. */ + RXlen = twi_slave_poll (RXbuffer, AC_TWI_SLAVE_RECV_BUFFER_SIZE); + + /* data availlable */ + if(RXlen != 0) + { + if (crc_compute (RXbuffer + 1, RXlen - 1) == RXbuffer[0]) + { + jack_update_status(RXbuffer[TWI_RX_JACK_FIELD]); + } + else + { + /* CRC not valid, dump values */ + } + } +} + +/* Initialisze specific TWI */ +void twi_init_specific(void) +{ + twi_init(AC_BEACON_TWI_ADDRESS); + twiTimer.interval = TWI_TASK_PERIOD; + twiTimer.mode = TIMER_REPEAT_MODE; + twiTimer.callback = twi_task; + HAL_StartAppTimer(&twiTimer); +} + diff --git a/digital/beacon/src/twi_specific.h b/digital/beacon/src/twi_specific.h new file mode 100644 index 00000000..ee244cb7 --- /dev/null +++ b/digital/beacon/src/twi_specific.h @@ -0,0 +1,68 @@ +/* twi_specific.h */ +/* twi specific funtion. {{{ + * + * 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 _TWI_SPECIFIC_H +#define _TWI_SPECIFIC_H + +#define TWI_TASK_PERIOD 4L + + +typedef enum +{ + TWI_RX_CRC_FIELD, + TWI_RX_SEQ_FIELD, + TWI_RX_JACK_FIELD, + TWI_RX_NB_ADV_FIELD, + TWI_RX_X_MSB_FIELD, + TWI_RX_X_LSB_FIELD, + TWI_RX_Y_MSB_FIELD, + TWI_RX_Y_LSB_FIELD +} TTWI_RX_field; + +typedef enum +{ + TWI_TX_CRC_FIELD, + TWI_TX_RFU1_FIELD, + TWI_TX_RFU2_FIELD, + TWI_TX_SEQ_FIELD, + TWI_TX_X1_MSB_FIELD, + TWI_TX_X1_LSB_FIELD, + TWI_TX_Y1_MSB_FIELD, + TWI_TX_Y1_LSB_FIELD, + TWI_TX_TRUST1_FIELD, + TWI_TX_X2_MSB_FIELD, + TWI_TX_X2_LSB_FIELD, + TWI_TX_Y2_MSB_FIELD, + TWI_TX_Y2_LSB_FIELD, + TWI_TX_TRUST2_FIELD +} TTWI_TX_field; + +/* This function manages the TWI RX/RX transferts */ +void twi_task(void); + +/* Initialisze specific TWI */ +void twi_init_specific(void); + +#endif \ No newline at end of file -- cgit v1.2.3 From 2fdb36ec76670c57015d7d6054190ed9154d04e5 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:10:06 +0200 Subject: digital/beacon: init position structure for COOR device --- digital/beacon/src/main_avr.c | 1 + 1 file changed, 1 insertion(+) diff --git a/digital/beacon/src/main_avr.c b/digital/beacon/src/main_avr.c index 180c2060..9c687e1e 100644 --- a/digital/beacon/src/main_avr.c +++ b/digital/beacon/src/main_avr.c @@ -64,6 +64,7 @@ void APL_TaskHandler(void) case DEVICE_TYPE_COORDINATOR: network_init(); twi_init_specific(); + position_init_struct(); uprintf("COORDINATOR initialisation OK !\n\r"); break; case DEVICE_TYPE_END_DEVICE: -- cgit v1.2.3 From cdd38c405ab09b07ba6d65f5296639ac542f08cd Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:10:41 +0200 Subject: digital/beacon: moditfy position init declaration for simu part --- digital/beacon/src/main_simu.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/beacon/src/main_simu.c b/digital/beacon/src/main_simu.c index 706b360b..0ad0dfdd 100644 --- a/digital/beacon/src/main_simu.c +++ b/digital/beacon/src/main_simu.c @@ -41,7 +41,7 @@ int main (int argc, char **argv) int angle_id = 0; int i = 0; /* Init global structures */ - init_struct(); + position_init_struct(); while(1) { -- cgit v1.2.3 From a91be193785c1675d30e1ecae4315ae903dd4857 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:11:28 +0200 Subject: digital/beacon: add -lm to the link command --- .../beacon/src/makefiles/Makefile_All_ZigBit_Atmega1281_Rf230_8Mhz_Gcc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/beacon/src/makefiles/Makefile_All_ZigBit_Atmega1281_Rf230_8Mhz_Gcc b/digital/beacon/src/makefiles/Makefile_All_ZigBit_Atmega1281_Rf230_8Mhz_Gcc index 3f770992..5c46bb6e 100644 --- a/digital/beacon/src/makefiles/Makefile_All_ZigBit_Atmega1281_Rf230_8Mhz_Gcc +++ b/digital/beacon/src/makefiles/Makefile_All_ZigBit_Atmega1281_Rf230_8Mhz_Gcc @@ -115,7 +115,7 @@ All_ZigBit_Atmega1281_Rf230_8Mhz_Gcc/Obj/twi_hard.avr.o: CFLAGS=$(APB_CFLAGS) $(EXE_PATH)/$(APP_NAME).elf: $(OBJS) - $(LD) $(LINKER_FLAGS) $(OBJS) -Wl,-\( $(LIBS) -Wl,-\) -o $@ + $(LD) $(LINKER_FLAGS) $(OBJS) -lm -Wl,-\( $(LIBS) -Wl,-\) -o $@ $(EXE_PATH)/$(APP_NAME).srec: $(EXE_PATH)/$(APP_NAME).elf $(OBJCOPY) -O srec --srec-len 128 $^ $@ -- cgit v1.2.3 From 7fbc838ce6be9ff8b40b4f2c6c5c407f0aa4c181 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:12:32 +0200 Subject: digital/beacon: add Opponent ID struct and coord type --- digital/beacon/src/position.h | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/digital/beacon/src/position.h b/digital/beacon/src/position.h index 8cc77f2a..c5052fda 100644 --- a/digital/beacon/src/position.h +++ b/digital/beacon/src/position.h @@ -38,6 +38,16 @@ typedef enum{ POSITION_IGNORE_ANGLE } TPositionStatus; +typedef enum{ + OPPONENT_1 = 1, + OPPONENT_2 +} TOpponent_ID; + +typedef enum{ + X, + Y +} TCoord_type; + /* Structures definition */ /* Beacon Structure */ -- cgit v1.2.3 From 3d2da1ff79576010ede40a87679b75dd421db819 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:13:23 +0200 Subject: digital/beacon: convert int to uint16_t --- digital/beacon/src/position.c | 3 ++- digital/beacon/src/position.h | 8 ++++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/digital/beacon/src/position.c b/digital/beacon/src/position.c index 96bfb5e6..75fbad48 100644 --- a/digital/beacon/src/position.c +++ b/digital/beacon/src/position.c @@ -55,7 +55,8 @@ void init_struct(void) } } -int update_position(int beaconID, int angleID, double angle) +/* This function update the opponent position when a new angle is avalaible */ +int update_position(uint16_t beaconID, uint16_t angleID, float angle) { static int last_ID[2] = {0}; int last_valid_id = 0; diff --git a/digital/beacon/src/position.h b/digital/beacon/src/position.h index c5052fda..215c64ba 100644 --- a/digital/beacon/src/position.h +++ b/digital/beacon/src/position.h @@ -53,16 +53,16 @@ typedef enum{ /* Beacon Structure */ typedef struct { - int angleNumber; + uint16_t angleNumber; float angle[MAX_OBSTACLE+1]; }beacon_s; /* Obstacle structure */ typedef struct { - int x; - int y; - int trust; + int16_t x; + int16_t y; + int8_t trust; }opponent_s; /* Coordinates structure */ -- cgit v1.2.3 From a55506ba721610388545ff868d291caffe95f49f Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:15:50 +0200 Subject: digital/beacon: Improve deconnection/reconnection state machine --- digital/beacon/src/network.c | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/digital/beacon/src/network.c b/digital/beacon/src/network.c index 51cb1ea2..eb334a89 100644 --- a/digital/beacon/src/network.c +++ b/digital/beacon/src/network.c @@ -144,7 +144,6 @@ void network_leave(void) ZDO_MgmtLeaveReq_t *zdpLeaveReq = &leaveReq.req.reqPayload.mgmtLeaveReq; APS_UnregisterEndpointReq_t unregEndpoint; - appState = APP_NETWORK_LEAVING_STATE; unregEndpoint.endpoint = endpointParams.simpleDescriptor->endpoint; APS_UnregisterEndpointReq(&unregEndpoint); @@ -164,9 +163,6 @@ void network_leave(void) /* Leave network response */ void zdpLeaveResp(ZDO_ZdpResp_t *zdpResp) { - // Try to rejoin the network - appState = APP_NETWORK_JOIN_REQUEST; - (void)zdpResp; } @@ -199,11 +195,7 @@ void ZDO_MgmtNwkUpdateNotf(ZDO_MgmtNwkUpdateNotf_t *nwkParams) break; case ZDO_NETWORK_LOST_STATUS: { - APS_UnregisterEndpointReq_t unregEndpoint; - unregEndpoint.endpoint = endpointParams.simpleDescriptor->endpoint; - APS_UnregisterEndpointReq(&unregEndpoint); - - // try to rejoin the network + network_leave(); appState = APP_NETWORK_JOIN_REQUEST; break; } @@ -269,6 +261,7 @@ void APS_DataConf(APS_DataConf_t* confInfo) if (MAX_RETRIES_BEFORE_REJOIN == retryCounter) { network_leave(); + appState = APP_NETWORK_JOIN_REQUEST; } else { -- cgit v1.2.3 From 130899fb27b2b841156d0e77376ef3142ba77d47 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:17:31 +0200 Subject: digital/beacon: angle is send only when device is connected to the network --- digital/beacon/src/network.c | 31 ++++++++++++++++++------------- 1 file changed, 18 insertions(+), 13 deletions(-) diff --git a/digital/beacon/src/network.c b/digital/beacon/src/network.c index eb334a89..6f184435 100644 --- a/digital/beacon/src/network.c +++ b/digital/beacon/src/network.c @@ -234,20 +234,25 @@ int8_t network_get_rssi(void) /* This function must be used to send data through zigbee network */ void network_send_data(TMessage_type type, uint16_t data) { - /* Message type*/ - zigbit_tx_buffer.message.data[NETWORK_MSG_TYPE_FIELD] = type; - /* Source address */ - zigbit_tx_buffer.message.data[NETWORK_MSG_ADDR_FIELD] = CS_NWK_ADDR; - - /* LSB Data */ - zigbit_tx_buffer.message.data[NETWORK_MSG_DATA_LSB_FIELD] = data; - - /* MSB Data */ - zigbit_tx_buffer.message.data[NETWORK_MSG_DATA_MSB_FIELD] = data >> 8; - - /* Bitcloud sending request */ - APS_DataReq(&config); + if(network_get_status() == APP_NETWORK_JOINED_STATE) + { + led_inverse(2); + /* Message type*/ + zigbit_tx_buffer.message.data[NETWORK_MSG_TYPE_FIELD] = type; + + /* Source address */ + zigbit_tx_buffer.message.data[NETWORK_MSG_ADDR_FIELD] = CS_NWK_ADDR; + + /* LSB Data */ + zigbit_tx_buffer.message.data[NETWORK_MSG_DATA_LSB_FIELD] = data; + + /* MSB Data */ + zigbit_tx_buffer.message.data[NETWORK_MSG_DATA_MSB_FIELD] = data >> 8; + + /* Bitcloud sending request */ + APS_DataReq(&config); + } } -- cgit v1.2.3 From 47baa758f3a938e8e078f2f746baee6268e71502 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:17:58 +0200 Subject: digital/beacon: add network_get_status() declaration into network.h --- digital/beacon/src/network.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/digital/beacon/src/network.h b/digital/beacon/src/network.h index a29a24d2..9fa1282c 100644 --- a/digital/beacon/src/network.h +++ b/digital/beacon/src/network.h @@ -66,6 +66,9 @@ void network_init(void); /* This function starts the network according to the defined configuraiton*/ void network_start(void); +/* This function returns the network status */ +uint16_t network_get_status(void); + /* ZDO_StartNetwork primitive confirmation callback */ void ZDO_StartNetworkConf(ZDO_StartNetworkConf_t* confirmInfo); -- cgit v1.2.3 From 6828871a64e6b8e31f3b5d9069a62696832a3697 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:18:23 +0200 Subject: digital/beacon: fix issue when sending 4 bytes --- digital/beacon/src/network.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/beacon/src/network.c b/digital/beacon/src/network.c index 6f184435..918ce3ad 100644 --- a/digital/beacon/src/network.c +++ b/digital/beacon/src/network.c @@ -120,7 +120,7 @@ void ZDO_StartNetworkConf(ZDO_StartNetworkConf_t* confirmInfo) config.clusterId = APP_CLUSTER_ID; // Desctination cluster ID config.srcEndpoint = APP_ENDPOINT; // Source endpoint config.asdu = &zigbit_tx_buffer.message; // application message pointer - config.asduLength = 3 + sizeof(zigbit_tx_buffer.message.messageId); // actual application message length + config.asduLength = 4 + sizeof(zigbit_tx_buffer.message.messageId); // actual application message length config.txOptions.acknowledgedTransmission = 0; // Acknowledged transmission enabled config.radius = 0; // Use maximal possible radius config.APS_DataConf = APS_DataConf; // Confirm handler Z -- cgit v1.2.3 From a84e8b9345568775fdd0fa9716b7379f6def3dd5 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:19:50 +0200 Subject: digital/beacon: start/stop the motor using NETWORK_JACK_STATE --- digital/beacon/src/network.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/digital/beacon/src/network.c b/digital/beacon/src/network.c index 918ce3ad..50b86d71 100644 --- a/digital/beacon/src/network.c +++ b/digital/beacon/src/network.c @@ -31,6 +31,7 @@ #include "debug_avr.h" #include "led.h" +#include "motor.h" // Endpoint parameters static SimpleDescriptor_t simpleDescriptor = { APP_ENDPOINT, APP_PROFILE_ID, 1, 1, 0, 0 , NULL, 0, NULL }; @@ -289,6 +290,7 @@ void APS_DataIndication(APS_DataInd_t* indData) switch(appMessage->data[NETWORK_MSG_TYPE_FIELD]) { case NETWORK_JACK_STATE: + motor_start_stop_control(); break; case NETWORK_OPPONENT_NUMBER: break; -- cgit v1.2.3 From 0a5ba20c1a69e82400cb2ecb4ab78bc589326c21 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:21:26 +0200 Subject: digital/beacon: update the opponnent position when a angle is received --- digital/beacon/src/network.c | 21 ++++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/digital/beacon/src/network.c b/digital/beacon/src/network.c index 50b86d71..bbcba4ec 100644 --- a/digital/beacon/src/network.c +++ b/digital/beacon/src/network.c @@ -30,8 +30,9 @@ #include "network.h" #include "debug_avr.h" #include "led.h" - #include "motor.h" +#include "position.h" +#include "misc.h" // Endpoint parameters static SimpleDescriptor_t simpleDescriptor = { APP_ENDPOINT, APP_PROFILE_ID, 1, 1, 0, 0 , NULL, 0, NULL }; @@ -283,7 +284,9 @@ void APS_DataConf(APS_DataConf_t* confInfo) /* APS data indication handler */ void APS_DataIndication(APS_DataInd_t* indData) { + uint8_t beacon = 0; uint16_t angle = 0; + uint16_t angle_id = 0; AppMessage_t *appMessage = (AppMessage_t *) indData->asdu; // Data received indication @@ -295,9 +298,21 @@ void APS_DataIndication(APS_DataInd_t* indData) case NETWORK_OPPONENT_NUMBER: break; case NETWORK_ANGLE_RAW: - angle = codewheel_convert_angle_raw2degrees((appMessage->data[NETWORK_MSG_DATA_MSB_FIELD]<<8) + appMessage->data[NETWORK_MSG_DATA_LSB_FIELD]); + + /* Beacon address */ + beacon = appMessage->data[NETWORK_MSG_ADDR_FIELD]; + + /* Angle ID */ + angle_id = appMessage->data[NETWORK_MSG_DATA_MSB_FIELD] >> 1; + + /* Angle value */ + angle = ((appMessage->data[NETWORK_MSG_DATA_MSB_FIELD]&0x01) << 8) + appMessage->data[NETWORK_MSG_DATA_LSB_FIELD]; + + /* For debug */ + uprintf("[%d] angle[%d] = %f\r\n",beacon,angle_id,codewheel_convert_angle_raw2degrees(angle)); + /* New angle is avaiiable, update position */ -// update_position(appMessage->data[NETWORK_MSG_ADDR_FIELD],appMessage->data[NETWORK_MSG_DATA_FIELD]); + update_position(beacon,angle_id,codewheel_convert_angle_raw2radians(angle)); break; case NETWORK_RESET: reset_avr(); -- cgit v1.2.3 From dddeed6ce0ba60ffee5d11a0c4babadda5e2f552 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:22:04 +0200 Subject: digitam/beacon: remove temp file --- .../lib/MakerulesBc_All_Atmega1281_Rf230_Gcc~ | 32 ---------------------- 1 file changed, 32 deletions(-) delete mode 100644 digital/beacon/src/Bitcloud_stack/lib/MakerulesBc_All_Atmega1281_Rf230_Gcc~ diff --git a/digital/beacon/src/Bitcloud_stack/lib/MakerulesBc_All_Atmega1281_Rf230_Gcc~ b/digital/beacon/src/Bitcloud_stack/lib/MakerulesBc_All_Atmega1281_Rf230_Gcc~ deleted file mode 100644 index eef6b708..00000000 --- a/digital/beacon/src/Bitcloud_stack/lib/MakerulesBc_All_Atmega1281_Rf230_Gcc~ +++ /dev/null @@ -1,32 +0,0 @@ -#---------------------------------------------- -#User application makerules - should be included into user application Makefile -#---------------------------------------------- - -include $(COMPONENTS_PATH)/../lib/Makerules_Atmega1281_Gcc - - -#-Compiler flags------------------------------- -CFLAGS = -Os -std=gnu99 -pipe -c -W -Wall -ffunction-sections -mmcu=atmega1281 -mcall-prologues -fshort-enums --param inline-call-cost=2 -DATMEGA1281 -DAT86RF230 -DNONE_OS -D_IEEE_ZIGBEE_COMPLIANCE_ -D_SYS_MAC_PHY_HWD_TASK_ -D_SYS_HAL_TASK_ -D_SYS_MAC_HWI_TASK_ -D_SYS_BSP_TASK_ -D_SYS_APL_TASK_ -D_SYS_NWK_TASK_ -D_SYS_APS_TASK_ -D_SYS_ZDO_TASK_ -D_COORDINATOR_ -D_ROUTER_ -D_ENDDEVICE_ -D_FFD_ -D_NWK_FAST_ROUTE_DISCOVERY_ -D_NWK_NONSTANDARD_BEACON_FILTER_ -D_NWK_GROUP_ -D_GROUP_TABLE_ -D_NWK_CHECK_OUT_BROADCAST_ -D_NWK_ROUTING_OPTIMIZATION_=3 -D_NWK_STOCHASTIC_ADDRESSING_ -D_RESOLVE_ADDR_CONFLICT_ -D_NWK_MESH_ROUTING_ -D_APS_FRAGMENTATION_ -D_APS_MULTICAST_ -D_GROUP_TABLE_ -D_BINDING_ -D_COMMISSIONING_ -D_POWER_FAILURE_ -D_NWK_PASSIVE_ACK_ -CFLAGS += $(BOARDCFLAGS) -#-Libraries names------------------------------ -CS_LIB = ConfigServer -PDS_LIB = PersistDataServer - -#-Stack components paths----------------------- -HAL_HWD_COMMON_PATH = $(COMPONENTS_PATH)/./HAL/avr/atmega1281/common -HAL_MAC_API_PATH = $(COMPONENTS_PATH)/./HAL/avr/atmega1281/zigBit -HAL_PATH = $(COMPONENTS_PATH)/./HAL -MAC_PHY_PATH = $(COMPONENTS_PATH)/./MAC_PHY -MAC_ENV_PATH = $(COMPONENTS_PATH)/./MAC_PHY/MAC_ENV -MAC_HWD_PATH = $(COMPONENTS_PATH)/./MAC_PHY/MAC_HWD_PHY -MAC_HWI_PATH = $(COMPONENTS_PATH)/./MAC_PHY/MAC_HWI -NWK_PATH = $(COMPONENTS_PATH)/./NWK -APS_PATH = $(COMPONENTS_PATH)/./APS -ZDO_PATH = $(COMPONENTS_PATH)/./ZDO -SSP_PATH = $(COMPONENTS_PATH)/./Security/ServiceProvider -TC_PATH = $(COMPONENTS_PATH)/./Security/TrustCentre -CS_PATH = $(COMPONENTS_PATH)/./ConfigServer -PDS_PATH = $(COMPONENTS_PATH)/./PersistDataServer -BSP_PATH = $(COMPONENTS_PATH)/./BSP -DRIVERS_PATH = $(COMPONENTS_PATH)/./HAL/drivers - -- cgit v1.2.3 From 9ec12521300f1d09404d1a1ae2f6d5632b15bce8 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:23:30 +0200 Subject: digital/beacon: add raw2radians codewheel conversion --- digital/beacon/src/codewheel.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/digital/beacon/src/codewheel.c b/digital/beacon/src/codewheel.c index fb8914c2..e53fef63 100644 --- a/digital/beacon/src/codewheel.c +++ b/digital/beacon/src/codewheel.c @@ -25,6 +25,7 @@ #include #include +#include #include "debug_avr.h" #include "codewheel.h" @@ -92,6 +93,12 @@ float codewheel_convert_angle_raw2degrees(uint16_t raw_value) return (float)raw_value*(float)360/(float)CODEWHEEL_CPR; } +/* This function converts the angle value from row format to radians */ +float codewheel_convert_angle_raw2radians(uint16_t raw_value) +{ + return (float)raw_value*(float)(2*M_PI)/(float)CODEWHEEL_CPR; +} + /* Codewheel complete turn IRQ vector for CodeWheel*/ ISR(TIMER3_COMPA_vect) { -- cgit v1.2.3 From c076c82d5952f52488561858ff0614fc627ff23e Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:23:45 +0200 Subject: digital/beacon: fix include order --- digital/beacon/src/codewheel.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/beacon/src/codewheel.c b/digital/beacon/src/codewheel.c index e53fef63..235643f4 100644 --- a/digital/beacon/src/codewheel.c +++ b/digital/beacon/src/codewheel.c @@ -23,8 +23,8 @@ * * }}} */ -#include #include +#include #include #include "debug_avr.h" #include "codewheel.h" -- cgit v1.2.3 From f79e9854b9aa5dd4099facb036c5e6845c6e2447 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:24:31 +0200 Subject: digital/beacon: add {} to an else --- digital/beacon/src/codewheel.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/digital/beacon/src/codewheel.c b/digital/beacon/src/codewheel.c index 235643f4..205c2e2b 100644 --- a/digital/beacon/src/codewheel.c +++ b/digital/beacon/src/codewheel.c @@ -108,5 +108,7 @@ ISR(TIMER3_COMPA_vect) codewheel_set_state(CODEWHEEL_REBASED); } else - OCR3A = CODEWHEEL_CPR; + { + OCR3A = CODEWHEEL_CPR; + } } -- cgit v1.2.3 From 36d1bbfc275e6ecb2ff267e224bac7a11f8b245e Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:25:12 +0200 Subject: digital/beacon: force calibration state during start --- digital/beacon/src/calibration.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/beacon/src/calibration.c b/digital/beacon/src/calibration.c index 1392519f..618fbcd3 100644 --- a/digital/beacon/src/calibration.c +++ b/digital/beacon/src/calibration.c @@ -45,6 +45,7 @@ void calibration_init_structure(void) /* This function starts the calibration task */ void calibration_start_task(void) { + calibration.state = CALIBRATION_INIT; calibrationTimer.interval = CALIBRATION_FAST_TASK_PERIOD; calibrationTimer.mode = TIMER_REPEAT_MODE; calibrationTimer.callback = calibration_task; @@ -59,7 +60,6 @@ void calibration_stop_task(void) { HAL_StopAppTimer(&calibrationTimer); motor_stop(); - calibration.state = CALIBRATION_INIT; } /* This function starts or stops the calibration task depending on the current state */ -- cgit v1.2.3 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/calibration.c | 76 ++++++++++++++++++++-------------------- digital/beacon/src/calibration.h | 11 +++--- digital/beacon/src/laser.c | 7 ++-- 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); } } } -- 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(-) 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(-) 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 From 631377be1c9e9e35de5346b70eb70f2ed8f38537 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:33:13 +0200 Subject: digital/beacon: fix laser struct --- digital/beacon/src/laser.h | 1 + 1 file changed, 1 insertion(+) diff --git a/digital/beacon/src/laser.h b/digital/beacon/src/laser.h index f0d32d93..f58cb3c8 100644 --- a/digital/beacon/src/laser.h +++ b/digital/beacon/src/laser.h @@ -40,6 +40,7 @@ typedef enum typedef struct { uint16_t angle; + uint16_t angle_id; } laser_s; /* This function initializes the laser pin input and associated interrupt */ -- cgit v1.2.3 From 18904c9fd8c88aaf355c0586095f932b205a764c Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:34:06 +0200 Subject: digital/beacon: add debug print --- digital/beacon/src/codewheel.c | 1 + 1 file changed, 1 insertion(+) diff --git a/digital/beacon/src/codewheel.c b/digital/beacon/src/codewheel.c index b28f756c..283439d6 100644 --- a/digital/beacon/src/codewheel.c +++ b/digital/beacon/src/codewheel.c @@ -108,6 +108,7 @@ ISR(TIMER3_COMPA_vect) { OCR3A = codewheel_get_rebase_offset(); codewheel_set_state(CODEWHEEL_REBASED); + uprintf("Rebased\r\n"); } else { -- cgit v1.2.3 From 3e4bd8280a6a19a8dabdf7ee5f17ae7e94baf40c Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:34:33 +0200 Subject: digital/beacon: fix header --- digital/beacon/src/codewheel.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/digital/beacon/src/codewheel.h b/digital/beacon/src/codewheel.h index f0ec26f5..5846c2e5 100644 --- a/digital/beacon/src/codewheel.h +++ b/digital/beacon/src/codewheel.h @@ -65,4 +65,7 @@ void codewheel_set_rebase_offset(uint16_t offset); /* This function converts the angle value from row format to degrees */ float codewheel_convert_angle_raw2degrees(uint16_t raw_value); +/* This function converts the angle value from row format to radians */ +float codewheel_convert_angle_raw2radians(uint16_t raw_value); + #endif -- cgit v1.2.3 From b4b3abb33cf07fb0ef00c81fc5592e5e7667e717 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:36:32 +0200 Subject: digital/beacon: add get coord and get trust API --- digital/beacon/src/position.c | 22 ++++++++++++++++++++++ digital/beacon/src/position.h | 15 +++++++++++++-- 2 files changed, 35 insertions(+), 2 deletions(-) diff --git a/digital/beacon/src/position.c b/digital/beacon/src/position.c index 75fbad48..f3320321 100644 --- a/digital/beacon/src/position.c +++ b/digital/beacon/src/position.c @@ -153,4 +153,26 @@ int update_position(uint16_t beaconID, uint16_t angleID, float angle) last_ID[0] = beaconID; } return 0; +} + +/* This function returns the requested coord according to the opponent number */ +int16_t position_get_coord(TOpponent_ID id, TCoord_type type) +{ + switch(type) + { + case X: + return opponent[id].x; + break; + case Y: + return opponent[id].y; + break; + default: + return 0; + } +} + +/* This function returns the trust according to opponent number */ +int8_t position_get_trust(TOpponent_ID id) +{ + return opponent[id].trust; } \ No newline at end of file diff --git a/digital/beacon/src/position.h b/digital/beacon/src/position.h index 215c64ba..67ec4173 100644 --- a/digital/beacon/src/position.h +++ b/digital/beacon/src/position.h @@ -26,7 +26,9 @@ #ifndef _POSITION_H #define _POSITION_H -#define MAX_OBSTACLE 2 +#include + +#define MAX_OBSTACLE 1 #define MAX_BEACON 3 #define MAX_TEMP_POSITION MAX_OBSTACLE * 2 #define OBSTACLE_RADIUS 100 @@ -81,6 +83,15 @@ typedef struct }recovery_s; /* This function is used to initialize all needed structures */ -void init_struct(void); +void position_init_struct(void); + +/* This function update the opponent position when a new angle is avalaible */ +int update_position(uint16_t beaconID, uint16_t angleID, float angle); + +/* This function returns the requested coord according to the opponent number */ +int16_t position_get_coord(TOpponent_ID id, TCoord_type type); + +/* This function returns the trust according to opponent number */ +int8_t position_get_trust(TOpponent_ID id); #endif \ No newline at end of file -- cgit v1.2.3 From 7253637b35f719f2eebdeb7f1d6a6b828ab5010b Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:37:20 +0200 Subject: digital/beacon: modify API name --- digital/beacon/src/position.c | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/digital/beacon/src/position.c b/digital/beacon/src/position.c index f3320321..79fc4c1f 100644 --- a/digital/beacon/src/position.c +++ b/digital/beacon/src/position.c @@ -23,6 +23,7 @@ * * }}} */ +#include #include "position.h" #include "debug_simu.h" #include "recovery.h" @@ -34,7 +35,7 @@ beacon_s beacon[MAX_BEACON+1]; opponent_s opponent[MAX_OBSTACLE+1]; /* This function is used to initialize all needed structures */ -void init_struct(void) +void position_init_struct(void) { int i = 0; int j = 0; @@ -58,16 +59,16 @@ void init_struct(void) /* This function update the opponent position when a new angle is avalaible */ int update_position(uint16_t beaconID, uint16_t angleID, float angle) { - static int last_ID[2] = {0}; - int last_valid_id = 0; - int which_formula = 0; + static uint16_t last_ID[2] = {0}; + uint16_t last_valid_id = 0; + uint16_t which_formula = 0; coord_s temp_position[MAX_TEMP_POSITION]; - int i = 0; + uint16_t i = 0; - int formula_status = 0; - int global_status = 0; - int update_status = UPDATE_OBSTACLE_NOT_FOUND; - int recovery_status = 0; + uint16_t formula_status = 0; + uint16_t global_status = 0; + uint16_t update_status = UPDATE_OBSTACLE_NOT_FOUND; + uint16_t recovery_status = 0; DEBUG_POSITION("Update_position with beaconID = %d and angleID = %d and angle = %f\n",(int)beaconID,(int) angleID, (double)angle); DEBUG_POSITION("last_ID[0] = %d last_ID[1] = %d\n",(int)last_ID[0],(int)last_ID[1]); -- cgit v1.2.3 From bcecd65375bb5e8c479f87703f2da3e39d03e3d7 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:38:16 +0200 Subject: digital/beacon: add motor speed management --- digital/beacon/src/debug_avr.c | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/digital/beacon/src/debug_avr.c b/digital/beacon/src/debug_avr.c index c99286aa..cc2b5d58 100644 --- a/digital/beacon/src/debug_avr.c +++ b/digital/beacon/src/debug_avr.c @@ -140,9 +140,18 @@ void usartRXCallback(uint16_t bytesToRead) case '0': network_send_data(NETWORK_RESET,0x1); break; + case 'w': + OCR0A--; + uprintf("OCR0A = %d\r\n",OCR0A); + break; + case 'x': + OCR0A++; + uprintf("OCR0A = %d\r\n",OCR0A); + break; /* Default */ default : uprintf(" ?? Unknown command ??\r\n"); + break; } } -- cgit v1.2.3 From 45dfaff3b05cb7f2e4ed42bd5464b7a2184bc227 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Wed, 16 May 2012 17:39:04 +0200 Subject: digital/beacon: modify debug task display --- digital/beacon/src/debug_avr.c | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/digital/beacon/src/debug_avr.c b/digital/beacon/src/debug_avr.c index cc2b5d58..1b0e0dfb 100644 --- a/digital/beacon/src/debug_avr.c +++ b/digital/beacon/src/debug_avr.c @@ -34,6 +34,7 @@ #include "network.h" #include "motor.h" #include "misc.h" +#include "position.h" HAL_UsartDescriptor_t appUsartDescriptor; // USART descriptor (required by stack) HAL_AppTimer_t debugTimer; // TIMER descripor used by the DEBUG task @@ -126,7 +127,7 @@ void usartRXCallback(uint16_t bytesToRead) calibration_start_stop_task(); break; case 'q': - calibration_set_laser_flag(SET); +// calibration_set_laser_flag(SET); break; case 'r': reset_avr(); @@ -208,15 +209,19 @@ void debug_task(void) uprintf("Status : 0x%x - ",network_get_status()); uprintf("LQI = %d - ",network_get_lqi()); uprintf("RSSI = %d - \r\n",network_get_rssi()); -#ifdef TYPE_END +#ifdef TYPE_COOR + uprintf("X = %d --- ",position_get_coord(OPPONENT_1,X)); + uprintf("Y = %d --- ",position_get_coord(OPPONENT_1,Y)); + uprintf("Trust = %d\r\n",position_get_trust(OPPONENT_1)); +#else + uprintf("## Calibration\r\n"); + uprintf("State : %d\r\n",calibration_get_state()); uprintf("## Servo\r\n"); uprintf("State : [1]=%d [2]=%d - ",servo_get_state(SERVO_1),servo_get_state(SERVO_2)); uprintf("Value : [1]=%d [2]=%d\r\n",servo_get_value(SERVO_1),servo_get_value(SERVO_2)); uprintf("## Codewheel\r\n"); - uprintf("Raw = %d - Degree = %f\r\n",codewheel_get_value(),codewheel_convert_angle_raw2degrees(codewheel_get_value())); - uprintf("## Calibration\r\n"); - uprintf("State : %d\r\n",calibration_get_state()); - uprintf("## Calibration\r\n"); - uprintf("State : %d\r\n",motor_get_state()); + uprintf("State = %d - Raw = %d - Degree = %f\r\n",codewheel_get_state(),codewheel_get_value(),codewheel_convert_angle_raw2degrees(codewheel_get_value())); + uprintf("## Motor\r\n"); + uprintf("State : %d ---- speed = %d\r\n",motor_get_state(),OCR0A); #endif } -- cgit v1.2.3