void nav_magGo(float heading_deg, float dist) {
if (dist > 0) {
magfollow_start(60, degtorad(heading_deg));
} else {
magfollow_start(-60, degtorad(heading_deg));
}
drive_waitDist(dist);
magfollow_stop();
}
void goto_tick() {
if (!enabled) {
return;
}
if (turning) { // Angle desired is such that we need to turn first
if (magfollow_getTurnEnabled()) { // for all following ticks we do nothing until magturn is done
// Do nothing
return;
} else { // magturn is done
turning = false;
}
} else { // since magturn is done, do magfollow
// Update current data
OdomData odom = odometry_getPos();
data.x_current = odom.x_pos;
data.y_current = odom.y_pos;
data.heading = odom.heading;
// Check if we're already there
//if (abs(data.x_desired - data.x_current) < pos_cutoff && abs(data.y_desired - data.y_current) < pos_cutoff) { // If we're within 20 cm on x and y of desired
if (((data.x_dir*(data.x_desired - data.x_current) - pos_cutoff) < 0) && ((data.y_dir*(data.y_desired - data.y_current) - pos_cutoff) < 0)) {
goto_stop(); // Then stop following
return;
}
// If not there, continue going there
// Drive at necessary heading with limiter for magfollow's PIDs
if (ctr >= 100) {
magfollow_start(data.vel, goto_findHeading());
ctr = 0;
} else {
ctr++;
}
}
}
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;
}
}
}