Example #1
0
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;
		}
	}
}