From fc9874e286d0d0f9ce9f1a698cdcc9d5659ec648 Mon Sep 17 00:00:00 2001 From: Eddieman855 <69999780+Eddieman855@users.noreply.github.com> Date: Fri, 2 Feb 2024 16:27:05 -0800 Subject: [PATCH] init commit --- .../frc2023/constants/ShooterConstants.java | 81 -------- .../frc2023/subsystems/Superstructure.java | 149 --------------- .../auto/AutoCommands.java | 22 ++- .../auto/AutonomousMode.java | 2 +- .../{frc2023 => frc2024}/auto/Repathing.java | 4 +- .../commands/DrivetrainCommands.java | 10 +- .../commands/IntakeCommands.java | 4 +- .../commands/KickerCommands.java | 8 +- .../commands/PivotCommands.java | 6 +- .../commands/ShooterCommands.java | 36 ++-- .../frc2024/commands/WristCommands.java | 54 ++++++ .../constants/AutoConstants.java | 2 +- .../constants/FieldConstants.java | 8 +- .../constants/GlobalConstants.java | 21 ++- .../constants/IntakeConstants.java | 4 +- .../constants/KickerConstants.java | 4 +- .../constants/PivotConstants.java | 19 +- .../frc2024/constants/ShooterConstants.java | 82 ++++++++ .../constants/SwerveDriveConstants.java | 2 +- .../constants/TunerConstants.java | 4 +- .../constants/VisionConstants.java | 2 +- .../frc2024/constants/WristConstants.java | 70 +++++++ .../{frc2023 => frc2024}/robot/Main.java | 2 +- .../{frc2023 => frc2024}/robot/Robot.java | 2 +- .../robot/RobotContainer.java | 161 +++++++++------- .../subsystems/Intake.java | 2 +- .../subsystems/Kicker.java | 2 +- .../subsystems/Pivot.java | 28 ++- .../subsystems/ShooterLeft.java} | 13 +- .../frc2024/subsystems/ShooterRight.java | 46 +++++ .../frc2024/subsystems/Superstructure.java | 175 ++++++++++++++++++ .../subsystems/SwerveDrive.java | 4 +- .../team3647/frc2024/subsystems/Wrist.java | 57 ++++++ .../util/AprilTagCamera.java | 2 +- .../util/AprilTagLimelight.java | 2 +- .../util/AprilTagPhotonVision.java | 4 +- .../{frc2023 => frc2024}/util/AutoDrive.java | 18 +- .../util/NeuralDetector.java | 4 +- .../util/TargetingUtil.java | 39 +++- .../util/VisionController.java | 4 +- .../util/VisionMeasurement.java | 2 +- src/main/java/team3647/lib/SwerveModule.java | 2 +- 42 files changed, 760 insertions(+), 403 deletions(-) delete mode 100644 src/main/java/team3647/frc2023/constants/ShooterConstants.java delete mode 100644 src/main/java/team3647/frc2023/subsystems/Superstructure.java rename src/main/java/team3647/{frc2023 => frc2024}/auto/AutoCommands.java (94%) rename src/main/java/team3647/{frc2023 => frc2024}/auto/AutonomousMode.java (94%) rename src/main/java/team3647/{frc2023 => frc2024}/auto/Repathing.java (96%) rename src/main/java/team3647/{frc2023 => frc2024}/commands/DrivetrainCommands.java (95%) rename src/main/java/team3647/{frc2023 => frc2024}/commands/IntakeCommands.java (92%) rename src/main/java/team3647/{frc2023 => frc2024}/commands/KickerCommands.java (84%) rename src/main/java/team3647/{frc2023 => frc2024}/commands/PivotCommands.java (92%) rename src/main/java/team3647/{frc2023 => frc2024}/commands/ShooterCommands.java (60%) create mode 100644 src/main/java/team3647/frc2024/commands/WristCommands.java rename src/main/java/team3647/{frc2023 => frc2024}/constants/AutoConstants.java (98%) rename src/main/java/team3647/{frc2023 => frc2024}/constants/FieldConstants.java (87%) rename src/main/java/team3647/{frc2023 => frc2024}/constants/GlobalConstants.java (79%) rename src/main/java/team3647/{frc2023 => frc2024}/constants/IntakeConstants.java (90%) rename src/main/java/team3647/{frc2023 => frc2024}/constants/KickerConstants.java (90%) rename src/main/java/team3647/{frc2023 => frc2024}/constants/PivotConstants.java (82%) create mode 100644 src/main/java/team3647/frc2024/constants/ShooterConstants.java rename src/main/java/team3647/{frc2023 => frc2024}/constants/SwerveDriveConstants.java (99%) rename src/main/java/team3647/{frc2023 => frc2024}/constants/TunerConstants.java (99%) rename src/main/java/team3647/{frc2023 => frc2024}/constants/VisionConstants.java (97%) create mode 100644 src/main/java/team3647/frc2024/constants/WristConstants.java rename src/main/java/team3647/{frc2023 => frc2024}/robot/Main.java (96%) rename src/main/java/team3647/{frc2023 => frc2024}/robot/Robot.java (99%) rename src/main/java/team3647/{frc2023 => frc2024}/robot/RobotContainer.java (58%) rename src/main/java/team3647/{frc2023 => frc2024}/subsystems/Intake.java (95%) rename src/main/java/team3647/{frc2023 => frc2024}/subsystems/Kicker.java (94%) rename src/main/java/team3647/{frc2023 => frc2024}/subsystems/Pivot.java (77%) rename src/main/java/team3647/{frc2023/subsystems/Shooter.java => frc2024/subsystems/ShooterLeft.java} (87%) create mode 100644 src/main/java/team3647/frc2024/subsystems/ShooterRight.java create mode 100644 src/main/java/team3647/frc2024/subsystems/Superstructure.java rename src/main/java/team3647/{frc2023 => frc2024}/subsystems/SwerveDrive.java (99%) create mode 100644 src/main/java/team3647/frc2024/subsystems/Wrist.java rename src/main/java/team3647/{frc2023 => frc2024}/util/AprilTagCamera.java (82%) rename src/main/java/team3647/{frc2023 => frc2024}/util/AprilTagLimelight.java (99%) rename src/main/java/team3647/{frc2023 => frc2024}/util/AprilTagPhotonVision.java (96%) rename src/main/java/team3647/{frc2023 => frc2024}/util/AutoDrive.java (89%) rename src/main/java/team3647/{frc2023 => frc2024}/util/NeuralDetector.java (98%) rename src/main/java/team3647/{frc2023 => frc2024}/util/TargetingUtil.java (86%) rename src/main/java/team3647/{frc2023 => frc2024}/util/VisionController.java (95%) rename src/main/java/team3647/{frc2023 => frc2024}/util/VisionMeasurement.java (97%) diff --git a/src/main/java/team3647/frc2023/constants/ShooterConstants.java b/src/main/java/team3647/frc2023/constants/ShooterConstants.java deleted file mode 100644 index 8116602..0000000 --- a/src/main/java/team3647/frc2023/constants/ShooterConstants.java +++ /dev/null @@ -1,81 +0,0 @@ -package team3647.frc2023.constants; - -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.configs.TalonFXConfigurator; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.util.Units; - -public class ShooterConstants { - public static final TalonFX kTopRoller = new TalonFX(GlobalConstants.ShooterIds.kShooterTopId); - public static final TalonFX kBottomRoller = - new TalonFX(GlobalConstants.ShooterIds.kShooterBottomId); - private static final TalonFXConfiguration kMasterConfig = new TalonFXConfiguration(); - - public static final double kGearboxReduction = 2.0; - public static final double kWheelDiameterMeters = Units.inchesToMeters(2); - public static final double kWheelRotationMeters = kWheelDiameterMeters * Math.PI; - public static final double kNativeVelToSurfaceMpS = - kWheelRotationMeters / GlobalConstants.kFalconTicksPerRotation * kGearboxReduction; - - public static final double kS = 0.23; - public static final double kV = 2.6; - public static final double kA = 0.0; - - public static final SimpleMotorFeedforward ff = new SimpleMotorFeedforward(kS, kV, kA); - - public static final double masterKP = 0.03; - public static final double masterKI = 0; - public static final double masterKD = 0; - - public static final double kNominalVoltage = 11.0; - public static final double kStallCurrent = 55.0; - public static final double kMaxCurrent = 40.0; - - static { - Slot0Configs kTopSlot0 = new Slot0Configs(); - Slot0Configs kBottomSlot0 = new Slot0Configs(); - - MotorOutputConfigs kTopMotor = new MotorOutputConfigs(); - MotorOutputConfigs kBottomMotor = new MotorOutputConfigs(); - - CurrentLimitsConfigs kTopCurrent = new CurrentLimitsConfigs(); - CurrentLimitsConfigs kBottomCurrent = new CurrentLimitsConfigs(); - - TalonFXConfigurator kTopRollerConfigurator = kTopRoller.getConfigurator(); - TalonFXConfigurator kBottomRollerConfigurator = kBottomRoller.getConfigurator(); - - kTopRollerConfigurator.apply(kMasterConfig); - - kTopMotor.NeutralMode = NeutralModeValue.Brake; - kBottomMotor.NeutralMode = NeutralModeValue.Brake; - kTopMotor.Inverted = InvertedValue.CounterClockwise_Positive; - kBottomMotor.Inverted = InvertedValue.CounterClockwise_Positive; - - kTopSlot0.kP = masterKP; - kTopSlot0.kI = masterKI; - kTopSlot0.kD = masterKD; - - kBottomSlot0.kP = masterKP; - kBottomSlot0.kI = masterKI; - kBottomSlot0.kD = masterKD; - - kTopCurrent.StatorCurrentLimitEnable = false; - kBottomCurrent.StatorCurrentLimitEnable = false; - // kTopCurrent.StatorCurrentLimit = kMaxCurrent; - // kBottomCurrent.StatorCurrentLimit = kMaxCurrent; - - kTopRollerConfigurator.apply(kTopMotor); - kTopRollerConfigurator.apply(kTopCurrent); - kTopRollerConfigurator.apply(kTopSlot0); - - kBottomRollerConfigurator.apply(kTopMotor); - kBottomRollerConfigurator.apply(kBottomCurrent); - kBottomRollerConfigurator.apply(kBottomSlot0); - } -} diff --git a/src/main/java/team3647/frc2023/subsystems/Superstructure.java b/src/main/java/team3647/frc2023/subsystems/Superstructure.java deleted file mode 100644 index bb7ad27..0000000 --- a/src/main/java/team3647/frc2023/subsystems/Superstructure.java +++ /dev/null @@ -1,149 +0,0 @@ -package team3647.frc2023.subsystems; - -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import java.util.function.DoubleSupplier; -import team3647.frc2023.commands.IntakeCommands; -import team3647.frc2023.commands.KickerCommands; -import team3647.frc2023.commands.PivotCommands; -import team3647.frc2023.commands.ShooterCommands; - -public class Superstructure { - - private final Intake intake; - private final Kicker kicker; - private final Shooter shooter; - private final Pivot pivot; - public final IntakeCommands intakeCommands; - public final KickerCommands kickerCommands; - public final ShooterCommands shooterCommands; - public final PivotCommands pivotCommands; - - private final DoubleSupplier pivotAngleSupplier; - private final double stowAngle = 40; - private final double shootSpeed; - private boolean hasPiece = false; - private boolean isShooting = false; - - public Superstructure( - Intake intake, - Kicker kicker, - Shooter shooter, - Pivot pivot, - DoubleSupplier pivotAngleSupplier, - double shootSpeed) { - this.intake = intake; - this.kicker = kicker; - this.shooter = shooter; - this.pivot = pivot; - this.pivotAngleSupplier = pivotAngleSupplier; - this.shootSpeed = shootSpeed; - - intakeCommands = new IntakeCommands(intake); - kickerCommands = new KickerCommands(kicker); - shooterCommands = new ShooterCommands(shooter); - pivotCommands = new PivotCommands(pivot); - } - - public Command feed() { - return kickerCommands.kick(); - } - - public Command spinUp() { - SlewRateLimiter filter = new SlewRateLimiter(3); - return shooterCommands.setVelocity(() -> filter.calculate(shootSpeed)); - } - - public double getDesiredSpeed() { - return shootSpeed; - } - - public boolean getPiece() { - return hasPiece; - } - - public Command setPiece() { - return Commands.runOnce(() -> this.hasPiece = true); - } - - public Command ejectPiece() { - return Commands.runOnce(() -> this.hasPiece = false); - } - - public Command isSHooting() { - return Commands.runOnce(() -> this.isShooting = true); - } - - public Command doneShooting() { - return Commands.runOnce(() -> this.isShooting = false); - } - - public boolean getIsShooting() { - return isShooting; - } - - public Command shoot() { - return Commands.parallel( - prep(), - spinUp(), - Commands.sequence( - Commands.waitSeconds(1), - // Commands.waitUntil(() -> shooter.velocityReached(shootSpeed, 1)) - // .withTimeout(3), - Commands.parallel(isSHooting(), intake(), ejectPiece()) - .withTimeout(1))) - .finallyDo((interrupted) -> doneShooting()); - } - - public Command shootManual() { - return Commands.parallel( - prepManual(), - spinUp(), - Commands.sequence( - Commands.waitSeconds(2), Commands.parallel(intake(), ejectPiece()))); - } - - public Command shootStow() { - return shoot().andThen(Commands.waitSeconds(0.5)).andThen(stowFromShoot()); - } - - public Command outtake() { - return Commands.parallel(intakeCommands.outtake(), kickerCommands.unkick()); - } - - public Command prep() { - SmartDashboard.putNumber("pivot supplier", pivotAngleSupplier.getAsDouble()); - return pivotCommands.setAngle(() -> pivotAngleSupplier.getAsDouble()); - } - - public Command prepManual() { - return pivotCommands.setAngle(() -> SmartDashboard.getNumber("pivot interp angle", 40)); - } - - public Command oscillateToCenter() { - return Commands.parallel(intakeCommands.oscillate(), kickerCommands.oscillate()); - } - - public Command stowFromShoot() { - return Commands.parallel( - pivotCommands.setAngle(() -> stowAngle), - shooterCommands.kill(), - kickerCommands.kill(), - intakeCommands.kill()) - .until(() -> pivot.angleReached(stowAngle, 5)); - } - - public Command intake() { - return Commands.parallel(intakeCommands.intake(), kickerCommands.kick()); - } - - public Command stowIntake() { - return Commands.parallel(intakeCommands.kill(), kickerCommands.kill()); - } - - public Command slightReverse() { - return outtake().withTimeout(0.4).andThen(stowIntake()); - } -} diff --git a/src/main/java/team3647/frc2023/auto/AutoCommands.java b/src/main/java/team3647/frc2024/auto/AutoCommands.java similarity index 94% rename from src/main/java/team3647/frc2023/auto/AutoCommands.java rename to src/main/java/team3647/frc2024/auto/AutoCommands.java index 466026e..4864f47 100644 --- a/src/main/java/team3647/frc2023/auto/AutoCommands.java +++ b/src/main/java/team3647/frc2024/auto/AutoCommands.java @@ -1,4 +1,4 @@ -package team3647.frc2023.auto; +package team3647.frc2024.auto; import com.choreo.lib.Choreo; import com.choreo.lib.ChoreoControlFunction; @@ -15,11 +15,11 @@ import java.util.function.BooleanSupplier; import java.util.function.Consumer; import java.util.function.Supplier; -import team3647.frc2023.constants.AutoConstants; -import team3647.frc2023.constants.FieldConstants; -import team3647.frc2023.subsystems.Superstructure; -import team3647.frc2023.subsystems.SwerveDrive; -import team3647.frc2023.util.TargetingUtil; +import team3647.frc2024.constants.AutoConstants; +import team3647.frc2024.constants.FieldConstants; +import team3647.frc2024.subsystems.Superstructure; +import team3647.frc2024.subsystems.SwerveDrive; +import team3647.frc2024.util.TargetingUtil; public class AutoCommands { private final SwerveDrive swerve; @@ -115,8 +115,12 @@ public Command spinUp() { return Commands.parallel(superstructure.spinUp(), superstructure.feed()); } - public double getPivotAngleByPose(Pose2d pose) { - return targeting.getPivotAngleByPose(pose) * 180 / Math.PI; + public double getPivotAngleByPose(Supplier pose) { + return targeting.getPivotAngleByPose(pose.get()) * 180 / Math.PI; + } + + public double getPivotAngle() { + return targeting.getPivotAngle(FieldConstants.kBlueSpeaker) * 180 / Math.PI; } public Command target() { @@ -153,7 +157,7 @@ public Command pathAndShootWithOverride(String path, Alliance color) { public Command followChoreoPathWithOverrideAndPivot(String path, Alliance color) { return Commands.deadline( followChoreoPathWithOverride(path, color), - superstructure.pivotCommands.setAngle(() -> getPivotAngleByPose(getFinal(path)))); + superstructure.pivotCommands.setAngle(() -> getPivotAngle())); } public Command followChoreoPathWithOverride(String path, Alliance color) { diff --git a/src/main/java/team3647/frc2023/auto/AutonomousMode.java b/src/main/java/team3647/frc2024/auto/AutonomousMode.java similarity index 94% rename from src/main/java/team3647/frc2023/auto/AutonomousMode.java rename to src/main/java/team3647/frc2024/auto/AutonomousMode.java index 3c63c61..a8e8b93 100644 --- a/src/main/java/team3647/frc2023/auto/AutonomousMode.java +++ b/src/main/java/team3647/frc2024/auto/AutonomousMode.java @@ -1,4 +1,4 @@ -package team3647.frc2023.auto; +package team3647.frc2024.auto; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/team3647/frc2023/auto/Repathing.java b/src/main/java/team3647/frc2024/auto/Repathing.java similarity index 96% rename from src/main/java/team3647/frc2023/auto/Repathing.java rename to src/main/java/team3647/frc2024/auto/Repathing.java index 2c71a87..381f6d4 100644 --- a/src/main/java/team3647/frc2023/auto/Repathing.java +++ b/src/main/java/team3647/frc2024/auto/Repathing.java @@ -1,7 +1,7 @@ -package team3647.frc2023.auto; +package team3647.frc2024.auto; import edu.wpi.first.math.geometry.Pose2d; -import team3647.frc2023.constants.FieldConstants; +import team3647.frc2024.constants.FieldConstants; public class Repathing { diff --git a/src/main/java/team3647/frc2023/commands/DrivetrainCommands.java b/src/main/java/team3647/frc2024/commands/DrivetrainCommands.java similarity index 95% rename from src/main/java/team3647/frc2023/commands/DrivetrainCommands.java rename to src/main/java/team3647/frc2024/commands/DrivetrainCommands.java index cf28286..ea60d21 100644 --- a/src/main/java/team3647/frc2023/commands/DrivetrainCommands.java +++ b/src/main/java/team3647/frc2024/commands/DrivetrainCommands.java @@ -1,4 +1,4 @@ -package team3647.frc2023.commands; +package team3647.frc2024.commands; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; @@ -12,9 +12,9 @@ import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import java.util.function.Supplier; -import team3647.frc2023.constants.FieldConstants; -import team3647.frc2023.subsystems.SwerveDrive; -import team3647.frc2023.util.AutoDrive.DriveMode; +import team3647.frc2024.constants.FieldConstants; +import team3647.frc2024.subsystems.SwerveDrive; +import team3647.frc2024.util.AutoDrive.DriveMode; import team3647.lib.LinearRegression; public class DrivetrainCommands { @@ -49,7 +49,7 @@ public Command driveVisionTeleop( // var translation = new Translation2d(motionXComponent, motionYComponent); - if (mode == DriveMode.SHOOT_ON_THE_MOVE && enabeld) { + if (mode != DriveMode.NONE && enabeld) { motionTurnComponent = autoDriveTwist2d.dtheta; } diff --git a/src/main/java/team3647/frc2023/commands/IntakeCommands.java b/src/main/java/team3647/frc2024/commands/IntakeCommands.java similarity index 92% rename from src/main/java/team3647/frc2023/commands/IntakeCommands.java rename to src/main/java/team3647/frc2024/commands/IntakeCommands.java index 6f988ba..9509099 100644 --- a/src/main/java/team3647/frc2023/commands/IntakeCommands.java +++ b/src/main/java/team3647/frc2024/commands/IntakeCommands.java @@ -1,10 +1,10 @@ -package team3647.frc2023.commands; +package team3647.frc2024.commands; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; import java.util.Set; -import team3647.frc2023.subsystems.Intake; +import team3647.frc2024.subsystems.Intake; public class IntakeCommands { diff --git a/src/main/java/team3647/frc2023/commands/KickerCommands.java b/src/main/java/team3647/frc2024/commands/KickerCommands.java similarity index 84% rename from src/main/java/team3647/frc2023/commands/KickerCommands.java rename to src/main/java/team3647/frc2024/commands/KickerCommands.java index ee5992f..b6ffc2c 100644 --- a/src/main/java/team3647/frc2023/commands/KickerCommands.java +++ b/src/main/java/team3647/frc2024/commands/KickerCommands.java @@ -1,10 +1,10 @@ -package team3647.frc2023.commands; +package team3647.frc2024.commands; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; import java.util.Set; -import team3647.frc2023.subsystems.Kicker; +import team3647.frc2024.subsystems.Kicker; public class KickerCommands { @@ -12,6 +12,10 @@ public Command kick() { return Commands.run(() -> kicker.openLoop(0.5), kicker); } + public Command slowFeed() { + return Commands.run(() -> kicker.openLoop(0.1), kicker); + } + public Command unkick() { return Commands.run(() -> kicker.openLoop(-0.1), kicker); } diff --git a/src/main/java/team3647/frc2023/commands/PivotCommands.java b/src/main/java/team3647/frc2024/commands/PivotCommands.java similarity index 92% rename from src/main/java/team3647/frc2023/commands/PivotCommands.java rename to src/main/java/team3647/frc2024/commands/PivotCommands.java index 45d6417..fcc181d 100644 --- a/src/main/java/team3647/frc2023/commands/PivotCommands.java +++ b/src/main/java/team3647/frc2024/commands/PivotCommands.java @@ -1,4 +1,4 @@ -package team3647.frc2023.commands; +package team3647.frc2024.commands; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -6,8 +6,8 @@ import edu.wpi.first.wpilibj2.command.Subsystem; import java.util.Set; import java.util.function.DoubleSupplier; -import team3647.frc2023.constants.PivotConstants; -import team3647.frc2023.subsystems.Pivot; +import team3647.frc2024.constants.PivotConstants; +import team3647.frc2024.subsystems.Pivot; public class PivotCommands { diff --git a/src/main/java/team3647/frc2023/commands/ShooterCommands.java b/src/main/java/team3647/frc2024/commands/ShooterCommands.java similarity index 60% rename from src/main/java/team3647/frc2023/commands/ShooterCommands.java rename to src/main/java/team3647/frc2024/commands/ShooterCommands.java index b2872a5..02bf74e 100644 --- a/src/main/java/team3647/frc2023/commands/ShooterCommands.java +++ b/src/main/java/team3647/frc2024/commands/ShooterCommands.java @@ -1,4 +1,4 @@ -package team3647.frc2023.commands; +package team3647.frc2024.commands; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -9,21 +9,31 @@ import java.util.Map; import java.util.Set; import java.util.function.DoubleSupplier; -import team3647.frc2023.subsystems.Shooter; + +import team3647.frc2024.subsystems.ShooterLeft; +import team3647.frc2024.subsystems.ShooterRight; import team3647.lib.LinearRegression; public class ShooterCommands { public Command shoot(DoubleSupplier bill) { - return Commands.run(() -> shooter.openLoop(bill.getAsDouble()), shooter); + return Commands.run(() -> {shooterRight.openLoop(bill.getAsDouble() * 0.8); shooterLeft.openLoop(bill.getAsDouble());}, shooterRight, shooterLeft); } public Command setVelocity(DoubleSupplier bill) { - return Commands.run(() -> shooter.setVelocity(bill.getAsDouble()), shooter); + return Commands.run(() -> {shooterRight.setVelocity(bill.getAsDouble() * 0.8); shooterLeft.setVelocity(bill.getAsDouble());}, shooterRight, shooterLeft); } public Command kill() { - return Commands.run(() -> shooter.openLoop(0), shooter); + return Commands.run(() -> {shooterRight.openLoop(0); shooterLeft.openLoop(0);}, shooterRight, shooterLeft); + } + + public Command killRight() { + return Commands.run(() -> shooterRight.openLoop(0), shooterRight); + } + + public Command killLeft() { + return Commands.run(() -> shooterLeft.openLoop(0), shooterLeft); } public Command characterize() { @@ -32,8 +42,8 @@ public Command characterize() { return Commands.runEnd( () -> { double desiredVoltage = filter.calculate(5); - shooter.setVoltage(desiredVoltage); - voltageVelocityMap.put(desiredVoltage, shooter.getVelocity()); + shooterLeft.setVoltage(desiredVoltage); + voltageVelocityMap.put(desiredVoltage, shooterLeft.getVelocity()); }, () -> { var xArray = @@ -53,15 +63,17 @@ public Command characterize() { SmartDashboard.putNumber("shooter kS", kS); SmartDashboard.putNumber("shooter kV", kV); }, - shooter); + shooterLeft); } private final Set requirements; - public ShooterCommands(Shooter shooter) { - this.shooter = shooter; - this.requirements = Set.of(shooter); + public ShooterCommands(ShooterRight shooterRight, ShooterLeft shooterLeft) { + this.shooterRight = shooterRight; + this.shooterLeft = shooterLeft; + this.requirements = Set.of(shooterRight, shooterLeft); } - public final Shooter shooter; + public final ShooterRight shooterRight; + public final ShooterLeft shooterLeft; } diff --git a/src/main/java/team3647/frc2024/commands/WristCommands.java b/src/main/java/team3647/frc2024/commands/WristCommands.java new file mode 100644 index 0000000..41317ed --- /dev/null +++ b/src/main/java/team3647/frc2024/commands/WristCommands.java @@ -0,0 +1,54 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package team3647.frc2024.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.Subsystem; +import java.util.Set; +import java.util.function.DoubleSupplier; +import team3647.frc2024.constants.WristConstants; +import team3647.frc2024.subsystems.Wrist; + +/** Add your docs here. */ +public class WristCommands { + + public Command openloop(DoubleSupplier demand) { + return Commands.run(() -> wrist.setOpenloop(demand.getAsDouble()), wrist); + } + + public Command setAngle(double angle) { + return Commands.run(() -> wrist.setAngle(angle), wrist); + } + + public Command holdPositionAtCall() { + return new Command() { + double degreeAtStart = WristConstants.kInitialDegree; + + @Override + public void initialize() { + degreeAtStart = wrist.getAngle(); + } + + @Override + public void execute() { + wrist.setAngle(degreeAtStart); + } + + @Override + public Set getRequirements() { + return requirements; + } + }; + } + + private final Wrist wrist; + private final Set requirements; + + public WristCommands(Wrist wrist) { + this.wrist = wrist; + this.requirements = Set.of(wrist); + } +} \ No newline at end of file diff --git a/src/main/java/team3647/frc2023/constants/AutoConstants.java b/src/main/java/team3647/frc2024/constants/AutoConstants.java similarity index 98% rename from src/main/java/team3647/frc2023/constants/AutoConstants.java rename to src/main/java/team3647/frc2024/constants/AutoConstants.java index 4c5d7a2..2e65568 100644 --- a/src/main/java/team3647/frc2023/constants/AutoConstants.java +++ b/src/main/java/team3647/frc2024/constants/AutoConstants.java @@ -1,4 +1,4 @@ -package team3647.frc2023.constants; +package team3647.frc2024.constants; import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.util.PIDConstants; diff --git a/src/main/java/team3647/frc2023/constants/FieldConstants.java b/src/main/java/team3647/frc2024/constants/FieldConstants.java similarity index 87% rename from src/main/java/team3647/frc2023/constants/FieldConstants.java rename to src/main/java/team3647/frc2024/constants/FieldConstants.java index 4c4c886..8b270fe 100644 --- a/src/main/java/team3647/frc2023/constants/FieldConstants.java +++ b/src/main/java/team3647/frc2024/constants/FieldConstants.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package team3647.frc2023.constants; +package team3647.frc2024.constants; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -25,7 +25,11 @@ public class FieldConstants { public static final double kSpeakerHeight = Units.inchesToMeters(83); - public static final Pose3d kBlueSpeaker = new Pose3d(0, 5.5, kSpeakerHeight, new Rotation3d()); + public static final Pose3d kBlueSpeaker = new Pose3d(0, 5.2, kSpeakerHeight, new Rotation3d()); + + public static final double kAmpHeight = Units.inchesToMeters(35); + + public static final Pose3d kBlueAmp = new Pose3d(1.957, 8.218, kAmpHeight, new Rotation3d()); public static final Pose2d kBlueAmpAlign = new Pose2d( diff --git a/src/main/java/team3647/frc2023/constants/GlobalConstants.java b/src/main/java/team3647/frc2024/constants/GlobalConstants.java similarity index 79% rename from src/main/java/team3647/frc2023/constants/GlobalConstants.java rename to src/main/java/team3647/frc2024/constants/GlobalConstants.java index e4bdd49..eaeaa5a 100644 --- a/src/main/java/team3647/frc2023/constants/GlobalConstants.java +++ b/src/main/java/team3647/frc2024/constants/GlobalConstants.java @@ -1,4 +1,4 @@ -package team3647.frc2023.constants; +package team3647.frc2024.constants; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -42,24 +42,31 @@ private SwerveDriveIds() {} public static final class PivotIds { public static final int kMasterId = 20; + public static final int kSlaveId = 21; } public static final class ShooterIds { - public static final int kShooterTopId = 21; - public static final int kShooterBottomId = 22; + public static final int kShooterLeftId = 22; + public static final int kShooterRightId = 23; } public static final class KickerIds { - public static final int kMasterId = 23; + public static final int kMasterId = 24; + } + + public static final class WristIds { + public static final int kMasterId = 25; } public static final class IntakeIds { - public static final int kMasterId = 24; - public static final int kSlaveId = 25; + public static final int kMasterId = 26; + public static final int kSlaveId = 27; } public static final class SensorIds { - public static final int tofID = 26; + public static final int wristId = 28; + public static final int pivotBackId = 29; + public static final int pivotFrontId = 30; } private GlobalConstants() {} diff --git a/src/main/java/team3647/frc2023/constants/IntakeConstants.java b/src/main/java/team3647/frc2024/constants/IntakeConstants.java similarity index 90% rename from src/main/java/team3647/frc2023/constants/IntakeConstants.java rename to src/main/java/team3647/frc2024/constants/IntakeConstants.java index 7f3f1de..6df012c 100644 --- a/src/main/java/team3647/frc2023/constants/IntakeConstants.java +++ b/src/main/java/team3647/frc2024/constants/IntakeConstants.java @@ -1,4 +1,4 @@ -package team3647.frc2023.constants; +package team3647.frc2024.constants; import com.ctre.phoenix6.configs.*; import com.ctre.phoenix6.hardware.TalonFX; @@ -22,7 +22,7 @@ public class IntakeConstants { kMasterCurrent.StatorCurrentLimitEnable = true; kMasterCurrent.StatorCurrentLimit = kMaxCurrent; kMasterMotorOutput.NeutralMode = NeutralModeValue.Brake; - kMasterMotorOutput.Inverted = InvertedValue.Clockwise_Positive; + kMasterMotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; kMasterConfigurator.apply(kMasterMotorOutput); } diff --git a/src/main/java/team3647/frc2023/constants/KickerConstants.java b/src/main/java/team3647/frc2024/constants/KickerConstants.java similarity index 90% rename from src/main/java/team3647/frc2023/constants/KickerConstants.java rename to src/main/java/team3647/frc2024/constants/KickerConstants.java index 8fbe14b..9fd7b82 100644 --- a/src/main/java/team3647/frc2023/constants/KickerConstants.java +++ b/src/main/java/team3647/frc2024/constants/KickerConstants.java @@ -1,4 +1,4 @@ -package team3647.frc2023.constants; +package team3647.frc2024.constants; import com.ctre.phoenix6.configs.*; import com.ctre.phoenix6.hardware.TalonFX; @@ -21,7 +21,7 @@ public class KickerConstants { kMasterCurrent.StatorCurrentLimitEnable = true; kMasterCurrent.StatorCurrentLimit = kMaxCurrent; kMasterMotorOutput.NeutralMode = NeutralModeValue.Brake; - kMasterMotorOutput.Inverted = InvertedValue.Clockwise_Positive; + kMasterMotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; kMasterConfigurator.apply(kMasterMotorOutput); } diff --git a/src/main/java/team3647/frc2023/constants/PivotConstants.java b/src/main/java/team3647/frc2024/constants/PivotConstants.java similarity index 82% rename from src/main/java/team3647/frc2023/constants/PivotConstants.java rename to src/main/java/team3647/frc2024/constants/PivotConstants.java index eb65ac8..59ef4bb 100644 --- a/src/main/java/team3647/frc2023/constants/PivotConstants.java +++ b/src/main/java/team3647/frc2024/constants/PivotConstants.java @@ -1,9 +1,10 @@ -package team3647.frc2023.constants; +package team3647.frc2024.constants; import com.ctre.phoenix6.configs.*; import com.ctre.phoenix6.controls.*; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.*; +import com.playingwithfusion.TimeOfFlight; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; @@ -11,11 +12,12 @@ public class PivotConstants { public static final TalonFX kMaster = new TalonFX(GlobalConstants.PivotIds.kMasterId); + public static final TalonFX kSlave = new TalonFX(GlobalConstants.PivotIds.kSlaveId); public static final boolean kMasterInvert = true; private static final TalonFXConfiguration kMasterConfig = new TalonFXConfiguration(); - private static final double kGearBoxRatio = 1 / 240.0; + private static final double kGearBoxRatio = 1 / 41.67; public static final double kNativePosToDegrees = kGearBoxRatio / GlobalConstants.kFalconTicksPerRotation * 360.0; @@ -25,8 +27,8 @@ public class PivotConstants { public static final double kMaxVelocityTicks = (400.0 / kNativeVelToDPS); public static final double kMaxAccelerationTicks = (200.0 / kNativeVelToDPS); - public static final double kMinDegree = 10.0; - public static final double kMaxDegree = 50.0; + public static final double kMinDegree = -20; + public static final double kMaxDegree = 70; private static final double masterKP = 0.3; private static final double masterKI = 0; @@ -42,12 +44,13 @@ public class PivotConstants { public static final Transform3d robotToPivot = new Transform3d( - new Translation3d(Units.inchesToMeters(5), 0, Units.inchesToMeters(4.5)), + new Translation3d(Units.inchesToMeters(0.3), 0, Units.inchesToMeters(17.2)), new Rotation3d()); - public static double[][] kPivotMap = { - {1.5, 50}, {2.15, 46.5}, {2.4, 44}, {3.6, 43.5}, - }; + public static final TimeOfFlight tofBack = + new TimeOfFlight(GlobalConstants.SensorIds.pivotBackId); + public static final TimeOfFlight tofFront = + new TimeOfFlight(GlobalConstants.SensorIds.pivotFrontId); static { Slot0Configs kMasterSlot0 = new Slot0Configs(); diff --git a/src/main/java/team3647/frc2024/constants/ShooterConstants.java b/src/main/java/team3647/frc2024/constants/ShooterConstants.java new file mode 100644 index 0000000..fd779e1 --- /dev/null +++ b/src/main/java/team3647/frc2024/constants/ShooterConstants.java @@ -0,0 +1,82 @@ +package team3647.frc2024.constants; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.util.Units; + +public class ShooterConstants { + public static final TalonFX kRightRoller = new TalonFX(GlobalConstants.ShooterIds.kShooterRightId); + public static final TalonFX kLeftRoller = + new TalonFX(GlobalConstants.ShooterIds.kShooterLeftId); + private static final TalonFXConfiguration kMasterConfig = new TalonFXConfiguration(); + + public static final double kGearboxReduction = 2.0; + public static final double kWheelDiameterMeters = Units.inchesToMeters(2); + public static final double kWheelRotationMeters = kWheelDiameterMeters * Math.PI; + public static final double kNativeVelToSurfaceMpS = + kWheelRotationMeters / GlobalConstants.kFalconTicksPerRotation * kGearboxReduction; + + // tune ff + public static final double kS = 0.23; + public static final double kV = 2.6; + public static final double kA = 0.0; + + public static final SimpleMotorFeedforward ff = new SimpleMotorFeedforward(kS, kV, kA); + + public static final double masterKP = 0.03; + public static final double masterKI = 0; + public static final double masterKD = 0; + + public static final double kNominalVoltage = 11.0; + public static final double kStallCurrent = 55.0; + public static final double kMaxCurrent = 40.0; + + static { + Slot0Configs kRightSlot0 = new Slot0Configs(); + Slot0Configs kLeftSlot0 = new Slot0Configs(); + + MotorOutputConfigs kRightMotor = new MotorOutputConfigs(); + MotorOutputConfigs kLeftMotor = new MotorOutputConfigs(); + + CurrentLimitsConfigs kRightCurrent = new CurrentLimitsConfigs(); + CurrentLimitsConfigs kLeftCurrent = new CurrentLimitsConfigs(); + + TalonFXConfigurator kRightRollerConfigurator = kRightRoller.getConfigurator(); + TalonFXConfigurator kLeftRollerConfigurator = kLeftRoller.getConfigurator(); + + kRightRollerConfigurator.apply(kMasterConfig); + + kRightMotor.NeutralMode = NeutralModeValue.Brake; + kLeftMotor.NeutralMode = NeutralModeValue.Brake; + kRightMotor.Inverted = InvertedValue.CounterClockwise_Positive; + kLeftMotor.Inverted = InvertedValue.Clockwise_Positive; + + kRightSlot0.kP = masterKP; + kRightSlot0.kI = masterKI; + kRightSlot0.kD = masterKD; + + kLeftSlot0.kP = masterKP; + kLeftSlot0.kI = masterKI; + kLeftSlot0.kD = masterKD; + + kRightCurrent.StatorCurrentLimitEnable = false; + kLeftCurrent.StatorCurrentLimitEnable = false; + // kRightCurrent.StatorCurrentLimit = kMaxCurrent; + // kLeftCurrent.StatorCurrentLimit = kMaxCurrent; + + kRightRollerConfigurator.apply(kRightMotor); + kRightRollerConfigurator.apply(kRightCurrent); + kRightRollerConfigurator.apply(kRightSlot0); + + kLeftRollerConfigurator.apply(kRightMotor); + kLeftRollerConfigurator.apply(kLeftCurrent); + kLeftRollerConfigurator.apply(kLeftSlot0); + } +} diff --git a/src/main/java/team3647/frc2023/constants/SwerveDriveConstants.java b/src/main/java/team3647/frc2024/constants/SwerveDriveConstants.java similarity index 99% rename from src/main/java/team3647/frc2023/constants/SwerveDriveConstants.java rename to src/main/java/team3647/frc2024/constants/SwerveDriveConstants.java index df3d707..da66e80 100644 --- a/src/main/java/team3647/frc2023/constants/SwerveDriveConstants.java +++ b/src/main/java/team3647/frc2024/constants/SwerveDriveConstants.java @@ -1,4 +1,4 @@ -package team3647.frc2023.constants; +package team3647.frc2024.constants; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.configs.*; diff --git a/src/main/java/team3647/frc2023/constants/TunerConstants.java b/src/main/java/team3647/frc2024/constants/TunerConstants.java similarity index 99% rename from src/main/java/team3647/frc2023/constants/TunerConstants.java rename to src/main/java/team3647/frc2024/constants/TunerConstants.java index 6e2335b..9eb0669 100644 --- a/src/main/java/team3647/frc2023/constants/TunerConstants.java +++ b/src/main/java/team3647/frc2024/constants/TunerConstants.java @@ -1,4 +1,4 @@ -package team3647.frc2023.constants; +package team3647.frc2024.constants; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; @@ -23,7 +23,7 @@ public class TunerConstants { .withKA(0); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput public static final Slot0Configs driveGains = - new Slot0Configs().withKP(0.25).withKI(0).withKD(0).withKS(0.2).withKV(0.1).withKA(0); + new Slot0Configs().withKP(0.25).withKI(0).withKD(0).withKS(0.2).withKV(0.12).withKA(0); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors public static final ClosedLoopOutputType steerClosedLoopOutput = ClosedLoopOutputType.Voltage; diff --git a/src/main/java/team3647/frc2023/constants/VisionConstants.java b/src/main/java/team3647/frc2024/constants/VisionConstants.java similarity index 97% rename from src/main/java/team3647/frc2023/constants/VisionConstants.java rename to src/main/java/team3647/frc2024/constants/VisionConstants.java index d3ee574..f4b63ce 100644 --- a/src/main/java/team3647/frc2023/constants/VisionConstants.java +++ b/src/main/java/team3647/frc2024/constants/VisionConstants.java @@ -1,4 +1,4 @@ -package team3647.frc2023.constants; +package team3647.frc2024.constants; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; diff --git a/src/main/java/team3647/frc2024/constants/WristConstants.java b/src/main/java/team3647/frc2024/constants/WristConstants.java new file mode 100644 index 0000000..b5414cf --- /dev/null +++ b/src/main/java/team3647/frc2024/constants/WristConstants.java @@ -0,0 +1,70 @@ +package team3647.frc2024.constants; + +import com.ctre.phoenix6.configs.*; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.*; +import com.playingwithfusion.TimeOfFlight; + +public final class WristConstants { + public static final TalonFX kMaster = new TalonFX(GlobalConstants.WristIds.kMasterId); + + private static final double kGearBoxRatio = 1.0 / 41.67; + private static final TalonFXConfiguration kMasterConfig = new TalonFXConfiguration(); + + public static final double kNativePosToDegrees = kGearBoxRatio * 360.0; + + public static final double kNativeVelToDPS = kNativePosToDegrees; + + private static final double masterKP = 0.5; + private static final double masterKI = 0; + private static final double masterKD = 0; + + public static final double nominalVoltage = 11.0; + public static final double kStallCurrent = 20.0; + public static final double kMaxCurrent = 15; + + public static final double kG = 0.0; + + public static final double kMaxVelocityTicks = (600.0 / kNativeVelToDPS) * 8; + public static final double kMaxAccelerationTicks = (300.0 / kNativeVelToDPS) * 8; + + public static final double kMinDegree = 0; + public static final double kMaxDegree = 90; + + public static final double kInitialDegree = 0; + + public static final TimeOfFlight tof = new TimeOfFlight(GlobalConstants.SensorIds.wristId); + + static { + Slot0Configs kMasterSlot0 = new Slot0Configs(); + VoltageConfigs kMasterVoltage = new VoltageConfigs(); + CurrentLimitsConfigs kMasterCurrent = new CurrentLimitsConfigs(); + MotionMagicConfigs kMasterMotionMagic = new MotionMagicConfigs(); + MotorOutputConfigs kMasterMotorOutput = new MotorOutputConfigs(); + SoftwareLimitSwitchConfigs kMasterSoftLimit = new SoftwareLimitSwitchConfigs(); + TalonFXConfigurator kMasterConfigurator = kMaster.getConfigurator(); + kMasterConfigurator.apply(kMasterConfig); + + kMasterSlot0.kP = masterKP; + kMasterSlot0.kI = masterKI; + kMasterSlot0.kD = masterKD; + kMasterCurrent.StatorCurrentLimitEnable = true; + kMasterCurrent.StatorCurrentLimit = kMaxCurrent; + kMasterMotionMagic.MotionMagicAcceleration = kMaxVelocityTicks; + kMasterMotionMagic.MotionMagicCruiseVelocity = kMaxAccelerationTicks; + kMasterMotorOutput.NeutralMode = NeutralModeValue.Brake; + kMasterMotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + kMasterSoftLimit.ForwardSoftLimitEnable = true; + kMasterSoftLimit.ForwardSoftLimitThreshold = kMaxDegree / kNativePosToDegrees; + kMasterSoftLimit.ReverseSoftLimitEnable = true; + kMasterSoftLimit.ReverseSoftLimitThreshold = kMinDegree / kNativePosToDegrees; + + kMasterConfigurator.apply(kMasterSlot0); + kMasterConfigurator.apply(kMasterVoltage); + kMasterConfigurator.apply(kMasterMotionMagic); + kMasterConfigurator.apply(kMasterMotorOutput); + kMasterConfigurator.apply(kMasterSoftLimit); + } + + private WristConstants() {} +} diff --git a/src/main/java/team3647/frc2023/robot/Main.java b/src/main/java/team3647/frc2024/robot/Main.java similarity index 96% rename from src/main/java/team3647/frc2023/robot/Main.java rename to src/main/java/team3647/frc2024/robot/Main.java index ddd6377..f95b738 100644 --- a/src/main/java/team3647/frc2023/robot/Main.java +++ b/src/main/java/team3647/frc2024/robot/Main.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package team3647.frc2023.robot; +package team3647.frc2024.robot; import edu.wpi.first.wpilibj.RobotBase; diff --git a/src/main/java/team3647/frc2023/robot/Robot.java b/src/main/java/team3647/frc2024/robot/Robot.java similarity index 99% rename from src/main/java/team3647/frc2023/robot/Robot.java rename to src/main/java/team3647/frc2024/robot/Robot.java index 1cbb35c..a08e5a5 100644 --- a/src/main/java/team3647/frc2023/robot/Robot.java +++ b/src/main/java/team3647/frc2024/robot/Robot.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package team3647.frc2023.robot; +package team3647.frc2024.robot; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; diff --git a/src/main/java/team3647/frc2023/robot/RobotContainer.java b/src/main/java/team3647/frc2024/robot/RobotContainer.java similarity index 58% rename from src/main/java/team3647/frc2023/robot/RobotContainer.java rename to src/main/java/team3647/frc2024/robot/RobotContainer.java index 92badb6..fd1726f 100644 --- a/src/main/java/team3647/frc2023/robot/RobotContainer.java +++ b/src/main/java/team3647/frc2024/robot/RobotContainer.java @@ -1,6 +1,5 @@ -package team3647.frc2023.robot; +package team3647.frc2024.robot; -import com.playingwithfusion.TimeOfFlight; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; @@ -8,33 +7,37 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.button.Trigger; -import team3647.frc2023.auto.AutoCommands; -import team3647.frc2023.auto.AutonomousMode; -import team3647.frc2023.commands.DrivetrainCommands; -import team3647.frc2023.constants.FieldConstants; -// import team3647.frc2023.auto.AutoCommands; -// // import team3647.frc2023.auto.AutoCommands; -// import team3647.frc2023.auto.AutonomousMode; -import team3647.frc2023.constants.GlobalConstants; -import team3647.frc2023.constants.IntakeConstants; -import team3647.frc2023.constants.KickerConstants; -import team3647.frc2023.constants.PivotConstants; -import team3647.frc2023.constants.ShooterConstants; -import team3647.frc2023.constants.SwerveDriveConstants; -import team3647.frc2023.constants.TunerConstants; -import team3647.frc2023.constants.VisionConstants; -import team3647.frc2023.subsystems.Intake; -import team3647.frc2023.subsystems.Kicker; -import team3647.frc2023.subsystems.Pivot; -import team3647.frc2023.subsystems.Shooter; -import team3647.frc2023.subsystems.Superstructure; -import team3647.frc2023.subsystems.SwerveDrive; -import team3647.frc2023.util.AprilTagPhotonVision; -import team3647.frc2023.util.AutoDrive; -import team3647.frc2023.util.AutoDrive.DriveMode; -import team3647.frc2023.util.NeuralDetector; -import team3647.frc2023.util.TargetingUtil; -import team3647.frc2023.util.VisionController; +import java.util.function.BooleanSupplier; +import team3647.frc2024.auto.AutoCommands; +import team3647.frc2024.auto.AutonomousMode; +import team3647.frc2024.commands.DrivetrainCommands; +import team3647.frc2024.constants.FieldConstants; +// import team3647.frc2024.auto.AutoCommands; +// // import team3647.frc2024.auto.AutoCommands; +// import team3647.frc2024.auto.AutonomousMode; +import team3647.frc2024.constants.GlobalConstants; +import team3647.frc2024.constants.IntakeConstants; +import team3647.frc2024.constants.KickerConstants; +import team3647.frc2024.constants.PivotConstants; +import team3647.frc2024.constants.ShooterConstants; +import team3647.frc2024.constants.SwerveDriveConstants; +import team3647.frc2024.constants.TunerConstants; +import team3647.frc2024.constants.VisionConstants; +import team3647.frc2024.constants.WristConstants; +import team3647.frc2024.subsystems.Intake; +import team3647.frc2024.subsystems.Kicker; +import team3647.frc2024.subsystems.Pivot; +import team3647.frc2024.subsystems.ShooterLeft; +import team3647.frc2024.subsystems.ShooterRight; +import team3647.frc2024.subsystems.Superstructure; +import team3647.frc2024.subsystems.SwerveDrive; +import team3647.frc2024.subsystems.Wrist; +import team3647.frc2024.util.AprilTagPhotonVision; +import team3647.frc2024.util.AutoDrive; +import team3647.frc2024.util.AutoDrive.DriveMode; +import team3647.frc2024.util.NeuralDetector; +import team3647.frc2024.util.TargetingUtil; +import team3647.frc2024.util.VisionController; import team3647.lib.GroupPrinter; import team3647.lib.inputs.Joysticks; @@ -52,7 +55,8 @@ public class RobotContainer { public RobotContainer() { pdh.clearStickyFaults(); - scheduler.registerSubsystem(swerve, shooter, intake, kicker, pivot); + scheduler.registerSubsystem( + swerve, shooterRight, shooterLeft, intake, kicker, pivot, wrist); configureDefaultCommands(); configureButtonBindings(); @@ -65,37 +69,34 @@ public RobotContainer() { private void configureButtonBindings() { - mainController.buttonX.whileTrue(drivetrainCommands.characterize()); - mainController.leftBumper.whileTrue(autoDrive.setMode(DriveMode.SHOOT_ON_THE_MOVE)); + mainController.rightTrigger.whileTrue(autoDrive.setMode(DriveMode.SHOOT_ON_THE_MOVE)); + mainController.leftTrigger.whileTrue(autoDrive.setMode(DriveMode.SHOOT_AT_AMP)); mainController - .leftBumper + .rightTrigger .whileTrue(superstructure.shoot()) .onFalse(superstructure.stowFromShoot()) .onFalse(superstructure.ejectPiece()); mainController - .rightBumper - .whileTrue(superstructure.shootManual()) + .leftTrigger + .whileTrue(superstructure.shoot()) .onFalse(superstructure.stowFromShoot()) .onFalse(superstructure.ejectPiece()); - mainController.leftBumper.onFalse(autoDrive.setMode(DriveMode.NONE)); - - mainController.buttonB.and(() -> !piece.getAsBoolean()).whileTrue(superstructure.intake()); - mainController.buttonB.onFalse(superstructure.intakeCommands.kill()); - mainController.buttonB.onFalse(superstructure.kickerCommands.kill()); - mainController.buttonX.onTrue(superstructure.outtake()); - mainController.buttonX.onFalse(superstructure.intakeCommands.kill()); - mainController.buttonX.onFalse(superstructure.kickerCommands.kill()); - mainController.buttonA.onTrue(superstructure.ejectPiece()); + mainController.rightTrigger.onFalse(autoDrive.setMode(DriveMode.NONE)); + mainController.leftTrigger.onFalse(autoDrive.setMode(DriveMode.NONE)); - tofPiece.onTrue(superstructure.setPiece()); - tofPiece.and(() -> !superstructure.getIsShooting()).onTrue(superstructure.slightReverse()); - - // mainController.buttonY.whileTrue(drivetrainCommands.characterize()); + mainController + .leftBumper + .and(() -> !superstructure.getPiece()) + .whileTrue(superstructure.intake()); + setPiece.onTrue(superstructure.setPiece()); + piece.and(isIntaking).onTrue(superstructure.passToShooter()); + mainController.leftBumper.onFalse(superstructure.intakeCommands.kill()); + mainController.leftBumper.onFalse(superstructure.kickerCommands.kill()); - // mainController.leftBumper.onTrue(superstructure.shootStow()); - // mainController.rightBumper.onTrue(superstructure.shootStow()); + mainController.buttonA.onTrue(superstructure.ejectPiece()); - // piece.whileTrue(superstructure.stowIntake()); + mainController.dPadUp.onTrue(targetingUtil.offsetUp()); + mainController.dPadDown.onTrue(targetingUtil.offsetDown()); } private void configureDefaultCommands() { @@ -112,7 +113,9 @@ private void configureDefaultCommands() { pivot.setDefaultCommand(superstructure.pivotCommands.holdPositionAtCall()); intake.setDefaultCommand(superstructure.intakeCommands.kill()); kicker.setDefaultCommand(superstructure.kickerCommands.kill()); - shooter.setDefaultCommand(superstructure.shooterCommands.kill()); + shooterRight.setDefaultCommand(superstructure.shooterCommands.killRight()); + shooterLeft.setDefaultCommand(superstructure.shooterCommands.killLeft()); + wrist.setDefaultCommand(superstructure.wristCommands.holdPositionAtCall()); } public void teleopInit() {} @@ -121,8 +124,6 @@ void configTestCommands() {} public void configureSmartDashboardLogging() { SmartDashboard.putNumber("pivot interp angle", 40); - printer.addBoolean("tof", () -> tofPiece.getAsBoolean()); - printer.addBoolean("shooht", () -> superstructure.getIsShooting()); // printer.addDouble("auto drive", () -> autoDrive.getVelocities().dtheta); } @@ -132,8 +133,6 @@ public Command getAutonomousCommand() { private final Joysticks mainController = new Joysticks(0); - private TimeOfFlight tof = new TimeOfFlight(GlobalConstants.SensorIds.tofID); - public final SwerveDrive swerve = new SwerveDrive( TunerConstants.DrivetrainConstants, @@ -146,16 +145,36 @@ public Command getAutonomousCommand() { TunerConstants.BackLeft, TunerConstants.BackRight); - public final Shooter shooter = - new Shooter( - ShooterConstants.kTopRoller, - ShooterConstants.kBottomRoller, + public final ShooterRight shooterRight = + new ShooterRight( + ShooterConstants.kRightRoller, ShooterConstants.kNativeVelToSurfaceMpS, 1, ShooterConstants.kNominalVoltage, 0.02, ShooterConstants.ff); + public final ShooterLeft shooterLeft = + new ShooterLeft( + ShooterConstants.kLeftRoller, + ShooterConstants.kNativeVelToSurfaceMpS, + 1, + ShooterConstants.kNominalVoltage, + 0.02, + ShooterConstants.ff); + + public final Wrist wrist = + new Wrist( + WristConstants.kMaster, + WristConstants.kNativeVelToDPS, + WristConstants.kNativePosToDegrees, + WristConstants.kMinDegree, + WristConstants.kMaxDegree, + WristConstants.nominalVoltage, + WristConstants.kG, + 0.02, + WristConstants.tof); + public final Kicker kicker = new Kicker(KickerConstants.kMaster, 1, 1, KickerConstants.kNominalVoltage, 0.02); @@ -171,13 +190,16 @@ public Command getAutonomousCommand() { public final Pivot pivot = new Pivot( PivotConstants.kMaster, + PivotConstants.kSlave, PivotConstants.kNativeVelToDPS, PivotConstants.kNativePosToDegrees, PivotConstants.kMinDegree, PivotConstants.kMaxDegree, PivotConstants.nominalVoltage, PivotConstants.maxKG, - 0.02); + 0.02, + PivotConstants.tofBack, + PivotConstants.tofFront); private final PowerDistribution pdh = new PowerDistribution(1, ModuleType.kRev); @@ -197,6 +219,7 @@ public Command getAutonomousCommand() { public final TargetingUtil targetingUtil = new TargetingUtil( FieldConstants.kBlueSpeaker, + FieldConstants.kBlueAmp, swerve::getOdoPose, swerve::getFieldRelativeChassisSpeeds, PivotConstants.robotToPivot); @@ -207,8 +230,10 @@ public Command getAutonomousCommand() { new Superstructure( intake, kicker, - shooter, + shooterRight, + shooterLeft, pivot, + wrist, autoDrive::getPivotAngle, targetingUtil.exitVelocity()); @@ -219,12 +244,10 @@ public Command getAutonomousCommand() { final GroupPrinter printer = GroupPrinter.getInstance(); - Trigger piece = new Trigger(() -> superstructure.getPiece()); + private final Trigger piece = new Trigger(() -> superstructure.getPiece()); + + private final Trigger setPiece = new Trigger(() -> wrist.hasPiece()); - Trigger tofPiece = - new Trigger( - () -> - (tof.getRange() < 100 - && !superstructure.getIsShooting() - && !DriverStation.isAutonomous())); + private final BooleanSupplier isIntaking = + () -> mainController.leftBumper.getAsBoolean() || DriverStation.isAutonomous(); } diff --git a/src/main/java/team3647/frc2023/subsystems/Intake.java b/src/main/java/team3647/frc2024/subsystems/Intake.java similarity index 95% rename from src/main/java/team3647/frc2023/subsystems/Intake.java rename to src/main/java/team3647/frc2024/subsystems/Intake.java index 530371e..894ae0a 100644 --- a/src/main/java/team3647/frc2023/subsystems/Intake.java +++ b/src/main/java/team3647/frc2024/subsystems/Intake.java @@ -1,4 +1,4 @@ -package team3647.frc2023.subsystems; +package team3647.frc2024.subsystems; import com.ctre.phoenix6.hardware.TalonFX; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/team3647/frc2023/subsystems/Kicker.java b/src/main/java/team3647/frc2024/subsystems/Kicker.java similarity index 94% rename from src/main/java/team3647/frc2023/subsystems/Kicker.java rename to src/main/java/team3647/frc2024/subsystems/Kicker.java index 0d5e3fa..3a470dc 100644 --- a/src/main/java/team3647/frc2023/subsystems/Kicker.java +++ b/src/main/java/team3647/frc2024/subsystems/Kicker.java @@ -1,4 +1,4 @@ -package team3647.frc2023.subsystems; +package team3647.frc2024.subsystems; import com.ctre.phoenix6.hardware.TalonFX; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/team3647/frc2023/subsystems/Pivot.java b/src/main/java/team3647/frc2024/subsystems/Pivot.java similarity index 77% rename from src/main/java/team3647/frc2023/subsystems/Pivot.java rename to src/main/java/team3647/frc2024/subsystems/Pivot.java index 740f5e1..83a7330 100644 --- a/src/main/java/team3647/frc2023/subsystems/Pivot.java +++ b/src/main/java/team3647/frc2024/subsystems/Pivot.java @@ -1,6 +1,7 @@ -package team3647.frc2023.subsystems; +package team3647.frc2024.subsystems; import com.ctre.phoenix6.hardware.TalonFX; +import com.playingwithfusion.TimeOfFlight; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; @@ -12,26 +13,35 @@ public class Pivot extends TalonFXSubsystem { - private double minAngle; - private double maxAngle; + private final double minAngle; + private final double maxAngle; - private double maxKG; + private final double maxKG; private Pose3d pose = new Pose3d(new Translation3d(), new Rotation3d(0, 0, 0)); + private final TimeOfFlight tofBack; + private final TimeOfFlight tofFront; + public Pivot( TalonFX master, + TalonFX slave, double ticksToMetersPerSec, double ticksToMeters, double minAngle, double maxAngle, double nominalVoltage, double maxKG, - double kDt) { + double kDt, + TimeOfFlight tofBack, + TimeOfFlight tofFront) { super(master, ticksToMetersPerSec, ticksToMeters, nominalVoltage, kDt); + super.addFollower(slave, false); this.minAngle = minAngle; this.maxAngle = maxAngle; this.maxKG = maxKG; + this.tofBack = tofBack; + this.tofFront = tofFront; } @Override @@ -75,6 +85,14 @@ public boolean angleReached(double targetAngle, double threshold) { return Math.abs(getAngle() - targetAngle) < threshold; } + public boolean backPiece() { + return tofBack.getRange() < 100; + } + + public boolean frontPiece() { + return tofFront.getRange() < 100; + } + @Override public String getName() { return "Pivot"; diff --git a/src/main/java/team3647/frc2023/subsystems/Shooter.java b/src/main/java/team3647/frc2024/subsystems/ShooterLeft.java similarity index 87% rename from src/main/java/team3647/frc2023/subsystems/Shooter.java rename to src/main/java/team3647/frc2024/subsystems/ShooterLeft.java index a1ecd28..cb50151 100644 --- a/src/main/java/team3647/frc2023/subsystems/Shooter.java +++ b/src/main/java/team3647/frc2024/subsystems/ShooterLeft.java @@ -1,24 +1,23 @@ -package team3647.frc2023.subsystems; +package team3647.frc2024.subsystems; + +import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.hardware.TalonFX; + import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import org.littletonrobotics.junction.Logger; import team3647.lib.TalonFXSubsystem; -public class Shooter extends TalonFXSubsystem { - +public class ShooterLeft extends TalonFXSubsystem{ private final SimpleMotorFeedforward ff; - public Shooter( + public ShooterLeft( TalonFX master, - TalonFX follower, double ticksToMetersPerSec, double ticksToMeters, double nominalVoltage, double kDt, SimpleMotorFeedforward ff) { super(master, ticksToMetersPerSec, ticksToMeters, nominalVoltage, kDt); - super.addFollower(follower, false); this.ff = ff; } diff --git a/src/main/java/team3647/frc2024/subsystems/ShooterRight.java b/src/main/java/team3647/frc2024/subsystems/ShooterRight.java new file mode 100644 index 0000000..a346389 --- /dev/null +++ b/src/main/java/team3647/frc2024/subsystems/ShooterRight.java @@ -0,0 +1,46 @@ +package team3647.frc2024.subsystems; + +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import org.littletonrobotics.junction.Logger; +import team3647.lib.TalonFXSubsystem; + +public class ShooterRight extends TalonFXSubsystem { + + private final SimpleMotorFeedforward ff; + + public ShooterRight( + TalonFX master, + double ticksToMetersPerSec, + double ticksToMeters, + double nominalVoltage, + double kDt, + SimpleMotorFeedforward ff) { + super(master, ticksToMetersPerSec, ticksToMeters, nominalVoltage, kDt); + this.ff = ff; + } + + public void openLoop(double demand) { + super.setOpenloop(demand); + Logger.recordOutput(getName() + "/openLoop", demand); + } + + public void setVelocity(double velocity) { + super.setVelocity(velocity, ff.calculate(velocity)); + Logger.recordOutput(getName() + "/velocity", velocity); + } + + public void setVoltage(double voltage) { + super.setVoltage(voltage); + Logger.recordOutput(getName() + "/voltage", voltage); + } + + public boolean velocityReached(double setpoint, double threshold) { + return Math.abs(super.getVelocity() - setpoint) < threshold; + } + + @Override + public String getName() { + return "Shooter"; + } +} diff --git a/src/main/java/team3647/frc2024/subsystems/Superstructure.java b/src/main/java/team3647/frc2024/subsystems/Superstructure.java new file mode 100644 index 0000000..a49440e --- /dev/null +++ b/src/main/java/team3647/frc2024/subsystems/Superstructure.java @@ -0,0 +1,175 @@ +package team3647.frc2024.subsystems; + +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import java.util.function.DoubleSupplier; +import team3647.frc2024.commands.IntakeCommands; +import team3647.frc2024.commands.KickerCommands; +import team3647.frc2024.commands.PivotCommands; +import team3647.frc2024.commands.ShooterCommands; +import team3647.frc2024.commands.WristCommands; + +public class Superstructure { + + private final Intake intake; + private final Kicker kicker; + private final ShooterRight shooterRight; + private final ShooterLeft shooterLeft; + private final Pivot pivot; + private final Wrist wrist; + public final IntakeCommands intakeCommands; + public final KickerCommands kickerCommands; + public final ShooterCommands shooterCommands; + public final PivotCommands pivotCommands; + public final WristCommands wristCommands; + + private final DoubleSupplier pivotAngleSupplier; + private final double pivotStowAngle = 40; + private final double pivotIntakeAngle = -20; + private final double wristStowAngle = 90; + private final double wristIntakeAngle = 0; + private final double shootSpeed; + private boolean hasPiece = false; + + public Superstructure( + Intake intake, + Kicker kicker, + ShooterRight shooterRight, + ShooterLeft shooterLeft, + Pivot pivot, + Wrist wrist, + DoubleSupplier pivotAngleSupplier, + double shootSpeed) { + this.intake = intake; + this.kicker = kicker; + this.shooterRight = shooterRight; + this.shooterLeft = shooterLeft; + this.pivot = pivot; + this.pivotAngleSupplier = pivotAngleSupplier; + this.shootSpeed = shootSpeed; + this.wrist = wrist; + + intakeCommands = new IntakeCommands(intake); + kickerCommands = new KickerCommands(kicker); + shooterCommands = new ShooterCommands(shooterRight, shooterLeft); + pivotCommands = new PivotCommands(pivot); + wristCommands = new WristCommands(wrist); + } + + public Command feed() { + return kickerCommands.kick(); + } + + public Command spinUp() { + SlewRateLimiter filter = new SlewRateLimiter(3); + return shooterCommands.setVelocity(() -> filter.calculate(shootSpeed)); + } + + public double getDesiredSpeed() { + return shootSpeed; + } + + public boolean getPiece() { + return hasPiece; + } + + public Command setPiece() { + return Commands.runOnce(() -> this.hasPiece = true); + } + + public Command ejectPiece() { + return Commands.runOnce(() -> this.hasPiece = false); + } + + public Command shoot() { + return Commands.parallel( + prep(), + spinUp(), + Commands.sequence( + Commands.waitUntil(() -> shooterLeft.velocityReached(shootSpeed, 1)) + .withTimeout(2), + Commands.parallel(feed(), ejectPiece()).withTimeout(1))); + } + + public Command shootManual() { + return Commands.parallel( + prepManual(), + spinUp(), + Commands.sequence( + Commands.waitSeconds(2), Commands.parallel(intake(), ejectPiece()))); + } + + public Command shootStow() { + return shoot().andThen(Commands.waitSeconds(0.5)).andThen(stowFromShoot()); + } + + public Command prep() { + SmartDashboard.putNumber("pivot supplier", pivotAngleSupplier.getAsDouble()); + return pivotCommands.setAngle(() -> pivotAngleSupplier.getAsDouble()); + } + + public Command prepManual() { + return pivotCommands.setAngle(() -> SmartDashboard.getNumber("pivot interp angle", 40)); + } + + public Command stowFromShoot() { + return Commands.parallel( + pivotCommands.setAngle(() -> pivotStowAngle), + shooterCommands.kill(), + kickerCommands.kill()) + .until(() -> pivot.angleReached(pivotStowAngle, 5)); + } + + public Command intake() { + return Commands.parallel(wristCommands.setAngle(wristIntakeAngle), intakeCommands.intake()); + } + + public Command passToShooter() { + return Commands.parallel( + intakeCommands.kill(), + wristCommands.setAngle(wristStowAngle), + pivotCommands.setAngle(() -> pivotIntakeAngle)) + .until( + () -> + wrist.angleReached(wristStowAngle, 1) + && pivot.angleReached(pivotIntakeAngle, 1)) + .andThen(shootThrough()); + } + + public Command shootThrough() { + return Commands.parallel(intakeCommands.intake(), kickerCommands.kick()) + .until(() -> pivot.backPiece()) + .andThen(stowIntake()); + } + + public Command stowIntake() { + if (pivot.frontPiece()) { + Commands.parallel( + intakeCommands.kill(), + pivotCommands.setAngle(() -> pivotStowAngle), + slightReverse()); + } + return Commands.parallel( + intakeCommands.kill(), + pivotCommands.setAngle(() -> pivotStowAngle), + slightForwards()); + } + + public Command slightForwards() { + return kickerCommands + .slowFeed() + .until(() -> pivot.frontPiece()) + .withTimeout(0.4) + .andThen(kickerCommands.kill()); + } + + public Command slightReverse() { + return kickerCommands + .unkick() + .until(() -> !pivot.frontPiece()) + .withTimeout(0.4) + .andThen(kickerCommands.kill()); + } +} diff --git a/src/main/java/team3647/frc2023/subsystems/SwerveDrive.java b/src/main/java/team3647/frc2024/subsystems/SwerveDrive.java similarity index 99% rename from src/main/java/team3647/frc2023/subsystems/SwerveDrive.java rename to src/main/java/team3647/frc2024/subsystems/SwerveDrive.java index d4c22d8..f9f5050 100644 --- a/src/main/java/team3647/frc2023/subsystems/SwerveDrive.java +++ b/src/main/java/team3647/frc2024/subsystems/SwerveDrive.java @@ -1,4 +1,4 @@ -package team3647.frc2023.subsystems; +package team3647.frc2024.subsystems; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; @@ -18,7 +18,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -import team3647.frc2023.util.VisionMeasurement; +import team3647.frc2024.util.VisionMeasurement; import team3647.lib.PeriodicSubsystem; public class SwerveDrive extends SwerveDrivetrain implements PeriodicSubsystem { diff --git a/src/main/java/team3647/frc2024/subsystems/Wrist.java b/src/main/java/team3647/frc2024/subsystems/Wrist.java new file mode 100644 index 0000000..920bcb3 --- /dev/null +++ b/src/main/java/team3647/frc2024/subsystems/Wrist.java @@ -0,0 +1,57 @@ +package team3647.frc2024.subsystems; + +import com.ctre.phoenix6.hardware.TalonFX; +import com.playingwithfusion.TimeOfFlight; +import edu.wpi.first.math.util.Units; +import team3647.lib.TalonFXSubsystem; + +public class Wrist extends TalonFXSubsystem { + private final double minDegree; + private final double maxDegree; + private final double kG; + private final TimeOfFlight tof; + + public Wrist( + TalonFX master, + double ticksToDegsPerSec, + double ticksToDegs, + double minDegree, + double maxDegree, + double nominalVoltage, + double kG, + double kDt, + TimeOfFlight tof) { + super(master, ticksToDegsPerSec, ticksToDegs, nominalVoltage, kDt); + this.minDegree = minDegree; + this.maxDegree = maxDegree; + this.kG = kG; + this.tof = tof; + } + + @Override + public void setEncoder(double degree) { + super.setEncoder(degree); + } + + public boolean hasPiece() { + return this.tof.getRange() < 300; + } + + public double getAngle() { + return super.getPosition(); + } + + public void setAngle(double angle) { + var ffVolts = kG * Math.cos(Units.degreesToRadians(angle)); + this.setPositionMotionMagic(angle, ffVolts); + } + + public boolean angleReached(double targetAngle, double threshold) { + return Math.abs(getAngle() - targetAngle) < threshold; + } + + @Override + public String getName() { + return "Wrist"; + } +} diff --git a/src/main/java/team3647/frc2023/util/AprilTagCamera.java b/src/main/java/team3647/frc2024/util/AprilTagCamera.java similarity index 82% rename from src/main/java/team3647/frc2023/util/AprilTagCamera.java rename to src/main/java/team3647/frc2024/util/AprilTagCamera.java index 9109041..9ba793d 100644 --- a/src/main/java/team3647/frc2023/util/AprilTagCamera.java +++ b/src/main/java/team3647/frc2024/util/AprilTagCamera.java @@ -1,4 +1,4 @@ -package team3647.frc2023.util; +package team3647.frc2024.util; import java.util.Optional; diff --git a/src/main/java/team3647/frc2023/util/AprilTagLimelight.java b/src/main/java/team3647/frc2024/util/AprilTagLimelight.java similarity index 99% rename from src/main/java/team3647/frc2023/util/AprilTagLimelight.java rename to src/main/java/team3647/frc2024/util/AprilTagLimelight.java index fc56693..3f49e8a 100644 --- a/src/main/java/team3647/frc2023/util/AprilTagLimelight.java +++ b/src/main/java/team3647/frc2024/util/AprilTagLimelight.java @@ -1,4 +1,4 @@ -// package team3647.frc2023.util; +// package team3647.frc2024.util; // import edu.wpi.first.math.VecBuilder; // import edu.wpi.first.math.geometry.Pose2d; diff --git a/src/main/java/team3647/frc2023/util/AprilTagPhotonVision.java b/src/main/java/team3647/frc2024/util/AprilTagPhotonVision.java similarity index 96% rename from src/main/java/team3647/frc2023/util/AprilTagPhotonVision.java rename to src/main/java/team3647/frc2024/util/AprilTagPhotonVision.java index 53b7318..878f28c 100644 --- a/src/main/java/team3647/frc2023/util/AprilTagPhotonVision.java +++ b/src/main/java/team3647/frc2024/util/AprilTagPhotonVision.java @@ -1,4 +1,4 @@ -package team3647.frc2023.util; +package team3647.frc2024.util; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; @@ -51,7 +51,7 @@ public Optional QueueToInputs() { } var visionPose = update.get().estimatedPose.toPose2d(); final var stdDevs = - VecBuilder.fill(0.005, 0.005, 0.005) + VecBuilder.fill(0.05, 0.05, 0.05) .times(GeomUtil.distance(drivePose.get(), visionPose)); VisionMeasurement measurement = VisionMeasurement.fromEstimatedRobotPose(update.get(), stdDevs); diff --git a/src/main/java/team3647/frc2023/util/AutoDrive.java b/src/main/java/team3647/frc2024/util/AutoDrive.java similarity index 89% rename from src/main/java/team3647/frc2023/util/AutoDrive.java rename to src/main/java/team3647/frc2024/util/AutoDrive.java index 2971b53..9beb6b6 100644 --- a/src/main/java/team3647/frc2023/util/AutoDrive.java +++ b/src/main/java/team3647/frc2024/util/AutoDrive.java @@ -1,4 +1,4 @@ -package team3647.frc2023.util; +package team3647.frc2024.util; import com.pathplanner.lib.path.GoalEndState; import com.pathplanner.lib.path.PathPlannerPath; @@ -15,9 +15,9 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import java.util.List; -import team3647.frc2023.constants.AutoConstants; -import team3647.frc2023.constants.FieldConstants; -import team3647.frc2023.subsystems.SwerveDrive; +import team3647.frc2024.constants.AutoConstants; +import team3647.frc2024.constants.FieldConstants; +import team3647.frc2024.subsystems.SwerveDrive; import team3647.lib.team6328.VirtualSubsystem; public class AutoDrive extends VirtualSubsystem { @@ -55,7 +55,7 @@ public AutoDrive(SwerveDrive swerve, NeuralDetector detector, TargetingUtil targ public enum DriveMode { INTAKE_FLOOR_PIECE, - ALIGN_TO_AMP, + SHOOT_AT_AMP, SHOOT_ON_THE_MOVE, NONE } @@ -65,7 +65,13 @@ public void periodic() { // if (this.mode == DriveMode.SHOOT_ON_THE_MOVE) { targetRot = targeting.shootAtSpeaker().rotation; // } + if (this.mode == DriveMode.SHOOT_AT_AMP) { + targetRot = targeting.shootAtAmp().rotation; + } SmartDashboard.putNumber("auto drive", getRot()); + SmartDashboard.putNumber( + "pivot setpoint", + targeting.getPivotAngle(FieldConstants.kBlueSpeaker) * 180 / Math.PI); detector.pieceCoordinate(swerve::getOdoPose).ifPresent(this::setTargetPose); } @@ -99,6 +105,8 @@ public double getPivotAngle() { switch (mode) { case SHOOT_ON_THE_MOVE: return Units.radiansToDegrees(targeting.shootAtSpeaker().pivot); + case SHOOT_AT_AMP: + return Units.radiansToDegrees(targeting.shootAtAmp().pivot); default: return 50; } diff --git a/src/main/java/team3647/frc2023/util/NeuralDetector.java b/src/main/java/team3647/frc2024/util/NeuralDetector.java similarity index 98% rename from src/main/java/team3647/frc2023/util/NeuralDetector.java rename to src/main/java/team3647/frc2024/util/NeuralDetector.java index 6ca8f15..efdd336 100644 --- a/src/main/java/team3647/frc2023/util/NeuralDetector.java +++ b/src/main/java/team3647/frc2024/util/NeuralDetector.java @@ -1,4 +1,4 @@ -package team3647.frc2023.util; +package team3647.frc2024.util; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -14,7 +14,7 @@ import edu.wpi.first.networktables.PubSubOption; import java.util.Optional; import java.util.function.*; -import team3647.frc2023.constants.VisionConstants; +import team3647.frc2024.constants.VisionConstants; public class NeuralDetector { private final NetworkTable table; diff --git a/src/main/java/team3647/frc2023/util/TargetingUtil.java b/src/main/java/team3647/frc2024/util/TargetingUtil.java similarity index 86% rename from src/main/java/team3647/frc2023/util/TargetingUtil.java rename to src/main/java/team3647/frc2024/util/TargetingUtil.java index 249df9e..eb75673 100644 --- a/src/main/java/team3647/frc2023/util/TargetingUtil.java +++ b/src/main/java/team3647/frc2024/util/TargetingUtil.java @@ -1,4 +1,4 @@ -package team3647.frc2023.util; +package team3647.frc2024.util; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; @@ -8,29 +8,43 @@ import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import java.util.function.Supplier; import team3647.lib.GeomUtil; public class TargetingUtil { private final Pose3d speakerPose; + private final Pose3d ampPose; private final Supplier drivePose; private final Supplier fieldRelativeSpeeds; private final Transform3d robotToShooter; - private final double shootSpeed = 9; + private final double shootSpeed = 10; + private double offset = 2; double kDt = 0.02; public TargetingUtil( Pose3d speakerPose, + Pose3d ampPose, Supplier drivePose, Supplier fieldRelativeSpeeds, Transform3d robotToShooter) { this.speakerPose = speakerPose; + this.ampPose = ampPose; this.drivePose = drivePose; this.fieldRelativeSpeeds = fieldRelativeSpeeds; this.robotToShooter = robotToShooter; } + public Command offsetUp() { + return Commands.runOnce(() -> offset += 0.1); + } + + public Command offsetDown() { + return Commands.runOnce(() -> offset -= 0.1); + } + // returns an object storing a pair of doubles, swerve angle change and pivot angle public AimingParameters shootAtPose(Pose3d pose) { return new AimingParameters(robotAngleToPose(pose), getPivotAngle(pose)); @@ -41,6 +55,11 @@ public AimingParameters shootAtSpeaker() { return shootAtPose(speakerPose); } + // returns the parameters for aiming at the speaker + public AimingParameters shootAtAmp() { + return shootAtPose(ampPose); + } + // returns the angle between the vector pointing straight back and the pose public double fieldAngleToPose(Pose3d pose) { final var currentPose = compensatedPose(); @@ -48,7 +67,8 @@ public double fieldAngleToPose(Pose3d pose) { VecBuilder.fill(pose.getX() - currentPose.getX(), pose.getY() - currentPose.getY()); double angle = Math.acos(toPose.dot(VecBuilder.fill(-1, 0)) / toPose.norm()) - * Math.signum(currentPose.getY() - pose.getY()); + * Math.signum(currentPose.getY() - pose.getY()) + * Math.signum(currentPose.getX() - pose.getX()); var newAngle = Math.atan( (exitVelocity() * Math.cos(getPivotAngleStationary(pose)) * Math.sin(angle) @@ -74,7 +94,8 @@ public double fieldAngleToPoseStationary(Pose3d pose) { VecBuilder.fill(pose.getX() - currentPose.getX(), pose.getY() - currentPose.getY()); double angle = Math.acos(toPose.dot(VecBuilder.fill(-1, 0)) / toPose.norm()) - * Math.signum(currentPose.getY() - pose.getY()); + * Math.signum(currentPose.getY() - pose.getY()) + * Math.signum(currentPose.getX() - pose.getX()); return angle; } @@ -88,7 +109,7 @@ public double robotAngleToPose(Pose3d pose) { double distance = GeomUtil.distance(pose.toPose2d().minus(currentPose)); - return newAngle - 0.05 * distance; + return newAngle - 0.01 * distance; } // returns the pivot angle @@ -117,12 +138,12 @@ public double getPivotAngle(Pose3d pose) { double shooterDistance = GeomUtil.distance(pose.toPose2d().minus(shooter2D)); double pivotAngle = Math.atan((pose.getZ() - shooterPose.getZ()) / shooterDistance) - + shooterDistance * Math.PI / 180 * 3; + + (shooterDistance + 0.3) * Math.PI / 180 * offset; double newPivotAngle = Math.atan( (exitVelocity() * Math.sin(pivotAngle) * Math.cos(angle)) / (exitVelocity() * Math.cos(pivotAngle) * Math.cos(angleOnTheMove) - + fieldRelativeSpeeds.get().vxMetersPerSecond)); + - fieldRelativeSpeeds.get().vxMetersPerSecond * 0.5)); SmartDashboard.putNumber("new pibotr angle", newPivotAngle * 180 / Math.PI); return newPivotAngle; } @@ -141,7 +162,7 @@ public double getPivotAngleStationary(Pose3d pose) { double shooterDistance = GeomUtil.distance(pose.toPose2d().minus(shooter2D)); double pivotAngle = Math.atan((pose.getZ() - shooterPose.getZ()) / shooterDistance) - + shooterDistance * Math.PI / 180 * 1; + + (shooterDistance + 0.3) * Math.PI / 180 * offset; return pivotAngle; } @@ -159,7 +180,7 @@ public double getPivotAngleByPose(Pose2d botPose) { double shooterDistance = GeomUtil.distance(pose.toPose2d().minus(shooter2D)); double pivotAngle = Math.atan((pose.getZ() - shooterPose.getZ()) / shooterDistance) - + (shooterDistance) * Math.PI / 180 * 2.6; + + (shooterDistance) * Math.PI / 180 * offset; return pivotAngle; } diff --git a/src/main/java/team3647/frc2023/util/VisionController.java b/src/main/java/team3647/frc2024/util/VisionController.java similarity index 95% rename from src/main/java/team3647/frc2023/util/VisionController.java rename to src/main/java/team3647/frc2024/util/VisionController.java index 520fdd8..a96a117 100644 --- a/src/main/java/team3647/frc2023/util/VisionController.java +++ b/src/main/java/team3647/frc2024/util/VisionController.java @@ -1,11 +1,11 @@ -package team3647.frc2023.util; +package team3647.frc2024.util; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.Timer; import java.util.function.Consumer; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; -import team3647.frc2023.subsystems.SwerveDrive; +import team3647.frc2024.subsystems.SwerveDrive; import team3647.lib.GeomUtil; import team3647.lib.team6328.VirtualSubsystem; diff --git a/src/main/java/team3647/frc2023/util/VisionMeasurement.java b/src/main/java/team3647/frc2024/util/VisionMeasurement.java similarity index 97% rename from src/main/java/team3647/frc2023/util/VisionMeasurement.java rename to src/main/java/team3647/frc2024/util/VisionMeasurement.java index 85909f5..7d3ae5c 100644 --- a/src/main/java/team3647/frc2023/util/VisionMeasurement.java +++ b/src/main/java/team3647/frc2024/util/VisionMeasurement.java @@ -1,4 +1,4 @@ -package team3647.frc2023.util; +package team3647.frc2024.util; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; diff --git a/src/main/java/team3647/lib/SwerveModule.java b/src/main/java/team3647/lib/SwerveModule.java index 37e7e20..21f32d9 100644 --- a/src/main/java/team3647/lib/SwerveModule.java +++ b/src/main/java/team3647/lib/SwerveModule.java @@ -13,7 +13,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import team3647.frc2023.constants.SwerveDriveConstants; +import team3647.frc2024.constants.SwerveDriveConstants; public class SwerveModule {