bool controlpanel_promptGains(const char *name, const PIDGains &curgains, PIDGains &gains) {
printf_P(PSTR("Setting gains for %s\n"), name);
printf_P(PSTR("Current gains: P %.4f I %.4f D %.4f MaxI %.4f\n"), curgains.p, curgains.i, curgains.d, curgains.maxi);
if (controlpanel_prompt("P", "%f", &gains.p) != 1)
gains.p = curgains.p;
if (controlpanel_prompt("I", "%f", &gains.i) != 1)
gains.i = curgains.i;
if (controlpanel_prompt("D", "%f", &gains.d) != 1)
gains.d = curgains.d;
if (controlpanel_prompt("MaxI", "%f", &gains.maxi) != 1)
gains.maxi = curgains.maxi;
printf_P(PSTR("New gains: P %.4f I %.4f D %.4f MaxI %.4f\n"), gains.p, gains.i, gains.d, gains.maxi);
return controlpanel_promptChar("Ok? [y/n]") == 'y';
}
void controlpanel_drive() {
float speed=20;
while (true) {
char ch=controlpanel_promptChar("Drive");
switch (ch) {
case ' ':
drive_stop();
break;
case 'x':
drive_stop(DM_TRAJ);
break;
case 'w':
drive_fd(speed);
break;
case 'a':
drive_lturn(speed);
break;
case 's':
drive_bk(speed);
break;
case 'd':
drive_rturn(speed);
break;
case 'W':
drive_fdDist(speed, 50);
break;
case 'A':
drive_lturnDeg(speed, 90);
break;
case 'S':
drive_bkDist(speed, 50);
break;
case 'D':
drive_rturnDeg(speed, 90);
break;
case '=':
speed += 2;
printf_P(PSTR("Speed: %f\n"), speed);
break;
case '-':
speed -= 2;
printf_P(PSTR("Speed: %f\n"), speed);
break;
case '+':
speed += 10;
printf_P(PSTR("Speed: %f\n"), speed);
break;
case '_':
speed -= 10;
printf_P(PSTR("Speed: %f\n"), speed);
break;
case 'p':
motorcontrol_setDebug(false);
printf_P(PSTR("Debug disabled\n"));
break;
case 'c':
motorcontrol_setEnabled(false);
printf_P(PSTR("Motor control disabled\n"));
break;
case 'P':
motorcontrol_setDebug(true);
break;
case 'q':
motorcontrol_setEnabled(false);
return;
case 'z': // Does moonwalk forwards
speed = 40;
for (int i = 0; i < 20; i++) {
drive_fd(speed);
_delay_ms(25);
drive_rturn(speed);
_delay_ms(50);
drive_fd(speed);
_delay_ms(25);
drive_lturn(speed);
_delay_ms(50);
}
drive_stop();
speed = 20;
break;
case 'Z': // Does moonwalk backwards
speed = 100;
for (int i = 0; i < 20; i++) {
drive_bk(speed);
_delay_ms(25);
drive_rturn(speed);
_delay_ms(50);
drive_bk(speed);
_delay_ms(25);
drive_lturn(speed);
_delay_ms(50);
}
speed = 20;
break;
case 'm': {
float amax = drive_getTrajAmax();
printf_P(PSTR("Current amax: %.2f\n"), amax);
if (controlpanel_prompt("amax", "%f", &amax) != 1) {
printf_P(PSTR("Cancelled.\n"));
continue;
}
drive_setTrajAmax(amax);
printf_P(PSTR("Amax set to: %.2f\n"), amax);
break;
}
default:
puts_P(unknown_str);
drive_stop();
break;
case '?':
static const char msg[] PROGMEM =
"Drive commands:\n"
" wasd - Control robot\n"
" space - Stop\n"
" -=_+ - Adjust speed\n"
" WASD - Execute distance moves\n"
" c - Disable motor control\n"
" Pp - Enable/Disable motor control debug\n"
" zZ - Moonwalk (WIP)\n"
" q - Back";
puts_P(msg);
break;
}
}
}
void controlpanel_sensorcomms() {
while (true) {
switch (controlpanel_promptChar("Comms")) {
case 'c':
printf_P(PSTR("Board count: %d\n"), sensorcomms_getOnlineBoardCount());
break;
case 'C': {
int count;
if (controlpanel_prompt("Board Count", "%d", &count) != 1) {
printf_P(PSTR("Cancelled.\n"));
} else {
sensorcomms_setOnlineBoardCount((uint8_t)count);
printf_P(PSTR("Overrode board count to %d\n"), count);
}
break;
}
case 's': {
static const char symbols[] = {'V', 'C', 'T', 'S'};
for (int board=0; board < BOARDNUM_MAX; board++) {
printf_P(PSTR("%c "), symbols[board]);
sensorcomms_printBoardStatus(sensorcomms_getBoardStatus((BoardNum)board));
putchar('\n');
}
break;
}
case 'u': {
int board;
if (controlpanel_prompt("Board", "%d", &board) != 1) {
printf_P(PSTR("Cancelled.\n"));
break;
}
sensorcomms_updateBoard((BoardNum)board);
printf_P(PSTR("Updating board %d\n"), board);
break;
}
case 'U': {
for (int board=0; board < sensorcomms_getOnlineBoardCount(); board++) {
sensorcomms_updateBoard((BoardNum)board);
printf_P(PSTR("Updating board %d\n"), board);
if (!sensorcomms_waitBoard((BoardNum)board, 1000)) {
printf_P(PSTR("board %d timed out\n"), board);
sensorcomms_cancelUpdate();
}
}
printf_P(PSTR("Done!\n"));
break;
}
case 'R':
if (controlpanel_promptChar("Really reset comms? [y/n]") == 'y') {
printf_P(PSTR("Comms reset\n"));
sensorcomms_reset();
}
break;
case 'd': {
const uint8_t *buf;
for (int board=0; board < BOARDNUM_MAX; board++) {
buf = sensorcomms_getBoardReading((BoardNum)board);
for (int i=0; i<sensorcomms_readinglen; i++)
printf_P(PSTR("%02x "), buf[i]);
if (sensorcomms_getBoardReadingValid((BoardNum)board))
printf_P(PSTR(" - valid\n"));
else
printf_P(PSTR(" - invalid\n"));
}
break;
}
case 'D': {
int decision;
if (controlpanel_prompt("Decision", "%d", &decision) != 1) {
printf_P(PSTR("Cancelled.\n"));
break;
}
sensordecision_prepare((uint8_t)decision);
if (!sensordecision_wait()) {
printf_P(PSTR("Decision timed out\n"));
sensorcomms_cancelUpdate();
} else {
printf_P(PSTR("Decision: %s\n"), sensordecision_isRight() ? "Right" : "Left");
}
break;
}
case 'P':
sensorcomms_setDebug(true);
printf_P(PSTR("Comm prints enabled\n"));
break;
case 'p':
sensorcomms_setDebug(false);
printf_P(PSTR("Comm prints disabled\n"));
break;
case '0':
sensordecision_prepare(0);
sensordecision_wait();
if (sensordecision_isRight()) {
printf_P(PSTR("Right!\n"));
} else {
printf_P(PSTR("Left!\n"));
}
break;
case '1':
sensordecision_prepare(1);
sensordecision_wait();
if (sensordecision_isRight()) {
printf_P(PSTR("Right!\n"));
} else {
printf_P(PSTR("Left!\n"));
}
break;
case '2':
sensordecision_prepare(2);
sensordecision_wait();
if (sensordecision_isRight()) {
printf_P(PSTR("Right!\n"));
} else {
printf_P(PSTR("Left!\n"));
}
break;
case '3':
sensordecision_prepare(3);
sensordecision_wait();
if (sensordecision_isRight()) {
printf_P(PSTR("Right!\n"));
} else {
printf_P(PSTR("Left!\n"));
}
break;
case 'q':
return;
// TODO help menu
}
}
}
void controlpanel_nav() {
while (true) {
switch (controlpanel_promptChar("Nav")) {
case 'i': // Linefollows until an intersection
if (nav_linefollowIntersection())
printf_P(PSTR("Intersection!\n"));
else
printf_P(PSTR("Error\n"));
break;
case 't': { // Linefollows for a user prompted number of turns
int turns;
if (!controlpanel_prompt("Turns", "%d", &turns)) {
printf_P(PSTR("Canceled.\n"));
break;
}
if (nav_linefollowTurns(turns))
printf_P(PSTR("Ok.\n"));
else
printf_P(PSTR("Error\n"));
break;
}
case 'g': { // Does a NavGo at a user prompted magnetometer heading and for a user prompted distance
float heading;
if (!controlpanel_prompt("Heading", "%f", &heading)) {
printf_P(PSTR("Canceled.\n"));
break;
}
float dist;
if (!controlpanel_prompt("Dist", "%f", &dist)) {
printf_P(PSTR("Canceled.\n"));
break;
}
nav_magGo(heading, dist);
break;
}
case 'f':
navfast_lap();
break;
case 'd':
navdeploy_lap();
break;
case 'p':
printf_P(PSTR("Pauses disabled\n"));
nav_setPauseEnabled(false);
break;
case 'P':
printf_P(PSTR("Pauses enabled\n"));
nav_setPauseEnabled(true);
break;
case 'q':
return;
default:
puts_P(unknown_str);
break;
case '?':
static const char msg[] PROGMEM =
"Nav commands\n"
" i - Intersection line follow\n"
" t - Turn counting line follow\n"
" g - magGo along heading for specific distance\n"
" f - run a fast lap\n"
" d - run a deploying lap\n"
" Pp - enable/disable pauses\n"
" q - back";
puts_P(msg);
break;
}
}
}
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_autonomy() {
float follow = 0;
char input = ' ';
OdomData odom;
while (true) {
char ch = controlpanel_promptChar("Autonomy");
switch (ch) {
case 'G': {
float x_des, y_des, vel;
odom = odometry_getPos();
printf_P(PSTR("Current Position, X (meters): %f, Y (meters): %f, Heading (deg): %f\n"), cmtom(odom.x_pos), cmtom(odom.y_pos), radtodeg(odom.heading));
controlpanel_prompt("Goto X (meters): ", "%f", &x_des);
controlpanel_prompt("Goto Y (meters): ", "%f", &y_des);
controlpanel_prompt("At, Vel (cm/s): ", "%f", &vel);
obstacleAvoidance_setEnabled(true);
goto_pos(mtocm(x_des), mtocm(y_des), vel);
break;
}
case 'g': {
float x_des, y_des, vel;
odom = odometry_getPos();
printf_P(PSTR("Current Position, X (meters): %f, Y (meters): %f, Heading (deg): %f\n"), cmtom(odom.x_pos), cmtom(odom.y_pos), radtodeg(odom.heading));
controlpanel_prompt("Goto X (meters): ", "%f", &x_des);
controlpanel_prompt("Goto Y (meters): ", "%f", &y_des);
controlpanel_prompt("At, Vel (cm/s): ", "%f", &vel);
goto_pos(mtocm(x_des), mtocm(y_des), vel);
break;
}
case 'e':
if (goto_getEnabled()) {
printf_P(PSTR("Goto: enabled, "));
} else {
printf_P(PSTR("Goto: disabled, "));
}
if (magfollow_enabled()) {
printf_P(PSTR("Magfollow: enabled, "));
} else {
printf_P(PSTR("Magfollow: disabled, "));
}
if (obstacleAvoidance_getEnabled()) {
printf_P(PSTR("Obstacle Avoidance: enabled.\n"));
} else {
printf_P(PSTR("Obstacle Avoidance: disabled.\n"));
}
break;
case 's': { // Sets current heading of robot to prompted heading from user
float newheading;
controlpanel_prompt("Heading (deg)", "%f", &newheading);
magfollow_setHeading(degtorad(newheading));
break;
}
case 'h': { // Prints out magnetometer calibrated heading
float heading = magfollow_getHeading();
heading = radtodeg(heading);
printf_P(PSTR("Mag Heading (deg): %f\n"), heading);
break;
}
case 'w': {
printf_P(PSTR("Currently Facing (deg): %f\n"), radtodeg(magfollow_getHeading()));
controlpanel_prompt("Follow at Heading (deg)", "%f", &follow);
magfollow_start(setSpeed, anglewrap(degtorad(follow)));
printf_P(PSTR("Following at (deg): %f\n"), follow);
break;
}
case 'a':
follow = follow + 5;
if (magfollow_enabled()) {
magfollow_start(setSpeed, anglewrap(degtorad(follow)));
printf_P(PSTR("Following at (deg): %f\n"), follow);
} else {
printf_P(PSTR("Not following, but heading set to (deg): %f\n"), follow);
}
break;
case 'd':
follow = follow - 5;
if (magfollow_enabled()) {
magfollow_start(setSpeed, anglewrap(degtorad(follow)));
printf_P(PSTR("Following at (deg): %f\n"), follow);
} else {
printf_P(PSTR("Not following, but heading set to (deg): %f\n"), follow);
}
break;
case 't':
printf_P(PSTR("Currently Facing (deg): %f\n"), radtodeg(magfollow_getHeading()));
controlpanel_prompt("Turn to Heading (deg)", "%f", &follow);
follow = degtorad(follow);
printf_P(PSTR("Currently at (deg): %f, Turning to (deg): %f\n"), radtodeg(magfollow_getHeading()), radtodeg(follow));
magfollow_turn(setSpeed, anglewrap(follow));
break;
case 'o':
obstacleAvoidance_setEnabled(false);
printf_P(PSTR("Obstacle Avoidance Disabled.\n"));
break;
case 'O':
obstacleAvoidance_setEnabled(true);
printf_P(PSTR("Obstacle Avoidance Enabled!\n"));
break;
case 'c': {
printf_P(PSTR("Beginning auto-cal!\nTurn robot to face 0 Degrees in field.\n"));
input = controlpanel_promptChar("Press 'Enter' to begin or any other key to cancel.");
if (input == 10) {
printf_P(PSTR("Calibrating...\n"));
calibrate_stationary();
} else {
printf_P(PSTR("Auto-cal Cancelled.\n"));
}
break;
}
case 'C': {
printf_P(PSTR("Beginning Competition Routine!\n"));
input = controlpanel_promptChar("Press 'Enter' to begin or any other key to cancel.");
if (input == 10) {
printf_P(PSTR("Calibrating...\n"));
Field field = calibrate_competition();
float vel;
controlpanel_prompt("Velocity (cm/s): ", "%f", &vel);
printf_P(PSTR("Running Spiral-In Course!\n"));
overlap_run(field.xi, field.yi, field.xj, field.yj, field.xk, field.yk, field.xl, field.yl, vel);
} else {
printf_P(PSTR("Auto-cal Cancelled.\n"));
}
break;
}
case 'i': {
printf_P(PSTR("Beginning competition auto-cal!\nTurn robot to face 0 Degrees in field.\n"));
input = controlpanel_promptChar("Press 'Enter' to begin or any other key to cancel.");
if (input == 10) {
printf_P(PSTR("Calibrating...\n"));
magfollow_setOffset(0);
} else {
printf_P(PSTR("Auto-cal Cancelled.\n"));
}
float xi, yi;
float xj, yj;
float xk, yk;
float xl, yl;
float vel;
controlpanel_prompt("Xi (meters): ", "%f", &xi);
controlpanel_prompt("Yi (meters): ", "%f", &yi);
controlpanel_prompt("Xj (meters): ", "%f", &xj);
controlpanel_prompt("Yj (meters): ", "%f", &yj);
controlpanel_prompt("Xk (meters): ", "%f", &xk);
controlpanel_prompt("Yk (meters): ", "%f", &yk);
controlpanel_prompt("Xl (meters): ", "%f", &xl);
controlpanel_prompt("Yl (meters): ", "%f", &yl);
controlpanel_prompt("Velocity (cm/s): ", "%f", &vel);
spiralIn_run(xi, yi, xj, yj, xk, yk, xl, yl, vel);
break;
}
case 'f':
debug_halt("testing");
break;
case ' ':
magfollow_stop();
obstacleAvoidance_setEnabled(false);
goto_setEnabled(false);
break;
case 'q':
magfollow_stop();
return;
case '?':
static const char msg[] PROGMEM =
"Control Panels:\n"
" G - Goto Coordinate w/ Obstacle Avoidance\n"
" g - Goto Coordinate\n"
" e - Print states of enables\n"
" s - Set Heading\n"
" h - Current Heading\n"
" w - Magfollow\n"
" a - Shift following left\n"
" d - Shift following right\n"
" t - MagTurn\n"
" O/o - Enable/Disable Obstacle Avoidance\n"
" c - Auto-Calibration Routine\n"
" C - Do Competition Routine\n"
" i - Run Spiral-In competition\n"
" f - Halt\n"
" ' ' - Stop\n"
" q - Quit";
puts_P(msg);
break;
default:
puts_P(unknown_str);
break;
}
}
}
