Team2890/Mothman
4
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Final adjustments

Browse Source
This commit is contained in:
Mehooliu
2026-03-05 19:52:32 -05:00
parent cc41ea3f1b
commit 0e5561dba0
13 changed files with 183 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files

79
src/main/deploy/pathplanner/autos/HP Auto + Climb With Auto Aim + Auto Climb.auto Normal file
View File

@@ -0,0 +1,79 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Deploy_Intake_Command"
}
},
{
"type": "path",
"data": {
"pathName": "Starting to HP"
}
},
{
"type": "wait",
"data": {
"waitTime": 1.5
}
},
{
"type": "path",
"data": {
"pathName": "HP to Hub"
}
},
{
"type": "named",
"data": {
"name": "Auto_Aim_To_Hub"
}
},
{
"type": "named",
"data": {
"name": "Shoot_Fuel_Command"
}
},
{
"type": "wait",
"data": {
"waitTime": 3.0
}
},
{
"type": "named",
"data": {
"name": "Kill_All"
}
},
{
"type": "named",
"data": {
"name": "PathPlan_To_Climb_Right_Offsetted"
}
},
{
"type": "named",
"data": {
"name": "PathPlan_Into_Climb_Right"
}
},
{
"type": "named",
"data": {
"name": "Lift_Robot"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

14
src/main/deploy/pathplanner/autos/HP Auto + Climb.auto
View File

@@ -34,6 +34,12 @@
"name": "Shoot_Fuel_Command"
}
},
{
"type": "wait",
"data": {
"waitTime": 1.0
}
},
{
"type": "named",
"data": {
@@ -43,7 +49,7 @@
{
"type": "wait",
"data": {
"waitTime": 3.0
"waitTime": 2.0
}
},
{
@@ -64,12 +70,6 @@
"pathName": "Side into Climb"
}
},
{
"type": "wait",
"data": {
"waitTime": 0.5
}
},
{
"type": "named",
"data": {

10
src/main/deploy/pathplanner/paths/HP to Hub.path
View File

@@ -3,13 +3,13 @@
"waypoints": [
{
"anchor": {
"x": 0.5,
"y": 0.65
"x": 0.447,
"y": 0.634
},
"prevControl": null,
"nextControl": {
"x": 0.8535533905932737,
"y": 1.0035533905932739
"x": 0.8005533905932738,
"y": 0.9875533905932738
},
"isLocked": false,
"linkedName": null
@@ -42,7 +42,7 @@
},
"goalEndState": {
"velocity": 0,
"rotation": 38.99099404250546
"rotation": 27.207
},
"reversed": false,
"folder": "Start to HP to Hub to Climb",

6
src/main/deploy/pathplanner/paths/Hub to side of Climb.path
View File

@@ -16,11 +16,11 @@
},
{
"anchor": {
"x": 1.08,
"x": 1.25,
"y": 2.0457840909090907
},
"prevControl": {
"x": 1.5498463103929543,
"x": 1.7198463103929542,
"y": 2.2167941625719254
},
"nextControl": null,
@@ -48,7 +48,7 @@
"folder": "Start to HP to Hub to Climb",
"idealStartingState": {
"velocity": 0,
"rotation": 22.013232308372665
"rotation": 27.207
},
"useDefaultConstraints": true
}

10
src/main/deploy/pathplanner/paths/Side into Climb.path
View File

@@ -3,24 +3,24 @@
"waypoints": [
{
"anchor": {
"x": 1.08,
"x": 1.25,
"y": 2.046
},
"prevControl": null,
"nextControl": {
"x": 1.0861477272727271,
"y": 2.5817386363636365
"x": 1.302590909090909,
"y": 2.6126590909090908
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.25,
"x": 1.37,
"y": 3.5
},
"prevControl": {
"x": 1.256147727272727,
"x": 1.3761477272727272,
"y": 3.190795454545455
},
"nextControl": null,

4
src/main/deploy/pathplanner/paths/Starting to HP.path
View File

@@ -16,11 +16,11 @@
},
{
"anchor": {
"x": 0.4471249999999998,
"x": 0.457431818181818,
"y": 0.63375
},
"prevControl": {
"x": 0.999027242432462,
"x": 1.0093340606142802,
"y": 0.6337499999999999
},
"nextControl": null,

18
src/main/java/frc/robot/Constants.java
View File

@@ -7,6 +7,8 @@ package frc.robot;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import com.pathplanner.lib.path.PathConstraints;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Pose2d;
@@ -98,7 +100,7 @@ public final class Constants {
* .withWidget(BuiltInWidgets.kNumberBar).getEntry();
*/
public static double INDEXER_AND_RAMP_MOTOR_RPM = 20000;
public static double INDEXER_AND_RAMP_MOTOR_RPM = 30000;
// this method called in robot periodic so values updated in elastic are
// constantly read and applied to RAMP_MOTOR_SPEED
@@ -128,16 +130,16 @@ public final class Constants {
public static final int INTAKE_ROTATOR_MOTOR_ID = 51;
public static class IntakeRotatorPID {
public static final double INTAKE_ROTATOR_P = .1;
public static final double INTAKE_ROTATOR_I = 0.0;
public static final double INTAKE_ROTATOR_D = 0.;
public static final double INTAKE_ROTATOR_P = .08;
public static final double INTAKE_ROTATOR_I = 0;
public static final double INTAKE_ROTATOR_D = 0.01;
}
public static final double INTAKE_MOTOR_P = 0.0001;
public static final double INTAKE_MOTOR_I = 0;
public static final double INTAKE_MOTOR_D = 0;
public static final double INTAKE_COLLECT_ENCODER_VALUE = -0.2;
public static final double INTAKE_COLLECT_ENCODER_VALUE = 1.4;
public static final double INTAKE_MIDDLE_ENCODER_VALUE = -3;
public static final double INTAKE_RETRACT_ENCODER_VALUE = -5.5;
@@ -175,6 +177,8 @@ public final class Constants {
public static Rotation2d HUB_ROTATION_BLUE;
public static Rotation2d HUB_ROTATION_RED;
public static Pose2d allianceHubPose = new Pose2d();
public static final double HUB_X_POSE_BLUE = 4.625;
public static final double HUB_Y_POSE_BLUE = 4.03;
@@ -182,10 +186,12 @@ public final class Constants {
public static final double HUB_X_POSE_RED = 11.915;
public static final double HUB_Y_POSE_RED = 4.03;
public static PathConstraints DRIVE_INTO_CLIMB_CONSTRAINTS;
public static final Transform3d ROBOT_TO_BACK_LEFT_CAM = new Transform3d(
new Translation3d(-Units.inchesToMeters(12.75), Units.inchesToMeters(6.25),
Units.inchesToMeters(13.1875)),
new Rotation3d(0, 0, Math.toRadians(205)));
new Rotation3d(0, 0, Math.toRadians(195)));
public static final Transform3d ROBOT_TO_BACK_RIGHT_CAM = new Transform3d(
new Translation3d(-Units.inchesToMeters(12.75), -Units.inchesToMeters(6.25), Units.inchesToMeters(14)),

3
src/main/java/frc/robot/Robot.java
View File

@@ -6,6 +6,7 @@ package frc.robot;
import java.util.logging.Logger;
import org.photonvision.PhotonUtils;
import org.photonvision.targeting.PhotonPipelineResult;
import com.pathplanner.lib.auto.AutoBuilder;
@@ -107,7 +108,7 @@ public class Robot extends TimedRobot {
TargetingSubsystems.updateShooterRPM(m_robotContainer.getSwerveDrive().getPose());
SmartDashboard.putNumber("Estimated Shooter RPM", Constants.ShooterConstants.SHOOTER_RPM);
SmartDashboard.putNumber("Distance From Hub: ", PhotonUtils.getDistanceToPose(m_robotContainer.getSwerveDrive().getPose(), Constants.TargetingConstants.allianceHubPose));
//Constants.ShooterConstants.getRampAndIndexerMotorSpeed();
//Constants.IntakeConstants.updateIntakeWheelsRPM();
//Constants.ShooterConstants.updateShooterRPM();

14
src/main/java/frc/robot/RobotContainer.java
View File

@@ -143,13 +143,19 @@ public class RobotContainer {
// Create the NamedCommands that will be used in PathPlanner
NamedCommands.registerCommand("test", Commands.print("I EXIST"));
NamedCommands.registerCommand("Shoot_Fuel_Command",
m_ShooterSubsystem.shootFuelCommand()
m_ShooterSubsystem.setShooterMotorsRPMAutoCommand()
.andThen(m_IntakeSubsystem.startIntakeMotorCommand()));
NamedCommands.registerCommand("Deploy_Intake_Command", m_IntakeSubsystem.deployIntakeCommand());
NamedCommands.registerCommand("Stop_Shooter_Command", m_ShooterSubsystem.stopShooterCommand());
NamedCommands.registerCommand("Lift_Robot_Command", m_ClimberSubsystem.liftRobotCommand());
NamedCommands.registerCommand("Assist_Shooter", m_IntakeSubsystem.assistFuelIntakeCommand());
NamedCommands.registerCommand("Lift_Robot", m_ClimberSubsystem.liftRobotCommand());
NamedCommands.registerCommand("Kill_All", killAllCommand());
NamedCommands.registerCommand("Auto_Aim_To_Hub", m_TargetingSubsystems.aimAtHubPose(drivebase, driverXbox));
NamedCommands.registerCommand("PathPlan_To_Climb_Right_Offsetted", drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_RIGHT_CLIMB_POSE_OFFSETTED, Constants.TargetingConstants.RED_RIGHT_CLIMB_POSE_OFFSETTED));
NamedCommands.registerCommand("PathPlan_To_Climb_Left_Offsetted", drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_LEFT_CLIMB_POSE_OFFSETTED, Constants.TargetingConstants.RED_LEFT_CLIMB_POSE_OFFSETTED));
NamedCommands.registerCommand("PathPlan_Into_Climb_Right", drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_RIGHT_CLIMB_POSE, Constants.TargetingConstants.RED_RIGHT_CLIMB_POSE));
NamedCommands.registerCommand("PathPlan_Into_Climb_Left", drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_LEFT_CLIMB_POSE, Constants.TargetingConstants.RED_LEFT_CLIMB_POSE));
// Have the autoChooser pull in all PathPlanner autos as options
@@ -230,12 +236,12 @@ public class RobotContainer {
operatorXbox.a().whileTrue(m_ShooterSubsystem.setIndexerAndRampMotorRPMCommand())
.onFalse(m_ShooterSubsystem.stopIndexerAndRampMotorCommand());
topButtons.axisGreaterThan(1, 0.3).toggleOnTrue(m_IntakeSubsystem.rotateIntakeCommand(Constants.IntakeConstants.INTAKE_MANUAL_SPEED * -1)).toggleOnFalse(m_IntakeSubsystem.rotateIntakeCommand(0));
topButtons.axisGreaterThan(1, 0.3).toggleOnTrue(m_IntakeSubsystem.rotateIntakeCommand(Constants.IntakeConstants.INTAKE_MANUAL_SPEED * -5)).toggleOnFalse(m_IntakeSubsystem.rotateIntakeCommand(0));
topButtons.axisGreaterThan(1, -0.3).toggleOnTrue(m_IntakeSubsystem.rotateIntakeCommand(Constants.IntakeConstants.INTAKE_MANUAL_SPEED)).toggleOnFalse(m_IntakeSubsystem.rotateIntakeCommand(0));
topButtons.button(3).onTrue(killAllCommand());
topButtons.button(6).whileTrue(m_TargetingSubsystems.aimAtHubPose(drivebase, driverXbox));
topButtons.button(1).onTrue(drivebase.driveToClimbPose(Constants.TargetingConstants.BLUE_LEFT_CLIMB_POSE_OFFSETTED, Constants.TargetingConstants.RED_LEFT_CLIMB_POSE_OFFSETTED));
topButtons.button(2).onTrue(drivebase.driveToClimbPose(Constants.TargetingConstants.BLUE_RIGHT_CLIMB_POSE_OFFSETTED, Constants.TargetingConstants.RED_RIGHT_CLIMB_POSE_OFFSETTED));
topButtons.button(1).onTrue(drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_LEFT_CLIMB_POSE_OFFSETTED, Constants.TargetingConstants.RED_LEFT_CLIMB_POSE_OFFSETTED));
topButtons.button(2).onTrue(drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_RIGHT_CLIMB_POSE_OFFSETTED, Constants.TargetingConstants.RED_RIGHT_CLIMB_POSE_OFFSETTED));
//topButtons.button(1).onTrue(drivebase.driveToPose(Constants.))

14
src/main/java/frc/robot/subsystems/ShooterSubsystem.java
View File

@@ -121,6 +121,18 @@ public class ShooterSubsystem extends SubsystemBase {
rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity);
}
public void setShooterMotorsRPMAuto(){
centerShooterMotorPIDController.setSetpoint(-2700, ControlType.kVelocity);
leftShooterMotorPIDController.setSetpoint(-2700, ControlType.kVelocity);
rightShooterMotorPIDController.setSetpoint(-2700, ControlType.kVelocity);
}
public Command setShooterMotorsRPMAutoCommand()
{
return runOnce(()-> setShooterMotorsRPMAuto()).andThen(new WaitCommand(1.5))
.andThen(() -> setIndexerAndRampMotorRPM());
}
// test individual motor code
public void setLeftShooterMotorRPM() {
leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity);
@@ -206,7 +218,7 @@ public class ShooterSubsystem extends SubsystemBase {
public Command shootFuelCommand() {
return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(2))
return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(1.5))
.andThen(() -> setIndexerAndRampMotorRPM());
};

26
src/main/java/frc/robot/subsystems/TargetingSubsystems.java
View File

@@ -55,7 +55,6 @@ public class TargetingSubsystems extends SubsystemBase {
PIDController photonAimPIDController = new PIDController(3, 0.01, 0);
static Pose2d allianceHubPose = new Pose2d();
public static Rotation2d hubThetaPose = new Rotation2d();
public static Optional<Alliance> alliance = DriverStation.getAlliance();
private static ShuffleboardTab cameras;
@@ -117,9 +116,9 @@ public class TargetingSubsystems extends SubsystemBase {
// Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose);
Rotation2d angleDifference = PhotonUtils.getYawToPose(currentRobotPose,
allianceHubPose);
Constants.TargetingConstants.allianceHubPose);
double angleSpeed = photonAimPIDController.calculate(currentRobotPose.getRotation().getRadians(), allianceHubPose.getRotation().getRadians());
double angleSpeed = photonAimPIDController.calculate(currentRobotPose.getRotation().getRadians(), Constants.TargetingConstants.allianceHubPose.getRotation().getRadians());
angleSpeed = MathUtil.clamp(angleSpeed, -3.0, 3.0);
@@ -150,7 +149,7 @@ public class TargetingSubsystems extends SubsystemBase {
hubThetaPose = new Rotation2d(
Math.atan2(Constants.TargetingConstants.HUB_Y_POSE_BLUE - swerveDrive.getPose().getY(), Constants.TargetingConstants.HUB_X_POSE_BLUE - swerveDrive.getPose().getX()));
allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_BLUE,
Constants.TargetingConstants.allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_BLUE,
Constants.TargetingConstants.HUB_Y_POSE_BLUE, hubThetaPose);
}
@@ -158,7 +157,7 @@ public class TargetingSubsystems extends SubsystemBase {
hubThetaPose = new Rotation2d(
Math.atan2(Constants.TargetingConstants.HUB_Y_POSE_RED - swerveDrive.getPose().getY(),
Constants.TargetingConstants.HUB_X_POSE_RED - swerveDrive.getPose().getX()));
allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_RED,
Constants.TargetingConstants.allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_RED,
Constants.TargetingConstants.HUB_Y_POSE_RED, hubThetaPose);
}
}
@@ -168,11 +167,12 @@ public class TargetingSubsystems extends SubsystemBase {
public static void updateShooterRPM(Pose2d currentRobotPose) {
double distance = PhotonUtils.getDistanceToPose(currentRobotPose,
allianceHubPose);
Constants.ShooterConstants.SHOOTER_RPM = -300 * Math.pow(distance, 3)
+ 1221.475 * Math.pow(distance, 2)
- 1955.00131 * distance
- 1630.07168;
Constants.TargetingConstants.allianceHubPose);
Constants.ShooterConstants.SHOOTER_RPM =
(-293.84123 * Math.pow(distance, 3))
+ (1360.01497 * Math.pow(distance, 2))
- (2391.17127 * distance)
- 1249.22704;
}
@@ -181,10 +181,10 @@ public class TargetingSubsystems extends SubsystemBase {
public void periodic() {
alliance = DriverStation.getAlliance();
SmartDashboard.putString("Target Hub Pose",
allianceHubPose.getX() + " " + allianceHubPose.getY() + " " + allianceHubPose.getRotation());
Constants.TargetingConstants.allianceHubPose.getX() + " " + Constants.TargetingConstants.allianceHubPose.getY() + " " + Constants.TargetingConstants.allianceHubPose.getRotation());
SmartDashboard.putString("Hub Pose", "x: " + allianceHubPose.getMeasureX() + " y: " + allianceHubPose.getY()
+ " angle: " + allianceHubPose.getRotation());
SmartDashboard.putString("Hub Pose", "x: " + Constants.TargetingConstants.allianceHubPose.getMeasureX() + " y: " + Constants.TargetingConstants.allianceHubPose.getY()
+ " angle: " + Constants.TargetingConstants.allianceHubPose.getRotation());
/*
* Shuffleboard.getTab("Vision").add("Photon Vision Yaw Value",

31
src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
View File

@@ -69,6 +69,10 @@ public class SwerveSubsystem extends SubsystemBase {
*/
private final boolean visionDriveTest = true;
/**
* PhotonVision class to keep an accurate odometry.
*/
@@ -120,10 +124,13 @@ public class SwerveSubsystem extends SubsystemBase {
setupPhotonVision();
// Stop the odometry thread if we are using vision that way we can synchronize
// updates better.
swerveDrive.stopOdometryThread();
//swerveDrive.stopOdometryThread();
}
setupPathPlanner();
SmartDashboard.putData("Rebuilt Field", rebuiltField);
Constants.TargetingConstants.DRIVE_INTO_CLIMB_CONSTRAINTS = new PathConstraints(1, 4.0,
swerveDrive.getMaximumChassisAngularVelocity(), Units.degreesToRadians(360));
}
/**
@@ -286,7 +293,7 @@ public class SwerveSubsystem extends SubsystemBase {
);
}
public Command driveToClimbPose(Pose2d blueAlliancePose, Pose2d redAlliancePose) {
public Command driveToClimbPoseOffsetted(Pose2d blueAlliancePose, Pose2d redAlliancePose) {
Pose2d goal;
Optional<Alliance> alliance = DriverStation.getAlliance();
@@ -308,6 +315,26 @@ public class SwerveSubsystem extends SubsystemBase {
);
}
public Command driveIntoClimbPose(Pose2d blueAlliancePose, Pose2d redAlliancePose) {
Pose2d goal;
Optional<Alliance> alliance = DriverStation.getAlliance();
if (alliance.get() == Alliance.Blue) {
goal = blueAlliancePose;
} else {
goal = redAlliancePose;
}
// Create the constraints to use while pathfinding
// Since AutoBuilder is configured, we can use it to build pathfinding commands
return AutoBuilder.pathfindToPose(
goal,
Constants.TargetingConstants.DRIVE_INTO_CLIMB_CONSTRAINTS,
edu.wpi.first.units.Units.MetersPerSecond.of(0) // Goal end velocity in meters/sec
);
}
/**
* Drive with {@link SwerveSetpointGenerator} from 254, implemented by
* PathPlanner.

6
src/main/java/frc/robot/subsystems/swervedrive/Vision.java
View File

@@ -319,14 +319,14 @@ public class Vision {
BACK_RIGHT_CAM("Rear Right Camera",
Constants.TargetingConstants.ROBOT_TO_BACK_RIGHT_CAM.getRotation(),
Constants.TargetingConstants.ROBOT_TO_BACK_RIGHT_CAM.getTranslation(),
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1));
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
/**
* Front Left Camera
*/
/*FRONT_LEFT_CAM("Front Left Camera",
FRONT_LEFT_CAM("Front Left Camera",
Constants.TargetingConstants.ROBOT_TO_FRONT_LEFT_CAM.getRotation(),
Constants.TargetingConstants.ROBOT_TO_FRONT_LEFT_CAM.getTranslation(),
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1));*/
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1));
/**