FairportRobotics
/
Castor2026
mirror of https://github.com/FairportRobotics/Castor2026.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Comp Commit

This commit is contained in:
Tyler Wilcox 2026-04-17 16:02:54 -04:00
parent 9a58b54277
commit e04bc20802
6 changed files with 61 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -151,7 +151,7 @@ public final class Constants {
public static final double DEFLECTOR_STORED_ANGLE = 0; public static final double DEFLECTOR_STORED_ANGLE = 0;
public static final double DEFLECTOR_SET_ANGLE1 = 100; public static final double DEFLECTOR_SET_ANGLE1 = 100;
public static final double DEFLECTOR_SET_ANGLE2 = 200; public static final double DEFLECTOR_SET_ANGLE2 = 200;
public static final double DEFLECTOR_SET_ANGLE3 = 300; // Return to 300 once hood is fixed public static final double DEFLECTOR_SET_ANGLE3 = 300;
public static final double DEFLECTOR_SERVO_RATIO = 1.0; public static final double DEFLECTOR_SERVO_RATIO = 1.0;
} }
@ -175,7 +175,7 @@ public final class Constants {
// Left/Right are from driver's perspective. Coordinates assume origin at blue side of field. // Left/Right are from driver's perspective. Coordinates assume origin at blue side of field.
// Rotation are zero because they don't matter. // Rotation are zero because they don't matter.
public static Pose3d BLUE_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 6.35, 0, new Rotation3d()); public static Pose3d BLUE_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 6.35, 0, new Rotation3d());
public static Pose3d BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 6.35, 0, new Rotation3d()); public static Pose3d BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 1.72, 0, new Rotation3d());
public static Pose3d BLUE_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 6.35, 0, new Rotation3d()); public static Pose3d BLUE_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 6.35, 0, new Rotation3d());
public static Pose3d BLUE_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 1.72, 0, new Rotation3d()); public static Pose3d BLUE_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 1.72, 0, new Rotation3d());

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

@ -14,6 +14,7 @@ import frc.robot.commands.AutoShootCommandChassisTurret;
import frc.robot.commands.AutoTurretCommand; import frc.robot.commands.AutoTurretCommand;
import frc.robot.commands.AutoTurretShootCommand; import frc.robot.commands.AutoTurretShootCommand;
import frc.robot.commands.Reverse; import frc.robot.commands.Reverse;
import frc.robot.commands.SetDeflectorCommand;
import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.HopperSubsystem;
import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeSubsystem;
@ -72,10 +73,10 @@ public class RobotContainer {
* *
*/ */
private void configureBindings() { private void configureBindings() {
/*m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE)); // m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE));
m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1)); // m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1));
m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3)); // m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3));
m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2));*/ // m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2));
//m_driverController.back().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem)); //m_driverController.back().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem));
m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem)); m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem));

6
src/main/java/frc/robot/Utils.java
View File

@ -4,6 +4,9 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance;
import frc.robot.Constants; import frc.robot.Constants;
import frc.robot.Constants.ShootingRegion; import frc.robot.Constants.ShootingRegion;
import frc.robot.Constants.ShootingRegionDimensions; import frc.robot.Constants.ShootingRegionDimensions;
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Angle;
@ -91,6 +94,9 @@ public class Utils {
region = Constants.ShootingRegion.RED_RIGHT_OPPONENT_ALLIANCE_ZONE; region = Constants.ShootingRegion.RED_RIGHT_OPPONENT_ALLIANCE_ZONE;
} }
} }
Logger.recordOutput("Utils-ShootingRegion", region);
return region; return region;
} }

3
src/main/java/frc/robot/commands/AutoTurretShootCommand.java
View File

