diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8a07237..7a547d0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,7 +1,3 @@ -// 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 frc.robot; import static edu.wpi.first.units.Units.Degrees; @@ -10,6 +6,7 @@ import static edu.wpi.first.units.Units.FeetPerSecond; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; @@ -167,15 +164,15 @@ public static class IntakeConstants { public static final int armMainID = 14; public static final int armFollowerID = 15; public static final int intakeID = 16; - public static final int armEncoderID = 17; - - public static final double armGearRatio = 3.0; + public static final int armMagnetID = 17; - public static final Angle maxPosition = Rotations.of(1.0); - public static final Angle minPosition = Rotations.of(0.0); + public static final double armGearRatio = (5 / 1) * (36 / 10); + // 111.182298 - 15.025 = 96; + public static final Angle minPosition = Degrees.of(0.0); + public static final Angle maxPosition = Degrees.of(96.157298); - public static final Angle downPosition = Rotations.of(1.0); - public static final Angle upPosition = Rotations.of(0.0); + public static final Angle downPosition = maxPosition; + public static final Angle upPosition = minPosition; public static final Angle armDownPositionTolerance = maxPosition.plus(minPosition).div(2); @@ -183,6 +180,10 @@ public static class IntakeConstants { public static final double intakeSpeed = .353; + public static final double armStallCurrent = 6.7; // amps + public static final double armStallVelocity = 0.1353; // rps + public static final double armZeroSpeed = -0.20; + public static final MotionMagicConfigs motionMagicConfigs = new MotionMagicConfigs() .withMotionMagicCruiseVelocity(RotationsPerSecond.of(0)) @@ -516,13 +517,17 @@ public static class AutoConstants { } public static class HoodConstants { - public static final int hoodMotorID = 21; + public static final int hoodMotorID = 21; // 21 + + public static final double hoodStallCurrent = 6.7; // amps + public static final double hoodStallVelocity = 0.1353; // rps + public static final double hoodZeroSpeed = -0.10; public static final Angle minAngle = Degrees.of(21.448); public static final Angle maxAngle = Degrees.of(59.231); - public static final double hoodGearRatio = - ((48 / 12) * (30 / 15) * (15 / 10)) / ((maxAngle.minus(minAngle)).in(Degrees) / 360); + public static final double hoodGearRatio = ((48 / 12) * (30 / 15) * (15 / 10)); + // / ((maxAngle.minus(minAngle)).in(Degrees) / 360); public static final MotionMagicConfigs motionMagicConfigs = new MotionMagicConfigs() @@ -570,9 +575,63 @@ public static class HoodConstants { } public static class SpindexerConstants { - public static final int SpindexerMotorID = 22; - public static final int SpindexerLaserID = 23; - public static final double SpindexerMotorSpeed = 0.5; - public static final double SpindexerDistance = 100; + public static final int spindexerMotorID = 22; + public static final int kickerMotorID = 23; + public static final double spindexerMotorSpeed = 0.5; + public static final double kickerMotorSpeed = 0.5; + + public static final MotorOutputConfigs motorOutputConfigs = + new MotorOutputConfigs() + .withInverted(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake); + + public static final CurrentLimitsConfigs currentLimitConfigs = + new CurrentLimitsConfigs().withSupplyCurrentLimit(45).withSupplyCurrentLimitEnable(true); + + public static final TalonFXConfiguration spindexerConfigs = + new TalonFXConfiguration() + .withCurrentLimits(currentLimitConfigs) + .withMotorOutput(motorOutputConfigs); + } + + public static class ShooterConstants { + public static final int shooterMotorID = 24; + + public static final double shooterGearRatio = 1; + public static final Distance flyWheelRadius = Inches.of(2.0); + + public static final LinearVelocity shooterSpeedTolerance = MetersPerSecond.of(0.1); + + public static final MotionMagicConfigs motionMagicConfigs = + new MotionMagicConfigs().withMotionMagicAcceleration(0.0).withMotionMagicJerk(0.0); + + public static final Slot0Configs slot0Configs = + new Slot0Configs() + .withKS(0.00) + .withKV(0.00) + .withKA(0.00) + .withKP(0.0) + .withKI(0.00) + .withKD(0.00) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign); + + public static final FeedbackConfigs feedbackConfigs = + new FeedbackConfigs().withSensorToMechanismRatio(shooterGearRatio); + + public static final MotorOutputConfigs motorOutputConfigs = + new MotorOutputConfigs() + .withInverted( + InvertedValue.CounterClockwise_Positive) // needs to spin left when wires up + .withNeutralMode(NeutralModeValue.Coast); + public static final CurrentLimitsConfigs currentLimitConfigs = + new CurrentLimitsConfigs().withSupplyCurrentLimit(45).withSupplyCurrentLimitEnable(true); + + public static final TalonFXConfiguration shooterConfigs = + new TalonFXConfiguration() + .withCurrentLimits(currentLimitConfigs) + .withSlot0(slot0Configs) + .withMotionMagic(motionMagicConfigs) + .withFeedback(feedbackConfigs) + .withMotorOutput(motorOutputConfigs); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7263a62..7d83811 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -32,6 +32,7 @@ import frc.robot.subsystems.Hood; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.Spindexer; import frc.robot.subsystems.Swerve; import frc.robot.subsystems.Turret; import frc.robot.util.AllianceUtil; @@ -86,6 +87,9 @@ public class RobotContainer { @Logged(name = "Intake") private final Intake intake = new Intake(); + @Logged(name = "Spindexer") + private final Spindexer spindexer = new Spindexer(); + @Logged(name = "3D Visualization") private final RobotVisualization robotVisualization = new RobotVisualization(turret, hood, swerve, shooter); @@ -131,7 +135,13 @@ public RobotContainer() { NamedCommands.registerCommand( "Shoot On The Move", new ShootOnTheMove( - swerve, turret, hood, shooter, AllianceUtil::getHubPose, robotVisualization)); + swerve, + turret, + hood, + shooter, + spindexer, + AllianceUtil::getHubPose, + robotVisualization)); NamedCommands.registerCommand( "Pathfind to Mid-Left Bumper", swerve.pathFindToPose(FieldConstants.midLBumperPose)); @@ -140,7 +150,8 @@ public RobotContainer() { NamedCommands.registerCommand( "FerryOTM", - new ShootOnTheMove(swerve, turret, hood, shooter, ferryPoseSupplier, robotVisualization)); + new ShootOnTheMove( + swerve, turret, hood, shooter, spindexer, ferryPoseSupplier, robotVisualization)); NamedCommands.registerCommand( "IntakeUntilFull", Commands.waitUntil(() -> !robotVisualization.canSimIntake())); @@ -148,11 +159,17 @@ public RobotContainer() { NamedCommands.registerCommand( "SOTM Until Empty", new ShootOnTheMove( - swerve, turret, hood, shooter, AllianceUtil::getHubPose, robotVisualization) + swerve, + turret, + hood, + shooter, + spindexer, + AllianceUtil::getHubPose, + robotVisualization) .until(() -> robotVisualization.isEmpty())); NamedCommands.registerCommand( - "Shoot", Commands.run(() -> shooter.setSpeed(MetersPerSecond.of(1))).withTimeout(1)); + "Shoot", Commands.run(() -> shooter.setGoalSpeed(MetersPerSecond.of(1))).withTimeout(1)); configureDriverBindings(); // configureOperatorBindings(); @@ -169,6 +186,9 @@ public RobotContainer() { configureFuelSim(); } + SmartDashboard.putData(intake.zeroArmCommand()); + SmartDashboard.putData(hood.zeroHoodCommand()); + goalShotTarget = AllianceUtil.getHubPose(); inAllianceZoneTrigger.onTrue( @@ -236,7 +256,13 @@ private void configureDriverBindings() { .and(tooCloseToHubTrigger.negate()) .whileTrue( new ShootOnTheMove( - swerve, turret, hood, shooter, goalShotTargetSupplier, robotVisualization)); + swerve, + turret, + hood, + shooter, + spindexer, + goalShotTargetSupplier, + robotVisualization)); swerve.setDefaultCommand( new GuidedTeleopSwerve( @@ -256,9 +282,11 @@ private void configureDriverBindings() { hood.setDefaultCommand(hood.aimForTarget(goalShotTargetSupplier, swerve::getRobotPose)); + intake.setDefaultCommand(intake.intakeToPosition(false)); + shootButton.whileTrue( new ShootOnTheMove( - swerve, turret, hood, shooter, goalShotTargetSupplier, robotVisualization)); + swerve, turret, hood, shooter, spindexer, goalShotTargetSupplier, robotVisualization)); } private void configureOperatorBindings() { @@ -268,15 +296,22 @@ private void configureOperatorBindings() { // .and(shootButton.negate()) .whileTrue( new ShootOnTheMove( - swerve, turret, hood, shooter, goalShotTargetSupplier, robotVisualization)); + swerve, + turret, + hood, + shooter, + spindexer, + goalShotTargetSupplier, + robotVisualization)); Trigger ferryMode = operatorController.leftTrigger(); // turret.setDefaultCommand(turret.faceTarget(AllianceUtil::getHubPose, swerve::getRobotPose)); - hood.setDefaultCommand(hood.aimForTarget(AllianceUtil::getHubPose, swerve::getRobotPose)); + // hood.setDefaultCommand(hood.aimForTarget(AllianceUtil::getHubPose, swerve::getRobotPose)); ferryMode.whileTrue( - new ShootOnTheMove(swerve, turret, hood, shooter, ferryPoseSupplier, robotVisualization)); + new ShootOnTheMove( + swerve, turret, hood, shooter, spindexer, ferryPoseSupplier, robotVisualization)); operatorController .y() diff --git a/src/main/java/frc/robot/commands/GuidedTeleopSwerve.java b/src/main/java/frc/robot/commands/GuidedTeleopSwerve.java index ca41d0a..37549ec 100644 --- a/src/main/java/frc/robot/commands/GuidedTeleopSwerve.java +++ b/src/main/java/frc/robot/commands/GuidedTeleopSwerve.java @@ -122,12 +122,23 @@ private double getMaxTranslationalSpeed() { } private double getForwardSpeed() { - return forwardRateLimiter.calculate( - -forwardSupplier.getAsDouble() * getMaxTranslationalSpeed()); + double forwardSpeed = + forwardRateLimiter.calculate(-forwardSupplier.getAsDouble() * getMaxTranslationalSpeed()); + if (Math.abs(forwardSupplier.getAsDouble()) <= .065) { + forwardSpeed = 0; + forwardRateLimiter.reset(0); + } + return forwardSpeed; } private double getStrafeSpeed() { - return strafeRateLimiter.calculate(-strafeSupplier.getAsDouble() * getMaxTranslationalSpeed()); + double stafeSpeed = + strafeRateLimiter.calculate(-strafeSupplier.getAsDouble() * getMaxTranslationalSpeed()); + if (Math.abs(strafeSupplier.getAsDouble()) <= .065) { + stafeSpeed = 0; + strafeRateLimiter.reset(0); + } + return stafeSpeed; } private double getRotationSpeed() { diff --git a/src/main/java/frc/robot/commands/PhysicsStationaryShoot.java b/src/main/java/frc/robot/commands/PhysicsStationaryShoot.java index 17c7a27..6b9a486 100644 --- a/src/main/java/frc/robot/commands/PhysicsStationaryShoot.java +++ b/src/main/java/frc/robot/commands/PhysicsStationaryShoot.java @@ -1,83 +1,84 @@ -// 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. +// // 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 frc.robot.commands; +// package frc.robot.commands; -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; +// import static edu.wpi.first.units.Units.Degrees; +// import static edu.wpi.first.units.Units.Meters; +// import static edu.wpi.first.units.Units.MetersPerSecond; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.TurretConstants; -import frc.robot.subsystems.Hood; -import frc.robot.subsystems.Shooter; -import frc.robot.util.HoodShotCalculator; -import frc.robot.util.HoodShotCalculator.ShotSolution; -import frc.robot.util.RobotVisualization; -import java.util.function.Supplier; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.geometry.Transform2d; +// import edu.wpi.first.units.measure.Distance; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj2.command.Command; +// import frc.robot.Constants.TurretConstants; +// import frc.robot.subsystems.Hood; +// import frc.robot.subsystems.Shooter; +// import frc.robot.util.HoodShotCalculator; +// import frc.robot.util.HoodShotCalculator.ShotSolution; +// import frc.robot.util.RobotVisualization; +// import java.util.function.Supplier; -/* You should consider using the more terse Command factories API instead -https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class PhysicsStationaryShoot extends Command { - private final Shooter shooter; - private final Hood hood; - private final Supplier robotPose; - private final Supplier targetPose; - private final Supplier targetHeight; +// /* You should consider using the more terse Command factories API instead +// https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +// public class PhysicsStationaryShoot extends Command { +// private final Shooter shooter; +// private final Hood hood; +// private final Supplier robotPose; +// private final Supplier targetPose; +// private final Supplier targetHeight; - public PhysicsStationaryShoot( - Shooter shooter, - Hood hood, - RobotVisualization robotVisualization, - Supplier robotPose, - Supplier targetPose, - Supplier targetHeight) { - this.shooter = shooter; - this.hood = hood; - this.robotPose = robotPose; - this.targetPose = targetPose; - this.targetHeight = targetHeight; - addRequirements(shooter, hood); - } +// public PhysicsStationaryShoot( +// Shooter shooter, +// Hood hood, +// RobotVisualization robotVisualization, +// Supplier robotPose, +// Supplier targetPose, +// Supplier targetHeight) { +// this.shooter = shooter; +// this.hood = hood; +// this.robotPose = robotPose; +// this.targetPose = targetPose; +// this.targetHeight = targetHeight; +// addRequirements(shooter, hood); +// } - // Called when the command is initially scheduled. - @Override - public void initialize() {} +// // Called when the command is initially scheduled. +// @Override +// public void initialize() {} - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - Pose2d turretPose = - robotPose - .get() - .transformBy( - new Transform2d(TurretConstants.robotToTurret.toTranslation2d(), Rotation2d.kZero)); +// // Called every time the scheduler runs while the command is scheduled. +// @Override +// public void execute() { +// Pose2d turretPose = +// robotPose +// .get() +// .transformBy( +// new Transform2d(TurretConstants.robotToTurret.toTranslation2d(), +// Rotation2d.kZero)); - Distance distance = - Meters.of(turretPose.getTranslation().getDistance(targetPose.get().getTranslation())); - ShotSolution solution = - HoodShotCalculator.solveShot(distance, targetHeight.get(), shooter.getExitVelocity()); - shooter.logSolution(solution); - hood.setTargetAngle(solution.hoodAngle()); +// Distance distance = +// Meters.of(turretPose.getTranslation().getDistance(targetPose.get().getTranslation())); +// ShotSolution solution = +// HoodShotCalculator.solveShot(distance, targetHeight.get(), shooter.getExitVelocity()); +// shooter.logSolution(solution); +// hood.setTargetAngle(solution.hoodAngle()); - SmartDashboard.putNumber("TESTING/Distance", distance.in(Meters)); - SmartDashboard.putNumber("TESTING/Angle", solution.hoodAngle().in(Degrees)); - SmartDashboard.putNumber("TESTING/Velocity", solution.exitVelocity().in(MetersPerSecond)); - } +// SmartDashboard.putNumber("TESTING/Distance", distance.in(Meters)); +// SmartDashboard.putNumber("TESTING/Angle", solution.hoodAngle().in(Degrees)); +// SmartDashboard.putNumber("TESTING/Velocity", solution.exitVelocity().in(MetersPerSecond)); +// } - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} +// // Called once the command ends or is interrupted. +// @Override +// public void end(boolean interrupted) {} - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} +// // Returns true when the command should end. +// @Override +// public boolean isFinished() { +// return false; +// } +// } diff --git a/src/main/java/frc/robot/commands/ShootOnTheMove.java b/src/main/java/frc/robot/commands/ShootOnTheMove.java index b62d286..5bebf16 100644 --- a/src/main/java/frc/robot/commands/ShootOnTheMove.java +++ b/src/main/java/frc/robot/commands/ShootOnTheMove.java @@ -10,11 +10,14 @@ import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.SimConstants; import frc.robot.subsystems.Hood; import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.Spindexer; import frc.robot.subsystems.Swerve; import frc.robot.subsystems.Turret; import frc.robot.util.RobotVisualization; @@ -28,6 +31,7 @@ public class ShootOnTheMove extends Command { private final Hood hood; private final Turret turret; private final Swerve swerve; + private final Spindexer spindexer; private Debouncer hoodSetPointDebouncer = new Debouncer(0.1); private Debouncer turretSetPointDebouncer = new Debouncer(0.1); @@ -35,7 +39,6 @@ public class ShootOnTheMove extends Command { private double turretTolerance = 3.53; // deg private double hoodTolerance = 3.53; // deg - private boolean shooterAtSetPoint = true; private LinearFilter accelXFilter = LinearFilter.movingAverage(2); private LinearFilter accelYFilter = LinearFilter.movingAverage(2); @@ -54,12 +57,14 @@ public ShootOnTheMove( Turret turret, Hood hood, Shooter shooter, + Spindexer spindexer, Supplier targetPoseSupplier, RobotVisualization robotVisualization) { this.swerve = swerve; this.turret = turret; this.hood = hood; this.shooter = shooter; + this.spindexer = spindexer; this.targetPoseSupplier = targetPoseSupplier; this.robotVisualization = robotVisualization; addRequirements(hood, turret, shooter); @@ -96,6 +101,7 @@ public void execute() { turret.setTargetAngle(shootingParameters.turretAngle()); hood.setTargetAngle(shootingParameters.hoodAngle()); + SmartDashboard.putNumber("HOOD NUMBER READ HERE", shootingParameters.hoodAngle().in(Degrees)); shooter.setGoalSpeed(shootingParameters.shooterSpeed()); swerve.setLookAheadPose(shootingParameters.lookAheadPosition()); @@ -106,24 +112,28 @@ public void execute() { if (turretSetPointDebouncer.calculate(Math.abs(turretErrorDeg) <= turretTolerance) && hoodSetPointDebouncer.calculate(Math.abs(hoodErrorDeg) <= hoodTolerance) - && shooterDebouncer.calculate(shooterAtSetPoint)) { - if (isFirstShot - || ((Timer.getFPGATimestamp() - startTime) > 1 / SimConstants.fuelsPerSecond)) { - robotVisualization.shootFuel(shootingParameters); - - startTime = Timer.getFPGATimestamp(); - isFirstShot = false; + && shooterDebouncer.calculate(shooter.shooterAtSetPoint())) { + if (RobotBase.isSimulation()) { // if sim and ready to shoot + if (isFirstShot + || ((Timer.getFPGATimestamp() - startTime) > 1 / SimConstants.fuelsPerSecond)) { + robotVisualization.shootFuel(shootingParameters); + + startTime = Timer.getFPGATimestamp(); + isFirstShot = false; + } + } else { // if real and ready to shoot + spindexer.runBoth(); } } else { - // indexer.stop(); + spindexer.stopBoth(); } } @Override public void end(boolean interrupted) { - // shooter.stop(); - // indexer.stop(); + shooter.stop(); + spindexer.stopBoth(); turret.stopTurret(); hood.stopHood(); } diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 432da98..0da1d26 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -68,26 +68,26 @@ public class TunerConstants { // 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("Default Name", "./logs/example.hoot"); + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("Default Name", "./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(4.76); + // 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(4.76); - // 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 = 5.4; + // 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 = 5.4; - private static final double kDriveGearRatio = 6.48; - private static final double kSteerGearRatio = 12.1; - private static final Distance kWheelRadius = Inches.of(2); + private static final double kDriveGearRatio = 6.48; + private static final double kSteerGearRatio = 12.1; + 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 = 13; + private static final int kPigeonId = 13; // These are only used for simulation private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); @@ -124,51 +124,49 @@ public class TunerConstants { .withSteerFrictionVoltage(kSteerFrictionVoltage) .withDriveFrictionVoltage(kDriveFrictionVoltage); - - // Front Left - private static final int kFrontLeftDriveMotorId = 1; - private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 3; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.174072265625); - private static final boolean kFrontLeftSteerMotorInverted = true; - private static final boolean kFrontLeftEncoderInverted = false; - - private static final Distance kFrontLeftXPos = Inches.of(11); - private static final Distance kFrontLeftYPos = Inches.of(11); - - // Front Right - private static final int kFrontRightDriveMotorId = 4; - private static final int kFrontRightSteerMotorId = 5; - private static final int kFrontRightEncoderId = 6; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.45556640625); - private static final boolean kFrontRightSteerMotorInverted = true; - private static final boolean kFrontRightEncoderInverted = false; - - private static final Distance kFrontRightXPos = Inches.of(11); - private static final Distance kFrontRightYPos = Inches.of(-11); - - // Back Left - private static final int kBackLeftDriveMotorId = 7; - private static final int kBackLeftSteerMotorId = 8; - private static final int kBackLeftEncoderId = 9; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.3955078125); - private static final boolean kBackLeftSteerMotorInverted = false; - private static final boolean kBackLeftEncoderInverted = false; - - private static final Distance kBackLeftXPos = Inches.of(-11); - private static final Distance kBackLeftYPos = Inches.of(11); - - // Back Right - private static final int kBackRightDriveMotorId = 10; - private static final int kBackRightSteerMotorId = 11; - private static final int kBackRightEncoderId = 12; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.436767578125); - private static final boolean kBackRightSteerMotorInverted = true; - private static final boolean kBackRightEncoderInverted = false; - - private static final Distance kBackRightXPos = Inches.of(-11); - private static final Distance kBackRightYPos = Inches.of(-11); - + // Front Left + private static final int kFrontLeftDriveMotorId = 1; + private static final int kFrontLeftSteerMotorId = 2; + private static final int kFrontLeftEncoderId = 3; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.174072265625); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(11); + private static final Distance kFrontLeftYPos = Inches.of(11); + + // Front Right + private static final int kFrontRightDriveMotorId = 4; + private static final int kFrontRightSteerMotorId = 5; + private static final int kFrontRightEncoderId = 6; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.45556640625); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(11); + private static final Distance kFrontRightYPos = Inches.of(-11); + + // Back Left + private static final int kBackLeftDriveMotorId = 7; + private static final int kBackLeftSteerMotorId = 8; + private static final int kBackLeftEncoderId = 9; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.3955078125); + private static final boolean kBackLeftSteerMotorInverted = false; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-11); + private static final Distance kBackLeftYPos = Inches.of(11); + + // Back Right + private static final int kBackRightDriveMotorId = 10; + private static final int kBackRightSteerMotorId = 12; + private static final int kBackRightEncoderId = 11; + private static final Angle kBackRightEncoderOffset = Rotations.of(0.436767578125); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-11); + private static final Distance kBackRightYPos = Inches.of(-11); public static final SwerveModuleConstants FrontLeft = ConstantCreator.createModuleConstants( @@ -203,7 +201,19 @@ public static Swerve createDrivetrain() { /** - * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. + * 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 static class TunerSwerveDrivetrain extends SwerveDrivetrain { /** diff --git a/src/main/java/frc/robot/subsystems/Hood.java b/src/main/java/frc/robot/subsystems/Hood.java index 28dfe23..2110c99 100644 --- a/src/main/java/frc/robot/subsystems/Hood.java +++ b/src/main/java/frc/robot/subsystems/Hood.java @@ -4,9 +4,10 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.MotionMagicVoltage; @@ -19,10 +20,13 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.HoodConstants; import frc.robot.Constants.TurretConstants; @@ -32,6 +36,8 @@ public class Hood extends SubsystemBase { private TalonFX hoodMotor; private StatusSignal hoodPosition; + private final StatusSignal hoodCurrent; + private final StatusSignal hoodVelocity; private SingleJointedArmSim hoodSim; @@ -46,6 +52,8 @@ public Hood() { zeroHood(); hoodPosition = hoodMotor.getPosition(); + hoodCurrent = hoodMotor.getStatorCurrent(); + hoodVelocity = hoodMotor.getVelocity(); hoodSim = new SingleJointedArmSim( @@ -63,6 +71,27 @@ private void zeroHood() { hoodMotor.setPosition(0); } + public Command zeroHoodCommand() { + return Commands.run( + () -> { + hoodMotor.set(HoodConstants.hoodZeroSpeed); + + hoodCurrent.refresh(); + hoodVelocity.refresh(); + }) + .until( + () -> + hoodCurrent.getValue().in(Amps) > HoodConstants.hoodStallCurrent + && Math.abs(hoodVelocity.getValue().in(RotationsPerSecond)) + < HoodConstants.hoodStallVelocity) + .andThen( + () -> { + hoodMotor.stopMotor(); + zeroHood(); + }) + .withName("Zero Hood Command"); + } + @Logged(name = "Hood Angle") public Angle getHoodAngle() { return hoodPosition.getValue(); @@ -89,9 +118,8 @@ public void moveHood(double speed) { hoodMotor.set(speed); } - public void setTargetAngle(Angle targetAngle) { - this.targetAngle = targetAngle; - hoodMotor.setControl(motionMagicRequest.withPosition(targetAngle.in(Rotations))); + public void setTargetAngle(Angle targetHoodAngle) { + targetAngle = targetHoodAngle; } public Angle getTargetAngle() { @@ -104,9 +132,9 @@ public Command stop() { public Command moveToAngle(Angle targetPose) { return run(() -> { - hoodMotor.setControl(motionMagicRequest.withPosition(targetPose.in(Rotations))); + hoodMotor.setControl(motionMagicRequest.withPosition(targetPose)); }) - .withName("Move Turret to Angle"); + .withName("Move Hood to Angle"); } public Command aimForTarget( @@ -129,6 +157,10 @@ public Command aimForTarget( @Override public void periodic() { hoodPosition.refresh(); + + hoodMotor.setControl(motionMagicRequest.withPosition(targetAngle)); + SmartDashboard.putNumber("Hood/targetAngle", targetAngle.in(Degrees)); + SmartDashboard.putNumber("Hood/Hood Angle", getHoodAngle().in(Degrees)); } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 6dfb16a..58eadb0 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -4,61 +4,58 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.MagnetSensorConfigs; -import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.MotorAlignmentValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.IntakeConstants; public class Intake extends SubsystemBase { /*Objects */ private final TalonFX armMainMotor; - private final Follower armFollowerMotor; + private final TalonFX armFollowerMotor; private final TalonFX intakeMotor; - private CANcoder armEncoder; + private final DigitalInput armMagnetSensor; - private final MotionMagicVoltage motionMagicRequest = new MotionMagicVoltage(0); + private boolean wasDeployedLastLoop; + + private final MotionMagicVoltage motionMagicRequest = + new MotionMagicVoltage(0).withEnableFOC(true); private StatusSignal armMainPosition; - // private StatusSignal armFollowerPosition; + private StatusSignal armFollowerPosition; + + private final StatusSignal armCurrent; + private final StatusSignal armVelocity; /*MotionMagic*/ public Intake() { intakeMotor = new TalonFX(IntakeConstants.intakeID); armMainMotor = new TalonFX(IntakeConstants.armMainID); - armFollowerMotor = new Follower(IntakeConstants.armMainID, MotorAlignmentValue.Opposed); - // armFollowerMotor = new TalonFX(IntakeConstants.armFollowerID); + armFollowerMotor = new TalonFX(IntakeConstants.armFollowerID); - armEncoder = new CANcoder(IntakeConstants.armEncoderID); + armMagnetSensor = new DigitalInput(IntakeConstants.armMagnetID); armMainMotor.getConfigurator().apply(IntakeConstants.armConfigs); - // armFollowerMotor.getConfigurator().apply(IntakeConstants.armConfigs); + armFollowerMotor.getConfigurator().apply(IntakeConstants.armConfigs); + + armCurrent = armMainMotor.getStatorCurrent(); + armVelocity = armMainMotor.getVelocity(); armMainPosition = armMainMotor.getPosition(); - // armFollowerPosition = armFollowerMotor.getPosition(); - - armEncoder - .getConfigurator() - .apply( - new CANcoderConfiguration() - .withMagnetSensor( - new MagnetSensorConfigs() - .withMagnetOffset(IntakeConstants.armMagnetOffset) - .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive))); - - armMainMotor.setPosition(getAbsoluteArmAngle().in(Rotations)); + armFollowerPosition = armFollowerMotor.getPosition(); } public void setIntakeSpeed(double speed) { @@ -77,20 +74,29 @@ public Command intakeToPosition(boolean downPosition) { return run(() -> { if (downPosition) { armMainMotor.setControl(motionMagicRequest.withPosition(IntakeConstants.downPosition)); + armFollowerMotor.setControl( + motionMagicRequest.withPosition(IntakeConstants.downPosition)); + } else { armMainMotor.setControl(motionMagicRequest.withPosition(IntakeConstants.upPosition)); + armFollowerMotor.setControl( + motionMagicRequest.withPosition(IntakeConstants.downPosition)); } }) - .withName("Intake working"); + .withName("Intake To Position"); } public Command intakeSequence(boolean intakeDown) { return run(() -> { if (intakeDown) { armMainMotor.setControl(motionMagicRequest.withPosition(IntakeConstants.downPosition)); + armFollowerMotor.setControl( + motionMagicRequest.withPosition(IntakeConstants.downPosition)); setIntakeSpeed(IntakeConstants.intakeSpeed); } else { armMainMotor.setControl(motionMagicRequest.withPosition(IntakeConstants.upPosition)); + armFollowerMotor.setControl( + motionMagicRequest.withPosition(IntakeConstants.upPosition)); stopIntake(); } }) @@ -98,25 +104,55 @@ public Command intakeSequence(boolean intakeDown) { } public boolean isIntakeDeployed() { - // return getAbsoluteArmAngle().in(Rotations) - // > IntakeConstants.armDownPositionTolerance.in(Rotations) - // ? true - // : false; + // return !armMagnetSensor.get(); return true; // FOR TESTING IN SIM } - public Angle getAbsoluteArmAngle() { - return armEncoder.getAbsolutePosition().getValue(); - } - public Angle getArmAngle() { return armMainPosition.getValue(); } + public void setZero() { + armMainMotor.setPosition(0); + armFollowerMotor.setPosition(0); + } + + public Command zeroArmCommand() { + return Commands.run( + () -> { + armMainMotor.set(IntakeConstants.armZeroSpeed); + armFollowerMotor.set(IntakeConstants.armZeroSpeed); + + armCurrent.refresh(); + armVelocity.refresh(); + }) + .until( + () -> + armCurrent.getValue().in(Amps) > IntakeConstants.armStallCurrent + && Math.abs(armVelocity.getValue().in(RotationsPerSecond)) + < IntakeConstants.armStallVelocity) + .andThen( + () -> { + armMainMotor.stopMotor(); + armFollowerMotor.stopMotor(); + setZero(); + }) + .withName("Zero Arm Command"); + } + @Override public void periodic() { + if (!isIntakeDeployed() && wasDeployedLastLoop) { + armMainMotor.setPosition(IntakeConstants.minPosition); + armFollowerMotor.setPosition(IntakeConstants.minPosition); + } + wasDeployedLastLoop = isIntakeDeployed(); + armMainPosition.refresh(); + armFollowerPosition.refresh(); + SmartDashboard.putNumber("Intake speed", intakeMotor.get()); SmartDashboard.putNumber("Intake Arm Position", armMainPosition.getValue().in(Rotations)); + SmartDashboard.putBoolean("Intake Arm Deployed", isIntakeDeployed()); } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index b46c91d..dd825da 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -1,48 +1,32 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; -import com.ctre.phoenix6.configs.MotionMagicConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.util.HoodShotCalculator.ShotSolution; +import frc.robot.Constants.ShooterConstants; public class Shooter extends SubsystemBase { /** Creates a new Shooter. */ - private TalonFX shooterMotor = new TalonFX(75); + private TalonFX shooterMotor = new TalonFX(ShooterConstants.shooterMotorID); - private double speed; - - private final MotionMagicVelocityVoltage shooterVelocity = new MotionMagicVelocityVoltage(0); + private final MotionMagicVelocityVoltage velocityRequest = new MotionMagicVelocityVoltage(0); private LinearVelocity goalSpeed = MetersPerSecond.of(0); - private LinearVelocity currentSpeed = MetersPerSecond.of(0); - private ShotSolution currentShotSolution; public Shooter() { - TalonFXConfiguration config = new TalonFXConfiguration(); - Slot0Configs slot0 = new Slot0Configs(); - config.Slot0.kP = 0.12; - config.Slot0.kI = 0.0; - config.Slot0.kD = 0.0; - config.Slot0.kV = 0.12; - config.Slot0 = slot0; - MotionMagicConfigs mm = new MotionMagicConfigs(); - mm.MotionMagicAcceleration = 80; - mm.MotionMagicJerk = 300; - config.MotionMagic = mm; - - shooterMotor.getConfigurator().apply(config); + shooterMotor.getConfigurator().apply(ShooterConstants.shooterConfigs); } - public void setSpeed(LinearVelocity speed) { - // this should be rotations per second but ill fix later - shooterMotor.setControl(shooterVelocity.withVelocity(speed.in(MetersPerSecond))); + public void setGoalSpeed(LinearVelocity speed) { + goalSpeed = speed; } public LinearVelocity getGoalSpeed() { @@ -50,34 +34,56 @@ public LinearVelocity getGoalSpeed() { } public void stop() { - shooterMotor.stopMotor(); + // shooterMotor.stopMotor(); + goalSpeed = MetersPerSecond.of(0); } - public void setGoalSpeed(LinearVelocity speed) { - goalSpeed = speed; + public AngularVelocity linearToAngularVelocity(LinearVelocity vel) { + return RadiansPerSecond.of( + vel.in(MetersPerSecond) / ShooterConstants.flyWheelRadius.in(Meters)); } - public void logSolution(ShotSolution solution) { - currentShotSolution = solution; - setGoalSpeed(solution.exitVelocity()); + public LinearVelocity angularToLinearVelocity(AngularVelocity vel) { + return MetersPerSecond.of( + vel.in(RadiansPerSecond) * ShooterConstants.flyWheelRadius.in(Meters)); } - public ShotSolution getShotSolution() { - return currentShotSolution; + public AngularVelocity getCurrentVelocity() { + return shooterMotor.getVelocity().getValue(); } - public LinearVelocity getExitVelocity() { - return MetersPerSecond.of(9.353); + public boolean shooterAtSetPoint() { + // LinearVelocity currentSpeed = angularToLinearVelocity(getCurrentVelocity()); + + // return Math.abs(currentSpeed.in(MetersPerSecond) - goalSpeed.in(MetersPerSecond)) + // < ShooterConstants.shooterSpeedTolerance.in(MetersPerSecond); + return true; } + // public void logSolution(ShotSolution solution) { + // currentShotSolution = solution; + // setGoalSpeed(solution.exitVelocity()); + // } + + // public ShotSolution getShotSolution() { + // return currentShotSolution; + // } + + // public LinearVelocity getExitVelocity() { + // return MetersPerSecond.of(9.353); + // } + @Override public void periodic() { - if (currentSpeed != goalSpeed) { - currentSpeed = goalSpeed; - setSpeed(currentSpeed); - } - - SmartDashboard.putNumber("Shooter/Current speed", shooterMotor.get()); - SmartDashboard.putNumber("Shooter/Goal speed", currentSpeed.in(MetersPerSecond)); + shooterMotor.setControl(velocityRequest.withVelocity(linearToAngularVelocity(goalSpeed))); + + SmartDashboard.putNumber( + "Shooter/Current Angular Velocity", getCurrentVelocity().in(RotationsPerSecond)); + SmartDashboard.putNumber( + "Shooter/Current Linear Velocity", + angularToLinearVelocity(getCurrentVelocity()).in(MetersPerSecond)); + SmartDashboard.putNumber( + "Shooter/Goal Angular Velocity", linearToAngularVelocity(goalSpeed).in(RotationsPerSecond)); + SmartDashboard.putNumber("Shooter/Goal Linear Velocity", goalSpeed.in(MetersPerSecond)); } } diff --git a/src/main/java/frc/robot/subsystems/Spindexer.java b/src/main/java/frc/robot/subsystems/Spindexer.java index 619398e..ec128fe 100644 --- a/src/main/java/frc/robot/subsystems/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/Spindexer.java @@ -4,8 +4,6 @@ package frc.robot.subsystems; -import au.grapplerobotics.LaserCan; -import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -14,93 +12,88 @@ import frc.robot.Constants.SpindexerConstants; public class Spindexer extends SubsystemBase { - private TalonFX SpindexerMotor; - private LaserCan SpindexerLaser; - private Debouncer SpindexDebouncer; + private TalonFX spindexerMotor; + private TalonFX kickerMotor; private Debouncer currentEmptyDebouncer = new Debouncer(0.4); /** Creates a new Spindexer. */ public Spindexer() { - SpindexerMotor = new TalonFX(SpindexerConstants.SpindexerMotorID); - SpindexerLaser = new LaserCan(SpindexerConstants.SpindexerLaserID); - SpindexDebouncer = new Debouncer(1.5); + spindexerMotor = new TalonFX(SpindexerConstants.spindexerMotorID); + kickerMotor = new TalonFX(SpindexerConstants.kickerMotorID); - TalonFXConfiguration spindexerConfig = new TalonFXConfiguration(); - spindexerConfig.Slot0.kP = 0.0; - spindexerConfig.Slot0.kI = 2.5; - spindexerConfig.Slot0.kD = 5.3; + spindexerMotor.getConfigurator().apply(SpindexerConstants.spindexerConfigs); + kickerMotor.getConfigurator().apply(SpindexerConstants.spindexerConfigs); + } - SpindexerMotor.getConfigurator().apply(new TalonFXConfiguration()); + public void stopSpindexerMotor() { + spindexerMotor.stopMotor(); } - public void setSpeed() { - SpindexerMotor.set(SpindexerConstants.SpindexerMotorSpeed); + public void stopKickerMotor() { + kickerMotor.stopMotor(); } - public void stopMotor() { - SpindexerMotor.stopMotor(); + public double getSpindexerSpeed() { + return spindexerMotor.get(); } - public double getSpeed() { - return SpindexerMotor.get(); + public double getKickerSpeed() { + return kickerMotor.get(); } public double getCurrent() { - return SpindexerMotor.getStatorCurrent().getValueAsDouble(); + return spindexerMotor.getStatorCurrent().getValueAsDouble(); } public boolean currentSaysEmpty() { return currentEmptyDebouncer.calculate(getCurrent() < 9.0); // random number need to test } - public Command runSpindexer() { - return run(this::setSpeed); + public void runBoth() { + spindexerMotor.set(SpindexerConstants.spindexerMotorSpeed); + kickerMotor.set(SpindexerConstants.kickerMotorSpeed); } - public Command stopSpindexer() { - return run(this::stopMotor); + public void stopBoth() { + spindexerMotor.set(0); + kickerMotor.set(0); } - public Command upSpeed(double speed) { - return run( - () -> { - SpindexerMotor.set(speed); - }); + public Command runSpindexer() { + return run(() -> spindexerMotor.set(SpindexerConstants.spindexerMotorSpeed)); } - public Command downSpeed(double speed) { - return run( - () -> { - SpindexerMotor.set(-speed); - }); + public Command runKicker() { + return run(() -> kickerMotor.set(SpindexerConstants.kickerMotorSpeed)); } - public Command runUntilEmptyCommand() { - return (runSpindexer()).until(() -> SpindexDebouncer.calculate(!beamBroken())); + public Command runBothCommand() { + return runKicker().alongWith(runSpindexer()); } - public boolean beamBroken() { - LaserCan.Measurement measurement = SpindexerLaser.getMeasurement(); + public Command stopSpindexer() { + return runOnce(this::stopSpindexerMotor); + } - if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { - if (measurement.distance_mm <= SpindexerConstants.SpindexerDistance) { - return true; + public Command stopKicker() { + return runOnce(this::stopKickerMotor); + } - } else { - return false; - } - } else { - return false; - } + public Command stopBothCommand() { + return stopSpindexer().alongWith(stopKicker()); + } + + public Command runUntilEmptyCommand() { + return (runSpindexer()).until(() -> currentSaysEmpty()); } public boolean isEmpty() { - return !beamBroken() && currentSaysEmpty(); + return (spindexerMotor.get() > 0.1) && currentSaysEmpty(); } @Override public void periodic() { SmartDashboard.putNumber("Spindexer Current", getCurrent()); - SmartDashboard.putBoolean("Spindexer Beam Broken", beamBroken()); + SmartDashboard.putBoolean("Spindexer Empty", isEmpty()); } } diff --git a/src/main/java/frc/robot/subsystems/Turret.java b/src/main/java/frc/robot/subsystems/Turret.java index cbe8bc1..9bdb816 100644 --- a/src/main/java/frc/robot/subsystems/Turret.java +++ b/src/main/java/frc/robot/subsystems/Turret.java @@ -147,7 +147,6 @@ public Angle getAbsoluteTurretPosition() { public void setTargetAngle(Angle desiredTurretAngle) { desiredAngle = optimizeAngle(desiredTurretAngle); - turretMotor.setControl(motionMagicRequest.withPosition(desiredAngle.in(Rotations))); } private Angle optimizeAngle(Angle desiredAngle) { @@ -208,6 +207,9 @@ public Pose3d[] zeroedComponentPoses() { @Override public void periodic() { turretPosition.refresh(); + + turretMotor.setControl(motionMagicRequest.withPosition(desiredAngle.in(Rotations))); + SmartDashboard.putNumber("Turret/TwoEncoder Angle", getAbsoluteTurretPosition().in(Degrees)); SmartDashboard.putNumber("Turret/Turret Angle", turretPosition.getValue().in(Degrees)); SmartDashboard.putNumber("Turret/desired turret angle", desiredAngle.in(Degrees));