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

IDK

This commit is contained in:
Tyler Wilcox 2026-04-16 13:23:59 -04:00
parent 3c7126e525
commit ed6ebb67c1
23 changed files with 298 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/main/deploy/pathplanner/autos/HUB_START.auto Normal file
View File

@ -0,0 +1,19 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "HUB_START"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

19
src/main/deploy/pathplanner/autos/LEFT_TRENCH_START.auto Normal file
View File

@ -0,0 +1,19 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "LEFT_TRENCH_START"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

19
src/main/deploy/pathplanner/autos/RIGHT_TRENCH_START.auto Normal file
View File

@ -0,0 +1,19 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "RIGHT_TRENCH_START"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

2
src/main/deploy/pathplanner/paths/1.1 Hub to Shoot.path
View File

@ -35,7 +35,7 @@
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false

2
src/main/deploy/pathplanner/paths/1.2 Shoot to Depot Path.path
View File

@ -35,7 +35,7 @@
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false

4
src/main/deploy/pathplanner/paths/9.1 Chaos Monkey Path.path
View File

@ -83,14 +83,14 @@
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -1.2282908533168926
"rotation": -1.2282908533168928
},
"reversed": false,
"folder": null,

54
src/main/deploy/pathplanner/paths/HUB_START.path Normal file
View File

@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 3.519556041182133,
"y": 4.059804750409042
},
"prevControl": null,
"nextControl": {
"x": 3.769556041182133,
"y": 4.059804750409042
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 3.519556041182133,
"y": 4.059804750409042
},
"prevControl": {
"x": 3.269556041182133,
"y": 4.059804750409042
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}

54
src/main/deploy/pathplanner/paths/LEFT_TRENCH_START.path Normal file
View File

@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 4.0,
"y": 7.423545951843262
},
"prevControl": null,
"nextControl": {
"x": 4.25,
"y": 7.423545951843262
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 4.0,
"y": 7.423545951843262
},
"prevControl": {
"x": 3.75,
"y": 7.423545951843262
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}

2
src/main/deploy/pathplanner/paths/M. Depot. .path
View File

@ -88,7 +88,7 @@
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false

54
src/main/deploy/pathplanner/paths/RIGHT_TRENCH_START.path Normal file
View File

@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 4.0,
"y": 0.6236959156237152
},
"prevControl": null,
"nextControl": {
"x": 4.25,
"y": 0.6236959156237152
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 4.0,
"y": 0.6236959156237152
},
"prevControl": {
"x": 3.75,
"y": 0.6236959156237152
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}

2
src/main/deploy/pathplanner/paths/Relay. L-Tr..path
View File

@ -174,7 +174,7 @@
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false

4
src/main/deploy/pathplanner/paths/Relay. R-Tr..path
View File

@ -85,7 +85,7 @@
},
"prevControl": {
"x": 6.020824888067715,
"y": 0.8531028227756466
"y": 0.8531028227756465
},
"nextControl": {
"x": 5.667271497474452,
@ -178,7 +178,7 @@
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false

2
src/main/deploy/pathplanner/paths/S. Hub. shoot 8. E-Hub.path
View File

@ -68,7 +68,7 @@
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false

4
src/main/deploy/pathplanner/paths/S. L-Tre. Preload..path
View File

@ -24,7 +24,7 @@
"y": 4.024072289156626
},
"nextControl": {
"x": 2.421654288011767,
"x": 2.421654288011768,
"y": 3.9837489460910525
},
"isLocked": false,
@ -63,7 +63,7 @@
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false

2
src/main/deploy/pathplanner/paths/S. R-Trench. E-Hub.path
View File

@ -68,7 +68,7 @@
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false

2
src/main/deploy/pathplanner/paths/Spin.path
View File

@ -72,7 +72,7 @@
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false

2
src/main/deploy/pathplanner/settings.json
View File

@ -6,7 +6,7 @@
"autoFolders": [],
"defaultMaxVel": 3.0,
"defaultMaxAccel": 3.0,
"defaultMaxAngVel": 540.0,
"defaultMaxAngVel": 360.0,
"defaultMaxAngAccel": 720.0,
"defaultNominalVoltage": 12.0,
"robotMass": 44.4521,

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

