Skip to content
This repository was archived by the owner on Sep 22, 2018. It is now read-only.

Commit 24b4c16

Browse files
NeopiboxNeopibox
committed
Changes ocurred during the French national robotic cup
1 parent 88964ee commit 24b4c16

File tree

4 files changed

+60
-53
lines changed

4 files changed

+60
-53
lines changed

‎code/ViveBeacons/ViveBeacons.cydsn/Position_finder.h

+3-2
Original file line numberDiff line numberDiff line change
@@ -22,13 +22,14 @@
2222
// Attributes
2323
typedef struct Position_finder Position_finder;
2424
struct Position_finder {
25-
double led_height;
26-
double tracker_led_offset[8][2];
2725
Position3D* beacon_position;
2826
Position2D* current_position;
2927
Position2D* previous_position;
3028
VIVE_sensors_data* vive_sensors_data_v;
3129
VIVE_sensors_data* vive_sensors_data_h;
30+
double tracker_led_offset[8][2];
31+
double led_height;
32+
uint16_t computation_time;
3233
};
3334

3435
// Public methods

‎code/ViveBeacons/ViveBeacons.cydsn/configuration.h

+5-5
Original file line numberDiff line numberDiff line change
@@ -24,14 +24,14 @@
2424
#define HORIZONTAL_AXIS K_AXIS
2525
#define VERTICAL_AXIS J_AXIS
2626

27-
// Beacon coordinates
28-
#define BEACON_COORD_X 315
29-
#define BEACON_COORD_Y 30
30-
#define BEACON_COORD_Z 315
27+
/*// Beacon coordinates
28+
#define BEACON_COORD_X ((left_side == 1) ? coord_x_left : coord_x_right)
29+
#define BEACON_COORD_Y ((left_side == 1) ? coord_y_left : coord_y_right)
30+
#define BEACON_COORD_Z ((left_side == 1) ? coord_z_left : coord_z_right)
3131
3232
#define BEACON_COORD_ALPHA 0
3333
#define BEACON_COORD_BETA CY_M_PI/2
34-
#define BEACON_COORD_GAMMA 0
34+
#define BEACON_COORD_GAMMA 0*/
3535

3636
// LEDs height
3737
#define LED_COORD_HEIGHT 0

‎code/ViveBeacons/ViveBeacons.cydsn/main.c

