//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 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);
}
}
/**
* Arret du robot
*/
void die(void)
{
nx_display_string("dying...\n");
nx_motors_stop(LEFT_MOTOR, TRUE);
nx_motors_stop(RIGHT_MOTOR, TRUE);
nx_radar_close(RADAR_SENSOR);
bt_die();
nx_systick_wait_ms(2000);
nx_core_halt();
}
static bool wheel_stop(U8 cmd, U8 *buffer) {
buffer = buffer; /* Shutting down compiler :) */
if (MSK_SELECT_MOTOR_0(cmd))
nx_motors_stop(0, MSK_BRAKE(cmd));
if (MSK_SELECT_MOTOR_1(cmd))
nx_motors_stop(1, MSK_BRAKE(cmd));
if (MSK_SELECT_MOTOR_2(cmd))
nx_motors_stop(2, MSK_BRAKE(cmd));
return TRUE;
}
//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);
}
//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) {
UWORD tbc_length = 0;
BYTE *tbc_data = NULL;
tbc_t *tbc = NULL;
nx_systick_wait_ms (1000);
nx__avr_init ();
nx_systick_wait_ms (1000);
tvm_init (&tvm);
for (;;) {
U8 buffer[NX_USB_PACKET_SIZE];
WORD usb = 0;
U32 pos = 0;
U32 i;
int running = 1;
nx_display_clear ();
nx_display_string ("I am the TVM.");
nx_display_end_line ();
if (tbc != NULL) {
nx_display_string ("OK to reload.");
nx_display_end_line ();
}
nx_usb_read (buffer, NX_USB_PACKET_SIZE);
while (usb == 0) {
if ((pos = nx_usb_data_read ()) >= 8) {
if ((tbc_length = valid_tbc_header (buffer))) {
tbc_length += 8;
tbc_data = (BYTE *) mem_pool;
tbc = NULL;
for (i = 0; i < pos; ++i) {
tbc_data[i] = buffer[i];
}
usb = tbc_length - pos;
}
} else {
switch (nx_avr_get_button ()) {
case BUTTON_OK:
if (tbc != NULL) {
usb = -1;
}
break;
case BUTTON_CANCEL:
nx__avr_power_down ();
break;
default:
nx_systick_wait_ms (100);
break;
}
}
}
if (usb > 0) {
nx_display_cursor_set_pos (0, 1);
nx_display_string ("Got header (");
nx_display_uint (tbc_length);
nx_display_string (")");
nx_display_end_line ();
nx_display_cursor_set_pos (0, 2);
nx_display_uint (usb);
nx_display_string (" ");
} else {
nx_display_string ("Reload TBC (");
nx_display_uint (tbc_length);
nx_display_string (")");
nx_display_end_line ();
}
while (usb > 0) {
U32 tmp;
if (usb >= NX_USB_PACKET_SIZE) {
nx_usb_read (&(tbc_data[pos]), NX_USB_PACKET_SIZE);
} else {
nx_usb_read (&(tbc_data[pos]), usb);
}
while (!(tmp = nx_usb_data_read ()))
continue;
pos += tmp;
usb -= tmp;
nx_display_cursor_set_pos (0, 2);
nx_display_uint (usb);
nx_display_string (" ");
}
nx_display_cursor_set_pos (0, 2);
nx_display_string ("Got TBC. ");
nx_display_end_line ();
nx_systick_wait_ms (200);
tvm_ectx_init (&tvm, &context);
context.mem_pool = mem_pool + tbc_length;
context.get_time = nxt_get_time;
context.modify_sync_flags = nxt_modify_sync_flags;
context.sffi_table = sffi_table;
context.sffi_table_length = sffi_table_length;
tlsf_init_memory_pool (NX_USERSPACE_SIZE - tbc_length,
(void *) context.mem_pool);
if ((tbc = load_context_with_tbc (&context, tbc, tbc_data, tbc_length)) == NULL) {
nx_display_string ("Decode failed!");
nx_systick_wait_ms (3000);
continue;
}
nx_display_string ("Running...");
nx_display_end_line ();
nx_systick_wait_ms (1000);
nx_display_clear ();
while (running) {
int ret = tvm_run (&context);
switch (ret) {
case ECTX_PREEMPT:
case ECTX_TIME_SLICE: {
/* Safe to continue. */
break;
}
case ECTX_SLEEP: {
WORD next = context.tnext;
WORD now = nxt_get_time (&context);
while (TIME_AFTER (next, now)) {
nx_systick_wait_ms (next - now);
now = nxt_get_time (&context);
}
break;
}
case ECTX_INTERRUPT: {
//clear_pending_interrupts ();
break;
}
case ECTX_EMPTY: {
//if (!waiting_on_interrupts ()) {
//terminate("deadlock", NULL);
//}
nx_display_end_line ();
nx_display_string ("Deadlock.");
break;
}
case ECTX_SHUTDOWN: {
nx_display_end_line ();
nx_display_string ("End of program.");
running = 0;
break;
}
default: {
nx_display_end_line ();
nx_display_string ("Error = ");
nx_display_uint (ret);
running = 0;
break;
}
}
}
for (i = 0; i < 3; ++i)
nx_motors_stop (i, FALSE);
for (i = 0; i < 4; ++i)
nx__sensors_disable (i);
nx_systick_wait_ms (3000);
}
/* NOTREACHED */
}
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;
}