@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.Constants.OperatorConstants;
import frc.robot.commands.AutoShootCommandChassisTurret;
import frc.robot.commands.AutoTurretCommand;
import frc.robot.commands.AutoTurretShootCommand;
import frc.robot.commands.Reverse;
import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.HopperSubsystem;
@ -80,7 +81,8 @@ public class RobotContainer {
m_driverController.rightBumper().onTrue(Commands.parallel(intakeSubsystem.intake()));
//m_driverController.a().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem));
m_driverController.start().whileTrue(intakeSubsystem.resetDeploy());
m_driverController.x().toggleOnTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
// m_driverController.x().toggleOnTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
m_driverController.x().whileTrue(new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem));
m_driverController.b().onTrue(new Reverse(intakeSubsystem, hopperSubsystem));
m_driverController.y().onTrue(intakeSubsystem.deploy());
}

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

@ -5,6 +5,7 @@ import frc.robot.Constants;
import frc.robot.Constants.ShootingRegion;
import frc.robot.Constants.ShootingRegionDimensions;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.units.measure.Angle;
public class Utils {
@ -93,7 +94,16 @@ public class Utils {
return region;
}
public static double getHoodAngleForDistance(double distanceInMeters){
if(distanceInMeters >= 0){
return 0;
}
return 0;
}
public static double getLauncherRPMForDistance(double distanceInMeters){
return (667.557*distanceInMeters) + 1024.699;
}
/**
* Determines the appropriate shooting target for the robot based on its alliance

11
src/main/java/frc/robot/commands/AutoTurretCommand.java
View File

@ -29,9 +29,6 @@ public class AutoTurretCommand extends Command {
private int[] shootingTagFilters;
private Pose3d closestTag;
private Pose3d scoringHubPose;
public AutoTurretCommand(TurretSubsystem turretSubsystem, DriveSubsystem driveSubsystem) {
this.turretSubsystem = turretSubsystem;
this.driveSubsystem = driveSubsystem;
@ -47,12 +44,12 @@ public class AutoTurretCommand extends Command {
DriverStation.getAlliance().ifPresent((aliance) -> {
if (aliance == Alliance.Blue) {
shootingTagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS;
scoringHubPose = Constants.FieldPoses.BLUE_HUB_POSE;
turretSubsystem.setTurretTargetPose(Constants.FieldPoses.BLUE_HUB_POSE);
} else {
shootingTagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS;
scoringHubPose = Constants.FieldPoses.RED_HUB_POSE;
turretSubsystem.setTurretTargetPose(Constants.FieldPoses.RED_HUB_POSE);
}
Logger.recordOutput("AutoTurret-ScoringTarget", scoringHubPose);
Logger.recordOutput("AutoTurret-ScoringTarget", turretSubsystem.getTurretTargetPose());
});
}
@ -65,7 +62,7 @@ public class AutoTurretCommand extends Command {
new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative() + 0.5))));
Logger.recordOutput("AutoTurret-TurretPose", turretPose);
Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d());
Translation2d targetTranslation = turretSubsystem.getTurretTargetPose().getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d());
Logger.recordOutput("AutoTurret-Translation", targetTranslation);
if(turretSubsystem.isTurretReady()){

32
src/main/java/frc/robot/commands/RelayCommand.java → src/main/java/frc/robot/commands/AutoTurretShootCommand.java
View File

@ -1,43 +1,63 @@
package frc.robot.commands;
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Angle;
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 edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.Constants;
import frc.robot.Constants.ShootingRegion;
import frc.robot.Utils;
import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.HopperSubsystem;
import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.TurretSubsystem;
public class RelayCommand extends Command{
public class AutoTurretShootCommand extends Command{
private HopperSubsystem hopperSubsystem;
private TurretSubsystem turretSubsystem;
private IntakeSubsystem intakeSubsystem;
private DriveSubsystem driveSubsystem;
private Command waitCommand = Commands.waitSeconds(1.5);
public RelayCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, IntakeSubsystem intakeSubsystem){
private Alliance alliance;
public AutoTurretShootCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, IntakeSubsystem intakeSubsystem, DriveSubsystem driveSubsystem){
this.hopperSubsystem = hopperSubsystem;
this.turretSubsystem = turretSubsystem;
this.intakeSubsystem = intakeSubsystem;
this.driveSubsystem = driveSubsystem;
addRequirements(hopperSubsystem, intakeSubsystem);
}
@Override
public void initialize() {
// turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3);
turretSubsystem.setLauncher(3000);
hopperSubsystem.spindexerOn();
CommandScheduler.getInstance().schedule(waitCommand);
turretSubsystem.setTurretMotorRotation(1); // Return to 0 after MIAMI VALLEY
alliance = DriverStation.getAlliance().get();
}
@Override
public void execute() {
ShootingRegion shootingRegion = Utils.findRobotShootingRegion(driveSubsystem.getBotPose(), alliance);
// if(shootingRegion == )
double distance = turretSubsystem.getTurretTargetPose().toPose2d().getTranslation().getDistance(driveSubsystem.getBotPose().getTranslation().toTranslation2d());
Logger.recordOutput("AutoTurretShoot-DistanceToTarget(Meters)", distance);
turretSubsystem.setTargetElevation(Utils.getHoodAngleForDistance(0));
turretSubsystem.setLauncher(Utils.getLauncherRPMForDistance(0));
if(turretSubsystem.isLauncherUpToSpeed() && waitCommand.isFinished()){
hopperSubsystem.feedKicker();
}else{
@ -58,7 +78,7 @@ public class RelayCommand extends Command{
turretSubsystem.setLauncher(0);
turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
turretSubsystem.setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
// turretSubsystem.setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
}
}

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

@ -163,15 +163,15 @@ public class DriveSubsystem extends TestableSubsystem {
driveSystem::getRobotPose2d,
driveSystem::setPose2d,
driveSystem::getRobotRelativeSpeeds,
(speeds, feedforwards) -> driveSystem.setChassisSpeed(speeds, new Translation2d()),
(speeds, feedforwards) -> {driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(-speeds.vxMetersPerSecond, -speeds.vyMetersPerSecond, -speeds.omegaRadiansPerSecond, driveSystem.getRobotHeading()), new Translation2d());},
new PPHolonomicDriveController(
new PIDConstants(3, 0, 0),
new PIDConstants(5, 0, 0),
new PIDConstants(Math.PI, 0, 0)),
config,
() -> {
var alliance = DriverStation.getAlliance();
if (alliance.isPresent()) {
return false;//alliance.get() == DriverStation.Alliance.Red;
return alliance.get() == DriverStation.Alliance.Red;
}
return false;
},
@ -188,6 +188,8 @@ public class DriveSubsystem extends TestableSubsystem {
e.printStackTrace();
}
}
@Override

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

@ -62,6 +62,7 @@ public class TurretSubsystem extends TestableSubsystem {
private StatusSignal<Angle> turretMotorPosition;
private StatusSignal<ForwardLimitValue> turretMotorForwardLimit;
private StatusSignal<ReverseLimitValue> turretMotorReverseLimit;
private StatusSignal<Double> turretPositionError;
private double forwardLimit = 0;
private double reverseLimit = 0;
@ -72,6 +73,8 @@ public class TurretSubsystem extends TestableSubsystem {
private Servo hood;
private Pose3d turretTarget;
public TurretSubsystem() {
// Turret configuration
@ -103,6 +106,7 @@ public class TurretSubsystem extends TestableSubsystem {
turretMotorPosition = turretMotor.getPosition();
turretMotorForwardLimit = turretMotor.getForwardLimit();
turretMotorReverseLimit = turretMotor.getReverseLimit();
turretPositionError = turretMotor.getClosedLoopError();
turretSim = turretMotor.getSimState();
@ -185,6 +189,14 @@ public class TurretSubsystem extends TestableSubsystem {
return getTurretMotorPosition() / Constants.ShooterConstants.TURRET_GEAR_RATIO;
}
public void setTurretTargetPose(Pose3d targetPose){
turretTarget = targetPose;
}
public Pose3d getTurretTargetPose(){
return turretTarget;
}
private double getTurretMotorPosition(){
return turretMotorPosition.refresh().getValueAsDouble() - turretOffsetInRotations;
}
@ -215,7 +227,7 @@ public class TurretSubsystem extends TestableSubsystem {
.withReverseSoftLimitEnable(true)
.withReverseSoftLimitThreshold(reverseLimit)
);
setTurretRobotRelative(0);
// setTurretRobotRelative(0);
// setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
}
}
@ -292,6 +304,10 @@ Logger.recordOutput("TurretSubsystem-TurretFieldTarget", turretAngle.getRotation
turretSim.setRawRotorPosition(posWithOffset);
}
public boolean isTurretAtTarget(){
return turretPositionError.getValueAsDouble() <= 0.5;
}
private double getRequestedPosTurretRelative() {
return (turretControlMode.getPositionMeasure().in(Units.Rotations) - turretOffsetInRotations) / Constants.ShooterConstants.TURRET_GEAR_RATIO;
}