示例#1
0
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();
}
示例#2
0
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++;
		}
	}
}
示例#3
0
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;
		}
	}
}