diff --git a/src/main/deploy/pathplanner/autos/Hub to Shoot.auto b/src/main/deploy/pathplanner/autos/1. Hub to Shoot.auto similarity index 59% rename from src/main/deploy/pathplanner/autos/Hub to Shoot.auto rename to src/main/deploy/pathplanner/autos/1. Hub to Shoot.auto index 236077b..5877b05 100644 --- a/src/main/deploy/pathplanner/autos/Hub to Shoot.auto +++ b/src/main/deploy/pathplanner/autos/1. Hub to Shoot.auto @@ -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 + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/9. Chaos Monkey.auto similarity index 88% rename from src/main/deploy/pathplanner/autos/New Auto.auto rename to src/main/deploy/pathplanner/autos/9. Chaos Monkey.auto index b82dda5..465748a 100644 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/9. Chaos Monkey.auto @@ -7,7 +7,7 @@ { "type": "path", "data": { - "pathName": "New Path" + "pathName": "9.1 Chaos Monkey Path" } }, { diff --git a/src/main/deploy/pathplanner/autos/Chaos Monkey.auto b/src/main/deploy/pathplanner/autos/Chaos Monkey.auto deleted file mode 100644 index 4796d1c..0000000 --- a/src/main/deploy/pathplanner/autos/Chaos Monkey.auto +++ /dev/null @@ -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 -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Hub to Depot.auto b/src/main/deploy/pathplanner/autos/Hub to Depot.auto deleted file mode 100644 index baf27d1..0000000 --- a/src/main/deploy/pathplanner/autos/Hub to Depot.auto +++ /dev/null @@ -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 -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Test.auto b/src/main/deploy/pathplanner/autos/Test.auto deleted file mode 100644 index b3197af..0000000 --- a/src/main/deploy/pathplanner/autos/Test.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Test Path" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Hub to Depot.path b/src/main/deploy/pathplanner/paths/1.1 Hub to Shoot Path.path similarity index 71% rename from src/main/deploy/pathplanner/paths/Hub to Depot.path rename to src/main/deploy/pathplanner/paths/1.1 Hub to Shoot Path.path index 2c8714f..37590bd 100644 --- a/src/main/deploy/pathplanner/paths/Hub to Depot.path +++ b/src/main/deploy/pathplanner/paths/1.1 Hub to Shoot Path.path @@ -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 } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Test Path.path b/src/main/deploy/pathplanner/paths/1.2 Shoot to Depot Path.path similarity index 76% rename from src/main/deploy/pathplanner/paths/Test Path.path rename to src/main/deploy/pathplanner/paths/1.2 Shoot to Depot Path.path index 9c7cd5b..55c2201 100644 --- a/src/main/deploy/pathplanner/paths/Test Path.path +++ b/src/main/deploy/pathplanner/paths/1.2 Shoot to Depot Path.path @@ -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 } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/1.3 Depot to Shoot Path.path similarity index 74% rename from src/main/deploy/pathplanner/paths/New Path.path rename to src/main/deploy/pathplanner/paths/1.3 Depot to Shoot Path.path index eac90bb..11dd115 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/1.3 Depot to Shoot Path.path @@ -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 } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Chaos Monkey Path.path b/src/main/deploy/pathplanner/paths/9.1 Chaos Monkey Path.path similarity index 62% rename from src/main/deploy/pathplanner/paths/Chaos Monkey Path.path rename to src/main/deploy/pathplanner/paths/9.1 Chaos Monkey Path.path index 08bc439..b209333 100644 --- a/src/main/deploy/pathplanner/paths/Chaos Monkey Path.path +++ b/src/main/deploy/pathplanner/paths/9.1 Chaos Monkey Path.path @@ -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 } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot to Shooting Point.path b/src/main/deploy/pathplanner/paths/Depot to Shooting Point.path deleted file mode 100644 index 6087546..0000000 --- a/src/main/deploy/pathplanner/paths/Depot to Shooting Point.path +++ /dev/null @@ -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 -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shooting Point to Depot.path b/src/main/deploy/pathplanner/paths/Shooting Point to Depot.path deleted file mode 100644 index 9a62b19..0000000 --- a/src/main/deploy/pathplanner/paths/Shooting Point to Depot.path +++ /dev/null @@ -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 -} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index affa113..d6d68cd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -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)); } /** diff --git a/src/main/java/frc/robot/commands/AutoTurretCommand.java b/src/main/java/frc/robot/commands/AutoTurretCommand.java index f4498b0..3054f32 100644 --- a/src/main/java/frc/robot/commands/AutoTurretCommand.java +++ b/src/main/java/frc/robot/commands/AutoTurretCommand.java @@ -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); } } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 9b5e753..3970ecb 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -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 turretMotorPosition; private StatusSignal turretMotorForwardLimit; private StatusSignal 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() {