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();
}
/**
* 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_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();
}
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();
}
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_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 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 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();
}
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;
}
/**
* 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_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_digitemp(void) {
U32 sensor = 2;
S16 temperature;
hello();
nx_display_clear();
nx_display_cursor_set_pos(0, 0);
nx_display_string(" Test of d-temp\n\n");
//nx_i2c_init();
nx_display_string("Press OK to stop\n\n");
digitemp_init(sensor);
if( ! digitemp_detect(sensor) ) {
nx_display_string("No temp-sensor!\n");
goodbye();
return;
}
digitemp_info(sensor);
while(nx_avr_get_button() != BUTTON_OK) {
temperature = digitemp_get_temperature(sensor);
nx_display_cursor_set_pos(6, 5);
nx_display_string(" ");
nx_display_cursor_set_pos(6, 5);
if(temperature < 0 ) {
nx_display_string("-");
temperature = -temperature;
}
nx_display_uint(temperature/2);
if( temperature & 1 )
nx_display_string(".5");
nx_display_string(" C");
nx_systick_wait_ms(100);
}
digitemp_close(sensor);
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_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 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();
}
void tests_motor(void) {
hello();
nx_display_clear();
nx_display_cursor_set_pos(0,0);
nx_display_string("--- AVR test ---\n"
"----------------\n");
nx__avr_set_motor(0, 80, 0);
nx_systick_wait_ms(1000);
nx__avr_set_motor(0, -80, 0);
nx_systick_wait_ms(1000);
nx__avr_set_motor(0, 80, 0);
nx_systick_wait_ms(1000);
nx__avr_set_motor(0, 0, 1);
nx_systick_wait_ms(200);
goodbye();
}
void tests_gdbdebug(void) {
U32 count = 0;
hello();
while(1) {
nx_display_cursor_set_pos(0, 0);
nx_display_string("Waiting for\nDebugger ...");
nx_display_uint(count);
dbg_breakpoint_arm(); /* Trigger a manual ARM Breakpoint */
dbg__test_arm_instrstep();
dbg__test_thumb_instrstep();
nx_systick_wait_ms(1000);
count++;
if (count >= 5)
break;
}
goodbye();
}
/* Clear the display. */
void nx_display_clear(void) {
memset(&display.buffer[0][0], 0, sizeof(display.buffer));
nx_display_cursor_set_pos(0, 0);
dirty_display();
}
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() {
/* We want to draw a moving ellipse, two arcs and two lines */
/* Declaring center point of the ellipse / arcs and points representing the lines */
point ellipse_c, q, r, s, t, u, v, w;
/* Initialize ellipse center position */
ellipse_c.x = 1;
ellipse_c.y = 1;
/* Initialize the first line position*/
q.x = 3;
q.y = 63;
s.x = 28;
s.y = 63;
/* Initialize the second line position */
r.x = 102;
r.y = 63;
t.x = 76;
t.y = 63;
/* initialize the triangle position */
u.x = 48;
u.y = 33;
v.x = 48;
v.y = 45;
w.x = 61;
w.y = 39;
/* Superior radius of the ellipse */
U8 sradius = 85;
/* Difference between the superior and inferior radius of the ellipse */
U8 delta = 73;
/* Offset angle for arcs rotation */
U32 offset_angle = 60;
/* Disable auto refresh of the screen, we want to control each frame */
nx_display_auto_refresh(FALSE);
/* Loop until the cancel button is pushed
Each loop iteration represent a frame composed of three steps*/
while (nx_avr_get_button() != BUTTON_CANCEL) {
/* Clear the screen */
nx_display_clear();
/* First step of the frame : calculate positions and parameters of shapes */
/* If the ellipse is in the first half of the screen,
make it follow a pseudo sinusoidal path */
if(ellipse_c.x < 53) {
ellipse_c.x ++;
delta--;
ellipse_c.y = (30 * sinf( ((float) ellipse_c.x) / 25) + 4);
}
/* If the ellipse have past the first third of the screen,
then start to move lines and rotate arcs */
if(ellipse_c.x > 34) {
if(ellipse_c.x < 49) {
q.x = (q.x) + 2;
s.x = (s.x) + 2;
r.x = (r.x) - 2;
t.x = (t.x) - 2;
offset_angle += ellipse_c.x;
}
else if(offset_angle > 5)
offset_angle -= 5;
}
/* Second step of the frame : draw shapes on the screen buffer*/
/* If the ellipse have past the first third of the screen,
draw moving arcs around and lines at the bottom of the screen */
if(ellipse_c.x > (34)) {
nx_display_arc(ellipse_c, 27, 45, offset_angle);
nx_display_arc(ellipse_c, 27, 45, (180 + offset_angle));
nx_display_line(q, s);
nx_display_line(r, t);
}
/* If the ellipse is at the middle of the screen,
write "NxOS on the screen */
if(ellipse_c.x > 52) {
nx_display_cursor_set_pos(7, 3);
nx_display_string("NxOS");
}
/* Just draw the ellipse, every frame */
nx_display_ellipse(ellipse_c, (sradius / 4), ((sradius - delta) / 3), (ellipse_c.x * 20) );
nx_display_triangle(u, v, w);
/* Last step of the frame : refresh the screen */
nx_display_refresh();
/* Wait a bit before next frame */
nx_systick_wait_ms(85);
}
/* Shutdown the brick */
nx_core_halt();
}
void tests_legocolor(void) {
#ifdef TEST_PORT4_I2C
U32 sensor = 3;
#else
U32 sensor = 2;
#endif
color_cal_data calibration_data;
color_mode mode;
color_values rawColor;
color_detected theColor;
hello();
/* Test all modes */
// for (mode = COLOR_MODE_NONE; mode < COLOR_NUM_MODES; mode++) {
for (mode = COLOR_MODE_FULL; mode < COLOR_NUM_MODES; mode++) {
nx_display_clear();
nx_display_cursor_set_pos(0, 0);
nx_display_string("Discovering...\n");
nx_color_init(sensor, mode, &calibration_data);
while (COLOR_NOTFOUND == nx_color_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");
}
#if 1
nx_color_info(sensor);
while (nx_avr_get_button() != BUTTON_OK);
#else
while (COLOR_READY != nx_color_detect(sensor)) {
nx_color_info(sensor);
nx_systick_wait_ms(1000);
nx_display_cursor_set_pos(0, 1);
}
nx_color_info(sensor);
while (nx_avr_get_button() != BUTTON_OK);
#endif
while (nx_avr_get_button() != BUTTON_RIGHT) {
// Go on and read and display the values.
nx_color_read_all_raw(sensor, &rawColor);
theColor = nx_color_classifier(&rawColor, &calibration_data);
nx_display_cursor_set_pos(0, 6);
nx_display_string("Color: ");
nx_display_string(nx_color2str(theColor));
nx_systick_wait_ms(1000);
}
nx_color_close(sensor);
}
goodbye();
}
void tests_ht_irlink(void) {
U32 sensor = 2;
U8 buffer[15];
hello();
nx_display_clear();
nx_display_cursor_set_pos(0, 0);
nx_display_string("Test of IRLink\n");
//nx_i2c_init();
nx_display_string("Press OK to stop\n");
ht_irlink_init(sensor);
if( ! ht_irlink_detect(sensor) ) {
nx_display_string("No IRLink!\n");
nx_systick_wait_ms(10000);
goodbye();
return;
}
ht_irlink_info(sensor);
/* This test is build for use with the Motorized Bulldozer (8275).
* You need about 1m x 0.5m space where the Bulldozer will drive.
* Because we don't have motor-feedback or encoders, all times are
* approximately and depending heavily on the battery-level of the PF
* and the ground over which the bulldozer drives.
*
* It should'nt be any problem to use this test with other models or
* just plain connected motors to see if all works.
*
* Maybe I've made a mistake building the bulldozer, but my model
* requires to backward drive motor A on channel 0 to drive the left side
* forward, it might be that you have to change that.
*
* The sequence of commands is the following:
*
* (1) Backward drive motor A (red, blade) and forward drive
* motor B (blue, ripper) on channel 1 with full power for 7s to
* be sure the front blade and the ripper are up.
* (2) Forward drive motor A (left) and backward drive motor B (right)
* on channel 0 with full power for 5s to drive the bulldozer
* about 1m forwards.
* (3) Forward drive motor A on channel 1 for 5s then backward drive
* him for 5s to move the front blade down an up.
* (4) Forward drive both motors on channel 0 with full power for 3s to turn
* the bulldozer to the right about 180 degrees,
* (5) Forward drive both motors on channel 0 pwm-controlled, raising the
* power from 3 to 6 and backwards to 0 to drive the bulldozer
* about 1m forwards.
* (6) Backward drive both motors on channel 0 pwm-controlled with power 7
* to turn the bulldoze to the left about 180 degrees.
* (7) Backward drive motor B on channel 1 for 5s then forward drive
* him for 5s to move the ripper down an up.
*/
U8 count;
/* (1) */
count = ht_irlink_encode_bitstream(HT_IRLINK_MODE_PF,
build_bitstream_PF_direct(1, PF_MOTOR_A_BACKWARD | PF_MOTOR_B_FORWARD),
buffer);
for ( unsigned i = 0; i < 7; ++i ) {
ht_irlink_transmit_buffer_4x(sensor, buffer, count);
nx_systick_wait_ms(1000);
}
/* (2) */
count = ht_irlink_encode_bitstream(HT_IRLINK_MODE_PF,
build_bitstream_PF_direct(0, PF_MOTOR_A_FORWARD | PF_MOTOR_B_BACKWARD),
buffer);
for ( unsigned i = 0; i < 5; ++i ) {
ht_irlink_transmit_buffer_4x(sensor, buffer, count);
nx_systick_wait_ms(1000);
}
/* (3) */
count = ht_irlink_encode_bitstream(HT_IRLINK_MODE_PF,
build_bitstream_PF_direct(1, PF_MOTOR_A_FORWARD ),
buffer);
for ( unsigned i = 0; i < 5; ++i ) {
ht_irlink_transmit_buffer_4x(sensor, buffer, count);
nx_systick_wait_ms(1000);
}
count = ht_irlink_encode_bitstream(HT_IRLINK_MODE_PF,
build_bitstream_PF_direct(1, PF_MOTOR_A_BACKWARD ),
buffer);
for ( unsigned i = 0; i < 5; ++i ) {
ht_irlink_transmit_buffer_4x(sensor, buffer, count);
nx_systick_wait_ms(1000);
}
/* (4) */
count = ht_irlink_encode_bitstream(HT_IRLINK_MODE_PF,
build_bitstream_PF_direct(0, PF_MOTOR_A_FORWARD | PF_MOTOR_B_FORWARD),
buffer);
for ( unsigned i = 0; i < 3; ++i ) {
ht_irlink_transmit_buffer_4x(sensor, buffer, count);
nx_systick_wait_ms(1000);
}
/* (5) */
for ( unsigned i = 0; i < 4; ++i ) {
count = ht_irlink_encode_bitstream(HT_IRLINK_MODE_PF,
build_bitstream_PF_pwm(0, PF_PWM_MOTOR_FWD_3 + i, PF_PWM_MOTOR_BACK_3 - i),
buffer);
ht_irlink_transmit_buffer_4x(sensor, buffer, count);
nx_systick_wait_ms(1000);
}
for ( unsigned i = 3; i ; --i ) {
count = ht_irlink_encode_bitstream(HT_IRLINK_MODE_PF,
build_bitstream_PF_pwm(0, PF_PWM_MOTOR_FWD_3 + i, PF_PWM_MOTOR_BACK_3 - i),
buffer);
ht_irlink_transmit_buffer_4x(sensor, buffer, count);
nx_systick_wait_ms(1000);
}
/* (6) */
for ( unsigned i = 0; i < 3; ++i ) {
count = ht_irlink_encode_bitstream(HT_IRLINK_MODE_PF,
build_bitstream_PF_pwm(0, PF_PWM_MOTOR_BACK_7, PF_PWM_MOTOR_BACK_7),
buffer);
ht_irlink_transmit_buffer_4x(sensor, buffer, count);
nx_systick_wait_ms(1000);
}
/* (7) */
count = ht_irlink_encode_bitstream(HT_IRLINK_MODE_PF,
build_bitstream_PF_direct(1, PF_MOTOR_B_BACKWARD ),
buffer);
for ( unsigned i = 0; i < 5; ++i ) {
ht_irlink_transmit_buffer_4x(sensor, buffer, count);
nx_systick_wait_ms(1000);
}
count = ht_irlink_encode_bitstream(HT_IRLINK_MODE_PF,
build_bitstream_PF_direct(1, PF_MOTOR_B_FORWARD ),
buffer);
for ( unsigned i = 0; i < 5; ++i ) {
ht_irlink_transmit_buffer_4x(sensor, buffer, count);
nx_systick_wait_ms(1000);
}
/* Display the receive buffer */
nx_display_cursor_set_pos(15, 4);
nx_display_string(" ");
nx_display_cursor_set_pos(12, 4);
nx_display_uint(count);
nx_display_string("):\n");
nx_display_string(" ");
nx_display_cursor_set_pos(0, 5);
for(unsigned i = 0; i < count; ++i) {
nx_display_hex(buffer[i]);
nx_display_string(" ");
}
nx_systick_wait_ms(10000);
/* this will display received messages continuously */
/*
while(nx_avr_get_button() != BUTTON_OK) {
// TODO: Check how to correctly handle receiving of messages.
count = ht_irlink_get_receive_buffer(sensor, buffer);
if(count) {
ht_irlink_clear_receive_buffer(sensor);
nx_display_cursor_set_pos(15, 4);
nx_display_string(" ");
nx_display_cursor_set_pos(12, 4);
nx_display_uint(count);
nx_display_string("):\n");
nx_display_string(" ");
nx_display_cursor_set_pos(0, 5);
for(unsigned i = 0; i < count; ++i) {
nx_display_hex(buffer[i]);
nx_display_string(" ");
}
// Clear the receive buffer, to receive more bytes.
// TODO: Is this really necessary?
ht_irlink_clear_receive_buffer(sensor);
}
nx_systick_wait_ms(20);
}
*/
ht_irlink_close(sensor);
goodbye();
}
void tests_fantom(void) {
#if !defined (__FANTOMENABLE__) && !defined (__DBGENABLE__)
U32 i, count = 0;
U32 lng = 0;
U8 *messagePtr;
char buffer[NX_USB_PACKET_SIZE];
hello();
while(1) {
nx_usb_read((U8 *)&buffer, NX_USB_PACKET_SIZE * sizeof(char));
nx_display_clear();
for (i = 0 ; i < 10 && !nx_usb_data_read(); i++)
{
nx_systick_wait_ms(200);
}
count++;
if (count >= 500)
break;
lng = nx_usb_data_read();
nx_display_cursor_set_pos(0, 0);
nx_display_string("Received Fantom message ...");
nx_display_uint(lng);
messagePtr = (U8 *)buffer;
if (fantom_filter_packet(&messagePtr, (U32 *)&lng, FALSE)) {
/* message was a fantom packet, so send any reply and clear it from our read buffers */
if (lng > 0) {
nx_usb_write(messagePtr, lng);
nx_display_cursor_set_pos(0, 4);
nx_display_string("Sent Fantom message ...");
nx_display_uint(lng);
}
}
nx_usb_read((U8 *)&buffer, NX_USB_PACKET_SIZE * sizeof(char));
}
goodbye();
#else
U32 count = 0;
#if defined (__FANTOMENABLE__) && !defined (__DBGENABLE__)
char buffer[NX_USB_PACKET_SIZE];
fantom_init((U8 *)&buffer, NX_USB_PACKET_SIZE * sizeof(char));
#else // defined (__DBGENABLE__)
// Nothing to do, fantom buffer already initialized by dbg__bkpt_init()
#endif
hello();
while(1) {
nx_display_cursor_set_pos(0, 0);
nx_display_string("Waiting for\nFantom Msg...\n");
nx_display_uint(count);
nx_display_string(" sec");
nx_systick_wait_ms(1000);
count++;
if (count >= 30)
break;
}
goodbye();
#endif
}
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();
}
void tests_usb(void) {
U16 i;
U32 lng = 0;
char buffer[NX_USB_PACKET_SIZE];
hello();
while(1) {
nx_display_cursor_set_pos(0, 0);
nx_display_string("Waiting command ...");
nx_usb_read((U8 *)&buffer, NX_USB_PACKET_SIZE * sizeof(char));
for (i = 0 ; i < 500 && !nx_usb_data_read(); i++)
{
nx_systick_wait_ms(200);
}
if (i >= 500)
break;
nx_display_clear();
lng = nx_usb_data_read();
if ((lng+1) < NX_USB_PACKET_SIZE)
buffer[lng+1] = '\0';
else
buffer[NX_USB_PACKET_SIZE-1] = '\0';
nx_display_cursor_set_pos(0, 0);
nx_display_string("==");
nx_display_uint(lng);
nx_display_cursor_set_pos(0, 1);
nx_display_string(buffer);
nx_display_cursor_set_pos(0, 2);
nx_display_hex((U32)(CMD_UNKNOWN));
/* Start interpreting */
i = tests_command(buffer);
if (i == 2) {
break;
}
if (i == 1) {
nx_usb_write((U8 *)CMD_UNKNOWN, sizeof(CMD_UNKNOWN)-1);
}
if (i == 0) {
nx_usb_write((U8 *)CMD_OK, sizeof(CMD_OK)-1);
}
nx_systick_wait_ms(500);
nx_display_clear();
}
goodbye();
}
void tests_bt(void) {
U16 i;
U16 lng = 0;
int port_handle __attribute__ ((unused));
int connection_handle = -1;
int bleh = -1;
char buffer[BT_PACKET_SIZE];
port_handle = -1;
for (i = 0 ; i < BT_PACKET_SIZE ; i++)
buffer[i] = 0;
lng = 0;
nx_bt_init();
hello();
nx_display_cursor_set_pos(0, 0);
nx_display_string("Setting discoverable ...");
nx_bt_set_friendly_name("Tulipe");
nx_bt_set_discoverable(TRUE);
port_handle = nx_bt_open_port();
nx_display_clear();
nx_display_cursor_set_pos(0, 0);
nx_display_string("Waiting connection ...");
while(TRUE) {
for (i = 0 ;
i < 800 && (!nx_bt_stream_opened() || nx_bt_stream_data_read() < 2);
i++)
{
if (connection_handle >= 0) {
nx_display_cursor_set_pos(0, 2);
nx_display_string("Waiting command ...");
}
if (nx_bt_has_dev_waiting_for_pin()) {
nx_bt_send_pin("1234");
} else if (nx_bt_connection_pending()) {
nx_bt_accept_connection(TRUE);
while ( (bleh = nx_bt_connection_established()) < 0)
nx_systick_wait_ms(100);
connection_handle = bleh;
nx_bt_debug();
nx_bt_stream_open(connection_handle);
nx_bt_debug();
nx_bt_stream_read((U8 *)&buffer, 2); /* we read the packet size first */
nx_display_cursor_set_pos(0, 4);
}
nx_bt_debug();
nx_systick_wait_ms(100);
}
if (!nx_bt_stream_opened() || i >= 800)
break;
lng = buffer[0] + (buffer[1] << 8);
nx_display_clear();
nx_display_cursor_set_pos(0, 0);
nx_display_string("Reading ...");
nx_display_cursor_set_pos(0, 1);
nx_display_uint(lng);
nx_bt_stream_read((U8 *)&buffer, lng);
for(i = 0;
nx_bt_stream_opened() && nx_bt_stream_data_read() < lng && i < 100;
i++)
nx_systick_wait_ms(100);
if (!nx_bt_stream_opened() || i >= 100)
break;
/* Start interpreting */
i = tests_command(buffer);
nx_bt_stream_read((U8 *)&buffer, 2);
if (i == 2) {
break;
}
/*
if (i == 1) {
nx_bt_stream_write((U8 *)CMD_UNKNOWN, sizeof(CMD_UNKNOWN)-1);
}
if (i == 0) {
nx_bt_stream_write((U8 *)CMD_OK, sizeof(CMD_OK)-1);
}
*/
nx_systick_wait_ms(100);
nx_display_clear();
}
if (nx_bt_stream_opened())
nx_bt_stream_close();
nx_systick_wait_ms(1000);
nx_bt_debug();
goodbye();
}
/**
* @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;
}
/** 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;
}
