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 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_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();
}
/** 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;
}
