//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);
}
}
//Arrete les moteurs et le robot avec CANCEL
static void watchdog(void) {
S8 data[MSG_SIZE];
switch (nx_avr_get_button()) {
case BUTTON_CANCEL:
nx_motors_stop(RIGHT_MOTOR, TRUE);
nx_motors_stop(LEFT_MOTOR, TRUE);
nx_core_halt();
case BUTTON_OK:
nx_systick_wait_ms(1000);
nx_motors_rotate(LEFT_MOTOR,MEDIUM_SPEED);
nx_motors_rotate(RIGHT_MOTOR, MEDIUM_SPEED);
break;
case BUTTON_LEFT:
data[0]=98;
data[1]=98;
nx_bt_stream_write((U8 *)data, 2);
nx_systick_wait_ms(1000);
break;
case BUTTON_RIGHT:
data[0]=99;
data[1]=99;
nx_bt_stream_write((U8 *)data, 2);
nx_systick_wait_ms(1000);
break;
case BUTTON_NONE:
break;
}
}
/**
* 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);
}
}
//avance en reduisant la vitesse des moteurs au départ et à l'arrivée vers le point choisi
void vForwardStop(U32 *last_left_counter, U32 *last_right_counter, int distance) {
U32 pos=0;
U32 pos1=0;
U32 pos2=0;
float fAngle;
fAngle=360*distance/(PI*55);
U32 uAngle=(U32)fAngle;
nx_motors_rotate(LEFT_MOTOR, 70);
nx_motors_rotate(RIGHT_MOTOR, 70);
nx_systick_wait_ms(50);
nx_motors_rotate(LEFT_MOTOR, 80);
nx_motors_rotate(RIGHT_MOTOR, 80);
nx_systick_wait_ms(50);
nx_motors_rotate(LEFT_MOTOR, 90);
nx_motors_rotate(RIGHT_MOTOR, 90);
while(pos<*last_left_counter+uAngle-100) {
pos=nx_motors_get_tach_count(LEFT_MOTOR);
nx_systick_wait_ms(1);
}
nx_motors_rotate(LEFT_MOTOR, 80);
nx_motors_rotate(RIGHT_MOTOR, 80);
while(pos1<*last_left_counter+uAngle && pos2<*last_right_counter+uAngle) {
pos1=nx_motors_get_tach_count(LEFT_MOTOR);
pos2=nx_motors_get_tach_count(RIGHT_MOTOR);
nx_systick_wait_ms(1);
}
if (pos1>=*last_left_counter+uAngle) {
nx_motors_stop(LEFT_MOTOR, TRUE);
while(pos2<*last_right_counter+uAngle) {
pos2=nx_motors_get_tach_count(RIGHT_MOTOR);
nx_systick_wait_ms(1);
}
nx_motors_stop(RIGHT_MOTOR, TRUE);
}
else{
nx_motors_stop(RIGHT_MOTOR, TRUE);
while(pos1<*last_left_counter+uAngle) {
pos1=nx_motors_get_tach_count(LEFT_MOTOR);
nx_systick_wait_ms(1);
}
nx_motors_stop(LEFT_MOTOR, TRUE);
}
*last_left_counter=nx_motors_get_tach_count(LEFT_MOTOR);
*last_right_counter=nx_motors_get_tach_count(RIGHT_MOTOR);
}
static bool robot_move(U8 cmd, U8 *buffer) {
S8 data_8;
U32 data_32;
data_8 = nx_extract_type(buffer, S8);
if (data_8 > 100 || data_8 < -100)
return FALSE;
if (MSK_MOV_TURN(cmd)) {
/* Turn the robot */
data_32 = nx_extract_type(buffer, U32);
nx_motors_rotate_angle(0, data_8, data_32, MSK_BRAKE(cmd));
nx_motors_rotate_angle(2, -data_8, data_32, MSK_BRAKE(cmd));
} else {
/* Forward until new command */
nx_motors_rotate(0, data_8);
nx_motors_rotate(2, data_8);
}
return TRUE;
}
//recule tant que la position voulue n'est pas atteinte
void vBackStop(U32 *last_left_counter, U32 *last_right_counter, int distance) {
U32 pos1=*last_left_counter;
U32 pos2=*last_right_counter;
float fAngle;
fAngle=360*distance/(PI*55);
U32 uAngle=(U32)fAngle;
nx_motors_rotate(LEFT_MOTOR, -80);
nx_motors_rotate(RIGHT_MOTOR, -80);
while(pos1>*last_left_counter-uAngle && pos2>*last_right_counter-uAngle) {
pos1=nx_motors_get_tach_count(LEFT_MOTOR);
pos2=nx_motors_get_tach_count(RIGHT_MOTOR);
nx_systick_wait_ms(1);
}
if (pos1<=*last_left_counter-uAngle) {
nx_motors_stop(LEFT_MOTOR, TRUE);
while(pos2>*last_right_counter-uAngle) {
pos2=nx_motors_get_tach_count(RIGHT_MOTOR);
nx_systick_wait_ms(1);
}
nx_motors_stop(RIGHT_MOTOR, TRUE);
}
else{
nx_motors_stop(RIGHT_MOTOR, TRUE);
while(pos1>*last_left_counter-uAngle) {
pos1=nx_motors_get_tach_count(LEFT_MOTOR);
nx_systick_wait_ms(1);
}
nx_motors_stop(LEFT_MOTOR, TRUE);
}
*last_left_counter=nx_motors_get_tach_count(LEFT_MOTOR);
*last_right_counter=nx_motors_get_tach_count(RIGHT_MOTOR);
}
/**
* Cette fonction permet de faire tourner le robot vers la gauche
*/
void vTurnLeft(U32 *last_left_counter, U32 *last_right_counter) {
U32 pos1=*last_left_counter;
U32 pos2=*last_right_counter;
float fAngle;
fAngle=360*PI*WHEEL_SPACING/(4*PI*55);
U32 uAngle=(U32)fAngle;
iOrientation=(iOrientation-1)%4;
nx_motors_rotate(LEFT_MOTOR, -80);
nx_motors_rotate(RIGHT_MOTOR, 80);
while(pos1>*(last_left_counter-uAngle) && pos2<(*last_right_counter+uAngle)) {
pos1=nx_motors_get_tach_count(LEFT_MOTOR);
pos2=nx_motors_get_tach_count(RIGHT_MOTOR);
nx_systick_wait_ms(1);
}
if (pos1>=*last_left_counter-uAngle) {
nx_motors_stop(LEFT_MOTOR, TRUE);
while(pos2<*last_right_counter+uAngle) {
pos2=nx_motors_get_tach_count(RIGHT_MOTOR);
nx_systick_wait_ms(1);
}
nx_motors_stop(RIGHT_MOTOR, TRUE);
}
else{
nx_motors_stop(RIGHT_MOTOR, TRUE);
while(pos1>*last_left_counter-uAngle) {
pos1=nx_motors_get_tach_count(LEFT_MOTOR);
nx_systick_wait_ms(1);
}
nx_motors_stop(LEFT_MOTOR, TRUE);
}
*last_left_counter=nx_motors_get_tach_count(LEFT_MOTOR);
*last_right_counter=nx_motors_get_tach_count(RIGHT_MOTOR);
}
void tests_tachy(void) {
int i;
hello();
nx_motors_rotate_angle(0, 80, 1024, TRUE);
nx_motors_rotate_time(1, -80, 3000, FALSE);
nx_motors_rotate(2, 80);
for (i=0; i<30; i++) {
nx_display_clear();
nx_display_cursor_set_pos(0,0);
nx_display_clear();
nx_display_cursor_set_pos(0,0);
nx_display_string("Tachymeter test\n"
"----------------\n");
nx_display_string("Tach A: ");
nx_display_hex(nx_motors_get_tach_count(0));
nx_display_end_line();
nx_display_string("Tach B: ");
nx_display_hex(nx_motors_get_tach_count(1));
nx_display_end_line();
nx_display_string("Tach C: ");
nx_display_hex(nx_motors_get_tach_count(2));
nx_display_end_line();
nx_display_string("Refresh: ");
nx_display_uint(i);
nx_display_end_line();
nx_systick_wait_ms(250);
}
nx_motors_stop(2, TRUE);
goodbye();
}
void main(void) {
nx_systick_install_scheduler(security_hook);
bool moving = FALSE;
S32 total_rotation = 0;
S8 speed = 60;
S8 start_angle = 0;
//tests_all();
//tests_usb();
//tests_bt();
//tests_usb_hardcore();
//tests_radar();
//tests_util();
//tests_defrag();
nx_radar_init(RADAR_SENSOR);
nx_lightsensor_init(LIGHT_SENSOR);
//nx_lightsensor_fire_spamlight(LIGHT_SENSOR);
/* for (fuffa_rot = 0; (nx_motors_get_tach_count(0) % 360) < 90; fuffa_rot++) {
nx_systick_wait_ms(1000);
nx_motors_rotate_angle(0, 100, 1, TRUE);
} */
for(;;) {
nx_systick_wait_ms(500);
//nx_display_cursor_set_pos(9, 3);
nx_display_clear();
total_rotation = nx_motors_get_tach_count(RADAR_MOTOR);
display_rotation_data(speed, total_rotation, start_angle);
switch (nx_avr_get_button()) {
case BUTTON_LEFT:
/* if (speed >= -95) {
speed -= 5;
} else {
speed = -100;
}*/
nx_motors_rotate (0, -60);
nx_systick_wait_ms(100);
start_angle = nx_motors_get_tach_count(0);
nx_motors_rotate_angle(RADAR_MOTOR, -35, 45, FALSE);
/*nx_systick_wait_ms(1000);
*nx_motors_rotate_angle(0, 100, 45, TRUE);
*/
break;
case BUTTON_RIGHT:
/*if (speed <= 95) {
speed += 5;
} else {
speed = 100;
}
nx_motors_rotate (0, speed);*/
nx_motors_rotate (0, 60);
nx_systick_wait_ms(100);
start_angle = nx_motors_get_tach_count(0);
nx_motors_rotate_angle(RADAR_MOTOR, 35, 45, FALSE);
break;
case BUTTON_OK:
if (moving) {
nx_motors_stop(RADAR_MOTOR, TRUE);
//nx_motors_stop(1, TRUE);
//nx_motors_stop(2, TRUE);
moving = !moving;
} else {
nx_motors_rotate(RADAR_MOTOR, speed);
//nx_motors_rotate(1, 20);
//nx_motors_rotate(2, 20);
moving = !moving;
}
break;
default:
break;
}
};
}
void main() {
/* On bootup, all the motors are stopped. Let's start one in
* continuous mode. In this mode, the motor will continue at the
* given speed until explicitely told to stop or do something else.
*/
nx_motors_rotate(0, 100);
wait();
/* Speed control goes from -100 (full reverse) to 100 (full
* forward). Let's reverse the motor's direction.
*/
nx_motors_rotate(0, -100);
wait();
/* Now, stop the motor. There are two options here. Either don't
* apply brakes, which lets the motor continue for a short while on
* its inertia, or apply braking, which forcefully tries to bring
* the motor to a halt as fast as possible. The following will
* demonstrate both stop modes. First, a braking stop.
*/
nx_motors_stop(0, TRUE);
wait();
nx_motors_rotate(0, 100);
wait();
/* And here, a coasting stop. */
nx_motors_stop(0, FALSE);
wait();
/* You can also request rotation by a given angle, instead of just
* blazing the motor on without limits. Note that there is no
* precise feedback control built into the motor driver (yet), which
* can cause it to overshoot the target angle because of its own
* inertia. Let's rotate 90 degrees, with a braking finish.
*/
nx_motors_rotate_angle(0, 100, 90, TRUE);
wait();
/* Finally, rotation can be set to stop after a given time. The
* function call returns immediately, and the motor driver will take
* care of stopping the motor after the specified time has
* elapsed. Let's rotate in reverse, for 1 second, with a braking
* finish.
*/
nx_motors_rotate_time(0, -100, 1000, TRUE);
wait();
/* Finally, if this information has any value to you, you can query
* the motor's current rotational position relative to its position
* when it booted up. What you actually get here is the raw value of
* the motor's tachymeter, which you should modulo 360 to get a more
* sensible angular value.
*/
nx_display_uint(nx_motors_get_tach_count(0));
wait();
}
/**
* @return 0 if success ; 1 if unknown command ; 2 if halt
*/
static int tests_command(char *buffer) {
int i;
S32 t;
/* Start interpreting */
i = 0;
if (streq(buffer, "motor"))
tests_motor();
else if (streq(buffer, "sound"))
tests_sound();
else if (streq(buffer, "util"))
tests_util();
else if (streq(buffer, "display"))
tests_display();
else if (streq(buffer, "sysinfo"))
tests_sysinfo();
else if (streq(buffer, "sensors"))
tests_sensors();
else if (streq(buffer, "tachy"))
tests_tachy();
else if (streq(buffer, "radar"))
tests_radar();
else if (streq(buffer, "ht_compass"))
tests_ht_compass();
else if (streq(buffer, "ht_accel"))
tests_ht_accel();
else if (streq(buffer, "ht_gyro"))
tests_ht_gyro();
else if (streq(buffer, "ht_irlink"))
tests_ht_irlink();
else if (streq(buffer, "digitemp"))
tests_digitemp();
else if (streq(buffer, "bt"))
tests_bt();
else if (streq(buffer, "bt2"))
tests_bt2();
else if (streq(buffer, "all"))
tests_all();
else if (streq(buffer, "halt"))
return 2;
else if (streq(buffer, "Al"))
nx_motors_rotate_angle(0, 90, 100, 1);
else if (streq(buffer, "Ar"))
nx_motors_rotate_angle(0, -90, 100, 1);
else if (streq(buffer, "Ac")) {
nx_motors_rotate(0, 75);
while((t = nx_motors_get_tach_count(0)) != 0) {
if (t < 0) {
nx_motors_rotate(0, 75);
} else {
nx_motors_rotate(0, -75);
}
nx_display_cursor_set_pos(1, 1);
nx_display_hex(t);
nx_display_string(" ");
}
nx_motors_stop(0, 1);
} else if (streq(buffer, "BCf")) {
nx_motors_rotate(1, -100);
nx_motors_rotate(2, -100);
nx_systick_wait_ms(MOVE_TIME_AV);
nx_motors_stop(1, 0);
nx_motors_stop(2, 0);
} else if (streq(buffer, "BCr")) {
nx_motors_rotate(1, 80);
nx_motors_rotate(2, 80);
nx_systick_wait_ms(MOVE_TIME_AR);
nx_motors_stop(1, 0);
nx_motors_stop(2, 0);
}
else {
i = 1;
}
return i;
}