void controlpanel_sonar() {
while (true) {
char ch = controlpanel_promptChar("Sonars");
switch (ch) {
case 'd':
printf_P(PSTR("LDist: %f, FDist: %f\n"), sonar_getDist(LEFT_SONAR), sonar_getDist(FRONT_SONAR));
break;
case 't':
printf_P(PSTR("Timer: %f\n"), sonar_getTimer());
break;
case 'q':
return;
case '?':
static const char msg[] PROGMEM =
"Encoder menu:\n"
" d - Distance (cm)\n"
" t - Timer\n"
" q - Back\n";
puts_P(msg);
break;
default:
puts_P(unknown_str);
break;
}
}
}
void controlpanel_odometry() {
OdomData odom;
while(true) {
char ch = controlpanel_promptChar("Odometry");
switch (ch) {
case 'p':
odom = odometry_getPos();
printf_P(PSTR("X: %f, Y: %f, Heading (deg): %f\n"), odom.x_pos, odom.y_pos, radtodeg(odom.heading));
break;
case 'q':
return;
case '?':
static const char msg[] PROGMEM =
"Control Panels:\n"
" p - Current Position Data\n"
" ' '- Stop\n"
" q - Quit";
puts_P(msg);
break;
default:
puts_P(unknown_str);
break;
}
}
}
void controlpanel_deploy() {
while (true) {
switch (controlpanel_promptChar("Deploy")) {
case 'o':
deploy_out();
break;
case 'O':
deploy_out(true);
break;
case 'i':
deploy_in();
break;
case 'I':
deploy_in(true);
break;
case 'b':
printf("Beambreak: %d\n", deploy_getBeamBreak());
break;
case 'd':
deploy_start(false);
break;
case 'D':
deploy_start(true);
break;
case ' ':
deploy_stop();
break;
case 'l':
deploy_in();
while(!deploy_getBeamBreak()) {
}
_delay_ms(10);
while(deploy_getBeamBreak()) {
}
_delay_ms(8000);
deploy_stop();
break;
case 'q':
deploy_stop();
return;
default:
deploy_stop();
puts_P(unknown_str);
static const char msg[] PROGMEM =
"Deploy commands\n"
" io - Deploy in/out\n"
" IO - Deploy in/out fast\n"
" b - Read break beam\n"
" d - Put module to deploy position\n"
" space - Stop\n"
" q - Back";
puts_P(msg);
break;
}
}
}
void controlpanel_gps() {
char rawData;
while (true) {
char ch = controlpanel_promptChar("GPS");
switch (ch) {
case 'r':
gps_printRaw();
break;
case 's':
gps_printStatus();
break;
case 'p':
gps_printPos();
break;
case 'l': {
GPSPos pos = gps_getPos();
printf_P(PSTR("Raw position: X: %f, Y: %f\n"), pos.X_Raw, pos.Y_Raw);
break;
}
case 'o':
gps_printOffset();
break;
case 'e':
if (gps_getEnabled()) {
gps_setEnabled(false);
printf_P(PSTR("GPS has been disabled!\n"));
} else {
gps_setEnabled(true);
printf_P(PSTR("GPS has been enabled!\n"));
}
break;
case 'q':
return;
case '?':
static const char msg[] PROGMEM =
"Sensor menu:\n"
" r - Raw Data\n"
" s - GPS Status\n"
" p - Position\n"
" l - Global UTM Position\n"
" o - Global UTM Origin Location (calibrated)\n"
" e - Enable/Disable GPS Odom update\n"
" q - Back\n";
puts_P(msg);
break;
default:
puts_P(unknown_str);
break;
}
}
}
void controlpanel_calibrate() {
PIDGains newgains;
while (true) {
char ch = controlpanel_promptChar("Calibrate");
switch (ch) {
case 'm':
if (controlpanel_promptGains("magfollow", magfollow_getGains(), newgains)) {
magfollow_setGains(newgains);
printf_P(PSTR("Gains set!\n"));
} else {
printf_P(PSTR("Cancelled.\n"));
}
break;
case 't':
if (controlpanel_promptGains("magturn", magfollow_getTurnGains(), newgains)) {
magfollow_setTurnGains(newgains);
printf_P(PSTR("Gains set!\n"));
} else {
printf_P(PSTR("Cancelled.\n"));
}
break;
case 'd':
if (controlpanel_promptGains("motorcontrol", motorcontrol_getGains(), newgains)) {
motorcontrol_setGains(newgains);
printf_P(PSTR("Gains set!\n"));
} else {
printf_P(PSTR("Cancelled.\n"));
}
break;
case 'f':
tests_feedforward();
break;
case 'q':
return;
case '?':
static const char msg[] PROGMEM =
"Calibrate Menu:\n"
" m - Magnetometer PID\n"
" t - Magturn PID\n"
" d - Motorcontrol PID\n"
" f - Feed-forward calibration !!MOVES MOTORS!!\n"
" q - Back\n";
puts_P(msg);
break;
default:
puts_P(unknown_str);
break;
}
}
}
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';
}
bool controlpanel() {
while (true) {
switch (controlpanel_promptChar("Main")) {
case 'd':
controlpanel_drive();
break;
case 'g':
controlpanel_gains();
break;
case 'm':
controlpanel_motor();
break;
case 's':
controlpanel_sensor();
break;
case 't':
controlpanel_tests();
break;
case 'n':
controlpanel_nav();
break;
case 'c':
controlpanel_sensorcomms();
break;
case 'D':
controlpanel_deploy();
break;
case 'G': // go
return true;
default:
puts_P(unknown_str);
break;
case '?':
static const char msg[] PROGMEM =
"Control Panels:\n"
" d - Drive\n"
" D - Deploy\n"
" g - Gains\n"
" m - Motor\n"
" s - Sensor\n"
" n - Nav\n"
" t - Tests";
puts_P(msg);
break;
}
}
}
void controlpanel_gains() {
PIDGains newgains;
while (true) {
char ch = controlpanel_promptChar("Gains");
switch (ch) {
case 'm':
if (controlpanel_promptGains("motorcontrol", motorcontrol_getGains(), newgains)) {
motorcontrol_setGains(newgains);
printf_P(PSTR("Gains set!\n"));
} else {
printf_P(PSTR("Cancelled.\n"));
}
break;
case 'M':
if (controlpanel_promptGains("magfollow", magfollow_getGains(), newgains)) {
magfollow_setGains(newgains);
printf_P(PSTR("Gains set!\n"));
} else {
printf_P(PSTR("Cancelled.\n"));
}
break;
case 'l':
if (controlpanel_promptGains("linefollow", linefollow_getGains(), newgains)) {
linefollow_setGains(newgains);
printf_P(PSTR("Gains set!\n"));
} else {
printf_P(PSTR("Cancelled.\n"));
}
break;
case 'q':
return;
case '?':
static const char msg[] PROGMEM =
"Gain commands:\n"
" m - Adjust Motor Gains\n"
" M - Adjust Magfollow Gains\n"
" l - Adjust Linefollow Gains\n"
" q - Back\n";
puts_P(msg);
break;
default:
printf_P(PSTR("Unknown Command, type '?' for help.\n"));
}
}
}
bool controlpanel() {
while (true) {
switch (controlpanel_promptChar("Main")) {
case 'd':
controlpanel_drive();
break;
case 's':
controlpanel_sensor();
break;
case 'l':
controlpanel_debug();
break;
case 'a':
controlpanel_autonomy();
break;
case 'o':
controlpanel_odometry();
break;
case 'c':
controlpanel_calibrate();
break;
default:
puts_P(unknown_str);
break;
case '?':
static const char msg[] PROGMEM =
"Control Panels:\n"
" d - Drive\n"
" s - Sensors\n"
" l - debug\n"
" a - Autonomy\n"
" o - Odometry\n"
" c - Calibrate\n"
" q - Quit";
puts_P(msg);
break;
}
}
}
void controlpanel_sensor() {
while (true) {
char ch = controlpanel_promptChar("Sensors");
switch (ch) {
case 'e':
controlpanel_encoder();
break;
case 'b':
printf_P(PSTR("%f\n"), adc_sampleBattery());
break;
case 'm':
controlpanel_magnetometer();
break;
case 's':
controlpanel_sonar();
break;
case 'g':
controlpanel_gps();
case 'q':
return;
case '?':
static const char msg[] PROGMEM =
"Sensor menu:\n"
" e - Encoders\n"
" b - Battery Voltage\n"
" m - Magnetometer\n"
" s - Sonar\n"
" g - GPS\n"
" q - Back\n";
puts_P(msg);
break;
default:
puts_P(unknown_str);
break;
}
}
}
void controlpanel_encoder() {
int ticks = 0;
while (true) {
char ch = controlpanel_promptChar("Encoders");
switch (ch) {
case 'p':
ticks = encoder_get(LEFT_ENCODER);
printf_P(PSTR("L Ticks: %d "), ticks);
ticks = encoder_get(RIGHT_ENCODER);
printf_P(PSTR("R Ticks: %d\n"), ticks);
break;
case 'r':
encoder_reset(LEFT_ENCODER);
encoder_reset(RIGHT_ENCODER);
break;
case 's':
motorcontrol_setEnabled(true);
printf_P(PSTR("L RPS: %f, R RPS: %f\n"), motorcontrol_getRPS(MOTOR_LEFT), motorcontrol_getRPS(MOTOR_RIGHT));
break;
case 'q':
return;
case '?':
static const char msg[] PROGMEM =
"Encoder menu:\n"
" p - position (ticks)\n"
" r - Reset counter\n"
" s - RPS of wheels\n"
" q - Back\n";
puts_P(msg);
break;
default:
puts_P(unknown_str);
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;
}
}
}
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_debug() {
bool error_led = false;
bool estop_led = false;
bool tick_led = false;
bool led2 = false;
bool led3 = false;
bool cut = false;
while (true) {
char ch = controlpanel_promptChar("LED");
switch (ch) {
case 'e':
debug_setLED(ERROR_LED, !error_led);
error_led = !error_led;
break;
case 's':
debug_setLED(ESTOP_LED, !estop_led);
estop_led = !estop_led;
break;
case 't':
debug_setLED(TICK_LED, !tick_led);
tick_led = !tick_led;
break;
case '2':
debug_setLED(OTHER2_LED, !led2);
led2 = !led2;
break;
case '3':
debug_setLED(OTHER3_LED, !led3);
led3 = !led3;
break;
case 'b':
cut = !cut;
debug_cutBuzzer(cut);
if (cut) {
printf_P(PSTR("Buzzer has been cut!\n"));
} else {
printf_P(PSTR("Buzzer has been enabled!\n"));
}
break;
case 'q':
debug_setLED(ERROR_LED, false);
debug_setLED(ESTOP_LED, false);
debug_setLED(TICK_LED, false);
debug_setLED(OTHER2_LED, false);
debug_setLED(OTHER3_LED, false);
return;
case '?':
static const char msg[] PROGMEM =
"LED commands:\n"
" e - Error LED\n"
" s - Estop LED\n"
" t - Tick LED\n"
" 2 - LED 2\n"
" 3 - LED 3\n"
" b - Cut/Uncut Buzzer\n"
" q - Back";
puts_P(msg);
break;
default:
puts_P(unknown_str);
break;
}
}
}
void controlpanel_tests() {
while (true) {
switch (controlpanel_promptChar("Tests")) {
case 'f': // Linefollow at user prompted velocity until key press
tests_linefollow();
break;
case 'p': // Ramps motors forwards to full speed then backwards to full speed
tests_PWM();
break;
case 'M': // Spins in place while printing raw magnetometer data for 1 rev
tests_mag();
break;
case 'm': // Follows a user prompted heading at prompted speed of magnetometer
tests_magfollow();
break;
case 'D': // Turns on debug printing when linefollowing
linefollow_setDebug(true);
break;
case 'd': // Turns off debug printing when linefollowing
linefollow_setDebug(false);
break;
case 'e': // Blinks through debug LEDs
tests_led();
break;
case 'L': // Prints minimum of each sensor on linesensor array over 4 seconds of readings
uint16_t min[8];
tests_linesensorMin(min);
for (int i = 0; i < 8; i++) {
printf_P(PSTR("%5u "), min[i]);
}
printf_P(PSTR("\n"));
break;
case 'l': // Prints out linesensor data while spinning wheels for encoder interference
tests_movingLineRead();
break;
case 'q':
return;
case 't':
printf_P(PSTR("Tick Length %u us\n"), tick_getLength());
break;
case 'r':
printf_P(PSTR("Random mode disabled\n"));
sensordecision_setRandomMode(false);
break;
case 'R':
printf_P(PSTR("Random mode enabled\n"));
sensordecision_setRandomMode(true);
break;
default:
puts_P(unknown_str);
break;
case '?':
static const char msg[] PROGMEM =
"Test commands\n"
" f - Linefollow test\n"
" p - Test motor PWM range (floors motors)\n"
" M - Test magnetometer (spins in place)\n"
" m - Magfollow test\n"
" Dd - Enables/Disable line follow debugging\n"
" e - Tests debug LEDs\n"
" L - Prints min val of each linesensor over 4 secs\n"
" l - Runs motors while printing linesensor data\n"
" t - Prints encoder ticks\n"
" q - Back";
puts_P(msg);
break;
}
}
}
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_motor() {
bool motenables[4] = {0, 0, 0, 0};
int16_t PWM = 0;
while (true) {
char ch = controlpanel_promptChar("Motor");
if (ch >= '0' && ch <= '3') {
int num = ch-'0';
bool &enable = motenables[num];
enable = !enable;
printf_P(PSTR("Motor %d %s\n"), num, enable ? "enabled" : "disabled");
} else if (ch == 'e') {
printf_P(PSTR("L %i R %i\n"), enc_get(MOTOR_LEFT), enc_get(MOTOR_RIGHT));
} else if (ch == 'E') {
printf_P(PSTR("Encoders reset\n"));
enc_reset(MOTOR_LEFT);
enc_reset(MOTOR_RIGHT);
} else {
switch (ch) {
case 'x':
PWM += 50;
break;
case 'z':
PWM -= 50;
break;
case 'X':
PWM += 200;
break;
case 'Z':
PWM -= 200;
break;
case 'a':
PWM = -motor_maxPWM;
break;
case 's':
PWM = motor_maxPWM;
break;
case 'd':
PWM = -PWM;
break;
case ' ':
PWM = 0;
break;
case 'q':
motor_allOff();
return;
default:
puts_P(unknown_str);
break;
case '?':
static const char msg[] PROGMEM =
"Motor commands:\n"
" 0123 - Toggle motors\n"
" zxZX - Adjust PWM\n"
" d - Flip PWM direction\n"
" space - Zero PWM\n"
" as - Max PWM\n"
" e - Display encoders\n"
" q - Back";
puts_P(msg);
break;
}
if (PWM > motor_maxPWM)
PWM = motor_maxPWM;
else if (PWM < -motor_maxPWM)
PWM = -motor_maxPWM;
printf_P(PSTR("PWM %d\n"), PWM);
}
for (uint8_t i=0; i<4; i++)
motor_setPWM(i, motenables[i] ? PWM : 0);
}
}
void controlpanel_drive() {
int16_t steer = 0;
while (true) {
char ch=controlpanel_promptChar("Drive");
switch (ch) {
case ' ':
drive_stop();
speed = 0;
steer = 0;
break;
case 'w':
steer = 0;
speed += speed_incrementer;
printf_P(PSTR("Speed: %f\n"), speed);
drive_fd(speed);
break;
case 'a':
steer -= steer_incrementer;
drive_steer(steer, speed);
break;
case 's':
steer = 0;
speed -= speed_incrementer;
printf_P(PSTR("Speed: %f\n"), speed);
drive_fd(speed); // speed will be negative!!
break;
case 'd':
steer += steer_incrementer;
drive_steer(steer, speed);
break;
case 'W':
steer = 0;
speed += large_speed_incrementer;
printf_P(PSTR("Speed: %f\n"), speed);
drive_fd(speed);
break;
case 'A':
drive_lturn(setSpeed - turn_reducer);
break;
case 'S':
steer = 0;
speed -= large_speed_incrementer;
printf_P(PSTR("Speed: %f\n"), speed);
drive_fd(speed); // speed will be negative!!
break;
case 'D':
drive_rturn(setSpeed - turn_reducer);
break;
case 'k':
weedwhacker_power(false);
break;
case 'K':
weedwhacker_power(true);
break;
case '=':
setSpeed += 100;
printf_P(PSTR("Speed: %f\n"), setSpeed);
break;
case '-':
setSpeed -= 100;
printf_P(PSTR("Speed: %f\n"), setSpeed);
break;
case '+':
setSpeed += 10;
printf_P(PSTR("Speed: %f\n"), setSpeed);
break;
case '_':
setSpeed -= 10;
printf_P(PSTR("Speed: %f\n"), setSpeed);
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 '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"
" k - Weedwhacker Kill\n"
" K - Weedwhacker Start\n"
" -=_+ - Adjust speed\n"
" WASD - Full speed movements\n"
" c - Disable motor control\n"
" Pp - Enable/Disable motor control debug\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;
}
}
}
