summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
Diffstat (limited to 'digital')
-rw-r--r--digital/beacon/src/Bitcloud_stack/lib/MakerulesBc_All_Atmega1281_Rf230_Gcc~32
-rw-r--r--digital/beacon/src/calibration.c78
-rw-r--r--digital/beacon/src/calibration.h11
-rw-r--r--digital/beacon/src/codewheel.c17
-rw-r--r--digital/beacon/src/codewheel.h3
-rw-r--r--digital/beacon/src/debug_avr.c32
-rw-r--r--digital/beacon/src/debug_avr.h1
-rw-r--r--digital/beacon/src/laser.c68
-rw-r--r--digital/beacon/src/laser.h6
-rw-r--r--digital/beacon/src/main_avr.c27
-rw-r--r--digital/beacon/src/main_simu.c2
-rw-r--r--digital/beacon/src/makefiles/Makefile_All_ZigBit_Atmega1281_Rf230_8Mhz_Gcc2
-rw-r--r--digital/beacon/src/misc.c11
-rw-r--r--digital/beacon/src/misc.h3
-rw-r--r--digital/beacon/src/motor.c31
-rw-r--r--digital/beacon/src/network.c70
-rw-r--r--digital/beacon/src/network.h6
-rw-r--r--digital/beacon/src/position.c44
-rw-r--r--digital/beacon/src/position.h33
-rw-r--r--digital/beacon/src/recovery.h2
-rw-r--r--digital/beacon/src/servo.c48
-rw-r--r--digital/beacon/src/servo.h20
-rw-r--r--digital/beacon/src/twi_specific.c96
-rw-r--r--digital/beacon/src/twi_specific.h68
24 files changed, 528 insertions, 183 deletions
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
-
diff --git a/digital/beacon/src/calibration.c b/digital/beacon/src/calibration.c
index 1392519f..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;
@@ -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 */
@@ -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/codewheel.c b/digital/beacon/src/codewheel.c
index fb8914c2..283439d6 100644
--- a/digital/beacon/src/codewheel.c
+++ b/digital/beacon/src/codewheel.c
@@ -23,10 +23,13 @@
*
* }}} */
-#include <types.h>
#include <avr/interrupt.h>
+#include <types.h>
+#include <math.h>
#include "debug_avr.h"
#include "codewheel.h"
+#include "laser.h"
+#include "network.h"
codewheel_s codewheel;
@@ -92,6 +95,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)
{
@@ -99,7 +108,11 @@ ISR(TIMER3_COMPA_vect)
{
OCR3A = codewheel_get_rebase_offset();
codewheel_set_state(CODEWHEEL_REBASED);
+ uprintf("Rebased\r\n");
}
else
- OCR3A = CODEWHEEL_CPR;
+ {
+ OCR3A = CODEWHEEL_CPR;
+ }
+ laser_reset_angle_id();
}
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
diff --git a/digital/beacon/src/debug_avr.c b/digital/beacon/src/debug_avr.c
index b195a4f2..1b0e0dfb 100644
--- a/digital/beacon/src/debug_avr.c
+++ b/digital/beacon/src/debug_avr.c
@@ -34,10 +34,10 @@
#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
-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
@@ -127,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();
@@ -138,9 +138,21 @@ void usartRXCallback(uint16_t bytesToRead)
case 'j':
jack_on_off();
break;
+ 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;
}
}
@@ -197,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
}
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
{
diff --git a/digital/beacon/src/laser.c b/digital/beacon/src/laser.c
index bd8be64b..f5fca7ff 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<<ICNC3)|(1<<ICES3);
@@ -85,14 +88,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<<OCIE3B);
@@ -119,35 +115,53 @@ void laser_set_angle(uint16_t angle)
laser.angle = angle;
}
+/* This function resets the angle id variable */
+void laser_reset_angle_id(void)
+{
+ laser.angle_id = 1;
+}
/* Zigbee sending IRQ vector */
ISR(TIMER3_COMPB_vect)
{
+ uint16_t angle_to_send;
+
+ /* For debug */
+ uprintf("angle[degree] = %f --- angle[raw] = %d\r\n",laser_get_angle_degree(),laser_get_angle_raw());
+
if(calibration_get_state() != SCANNING_STATE_CALIBRATED)
{
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)))
+ 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);
}
}
}
else
{
- // TODO: Send angle
+ angle_to_send = laser_get_angle_raw() + (laser.angle_id << 9);
+#ifdef LOL_NUMBER_2
+ angle_to_send = (CODEWHEEL_CPR/4 - laser_get_angle_raw()) + (laser.angle_id << 9);
+#endif
+ network_send_data(NETWORK_ANGLE_RAW,angle_to_send);
+ if((laser_get_angle_degree() > 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 */
@@ -159,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 */
@@ -171,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:
diff --git a/digital/beacon/src/laser.h b/digital/beacon/src/laser.h
index f50c5b07..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 */
@@ -64,6 +65,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
diff --git a/digital/beacon/src/main_avr.c b/digital/beacon/src/main_avr.c
index 4f5cc5eb..9c687e1e 100644
--- a/digital/beacon/src/main_avr.c
+++ b/digital/beacon/src/main_avr.c
@@ -26,6 +26,7 @@
#include <types.h>
#include <util/delay.h>
#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,8 @@ void APL_TaskHandler(void)
{
case DEVICE_TYPE_COORDINATOR:
network_init();
- twi_init(AC_BEACON_TWI_ADDRESS);
+ twi_init_specific();
+ position_init_struct();
uprintf("COORDINATOR initialisation OK !\n\r");
break;
case DEVICE_TYPE_END_DEVICE:
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)
{
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 $^ $@
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
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<<PB7);
+ /* Select ouptut */
+ DDRB |= (1<<PB7);
+
+ OCR0A = 0;
+
+ /* Fast PWM 10bits with TOP=0x03FF */
+ TCCR0A |= (1<<WGM01)|(1<<WGM00);
+
+ /* Prescaler = 1 */
+ TCCR0B |= (1<<CS01);
+
+ /* Postive Logic */
+ TCCR0A |= (1<<COM0A1);
+
+ /* Enable Interrupts */
+ sei();
}
/* This function starts the motor rotation */
void motor_start(void)
{
- PORTB |= (1<<PORTB7);
+ OCR0A = 115;
+
}
/* This function stops the motor rotation */
void motor_stop(void)
{
- PORTB &= ~(1<<PORTB7);
+ OCR0A = 0;
}
+/* This function returns the motor state */
TMotor_state motor_get_state(void)
{
- if(PORTB&(1<<PORTB7))
+ if(OCR0A != 0)
return MOTOR_IN_ROTATION;
else
return MOTOR_STOPPED;
@@ -54,9 +71,13 @@ TMotor_state motor_get_state(void)
/* This function starts or stops the motor according to the current state */
void motor_start_stop_control(void)
{
-
if(motor_get_state() == MOTOR_IN_ROTATION)
motor_stop();
else
motor_start();
}
+
+
+ISR(TIMER0_COMPA_vect)
+{
+} \ No newline at end of file
diff --git a/digital/beacon/src/network.c b/digital/beacon/src/network.c
index fe413314..bbcba4ec 100644
--- a/digital/beacon/src/network.c
+++ b/digital/beacon/src/network.c
@@ -30,7 +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 };
@@ -120,7 +122,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
@@ -144,7 +146,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 +165,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 +197,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;
}
@@ -242,20 +236,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);
+ }
}
@@ -269,6 +268,7 @@ void APS_DataConf(APS_DataConf_t* confInfo)
if (MAX_RETRIES_BEFORE_REJOIN == retryCounter)
{
network_leave();
+ appState = APP_NETWORK_JOIN_REQUEST;
}
else
{
@@ -284,20 +284,38 @@ 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
switch(appMessage->data[NETWORK_MSG_TYPE_FIELD])
{
case NETWORK_JACK_STATE:
+ motor_start_stop_control();
break;
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();
break;
default:
uprintf("Unknown data type received = %x\r\n",appMessage->data[NETWORK_MSG_TYPE_FIELD]);
diff --git a/digital/beacon/src/network.h b/digital/beacon/src/network.h
index 88c875f2..9fa1282c 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;
@@ -65,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);
diff --git a/digital/beacon/src/position.c b/digital/beacon/src/position.c
index 96bfb5e6..79fc4c1f 100644
--- a/digital/beacon/src/position.c
+++ b/digital/beacon/src/position.c
@@ -23,6 +23,7 @@
*
* }}} */
+#include <stdint.h>
#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;
@@ -55,18 +56,19 @@ 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;
- 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]);
@@ -152,4 +154,26 @@ int update_position(int beaconID, int angleID, double 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 8cc77f2a..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 <stdint.h>
+
+#define MAX_OBSTACLE 1
#define MAX_BEACON 3
#define MAX_TEMP_POSITION MAX_OBSTACLE * 2
#define OBSTACLE_RADIUS 100
@@ -38,21 +40,31 @@ 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 */
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 */
@@ -71,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
diff --git a/digital/beacon/src/recovery.h b/digital/beacon/src/recovery.h
index 08629e70..2f7ec3dc 100644
--- a/digital/beacon/src/recovery.h
+++ b/digital/beacon/src/recovery.h
@@ -28,7 +28,7 @@
#include "position.h"
-#define MAX_POINT_RECOVERY 500
+#define MAX_POINT_RECOVERY 100
#define RECOVERY_TRICKY_THRESHOLD MAX_POINT_RECOVERY/3
/* Status returns */
diff --git a/digital/beacon/src/servo.c b/digital/beacon/src/servo.c
index 8069f566..8aa76be0 100644
--- a/digital/beacon/src/servo.c
+++ b/digital/beacon/src/servo.c
@@ -30,6 +30,7 @@
servo_s servo1;
servo_s servo2;
+static HAL_AppTimer_t waveTimer; // TIMER descripor used by the DEBUG task
/* This function initializes low and high level modules for servos */
void servo_init(void)
@@ -244,6 +245,53 @@ 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)
+{
+
+ 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);
+
+ /* 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 <= average_value - SERVO_WAVE_OFFSET) || (current_value >= 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..7066643c 100644
--- a/digital/beacon/src/servo.h
+++ b/digital/beacon/src/servo.h
@@ -29,9 +29,18 @@
#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
+#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
@@ -95,4 +104,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
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