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

Fixed merge conflicts

This commit is contained in:
Team578 Driver Station 2026-04-13 18:23:18 -04:00
parent 99fe2ecfbb c2fa27aa5c
commit fcf94c1327
14 changed files with 151 additions and 245 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
src/main/deploy/pathplanner/autos/Hub to Shoot.auto → src/main/deploy/pathplanner/autos/1. Hub to Shoot.auto
View File

@ -7,30 +7,24 @@
{
"type": "path",
"data": {
"pathName": "Hub to Depot"
}
},
{
"type": "path",
"data": {
"pathName": "Depot to Shooting Point"
"pathName": "1.1 Hub to Shoot Path"
}
},
{
"type": "deadline",
"data": {
"commands": [
{
"type": "wait",
"data": {
"waitTime": 10.0
}
},
{
"type": "named",
"data": {
"name": "ChassisAlignShoot"
}
},
{
"type": "wait",
"data": {
"waitTime": 10.0
}
}
]
}
@ -38,7 +32,32 @@
{
"type": "path",
"data": {
"pathName": "Shooting Point to Depot"
"pathName": "1.2 Shoot to Depot Path"
}
},
{
"type": "path",
"data": {
"pathName": "1.3 Depot to Shoot Path"
}
},
{
"type": "deadline",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "ChassisAlignShoot"
}
},
{
"type": "wait",
"data": {
"waitTime": 10.0
}
}
]
}
}
]

2
src/main/deploy/pathplanner/autos/New Auto.auto → src/main/deploy/pathplanner/autos/9. Chaos Monkey.auto
View File

@ -7,7 +7,7 @@
{
"type": "path",
"data": {
"pathName": "New Path"
"pathName": "9.1 Chaos Monkey Path"
}
},
{

19
src/main/deploy/pathplanner/autos/Chaos Monkey.auto
View File

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

19
src/main/deploy/pathplanner/autos/Hub to Depot.auto
View File

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

19
src/main/deploy/pathplanner/autos/Test.auto
View File

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

20
src/main/deploy/pathplanner/paths/Hub to Depot.path → src/main/deploy/pathplanner/paths/1.1 Hub to Shoot Path.path
View File

@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 3.5383012048192777,
"y": 3.9038674698795175
"x": 3.5,
"y": 4.0
},
"prevControl": null,
"nextControl": {
"x": 0.8527779926060135,
"y": 6.111286763858336
"x": 3.0,
"y": 4.0
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.0,
"y": 6.0
"x": 2.0,
"y": 4.0
},
"prevControl": {
"x": 2.6290964074675798,
"y": 4.671725238604007
"x": 2.5,
"y": 4.0
},
"nextControl": null,
"isLocked": false,
@ -48,7 +48,11 @@
"folder": null,
"idealStartingState": {
"velocity": 0,
<<<<<<< HEAD:src/main/deploy/pathplanner/paths/Hub to Depot.path
"rotation": -179.99999999992184
=======
"rotation": -179.69626968995792
>>>>>>> c2fa27aa5cc22124b930645e802b8176658edb7a:src/main/deploy/pathplanner/paths/1.1 Hub to Shoot Path.path
},
"useDefaultConstraints": false
}

18
src/main/deploy/pathplanner/paths/Test Path.path → src/main/deploy/pathplanner/paths/1.2 Shoot to Depot Path.path
View File

@ -4,24 +4,24 @@
{
"anchor": {
"x": 2.0,
"y": 3.892939759036145
"y": 4.0
},
"prevControl": null,
"nextControl": {
"x": 2.999999999999999,
"y": 3.892939759036145
"x": 2.2144854970378125,
"y": 3.8554998234204434
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 3.0,
"y": 3.892939759036145
"x": 1.0,
"y": 6.0
},
"prevControl": {
"x": 2.0,
"y": 3.892939759036145
"x": 1.2166471375173105,
"y": 5.875244167248359
},
"nextControl": null,
"isLocked": false,
@ -42,13 +42,13 @@
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
"rotation": -178.09121133641756
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 1.0511807495437313
"rotation": 0.4910281334858283
},
"useDefaultConstraints": true
}

18
src/main/deploy/pathplanner/paths/New Path.path → src/main/deploy/pathplanner/paths/1.3 Depot to Shoot Path.path
View File

