Skip to content

Refactor DriveTowardsThing command to use constants for drive speeds and timeout cycles #34

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
gbuckholtz wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Refactor DriveTowardsThing command to use constants for drive speeds and timeout cycles #34

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 17 additions & 6 deletions src/main/java/frc/robot/commands/auto/DriveTowardsThing.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,17 @@ public class DriveTowardsThing extends Command {
private static final int SLIGHT_LEFT_BOUND = 156;
private static final int STRAIGHT_BOUND = 164;
private static final int SLIGHT_RIGHT_BOUND = 265;

// Drive speed constants
private static final double FORWARD_SPEED = -0.5;
private static final double LEFT_ROTATION_SPEED = -0.1;
private static final double SLIGHT_LEFT_ROTATION_SPEED = -0.05;
private static final double SLIGHT_RIGHT_ROTATION_SPEED = 0.05;
private static final double RIGHT_ROTATION_SPEED = 0.1;
private static final double STOPPED = 0.0;

// Timeout constant
private static final int STRAIGHT_TIMEOUT_CYCLES = 150;

public DriveTowardsThing(Drivetrain drivetrain, Camera camera) {
addRequirements(drivetrain, camera);
Expand Down Expand Up @@ -72,20 +83,20 @@ public void execute() {
if (straightTimeout <= 0) {
long x = camera.getX();
if (x < LEFT_BOUND) {
drivetrain.driveRobotRelative(0.0, -0.1, 0.0);
drivetrain.driveRobotRelative(STOPPED, LEFT_ROTATION_SPEED, STOPPED);
System.out.println("AUTO - DRIVETOWARDSRING: LEFT");
} else if (x >= LEFT_BOUND && x < SLIGHT_LEFT_BOUND) {
drivetrain.driveRobotRelative(0.0, -0.05, 0.0);
drivetrain.driveRobotRelative(STOPPED, SLIGHT_LEFT_ROTATION_SPEED, STOPPED);
System.out.println("AUTO - DRIVETOWARDSRING: SLIGHT LEFT");
} else if (x >= SLIGHT_LEFT_BOUND && x < STRAIGHT_BOUND) {
drivetrain.driveRobotRelative(-0.5, 0.0, 0.0);
straightTimeout = 150;
drivetrain.driveRobotRelative(FORWARD_SPEED, STOPPED, STOPPED);
straightTimeout = STRAIGHT_TIMEOUT_CYCLES;
System.out.println("AUTO - DRIVETOWARDSRING: STRAIGHT");
} else if (x >= STRAIGHT_BOUND && x < SLIGHT_RIGHT_BOUND) {
drivetrain.driveRobotRelative(0.0, 0.05, 0.0);
drivetrain.driveRobotRelative(STOPPED, SLIGHT_RIGHT_ROTATION_SPEED, STOPPED);
System.out.println("AUTO - DRIVETOWARDSRING: SLIGHT RIGHT");
} else {
drivetrain.driveRobotRelative(0.0, 0.1, 0.0);
drivetrain.driveRobotRelative(STOPPED, RIGHT_ROTATION_SPEED, STOPPED);
System.out.println("AUTO - DRIVETOWARDSRING: RIGHT");
}
} else {
Expand Down