//est moins precise que vForwardStop
void vForward(U32 distance) {
float angle;
angle=360*distance/(PI*55);
nx_motors_rotate_angle(LEFT_MOTOR, HIGH_SPEED, (U32)angle, TRUE);
nx_motors_rotate_angle(RIGHT_MOTOR, HIGH_SPEED, (U32)angle, TRUE);
nx_systick_wait_ms(500+distance/200*1000);
}
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;
}
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;
}
