Skip to content

Commit

Permalink
After math 54 - make shooting faster
Browse files Browse the repository at this point in the history
  • Loading branch information
@team6101
team6101 committed Mar 16, 2024
1 parent eb95b4b commit 62e7a51
Show file tree
Hide file tree
Showing 5 changed files with 71 additions and 48 deletions.
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,7 @@ public class Constants {
public static final double APRIL_TAG_ROTATION_ZONE = 5;

/*PID GAINS *///0.00001, 100
public static final Gains ROTATION_GAINS = new Gains(0.011, 0.0001, 0.000935, 0, 0, 0.3);
public static final Gains ROTATION_GAINS = new Gains(0.015, 0.0001, 0.000935, 0, 0, 0.3);
public static final double ROTATION_DEGREE_TOLERANCE = 3;
public static final double ROTATION_DEGREE_PER_SECOND_TOLERANCE = 30;
public static final Gains NORMAL_ROBOT_GAINS = new Gains(0.0005, 0.0, 0, 0, 0, 1.);
Expand Down
63 changes: 33 additions & 30 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -126,36 +126,7 @@ public void robotInit() {

// aprilTagHighlighter = new AprilTagHighlighter();
autonRouteChooser.setDefaultOption("move forward", AutonRoutes.GO_FORWARD_OUT_OF_STARTING_ZONE);
autonRouteChooser.addOption("move forward", AutonRoutes.GO_FORWARD_OUT_OF_STARTING_ZONE);
autonRouteChooser.addOption("move backwards", AutonRoutes.GO_BACKWARD_OUT_OF_STARTING_ZONE);
autonRouteChooser.addOption("just shoot", AutonRoutes.JUST_SHOOT);

autonRouteChooser.addOption("shoot and back up", AutonRoutes.SHOOT_AND_BACK_UP_FROM_CENTER);
autonRouteChooser.addOption("shoot and back up forever", AutonRoutes.SHOOT_AND_BACK_UP_SKETCHILY);

autonRouteChooser.addOption(
"shoot backup rotate backup from amp side",
AutonRoutes.SHOOT_BACK_UP_ROTATE_FROM_AMP_SIDE
);
autonRouteChooser.addOption(
"shoot backup rotate backup from source side",
AutonRoutes.SHOOT_BACK_UP_ROTATE_FROM_TERMINAL_SIDE
);

autonRouteChooser.addOption("backup turn backup", AutonRoutes.BACKUP_TURN_BACKUP);
autonRouteChooser.addOption("explode hidden bomb", AutonRoutes.BOOM);
autonRouteChooser.addOption("shoot backup intake forward shoot", AutonRoutes.SHOOT_BACKUP_INTAKE_FORWARD_SHOOT);

autonRouteChooser.addOption("Test old rotation PID", AutonRoutes.TEST_ROTATION);
autonRouteChooser.addOption("Test new rotation PID", AutonRoutes.TEST_ROTATION_WITH_PID_COMMAND);
autonRouteChooser.addOption("Test SmartMotion movement", AutonRoutes.TEST_SMART_MOTION_MOVEMENT);
autonRouteChooser.addOption(
"Test SmartMotion backward movement",
AutonRoutes.TEST_SMART_MOTION_BACKWARD_MOVEMENT
);
autonRouteChooser.addOption("Test Manual PID movement", AutonRoutes.TEST_PID_MOVEMENT);
autonRouteChooser.addOption("Test Manual Backward PID movement", AutonRoutes.TEST_PID_BACKWARD_MOVEMENT);

setupAutonRoutes();
//This is an emergency button for switching controllers mid match
SmartDashboard.putData(
"SWITCH CONTROLLERS",
Expand Down Expand Up @@ -260,6 +231,38 @@ public boolean isFinished() {
System.out.println("OTHER GYRO: " + gyro.getAngle());
}

public void setupAutonRoutes() {
autonRouteChooser.addOption("move forward", AutonRoutes.GO_FORWARD_OUT_OF_STARTING_ZONE);
autonRouteChooser.addOption("move backwards", AutonRoutes.GO_BACKWARD_OUT_OF_STARTING_ZONE);
autonRouteChooser.addOption("just shoot", AutonRoutes.JUST_SHOOT);

autonRouteChooser.addOption("shoot and back up", AutonRoutes.SHOOT_AND_BACK_UP_FROM_CENTER);
autonRouteChooser.addOption("shoot and back up forever", AutonRoutes.SHOOT_AND_BACK_UP_SKETCHILY);

autonRouteChooser.addOption(
"shoot backup rotate backup from amp side",
AutonRoutes.SHOOT_BACK_UP_ROTATE_FROM_AMP_SIDE
);
autonRouteChooser.addOption(
"shoot backup rotate backup from source side",
AutonRoutes.SHOOT_BACK_UP_ROTATE_FROM_TERMINAL_SIDE
);

autonRouteChooser.addOption("backup turn backup", AutonRoutes.BACKUP_TURN_BACKUP);
autonRouteChooser.addOption("explode hidden bomb", AutonRoutes.BOOM);
autonRouteChooser.addOption("shoot backup intake forward shoot", AutonRoutes.SHOOT_BACKUP_INTAKE_FORWARD_SHOOT);

autonRouteChooser.addOption("Test old rotation PID", AutonRoutes.TEST_ROTATION);
autonRouteChooser.addOption("Test new rotation PID", AutonRoutes.TEST_ROTATION_WITH_PID_COMMAND);
autonRouteChooser.addOption("Test SmartMotion movement", AutonRoutes.TEST_SMART_MOTION_MOVEMENT);
autonRouteChooser.addOption(
"Test SmartMotion backward movement",
AutonRoutes.TEST_SMART_MOTION_BACKWARD_MOVEMENT
);
autonRouteChooser.addOption("Test Manual PID movement", AutonRoutes.TEST_PID_MOVEMENT);
autonRouteChooser.addOption("Test Manual Backward PID movement", AutonRoutes.TEST_PID_BACKWARD_MOVEMENT);
}

/*
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
* that you want ran during disabled, autonomous, teleoperated and test.
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/auton/AutonMoveInches.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ public boolean isDone() {
isDoneDebounceTime = 0;
}

if (isDoneDebounceTime > 1.5) {
if (isDoneDebounceTime > 0.5) {
System.out.println("We moved the correct amount of inches!");
return true;
}
Expand Down
35 changes: 20 additions & 15 deletions src/main/java/frc/robot/auton/AutonRoutes.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,16 +17,21 @@ public class AutonRoutes {

public static final ArrayDeque<AutonAction> JUST_SHOOT = new ArrayDeque<AutonAction>(List.of(new AutonShoot()));

public static ArrayDeque<AutonAction> SHOOT_BACK_UP_ROTATE_FROM_TERMINAL_SIDE;
public static ArrayDeque<AutonAction> SHOOT_BACK_UP_ROTATE_FROM_TERMINAL_SIDE = new ArrayDeque<>();

public static ArrayDeque<AutonAction> SHOOT_BACK_UP_ROTATE_FROM_AMP_SIDE;
public static ArrayDeque<AutonAction> SHOOT_BACK_UP_ROTATE_FROM_AMP_SIDE = new ArrayDeque<>();

public static final ArrayDeque<AutonAction> SHOOT_BACK_UP_ROTATE_FROM_TERMINAL_SIDE_RED = new ArrayDeque<
AutonAction
>(List.of(new AutonShoot(), new AutonMoveInches(-84)));

public static final ArrayDeque<AutonAction> SHOOT_BACK_UP_ROTATE_FROM_AMP_SIDE_RED = new ArrayDeque<AutonAction>(
List.of(new AutonShoot(), new AutonMoveInches(-40), new AutonRotate(-45), new AutonMoveInches(-65))
List.of(
new AutonShoot(),
new AutonMoveInches(-40),
new AutonRotateWithPIDCommand(-45),
new AutonMoveInches(-65)
)
);

public static final ArrayDeque<AutonAction> SHOOT_ROTATE_BACK_UP_FROM_TERMINAL_SIDE_BLUE = new ArrayDeque<
Expand All @@ -45,7 +50,7 @@ public class AutonRoutes {
List.of(new AutonShoot(), new AutonMoveInches(-84))
);

public static ArrayDeque<AutonAction> BACKUP_TURN_BACKUP;
public static ArrayDeque<AutonAction> BACKUP_TURN_BACKUP = new ArrayDeque<>();

private static final ArrayDeque<AutonAction> BLUE_BACKUP_TURN_BACKUP = new ArrayDeque<AutonAction>(
List.of(new AutonMoveInches(-24), new AutonRotate(-135), new AutonMoveInches(36))
Expand All @@ -55,9 +60,9 @@ public class AutonRoutes {
List.of(new AutonMoveInches(-24), new AutonRotate(135), new AutonMoveInches(36))
);

public static ArrayDeque<AutonAction> MESS_UP_CENTER_RINGS_FROM_AMP_SIDE;
public static ArrayDeque<AutonAction> MESS_UP_CENTER_RINGS_FROM_AMP_SIDE = new ArrayDeque<>();

public static ArrayDeque<AutonAction> MESS_UP_CENTER_RINGS_FROM_TERMINAL_SIDE;
public static ArrayDeque<AutonAction> MESS_UP_CENTER_RINGS_FROM_TERMINAL_SIDE = new ArrayDeque<>();

private static final ArrayDeque<AutonAction> MESS_UP_CENTER_RINGS_TURN_LEFT = new ArrayDeque<AutonAction>(
List.of(new AutonMoveInches(312), new AutonRotate(-90), new AutonMoveInches(252))
Expand All @@ -70,18 +75,18 @@ public class AutonRoutes {
/**Paths will slightly differ based on what team we're on, so these will correct them. */
public static void setupCorrectAutonPaths() {
if (DriverStation.getAlliance().get() == Alliance.Red) {
BACKUP_TURN_BACKUP = RED_BACKUP_TURN_BACKUP;
MESS_UP_CENTER_RINGS_FROM_AMP_SIDE = MESS_UP_CENTER_RINGS_TURN_LEFT;
MESS_UP_CENTER_RINGS_FROM_TERMINAL_SIDE = MESS_UP_CENTER_RINGS_TURN_RIGHT;
SHOOT_BACK_UP_ROTATE_FROM_TERMINAL_SIDE = SHOOT_BACK_UP_ROTATE_FROM_TERMINAL_SIDE_RED;
SHOOT_BACK_UP_ROTATE_FROM_AMP_SIDE = SHOOT_BACK_UP_ROTATE_FROM_AMP_SIDE_RED;
BACKUP_TURN_BACKUP.addAll(RED_BACKUP_TURN_BACKUP);
MESS_UP_CENTER_RINGS_FROM_AMP_SIDE.addAll(MESS_UP_CENTER_RINGS_TURN_LEFT);
MESS_UP_CENTER_RINGS_FROM_TERMINAL_SIDE.addAll(MESS_UP_CENTER_RINGS_TURN_RIGHT);
SHOOT_BACK_UP_ROTATE_FROM_TERMINAL_SIDE.addAll(SHOOT_BACK_UP_ROTATE_FROM_TERMINAL_SIDE_RED);
SHOOT_BACK_UP_ROTATE_FROM_AMP_SIDE.addAll(SHOOT_BACK_UP_ROTATE_FROM_AMP_SIDE_RED);
System.out.println("Configuring auton routes for RED team");
} else {
BACKUP_TURN_BACKUP = BLUE_BACKUP_TURN_BACKUP;
MESS_UP_CENTER_RINGS_FROM_AMP_SIDE = MESS_UP_CENTER_RINGS_TURN_RIGHT;
MESS_UP_CENTER_RINGS_FROM_TERMINAL_SIDE = MESS_UP_CENTER_RINGS_TURN_LEFT;
SHOOT_BACK_UP_ROTATE_FROM_TERMINAL_SIDE = SHOOT_ROTATE_BACK_UP_FROM_TERMINAL_SIDE_BLUE;
SHOOT_BACK_UP_ROTATE_FROM_AMP_SIDE = SHOOT_BACK_UP_ROTATE_FROM_AMP_SIDE_BLUE;
MESS_UP_CENTER_RINGS_FROM_AMP_SIDE.addAll(MESS_UP_CENTER_RINGS_TURN_RIGHT);
MESS_UP_CENTER_RINGS_FROM_TERMINAL_SIDE.addAll(MESS_UP_CENTER_RINGS_TURN_LEFT);
SHOOT_BACK_UP_ROTATE_FROM_TERMINAL_SIDE.addAll(SHOOT_ROTATE_BACK_UP_FROM_TERMINAL_SIDE_BLUE);
SHOOT_BACK_UP_ROTATE_FROM_AMP_SIDE.addAll(SHOOT_BACK_UP_ROTATE_FROM_AMP_SIDE_BLUE);
System.out.println("Configuring auton routes for BLUE team");
}
}
Expand Down
17 changes: 16 additions & 1 deletion src/main/java/frc/robot/auton/RotatePID.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,8 @@

public class RotatePID extends PIDCommand {

double targetDegree;

public RotatePID(double relativeTurnDegree) {
super(
// The controller that the command will use
Expand All @@ -21,16 +23,29 @@ public RotatePID(double relativeTurnDegree) {
QuickActions.turn(output);
}
);
targetDegree = Robot.getGyroscope().getAngle() + relativeTurnDegree;
// Use addRequirements() here to declare subsystem dependencies.
// getController().enableContinuousInput(-180, 180);
// Configure additional PID options by calling `getController` here.
getController()
.setTolerance(Constants.ROTATION_DEGREE_TOLERANCE, Constants.ROTATION_DEGREE_PER_SECOND_TOLERANCE);

System.out.println("Current degree: " + Robot.getGyroscope().getAngle() + " target: " + targetDegree);
}

int doneDebounceTime = 0;

// Returns true when the command should end.
@Override
public boolean isFinished() {
return getController().atSetpoint();
System.out.println(
"current: " + Robot.getGyroscope().getAngle() + " target:" + targetDegree + " debounce: " + doneDebounceTime
);
if (Math.abs(Robot.getGyroscope().getAngle() - targetDegree) < Constants.ROTATION_DEGREE_TOLERANCE) {
doneDebounceTime++;
} else {
doneDebounceTime = 0;
}
return doneDebounceTime > 25;
}
}

0 comments on commit 62e7a51

Please sign in to comment.