void controlpanel_sensor() {
while (true) {
switch (controlpanel_promptChar("Sensor")) {
case 'a': // Prints raw adc sensor values pins 0 - 7
for (int i=0; i<8; i++)
printf_P(PSTR("%4d "), adc_sampleAverage(i, 10));
putchar('\n');
break;
case 'r': // Prints all rangefinder readings in centimeters
printf_P(PSTR("Side Left: %5f, Front Left: %5f, Front Right: %5f, Side Right: %5f\n"), adc_sampleRangeFinder(ADC_SIDE_LEFT_RANGE), adc_sampleRangeFinder(ADC_FRONT_LEFT_RANGE), adc_sampleRangeFinder(ADC_FRONT_RIGHT_RANGE), adc_sampleRangeFinder(ADC_SIDE_RIGHT_RANGE));
break;
case 'l': { // Prints out raw linesensor data
uint16_t linebuf[linesensor_count];
linesensor_read(linebuf);
for (int i=0; i<linesensor_count; i++)
printf_P(PSTR("%-5u "), linebuf[i]);
putchar('\n');
break;
}
case 'L': { // Prints out full crunched linesensor data
debug_resetTimer();
LineFollowResults results = linefollow_readSensor();
uint16_t time = debug_getTimer();
printf_P(PSTR("Light:\t"));
for (int i=0; i<linesensor_count; i++)
printf_P(PSTR("%2.2f\t"), results.light[i]);
putchar('\n');
printf_P(PSTR("Thresh:\t"));
for (int i=0; i<linesensor_count; i++)
printf_P(PSTR("%d\t"), results.thresh[i]);
putchar('\n');
printf_P(PSTR("Center:\t%f\n"), results.center);
printf_P(PSTR("Turn:\t")); linefollow_printTurn(results.turn); putchar('\n');
printf_P(PSTR("Feat:\t")); linefollow_printFeature(results.feature); putchar('\n');
printf_P(PSTR("Time:\t%uus\n"), time);
break;
}
case 't': {
float thresh;
printf_P(PSTR("Current threshold: %f\n"), linefollow_getThresh());
if (controlpanel_prompt("Threshold", "%f", &thresh) == 1) {
printf_P(PSTR("Threshold changed to %f\n"), thresh);
linefollow_setThresh(thresh);
} else {
printf_P(PSTR("Cancelled.\n"));
}
break;
}
case 'b':
printf_P(PSTR("Battery voltage: %.2f\n"), adc_getBattery());
break;
case 'm': { // Prints out raw magnetometer data
MagReading reading = mag_getReading();
printf_P(PSTR("mag: %5d %5d %5d\n"), reading.x, reading.y, reading.z);
break;
}
case 'M': { // Prints out magnetometer calibrated heading
float heading = magfollow_getHeading();
heading = radtodeg(heading);
printf_P(PSTR("Mag Heading: %f\n"), heading);
break;
}
case 'H': { // Sets current heading of robot to prompted heading from user
float newheading;
controlpanel_prompt("Heading", "%f", &newheading);
magfollow_setHeading(degtorad(newheading));
break;
}
case 'q':
return;
default:
puts_P(unknown_str);
break;
case '?':
static const char msg[] PROGMEM =
"Sensor commands\n"
" a - Dump analog port A\n"
" r - Rangefinder control panel\n"
" l - Raw line sensor readings\n"
" L - Processed line sensor readings\n"
" b - Battery voltage (approx)\n"
" m - Magnetometer\n"
" M - Magnetometer Heading\n"
" H - Set Magnetometer Heading\n"
" q - Back";
puts_P(msg);
break;
}
}
}
void controlpanel_magnetometer() {
MagReading dir;
float heading;
float first_heading;
while (true) {
char ch = controlpanel_promptChar("Magnetometer");
switch (ch) {
case 'x':
dir = mag_getReading();
printf_P(PSTR("X: %d\n"), dir.x);
break;
case 'y':
dir = mag_getReading();
printf_P(PSTR("Y: %d\n"), dir.y);
break;
case 'z':
dir = mag_getReading();
printf_P(PSTR("Z: %d\n"), dir.z);
break;
case 'h':
heading = magfollow_getHeading();
printf_P(PSTR("Heading: %f\n"), heading);
break;
case 'j':
printf_P(PSTR("Raw heading: %f\n"), magfollow_getRawHeading());
break;
case 'l':
motorcontrol_setRPS(MOTOR_LEFT, -.1);
motorcontrol_setRPS(MOTOR_RIGHT, .1);
first_heading = magfollow_getHeading();
heading = magfollow_getHeading();
while (sign(heading - first_heading)*(heading - first_heading) < 1) {
dir = mag_getReading();
printf_P(PSTR("%d %d %d\n"), dir.x, dir.y, dir.z);
heading = magfollow_getHeading();
}
while (heading != first_heading) {
heading = magfollow_getHeading();
dir = mag_getReading();
printf_P(PSTR("%d %d %d\n"), dir.x, dir.y, dir.z);
}
while (sign(heading - first_heading)*(heading - first_heading) < 1) {
dir = mag_getReading();
printf_P(PSTR("%d %d %d\n"), dir.x, dir.y, dir.z);
heading = magfollow_getHeading();
}
motorcontrol_setRPS(MOTOR_LEFT, 0);
motorcontrol_setRPS(MOTOR_RIGHT, 0);
break;
case 'q':
return;
case '?':
static const char msg[] PROGMEM =
"Encoder menu:\n"
" x - x value\n"
" y - y value\n"
" z - z value\n"
" h - Heading\n"
" j - Raw Heading\n"
" l - Calibrate\n"
" q - Back\n";
puts_P(msg);
break;
default:
puts_P(unknown_str);
break;
}
}
}
