/**
* 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);
}
}
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_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_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_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 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();
}
/** 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");
}
void nx_display_int(S32 val) {
if( val < 0 ) {
nx_display_string("-");
val = -val;
}
nx_display_uint(val);
}
/**
* 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_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 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_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_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();
}
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_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();
}
static void i2c_log_uint(U32 val)
{
if (I2C_LOG)
nx_display_uint(val);
}
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_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 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();
}
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_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_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 main() {
/* We'll use this buffer to read data from the RS485 bus. This needs
* to be volatile because it gets modified outside of the main code
* path (in interrupt handlers).
*/
U8 buffer_hello[12] = "Hello world";
U8 buffer_empty[12] = {0};
/* Install our emergency shutdown hook. See above for details. */
nx_systick_install_scheduler(shutdown_hook);
/* Initialize the RS485 driver for communication at 9600 bits per
* second with no timeout. If you need special communication modes
* (number of start/stop bits, parity checks...), you can specify an
* explicit mode register value.
*/
nx_rs485_init(RS485_BR_9600, 0, 0, FALSE);
for (int i = 0; i < 10; ++i) {
/* This is just to alternate between sending two different
* messages. One time we send "hello world", the other an empty
* string.
*/
U8 *buffer = (i % 2 == 0) ? buffer_hello : buffer_empty;
/* Display what we are transmitting, for the record */
nx_display_clear();
nx_display_string("Iteration ");
nx_display_uint(i+1);
nx_display_string("\n");
nx_display_string((char*)buffer);
/* Wait a bit before actually sending, to get a nice obvious
* delayed transmission effect.
*/
nx_systick_wait_ms(1000);
/* Try to write 12 bytes to the RS485 bus. This call merely
* instructs the RS485 driver to start writing data, and returns
* immediately.
*
* Once the read of all 12 bytes is complete, the given callback
* function will be called, with a status value, indicating
* whether or not there was an error.
*/
nx_rs485_send(buffer, sizeof(buffer), send_callback);
/* Since the above function returns immediately, we need a way to
* wait for the receive to complete. How you do this depends on
* how you want your application kernel to work, but in this case,
* we'll just block on our spinlock, until the callback unlocks it
* after the write completes.
*/
nx_spinlock_acquire(&lock);
/* Wait before transmitting the next round, again purely for
* effect.
*/
nx_systick_wait_ms(1000);
}
}
