bool nav_linefollowIntersection() { while (true) { if (!nav_linefollow()) return false; if (linefollow_getLastFeature() == FEATURE_INTERSECTION) return true; else if (linefollow_getLastTurn() == TURN_LEFT) { drive_lturn(50, DM_BANG); linefollow_waitLine(2, 7); } else if (linefollow_getLastTurn() == TURN_RIGHT) { drive_rturn(50, DM_BANG); linefollow_waitLine(0, 5); } else return false; } }
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_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; } } }