Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-09 08:48:04 -06:00
Code Issues Packages Projects Releases Wiki Activity

After-cougars-scrimmage

Browse Source
This commit is contained in:
Michael Mikovsky
2025-02-24 17:10:12 -07:00
parent 460d4fbbd4
commit 761a56cf0f
6 changed files with 137 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/pathplanner/autos/Line-up-no-arm.auto
+25
View File
@@ -0,0 +1,25 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Top Blue Cage to Reef"
}
},
{
"type": "named",
"data": {
"name": "lineup-no-arm"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}
src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path
+8 -8
View File
@@ -8,20 +8,20 @@
},
"prevControl": null,
"nextControl": {
"x": 7.270707070707072,
"y": 7.013257575757576
"x": 6.599873737373737,
"y": 8.029671717171716
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 5.309027777777779,
"y": 5.3768308080808085
"x": 5.464431818181818,
"y": 5.6403977272727275
},
"prevControl": {
"x": 4.945156854248459,
"y": 5.08761998976538
"x": 5.240820707070706,
"y": 5.254160353535354
},
"nextControl": null,
"isLocked": false,
@@ -42,13 +42,13 @@
},
"goalEndState": {
"velocity": 0,
"rotation": 60.255118703057796
"rotation": -122.99770510121631
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -2.223961240385466
"rotation": 180.0
},
"useDefaultConstraints": true
}
src/main/java/frc4388/robot/RobotContainer.java
+66 -32
View File
@@ -117,38 +117,48 @@ public class RobotContainer {
// true, false);
// private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision);
private Command AprilLidarAlignL4Right = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
// new InstantCommand(() -> System.out.println("Soup")),
// new WaitCommand(1),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1 - Units.inchesToMeters(0), Side.RIGHT),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.RIGHT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
);
private Command AprilLidarAlignL4Left = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
// new InstantCommand(() -> System.out.println("Soup")),
// new WaitCommand(1),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1 - Units.inchesToMeters(0), Side.LEFT),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true),
new Translation2d(0,1), new Translation2d(), 500, true),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
);
private Command AprilLidarAlignL3Left = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.LEFT),
// new InstantCommand(() -> System.out.println("Soup")),
@@ -166,6 +176,7 @@ public class RobotContainer {
);
private Command AprilLidarAlignL3Right = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT),
@@ -187,6 +198,7 @@ public class RobotContainer {
);
private Command AprilLidarAlignL2Left = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
@@ -200,6 +212,7 @@ public class RobotContainer {
);
private Command AprilLidarAlignL2Right = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.RIGHT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
@@ -231,6 +244,7 @@ public class RobotContainer {
);
private Command leftAlgaeRemove = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
@@ -243,6 +257,7 @@ public class RobotContainer {
);
private Command rightAlgaeRemove = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
@@ -261,9 +276,18 @@ public class RobotContainer {
*/
public RobotContainer() {
NamedCommands.registerCommand("AutoGotoPosition", AprilLidarAlignL4Right);
NamedCommands.registerCommand("april-allign", AprilLidarAlignL4Right);
NamedCommands.registerCommand("place-coral", placeCoral);
NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left);
NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right);
NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL3Left);
NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL3Right);
NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL2Left);
NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL2Right);
NamedCommands.registerCommand("lineup-no-arm", new SequentialCommandGroup(
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar)
));
NamedCommands.registerCommand("grab-coral", grabCoral);
configureButtonBindings();
@@ -333,10 +357,6 @@ public class RobotContainer {
*/
private void configureButtonBindings() {
// ? /* Test button box bindings */
new JoystickButton(getButtonBox(), ButtonBox.White)
.onTrue(new InstantCommand(() -> System.out.println("White!")));
// ? /* Driver Buttons */
DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
@@ -360,18 +380,18 @@ public class RobotContainer {
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
.onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepUp()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 0)
.onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp()));
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180)
.onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepDown()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 180)
.onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 90)
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp()));
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90)
.onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepUp()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 270)
.onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepDown()));
@@ -419,23 +439,35 @@ public class RobotContainer {
new JoystickButton(getButtonBox(), ButtonBox.Four)
.onTrue(rightAlgaeRemove);
// Cancel button
new JoystickButton(getButtonBox(), ButtonBox.White)
.onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endefectorStop();}, m_robotElevator));
// Score prep
// Prime 4
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator));
// Prime 3
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator));
//Trims
new Trigger(() -> getDeadbandedDriverController().getPOV() == 0)
.onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp()));
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
.onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepUp()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 180)
.onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown()));
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180)
.onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepDown()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 90)
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90)
.onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepUp()));
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 270)
.onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepDown()));
// ? /* Programer Buttons (Controller 3)*/
@@ -488,16 +520,18 @@ public class RobotContainer {
*/
public Command getAutonomousCommand() {
//return autoPlayback;
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision)
//return autoChooser.getSelected();
// try{
// // Load the path you want to follow using its name in the GUI
return autoCommand;
// // // Load the path you want to follow using its name in the GUI
// return autoCommand;
// } catch (Exception e) {
// DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
// return Commands.none();
return Commands.none();
// }
// return new PathPlannerAuto("Line-up-no-arm");
// zach told me to do the below comment
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
// return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos);
src/main/java/frc4388/robot/commands/GotoLastApril.java
+24 -3
View File
@@ -1,9 +1,13 @@
package frc4388.robot.commands;
import java.util.Optional;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
@@ -51,6 +55,8 @@ public class GotoLastApril extends Command {
tagRelativeXError = val;
}
Alliance alliance = null;
@Override
public void initialize() {
xPID.initialize();
@@ -58,6 +64,9 @@ public class GotoLastApril extends Command {
this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), side,
Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()),
distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get()));
Optional<Alliance> a = DriverStation.getAlliance();
if(!a.isEmpty())
alliance = a.get();
}
double xerr;
@@ -66,8 +75,18 @@ public class GotoLastApril extends Command {
@Override
public void execute() {
xerr = targetpos.getX() - vision.getPose2d().getX();
yerr = targetpos.getY() - vision.getPose2d().getY();
if (alliance == Alliance.Red) {
xerr = -(targetpos.getX() - vision.getPose2d().getX());
yerr = (targetpos.getY() - vision.getPose2d().getY());
}
else{
xerr = targetpos.getX() - vision.getPose2d().getX();
yerr = targetpos.getY() - vision.getPose2d().getY();
}
roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees();
// SmartDashboard.putNumber("PID X Error", xerr);
@@ -80,8 +99,8 @@ public class GotoLastApril extends Command {
xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1;
// System.out.println(xerr + ", " + yerr + ", " + roterr + ", " + rotoutput);
Translation2d leftStick = new Translation2d(
Math.max(Math.min(-youtput, 1), -1),
@@ -94,6 +113,8 @@ public class GotoLastApril extends Command {
0
);
setTagRelativeXError(
new Translation2d(xerr, yerr).getAngle()
.rotateBy(targetpos.getRotation())
src/main/java/frc4388/robot/subsystems/Elevator.java
+1 -1
View File
@@ -227,7 +227,7 @@ public class Elevator extends SubsystemBase {
SmartDashboard.putString("State", currentState.toString());
if (currentState == CoordinationState.Waiting) {
periodicWaiting();
// periodicWaiting();
} else if (currentState == CoordinationState.WatingBeamTriped) {
// periodicWaitingTripped();
} else if (currentState == CoordinationState.Ready) {
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+13 -3
View File
@@ -141,10 +141,20 @@ public class SwerveDrive extends Subsystem {
return; // don't bother doing swerve drive math and return early.
leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
if (SwerveDriveConstants.INVERT_X) leftStick = new Translation2d(-leftStick.getX(), leftStick.getY());
if (SwerveDriveConstants.INVERT_Y) leftStick = new Translation2d(leftStick.getX(), -leftStick.getY());
Optional<Alliance> alliance = DriverStation.getAlliance();
if(!alliance.isEmpty()){
if (alliance.get() == Alliance.Red)
leftStick = new Translation2d(-leftStick.getX(), leftStick.getY());
else
leftStick = new Translation2d(leftStick.getX(), -leftStick.getY());
// if (alliance.get() != Alliance.Red) leftStick = new Translation2d(leftStick.getX(), -leftStick.getY());
}
if (SwerveDriveConstants.INVERT_ROTATION) rightStick.times(-1);
stopped = false;
if (fieldRelative) {
// ! drift correction