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", "type": "path",
"data": { "data": {
"pathName": "Hub to Depot" "pathName": "1.1 Hub to Shoot Path"
}
},
{
"type": "path",
"data": {
"pathName": "Depot to Shooting Point"
} }
}, },
{ {
"type": "deadline", "type": "deadline",
"data": { "data": {
"commands": [ "commands": [
{
"type": "wait",
"data": {
"waitTime": 10.0
}
},
{ {
"type": "named", "type": "named",
"data": { "data": {
"name": "ChassisAlignShoot" "name": "ChassisAlignShoot"
} }
},
{
"type": "wait",
"data": {
"waitTime": 10.0
}
} }
] ]
} }
@ -38,7 +32,32 @@
{ {
"type": "path", "type": "path",
"data": { "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", "type": "path",
"data": { "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": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 3.5383012048192777, "x": 3.5,
"y": 3.9038674698795175 "y": 4.0
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 0.8527779926060135, "x": 3.0,
"y": 6.111286763858336 "y": 4.0
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
}, },
{ {
"anchor": { "anchor": {
"x": 1.0, "x": 2.0,
"y": 6.0 "y": 4.0
}, },
"prevControl": { "prevControl": {
"x": 2.6290964074675798, "x": 2.5,
"y": 4.671725238604007 "y": 4.0
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@ -48,7 +48,11 @@
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
<<<<<<< HEAD:src/main/deploy/pathplanner/paths/Hub to Depot.path
"rotation": -179.99999999992184 "rotation": -179.99999999992184
=======
"rotation": -179.69626968995792
>>>>>>> c2fa27aa5cc22124b930645e802b8176658edb7a:src/main/deploy/pathplanner/paths/1.1 Hub to Shoot Path.path
}, },
"useDefaultConstraints": false "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": { "anchor": {
"x": 2.0, "x": 2.0,
"y": 3.892939759036145 "y": 4.0
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 2.999999999999999, "x": 2.2144854970378125,
"y": 3.892939759036145 "y": 3.8554998234204434
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
}, },
{ {
"anchor": { "anchor": {
"x": 3.0, "x": 1.0,
"y": 3.892939759036145 "y": 6.0
}, },
"prevControl": { "prevControl": {
"x": 2.0, "x": 1.2166471375173105,
"y": 3.892939759036145 "y": 5.875244167248359
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 0.0 "rotation": -178.09121133641756
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 1.0511807495437313 "rotation": 0.4910281334858283
}, },
"useDefaultConstraints": true "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": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 1.2106987951807227, "x": 1.0,
"y": 3.892939759036145 "y": 6.0
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 2.210698795180722, "x": 1.453699105174549,
"y": 3.892939759036145 "y": 6.084627986890304
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
}, },
{ {
"anchor": { "anchor": {
"x": 2.904493975903614, "x": 2.0,
"y": 3.892939759036145 "y": 4.0
}, },
"prevControl": { "prevControl": {
"x": 1.904493975903614, "x": 2.266577414275921,
"y": 3.892939759036145 "y": 3.829411518660614
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@ -48,7 +48,7 @@
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 1.0511807495437313 "rotation": -179.69626968995792
}, },
"useDefaultConstraints": false "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": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 0.5, "x": 3.5184925161825644,
"y": 7.5 "y": 7.5345006466771025
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 1.5000000000000007, "x": 4.489963675146704,
"y": 7.5 "y": 7.219438447522688
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
@ -17,15 +17,15 @@
{ {
"anchor": { "anchor": {
"x": 7.6, "x": 7.6,
"y": 6.81063855421925 "y": 6.812200117368956
}, },
"prevControl": { "prevControl": {
"x": 7.498127631539383, "x": 7.532870181778978,
"y": 7.805436031369352 "y": 7.8099443669167
}, },
"nextControl": { "nextControl": {
"x": 7.745476905739448, "x": 7.650893142287796,
"y": 5.390036864084266 "y": 6.055780073270545
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
@ -33,15 +33,15 @@
{ {
"anchor": { "anchor": {
"x": 7.6, "x": 7.6,
"y": 0.9424578313276835 "y": 0.9617601315610045
}, },
"prevControl": { "prevControl": {
"x": 7.647828711717099, "x": 7.654625343431328,
"y": 1.4502101472608384 "y": 1.468826273060325
}, },
"nextControl": { "nextControl": {
"x": 7.526915662650708, "x": 7.527819365080411,
"y": 0.16659036144816464 "y": 0.2917349428743933
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
@ -52,15 +52,24 @@
"y": 0.6692650602433468 "y": 0.6692650602433468
}, },
"prevControl": { "prevControl": {
"x": 0.19442168674709331, "x": 1.096874747061515,
"y": 0.8331807228939487 "y": 0.6440356562159093
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
} }
], ],
"rotationTargets": [], "rotationTargets": [
{
"waypointRelativePos": 1.2332261810856255,
"rotationDegrees": -90.0
},
{
"waypointRelativePos": 1.7516108262639798,
"rotationDegrees": 0.0
}
],
"constraintZones": [], "constraintZones": [],
"pointTowardsZones": [], "pointTowardsZones": [],
"eventMarkers": [ "eventMarkers": [
@ -81,13 +90,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 0.0 "rotation": -1.2282908533168926
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 0.0 "rotation": -0.1533900770684255
}, },
"useDefaultConstraints": true "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.Constants.OperatorConstants;
import frc.robot.commands.AutoShootCommandChassisTurret; import frc.robot.commands.AutoShootCommandChassisTurret;
import frc.robot.commands.AutoShootCommandChassisTurretPathPlanner; import frc.robot.commands.AutoShootCommandChassisTurretPathPlanner;
import frc.robot.commands.AutoTurretCommand;
import frc.robot.commands.RelayCommand; import frc.robot.commands.RelayCommand;
import frc.robot.commands.SetDeflectorCommand; import frc.robot.commands.SetDeflectorCommand;
import frc.robot.commands.ZoneCheck; import frc.robot.commands.ZoneCheck;
@ -56,6 +57,7 @@ public class RobotContainer {
NamedCommands.registerCommand("ChassisAlignShoot", new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); 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()); Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d());
Logger.recordOutput("AutoTurret-Translation", targetTranslation); Logger.recordOutput("AutoTurret-Translation", targetTranslation);
if(turretSubsystem.isTurretReady()){ if(turretSubsystem.isTurretReady()){
// turretSubsystem.setTurretFieldRelative(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure()); turretSubsystem.setTurretFieldRelative(botPose, targetTranslation.getAngle().getRotations());
// turretSubsystem.setTurretRotation(Angle.ofRelativeUnits(-180, Units.Degrees)); // 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 com.revrobotics.spark.config.SparkMaxConfig;
import frc.robot.Constants; 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.AngleUnit;
import edu.wpi.first.units.Units; import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Angle;
@ -58,6 +61,9 @@ public class TurretSubsystem extends TestableSubsystem {
private StatusSignal<Angle> turretMotorPosition; private StatusSignal<Angle> turretMotorPosition;
private StatusSignal<ForwardLimitValue> turretMotorForwardLimit; private StatusSignal<ForwardLimitValue> turretMotorForwardLimit;
private StatusSignal<ReverseLimitValue> turretMotorReverseLimit; private StatusSignal<ReverseLimitValue> turretMotorReverseLimit;
private double forwardLimit = 0;
private double reverseLimit = 0;
private SparkMax launcherMotor; private SparkMax launcherMotor;
private SparkClosedLoopController launcherControler; private SparkClosedLoopController launcherControler;
@ -192,20 +198,23 @@ public class TurretSubsystem extends TestableSubsystem {
{ // Turret has tripped the switch { // Turret has tripped the switch
turretMotor.stopMotor(); turretMotor.stopMotor();
turretOffsetInRotations = (turretMotorPosition.refresh().getValueAsDouble() - 1.323); turretOffsetInRotations = (turretMotorPosition.refresh().getValueAsDouble() - 1.323);
if(!Robot.isReal()){
turretOffsetInRotations = 1.5; // Flip turret around
}
turretState = TurretState.READY; turretState = TurretState.READY;
// Angle forwardLimit = Angle.ofRelativeUnits(-90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).plus(turretOffset); forwardLimit = (1.5 + turretOffsetInRotations);
// Angle reverseLimit = Angle.ofRelativeUnits(90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).minus(turretOffset); reverseLimit = (-1.5 + turretOffsetInRotations);
// Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit); Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit);
// Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit); Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit);
turretMotor.getConfigurator().apply( turretMotor.getConfigurator().apply(
new SoftwareLimitSwitchConfigs() new SoftwareLimitSwitchConfigs()
// .withForwardSoftLimitEnable(true) .withForwardSoftLimitEnable(true)
// .withForwardSoftLimitThreshold(forwardLimit) .withForwardSoftLimitThreshold(forwardLimit)
// .withReverseSoftLimitEnable(true) .withReverseSoftLimitEnable(true)
// .withReverseSoftLimitThreshold(reverseLimit) .withReverseSoftLimitThreshold(reverseLimit)
); );
// setTurretRobotRelative(Angle.ofRelativeUnits(0, Units.Degrees)); setTurretRobotRelative(0);
setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY // 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-TurretLimitSwitch", turretLimitSwitch.get());
Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity()); Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity());
Logger.recordOutput("TurretSubsystem-TurretState", turretState); Logger.recordOutput("TurretSubsystem-TurretState", turretState);
Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngleRobotRelative()); Logger.recordOutput("TurretSubsystem-TurretPositionRobotRelative", getTurretAngleRobotRelative());
Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition()); Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition());
Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", turretMotorPosition.refresh().getValue().in(Units.Degrees)); Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", turretMotorPosition.refresh().getValue().in(Units.Rotations));
Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw", turretControlMode.getPositionMeasure().in(Units.Degrees)); Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw", turretControlMode.getPositionMeasure().in(Units.Rotations));
Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative()); Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative());
Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffsetInRotations); Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffsetInRotations);
Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue()); 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) if(turretState != TurretState.READY)
{ {
// Not ready to set angle // Not ready to set angle
return; 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) public void setTurretRobotRelative(double pos)
@ -243,12 +260,32 @@ public class TurretSubsystem extends TestableSubsystem {
return; 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){ public void setTurretMotorRotation(double pos){
turretMotor.setControl(turretControlMode.withPosition((pos + turretOffsetInRotations))); double posWithOffset = pos + turretOffsetInRotations;
turretSim.setRawRotorPosition(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() { private double getRequestedPosTurretRelative() {