diff --git a/src/main/deploy/pathplanner/autos/Center Left Score Only.auto b/src/main/deploy/pathplanner/autos/Center Left Score Only.auto new file mode 100644 index 0000000..f274100 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Left Score Only.auto @@ -0,0 +1,32 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Left Start" + } + }, + { + "type": "named", + "data": { + "name": "emptyHopper" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center Right Score Only.auto b/src/main/deploy/pathplanner/autos/Center Right Score Only.auto new file mode 100644 index 0000000..df1cdbe --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Right Score Only.auto @@ -0,0 +1,32 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Right Start" + } + }, + { + "type": "named", + "data": { + "name": "emptyHopper" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center Score Only.auto b/src/main/deploy/pathplanner/autos/Center Score Only.auto new file mode 100644 index 0000000..68f5377 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Score Only.auto @@ -0,0 +1,32 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Start" + } + }, + { + "type": "named", + "data": { + "name": "emptyHopper" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Outpost Pickup + Score.auto b/src/main/deploy/pathplanner/autos/Outpost Pickup + Score.auto new file mode 100644 index 0000000..ecf1b35 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Outpost Pickup + Score.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Drive to Outpost" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "emptyHopper" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Outpost Score Only.auto b/src/main/deploy/pathplanner/autos/Outpost Score Only.auto new file mode 100644 index 0000000..1cfd10d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Outpost Score Only.auto @@ -0,0 +1,32 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Side Start" + } + }, + { + "type": "named", + "data": { + "name": "emptyHopper" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Left Start.path b/src/main/deploy/pathplanner/paths/Center Left Start.path new file mode 100644 index 0000000..e7ea659 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Left Start.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.600510083036774, + "y": 5.159335705812574 + }, + "prevControl": null, + "nextControl": { + "x": 3.850078676330523, + "y": 5.144655200324707 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.600510083036774, + "y": 5.159335705812574 + }, + "prevControl": { + "x": 3.350510083036774, + "y": 5.159335705812574 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.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/Center Right Start.path b/src/main/deploy/pathplanner/paths/Center Right Start.path new file mode 100644 index 0000000..52a928c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Right Start.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.482158956109135, + "y": 3.4163463819691584 + }, + "prevControl": null, + "nextControl": { + "x": 3.7317275494028843, + "y": 3.4016658764812915 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.482158956109135, + "y": 3.4163463819691584 + }, + "prevControl": { + "x": 3.232158956109135, + "y": 3.4163463819691584 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.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/Center Start.path b/src/main/deploy/pathplanner/paths/Center Start.path new file mode 100644 index 0000000..e8f8bd1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Start.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.482158956109135, + "y": 4.115693950177936 + }, + "prevControl": null, + "nextControl": { + "x": 3.7317275494028843, + "y": 4.101013444690069 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.482158956109135, + "y": 4.115693950177936 + }, + "prevControl": { + "x": 3.232158956109135, + "y": 4.115693950177936 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.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/Drive to Outpost.path b/src/main/deploy/pathplanner/paths/Drive to Outpost.path new file mode 100644 index 0000000..80b8deb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Drive to Outpost.path @@ -0,0 +1,105 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.923285883748518, + "y": 0.6835112692763936 + }, + "prevControl": null, + "nextControl": { + "x": 2.524590747330962, + "y": 0.6727520759193353 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.3201660735468574, + "y": 0.6835112692763936 + }, + "prevControl": { + "x": 3.6650652431791224, + "y": 0.6727520759193351 + }, + "nextControl": { + "x": 1.144297204873398, + "y": 0.6929182202257816 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.3201660735468574, + "y": 0.6835112692763936 + }, + "prevControl": { + "x": 1.588540925266905, + "y": 0.7157888493475679 + }, + "nextControl": { + "x": 3.05179122182681, + "y": 0.6512336892052193 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.4050296559905108, + "y": 0.6835112692763936 + }, + "prevControl": { + "x": 1.0613404507710558, + "y": 0.7588256227758006 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 2.1474245115453034, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0123734533183526, + "maxWaypointRelativePos": 1.9775028121484612, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 30.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 89.23097531742198 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 178.8542371618249 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Side Start.path b/src/main/deploy/pathplanner/paths/Left Side Start.path new file mode 100644 index 0000000..69d14fc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Side Start.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.977081850533809, + "y": 7.354211150652431 + }, + "prevControl": null, + "nextControl": { + "x": 4.226594995154531, + "y": 7.369805722191225 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.977081850533809, + "y": 7.354211150652431 + }, + "prevControl": { + "x": 3.727081850533809, + "y": 7.354211150652431 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.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/Right Side Start.path b/src/main/deploy/pathplanner/paths/Right Side Start.path new file mode 100644 index 0000000..b808861 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Side Start.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.977081850533809, + "y": 0.6835112692763936 + }, + "prevControl": null, + "nextControl": { + "x": 4.226594995154531, + "y": 0.6991058408151881 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.977081850533809, + "y": 0.6835112692763936 + }, + "prevControl": { + "x": 3.727081850533809, + "y": 0.6835112692763936 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.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/settings.json b/src/main/deploy/pathplanner/settings.json index b381d04..15e092c 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.965, - "robotLength": 0.965, + "robotWidth": 0.8255, + "robotLength": 0.8255, "holonomicMode": true, "pathFolders": [], "autoFolders": [], @@ -15,7 +15,7 @@ "driveWheelRadius": 0.048, "driveGearing": 5.143, "maxDriveSpeed": 5.45, - "driveMotorType": "falcon500", + "driveMotorType": "krakenX60FOC", "driveCurrentLimit": 60.0, "wheelCOF": 1.2, "flModuleX": 0.273, diff --git a/src/main/java/frc/robot/CanID.java b/src/main/java/frc/robot/CanID.java index 57b570c..39c7367 100644 --- a/src/main/java/frc/robot/CanID.java +++ b/src/main/java/frc/robot/CanID.java @@ -12,11 +12,11 @@ public enum CanID { TURRET_MOTOR(0), - INTAKE_MOTOR(6), + INTAKE_MOTOR(41), INTAKE_DEPLOY_MOTOR(11), - TURRET_PINION_CANCODER(19), - TURRET_FOLLOWER_CANCODER(20), + TURRET_PINION_CANCODER(20), + TURRET_FOLLOWER_CANCODER(19), FINDEXER_MOTOR(18); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5f6d25c..b3d9491 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,6 +7,7 @@ import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.pathplanner.lib.auto.NamedCommands; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.geometry.Rotation2d; @@ -18,6 +19,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.lib.SpikeController; +import frc.robot.commands.EmptyHopper; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.drive.CommandSwerveDrivetrain; import frc.robot.subsystems.findexer.Findexer; @@ -42,7 +44,7 @@ public class RobotContainer { private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - private final Telemetry logger = new Telemetry(MaxSpeed); + // private final Telemetry logger = new Telemetry(MaxSpeed); private final CommandXboxController driverController = new SpikeController(0, 0.05); private final CommandXboxController operatorController = new SpikeController(1, 0.05); @@ -70,6 +72,8 @@ public RobotContainer() { SmartDashboard.putData("Auto Path", autoChooser); configureBindings(); + + NamedCommands.registerCommand("emptyHopper", new EmptyHopper(shooter, targeting, 15)); } public static CommandSwerveDrivetrain getDrive() { @@ -93,7 +97,7 @@ private void setupSwerveBindings() { // negative Y // (forward) .withVelocityY(-driverController.getLeftX() * MaxSpeed) // Drive left with negative X (left) - .withRotationalRate(driverController.getRightX() * MaxAngularRate) // Drive counterclockwise + .withRotationalRate(-driverController.getRightX() * MaxAngularRate) // Drive counterclockwise // with negative X (left) )); @@ -117,30 +121,49 @@ private void setupSwerveBindings() { // reset the field-centric heading on left bumper press driverController.leftBumper().onTrue(drive.runOnce(() -> drive.seedFieldCentric())); + + // operatorController.y().onTrue(vision.runOnce(() -> { + // var estimatedPose = vision.getEstimatedPositionFromCameras(); + // if (estimatedPose != null) { + // Logger.recordOutput("Vision/SnapshotEstimate", estimatedPose); + // drive.resetPose(estimatedPose); + // } + // })); } private void setupIntakeBindings() { - // toggle intake on B press - // operatorController.b().onTrue(intake.run(intake::toggleIntake)); + // toggle intake on A press + operatorController.rightBumper().onTrue(intake.runOnce(intake::toggleIntake)); + + operatorController.a().onTrue(intake.runOnce(intake::switchDirection)); } private void setupTargetingBindings() { - operatorController.rightBumper().onTrue(targeting.run(targeting::setTargetingHub)); - operatorController.leftBumper().onTrue(targeting.run(targeting::setTargetingShuttle)); + operatorController.y().onTrue(targeting.runOnce(targeting::setTargetingHub)); + operatorController.x().onTrue(targeting.runOnce(targeting::setTargetingShuttleLeft)); + operatorController.b().onTrue(targeting.runOnce(targeting::setTargetingShuttleRight)); } private void setupShooterBindings() { // toggle shooter on right trigger hold - driverController.rightTrigger() - .onTrue(shooter.run(() -> shooter.setDriverRequestingShooting(true))) - .onFalse(shooter.run(() -> shooter.setDriverRequestingShooting(false))); + driverController.rightTrigger() + .onTrue(shooter.runOnce(() -> shooter.setDriverRequestingShooting(true))) + .onFalse(shooter.runOnce(() -> shooter.setDriverRequestingShooting(false))); + driverController.leftTrigger() + .onTrue(shooter.runOnce(() -> shooter.setRequestingWithForce(true))) + .onFalse(shooter.runOnce(() -> shooter.setRequestingWithForce(false))); + + operatorController.rightStick().onTrue(shooter.runOnce(() -> shooter.zeroHood())); + + operatorController.leftBumper().onTrue(turret.runOnce(turret::toggleAimingOverride)); - operatorController.x().onTrue(shooter.runOnce(shooter::zeroHood)); + operatorController.leftStick().onTrue(trigger.runOnce(() -> trigger.setReverseTrigger(true))); + operatorController.leftStick().onFalse(trigger.runOnce(() -> trigger.setReverseTrigger(false))); - operatorController.povUp().onTrue(shooter.runOnce(() -> shooter.changeDistanceTrim(0.1))); - operatorController.povDown().onTrue(shooter.runOnce(() -> shooter.changeDistanceTrim(-0.1))); - operatorController.povLeft().onTrue(turret.runOnce(() -> turret.changeTrim(2))); - operatorController.povRight().onTrue(turret.runOnce(() -> turret.changeTrim(-2))); + operatorController.povUp().onTrue(shooter.runOnce(() -> shooter.changeDistanceTrim(0.1))); + operatorController.povDown().onTrue(shooter.runOnce(() -> shooter.changeDistanceTrim(-0.1))); + operatorController.povLeft().onTrue(turret.runOnce(() -> turret.changeTrim(2))); + operatorController.povRight().onTrue(turret.runOnce(() -> turret.changeTrim(-2))); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/EmptyHopper.java b/src/main/java/frc/robot/commands/EmptyHopper.java new file mode 100644 index 0000000..cd72c64 --- /dev/null +++ b/src/main/java/frc/robot/commands/EmptyHopper.java @@ -0,0 +1,47 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.targeting.Targeting; + +public class EmptyHopper extends Command { + private Shooter shooter; + private Targeting targeting; + + private double scoringTime; + private Timer scoringTimer = new Timer(); + + public EmptyHopper(Shooter shooter, Targeting targeting, double forTime) { + this.shooter = shooter; + this.targeting = targeting; + + this.scoringTime = forTime; + + this.shooter.setDriverRequestingShooting(true); + + this.targeting.setTargetingHub(); + } + + @Override + public void initialize() { + this.scoringTimer.restart(); + this.targeting.setTargetingHub(); + } + + @Override + public void execute() { + this.targeting.setTargetingHub(); + } + + @Override + public boolean isFinished() { + return this.scoringTimer.hasElapsed(scoringTime); + } + + @Override + public void end(boolean interrupted) { + // TODO Auto-generated method stub + this.shooter.setDriverRequestingShooting(false); + } +} diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 7449259..9525b32 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -261,10 +261,10 @@ public TunerSwerveDrivetrain( * unspecified or set to 0 Hz, this is 250 Hz on * CAN FD, and 100 Hz on CAN 2.0. * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta], with units in meters + * in the form [x, y, theta]áµ€, with units in meters * and radians * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta], with units in meters + * in the form [x, y, theta]áµ€, with units in meters * and radians * @param modules Constants for each specific module */ diff --git a/src/main/java/frc/robot/generated/TunerConstantsMain.java b/src/main/java/frc/robot/generated/TunerConstantsMain.java new file mode 100644 index 0000000..dd5da93 --- /dev/null +++ b/src/main/java/frc/robot/generated/TunerConstantsMain.java @@ -0,0 +1,285 @@ +package frc.robot.generated; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.*; +import com.ctre.phoenix6.hardware.*; +import com.ctre.phoenix6.signals.*; +import com.ctre.phoenix6.swerve.*; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.measure.*; +import frc.robot.subsystems.drive.CommandSwerveDrivetrain; + +// Generated by the 2026 Tuner X Swerve Project Generator +// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html +public class TunerConstantsMain { + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(100).withKI(0).withKD(0.5) + .withKS(0.1).withKV(1.59).withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = new Slot0Configs() + .withKP(0.1).withKI(0).withKD(0) + .withKS(0).withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final Current kSlipCurrent = Amps.of(120); + + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true) + ); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("Canivore_Drivetrain", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(3.79); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 3.5714285714285716; + + private static final double kDriveGearRatio = 8.142857142857142; + private static final double kSteerGearRatio = 12.8; + private static final Distance kWheelRadius = Inches.of(2); + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final int kPigeonId = 62; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + + // Front Left + private static final int kFrontLeftDriveMotorId = 41; + private static final int kFrontLeftSteerMotorId = 27; + private static final int kFrontLeftEncoderId = 25; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.32666015625); + private static final boolean kFrontLeftSteerMotorInverted = false; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(10.25); + private static final Distance kFrontLeftYPos = Inches.of(10.25); + + // Front Right + private static final int kFrontRightDriveMotorId = 42; + private static final int kFrontRightSteerMotorId = 7; + private static final int kFrontRightEncoderId = 0; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.48828125); + private static final boolean kFrontRightSteerMotorInverted = false; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(10.25); + private static final Distance kFrontRightYPos = Inches.of(-10.25); + + // Back Left + private static final int kBackLeftDriveMotorId = 43; + private static final int kBackLeftSteerMotorId = 30; + private static final int kBackLeftEncoderId = 22; + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.04833984375); + private static final boolean kBackLeftSteerMotorInverted = false; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-10.25); + private static final Distance kBackLeftYPos = Inches.of(10.25); + + // Back Right + private static final int kBackRightDriveMotorId = 46; + private static final int kBackRightSteerMotorId = 50; + private static final int kBackRightEncoderId = 31; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.49365234375); + private static final boolean kBackRightSteerMotorInverted = false; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-10.25); + private static final Distance kBackRightYPos = Inches.of(-10.25); + + + public static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, + kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted + ); + public static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, + kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted + ); + public static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, + kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted + ); + public static final SwerveModuleConstants BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, + kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted + ); + + /** + * Creates a CommandSwerveDrivetrain instance. + * This should only be called once in your robot program,. + */ + public static CommandSwerveDrivetrain createDrivetrain() { + return new CommandSwerveDrivetrain( + DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight + ); + } + + + /** + * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. + */ + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation + * in the form [x, y, theta]áµ€, with units in meters + * and radians + * @param visionStandardDeviation The standard deviation for vision calculation + * in the form [x, y, theta]áµ€, with units in meters + * and radians + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, + odometryStandardDeviation, visionStandardDeviation, modules + ); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java index 77519a3..96cea7e 100644 --- a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java @@ -2,6 +2,7 @@ import static edu.wpi.first.units.Units.*; +import java.util.Optional; import java.util.function.Supplier; import com.ctre.phoenix6.SignalLogger; @@ -336,7 +337,13 @@ private void configureAutoBuilder() { new PIDConstants(5.0, 0, 0, 0) ), config, - () -> false, + () -> { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, this ); Pathfinding.setPathfinder(new LocalADStarAK()); diff --git a/src/main/java/frc/robot/subsystems/findexer/Findexer.java b/src/main/java/frc/robot/subsystems/findexer/Findexer.java index 8f8cbfa..6100bea 100644 --- a/src/main/java/frc/robot/subsystems/findexer/Findexer.java +++ b/src/main/java/frc/robot/subsystems/findexer/Findexer.java @@ -4,7 +4,7 @@ import frc.robot.subsystems.trigger.Trigger; public class Findexer extends SpikeSystem { - private static final double FEEDING_RPS = -20.0; // feeding velocity in rotations per second + private static final double FEEDING_RPS = -30.0 * 9; // feeding velocity in rotations per second private final Trigger trigger; diff --git a/src/main/java/frc/robot/subsystems/findexer/FindexerIOTalonFX.java b/src/main/java/frc/robot/subsystems/findexer/FindexerIOTalonFX.java index 94d9eca..f4b2c40 100644 --- a/src/main/java/frc/robot/subsystems/findexer/FindexerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/findexer/FindexerIOTalonFX.java @@ -11,18 +11,20 @@ public class FindexerIOTalonFX implements FindexerIO { private final TalonFX motor; - private final VelocityVoltage velocityCommand = new VelocityVoltage(0); + private final VelocityVoltage velocityControl = new VelocityVoltage(0.0); private final StatusSignal motorRps; // Rotations per second public FindexerIOTalonFX() { this.motor = new TalonFX(CanID.FINDEXER_MOTOR.getID()); this.motorRps = motor.getVelocity(); Slot0Configs config = new Slot0Configs(); - config.kP = 0.5; + config.kP = 0.1; config.kI = 0.0; config.kD = 0.0; config.kS = 0.0; + config.kV = 0.1; + this.motor.getConfigurator().apply(config); this.motor.optimizeBusUtilization(); @@ -34,9 +36,8 @@ public FindexerIOTalonFX() { */ @Override public void setSpeed(double rps) { - this.velocityCommand.withVelocity(rps); - - this.motor.setControl(velocityCommand); + this.velocityControl.Velocity = rps; + this.motor.setControl(velocityControl); } /** diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 3bd4cb1..fa22074 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -21,12 +21,12 @@ public enum IntakeState { private final CommandSwerveDrivetrain drivetrain; private boolean running = false; // True if the intake is running, False otherwise + private boolean forward = true; // Intake constructor public Intake(CommandSwerveDrivetrain drivetrain) { super("Intake", new IntakeIOInputsAutoLogged()); this.drivetrain = drivetrain; - enable(); } // Intake periodic function @@ -57,6 +57,8 @@ public void onPeriodic() { // break; // } doDeployedState(); + } else { + doRetractedState(); } } @@ -77,15 +79,19 @@ private void doDeployingState() { // Run the DEPLOYED state periodic actions private void doDeployedState() { - // Get the absolute velocity of the entire robot - double robotAbsoluteVelocity = Math.abs(drivetrain.getState().Speeds.vxMetersPerSecond); + // // Get the absolute velocity of the entire robot + // double robotAbsoluteVelocity = Math.abs(drivetrain.getState().Speeds.vxMetersPerSecond); + + // // Adjust the intake speed, increase the intake as the robot moves faster + // // Cap at MAX_SPEED + // double speed = Math.min(BASE_SPEED_INTAKE + robotAbsoluteVelocity * SPEED_PER_MPS, MAX_SPEED); - // Adjust the intake speed, increase the intake as the robot moves faster - // Cap at MAX_SPEED - double speed = Math.min(BASE_SPEED_INTAKE + robotAbsoluteVelocity * SPEED_PER_MPS, MAX_SPEED); + // if (forward == false) { + // speed *= -1; + // } // Update the intakeIO on speed - intakeIO.setIntakeSpeed(speed); + intakeIO.setIntakeSpeed(70); } // Run the RETRACTING state periodic actions @@ -118,6 +124,10 @@ public void enable() { running = true; } + public void switchDirection() { + forward = !forward; + } + // Disables the intake public void disable() { running = false; @@ -154,10 +164,11 @@ protected Runnable setupDataRefresher() { // Toggles the intake between deployed and retracted states public void toggleIntake() { - if (intakeIO.getIntakeState() == IntakeState.DEPLOYED || intakeIO.getIntakeState() == IntakeState.DEPLOYING) { - retract(); - } else { - deploy(); - } + this.running = !this.running; + // if (intakeIO.getIntakeState() == IntakeState.DEPLOYED || intakeIO.getIntakeState() == IntakeState.DEPLOYING) { + // retract(); + // } else { + // deploy(); + // } } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java index 45a48d3..0d5535d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java @@ -3,7 +3,9 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.AngularAcceleration; @@ -17,48 +19,50 @@ public class IntakeIOTalonFX implements IntakeIO, IORefresher { // TalonFX Motors private final TalonFX intakeMotor; - private final TalonFX deployMotor; + // private final TalonFX deployMotor; + + private final VelocityVoltage velocityControl = new VelocityVoltage(0.0); // Status Signals private final StatusSignal intakeVelocity; private final StatusSignal intakeCurrent; - private final StatusSignal deployVelocity; - private final StatusSignal deployCurrent; + // private final StatusSignal deployVelocity; + // private final StatusSignal deployCurrent; // Inputs for logging private IntakeIOInputs intakeIO; // IntakeIOTalonFX constructor public IntakeIOTalonFX() { - intakeMotor = new TalonFX(CanID.INTAKE_MOTOR.getID()); // Setup the intake motor with the CAN ID - deployMotor = new TalonFX(CanID.INTAKE_DEPLOY_MOTOR.getID()); // Set up deploy motor with the CAN ID + intakeMotor = new TalonFX(CanID.INTAKE_MOTOR.getID(), "Canivore_Drivetrain"); // Setup the intake motor with the CAN ID + // deployMotor = new TalonFX(CanID.INTAKE_DEPLOY_MOTOR.getID()); // Set up deploy motor with the CAN ID // Configure motors - deployMotor.getConfigurator().apply(getDeployMotorConfig()); + // deployMotor.getConfigurator().apply(getDeployMotorConfig()); intakeMotor.getConfigurator().apply(getIntakeMotorConfig()); // Configure motor signals intakeVelocity = intakeMotor.getVelocity(); intakeCurrent = intakeMotor.getStatorCurrent(); - deployVelocity = deployMotor.getVelocity(); - deployCurrent = deployMotor.getStatorCurrent(); + // deployVelocity = deployMotor.getVelocity(); + // deployCurrent = deployMotor.getStatorCurrent(); intakeMotor.optimizeBusUtilization(); - deployMotor.optimizeBusUtilization(); + // deployMotor.optimizeBusUtilization(); } // Fetches data from the motors @Override public void refreshData() { - BaseStatusSignal.refreshAll(intakeVelocity, intakeCurrent, deployVelocity, deployCurrent); + BaseStatusSignal.refreshAll(intakeVelocity, intakeCurrent); } @Override public void updateInputs(IntakeIOInputs inputs) { inputs.intakeVelocityRPS = intakeVelocity.getValueAsDouble(); inputs.intakeCurrentAmps = intakeCurrent.getValueAsDouble(); - inputs.deployVelocityRPS = deployVelocity.getValueAsDouble(); - inputs.deployCurrentAmps = deployCurrent.getValueAsDouble(); + // inputs.deployVelocityRPS = deployVelocity.getValueAsDouble(); + // inputs.deployCurrentAmps = deployCurrent.getValueAsDouble(); intakeIO = inputs; } @@ -66,14 +70,17 @@ public void updateInputs(IntakeIOInputs inputs) { // double speed - Speed to set the motor to in Rotations Per Second @Override public void setIntakeSpeed(double speed) { - intakeMotor.set(speed); + // intakeMotor.set(speed); + this.velocityControl.withVelocity(speed); + intakeMotor.setControl(this.velocityControl); } // Set the speed of the deploy motor // double speed - Speed to set the motor to in Rotations Per Second @Override public void setDeploySpeed(double speed) { - deployMotor.set(speed); + this.velocityControl.withVelocity(speed); + // deployMotor.setControl(this.velocityControl); } // Return the state of the Intake @@ -99,12 +106,15 @@ public static TalonFXConfiguration getIntakeMotorConfig() { // Set Intake motor to Coast when not on intakeConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; + intakeConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; // Intake motor PID values - intakeConfig.Slot0.kP = 0.1; + intakeConfig.Slot0.kP = 0.6; intakeConfig.Slot0.kI = 0.0; intakeConfig.Slot0.kD = 0.0; + intakeConfig.Slot0.kV = 0.15; + return intakeConfig; } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 996f31c..00b2455 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,8 +1,11 @@ package frc.robot.subsystems.shooter; import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.lib.subsystem.SpikeSystem; import frc.robot.subsystems.targeting.ShotData; import frc.robot.subsystems.targeting.Targeting; @@ -10,11 +13,17 @@ public class Shooter extends SpikeSystem { private static final double SHOOTER_READY_THRESHOLD_RPS = 2.0; // RPS threshold to consider the shooter ready + private boolean readFromData = true; + private ShooterIO shooterIO; private boolean driverRequestingShooting = false; // Whether the driver is currently requesting to shoot + private boolean requestingWithForce = false; // Whether the driver is requesting to shoot with force, which bypasses the normal checks for whether the shooter is ready and just runs the flywheel and hood at the target values public Shooter() { super("Shooter", new ShooterIOInputsAutoLogged()); + SmartDashboard.putNumber("TargetRPM", 0); + SmartDashboard.putNumber("TargetHoodAngle", 0); + SmartDashboard.putBoolean("ReadFromData", true); } /** @@ -22,20 +31,44 @@ public Shooter() { */ @Override public void onPeriodic() { + double distToTarget = getDistanceToTarget(); // distance in meters + readFromData = SmartDashboard.getBoolean("ReadFromData", true); + // double targetRPM = ShotData.distanceToRPM.get(distToTarget); + // double hoodAngle = ShotData.distanceToHoodAngle.get(distToTarget); + + double targetRPM = 0; + + if (readFromData) { + targetRPM = ShotData.distanceToRPM.get(distToTarget); + } else { + targetRPM = SmartDashboard.getNumber("TargetRPM", 0); + } - double targetRPM = ShotData.distanceToRPM.get(distToTarget); - double hoodAngle = ShotData.distanceToHoodAngle.get(distToTarget); + double hoodAngle = 0; + if (readFromData) { + hoodAngle = ShotData.distanceToHoodAngle.get(distToTarget); + } else { + hoodAngle = SmartDashboard.getNumber("TargetHoodAngle", 0); + } if (io.isZeroing) { shooterIO.runZeroingHood(); } else { - shooterIO.setHoodAngle(hoodAngle); + if (driverRequestingShooting) { + shooterIO.setHoodAngle(hoodAngle); + } else { + shooterIO.setHoodAngle(15); // set hood to default position when not shooting + } } // put to recovery mode if the driver is requesting to shoot // more direct control rather than smooth trajectory generation - shooterIO.setFlywheelVelocity(targetRPM / 60.0); // convert RPM to RPS + if (driverRequestingShooting) { + shooterIO.setFlywheelVelocity(targetRPM / 60.0); // convert RPM to RPS + } else { + shooterIO.setFlywheelVelocity(0); + } } /** @@ -62,7 +95,8 @@ public boolean isAtTargetRPS() { */ public double getDistanceToTarget() { Translation2d toGoal = Targeting.differenceBetweenRobotAndTarget(); - return toGoal.getNorm() + io.distanceTrimMeters; + Logger.recordOutput("Targeting/DistanceToTarget", toGoal.getNorm()); + return toGoal.getNorm() + io.distanceTrimMeters; // add distance trim to adjust the distance based on operator controller input } /** @@ -73,6 +107,10 @@ public void setDriverRequestingShooting(boolean isRequesting) { this.driverRequestingShooting = isRequesting; } + public void setRequestingWithForce(boolean isRequestingWithForce) { + this.requestingWithForce = isRequestingWithForce; + } + public void zeroHood() { shooterIO.zeroHood(); } @@ -81,10 +119,19 @@ public void zeroHood() { * Returns whether the driver is currently requesting to shoot. * @return true if the driver is requesting to shoot, false otherwise */ - public boolean isDriverRequestingShooting() { + public boolean isShootingRequested() { return this.driverRequestingShooting; } + public boolean isRequestingWithForce() { + return this.requestingWithForce; + } + + /** + * Changes the distance trim by a certain amount of meters. This is used to make minor adjustments to the distance based on operator controller input. + * @param deltaDistance the amount of meters to change the distance trim by. Positive values add to the distance, and negative values subtract from the distance. + */ + public void changeDistanceTrim(double deltaDistance) { shooterIO.changeDistanceTrim(deltaDistance); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index abcae87..8d55030 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -55,7 +55,7 @@ public ShooterIOTalonFX() { // hood encoder configs CANcoderConfiguration hoodEncoderConfig = new CANcoderConfiguration(); - hoodEncoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; + hoodEncoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; this.hoodEncoder.getConfigurator().apply(hoodEncoderConfig); // apply default configs to encoder before // using it for feedback // constrains the range to [0, 1) @@ -93,19 +93,13 @@ public ShooterIOTalonFX() { // flywheel configs (units in AMPS) var flywheelSlot0 = new Slot0Configs(); - flywheelSlot0.kP = 100; // amps / rps of error + flywheelSlot0.kP = 20; // amps / rps of error flywheelSlot0.kI = 0.0; flywheelSlot0.kD = 0.0; - flywheelSlot0.kS = 2.0; // amps needed to overcome static friction + flywheelSlot0.kS = 0.0; // amps needed to overcome static friction flywheelSlot0.kV = 0.0; // not used for torque control - var flywheelTorqueConfigs = new TorqueCurrentConfigs(); - flywheelTorqueConfigs.PeakForwardTorqueCurrent = 120; // amps, could up to 150A if needed - flywheelTorqueConfigs.PeakReverseTorqueCurrent = -10; // prevents the motor from braking when overshooting - flywheelTorqueConfigs.TorqueNeutralDeadband = 0; - this.flywheelMotor.getConfigurator().apply(flywheelSlot0); - this.flywheelMotor.getConfigurator().apply(flywheelTorqueConfigs); this.motorVelocity = flywheelMotor.getVelocity(); this.hoodAngle = hoodEncoder.getAbsolutePosition(); @@ -116,8 +110,6 @@ public ShooterIOTalonFX() { // force refresh before zero calculations BaseStatusSignal.refreshAll(motorVelocity, hoodAngle, hoodMotorPosition, hoodMotorVoltage, hoodMotorCurrent); - this.hoodEncoder.setPosition(0); - flywheelMotor.optimizeBusUtilization(); hoodMotor.optimizeBusUtilization(); } @@ -182,13 +174,13 @@ public void updateInputs(ShooterIOInputs inputs) { public void setFlywheelVelocity(double rps) { this.flywheelRPSSetPoint = rps; - boolean firstCommand = Double.isNaN(lastAppliedFlywheelRPSSetPoint); - boolean meaningfulChange = firstCommand - || (Math.abs(rps - lastAppliedFlywheelRPSSetPoint) >= FLYWHEEL_SETPOINT_UPDATE_DEADBAND_RPS); + // boolean firstCommand = Double.isNaN(lastAppliedFlywheelRPSSetPoint); + // boolean meaningfulChange = firstCommand + // || (Math.abs(rps - lastAppliedFlywheelRPSSetPoint) >= FLYWHEEL_SETPOINT_UPDATE_DEADBAND_RPS); - if (!meaningfulChange) { - return; - } + // if (!meaningfulChange) { + // return; + // } this.flywheelControl.withVelocity(rps); flywheelMotor.setControl(this.flywheelControl); @@ -221,4 +213,4 @@ private double angleToEncoder(double angle) { return 0.0; } else return Math.min(angle, 1.0); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/targeting/ShotData.java b/src/main/java/frc/robot/subsystems/targeting/ShotData.java index 111cff4..c3930d2 100644 --- a/src/main/java/frc/robot/subsystems/targeting/ShotData.java +++ b/src/main/java/frc/robot/subsystems/targeting/ShotData.java @@ -7,28 +7,37 @@ public class ShotData { public static final InterpolatingDoubleTreeMap distanceToHoodAngle = new InterpolatingDoubleTreeMap(); static { + distanceToRPM.put(3.14, 2200.0); + distanceToHoodAngle.put(3.14, 30.0); + + distanceToRPM.put(4.7, 2550.0); + distanceToHoodAngle.put(4.7, 36.0); + distanceToRPM.put(1.5, 1900.0); distanceToHoodAngle.put(1.5, 15.0); - distanceToRPM.put(2.0, 2000.0); - distanceToHoodAngle.put(2.0, 17.0); + distanceToRPM.put(2.5, 2300.0); + distanceToHoodAngle.put(2.5, 25.0); + + // distanceToRPM.put(2.96, 2300.0); + // distanceToHoodAngle.put(2.96, 30.0); - distanceToRPM.put(2.7, 1900.0); - distanceToHoodAngle.put(2.7, 30.0); + // distanceToRPM.put(2.7, 1900.0); + // distanceToHoodAngle.put(2.7, 30.0); - distanceToRPM.put(3.0, 2000.0); - distanceToHoodAngle.put(3.0, 30.0); + // distanceToRPM.put(3.0, 2000.0); + // distanceToHoodAngle.put(3.0, 30.0); - distanceToRPM.put(3.6, 2100.0); - distanceToHoodAngle.put(3.6, 30.0); + // distanceToRPM.put(3.6, 2100.0); + // distanceToHoodAngle.put(3.6, 30.0); - distanceToRPM.put(4.0, 2250.0); - distanceToHoodAngle.put(4.0, 30.0); + // distanceToRPM.put(4.0, 2250.0); + // distanceToHoodAngle.put(4.0, 30.0); - distanceToRPM.put(5.5, 2400.0); - distanceToHoodAngle.put(5.5, 40.0); + // distanceToRPM.put(5.5, 2400.0); + // distanceToHoodAngle.put(5.5, 40.0); - distanceToRPM.put(7.5, 3000.0); + distanceToRPM.put(7.5, 3100.0); distanceToHoodAngle.put(7.5, 40.0); } } diff --git a/src/main/java/frc/robot/subsystems/targeting/Targeting.java b/src/main/java/frc/robot/subsystems/targeting/Targeting.java index c5970d9..61e5da2 100644 --- a/src/main/java/frc/robot/subsystems/targeting/Targeting.java +++ b/src/main/java/frc/robot/subsystems/targeting/Targeting.java @@ -5,6 +5,8 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.Elastic; import frc.lib.Elastic.Notification; @@ -18,14 +20,27 @@ public class Targeting extends SubsystemBase { private static final double NOMINAL_SHOT_TIME_S = 0.3; // see github issue #23 (https://github.com/Team293/Rebuilt/issues/23) private static ShotCompensation.AdjustedShot shotData = new ShotCompensation.AdjustedShot(0.0, 0.0, 0.0, 0.0, 0.0); - private Translation2d targetPos = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); + private static Translation2d targetPos = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); private final CommandSwerveDrivetrain drive; + private static final double FIELD_WIDTH = 8.07; // meters + private static final double FIELD_LENGTH = 16.54; // meters + + private static final double shuttlingXOffset = 1.5; + private static final double shuttlingYOffset = 1.5; + + private boolean overrideRedAlliance = false; + private boolean overrideBlueAlliance = false; + + public Targeting(CommandSwerveDrivetrain drive) { this.drive = drive; setTargetingHub(); - Logger.recordOutput("Targeting/HubTarget", FieldConstants.Hub.oppTopCenterPoint); - Logger.recordOutput("Targeting/ShuttleTarget", new Pose2d(0, 0, new Rotation2d())); + Logger.recordOutput("HubTarget", FieldConstants.Hub.oppTopCenterPoint); + Logger.recordOutput("ShuttleTarget", new Pose2d(0, 0, new Rotation2d())); + + SmartDashboard.putBoolean("OverrideBlueAlliance", overrideBlueAlliance); + SmartDashboard.putBoolean("OverrideRedAlliance", overrideRedAlliance); } public static Translation2d differenceBetweenRobotAndTarget() { @@ -39,8 +54,12 @@ public static Translation2d differenceBetweenRobotAndTarget() { .plus(robotPose.getTranslation()); // vector from the turret pivot directly to the goal - Translation2d goalPose = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); - return goalPose.minus(turretPivot); + Translation2d goalPose = targetPos; + Translation2d toGoal = goalPose.minus(turretPivot); + + Logger.recordOutput("Turret/TurretPivot", new Pose2d(turretPivot, toGoal.getAngle())); + Logger.recordOutput("Targeting/TargetPose", new Pose2d(goalPose, new Rotation2d())); + return toGoal; } /** @@ -56,12 +75,18 @@ public void periodic() { .plus(robotPose.getTranslation()); Pose2d turretPivotPose = new Pose2d(turretPivot, robotPose.getRotation()); - shotData = ShotCompensation.compensateForMovement( - turretPivotPose, - drive.getState().Speeds, - new Pose2d(targetPos, new Rotation2d()), - NOMINAL_SHOT_TIME_S - ); + // shotData = ShotCompensation.compensateForMovement( + // turretPivotPose, + // drive.getState().Speeds, + // new Pose2d(targetPos, new Rotation2d()), + // NOMINAL_SHOT_TIME_S + // ); + + if (DriverStation.isAutonomous()) { + setTargetingHub(); + } + + Logger.recordOutput("Targeting/TurretPivot", turretPivotPose); } /** @@ -72,18 +97,65 @@ public void setTargetingHub() { new Notification(NotificationLevel.INFO, "Switched Modes", "Switched modes to SCORING mode") ); Elastic.selectTab("Scoring Mode"); - targetPos = FieldConstants.Hub.innerCenterPoint.toTranslation2d(); + + overrideBlueAlliance = SmartDashboard.getBoolean("OverrideBlueAlliance", overrideBlueAlliance); + overrideRedAlliance = SmartDashboard.getBoolean("OverrideRedAlliance", overrideRedAlliance); + + // if (!DriverStation.getAlliance().isPresent() && isRedAlliance) { + // if (isRedAlliance) { + // targetPos = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); + // } else { + // targetPos = FieldConstants.Hub.innerCenterPoint.toTranslation2d(); + // } + // return; + // } + + if (overrideRedAlliance) { + targetPos = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); + return; + } + + if (overrideBlueAlliance) { + targetPos = FieldConstants.Hub.innerCenterPoint.toTranslation2d(); + return; + } + + + if (DriverStation.getAlliance().isPresent()) { + if (DriverStation.getAlliance().get().equals(DriverStation.Alliance.Red)) { + targetPos = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); + } + if (DriverStation.getAlliance().get().equals(DriverStation.Alliance.Blue)) { + targetPos = FieldConstants.Hub.innerCenterPoint.toTranslation2d(); + } + } } /** * Set the target location to 0, 0 */ - public void setTargetingShuttle() { + public void setTargetingShuttleRight() { + Elastic.sendNotification( + new Notification(NotificationLevel.INFO, "Switched Modes", "Switched modes to SHUTTLING mode") + ); + Elastic.selectTab("Shuttling Mode"); + if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get().equals(DriverStation.Alliance.Red)) { + targetPos = new Translation2d(FIELD_LENGTH - shuttlingXOffset, FIELD_WIDTH - shuttlingYOffset); + } else { + targetPos = new Translation2d(0 + shuttlingXOffset, 0 + shuttlingYOffset); + } + } + + public void setTargetingShuttleLeft() { Elastic.sendNotification( new Notification(NotificationLevel.INFO, "Switched Modes", "Switched modes to SHUTTLING mode") ); Elastic.selectTab("Shuttling Mode"); - targetPos = new Translation2d(0, 0); + if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get().equals(DriverStation.Alliance.Red)) { + targetPos = new Translation2d(FIELD_LENGTH - shuttlingXOffset, 0 + shuttlingYOffset); + } else { + targetPos = new Translation2d(0 + shuttlingXOffset, FIELD_WIDTH - shuttlingYOffset); + } } /** diff --git a/src/main/java/frc/robot/subsystems/trigger/Trigger.java b/src/main/java/frc/robot/subsystems/trigger/Trigger.java index 5b43603..4561604 100644 --- a/src/main/java/frc/robot/subsystems/trigger/Trigger.java +++ b/src/main/java/frc/robot/subsystems/trigger/Trigger.java @@ -7,13 +7,15 @@ public class Trigger extends SpikeSystem { private static final int PROXIMITY_SENSOR_CHANNEL = 2; // DIO channel for the proximity sensor - private final static double TRIGGER_SPEED = 20.0; // Rotations per second + private final static double TRIGGER_SPEED = 30.0; // Rotations per second private final Shooter shooter; private final Turret turret; private TriggerIO triggerIO; + private boolean reverseTrigger = false; + public Trigger(Shooter shooter, Turret turret) { super("Trigger", new TriggerIOInputsAutoLogged()); @@ -24,13 +26,15 @@ public Trigger(Shooter shooter, Turret turret) { // Activate motor if proximity sensor detects a ball in the indexer @Override public void onPeriodic() { - if (mechanismReadyForBalls()) { + if (this.reverseTrigger) { + triggerIO.setSpeed(-TRIGGER_SPEED); + } else if (mechanismReadyForBalls()) { // run the indexer if the mechanisms are ready for balls // run it regardless of ball in indexer, so that it can feed a ball in if there is one queued up triggerIO.setSpeed(TRIGGER_SPEED); - } else if (needsFeeding()) { - // bring the ball to the indexer and stop once we see a ball - triggerIO.setSpeed(TRIGGER_SPEED); + // } else if (needsFeeding()) { + // // bring the ball to the indexer and stop once we see a ball + // triggerIO.setSpeed(TRIGGER_SPEED); } else { // stop the indexer if the mechanisms aren't ready and we have a ball queued triggerIO.setSpeed(0.0); @@ -42,7 +46,12 @@ public void onPeriodic() { * @return true if there is a ball in the indexer, false otherwise */ private boolean hasBallQueued() { - return super.io.proximitySensor; + // return super.io.proximitySensor; + return false; + } + + public void setReverseTrigger(boolean reverse) { + this.reverseTrigger = reverse; } /** @@ -50,7 +59,10 @@ private boolean hasBallQueued() { * @return true if the shooter is at target RPS, the turret is at target angle, and the driver is requesting to shoot, false otherwise */ private boolean mechanismReadyForBalls() { - return shooter.isAtTargetRPS() && turret.isAtTargetAngle() && shooter.isDriverRequestingShooting(); + if (shooter.isRequestingWithForce()) { + return true; + } + return shooter.isAtTargetRPS() && turret.isAtTargetAngle() && shooter.isShootingRequested(); } /** @@ -59,7 +71,7 @@ private boolean mechanismReadyForBalls() { * @return true if the indexer needs a ball, false otherwise */ public boolean needsFeeding() { - return !hasBallQueued(); + return mechanismReadyForBalls(); } @Override diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index e1edec6..c830fe2 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -1,18 +1,18 @@ package frc.robot.subsystems.turret; -import frc.lib.AngleUtils; -import frc.lib.LowPassFilter; + import frc.lib.subsystem.SpikeSystem; import frc.robot.RobotContainer; import frc.robot.subsystems.targeting.Targeting; +import frc.robot.subsystems.targeting.ShotCompensation; import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Translation2d; public class Turret extends SpikeSystem { public static final double TURRET_AIMING_TOLERANCE_DEGREES = 5.0; // degrees within which we consider the turret to be aimed at the target (+-) - private static final double TURRET_TARGET_FILTER_ALPHA = 0.15; // smaller = smoother, larger = more responsive // HARDWARE CONSTANTS // gearing @@ -22,19 +22,21 @@ public class Turret extends SpikeSystem { public static final double TURRET_GEAR_RATIO = TURRET_GEAR_TEETH / PINION_ENCODER_TEETH; // gear ratio from motor to turret public static final double DEGREES_PER_REV = 360.0; // degrees in one revolution + public static final double NORMALIZED_REVOLUTION = 1.0; // one full revolution in normalized units public static final double ENCODER_COMBINED_TEETH = PINION_ENCODER_TEETH * FOLLOWER_ENCODER_TEETH; public static final double ENCODER_COMBINED_PERIOD_REV = ENCODER_COMBINED_TEETH / PINION_ENCODER_TEETH; public static final double ENCODER_COMBINED_PERIOD_TURRET_REV = ENCODER_COMBINED_PERIOD_REV * (PINION_ENCODER_TEETH / TURRET_GEAR_TEETH); - public static final double TURRET_CENTER_OFFSET_DEG = -17; // subtracted from robot relative heading - public static final double TURRET_ROBOT_OFFSET_DEG = 120; // subtracted from robot relative heading to get turret relative heading + public static final double TURRET_CENTER_OFFSET_DEG = -88.1; // subtracted from robot relative heading + public static final double TURRET_ROBOT_OFFSET_DEG = 51.8; // subtracted from robot relative heading to get turret relative heading + + public static final Translation2d TURRET_OFFSET_FROM_CENTER = new Translation2d(-0.3, -0.2); // distance from the center of the robot to the center of the turret, in meters - public static final Translation2d TURRET_OFFSET_FROM_CENTER = new Translation2d(0.3, 0); // distance from the center of the robot to the center of the turret, in meters + private boolean overrideAutomaticAiming = false; private TurretIO turretIO; - private final LowPassFilter turretTargetFilter = new LowPassFilter(TURRET_TARGET_FILTER_ALPHA); public Turret() { super("Turret", new TurretIOInputsAutoLogged()); @@ -42,36 +44,54 @@ public Turret() { @Override public void onPeriodic() { - double rawTargetAngleDeg = getTurretAngleDegreesFieldRelative(); - double filteredTargetAngleDeg = calculateFilteredTargetAngle(rawTargetAngleDeg); + ShotCompensation.AdjustedShot shotData = Targeting.getShotData(); + // turretIO.setTurretAngleFieldRelativeDegrees(0); - io.targetTurretDegreesFilteredFieldRelative = filteredTargetAngleDeg; - this.turretIO.setTurretAngleFieldRelativeDegrees(filteredTargetAngleDeg); - } + // if (shotData != null) { + // double newTargetAngleDeg = shotData.turretAngleDeg(); + + // this.turretIO.setTurretAngleFieldRelativeDegrees(newTargetAngleDeg); + // } + + if (this.overrideAutomaticAiming) { + this.turretIO.setTurretAngleRobotRelativeDegrees(0); + } else { + this.turretIO.setTurretAngleFieldRelativeDegrees(getTurretAngleDegreesFieldRelative()); + } - private double calculateFilteredTargetAngle(double rawTargetAngleDeg) { - return AngleUtils.filterWrappedAngleDeg(turretTargetFilter, rawTargetAngleDeg, -180.0, 180.0); } public double getTurretAngleDegreesFieldRelative() { Translation2d toGoal = Targeting.differenceBetweenRobotAndTarget(); - return toGoal.getAngle().getDegrees(); + + double angleToTarget = toGoal.getAngle().getDegrees(); + Logger.recordOutput("Targeting/AngleToTargetDeg", angleToTarget); + return angleToTarget; } + @Override protected Runnable setupDataRefresher() { turretIO = new TurretIOTalonFX(RobotContainer.getDrive()); return useAsyncDataRefresher(turretIO); } + public void toggleAimingOverride() { + this.overrideAutomaticAiming = !this.overrideAutomaticAiming; + } + /** * Checks if the turret is at the target angle, within the tolerance defined by TURRET_AIMING_TOLERANCE_DEGREES. * @return true if the turret is at the target angle, false otherwise */ - @AutoLogOutput(key = "Turret/IsAtTargetAngle") + + @AutoLogOutput(key="Turret/IsAtTargetAngle") public boolean isAtTargetAngle() { - double error = Math.abs(io.turretAngleDegreesRobotRelativeFiltered - io.targetRobotRelativeTurretDegrees); - return error <= TURRET_AIMING_TOLERANCE_DEGREES; + double error = + Math.abs(io.turretAngleDegreesRobotRelative - io.processedTargetTurretDegrees); + + // return error <= TURRET_AIMING_TOLERANCE_DEGREES; + return true; } public void changeTrim(double deltaDegrees) { diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/turret/TurretIO.java index 1737349..3e5918d 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIO.java @@ -9,21 +9,18 @@ public interface TurretIO extends IORefresher, BaseIO { @AutoLog public static class TurretIOInputs extends BaseInputClass { - public double fieldRelativeTurretAngleDegrees = 0.0; // measured turret heading on the field, in degrees [-180, 180) - public double turretRelativeTurretAngleDegrees = 0.0; // measured turret heading in turret-frame (after center-offset), in degrees [-180, 180) - public double robotRelativeTurretAngleDegrees = 0.0; // measured turret heading in robot-frame (after robot-offset), in degrees [-180, 180) - public double targetFieldRelativeTurretDegrees = 0.0; // raw field-relative target sent to the turret, in degrees - public double targetRobotRelativeTurretDegrees = 0.0; // robot-relative target after wrapping and trim applied, in degrees - public double targetTurretMotorRotations = 0.0; // setpoint sent to the TalonFX position controller, in motor rotations - public double turretOffsetRotations = 0.0; // motor position written at last zero/recalibration, in rotations - public double turretMotorPositionRotations = 0.0; // raw TalonFX rotor position, in rotations - public double pinionEncoderRotations = 0.0; // raw absolute position of the pinion (drive) CANcoder, in rotations [0, 1) - public double followerEncoderRotations = 0.0; // raw absolute position of the follower CANcoder, in rotations [0, 1) - public double rawTurretMechanismRotations = 0.0; // continuous turret mechanism position from CRT unwrapping, in revolutions - public double pinionEncoderRevsCalculated = 0.0; // continuous pinion position resolved by the CRT algorithm, in revolutions - public double turretTrimDegrees = 0.0; // operator-applied fine-trim offset, in degrees - public double targetTurretDegreesFilteredFieldRelative = 0.0; // filtered field-relative target angle sent to the turret, in degrees [-180, 180) - public double turretAngleDegreesRobotRelativeFiltered = 0.0; // filtered measured turret angle in robot-frame, in degrees [-180, 180) + public double turretAngleDegreesFieldRelative = 0.0; // current angle of the turret, field-relative, in degrees + public double turretAngleDegreesTurretRelative = 0.0; // current angle of the turret, turret-relative, in degrees, after setting a turret center offset + public double turretAngleDegreesRobotRelative = 0.0; // current angle of the turret, robot-relative, in degrees, after setting a robot center offset + public double targetTurretDegrees = 0.0; // target angle for the turret, field-relative, in degrees + public double processedTargetTurretDegrees = 0.0; // processed target angle for the turret, field-relative, in degrees, updated when recalculation is called + public double targetTurretMotorRotations = 0.0; // target position for the turret motor, in rotations + public double turretOffsetRotations = 0.0; // calculated turret motor rotations, updated when recalculation is called + public double turretMotorPositionRotations = 0.0; // current position of the turret motor, in rotations + public double pinionEncoderRotations = 0.0; // current rotations of the pinion encoder + public double followerEncoderRotations = 0.0; // current rotations of the follower + public double rawTurretMechanismRotations = 0.0; // raw rotations of the entire turret mechanism + public double turretTrimDegrees = 0.0; // minor adjustment to the turret angle based on operator controller input, in degrees } /** @@ -32,6 +29,11 @@ public static class TurretIOInputs extends BaseInputClass { */ void setTurretAngleFieldRelativeDegrees(double fieldRelativeAngleDegrees); + /** + * Set the angle of the turret relative to the robot. + */ + void setTurretAngleRobotRelativeDegrees(double robotRelativeDegrees); + /** * Recalculates the turret motor zero position to fix any encoder drift. */ diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index 915dff5..4b3dc67 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -1,5 +1,8 @@ package frc.robot.subsystems.turret; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.*; @@ -7,17 +10,20 @@ import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.units.measure.Angle; -import frc.lib.AngleUtils; import frc.lib.LowPassFilter; import frc.robot.CanID; import frc.robot.subsystems.drive.CommandSwerveDrivetrain; public class TurretIOTalonFX implements TurretIO { - private static final double TURRET_MEASUREMENT_FILTER_ALPHA = 0.25; // smaller = smoother, larger = more responsive + // KS KV CONSTANTS + private static final double kS = 0.35; // volts needed to overcome static friction + private static final double kV = 0.20; // volts per (rotation per second) to maintain motion // SUBSYSTEMS private final CommandSwerveDrivetrain drive; @@ -36,19 +42,19 @@ public class TurretIOTalonFX implements TurretIO { private boolean isInitialized = false; // whether the turret has been initialized with a known position yet private double lastPositionRevs = 0.0; // last calculated position of the turret in revolutions private double lastPinionRevs = 0.0; // last calculated position of the pinion encoder in revolutions - private double cachedPinionEncoderRevs = 0.0; // cached result of the CRT pinion calculation, updated each refreshData() // VALUES private double targetTurretDegreesFieldRelative; // target angle of the turret in degrees, relative to the field private double processedTargetTurretDegreesFieldRelative; // processed target angle of the turret in degrees, relative to the field private double targetTurretAngleMotorRevs; // target angle of the turret in motor rotations private double calculatedMotorOffsetRevs; // calculated offset in motor rotations based on the current position of the turret and the pinion encoder reading + private double targetTurretDegreesTurretRelative = 0; // COMMANDS private final MotionMagicVoltage mmRequest = new MotionMagicVoltage(0.0); + private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(kS, kV); // ks, kv private double turretTrimDegrees = 0.0; - private final LowPassFilter turretMeasurementFilter = new LowPassFilter(TURRET_MEASUREMENT_FILTER_ALPHA); public TurretIOTalonFX(CommandSwerveDrivetrain drive) { // SUBSYSTEMS @@ -61,24 +67,20 @@ public TurretIOTalonFX(CommandSwerveDrivetrain drive) { // CONFIGURATIONS var turretMotorConfig = getTurretMotionConfigs(); + var pinionEncoderConfig = getPinionEncoderConfigs(); + var followerEncoderConfig = getFollowerEncoderConfigs(); var turretMotorFeedbackConfig = getTurretMotorFeedbackConfigs(); var turretSoftwareLimitConfig = getTurretSoftwareLimitConfigs(); - var encoderConfig = getEncoderConfigs(); this.turretMotor.getConfigurator().apply(turretMotorConfig.getFirst()); this.turretMotor.getConfigurator().apply(turretMotorConfig.getSecond()); this.turretMotor.getConfigurator().apply(turretMotorFeedbackConfig); this.turretMotor.getConfigurator().apply(turretSoftwareLimitConfig); - // invert motor this.turretMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)); - this.pinionEncoder.getConfigurator().apply(encoderConfig); - this.followerEncoder.getConfigurator().apply(encoderConfig); - - this.turretMotor.optimizeBusUtilization(); - this.pinionEncoder.optimizeBusUtilization(); - this.followerEncoder.optimizeBusUtilization(); + this.pinionEncoder.getConfigurator().apply(pinionEncoderConfig); + this.followerEncoder.getConfigurator().apply(followerEncoderConfig); // SIGNALS this.turretMotorPosition = this.turretMotor.getPosition(); @@ -107,16 +109,19 @@ public void setTurretAngleFieldRelativeDegrees(double fieldRelativeAngleDegrees) setTurretAngleRobotRelativeDegrees(targetRobotRelativeDeg); } - private void setTurretAngleRobotRelativeDegrees(double robotRelativeAngleDegrees) { + public void setTurretAngleRobotRelativeDegrees(double robotRelativeAngleDegrees) { setTurretAngleTurretRelativeDegrees(robotRelativeAngleDegrees + Turret.TURRET_ROBOT_OFFSET_DEG); } private void setTurretAngleTurretRelativeDegrees(double angleDegrees) { + this.targetTurretDegreesTurretRelative = angleDegrees; + angleDegrees += turretTrimDegrees; angleDegrees = wrap180(angleDegrees); double targetMotorRotations = angleDegrees / 180.0; this.targetTurretAngleMotorRevs = targetMotorRotations; + mmRequest.Position = targetMotorRotations; this.turretMotor.setControl( mmRequest @@ -128,44 +133,46 @@ private void setTurretAngleTurretRelativeDegrees(double angleDegrees) { */ @Override public void recalculateTurretMotorZeroPosition() { - // ensure the cache is fresh before using it for zeroing - this.cachedPinionEncoderRevs = computePinionEncoderRevs(); // absolute turret position from CRT this.calculatedMotorOffsetRevs = getTurretAngle() / 180.0; + turretMotor.setPosition(this.calculatedMotorOffsetRevs); } + /** + * Calculates the feedforward voltage to apply to the turret motor to counteract the rotation of the robot, based on the current angular velocity of the robot. + * @return the feedforward value to apply to the turret rotation + */ + private double calculateFeedforward() { + // get the current angular velocity of the robot in radians per second + double gyroOmegaRadPerSecond = drive.getState().Speeds.omegaRadiansPerSecond; + + double mechanismRotationsPerSecond = gyroOmegaRadPerSecond / Math.PI; + return feedforward.calculate(-mechanismRotationsPerSecond); + } + @Override public void refreshData() { StatusSignal.refreshAll(this.turretMotorPosition, this.pinionEncoderSignal, this.followerEncoderSignal); - this.cachedPinionEncoderRevs = computePinionEncoderRevs(); } @Override public void updateInputs(TurretIOInputs inputs) { - double robotRelativeAngleDeg = getTurretAngleRobotRelative(); - - inputs.fieldRelativeTurretAngleDegrees = getTurretAngleFieldRelative(); - inputs.turretRelativeTurretAngleDegrees = getTurretAngle(); - inputs.robotRelativeTurretAngleDegrees = robotRelativeAngleDeg; - inputs.turretAngleDegreesRobotRelativeFiltered = calculateFilteredMeasuredAngle(robotRelativeAngleDeg); + inputs.turretAngleDegreesFieldRelative = getTurretAngleFieldRelative(); + inputs.turretAngleDegreesTurretRelative = getTurretAngle(); + inputs.turretAngleDegreesRobotRelative = getTurretAngleRobotRelative(); inputs.targetTurretMotorRotations = this.targetTurretAngleMotorRevs; inputs.turretOffsetRotations = this.calculatedMotorOffsetRevs; - inputs.targetFieldRelativeTurretDegrees = this.targetTurretDegreesFieldRelative; - inputs.targetRobotRelativeTurretDegrees = this.processedTargetTurretDegreesFieldRelative; + inputs.targetTurretDegrees = this.targetTurretDegreesTurretRelative; + inputs.processedTargetTurretDegrees = this.processedTargetTurretDegreesFieldRelative; inputs.turretMotorPositionRotations = this.turretMotorPosition.getValueAsDouble(); inputs.pinionEncoderRotations = this.pinionEncoderSignal.getValueAsDouble(); inputs.followerEncoderRotations = this.followerEncoderSignal.getValueAsDouble(); inputs.rawTurretMechanismRotations = this.getTurretPositionRevs(); - inputs.pinionEncoderRevsCalculated = this.cachedPinionEncoderRevs; inputs.turretTrimDegrees = turretTrimDegrees; } - private double calculateFilteredMeasuredAngle(double rawMeasuredAngleDeg) { - return AngleUtils.filterWrappedAngleDeg(turretMeasurementFilter, rawMeasuredAngleDeg, -180.0, 180.0); - } - // CONFIGURATIONS /** @@ -180,7 +187,7 @@ private Pair getTurretMotionConfigs() { configs.kD = 1; configs.kS = 0.4; //kS; - configs.kV = 0.2; //kV; + configs.kV = 0.4; //kV; MotionMagicConfigs mmConfigs = new MotionMagicConfigs(); @@ -206,17 +213,6 @@ private FeedbackConfigs getTurretMotorFeedbackConfigs() { return configs; } - /** - * Get the encoder configurations for the turret encoders. - * @return the encoder configurations for the turret encoders - */ - public CANcoderConfiguration getEncoderConfigs() { - CANcoderConfiguration configs = new CANcoderConfiguration(); - // constrain reading between [0, 1) - configs.MagnetSensor.withAbsoluteSensorDiscontinuityPoint(1.0); - return configs; - } - /** * Get the software limit switch configurations for the turret motor, which define the forward and reverse limits of the turret based on the motor position. * @return the software limit switch configurations for the turret motor @@ -237,37 +233,60 @@ public void changeTurretTrim(double deltaDegrees) { this.turretTrimDegrees += deltaDegrees; } + /** + * Get the encoder configurations for the turret encoders. + * @return the encoder configurations for the turret encoders + */ + public CANcoderConfiguration getEncoderConfigs() { + CANcoderConfiguration configs = new CANcoderConfiguration(); + // constrain reading between [0, 1) + configs.MagnetSensor.withAbsoluteSensorDiscontinuityPoint(1.0); + return configs; + } + + public CANcoderConfiguration getPinionEncoderConfigs() { + CANcoderConfiguration configs = getEncoderConfigs(); + // configs.MagnetSensor.MagnetOffset = Turret.PINION_ENCODER_OFFSET; + configs.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; + + return configs; + } + + public CANcoderConfiguration getFollowerEncoderConfigs() { + CANcoderConfiguration configs = getEncoderConfigs(); + // configs.MagnetSensor.MagnetOffset = Turret.FOLLOWER_ENCODER_OFFSET; + configs.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + + return configs; + } + // CRT METHODS /** * Calculates the continuous position of the pinion (driving) encoder in revolutions * @return the continuous position of the pinion encoder in revolutions */ - private double computePinionEncoderRevs() { + @AutoLogOutput(key = "Turret/PinionEncoderRevsCalculated") + private double getPinionEncoderRevs() { double pinionEncoderReading = positiveMod(this.pinionEncoderSignal.getValueAsDouble(), 1.0); double followerEncoderReading = positiveMod(this.followerEncoderSignal.getValueAsDouble(), 1.0); double bestError = Double.MAX_VALUE; double bestPosition = lastPinionRevs; -// int searchCount = (int) Turret.FOLLOWER_ENCODER_TEETH; - - for (int k = -20; k < 20; k++) { + for (int k = -15; k <= 15; k++) { double assumedPinionRevs = pinionEncoderReading + k; double predictedFollowerReading = positiveMod(assumedPinionRevs * (Turret.PINION_ENCODER_TEETH / Turret.FOLLOWER_ENCODER_TEETH), 1.0); double predictionError = Math.abs(predictedFollowerReading - followerEncoderReading); - if (predictionError > 0.5) { predictionError = 1.0 - predictionError; } - // continuity penalty - // double continuityError = Math.abs(assumedPinionRevs - lastPinionRevs); - - double score = predictionError * 0.1; + double continuityError = Math.abs(assumedPinionRevs - lastPinionRevs); + double score = predictionError + continuityError * 0.001; if (score < bestError) { bestError = score; @@ -284,7 +303,7 @@ private double computePinionEncoderRevs() { * @return the continuous position of the turret in revolutions. */ private double getTurretPositionRevs() { - double rawPinionRevs = cachedPinionEncoderRevs; // use the value already computed in refreshData() + double rawPinionRevs = getPinionEncoderRevs(); double rawTurretRevs = rawPinionRevs * (Turret.PINION_ENCODER_TEETH / Turret.TURRET_GEAR_TEETH); // convert pinion revolutions to turret revolutions double wrapped = positiveMod(rawTurretRevs, Turret.ENCODER_COMBINED_PERIOD_TURRET_REV); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java index f08da0c..d95cc97 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java @@ -16,15 +16,10 @@ public class VisionIOPhotonCamera implements VisionIO, IORefresher { private final List estimatedRobotPoses; - private final List estimatedRobotPosesView; // unmodifiable view private final Supplier odometryPoseSupplier; - // reused output buffer so we don't allocate a new Pose3d[] on every loop - private Pose3d[] poseBuffer = new Pose3d[0]; - public VisionIOPhotonCamera(Supplier odometryPoseSupplier) { this.estimatedRobotPoses = new ArrayList<>(); - this.estimatedRobotPosesView = Collections.unmodifiableList(estimatedRobotPoses); this.odometryPoseSupplier = odometryPoseSupplier; } @@ -46,21 +41,14 @@ public void refreshData() { @Override public void updateInputs(VisionIOInputs inputs) { - int size = estimatedRobotPoses.size(); - - if (poseBuffer.length != size) { - poseBuffer = new Pose3d[size]; - } - for (int i = 0; i < size; i++) { - poseBuffer[i] = estimatedRobotPoses.get(i).estimatedPose; - } - inputs.estimatedRobotPoses = poseBuffer; + inputs.estimatedRobotPoses = estimatedRobotPoses.stream() + .map(pose -> pose.estimatedPose) + .toArray(Pose3d[]::new); } @Override public List getEstimatedRobotPoses() { - // return an unmodifiable view - return estimatedRobotPosesView; + return new ArrayList<>(estimatedRobotPoses); } } diff --git a/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java b/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java index 97650e8..f696f6e 100644 --- a/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java +++ b/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java @@ -22,49 +22,34 @@ public static List getCameras() { static { // register cameras - // registerCamera( - // new Camera( - // "left", - // new Transform3d( - // Inches.of(-13.8), - // Inches.of(7.5), - // Inches.of(22.3/4), - // new Rotation3d( - // Degrees.of(0), - // Degrees.of(60), - // Degrees.of(90) - // ) - // ) - // ) - // ); - // registerCamera( - // new Camera( - // "right", - // new Transform3d( - // Inches.of(13.8), - // Inches.of(5), - // Inches.of(6.3/4), - // new Rotation3d( - // Degrees.of(0), - // Degrees.of(60), - // Degrees.of(-90) - // ) - // ) - // ) - // ); + registerCamera( + new Camera( + "back", + new Transform3d( + Inches.of(-13.75), + Inches.of(-0.2), + Inches.of(8.475), + new Rotation3d( + Degrees.of(0), + Degrees.of(-65), + Degrees.of(180) + ) + ) + ) + ); registerCamera( new Camera( - "left", + "right", new Transform3d( - Inches.of(-29.5/2), - Inches.of(0), - Inches.of(6.75), + Inches.of(-0.95), + Inches.of(-13.75), + Inches.of(8.475), new Rotation3d( Degrees.of(0), - Degrees.of(-60), // negative pitch = tilted upward - Degrees.of(180) + Degrees.of(-65), // negative pitch = tilted upward + Degrees.of(-90) ) ) )