@ -42,6 +42,7 @@ public class AutoTurretShootCommand extends Command{
hopperSubsystem.spindexerOn(); hopperSubsystem.spindexerOn();
CommandScheduler.getInstance().schedule(waitCommand); CommandScheduler.getInstance().schedule(waitCommand);
alliance = DriverStation.getAlliance().get(); alliance = DriverStation.getAlliance().get();
Logger.recordOutput("AutoTurretShoot-State", "SHOOTING");
} }
@Override @Override
@ -73,6 +74,8 @@ public class AutoTurretShootCommand extends Command{
turretSubsystem.setLauncher(0); turretSubsystem.setLauncher(0);
turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE); turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
Logger.recordOutput("AutoTurretShoot-State", "NOT SHOOTING");
DriverStation.getAlliance().ifPresent((aliance) -> { DriverStation.getAlliance().ifPresent((aliance) -> {
if (aliance == Alliance.Blue) { if (aliance == Alliance.Blue) {
turretSubsystem.setTurretTargetPose(Constants.FieldPoses.BLUE_HUB_POSE); turretSubsystem.setTurretTargetPose(Constants.FieldPoses.BLUE_HUB_POSE);

54
src/main/java/frc/robot/subsystems/DriveSubsystem.java
View File

@ -1,8 +1,9 @@
package frc.robot.subsystems; package frc.robot.subsystems;
import static org.fairportrobotics.frc.posty.assertions.Assertions.assertThat;
import org.fairportrobotics.frc.posty.TestableSubsystem; import org.fairportrobotics.frc.posty.TestableSubsystem;
import org.fairportrobotics.frc.posty.test.PostTest; import org.fairportrobotics.frc.posty.test.PostTest;
import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveBuilder; import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveBuilder;
import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveDriveSystem; import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveDriveSystem;
import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveModule; import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveModule;
@ -10,8 +11,6 @@ import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.Logger;
import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;
import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.PIDConstants;
import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.config.RobotConfig;
import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.controllers.PPHolonomicDriveController;
@ -23,12 +22,12 @@ import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotState;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.Constants; import frc.robot.Constants;
public class DriveSubsystem extends TestableSubsystem { public class DriveSubsystem extends TestableSubsystem {
@ -137,7 +136,7 @@ public class DriveSubsystem extends TestableSubsystem {
.build(); .build();
// Set vision measurement confidence values // Set vision measurement confidence values
driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 999999)); driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 0.1));
this.setDefaultCommand(Commands.run(new Runnable() { this.setDefaultCommand(Commands.run(new Runnable() {
@ -199,16 +198,45 @@ public class DriveSubsystem extends TestableSubsystem {
Logger.recordOutput("RobotPose", driveSystem.getRobotPose3d()); Logger.recordOutput("RobotPose", driveSystem.getRobotPose3d());
LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME); LimelightHelpers.SetRobotOrientation(Constants.CameraConstanst.FRONT_CAMERA_NAME, driveSystem.getRobotPose2d().getRotation().getDegrees(), 0, 0, 0, 0, 0);
if (frontCameraPose.tagCount >= 1) { LimelightHelpers.SetRobotOrientation(Constants.CameraConstanst.BACK_CAMERA_NAME, driveSystem.getRobotPose2d().getRotation().getDegrees(), 0, 0, 0, 0, 0);
driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontCameraPose.pose), frontCameraPose.timestampSeconds);
Logger.recordOutput("FrontPoseEstimate", frontCameraPose.pose); // MegaTag 2 impl
if(RobotState.isDisabled()){
LimelightHelpers.SetIMUMode(Constants.CameraConstanst.FRONT_CAMERA_NAME, 1);
LimelightHelpers.SetIMUMode(Constants.CameraConstanst.BACK_CAMERA_NAME, 1);
}else{
LimelightHelpers.SetIMUMode(Constants.CameraConstanst.FRONT_CAMERA_NAME, 4);
LimelightHelpers.SetIMUMode(Constants.CameraConstanst.BACK_CAMERA_NAME, 4);
} }
LimelightHelpers.PoseEstimate backCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME);
if (backCameraPose.tagCount >= 1) { LimelightHelpers.PoseEstimate frontTag = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.CameraConstanst.FRONT_CAMERA_NAME);
driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds); LimelightHelpers.PoseEstimate backTag = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.CameraConstanst.BACK_CAMERA_NAME);
Logger.recordOutput("BackPoseEstimate", backCameraPose.pose);
if(frontTag.tagCount >= 2){
driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontTag.pose), frontTag.timestampSeconds);
Logger.recordOutput("FrontPose-Estimate", frontTag.pose);
Logger.recordOutput("FrontPose-IsMegaTag2", frontTag.isMegaTag2);
} }
if(backTag.tagCount >= 2){
driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backTag.pose), backTag.timestampSeconds);
Logger.recordOutput("BackPose-Estimate", backTag.pose);
Logger.recordOutput("BackPose-IsMegaTag2", frontTag.isMegaTag2);
}
// MegaTag 1 Impl
// LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME);
// if (frontCameraPose.tagCount >= 2) {
// driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontCameraPose.pose), frontCameraPose.timestampSeconds);
// Logger.recordOutput("FrontPoseEstimate", frontCameraPose.pose);
// }
// LimelightHelpers.PoseEstimate backCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME);
// if (backCameraPose.tagCount >= 2) {
// driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds);
// Logger.recordOutput("BackPoseEstimate", backCameraPose.pose);
// }
} }
@Override @Override

5
src/main/java/frc/robot/subsystems/TurretSubsystem.java
View File

@ -115,7 +115,7 @@ public class TurretSubsystem extends TestableSubsystem {
SparkMaxConfig config = new SparkMaxConfig().apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO); SparkMaxConfig config = new SparkMaxConfig().apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO);
config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED); config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED);
config.voltageCompensation(10); config.voltageCompensation(10);
config.closedLoop.p(0.0002).i(0.000001).d(0.0005); // I = 0.0000001 config.closedLoop.p(0.0001).i(0.000004).d(0.0010); // I = 0.0000001
config.closedLoop.feedForward.kS(0.2).kV(0.000).kA(0.000); config.closedLoop.feedForward.kS(0.2).kV(0.000).kA(0.000);
// config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0); // config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0);
config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
@ -243,6 +243,7 @@ public class TurretSubsystem extends TestableSubsystem {
Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffsetInRotations); Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffsetInRotations);
Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue()); Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue());
Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue()); Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue());
Logger.recordOutput("TurretSubsystem-TurretPositionError", turretPositionError.refresh().getValueAsDouble());
} }
} }
@ -303,7 +304,7 @@ Logger.recordOutput("TurretSubsystem-TurretFieldTarget", turretAngle.getRotation
} }
public boolean isTurretAtTarget(){ public boolean isTurretAtTarget(){
return turretPositionError.getValueAsDouble() <= 0.5; return turretPositionError.refresh().getValueAsDouble() <= 0.1;
} }
private double getRequestedPosTurretRelative() { private double getRequestedPosTurretRelative() {