/**
* 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);
}
}
void tests_ht_compass(void) {
#ifdef TEST_PORT4_I2C
U32 sensor = 3;
#else
U32 sensor = 2;
#endif
hello();
nx_display_clear();
nx_display_cursor_set_pos(0, 0);
nx_display_string("Test of compass\n\n");
//nx_i2c_init();
nx_display_string("Press OK to stop\n\n");
ht_compass_init(sensor);
if( ! ht_compass_detect(sensor) ) {
nx_display_string("No compass!\n");
goodbye();
return;
}
ht_compass_info(sensor);
while(nx_avr_get_button() != BUTTON_OK) {
nx_display_cursor_set_pos(9, 6);
nx_display_string(" ");
nx_display_cursor_set_pos(9, 6);
nx_display_uint( ht_compass_read_heading(sensor) );
nx_systick_wait_ms(100);
}
ht_compass_close(sensor);
goodbye();
}
void main(void) {
char *entries[] = {"Browse", "Sysinfo", "Settings", "Format", "Item5", "Item6", "Item7", "Halt", NULL};
gui_text_menu_t menu;
U8 res;
menu.entries = entries;
menu.title = "Home menu";
menu.active_mark = "> ";
while (TRUE) {
res = nx_gui_text_menu(menu);
nx_display_end_line();
switch (res) {
case 7:
nx_display_clear();
nx_display_string(">> Halting...");
nx_systick_wait_ms(1000);
return;
break;
default:
nx_display_clear();
nx_display_string("You pressed:\n");
nx_display_string(entries[res]);
nx_display_end_line();
nx_display_string("\nOk to go back");
while (nx_avr_get_button() != BUTTON_OK);
break;
}
}
}
void nx_assert_error(const char *file, const int line,
const char *expr, const char *msg) {
const char *basename = strrchr(file, '/');
basename = basename ? basename+1 : file;
/* Try to halt as many moving parts of the system as possible. */
nx_systick_install_scheduler(NULL);
nx__avr_set_motor(0, 0, TRUE);
nx__avr_set_motor(1, 0, TRUE);
nx__avr_set_motor(2, 0, TRUE);
nx_display_clear();
nx_sound_freq_async(440, 1000);
nx_display_string("** Assertion **\n");
nx_display_string(basename);
nx_display_string(":");
nx_display_uint(line);
nx_display_end_line();
nx_display_string(expr);
nx_display_end_line();
nx_display_string(msg);
nx_display_end_line();
while (nx_avr_get_button() != BUTTON_CANCEL);
nx_core_halt();
}
/**
* Initialisation des differents capteurs et du bluetooth
*/
void init(void)
{
nx_display_clear();
nx_display_cursor_set_pos(0, 0);
nx_display_string("Lo52 project\n");
nx_display_cursor_set_pos(0, 2);
//on affiche le niveau de la batterie
nx_display_uint(nx_avr_get_battery_voltage());
nx_display_cursor_set_pos(4, 2);
nx_display_string("/4000\n");
nx_systick_install_scheduler(watchdog);
// initialise le radar ultrason
nx_radar_init(RADAR_SENSOR);
//initialise le bluetooth
bt_init();
nx_display_string("bluetooth...OK");
//initialise le son
nx__sound_init();
// initialise le capteur de contact
nx_sensors_analog_enable(TOUCH_SENSOR);
//initialise le capteur de luminosité
nx_sensors_analog_enable(LIGHT_SENSOR);
nx_sensors_analog_digi_set(LIGHT_SENSOR, DIGI0);
}
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");
}
void tests_bt2(void) {
/*int i;
*/
hello();
/* Configuring the BT */
nx_bt_init();
nx_display_clear();
nx_display_string("Setting friendly name ...");
nx_display_end_line();
nx_bt_set_friendly_name("tulipe");
nx_display_string("Setting as discoverable ...");
nx_display_end_line();
nx_bt_set_discoverable(TRUE);
/* Listing known devices */
tests_bt_list_known_devices();
/* Scanning & adding */
tests_bt_scan_and_add();
/* Listing known devices */
tests_bt_list_known_devices();
/* Scanning & removing */
tests_bt_scan_and_remove();
/* Listing known devices */
tests_bt_list_known_devices();
/*
for (i = 0 ; i < 10 ; i++)
{
nx_display_clear();
nx_bt_debug();
nx_systick_wait_ms(1000);
}
*/
goodbye();
}
/** Display connected radar's information. */
void nx_radar_info(U32 sensor) {
U8 buf[9];
// Product ID (LEGO)
memset(buf, 0, sizeof(buf));
nx_radar_read(sensor, RADAR_PRODUCT_ID, buf);
nx_display_string((char *)buf);
nx_display_string(" ");
// Sensor Type (Sonar)
memset(buf, 0, sizeof(buf));
nx_radar_read(sensor, RADAR_SENSOR_TYPE, buf);
nx_display_string((char *)buf);
nx_display_string(" ");
// Version (V1.0)
memset(buf, 0, sizeof(buf));
nx_radar_read(sensor, RADAR_VERSION, buf);
nx_display_string((char *)buf);
nx_display_end_line();
// Measurement units
nx_display_string("Units: ");
memset(buf, 0, sizeof(buf));
nx_radar_read(sensor, RADAR_MEASUREMENT_UNITS, buf);
nx_display_string((char *)buf);
nx_display_end_line();
// Measurement interval
nx_display_string("Interval: ");
nx_display_uint(nx_radar_read_value(sensor, RADAR_INTERVAL));
nx_display_string(" ms?\n");
}
static void send_callback(void) {
static U8 flag = 1;
nx_display_clear();
nx_display_cursor_set_pos(0,6);
nx_display_string(flag ? "T\n" : " \n");
out_buffer[5] = (flag ? 0 : ' ');
nx_display_string((const char *)out_buffer);
flag = (flag+1) % 2;
lock = 0;
}
void tests_sysinfo(void) {
U32 i;
U32 t = 0;
const U32 display_seconds = 15;
U8 avr_major, avr_minor;
hello();
nx_avr_get_version(&avr_major, &avr_minor);
for (i=0; i<(display_seconds*4); i++) {
if (i % 4 == 0)
t = nx_systick_get_ms();
nx_display_clear();
nx_display_cursor_set_pos(0,0);
nx_display_string("- System info -\n"
"----------------\n");
nx_display_string("Time : ");
nx_display_uint(t);
nx_display_end_line();
nx_display_string("Boot from ");
if (NX_BOOT_FROM_SAMBA)
nx_display_string("SAM-BA");
else if (NX_BOOT_FROM_ENH_FW)
nx_display_string("ENH-FW");
else
nx_display_string("ROM");
nx_display_end_line();
nx_display_string("Free RAM: ");
nx_display_uint(NX_USERSPACE_SIZE);
nx_display_end_line();
nx_display_string("Buttons: ");
nx_display_uint(nx_avr_get_button());
nx_display_end_line();
nx_display_string("Battery: ");
nx_display_uint(nx_avr_get_battery_voltage());
nx_display_string(" mV");
nx_display_end_line();
nx_systick_wait_ms(250);
}
goodbye();
}
void tests_sound(void) {
enum {
end = 0, sleep500 = 1, si = 990, dod = 1122,
re = 1188, mi = 1320, fad = 1496, sol = 1584,
} pain[] = {
si, sleep500,
fad, si, sol, sleep500,
fad, mi, fad, sleep500,
mi, fad, sol, sol, fad, mi, si, sleep500,
fad, si, sol, sleep500,
fad, mi, re, sleep500,
mi, re, dod, dod, re, dod, si, end
};
int i = 0;
hello();
nx_display_clear();
nx_display_cursor_set_pos(0,0);
nx_display_string("-- Sound test --\n"
"----------------\n");
while (pain[i] != end) {
if (pain[i] == sleep500)
nx_systick_wait_ms(150);
else
nx_sound_freq(pain[i], 150);
nx_systick_wait_ms(150);
i++;
}
nx_systick_wait_ms(1000);
goodbye();
}
void nx__abort(bool data, U32 pc, U32 cpsr) {
nx_interrupts_disable();
nx_display_auto_refresh(FALSE);
//nx_display_clear();
if (data) {
nx_display_string("Data");
} else {
nx_display_string("Prefetch");
}
nx_display_string(" abort\nPC: ");
nx_display_hex(pc);
nx_display_string("\nCPSR: ");
nx_display_hex(cpsr);
nx__lcd_sync_refresh();
while(1);
}
void nx_display_hex(U32 val) {
const char hex[16] = "0123456789ABCDEF";
char buf[9];
char *ptr = &buf[8];
if (val == 0) {
ptr--;
*ptr = hex[0];
} else {
while (val != 0) {
ptr--;
*ptr = hex[val & 0xF];
val >>= 4;
}
while (ptr != buf) {
ptr--;
*ptr = hex[0];
}
}
buf[8] = '\0';
nx_display_string(ptr);
dirty_display();
}
void nx_display_int(S32 val) {
if( val < 0 ) {
nx_display_string("-");
val = -val;
}
nx_display_uint(val);
}
void handle_cmd(void)
{
S8 speed[MSG_SIZE];
//reception du message
nx_bt_stream_read((U8 *)speed, MSG_SIZE);
while (!quit && nx_bt_stream_data_read() < 1) {
detect_obstacle();
vForwardUntilWall(&pos1Global,&pos2Global);
vForwardUntilWhite(&pos1Global,&pos2Global);
nx_systick_wait_ms(10);
nx_display_cursor_set_pos(0, 5);
nx_display_string("boucle");
}
if (speed[0] > 100 || speed[0] < -100
|| speed[1] > 100 || speed[1] < -100)
speed[0] = speed[1] = 0;
nx_motors_rotate_time(LEFT_MOTOR, speed[0], MOVE_TIME, FALSE);
nx_motors_rotate_time(RIGHT_MOTOR, speed[1], MOVE_TIME, FALSE);
return;
}
void tests_ht_gyro(void) {
U32 sensor = 2;
hello();
nx_display_clear();
nx_display_cursor_set_pos(0, 0);
nx_display_string(" Test of ht_gyro\n\n");
nx_display_string("Press OK calc 0\n");
nx_display_string("The Gyro must\nstand still\n");
while(nx_avr_get_button() != BUTTON_OK)
nx_systick_wait_ms(10);
nx_display_string("Calculating 0\n");
ht_gyro_init(sensor);
U32 zero = ht_gyro_calculate_average_zero(sensor);
nx_display_string("Zero: ");
nx_display_uint(zero);
nx_display_end_line();
nx_systick_wait_ms(1000); /* Give the user time to release the OK-button */
U32 rotation;
while(nx_avr_get_button() != BUTTON_OK) {
nx_display_cursor_set_pos(0, 7);
nx_display_string(" ");
rotation = ht_gyro_get_value(sensor);
nx_display_cursor_set_pos(0, 7);
nx_display_uint( rotation );
nx_display_string(" ");
nx_display_int( (S32)rotation - zero );
nx_systick_wait_ms(100);
}
ht_gyro_close(sensor);
goodbye();
}
/**
* 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();
}
void main(void)
{
init();
nx_systick_wait_ms(5000);
/*
vDisplaySensorsValues();
U32 pos1=0;
U32 pos2=0;
vForwardStop(&pos1,&pos2,500);
nx_systick_wait_ms(1000);
vTurnRight(&pos1,&pos2);
nx_systick_wait_ms(1000);
vForwardStop(&pos1,&pos2,250);
nx_systick_wait_ms(1000);
vTurnLeft(&pos1,&pos2);
nx_systick_wait_ms(1000);
vForwardStop(&pos1,&pos2,250);
nx_systick_wait_ms(1000);
vTurnRight(&pos1,&pos2);
nx_systick_wait_ms(1000);
vForwardStop(&pos1,&pos2,250);
nx_systick_wait_ms(1000);
vTurnRight(&pos1,&pos2);
nx_systick_wait_ms(1000);
vForwardStop(&pos1,&pos2,500);
nx_systick_wait_ms(1000);
vTurnRight(&pos1,&pos2);
nx_systick_wait_ms(1000);
vForwardStop(&pos1,&pos2,250);
nx_systick_wait_ms(1000);
vTurnLeft(&pos1,&pos2);
nx_systick_wait_ms(1000);
vForwardStop(&pos1,&pos2,250);
*/
nx_display_cursor_set_pos(0, 5);
nx_display_string("en attente");
while (!quit) {
if (!nx_bt_stream_opened() || nx_bt_connection_pending())
bt_wait_connection();
else
{
nx_sound_freq(DO, 200);
nx_sound_freq(DO, 200);
nx_sound_freq(DO, 200);
handle_cmd();
}
}
die();
}
void tests_display(void) {
char buf[2] = { 0, 0 };
int i;
hello();
nx_display_clear();
nx_display_cursor_set_pos(0, 0);
nx_display_string("- Display test -\n"
"----------------\n");
for (i=32; i<128; i++) {
buf[0] = i;
nx_display_string(buf);
if ((i % 16) == 15)
nx_display_string("\n");
}
nx_systick_wait_ms(5000);
goodbye();
}
static void tests_bt_list_known_devices(void) {
bt_device_t dev;
/* Listing known devices */
nx_display_clear();
nx_display_string("Known devices: ");
nx_display_end_line();
nx_bt_begin_known_devices_dumping();
while(nx_bt_get_state() == BT_STATE_KNOWN_DEVICES_DUMPING) {
if (nx_bt_has_known_device()) {
nx_bt_get_known_device(&dev);
nx_display_string("# ");
nx_display_string(dev.name);
nx_display_end_line();
}
}
nx_systick_wait_ms(2000);
}
static void display_rotation_data (S8 vel, S32 tot, S8 sangle) {
nx_display_clear();
/* Display Light Intensity as read by the Light Sensor */
nx_display_string("Luminosità: ");
nx_display_uint(nx_lightsensor_get_raw(LIGHT_SENSOR));
nx_display_end_line();
/* Display Radar Motor's Speed */
nx_display_string("Velocità: ");
if (vel >= 0) {
nx_display_uint(vel);
} else {
nx_display_string("-");
nx_display_uint(vel);
};
nx_display_end_line();
/* Display actual angle of tha Radar's Motor */
nx_display_string("Start Angle: ");
nx_display_int(sangle);
nx_display_end_line();
nx_display_string("Angolo: ");
if (tot >= 0) {
nx_display_uint(tot % 360);
} else {
nx_display_string("-");
nx_display_uint((tot*-1) % 360);
}
nx_display_end_line();
}
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 usb_recv(void) {
U8 buf[RCMD_BUF_LEN];
nx_display_clear();
nx_display_string("<< ");
do {
memset(buf, 0, RCMD_BUF_LEN);
usb_readline(buf);
if (!*buf) {
continue;
}
// Disable local echo
// nx_display_string((char *)buf);
nx_display_end_line();
nx_rcmd_do((char *)buf);
nx_systick_wait_ms(50);
nx_display_string("<< ");
} while (!streq((char *)buf, "end"));
}
static void test_display(void) {
U32 counter = 0;
nx_display_clear();
while(1) {
counter++;
nx_display_cursor_set_pos(0,0);
nx_display_hex(counter);
nx_display_end_line();
nx_display_hex(sleep_iter);
nx_display_end_line();
nx_display_uint(sleep_time);
nx_display_string(" / ");
nx_display_uint(wakeup_time);
}
}
static void tests_bt_scan_and_remove(void) {
bt_device_t dev;
nx_display_clear();
nx_display_string("Scanning and removing ...");
nx_display_end_line();
nx_bt_begin_inquiry(/* max dev : */ 255,
/* timeout : */ 0x20,
/* class : */ (U8[]){ 0, 0, 0, 0 });
while(nx_bt_get_state() == BT_STATE_INQUIRING) {
if (nx_bt_has_found_device()) {
nx_bt_get_discovered_device(&dev);
nx_display_string("# ");
nx_display_string(dev.name);
nx_display_end_line();
nx_bt_remove_known_device(dev.addr);
}
}
}
/* This function is called by the RS485 driver when a requested write
* completes.
*/
static void send_callback(nx_rs485_error_t err) {
if (err == RS485_SUCCESS) {
/* The write completed successfully, release the spinlock and
* allow the main code to continue.
*/
nx_spinlock_release(&lock);
} else {
/* There was some error. You can switch on the exact value and
* report specific errors, but for this demo we'll just display an
* error and fail.
*/
nx_display_clear();
nx_display_string("TX error!");
}
}
bool ht_color_perform_calibration(U32 sensor, U8 mode) {
U8 str[1] = { mode };
switch (mode) {
case HT_COLOR_CAL_WHITEPOINT:
case HT_COLOR_CAL_BLACKPOINT:
return (nx_i2c_memory_write(sensor, HT_COLOR_COMMAND, str, 1) == I2C_ERR_OK);
break;
default:
nx_display_string("Unknown Cal. Command!\n");
nx_systick_wait_ms(1000);
return FALSE;
break;
}
}
void tests_ht_accel(void) {
#ifdef TEST_PORT4_I2C
U32 sensor = 3;
#else
U32 sensor = 2;
#endif
hello();
nx_display_clear();
nx_display_cursor_set_pos(0, 0);
nx_display_string("Test of ht_accel\n\n");
//nx_i2c_init();
nx_display_string("Press OK to stop\n");
ht_accel_init(sensor);
if( ! ht_accel_detect(sensor) ) {
nx_display_string("No accel!\n");
goodbye();
return;
}
ht_accel_info(sensor);
ht_accel_values values;
while(nx_avr_get_button() != BUTTON_OK) {
nx_display_cursor_set_pos(3, 5);
if ( ! ht_accel_read_values(sensor, &values) ) {
nx_display_string("Error reading!");
break;
}
nx_display_string(" ");
nx_display_cursor_set_pos(3, 5);
nx_display_int( values.x );
nx_display_cursor_set_pos(3, 6);
nx_display_string(" ");
nx_display_cursor_set_pos(3, 6);
nx_display_int( values.y );
nx_display_cursor_set_pos(3, 7);
nx_display_string(" ");
nx_display_cursor_set_pos(3, 7);
nx_display_int( values.z );
nx_systick_wait_ms(100);
}
ht_accel_close(sensor);
goodbye();
}
void main(void) {
char *entries[] = {"Replay", "Record", "From USB", "Halt", NULL};
gui_text_menu_t menu;
U8 res;
/*
U32 nulldata[EFC_PAGE_WORDS] = {0};
for (res=128; res<140; res++)
nx__efc_write_page(nulldata, res);
*/
menu.entries = entries;
menu.title = "Tag route";
menu.active_mark = GUI_DEFAULT_TEXT_MARK;
while (TRUE) {
res = nx_gui_text_menu(menu);
switch (res) {
case 0:
replay(ROUTE_FILE);
break;
case 1:
record(ROUTE_FILE);
break;
case 2:
usb_recv();
break;
case 3:
return;
break;
default:
continue;
break;
}
nx_display_string("\nOk to go back");
while (nx_avr_get_button() != BUTTON_OK);
nx_systick_wait_ms(500);
}
}
