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;
}
}
}