@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 1.2106987951807227,
"y": 3.892939759036145
"x": 1.0,
"y": 6.0
},
"prevControl": null,
"nextControl": {
"x": 2.210698795180722,
"y": 3.892939759036145
"x": 1.453699105174549,
"y": 6.084627986890304
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.904493975903614,
"y": 3.892939759036145
"x": 2.0,
"y": 4.0
},
"prevControl": {
"x": 1.904493975903614,
"y": 3.892939759036145
"x": 2.266577414275921,
"y": 3.829411518660614
},
"nextControl": null,
"isLocked": false,
@ -48,7 +48,7 @@
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 1.0511807495437313
"rotation": -179.69626968995792
},
"useDefaultConstraints": false
}

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

@ -3,13 +3,13 @@
"waypoints": [
{
"anchor": {
"x": 0.5,
"y": 7.5
"x": 3.5184925161825644,
"y": 7.5345006466771025
},
"prevControl": null,
"nextControl": {
"x": 1.5000000000000007,
"y": 7.5
"x": 4.489963675146704,
"y": 7.219438447522688
},
"isLocked": false,
"linkedName": null
@ -17,15 +17,15 @@
{
"anchor": {
"x": 7.6,
"y": 6.81063855421925
"y": 6.812200117368956
},
"prevControl": {
"x": 7.498127631539383,
"y": 7.805436031369352
"x": 7.532870181778978,
"y": 7.8099443669167
},
"nextControl": {
"x": 7.745476905739448,
"y": 5.390036864084266
"x": 7.650893142287796,
"y": 6.055780073270545
},
"isLocked": false,
"linkedName": null
@ -33,15 +33,15 @@
{
"anchor": {
"x": 7.6,
"y": 0.9424578313276835
"y": 0.9617601315610045
},
"prevControl": {
"x": 7.647828711717099,
"y": 1.4502101472608384
"x": 7.654625343431328,
"y": 1.468826273060325
},
"nextControl": {
"x": 7.526915662650708,
"y": 0.16659036144816464
"x": 7.527819365080411,
"y": 0.2917349428743933
},
"isLocked": false,
"linkedName": null
@ -52,15 +52,24 @@
"y": 0.6692650602433468
},
"prevControl": {
"x": 0.19442168674709331,
"y": 0.8331807228939487
"x": 1.096874747061515,
"y": 0.6440356562159093
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"rotationTargets": [
{
"waypointRelativePos": 1.2332261810856255,
"rotationDegrees": -90.0
},
{
"waypointRelativePos": 1.7516108262639798,
"rotationDegrees": 0.0
}
],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [
@ -81,13 +90,13 @@
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
"rotation": -1.2282908533168926
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
"rotation": -0.1533900770684255
},
"useDefaultConstraints": true
}

54
src/main/deploy/pathplanner/paths/Depot to Shooting Point.path
View File

@ -1,54 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 15.5,
"y": 2.0
},
"prevControl": null,
"nextControl": {
"x": 15.387524472828067,
"y": 2.4871850323926177
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 14.5,
"y": 4.0
},
"prevControl": {
"x": 14.699374534462622,
"y": 3.5414699628074375
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.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/Shooting Point to Depot.path
View File

@ -1,54 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 14.5,
"y": 4.0
},
"prevControl": null,
"nextControl": {
"x": 14.387524472828067,
"y": 4.487185032392618
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 15.5,
"y": 2.0
},
"prevControl": {
"x": 15.699374534462622,
"y": 1.5414699628074375
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.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/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.AutoShootCommandChassisTurretPathPlanner;
import frc.robot.commands.AutoTurretCommand;
import frc.robot.commands.RelayCommand;
import frc.robot.commands.SetDeflectorCommand;
import frc.robot.commands.ZoneCheck;
@ -56,6 +57,7 @@ public class RobotContainer {
NamedCommands.registerCommand("ChassisAlignShoot", new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
}
/**

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

@ -68,10 +68,10 @@ public class AutoTurretCommand extends Command {
Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d());
Logger.recordOutput("AutoTurret-Translation", targetTranslation);
if(turretSubsystem.isTurretReady()){
// turretSubsystem.setTurretFieldRelative(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure());
// turretSubsystem.setTurretRotation(Angle.ofRelativeUnits(-180, Units.Degrees));
turretSubsystem.setTurretFieldRelative(botPose, targetTranslation.getAngle().getRotations());
// turretSubsystem.setTurretRobotRelative(0.25);
// turretSubsystem.setTurretMotorRotation(0);
}
}

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

@ -34,6 +34,9 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.config.SparkMaxConfig;
import frc.robot.Constants;
import frc.robot.Robot;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.units.AngleUnit;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Angle;
@ -58,6 +61,9 @@ public class TurretSubsystem extends TestableSubsystem {
private StatusSignal<Angle> turretMotorPosition;
private StatusSignal<ForwardLimitValue> turretMotorForwardLimit;
private StatusSignal<ReverseLimitValue> turretMotorReverseLimit;
private double forwardLimit = 0;
private double reverseLimit = 0;
private SparkMax launcherMotor;
private SparkClosedLoopController launcherControler;
@ -192,20 +198,23 @@ public class TurretSubsystem extends TestableSubsystem {
{ // Turret has tripped the switch
turretMotor.stopMotor();
turretOffsetInRotations = (turretMotorPosition.refresh().getValueAsDouble() - 1.323);
if(!Robot.isReal()){
turretOffsetInRotations = 1.5; // Flip turret around
}
turretState = TurretState.READY;
// Angle forwardLimit = Angle.ofRelativeUnits(-90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).plus(turretOffset);
// Angle reverseLimit = Angle.ofRelativeUnits(90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).minus(turretOffset);
// Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit);
// Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit);
forwardLimit = (1.5 + turretOffsetInRotations);
reverseLimit = (-1.5 + turretOffsetInRotations);
Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit);
Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit);
turretMotor.getConfigurator().apply(
new SoftwareLimitSwitchConfigs()
// .withForwardSoftLimitEnable(true)
// .withForwardSoftLimitThreshold(forwardLimit)
// .withReverseSoftLimitEnable(true)
// .withReverseSoftLimitThreshold(reverseLimit)
.withForwardSoftLimitEnable(true)
.withForwardSoftLimitThreshold(forwardLimit)
.withReverseSoftLimitEnable(true)
.withReverseSoftLimitThreshold(reverseLimit)
);
// setTurretRobotRelative(Angle.ofRelativeUnits(0, Units.Degrees));
setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
setTurretRobotRelative(0);
// setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
}
}
@ -214,10 +223,10 @@ public class TurretSubsystem extends TestableSubsystem {
Logger.recordOutput("TurretSubsystem-TurretLimitSwitch", turretLimitSwitch.get());
Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity());
Logger.recordOutput("TurretSubsystem-TurretState", turretState);
Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngleRobotRelative());
Logger.recordOutput("TurretSubsystem-TurretPositionRobotRelative", getTurretAngleRobotRelative());
Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition());
Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", turretMotorPosition.refresh().getValue().in(Units.Degrees));
Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw", turretControlMode.getPositionMeasure().in(Units.Degrees));
Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", turretMotorPosition.refresh().getValue().in(Units.Rotations));
Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw", turretControlMode.getPositionMeasure().in(Units.Rotations));
Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative());
Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffsetInRotations);
Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue());
@ -226,13 +235,21 @@ public class TurretSubsystem extends TestableSubsystem {
}
public void setTurretFieldRelative(double pos)
public void setTurretFieldRelative(Pose3d robotPose, double pos)
{
if(turretState != TurretState.READY)
{
// Not ready to set angle
return;
}
double correctedPos = -pos; // Flip angle to match CCW positive. Add 0.5 to flip around
double robotRotations = robotPose.getRotation().toRotation2d().getRotations();
Angle turretAngle = Angle.ofRelativeUnits(robotRotations, Units.Rotations).plus(Angle.ofRelativeUnits(correctedPos, Units.Rotations));
setTurretMotorRotation((turretAngle.in(Units.Rotation) * Constants.ShooterConstants.TURRET_GEAR_RATIO));
}
public void setTurretRobotRelative(double pos)
@ -243,12 +260,32 @@ public class TurretSubsystem extends TestableSubsystem {
return;
}
setTurretMotorRotation(pos * Constants.ShooterConstants.TURRET_GEAR_RATIO);
double correctedPos = -pos; // Flip angle to match CCW positive
setTurretMotorRotation(correctedPos * Constants.ShooterConstants.TURRET_GEAR_RATIO);
}
public void setTurretMotorRotation(double pos){
turretMotor.setControl(turretControlMode.withPosition((pos + turretOffsetInRotations)));
turretSim.setRawRotorPosition(pos + turretOffsetInRotations);
double posWithOffset = pos + turretOffsetInRotations;
Logger.recordOutput("Blah", posWithOffset);
if(Robot.isSimulation()){
if(posWithOffset >= forwardLimit){
turretSim.setForwardLimit(true);
} else {
turretSim.setForwardLimit(false);
}
if(posWithOffset <= reverseLimit){
turretSim.setReverseLimit(true);
} else {
turretSim.setReverseLimit(false);
}
}
turretMotor.setControl(turretControlMode.withPosition(posWithOffset));
turretSim.setRawRotorPosition(posWithOffset);
}
private double getRequestedPosTurretRelative() {