+44-45
Original file line numberDiff line numberDiff line change
@@ -37,70 +37,69 @@ int main(void) {
3737
position_finder = Position_finder_create();
3838
uart_commands_manager = UART_commands_manager_create();
3939

40-
CyGlobalIntEnable; /* Enable global interrupts. */
41-
/*
42-
USB_Serial_Start(0, USB_Serial_5V_OPERATION);
43-
while(!USB_Serial_GetConfiguration());
44-
USB_Serial_CDC_Init();
45-
46-
USB_Serial_PutString("START\n");
47-
while(USB_Serial_CDCIsReady() == 0u);
48-
CyDelay(1);
49-
50-
CyDelay(3000);
51-
52-
USB_Serial_PutString("INIT\n");
53-
while(USB_Serial_CDCIsReady() == 0u);
54-
CyDelay(1);*/
55-
56-
// Beacon position
57-
Position3D beacon_position;
58-
beacon_position.x = BEACON_COORD_X;
59-
beacon_position.y = BEACON_COORD_Y;
60-
beacon_position.z = BEACON_COORD_Z;
61-
62-
beacon_position.alpha = BEACON_COORD_ALPHA;
63-
beacon_position.beta = BEACON_COORD_BETA;
64-
beacon_position.gamma = BEACON_COORD_GAMMA;
40+
CyGlobalIntEnable;
6541

6642
// Initialization
67-
Position_finder_init(position_finder, &beacon_position, LED_COORD_HEIGHT, tracker_led_offset);
68-
VIVE_sensors_init(vive_sensors);
6943
UART_commands_manager_init(uart_commands_manager);
44+
UART_commands_manager_register_command(uart_commands_manager, "SBPOS", callback_set_beacon_position);
45+
UART_commands_manager_register_command(uart_commands_manager, "AYR", callback_are_you_ready);
46+
UART_commands_manager_register_command(uart_commands_manager, "GPOS", callback_send_position);
47+
48+
VIVE_sensors_init(vive_sensors);
49+
while(beacon_position_initialized == false) // Wait until beacon position is initialized
50+
UART_commands_manager_check_commands(uart_commands_manager);
51+
52+
Position_finder_init(position_finder, &beacon_position, LED_COORD_HEIGHT, tracker_led_offset);
7053

71-
UART_commands_manager_register_command(uart_commands_manager, "GETPOS", callback_get_position);
54+
configured = true;
7255

7356
/* Place your initialization/startup code here (e.g. MyInst_Start()) */
74-
/*
75-
USB_Serial_PutString("READY\n");
76-
while(USB_Serial_CDCIsReady() == 0u);*/
7757

7858
for(;;) {
7959
UART_commands_manager_check_commands(uart_commands_manager);
8060

81-
if(VIVE_pulses_decoded) {
82-
VIVE_pulses_decoded = 0;
83-
vive_sensors_data = VIVE_sensors_process_pulses(vive_sensors);
84-
Position_finder_find_position(position_finder, vive_sensors_data);
85-
86-
/*
87-
char buffer[512];
88-
sprintf(buffer, "HEADING %f\n", position_finder->current_position.a);
89-
USB_Serial_PutString(buffer);
90-
while(USB_Serial_CDCIsReady() == 0u);*/
61+
if(configured) {
62+
if(VIVE_pulses_decoded) {
63+
VIVE_pulses_decoded = 0;
64+
vive_sensors_data = VIVE_sensors_process_pulses(vive_sensors);
65+
Position_finder_find_position(position_finder, vive_sensors_data);
66+
}
9167
}
9268
}
9369
}
9470

95-
void callback_get_position() {
71+
void callback_set_beacon_position() {
72+
// Beacon position
73+
beacon_position.x = atof(UART_commands_manager_get_next_token(uart_commands_manager));
74+
beacon_position.y = atof(UART_commands_manager_get_next_token(uart_commands_manager));
75+
beacon_position.z = atof(UART_commands_manager_get_next_token(uart_commands_manager));
76+
77+
beacon_position.alpha = atof(UART_commands_manager_get_next_token(uart_commands_manager));
78+
beacon_position.beta = atof(UART_commands_manager_get_next_token(uart_commands_manager));
79+
beacon_position.gamma = atof(UART_commands_manager_get_next_token(uart_commands_manager));
80+
81+
beacon_position_initialized = true;
82+
}
83+
84+
// callback functions
85+
void callback_are_you_ready() {
86+
if(configured)
87+
UART_commands_manager_send_command(uart_commands_manager, "READY", "");
88+
}
89+
90+
void callback_send_position() {
9691
// Send back the current position
92+
bool is_valid = false;
9793
char buffer[192];
9894
Position2D* position = position_finder->current_position;
95+
9996
float x = (float) position->x;
10097
float y = (float) position->y;
10198
float a = (float) position->a;
102-
sprintf(buffer, "%.6f %.6f %.6f", x, y, a);
103-
UART_commands_manager_send_command(uart_commands_manager, "POS", buffer);
99+
100+
sprintf(buffer, "%d %.6f %.6f %.6f", is_valid, x, y, a);
101+
102+
if(configured == true) // If not configured, don't send the position
103+
UART_commands_manager_send_command(uart_commands_manager, "POS", buffer);
104104
}
105-
106105
/* [] END OF FILE */

‎code/ViveBeacons/ViveBeacons.cydsn/main.h

+8-1
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,14 @@ extern VIVE_sensors* vive_sensors;
3939
extern Position_finder* position_finder;
4040

4141
// Callback functions
42-
void callback_get_position();
42+
void callback_are_you_ready();
43+
void callback_send_position();
44+
void callback_set_beacon_position();
45+
46+
// Global variables
47+
bool beacon_position_initialized = false;
48+
bool configured = false;
49+
Position3D beacon_position;
4350

4451
#endif
4552

0 commit comments

Comments
 (0)