/**
* 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);
}
}
//teste si le robot est sur un drapeau
void vForwardUntilWhite(U32 *last_left_counter, U32 *last_right_counter) {
if (nx_sensors_analog_get(LIGHT_SENSOR)<500) {
//arrete les moteurs
nx_motors_stop(RIGHT_MOTOR, TRUE);
nx_motors_stop(LEFT_MOTOR, TRUE);
//envoi message
S8 data[MSG_SIZE];
data[0]=2;
data[1]=iPositionX;
data[2]=iPositionY;
data[3]=iOrientation;
nx_bt_stream_write((U8 *)data, MSG_SIZE);
*last_left_counter=nx_motors_get_tach_count(LEFT_MOTOR);
*last_right_counter=nx_motors_get_tach_count(RIGHT_MOTOR);
vPlaySond();
//attends 3 secondes
nx_systick_wait_ms(3000);
//avance pour sortir du drapeau
vForwardStop(last_left_counter, last_right_counter,150); //changer ici en focntion de la taille du drapeau
//avance
nx_motors_rotate(LEFT_MOTOR,MEDIUM_SPEED);
nx_motors_rotate(RIGHT_MOTOR, MEDIUM_SPEED);
}
}
/**
* Teste si le robot n'est pas en contact avec un mur
*/
void vForwardUntilWall(U32 *last_left_counter, U32 *last_right_counter) {
//si on touche un obstacle
if (nx_sensors_analog_get(TOUCH_SENSOR)<500) {
//arrete les moteurs
nx_motors_stop(RIGHT_MOTOR, TRUE);
nx_motors_stop(LEFT_MOTOR, TRUE);
S8 data[MSG_SIZE];
data[0]=1;
data[1]=iPositionX;
data[2]=iPositionY;
data[3]=iOrientation;
nx_bt_stream_write((U8 *)data, MSG_SIZE);
*last_left_counter=nx_motors_get_tach_count(LEFT_MOTOR);
*last_right_counter=nx_motors_get_tach_count(RIGHT_MOTOR);
//recule
vBackStop(last_left_counter, last_right_counter,50);
//tourne à gauche
vTurnLeft(last_left_counter, last_right_counter);
//avance
nx_motors_rotate(LEFT_MOTOR,MEDIUM_SPEED);
nx_motors_rotate(RIGHT_MOTOR, MEDIUM_SPEED);
}
}
void tests_sensors(void) {
U32 i, sensor;
const U32 display_seconds = 15;
hello();
for (sensor=0; sensor<NXT_N_SENSORS; sensor++) {
nx_sensors_analog_enable(sensor);
}
for (i=0; i<(display_seconds*4); i++) {
nx_display_clear();
nx_display_cursor_set_pos(0,0);
nx_display_string("- Sensor info -\n"
"----------------\n");
for (sensor=0; sensor<NXT_N_SENSORS; sensor++){
nx_display_string("Port ");
nx_display_uint(sensor);
nx_display_string(": ");
nx_display_uint(nx_sensors_analog_get(sensor));
nx_display_end_line();
}
nx_systick_wait_ms(250);
}
for (sensor=0; sensor<NXT_N_SENSORS; sensor++) {
nx_sensors_analog_disable(sensor);
}
goodbye();
}
void sensors_light_calibrate(void) {
// Recuperation de la valeur de luminosite courante : on est sur un
// drapeau
light_threshold = nx_sensors_analog_get(SENSORS_LIGHT) - 6;
nx_display_string("New light threshold : ");
nx_display_uint(light_threshold);
nx_display_string("\n");
}
// Return TRUE if an obstacle has been encountered, FALSE otherwise.
bool sensors_contact(void) {
if (nx_sensors_analog_get(SENSORS_TOUCH) < SENSORS_TOUCH_THRESHOLD)
return TRUE;
return FALSE;
}
// Return TRUE if we are on a flag.
bool sensors_flag(void) {
if (nx_sensors_analog_get(SENSORS_LIGHT) < light_threshold)
return TRUE;
return FALSE;
}
