/**
* Cette fonction affiche les valeurs des capteurs
*/
void vDisplaySensorsValues() {
U8 radar;
U32 contact;
U32 light;
while(TRUE) {
nx_display_clear();
nx_display_cursor_set_pos(0, 5);
nx_display_string("Touch");
contact=nx_sensors_analog_get(TOUCH_SENSOR);
nx_display_cursor_set_pos(7, 5);
nx_display_uint(contact);
nx_display_cursor_set_pos(0, 6);
nx_display_string("Radar");
radar=nx_radar_read_distance(RADAR_SENSOR, 0);
nx_display_cursor_set_pos(7, 6);
nx_display_uint(radar);
nx_display_cursor_set_pos(0, 7);
nx_display_string("Light");
light=nx_sensors_analog_get(LIGHT_SENSOR);
nx_display_cursor_set_pos(7, 7);
nx_display_uint(light);
nx_systick_wait_ms(50);
}
}
/**
* Detecte si la valeur est inferieure à 20 et envoie un message
*/
void detect_obstacle(void)
{
if (nx_radar_read_distance(RADAR_SENSOR, 0) <= 20) {
S8 data[MSG_SIZE];
data[0]=0;
data[1]=iPositionX;
data[2]=iPositionY;
data[3]=iOrientation;
nx_bt_stream_write((U8 *)data, MSG_SIZE);
}
}
static bool get_info(U8 cmd, U8 *buffer) {
cmd = MSK_GET_INFO(cmd);
buffer=buffer;
switch (cmd) {
case GETCMD_RADAR:
if (nx_radar_detect(RADAR)) {
buffer[0] = 0;
buffer[1] = nx_radar_read_distance(RADAR, 0);
} else
buffer[0] = GETCMD_RADAR_EDETECT;
break;
default:
return FALSE;
}
return TRUE;
}
void tests_radar(void) {
#ifdef TEST_PORT4_I2C
U32 sensor = 3;
#else
U32 sensor = 2;
#endif
U8 reading;
S8 object;
U8 objects[8];
hello();
nx_display_clear();
nx_display_cursor_set_pos(0, 0);
nx_radar_init(sensor);
nx_display_string("Discovering...\n");
while (!nx_radar_detect(sensor)) {
nx_display_string("Error! Retrying\n");
nx_systick_wait_ms(500);
nx_display_clear();
nx_display_cursor_set_pos(0, 0);
nx_display_string("Discovering...\n");
}
nx_display_string("Found.\n\n");
nx_radar_info(sensor);
while (nx_avr_get_button() != BUTTON_OK);
// We are toggling between reading all values
// and reading them one after another. Just for fun and to test
// both ways to communicate with the US.
bool read_toggle = TRUE;
while (nx_avr_get_button() != BUTTON_RIGHT) {
// We are using the single shot mode, in continuous mode
// the US doesn't seem to measure more than one byte.
nx_radar_set_op_mode(sensor, RADAR_MODE_SINGLE_SHOT);
// Give the sensor the time to receive the echos and store
// the measurements.
nx_systick_wait_ms(100);
// Go on and read and display the values.
nx_display_clear();
nx_display_cursor_set_pos(0, 0);
if( ! read_toggle ) {
memset(objects, 0, sizeof(objects));
if( ! nx_radar_read_all(sensor, objects) ) {
nx_display_string("Error reading!\n");
break;
}
}
for (object=0 ; object<8 ; object++) {
nx_display_uint(object);
nx_display_string("> ");
if( ! read_toggle )
reading = objects[object];
else
reading = nx_radar_read_distance(sensor, object);
if (reading > 0x00 && reading < 0xFF) {
nx_display_uint(reading);
nx_display_string(" cm");
} else {
nx_display_string("n/a");
}
if( ! object ) {
if( ! read_toggle )
nx_display_string(" (read8)");
else
nx_display_string(" (read1)");
}
nx_display_end_line();
}
read_toggle = ! read_toggle;
nx_systick_wait_ms(1000);
}
nx_radar_close(sensor);
goodbye();
}
// Return TRUE if a wall is detected on the right of the robot.
bool sensors_wall(void) {
if (nx_radar_read_distance(SENSORS_RADAR, 0) < SENSORS_RADAR_THRESHOLD)
return TRUE;
return FALSE;
}
