/** 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 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 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();
}
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 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();
}
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();
}
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);
}
}
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 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();
}
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);
}
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 tests_ht_color(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_color\n\n");
//nx_i2c_init();
nx_display_string("Press OK to stop\n");
ht_color_init(sensor);
if( ! ht_color_detect(sensor) ) {
nx_display_string("No color!\n");
goodbye();
return;
}
ht_color_info(sensor);
ht_color_values values;
while(nx_avr_get_button() != BUTTON_OK) {
if ( ! ht_color_read_values(sensor, &values) ) {
nx_display_string("Error reading!");
break;
}
nx_display_cursor_set_pos(0, 5);
nx_display_string("#: ");
nx_display_int(values.colornum);
nx_display_string(" ");
nx_display_cursor_set_pos(9, 5);
nx_display_string("R: ");
nx_display_int(values.redval);
nx_display_string(" ");
nx_display_cursor_set_pos(0, 6);
nx_display_string("G: ");
nx_display_int(values.greenval);
nx_display_string(" ");
nx_display_cursor_set_pos(9, 6);
nx_display_string("B: ");
nx_display_int(values.blueval);
nx_display_string(" ");
nx_display_end_line();
nx_systick_wait_ms(100);
}
ht_color_close(sensor);
goodbye();
}
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);
}
}
}
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"));
}
/** Calibrate the HT Color Sensor
*
* @param sensor The sensor port number.
*
* Note: This function is DANGEROUS!
*/
void ht_color_calibrate(void) {
/** Reference:
* http://www.mindstorms.rwth-aachen.de/subversion/branches/livecd/RWTHMindstormsNXT/CalibrateColor.m
*
* Description
* Do not use this function with the HiTechnic Color Sensor V2. It has a
* bright white flashing LED.
* This function is intended for the HiTechnic Color Sensor V1 (milky,
* weak white LED).
*
* Calibrate the color sensor with white and black reference value.
* It's not known whether calibration of the color sensor makes sense.
* HiTechnic doku says nothing, some people say it is necessary,
* but it works and has effect ;-). The sensor LEDs make a short flash
* after successful calibration. When calibrated, the sensor keeps this
* information in non-volatile memory.
* There are two different modes for calibration:
* * mode = 1: white balance calibration
* Puts the sensor into white balance calibration mode. For best results
* the sensor should be pointed at a diffuse white surface at a distance
* of approximately 15mm before calling this method. After a fraction of
* a second the sensor lights will flash and the calibration is
* done.
* * mode = 2: black level calibration
* Puts the sensor into black/ambient level calibration mode. For best
* results the sensor should be pointed in a direction with no obstacles
* for 50cm or so. This reading the sensor will use as a base level for
* other readings. After a fraction of a second the sensor lights will
* flash and the calibration is done. When calibrated, the sensor keeps
* this information in non-volatile memory.
*
* Author: Rainer Schnitzler, Linus Atorf (see AUTHORS)
* Date: 2010/09/16
* Copyright: 2007-2011, RWTH Aachen University
*/
#ifdef TEST_PORT4_I2C
U32 sensor = 3;
#else
U32 sensor = 2;
#endif
bool status;
nx_display_clear();
nx_display_cursor_set_pos(0, 0);
nx_display_string("HT Color Sensor\n");
nx_display_string("Cal on Port ");
nx_display_int(sensor);
nx_display_end_line();
nx_display_string("* For V1 Only *\n\n");
nx_display_string("White Point Cal:\n");
nx_display_string("15 mm (dif surf)\n");
nx_display_string("Press OK...\n");
while(nx_avr_get_button() != BUTTON_OK);
nx_display_clear();
nx_display_cursor_set_pos(0, 0);
ht_color_init(sensor);
if( ! ht_color_detect(sensor) ) {
nx_display_string("No color!\n");
goodbye();
return;
}
status = ht_color_perform_calibration(sensor, HT_COLOR_CAL_WHITEPOINT);
if (! status) {
nx_display_string("White Point Cal failed!\n");
nx_systick_wait_ms(1000);
while(nx_avr_get_button() != BUTTON_OK);
return;
}
nx_systick_wait_ms(1000);
nx_display_string("Black Point Cal:\n");
nx_display_string(">50 cm (no obs)\n");
nx_display_string("Press OK...\n");
while(nx_avr_get_button() != BUTTON_OK);
status = ht_color_perform_calibration(sensor, HT_COLOR_CAL_BLACKPOINT);
if (! status) {
nx_display_string("Black Point Cal failed!\n");
nx_systick_wait_ms(1000);
while(nx_avr_get_button() != BUTTON_OK);
return;
}
nx_display_string("Calibration done!\n");
nx_systick_wait_ms(1000);
ht_color_close(sensor);
return;
}
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 tests_radar(void) {
#ifdef TEST_PORT4_I2C
U32 sensor = 3;
#else
U32 sensor = 2;
#endif
U8 reading;
S8 object;
U8 objects[8];
hello();
nx_display_clear();
nx_display_cursor_set_pos(0, 0);
nx_radar_init(sensor);
nx_display_string("Discovering...\n");
while (!nx_radar_detect(sensor)) {
nx_display_string("Error! Retrying\n");
nx_systick_wait_ms(500);
nx_display_clear();
nx_display_cursor_set_pos(0, 0);
nx_display_string("Discovering...\n");
}
nx_display_string("Found.\n\n");
nx_radar_info(sensor);
while (nx_avr_get_button() != BUTTON_OK);
// We are toggling between reading all values
// and reading them one after another. Just for fun and to test
// both ways to communicate with the US.
bool read_toggle = TRUE;
while (nx_avr_get_button() != BUTTON_RIGHT) {
// We are using the single shot mode, in continuous mode
// the US doesn't seem to measure more than one byte.
nx_radar_set_op_mode(sensor, RADAR_MODE_SINGLE_SHOT);
// Give the sensor the time to receive the echos and store
// the measurements.
nx_systick_wait_ms(100);
// Go on and read and display the values.
nx_display_clear();
nx_display_cursor_set_pos(0, 0);
if( ! read_toggle ) {
memset(objects, 0, sizeof(objects));
if( ! nx_radar_read_all(sensor, objects) ) {
nx_display_string("Error reading!\n");
break;
}
}
for (object=0 ; object<8 ; object++) {
nx_display_uint(object);
nx_display_string("> ");
if( ! read_toggle )
reading = objects[object];
else
reading = nx_radar_read_distance(sensor, object);
if (reading > 0x00 && reading < 0xFF) {
nx_display_uint(reading);
nx_display_string(" cm");
} else {
nx_display_string("n/a");
}
if( ! object ) {
if( ! read_toggle )
nx_display_string(" (read8)");
else
nx_display_string(" (read1)");
}
nx_display_end_line();
}
read_toggle = ! read_toggle;
nx_systick_wait_ms(1000);
}
nx_radar_close(sensor);
goodbye();
}
