diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 110b4be..2a4f884 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2024RobotCode"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 90; - public static final String GIT_SHA = "3b8e10f3940dede6b9d8654e9943fa7abb0e6d0d"; - public static final String GIT_DATE = "2024-02-20 00:54:38 EST"; + public static final int GIT_REVISION = 93; + public static final String GIT_SHA = "f602db19c27b93de193ace204f2b8a46a187c8db"; + public static final String GIT_DATE = "2024-02-23 19:03:52 EST"; public static final String GIT_BRANCH = "main"; - public static final String BUILD_DATE = "2024-02-22 20:21:31 EST"; - public static final long BUILD_UNIX_TIME = 1708651291962L; + public static final String BUILD_DATE = "2024-02-23 21:51:21 EST"; + public static final long BUILD_UNIX_TIME = 1708743081578L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9237815..18313df 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -61,10 +61,10 @@ public static class IntakeConstants { } public static final class ShooterConstants { - public static final double FEEDER_CURRENT_LIMIT = 0; + public static final double FEEDER_CURRENT_LIMIT = 40; public static final boolean FEEDER_CURRENT_LIMIT_ENABLED = true; - public static final double FLYWHEEL_CURRENT_LIMIT = 0; + public static final double FLYWHEEL_CURRENT_LIMIT = 40; public static final boolean FLYWHEEL_CURRENT_LIMIT_ENABLED = true; public static final double FLYWHEEL_THRESHOLD = 0; @@ -86,6 +86,9 @@ public static class ElevatorConstants { public static final double PIVOT_THRESHOLD = 0; public static final double EXTENDER_THRESHOLD = 0; + public static final double PIVOT_RATIO = 100.0; + public static final double EXTENDER_RATIO = 0; + public static final double[] PIVOT_PID = {0, 0, 0}; public static final double[] EXTENDER_PID = {0, 0, 0}; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7c4f5e0..ea365e2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,15 +14,12 @@ package frc.robot; import com.pathplanner.lib.auto.AutoBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.commands.DriveCommands; import frc.robot.subsystems.Elevator.Elevator; import frc.robot.subsystems.Elevator.ElevatorExtenderIO; @@ -79,17 +76,19 @@ public RobotContainer() { new ModuleIOTalonFX(2), new ModuleIOTalonFX(3)); intake = new Intake(new IntakeRollerIOSparkFlex(RobotMap.IntakeIDs.ROLLERS)); - // shooter = - // new Shooter( - // new FlywheelIOTalonFX(RobotMap.ShooterIDs.FLYWHEEL_ONE), - // new FlywheelIOTalonFX(RobotMap.ShooterIDs.FLYWHEEL_ONE), - // new FeederIOTalonFX(RobotMap.ShooterIDs.FEEDER)); - // elevator = - // new Elevator( - // new ElevatorPivotIOTalonFX( - // RobotMap.ElevatorIDs.PIVOT_ONE, RobotMap.ElevatorIDs.PIVOT_TWO), - // new ElevatorExtenderIOTalonFX( - // RobotMap.ElevatorIDs.EXTENDER_ONE, RobotMap.ElevatorIDs.EXTENDER_TWO)); + shooter = + new Shooter( + new FlywheelIOTalonFX( + RobotMap.ShooterIDs.FLYWHEEL_ONE, RobotMap.ShooterIDs.FLYWHEEL_TWO), + new FeederIOTalonFX(RobotMap.ShooterIDs.FEEDER)); + elevator = + new Elevator( + new ElevatorPivotIOTalonFX( + RobotMap.ElevatorIDs.PIVOT_ONE, + RobotMap.ElevatorIDs.PIVOT_TWO, + RobotMap.ElevatorIDs.GYRO), + new ElevatorExtenderIOTalonFX( + RobotMap.ElevatorIDs.EXTENDER_ONE, RobotMap.ElevatorIDs.EXTENDER_TWO)); // led = new LED(new LED_IOSpark(RobotMap.LEDIDs.CHANNEL)); break; case REPLAY: @@ -101,7 +100,7 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim()); intake = new Intake(new IntakeRollerIOSim()); - shooter = new Shooter(new FlywheelIOSim(), new FlywheelIOSim(), new FeederIOSim()); + shooter = new Shooter(new FlywheelIOSim(), new FeederIOSim()); elevator = new Elevator(new ElevatorPivotIOSim(), new ElevatorExtenderIOSim()); led = new LED(new LED_IOSpark(RobotMap.LEDIDs.CHANNEL)); break; @@ -115,7 +114,7 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim()); intake = new Intake(new IntakeRollerIOSim()); - shooter = new Shooter(new FlywheelIOSim(), new FlywheelIOSim(), new FeederIOSim()); + shooter = new Shooter(new FlywheelIOSim(), new FeederIOSim()); elevator = new Elevator(new ElevatorPivotIOSim(), new ElevatorExtenderIOSim()); led = new LED(new LED_IOSpark(RobotMap.LEDIDs.CHANNEL)); break; @@ -132,13 +131,10 @@ public RobotContainer() { new ModuleIO() {}); shooter = new Shooter( - new FlywheelIOTalonFX(RobotMap.ShooterIDs.FLYWHEEL_ONE), - new FlywheelIOTalonFX(RobotMap.ShooterIDs.FLYWHEEL_ONE), + new FlywheelIOTalonFX( + RobotMap.ShooterIDs.FLYWHEEL_ONE, RobotMap.ShooterIDs.FLYWHEEL_TWO), new FeederIOTalonFX(RobotMap.ShooterIDs.FEEDER)); - elevator = - new Elevator( - new ElevatorPivotIO() {}, - new ElevatorExtenderIO() {}); + elevator = new Elevator(new ElevatorPivotIO() {}, new ElevatorExtenderIO() {}); led = new LED(new LED_IO() {}); break; } @@ -148,11 +144,9 @@ public RobotContainer() { // Intake SysID Routines autoChooser.addDefaultOption( - "Intake SysID (Dynamic Forward)", - intake.sysIdDynamic(SysIdRoutine.Direction.kForward)); + "Intake SysID (Dynamic Forward)", intake.sysIdDynamic(SysIdRoutine.Direction.kForward)); autoChooser.addDefaultOption( - "Intake SysID (Dynamic Reverse)", - intake.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + "Intake SysID (Dynamic Reverse)", intake.sysIdDynamic(SysIdRoutine.Direction.kReverse)); autoChooser.addDefaultOption( "Intake SysID (Quasistatic Forward)", intake.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); @@ -175,18 +169,32 @@ private void configureButtonBindings() { () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> -controller.getRightX())); - controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); - controller - .b() - .onTrue( - Commands.runOnce( - () -> - drive.setPose( - new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), - drive) - .ignoringDisable(true)); - - controller.a().whileTrue(intake.sysIdQuasistatic(Direction.kForward)); + // controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); + // controller + // .b() + // .onTrue( + // Commands.runOnce( + // () -> + // drive.setPose( + // new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), + // drive) + // .ignoringDisable(true)); + + // controller.a().whileTrue(intake.sysIdQuasistatic(Direction.kForward)); + // controller.a().onTrue(new SetPivotTarget(20, elevator)); + // controller.a().onFalse(new InstantCommand(elevator::pivotStop, elevator)); + + // controller.a().onTrue(new SetExtenderTarget(0, elevator)); + // controller.a().onFalse(new InstantCommand(elevator::elevatorStop, elevator)); + + controller.a().onTrue(new InstantCommand(() -> shooter.setShooterVelocitys(300, 300), shooter)); + controller.a().onFalse(new InstantCommand(shooter::stopShooterMotors, shooter)); + + controller.b().onTrue(new InstantCommand(() -> shooter.runFeeders(1500), shooter)); + controller.b().onFalse(new InstantCommand(shooter::stopFeeders, shooter)); + + // controller.a().onTrue(new SetExtenderTarget(0, elevator)); + // controller.a().onFalse(new InstantCommand(elevator::elevatorStop, elevator)); // Schedule `exampleMethodCommand` when the Xbox controller's B button is // pressed, diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 18d1c9d..aeaeed9 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -6,17 +6,17 @@ public static class IntakeIDs { } public static class ShooterIDs { - public static int FLYWHEEL_ONE = 0; // TODO:: 11 - public static int FLYWHEEL_TWO = 0; // TODO:: 12 - public static int FEEDER = 0; // TODO:: 13 + public static int FLYWHEEL_ONE = 11; // TODO:: 11 + public static int FLYWHEEL_TWO = 12; // TODO:: 12 + public static int FEEDER = 13; // TODO:: 13 } public static class ElevatorIDs { public static final int GYRO = 10; - public static final int PIVOT_ONE = 0; // TODO:: 14 - public static final int PIVOT_TWO = 0; // TODO:: 15 - public static final int EXTENDER_ONE = 0; // TODO:: 8 - public static final int EXTENDER_TWO = 0; // TODO:: 9 + public static final int PIVOT_ONE = 14; // TODO:: 14 + public static final int PIVOT_TWO = 15; // TODO:: 15 + public static final int EXTENDER_ONE = 8; // TODO:: 8 + public static final int EXTENDER_TWO = 9; // TODO:: 9 } public static class LEDIDs { diff --git a/src/main/java/frc/robot/commands/SetExtenderTarget.java b/src/main/java/frc/robot/commands/SetExtenderTarget.java index 339a257..3ee453d 100644 --- a/src/main/java/frc/robot/commands/SetExtenderTarget.java +++ b/src/main/java/frc/robot/commands/SetExtenderTarget.java @@ -5,13 +5,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.RobotMap; import frc.robot.subsystems.Elevator.Elevator; -import frc.robot.subsystems.Elevator.ElevatorExtenderIOSim; -import frc.robot.subsystems.Elevator.ElevatorExtenderIOTalonFX; -import frc.robot.subsystems.Elevator.ElevatorPivotIOSim; -import frc.robot.subsystems.Elevator.ElevatorPivotIOTalonFX; public class SetExtenderTarget extends Command { /** Creates a new ExtendElevator. */ @@ -21,13 +15,11 @@ public class SetExtenderTarget extends Command { public SetExtenderTarget(double setPoint, Elevator elevator) { // Use addRequirements() here to declare subsystem dependencies. - //addRequirements(elevator); + // addRequirements(elevator); setPoint = this.setPoint; this.elevator = elevator; - } - // Called when the command is initially scheduled. @Override public void initialize() { @@ -49,4 +41,4 @@ public void end(boolean interrupted) { public boolean isFinished() { return elevator.extenderAtSetpoint(); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/commands/SetPivotTarget.java b/src/main/java/frc/robot/commands/SetPivotTarget.java index ef7350f..bb42929 100644 --- a/src/main/java/frc/robot/commands/SetPivotTarget.java +++ b/src/main/java/frc/robot/commands/SetPivotTarget.java @@ -1,21 +1,17 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.RobotMap; import frc.robot.subsystems.Elevator.Elevator; -import frc.robot.subsystems.Elevator.ElevatorExtenderIOSim; -import frc.robot.subsystems.Elevator.ElevatorExtenderIOTalonFX; -import frc.robot.subsystems.Elevator.ElevatorPivotIOSim; -import frc.robot.subsystems.Elevator.ElevatorPivotIOTalonFX; public class SetPivotTarget extends Command { /** Creates a new SetPivotTarget. */ private final Elevator elevator; + private double setPoint; + public SetPivotTarget(double setPoint, Elevator elevator) { setPoint = this.setPoint; - this.elevator = elevator; + this.elevator = elevator; // Use addRequirements() here to declare subsystem dependencies. } @@ -35,4 +31,4 @@ public void end(boolean interrupted) {} public boolean isFinished() { return elevator.pivotAtSetpoint(); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/Elevator/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator/Elevator.java index 740b461..4a34b28 100644 --- a/src/main/java/frc/robot/subsystems/Elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator/Elevator.java @@ -20,6 +20,9 @@ public class Elevator extends SubsystemBase { private static final LoggedTunableNumber extenderkP = new LoggedTunableNumber("elevatorExtenderkP"); + TrapezoidProfile pivotProfile; + TrapezoidProfile extenderProfile; + private final TrapezoidProfile.Constraints pivotConstraints = new TrapezoidProfile.Constraints(Math.PI / 4, Math.PI / 3); private TrapezoidProfile.State pivotGoal = new TrapezoidProfile.State(); @@ -39,10 +42,15 @@ public Elevator(ElevatorPivotIO pivot, ElevatorExtenderIO extender) { switch (Constants.currentMode) { case REAL: - elevatorFFModel = new ElevatorFeedforward(0.02, 0.05, 1.4); - pivotFFModel = new ArmFeedforward(0, 0.4, 0.7); - pivotkP.initDefault(Constants.ElevatorConstants.PIVOT_PID[0]); - extenderkP.initDefault(Constants.ElevatorConstants.EXTENDER_PID[0]); + // elevatorFFModel = new ElevatorFeedforward(0.02, 0.05, 1.4); + // pivotFFModel = new ArmFeedforward(0, 0.4, 0.7); + // pivotkP.initDefault(Constants.ElevatorConstants.PIVOT_PID[0]); + // extenderkP.initDefault(Constants.ElevatorConstants.EXTENDER_PID[0]); + + elevatorFFModel = new ElevatorFeedforward(0, 0, 0.05); + pivotFFModel = new ArmFeedforward(0, 0, 0); + pivotkP.initDefault(0); + extenderkP.initDefault(0.04); break; case REPLAY: elevatorFFModel = new ElevatorFeedforward(0.02, 0.05, 1.4); @@ -63,6 +71,15 @@ public Elevator(ElevatorPivotIO pivot, ElevatorExtenderIO extender) { extenderkP.initDefault(15); break; } + setPivotGoal(30); + pivotProfile = new TrapezoidProfile(pivotConstraints); + + setExtenderGoal(-3); + extenderProfile = new TrapezoidProfile(extenderConstraints); + extenderCurrent = extenderProfile.calculate(0, extenderCurrent, extenderGoal); + + // TODO:: set up default extender pose + pivotCurrent = pivotProfile.calculate(0, pivotCurrent, pivotGoal); pivotkP.initDefault(0); extenderkP.initDefault(15); @@ -128,9 +145,6 @@ public void periodic() { pivot.updateInputs(pInputs); extender.updateInputs(eInputs); - TrapezoidProfile pivotProfile = new TrapezoidProfile(pivotConstraints); - TrapezoidProfile extenderProfile = new TrapezoidProfile(extenderConstraints); - extenderCurrent = extenderProfile.calculate(Constants.LOOP_PERIOD_SECS, extenderCurrent, extenderGoal); pivotCurrent = pivotProfile.calculate(Constants.LOOP_PERIOD_SECS, pivotCurrent, pivotGoal); diff --git a/src/main/java/frc/robot/subsystems/Elevator/ElevatorExtenderIOTalonFX.java b/src/main/java/frc/robot/subsystems/Elevator/ElevatorExtenderIOTalonFX.java index ab60ca7..66e36e6 100644 --- a/src/main/java/frc/robot/subsystems/Elevator/ElevatorExtenderIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/Elevator/ElevatorExtenderIOTalonFX.java @@ -11,6 +11,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.util.Units; import frc.robot.Constants; +import frc.robot.util.Conversions; public class ElevatorExtenderIOTalonFX implements ElevatorExtenderIO { private final TalonFX leader; @@ -27,17 +28,17 @@ public ElevatorExtenderIOTalonFX(int lead, int follow) { config.CurrentLimits.StatorCurrentLimit = Constants.ElevatorConstants.EXTENDER_CURRENT_LIMIT; config.CurrentLimits.StatorCurrentLimitEnable = Constants.ElevatorConstants.EXTENDER_CURRENT_LIMIT_ENABLED; - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.NeutralMode = NeutralModeValue.Coast; config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor; - leader = new TalonFX(lead); - follower = new TalonFX(follow); + leader = new TalonFX(lead, Constants.CANBUS); + follower = new TalonFX(follow, Constants.CANBUS); leader.getConfigurator().apply(config); positionSetpoint = Constants.ElevatorConstants.EXTENDER_RETRACT; - follower.setControl(new Follower(lead, false)); + follower.setControl(new Follower(lead, true)); elevatorPosition = leader.getPosition(); elevatorVelocity = leader.getVelocity(); @@ -51,7 +52,8 @@ public ElevatorExtenderIOTalonFX(int lead, int follow) { @Override public void updateInputs(ElevatorExtenderIOInputs inputs) { BaseStatusSignal.refreshAll(elevatorPosition, elevatorVelocity, appliedVolts, currentAmps); - inputs.elevatorPosition = Units.rotationsToDegrees(elevatorPosition.getValueAsDouble()); + inputs.elevatorPosition = + Conversions.motorRotToInches(elevatorPosition.getValueAsDouble(), 5.97, 15); inputs.elevatorVelocity = Units.rotationsPerMinuteToRadiansPerSecond(elevatorVelocity.getValueAsDouble()); inputs.appliedVolts = appliedVolts.getValueAsDouble(); @@ -61,7 +63,16 @@ public void updateInputs(ElevatorExtenderIOInputs inputs) { @Override public void setPositionSetpoint(double position, double ffVolts) { this.positionSetpoint = position; - leader.setControl(new PositionVoltage(position, 0, false, ffVolts, 0, false, false, false)); + leader.setControl( + new PositionVoltage( + Conversions.inchesToMotorRot(position, 5.97, 15), + 0, + false, + ffVolts, + 0, + false, + false, + false)); } @Override diff --git a/src/main/java/frc/robot/subsystems/Elevator/ElevatorPivotIOTalonFX.java b/src/main/java/frc/robot/subsystems/Elevator/ElevatorPivotIOTalonFX.java index ffca00d..e9f40e1 100644 --- a/src/main/java/frc/robot/subsystems/Elevator/ElevatorPivotIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/Elevator/ElevatorPivotIOTalonFX.java @@ -3,19 +3,17 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.util.Units; import frc.robot.Constants; +import frc.robot.util.Conversions; +import org.littletonrobotics.junction.Logger; public class ElevatorPivotIOTalonFX implements ElevatorPivotIO { private final TalonFX leader; @@ -25,6 +23,8 @@ public class ElevatorPivotIOTalonFX implements ElevatorPivotIO { private double positionSetpoint; + private double startAngle; + private final StatusSignal pivotPosition; private final StatusSignal pivotVelocity; private final StatusSignal appliedVolts; @@ -36,17 +36,17 @@ public ElevatorPivotIOTalonFX(int leadID, int followID, int gyroID) { config.CurrentLimits.StatorCurrentLimit = Constants.ElevatorConstants.PIVOT_CURRENT_LIMIT; config.CurrentLimits.StatorCurrentLimitEnable = Constants.ElevatorConstants.PIVOT_CURRENT_LIMIT_ENABLED; - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.NeutralMode = NeutralModeValue.Coast; config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor; - leader = new TalonFX(leadID); - follower = new TalonFX(followID); - pigeon = new Pigeon2(gyroID); + leader = new TalonFX(leadID, Constants.CANBUS); + follower = new TalonFX(followID, Constants.CANBUS); + pigeon = new Pigeon2(gyroID, Constants.CANBUS); - pigeon.getConfigurator().apply(new Pigeon2Configuration()); + // pigeon.getConfigurator().apply(new Pigeon2Configuration()); leader.getConfigurator().apply(config); - follower.setControl(new Follower(leadID, false)); + follower.setControl(new Follower(leadID, true)); pivotPosition = leader.getPosition(); pivotVelocity = leader.getVelocity(); @@ -55,14 +55,21 @@ public ElevatorPivotIOTalonFX(int leadID, int followID, int gyroID) { pitch = pigeon.getRoll(); + startAngle = pitch.getValueAsDouble(); + + leader.setPosition( + Conversions.degreesToFalcon(startAngle, Constants.ElevatorConstants.PIVOT_RATIO)); + positionSetpoint = Constants.ElevatorConstants.PIVOT_STOW; + Logger.recordOutput("start angle", startAngle); + pigeon.optimizeBusUtilization(); leader.optimizeBusUtilization(); follower.optimizeBusUtilization(); BaseStatusSignal.setUpdateFrequencyForAll( - 100, pivotPosition, pivotVelocity, appliedVolts, currentAmps); + 100, pivotPosition, pivotVelocity, appliedVolts, currentAmps, pitch); } @Override @@ -70,9 +77,12 @@ public void updateInputs(ElevatorPivotIOInputs inputs) { BaseStatusSignal.refreshAll(pivotPosition, pivotVelocity, appliedVolts, currentAmps, pitch); inputs.gyroConnected = BaseStatusSignal.refreshAll(pitch).equals(StatusCode.OK); inputs.pitch = pitch.getValueAsDouble(); - inputs.pivotPosition = Units.rotationsToDegrees(pivotPosition.getValueAsDouble()); + inputs.pivotPosition = + Conversions.falconToDegrees( + pivotPosition.getValueAsDouble(), Constants.ElevatorConstants.PIVOT_RATIO); inputs.pivotVelocity = - Units.rotationsPerMinuteToRadiansPerSecond(pivotVelocity.getValueAsDouble()); + Conversions.falconToDegrees( + pivotVelocity.getValueAsDouble() * 2048, Constants.ElevatorConstants.PIVOT_RATIO); inputs.appliedVolts = appliedVolts.getValueAsDouble(); inputs.currentAmps = currentAmps.getValueAsDouble(); } @@ -80,7 +90,16 @@ public void updateInputs(ElevatorPivotIOInputs inputs) { @Override public void setPositionSetpoint(double position, double ffVolts) { this.positionSetpoint = position; - leader.setControl(new PositionVoltage(position, 0, false, ffVolts, 0, false, false, false)); + leader.setControl( + new PositionVoltage( + Conversions.degreesToFalcon(position, Constants.ElevatorConstants.PIVOT_RATIO), + 0, + false, + ffVolts, + 0, + false, + false, + false)); } @Override diff --git a/src/main/java/frc/robot/subsystems/Shooter/FeederIOTalonFX.java b/src/main/java/frc/robot/subsystems/Shooter/FeederIOTalonFX.java index a11bcdd..be10051 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/FeederIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/Shooter/FeederIOTalonFX.java @@ -7,7 +7,6 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.util.Units; import frc.robot.Constants; public class FeederIOTalonFX implements FeederIO { @@ -26,7 +25,7 @@ public FeederIOTalonFX(int id) { Constants.ShooterConstants.FLYWHEEL_CURRENT_LIMIT_ENABLED; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - falcon = new TalonFX(id); + falcon = new TalonFX(id, Constants.CANBUS); falcon.getConfigurator().apply(config); @@ -39,8 +38,7 @@ public FeederIOTalonFX(int id) { @Override public void updateInputs(FeederIOInputs inputs) { - inputs.feederVelocity = - Units.rotationsPerMinuteToRadiansPerSecond(feederVelocity.getValueAsDouble()); + inputs.feederVelocity = feederVelocity.getValueAsDouble(); inputs.appliedVolts = appliedAmps.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/Shooter/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/Shooter/FlywheelIOTalonFX.java index ac56979..7964105 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/Shooter/FlywheelIOTalonFX.java @@ -4,44 +4,50 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.util.Units; import frc.robot.Constants; public class FlywheelIOTalonFX implements FlywheelIO { - private final TalonFX falcon; + private final TalonFX leader; + private final TalonFX follower; private final StatusSignal shooterVelocity; private final StatusSignal appliedAmps; private final StatusSignal currentAmps; private double velocitySetpoint = 0.0; - public FlywheelIOTalonFX(int id) { + public FlywheelIOTalonFX(int leadID, int followID) { TalonFXConfiguration config = new TalonFXConfiguration(); config.CurrentLimits.StatorCurrentLimit = Constants.ShooterConstants.FLYWHEEL_CURRENT_LIMIT; config.CurrentLimits.StatorCurrentLimitEnable = Constants.ShooterConstants.FLYWHEEL_CURRENT_LIMIT_ENABLED; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - falcon = new TalonFX(id); + leader = new TalonFX(leadID, Constants.CANBUS); + follower = new TalonFX(followID, Constants.CANBUS); - falcon.getConfigurator().apply(config); + leader.getConfigurator().apply(config); - shooterVelocity = falcon.getVelocity(); - appliedAmps = falcon.getMotorVoltage(); - currentAmps = falcon.getStatorCurrent(); + follower.setControl(new Follower(leadID, true)); + + shooterVelocity = leader.getVelocity(); + appliedAmps = leader.getMotorVoltage(); + currentAmps = leader.getStatorCurrent(); BaseStatusSignal.setUpdateFrequencyForAll(100, shooterVelocity, appliedAmps, currentAmps); } @Override public void updateInputs(FlywheelIOInputs inputs) { - inputs.shooterVelocity = - Units.rotationsPerMinuteToRadiansPerSecond(shooterVelocity.getValueAsDouble()); + inputs.shooterVelocity = shooterVelocity.getValueAsDouble(); inputs.appliedVolts = appliedAmps.getValueAsDouble(); @@ -52,7 +58,7 @@ public void updateInputs(FlywheelIOInputs inputs) { @Override public void setVelocity(double velocity, double ffVolts) { this.velocitySetpoint = velocity; - falcon.setControl( + leader.setControl( new VelocityVoltage( Units.radiansToRotations(velocity), 0, true, ffVolts, 0, false, false, false)); } @@ -60,7 +66,7 @@ public void setVelocity(double velocity, double ffVolts) { @Override public void stop() { velocitySetpoint = 0; - falcon.stopMotor(); + leader.stopMotor(); } @Override @@ -71,6 +77,6 @@ public void configurePID(double kP, double kI, double kD) { configs.kI = kI; configs.kD = kD; - falcon.getConfigurator().apply(configs); + leader.getConfigurator().apply(configs); } } diff --git a/src/main/java/frc/robot/subsystems/Shooter/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java index b017ea6..e116bcb 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java @@ -11,25 +11,25 @@ public class Shooter extends SubsystemBase { /** Creates a new Shooter. */ - private final FlywheelIO shooterMotor1; + private final FlywheelIO flywheels; - private final FlywheelIO shooterMotor2; + // private final FlywheelIO shooterMotor2; private final FeederIO feeder; - private final FlywheelIOInputsAutoLogged s1Inputs = new FlywheelIOInputsAutoLogged(); - private final FlywheelIOInputsAutoLogged s2Inputs = new FlywheelIOInputsAutoLogged(); + private final FlywheelIOInputsAutoLogged fInputs = new FlywheelIOInputsAutoLogged(); + // private final FlywheelIOInputsAutoLogged s2Inputs = new FlywheelIOInputsAutoLogged(); private final FeederIOInputsAutoLogged feedInputs = new FeederIOInputsAutoLogged(); private final SimpleMotorFeedforward flywheelFFModel; private final SimpleMotorFeedforward feederFFModel; - public Shooter(FlywheelIO shooterMotor1, FlywheelIO shooterMotor2, FeederIO feeder) { + public Shooter(FlywheelIO flywheels, FeederIO feeder) { switch (Constants.currentMode) { case REAL: - flywheelFFModel = new SimpleMotorFeedforward(0, 0.03); - feederFFModel = new SimpleMotorFeedforward(0, 0.03); + flywheelFFModel = new SimpleMotorFeedforward(0, 3); + feederFFModel = new SimpleMotorFeedforward(0, 0.3); break; case REPLAY: flywheelFFModel = new SimpleMotorFeedforward(0, 0.03); @@ -44,20 +44,20 @@ public Shooter(FlywheelIO shooterMotor1, FlywheelIO shooterMotor2, FeederIO feed feederFFModel = new SimpleMotorFeedforward(0, 0.03); break; } - this.shooterMotor1 = shooterMotor1; - this.shooterMotor2 = shooterMotor2; + this.flywheels = flywheels; + // this.shooterMotor2 = shooterMotor2; // TODO:: Make these constants - shooterMotor1.configurePID(0, 0, 0); - shooterMotor2.configurePID(0, 0, 0); + flywheels.configurePID(0.5, 0, 0); + // shooterMotor2.configurePID(0.5, 0, 0); this.feeder = feeder; // TODO:: Make these constants - feeder.configurePID(0, 0, 0); + feeder.configurePID(0.5, 0, 0); } public void stopShooterMotors() { - shooterMotor1.stop(); - shooterMotor2.stop(); + flywheels.stop(); + // shooterMotor2.stop(); } public void stopFeeders() { @@ -69,18 +69,18 @@ public void runFeeders(double velocity) { } public void setShooterVelocitys(double velocity1, double velocity2) { - shooterMotor1.setVelocity(velocity1, flywheelFFModel.calculate(velocity1)); - shooterMotor2.setVelocity(velocity2, flywheelFFModel.calculate(velocity2)); + flywheels.setVelocity(velocity1, flywheelFFModel.calculate(velocity1)); + // shooterMotor2.setVelocity(velocity2, flywheelFFModel.calculate(velocity2)); } public double[] getFlywheelVelocities() { - return new double[] {s1Inputs.shooterVelocity, s2Inputs.shooterVelocity}; + return new double[] {fInputs.shooterVelocity, fInputs.shooterVelocity}; } public double[] getFlywheelErrors() { return new double[] { - s1Inputs.velocitySetpoint - getFlywheelVelocities()[0], - s2Inputs.velocitySetpoint - getFlywheelVelocities()[1] + fInputs.velocitySetpoint - getFlywheelVelocities()[0], + fInputs.velocitySetpoint - getFlywheelVelocities()[1] }; } @@ -93,13 +93,13 @@ public boolean atFlywheelSetpoints() { public void periodic() { // This method will be called once per scheduler run - shooterMotor1.updateInputs(s1Inputs); - shooterMotor2.updateInputs(s2Inputs); + flywheels.updateInputs(fInputs); + // shooterMotor2.updateInputs(s2Inputs); feeder.updateInputs(feedInputs); - Logger.processInputs("Flywheel 1", s1Inputs); - Logger.processInputs("Flywheel 2", s2Inputs); + Logger.processInputs("Flywheel 1", fInputs); + // Logger.processInputs("Flywheel 2", s2Inputs); Logger.processInputs("Feeder", feedInputs); } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 29fe24c..048c846 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -7,7 +7,6 @@ import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.units.Measure; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; diff --git a/src/main/java/frc/robot/util/Conversions.java b/src/main/java/frc/robot/util/Conversions.java index a5cfeb1..e30b44d 100644 --- a/src/main/java/frc/robot/util/Conversions.java +++ b/src/main/java/frc/robot/util/Conversions.java @@ -26,7 +26,8 @@ public static double degreesToCANcoder(double degrees, double gearRatio) { * @return Degrees of Rotation of Mechanism */ public static double falconToDegrees(double positionCounts, double gearRatio) { - return positionCounts * (360.0 / (gearRatio * 2048.0)); + // return positionCounts * (360.0 / (gearRatio * 2048.0)); + return positionCounts * (360.0 / (gearRatio)); } /** @@ -35,7 +36,8 @@ public static double falconToDegrees(double positionCounts, double gearRatio) { * @return Falcon Position Counts */ public static double degreesToFalcon(double degrees, double gearRatio) { - return degrees / (360.0 / (gearRatio * 2048.0)); + // return degrees / (360.0 / (gearRatio * 2048.0)); + return degrees / (360.0 / (gearRatio)); } /**