diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1f8d5ef..595f87a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -49,6 +49,7 @@ public static final class HardwarePorts { public static final int climbLeaderMotor = 3; public static final int climbFollowerMotor = 4; public static final int pivotMotor = 30; + public static final int pivotCANcoderID = 31; } @@ -78,8 +79,8 @@ public static final class SwerveConstants { .SDSMK4i(COTSFalconSwerveConstants.driveGearRatios.SDSMK4_L2); /* Drivetrain Constants */ - public static final double trackWidth = 0.5715; - public static final double wheelBase = 0.5715; + public static final double trackWidth = 0.47625; + public static final double wheelBase = 0.47625; public static final double wheelCircumference = chosenModule.wheelCircumference; /* * Swerve Kinematics diff --git a/src/main/java/frc/robot/commands/SetPivot.java b/src/main/java/frc/robot/commands/SetPivot.java new file mode 100644 index 0000000..084b14a --- /dev/null +++ b/src/main/java/frc/robot/commands/SetPivot.java @@ -0,0 +1,46 @@ +package frc.robot.commands; + +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Pivot; +import frc.robot.subsystems.Pivot.PivotState;; + +public class SetPivot extends Command { + Pivot s_Pivot; + PivotState state; + ProfiledPIDController pivotController = new ProfiledPIDController(0.06, 1e-2, 1e-3, new TrapezoidProfile.Constraints(500000, 3000*1e5)); + + public SetPivot(PivotState state) { + s_Pivot = Pivot.getInstance(); + this.state = state; + addRequirements(s_Pivot); + } + + @Override + public void initialize() { + s_Pivot.setState(state); + pivotController.reset(s_Pivot.getCANcoderPosition()); + } + + @Override + public void execute() { + double voltage = pivotController.calculate(s_Pivot.getCANcoderPosition(), s_Pivot.getSetPoint()); + if (Math.abs(s_Pivot.getCANcoderPosition() - s_Pivot.getSetPoint()) < 15) { + voltage = 0.7; + } + s_Pivot.setVoltage(voltage); + } + + @Override + public boolean isFinished() { + return Math.abs(s_Pivot.getSetPoint() - s_Pivot.getCANcoderPosition()) < 45; + } + + @Override + public void end(boolean interrupted) { + s_Pivot.setVoltage(0); + SmartDashboard.putString("eleEnd", "elevator end"); + } +} diff --git a/src/main/java/frc/robot/commands/TossCommand.java b/src/main/java/frc/robot/commands/TossCommand.java deleted file mode 100644 index 3681329..0000000 --- a/src/main/java/frc/robot/commands/TossCommand.java +++ /dev/null @@ -1,39 +0,0 @@ -// 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; - -import frc.robot.subsystems.ShooterSubsystem; -import edu.wpi.first.wpilibj2.command.Command; - -/** An example command that uses an example subsystem. */ -public class TossCommand extends Command { - private final ShooterSubsystem s_shooter; - private boolean finish; - - public TossCommand() { - s_shooter = ShooterSubsystem.getInstance(); - addRequirements(s_shooter); - finish = false; - } - - @Override - public void initialize() {} - - @Override - public void execute() { - s_shooter.setVoltage(10); - finish = true; - } - - @Override - public void end(boolean interrupted) { - s_shooter.setVoltage(0); - } - - @Override - public boolean isFinished() { - return finish; - } -} diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 34e08cc..21a990d 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -1,5 +1,10 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.MagnetSensorConfigs; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -16,7 +21,7 @@ public static Pivot getInstance() { return instance; } - private enum PivotState { + public enum PivotState { GROUND(0); private double pos; @@ -27,14 +32,31 @@ private PivotState(double position) { } private CANSparkFlex mPivotMotor; + private CANcoder pivotCANcoder; + private PivotState curState = PivotState.GROUND; public Pivot() { mPivotMotor = new CANSparkFlex(Constants.HardwarePorts.pivotMotor, MotorType.kBrushless); configMotor(); + + pivotCANcoder = new CANcoder(Constants.HardwarePorts.pivotCANcoderID); + configCANcoder(); } - - public void setPosition(PivotState state) { - mPivotMotor.getEncoder().setPosition(state.pos); + + public void setState(PivotState state) { + curState = state; + } + + public double getCANcoderPosition() { + return pivotCANcoder.getPosition().getValueAsDouble(); + } + + public double getSetPoint() { + return curState.pos; + } + + public void setVoltage(double voltage) { + mPivotMotor.setVoltage(voltage); } private void configMotor() { @@ -46,4 +68,16 @@ private void configMotor() { mPivotMotor.getPIDController().setD(Constants.hardwarePIDs.pivotkD); } + private void configCANcoder(){ + /* Swerve CANCoder Configuration */ + // CANcoder is always initialized to absolute position on boot in Phoenix 6 - https://www.chiefdelphi.com/t/what-kind-of-encoders-are-built-into-the-kraken-motors/447253/7 + + MagnetSensorConfigs magnetSensorConfigs = new MagnetSensorConfigs(); + magnetSensorConfigs.AbsoluteSensorRange = AbsoluteSensorRangeValue.Unsigned_0To1; + magnetSensorConfigs.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + + CANcoderConfiguration swerveCanCoderConfig = new CANcoderConfiguration(); + swerveCanCoderConfig.MagnetSensor = magnetSensorConfigs; + pivotCANcoder.getConfigurator().apply(swerveCanCoderConfig); + } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java deleted file mode 100644 index cb96472..0000000 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ /dev/null @@ -1,54 +0,0 @@ -// 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.subsystems; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import com.ctre.phoenix6.controls.StrictFollower; -import com.ctre.phoenix6.hardware.TalonFX; - -public class ShooterSubsystem extends SubsystemBase { - - private static ShooterSubsystem instance; - public static ShooterSubsystem getInstance() { - if (instance == null) - instance = new ShooterSubsystem(); - return instance; - } - - private TalonFX shooterLeaderM; - private TalonFX shooterFollowerM; - - private double voltage; - - public ShooterSubsystem() { - shooterLeaderM = new TalonFX(Constants.HardwarePorts.shooterLeaderM); - shooterFollowerM = new TalonFX(Constants.HardwarePorts.shooterFollowerM); - shooterFollowerM.setInverted(true); - shooterFollowerM.setControl(new StrictFollower(Constants.HardwarePorts.shooterLeaderM)); - } - - - public void setVoltage(double voltage) { - this.voltage = voltage; - shooterLeaderM.setVoltage(voltage); - } - - public double getVoltageSetpoint() { - return voltage; - } - - @Override - public void periodic() { - SmartDashboard.putNumber("Motor Voltage Setpoint", getVoltageSetpoint()); - } - - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation - } -}