From 1c2dd4cf576b3a63bf4c770c371bd9f48a8aade7 Mon Sep 17 00:00:00 2001 From: charlesmackenzie Date: Sun, 10 Dec 2023 01:27:03 -0500 Subject: [PATCH] Naively implement hardware IO --- src/main/java/frc/robot/Constants.java | 8 ++ src/main/java/frc/robot/RobotContainer.java | 32 ++++- .../robot/subsystems/arm/ArmSubsystem.java | 11 +- .../robot/subsystems/arm/GenericArmJoint.java | 8 +- .../java/frc/robot/subsystems/arm/Pivot.java | 135 ------------------ .../frc/robot/subsystems/arm/Telescope.java | 134 ----------------- .../frc/robot/subsystems/arm/pivot/Pivot.java | 112 +++++++++++++++ .../robot/subsystems/arm/pivot/PivotIO.java | 20 +++ .../subsystems/arm/pivot/PivotIOFalcon.java | 63 ++++++++ .../subsystems/arm/telescope/Telescope.java | 131 +++++++++++++++++ .../subsystems/arm/telescope/TelescopeIO.java | 22 +++ .../arm/telescope/TelescopeIOFalcon.java | 46 ++++++ .../subsystems/arm/{ => wrist}/Wrist.java | 84 ++++++----- .../robot/subsystems/arm/wrist/WristIO.java | 22 +++ .../subsystems/arm/wrist/WristIOFalcon.java | 47 ++++++ 15 files changed, 549 insertions(+), 326 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/arm/Pivot.java delete mode 100644 src/main/java/frc/robot/subsystems/arm/Telescope.java create mode 100644 src/main/java/frc/robot/subsystems/arm/pivot/Pivot.java create mode 100644 src/main/java/frc/robot/subsystems/arm/pivot/PivotIO.java create mode 100644 src/main/java/frc/robot/subsystems/arm/pivot/PivotIOFalcon.java create mode 100644 src/main/java/frc/robot/subsystems/arm/telescope/Telescope.java create mode 100644 src/main/java/frc/robot/subsystems/arm/telescope/TelescopeIO.java create mode 100644 src/main/java/frc/robot/subsystems/arm/telescope/TelescopeIOFalcon.java rename src/main/java/frc/robot/subsystems/arm/{ => wrist}/Wrist.java (55%) create mode 100644 src/main/java/frc/robot/subsystems/arm/wrist/WristIO.java create mode 100644 src/main/java/frc/robot/subsystems/arm/wrist/WristIOFalcon.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c635542..ca2d0c0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -23,6 +23,14 @@ public final class Constants { + public enum Mode { + REAL, + SIM, + REPLAY, + } + + public static final Mode mode = Mode.REAL; + public static final double loopTime = 0.02; public static final class CANDevices { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a8b456c..8dcd68d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,11 +5,8 @@ package frc.robot; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -19,8 +16,13 @@ import frc.robot.Constants.GamePieceMode; import frc.robot.Constants.Position; import frc.robot.subsystems.arm.ArmSubsystem; -import frc.robot.subsystems.arm.ArmSubsystem.ActiveArmSide; import frc.robot.subsystems.arm.ArmSubsystem.ArmPosition; +import frc.robot.subsystems.arm.pivot.PivotIO; +import frc.robot.subsystems.arm.pivot.PivotIOFalcon; +import frc.robot.subsystems.arm.telescope.TelescopeIO; +import frc.robot.subsystems.arm.telescope.TelescopeIOFalcon; +import frc.robot.subsystems.arm.wrist.WristIO; +import frc.robot.subsystems.arm.wrist.WristIOFalcon; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.LEDManager; // import frc.robot.subsystems.drive.Drive; @@ -32,11 +34,11 @@ public class RobotContainer { // private final Drive drive = new Drive(); - private final ArmSubsystem arm = new ArmSubsystem(); - private final IntakeSubsystem intake = new IntakeSubsystem(() -> arm.getArmSide()); + private final ArmSubsystem arm; + private final IntakeSubsystem intake; @SuppressWarnings("unused") private final Vision vision = new Vision(); - private final LEDManager ledManager = new LEDManager(() -> arm.getArmSide()); + private final LEDManager ledManager; private final Thrustmaster leftStick = new Thrustmaster(0); private final Thrustmaster rightStick = new Thrustmaster(1); @@ -51,6 +53,22 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + switch (Constants.mode) { + case REAL: + arm = new ArmSubsystem( + new PivotIOFalcon(), + new TelescopeIOFalcon(), + new WristIOFalcon()); + break; + default: + arm = new ArmSubsystem( + new PivotIO() {}, + new TelescopeIO() {}, + new WristIO() {}); + } + + intake = new IntakeSubsystem(arm::getArmSide); + ledManager = new LEDManager(arm::getArmSide); configureSubsystems(); configureCompBindings(); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index cec2d1c..a0e1688 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -15,6 +15,12 @@ import frc.robot.Constants.PivotConstants; import frc.robot.Constants.TelescopeConstants; import frc.robot.Constants.WristConstants; +import frc.robot.subsystems.arm.pivot.Pivot; +import frc.robot.subsystems.arm.pivot.PivotIO; +import frc.robot.subsystems.arm.telescope.Telescope; +import frc.robot.subsystems.arm.telescope.TelescopeIO; +import frc.robot.subsystems.arm.wrist.Wrist; +import frc.robot.subsystems.arm.wrist.WristIO; public class ArmSubsystem extends SubsystemBase { @@ -59,18 +65,21 @@ public class ArmSubsystem extends SubsystemBase { new Color8Bit(Color.kCoral))); - public ArmSubsystem() { + public ArmSubsystem(PivotIO pivotIO, TelescopeIO telescopeIO, WristIO wristIO) { pivot = new Pivot( + pivotIO, new TrapezoidProfile.Constraints( Units.degreesToRadians(360), Units.degreesToRadians(600)), () -> telescope.getPosition()); telescope = new Telescope( + telescopeIO, new TrapezoidProfile.Constraints(3, 3), () -> pivot.getPosition()); wrist = new Wrist( + wristIO, new TrapezoidProfile.Constraints( Units.degreesToRadians(630), Units.degreesToRadians(810)), diff --git a/src/main/java/frc/robot/subsystems/arm/GenericArmJoint.java b/src/main/java/frc/robot/subsystems/arm/GenericArmJoint.java index da27cfd..dd6aec2 100644 --- a/src/main/java/frc/robot/subsystems/arm/GenericArmJoint.java +++ b/src/main/java/frc/robot/subsystems/arm/GenericArmJoint.java @@ -41,7 +41,7 @@ public void setSetpoint(double setpoint) { } public void runControls() { - update(); + updateInputs(); if (!active) { setOutput(0.0); @@ -67,11 +67,7 @@ public void runControls() { public abstract void setBrakeMode(boolean brake); - /** - * Optional Override: Update internal state periodically - */ - protected void update() { - } + protected abstract void updateInputs(); /** * Set the control input to this joint, in volts diff --git a/src/main/java/frc/robot/subsystems/arm/Pivot.java b/src/main/java/frc/robot/subsystems/arm/Pivot.java deleted file mode 100644 index ae61105..0000000 --- a/src/main/java/frc/robot/subsystems/arm/Pivot.java +++ /dev/null @@ -1,135 +0,0 @@ -package frc.robot.subsystems.arm; - -import java.util.function.DoubleSupplier; - -import com.ctre.phoenix6.controls.CoastOut; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.StaticBrake; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.DutyCycleEncoder; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Constants; -import frc.robot.Constants.CANDevices; -import frc.robot.Constants.PivotConstants; -import frc.robot.Constants.TelescopeConstants; - -public class Pivot extends GenericArmJoint { - - private TalonFX rightMotor = new TalonFX(CANDevices.rightPivotMotorID, CANDevices.canivoreName); - private DutyCycleEncoder encoder = new DutyCycleEncoder(CANDevices.pivotEncoderID); - private TalonFX leftMotor = new TalonFX(CANDevices.leftPivotMotorID, CANDevices.canivoreName); - - private double velocity = 0.0; - private double lastPosition; - - public final PIDController feedbackController = new PIDController( - PivotConstants.kP, - 0, - PivotConstants.kD); - private final ArmFeedforward feedforward = new ArmFeedforward( - PivotConstants.kS, - PivotConstants.kG, - PivotConstants.kV, - PivotConstants.kA); - - private final DoubleSupplier telescopePositionSupplier; - - public Pivot( - TrapezoidProfile.Constraints constraints, - DoubleSupplier telescopePositionSupplier, - double range, - double defaultSetpoint - ) { - super(constraints, range, defaultSetpoint); - - this.telescopePositionSupplier = telescopePositionSupplier; - - rightMotor.setInverted(false); - - leftMotor.setControl(new Follower(CANDevices.rightPivotMotorID, true)); - - rightMotor.setControl(new StaticBrake()); - - //I have no idea how to set a current limit from the API - - lastPosition = getPosition(); - } - - public Pivot(TrapezoidProfile.Constraints contraints, DoubleSupplier telescopePositionSupplier, double range) { - this(contraints, telescopePositionSupplier, range, Math.PI / 2); - } - - public Pivot(TrapezoidProfile.Constraints contraints, DoubleSupplier telescopePositionSupplier) { - this(contraints, telescopePositionSupplier, 0.03); - } - - @Override - public double getPosition() { - return encoder.getAbsolutePosition() * 2 * Math.PI + PivotConstants.encoderOffsetRad; - } - - @Override - public double getVelocity() { - return velocity; - } - - @Override - public void setBrakeMode(boolean brake) { - rightMotor.setControl(brake ? new StaticBrake() : new CoastOut()); - } - - @Override - public void jogSetpointPositive() { - setpoint = MathUtil.clamp( - setpoint + Math.PI / 64, - PivotConstants.maxFwdRotationRad, - PivotConstants.maxBackRotationRad); - } - - @Override - public void jogSetpointNegative() { - setpoint = MathUtil.clamp( - setpoint - Math.PI / 64, - PivotConstants.maxFwdRotationRad, - PivotConstants.maxBackRotationRad); - } - - @Override - protected double calculateControl(TrapezoidProfile.State setpointState) { - SmartDashboard.putNumber("Pivot/control-setpoint", setpointState.position); - return feedbackController.calculate(getPosition(), setpointState.position) - + feedforward.calculate(setpointState.position, setpointState.velocity) - // Compensates for telescope extention - // Gravity constant * Telescope Extention (proportion) * Cosine of Angle - + PivotConstants.extraKg - * telescopePositionSupplier.getAsDouble() / TelescopeConstants.maxPosM - * Math.cos(getPosition()); - } - - @Override - protected void update() { - findVelocity(); - } - - @Override - protected void setOutput(double volts) { - rightMotor.setControl(new VoltageOut(volts)); - } - - @Override - protected void resetControlLoop() { - feedbackController.reset(); - } - - private void findVelocity() { - velocity = (getPosition() - lastPosition) / Constants.loopTime; - lastPosition = getPosition(); - } - -} diff --git a/src/main/java/frc/robot/subsystems/arm/Telescope.java b/src/main/java/frc/robot/subsystems/arm/Telescope.java deleted file mode 100644 index 40ec54b..0000000 --- a/src/main/java/frc/robot/subsystems/arm/Telescope.java +++ /dev/null @@ -1,134 +0,0 @@ -package frc.robot.subsystems.arm; - -import java.util.function.DoubleSupplier; - -import com.ctre.phoenix6.controls.CoastOut; -import com.ctre.phoenix6.controls.StaticBrake; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Timer; -import frc.robot.Constants.CANDevices; -import frc.robot.Constants.TelescopeConstants; - -public class Telescope extends GenericArmJoint { - - private TalonFX motor = new TalonFX(CANDevices.telescopeMotorID); - - private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward( - TelescopeConstants.kS, - TelescopeConstants.kV); - private final PIDController feedbackController = - new PIDController(TelescopeConstants.kP, 0, 0); - - private final DoubleSupplier pivotAngleSupplier; - - private final Timer homeTimer = new Timer(); - - - public Telescope( - TrapezoidProfile.Constraints constraints, - DoubleSupplier pivotAngleSupplier, - double range, - double defaultSetpoint - ) { - super(constraints, range, defaultSetpoint); - - this.pivotAngleSupplier = pivotAngleSupplier; - - motor.setInverted(false); - motor.setControl(new StaticBrake()); - - //I have no idea how to set a current limit from the API - } - - public Telescope(TrapezoidProfile.Constraints constraints, DoubleSupplier pivotPositionSupplier, double range) { - this(constraints, pivotPositionSupplier, range, TelescopeConstants.stowedPosition); - } - - public Telescope(TrapezoidProfile.Constraints constraints, DoubleSupplier pivotPositionSupplier) { - this(constraints, pivotPositionSupplier, 0.02); - } - - @Override - public double getPosition() { - return motor.getPosition().getValueAsDouble() * 2 * Math.PI - * TelescopeConstants.conversionM; - } - - @Override - public double getVelocity() { - return motor.getVelocity().getValueAsDouble() * 2 * Math.PI - * TelescopeConstants.conversionM; - } - - @Override - public void setBrakeMode(boolean brake) { - motor.setControl(brake ? new StaticBrake() : new CoastOut()); - } - - @Override - public void jogSetpointPositive() { - setpoint = MathUtil.clamp( - setpoint + 0.05, - TelescopeConstants.minPosM, - TelescopeConstants.maxPosM); - } - - @Override - public void jogSetpointNegative() { - setpoint = MathUtil.clamp( - setpoint - 0.05, - TelescopeConstants.minPosM, - TelescopeConstants.maxPosM); - } - - @Override - public void home() { - homing = true; - - setOutput(-1.0); - - homeTimer.reset(); - homeTimer.start(); - } - - @Override - protected boolean runHomingLogic() { - if (motor.getStatorCurrent().getValue() < 30) { - homeTimer.reset(); - } - - if (homeTimer.hasElapsed(0.3)) { - motor.setPosition(0); - - homing = false; - return true; - } - - return false; - } - - @Override - protected double calculateControl(TrapezoidProfile.State setpoint) { - return feedbackController.calculate(getPosition(), setpoint.position) - + feedforward.calculate(setpoint.velocity) - // Compensates for weight of telescope as the pivot goes up - // Gravity Constant * Sine of Angle - + TelescopeConstants.kG * Math.sin(pivotAngleSupplier.getAsDouble()); - } - - @Override - protected void setOutput(double volts) { - motor.setControl(new VoltageOut(volts)); - } - - @Override - protected void resetControlLoop() { - feedbackController.reset(); - } -} diff --git a/src/main/java/frc/robot/subsystems/arm/pivot/Pivot.java b/src/main/java/frc/robot/subsystems/arm/pivot/Pivot.java new file mode 100644 index 0000000..8030828 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/pivot/Pivot.java @@ -0,0 +1,112 @@ +package frc.robot.subsystems.arm.pivot; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants.PivotConstants; +import frc.robot.Constants.TelescopeConstants; +import frc.robot.subsystems.arm.GenericArmJoint; + +public class Pivot extends GenericArmJoint { + + private final PivotIO io; + private final PivotIOInputsAutoLogged inputs = new PivotIOInputsAutoLogged(); + + public final PIDController feedbackController = new PIDController( + PivotConstants.kP, + 0, + PivotConstants.kD); + private final ArmFeedforward feedforward = new ArmFeedforward( + PivotConstants.kS, + PivotConstants.kG, + PivotConstants.kV, + PivotConstants.kA); + + private final DoubleSupplier telescopePositionSupplier; + + public Pivot( + PivotIO io, + TrapezoidProfile.Constraints constraints, + DoubleSupplier telescopePositionSupplier, + double range, + double defaultSetpoint) { + super(constraints, range, defaultSetpoint); + + this.telescopePositionSupplier = telescopePositionSupplier; + this.io = io; + } + + public Pivot( + PivotIO io, + TrapezoidProfile.Constraints contraints, + DoubleSupplier telescopePositionSupplier, + double range) { + this(io, contraints, telescopePositionSupplier, range, Math.PI / 2); + } + + public Pivot(PivotIO io, TrapezoidProfile.Constraints contraints, DoubleSupplier telescopePositionSupplier) { + this(io, contraints, telescopePositionSupplier, 0.03); + } + + @Override + public double getPosition() { + return inputs.positionRad; + } + + @Override + public double getVelocity() { + return inputs.velocityRadS; + } + + @Override + public void setBrakeMode(boolean brake) { + io.setBrakeMode(brake); + } + + @Override + public void jogSetpointPositive() { + setpoint = MathUtil.clamp( + setpoint + Math.PI / 64, + PivotConstants.maxFwdRotationRad, + PivotConstants.maxBackRotationRad); + } + + @Override + public void jogSetpointNegative() { + setpoint = MathUtil.clamp( + setpoint - Math.PI / 64, + PivotConstants.maxFwdRotationRad, + PivotConstants.maxBackRotationRad); + } + + @Override + protected double calculateControl(TrapezoidProfile.State setpointState) { + SmartDashboard.putNumber("Pivot/control-setpoint", setpointState.position); + return feedbackController.calculate(getPosition(), setpointState.position) + + feedforward.calculate(setpointState.position, setpointState.velocity) + // Compensates for telescope extention + // Gravity constant * Telescope Extention (proportion) * Cosine of Angle + + PivotConstants.extraKg + * telescopePositionSupplier.getAsDouble() / TelescopeConstants.maxPosM + * Math.cos(getPosition()); + } + + @Override + protected void updateInputs() { + io.updateInputs(inputs); + } + + @Override + protected void setOutput(double volts) { + io.setOutput(volts); + } + + @Override + protected void resetControlLoop() { + feedbackController.reset(); + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/pivot/PivotIO.java b/src/main/java/frc/robot/subsystems/arm/pivot/PivotIO.java new file mode 100644 index 0000000..a020abb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/pivot/PivotIO.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.arm.pivot; + +import org.littletonrobotics.junction.AutoLog; + +public interface PivotIO { + + @AutoLog + public static class PivotIOInputs { + public double positionRad = 0.0; + public double velocityRadS = 0.0; + public double appliedVolts = 0.0; + public double statorCurrent = 0.0; + } + + public default void updateInputs(PivotIOInputsAutoLogged inputs) {} + + public default void setBrakeMode(boolean brake) {} + + public default void setOutput(double volts) {} +} diff --git a/src/main/java/frc/robot/subsystems/arm/pivot/PivotIOFalcon.java b/src/main/java/frc/robot/subsystems/arm/pivot/PivotIOFalcon.java new file mode 100644 index 0000000..98043fd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/pivot/PivotIOFalcon.java @@ -0,0 +1,63 @@ +package frc.robot.subsystems.arm.pivot; + +import com.ctre.phoenix6.controls.CoastOut; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.StaticBrake; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import frc.robot.Constants; +import frc.robot.Constants.CANDevices; +import frc.robot.Constants.PivotConstants; + +public class PivotIOFalcon implements PivotIO { + + private TalonFX rightMotor = new TalonFX(CANDevices.rightPivotMotorID, CANDevices.canivoreName); + private DutyCycleEncoder encoder = new DutyCycleEncoder(CANDevices.pivotEncoderID); + private TalonFX leftMotor = new TalonFX(CANDevices.leftPivotMotorID, CANDevices.canivoreName); + + private double velocity = 0.0; + private double lastPosition; + + public PivotIOFalcon() { + rightMotor.setInverted(false); + + leftMotor.setControl(new Follower(CANDevices.rightPivotMotorID, true)); + + rightMotor.setControl(new StaticBrake()); + + //I have no idea how to set a current limit with the API + + lastPosition = getPosition(); + } + + @Override + public void updateInputs(PivotIOInputsAutoLogged inputs) { + updateVelocity(); + + inputs.positionRad = getPosition(); + inputs.velocityRadS = velocity; + inputs.appliedVolts = rightMotor.getMotorVoltage().getValueAsDouble(); + inputs.statorCurrent = rightMotor.getStatorCurrent().getValueAsDouble(); + } + + @Override + public void setBrakeMode(boolean brake) { + rightMotor.setControl(brake ? new StaticBrake() : new CoastOut()); + } + + @Override + public void setOutput(double volts) { + rightMotor.setControl(new VoltageOut(volts)); + } + + private double getPosition() { + return encoder.getAbsolutePosition() * 2 * Math.PI + PivotConstants.encoderOffsetRad; + } + + private void updateVelocity() { + velocity = (getPosition() - lastPosition) / Constants.loopTime; + lastPosition = getPosition(); + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/telescope/Telescope.java b/src/main/java/frc/robot/subsystems/arm/telescope/Telescope.java new file mode 100644 index 0000000..00bf30c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/telescope/Telescope.java @@ -0,0 +1,131 @@ +package frc.robot.subsystems.arm.telescope; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants.TelescopeConstants; +import frc.robot.subsystems.arm.GenericArmJoint; + +public class Telescope extends GenericArmJoint { + + private final TelescopeIO io; + private final TelescopeIOInputsAutoLogged inputs = new TelescopeIOInputsAutoLogged(); + + private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward( + TelescopeConstants.kS, + TelescopeConstants.kV); + private final PIDController feedbackController = new PIDController(TelescopeConstants.kP, 0, 0); + + private final DoubleSupplier pivotAngleSupplier; + + private final Timer homeTimer = new Timer(); + + public Telescope( + TelescopeIO io, + TrapezoidProfile.Constraints constraints, + DoubleSupplier pivotAngleSupplier, + double range, + double defaultSetpoint) { + super(constraints, range, defaultSetpoint); + + this.io = io; + this.pivotAngleSupplier = pivotAngleSupplier; + } + + public Telescope( + TelescopeIO io, + TrapezoidProfile.Constraints constraints, + DoubleSupplier pivotPositionSupplier, + double range) { + this(io, constraints, pivotPositionSupplier, range, TelescopeConstants.stowedPosition); + } + + public Telescope(TelescopeIO io, TrapezoidProfile.Constraints constraints, DoubleSupplier pivotPositionSupplier) { + this(io, constraints, pivotPositionSupplier, 0.02); + } + + @Override + public void updateInputs() { + io.updateInputs(inputs); + } + + @Override + public double getPosition() { + return inputs.positionM; + } + + @Override + public double getVelocity() { + return inputs.velocityMS; + } + + @Override + public void setBrakeMode(boolean brake) { + io.setBrakeMode(brake); + } + + @Override + public void jogSetpointPositive() { + setpoint = MathUtil.clamp( + setpoint + 0.05, + TelescopeConstants.minPosM, + TelescopeConstants.maxPosM); + } + + @Override + public void jogSetpointNegative() { + setpoint = MathUtil.clamp( + setpoint - 0.05, + TelescopeConstants.minPosM, + TelescopeConstants.maxPosM); + } + + @Override + public void home() { + homing = true; + + setOutput(-1.0); + + homeTimer.reset(); + homeTimer.start(); + } + + @Override + protected boolean runHomingLogic() { + if (inputs.statorCurrent < 30) { + homeTimer.reset(); + } + + if (homeTimer.hasElapsed(0.3)) { + io.setSensorPosition(0); + + homing = false; + return true; + } + + return false; + } + + @Override + protected double calculateControl(TrapezoidProfile.State setpoint) { + return feedbackController.calculate(getPosition(), setpoint.position) + + feedforward.calculate(setpoint.velocity) + // Compensates for weight of telescope as the pivot goes up + // Gravity Constant * Sine of Angle + + TelescopeConstants.kG * Math.sin(pivotAngleSupplier.getAsDouble()); + } + + @Override + protected void setOutput(double volts) { + io.setOutput(volts); + } + + @Override + protected void resetControlLoop() { + feedbackController.reset(); + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/telescope/TelescopeIO.java b/src/main/java/frc/robot/subsystems/arm/telescope/TelescopeIO.java new file mode 100644 index 0000000..b31e71e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/telescope/TelescopeIO.java @@ -0,0 +1,22 @@ +package frc.robot.subsystems.arm.telescope; + +import org.littletonrobotics.junction.AutoLog; + +public interface TelescopeIO { + + @AutoLog + public static class TelescopeIOInputs { + public double positionM = 0.0; + public double velocityMS = 0.0; + public double appliedVolts = 0.0; + public double statorCurrent = 0.0; + } + + public default void updateInputs(TelescopeIOInputsAutoLogged inputs) {} + + public default void setBrakeMode(boolean brake) {} + + public default void setOutput(double volts) {} + + public default void setSensorPosition(double position) {} +} diff --git a/src/main/java/frc/robot/subsystems/arm/telescope/TelescopeIOFalcon.java b/src/main/java/frc/robot/subsystems/arm/telescope/TelescopeIOFalcon.java new file mode 100644 index 0000000..1f39fb4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/telescope/TelescopeIOFalcon.java @@ -0,0 +1,46 @@ +package frc.robot.subsystems.arm.telescope; + +import com.ctre.phoenix6.controls.CoastOut; +import com.ctre.phoenix6.controls.StaticBrake; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; + +import frc.robot.Constants.CANDevices; +import frc.robot.Constants.TelescopeConstants; + +public class TelescopeIOFalcon implements TelescopeIO { + + private TalonFX motor = new TalonFX(CANDevices.telescopeMotorID); + + public TelescopeIOFalcon() { + motor.setInverted(false); + motor.setControl(new StaticBrake()); + + //I have no idea how to set a current limit from the API + } + + @Override + public void updateInputs(TelescopeIOInputsAutoLogged inputs) { + inputs.positionM = motor.getPosition().getValueAsDouble() * 2 * Math.PI + * TelescopeConstants.conversionM; + inputs.velocityMS = motor.getVelocity().getValueAsDouble() * 2 * Math.PI + * TelescopeConstants.conversionM; + inputs.appliedVolts = motor.getMotorVoltage().getValueAsDouble(); + inputs.statorCurrent = motor.getStatorCurrent().getValueAsDouble(); + } + + @Override + public void setBrakeMode(boolean brake) { + motor.setControl(brake ? new StaticBrake() : new CoastOut()); + } + + @Override + public void setOutput(double volts) { + motor.setControl(new VoltageOut(volts)); + } + + @Override + public void setSensorPosition(double pos) { + motor.setPosition(pos / (2 * Math.PI) / TelescopeConstants.conversionM); + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/Wrist.java b/src/main/java/frc/robot/subsystems/arm/wrist/Wrist.java similarity index 55% rename from src/main/java/frc/robot/subsystems/arm/Wrist.java rename to src/main/java/frc/robot/subsystems/arm/wrist/Wrist.java index bb30f48..198777b 100644 --- a/src/main/java/frc/robot/subsystems/arm/Wrist.java +++ b/src/main/java/frc/robot/subsystems/arm/wrist/Wrist.java @@ -1,30 +1,25 @@ -package frc.robot.subsystems.arm; +package frc.robot.subsystems.arm.wrist; import java.util.function.DoubleSupplier; -import com.ctre.phoenix6.controls.CoastOut; -import com.ctre.phoenix6.controls.StaticBrake; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; - import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.Timer; -import frc.robot.Constants.CANDevices; import frc.robot.Constants.WristConstants; +import frc.robot.subsystems.arm.GenericArmJoint; public class Wrist extends GenericArmJoint { - private TalonFX motor = new TalonFX(CANDevices.wristMotorID); - - private final PIDController controller = - new PIDController(WristConstants.kP, WristConstants.kI, WristConstants.kD); + private final WristIO io; + private final WristIOInputsAutoLogged inputs = new WristIOInputsAutoLogged(); + + private final PIDController controller = new PIDController(WristConstants.kP, WristConstants.kI, WristConstants.kD); private final ArmFeedforward feedforward = new ArmFeedforward( - WristConstants.kS, - WristConstants.kG, - WristConstants.kV, - WristConstants.kA); + WristConstants.kS, + WristConstants.kG, + WristConstants.kV, + WristConstants.kA); private final DoubleSupplier pivotAngleSupplier; @@ -32,56 +27,57 @@ public class Wrist extends GenericArmJoint { private boolean homingSecondStep = false; public Wrist( - TrapezoidProfile.Constraints constraints, - DoubleSupplier pivotAngleSupplier, - double range, - double defaultSetpoint - ) { + WristIO io, + TrapezoidProfile.Constraints constraints, + DoubleSupplier pivotAngleSupplier, + double range, + double defaultSetpoint) { super(constraints, range, defaultSetpoint); this.pivotAngleSupplier = pivotAngleSupplier; - - motor.setInverted(false); - - motor.setControl(new StaticBrake()); - - //I have no idea how to set a current limit from the API + this.io = io; controller.setTolerance(0.05); } - public Wrist(TrapezoidProfile.Constraints constraints, DoubleSupplier pivotAngleSupplier, double range) { - this(constraints, pivotAngleSupplier, range, 0.0); + public Wrist(WristIO io, TrapezoidProfile.Constraints constraints, DoubleSupplier pivotAngleSupplier, + double range) { + this(io, constraints, pivotAngleSupplier, range, 0.0); + } + + public Wrist(WristIO io, TrapezoidProfile.Constraints constraints, DoubleSupplier pivotAngleSupplier) { + this(io, constraints, pivotAngleSupplier, 0.01); } - public Wrist(TrapezoidProfile.Constraints constraints, DoubleSupplier pivotAngleSupplier) { - this(constraints, pivotAngleSupplier, 0.01); + @Override + protected void updateInputs() { + io.updateInputs(inputs); } @Override public double getPosition() { - return motor.getPosition().getValueAsDouble() * 2 * Math.PI * WristConstants.gearRatio; + return inputs.positionRad; } @Override public double getVelocity() { - return motor.getVelocity().getValueAsDouble() * 2 * Math.PI * WristConstants.gearRatio; + return inputs.velocityRadS; } @Override public void setBrakeMode(boolean brake) { - motor.setControl(brake ? new StaticBrake() : new CoastOut()); + io.setBrakeMode(brake); } @Override public void jogSetpointPositive() { - //No clamping because the home is very unreliable + // No clamping because the home is very unreliable setpoint += Math.PI / 24; } @Override public void jogSetpointNegative() { - //No clamping because the home is very unreliable + // No clamping because the home is very unreliable setpoint -= Math.PI / 24; } @@ -100,11 +96,13 @@ public void home() { protected boolean runHomingLogic() { /* * The homing logic runs in two steps: - * Step 1: Run the motor until it experiences resistence, assumed to be from the arm - * Step 2: Wait a little bit for the compliant wheels to squish back, ensuring an accurate reading + * Step 1: Run the motor until it experiences resistence, assumed to be from the + * arm + * Step 2: Wait a little bit for the compliant wheels to squish back, ensuring + * an accurate reading */ if (!homingSecondStep) { - if (Math.abs(motor.getStatorCurrent().getValueAsDouble()) < 60) { + if (Math.abs(inputs.statorCurrent) < 60) { homeTimer.reset(); } else if (homeTimer.hasElapsed(0.2)) { homeTimer.reset(); @@ -122,22 +120,22 @@ protected boolean runHomingLogic() { } private void resetOffset() { - motor.setPosition( - WristConstants.homedPosition / (2 * Math.PI) / WristConstants.gearRatio); + io.setSensorPosition(WristConstants.homedPosition); } @Override protected double calculateControl(TrapezoidProfile.State setpoint) { double adjustedPosition = getPosition() + pivotAngleSupplier.getAsDouble(); - // TODO: Investigate the merits of having separate PID constants for when the wrist isn't moving + // TODO: Investigate the merits of having separate PID constants for when the + // wrist isn't moving return controller.calculate(adjustedPosition, setpoint.position) - + feedforward.calculate(setpoint.position, setpoint.velocity); + + feedforward.calculate(setpoint.position, setpoint.velocity); } @Override public void setOutput(double volts) { - motor.setControl(new VoltageOut(volts)); + io.setOuput(volts); } @Override diff --git a/src/main/java/frc/robot/subsystems/arm/wrist/WristIO.java b/src/main/java/frc/robot/subsystems/arm/wrist/WristIO.java new file mode 100644 index 0000000..5921e65 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/wrist/WristIO.java @@ -0,0 +1,22 @@ +package frc.robot.subsystems.arm.wrist; + +import org.littletonrobotics.junction.AutoLog; + +public interface WristIO { + + @AutoLog + public static class WristIOInputs { + public double positionRad = 0.0; + public double velocityRadS = 0.0; + public double appliedVolts = 0.0; + public double statorCurrent = 0.0; + } + + public default void updateInputs(WristIOInputsAutoLogged inputs) {} + + public default void setBrakeMode(boolean brake) {} + + public default void setOuput(double volts) {} + + public default void setSensorPosition(double position) {} +} diff --git a/src/main/java/frc/robot/subsystems/arm/wrist/WristIOFalcon.java b/src/main/java/frc/robot/subsystems/arm/wrist/WristIOFalcon.java new file mode 100644 index 0000000..436dbc3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/wrist/WristIOFalcon.java @@ -0,0 +1,47 @@ +package frc.robot.subsystems.arm.wrist; + +import com.ctre.phoenix6.controls.CoastOut; +import com.ctre.phoenix6.controls.StaticBrake; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; + +import frc.robot.Constants.CANDevices; +import frc.robot.Constants.WristConstants; + +public class WristIOFalcon implements WristIO { + + private TalonFX motor = new TalonFX(CANDevices.wristMotorID); + + public WristIOFalcon() { + motor.setInverted(false); + + motor.setControl(new StaticBrake()); + + //I have no idea how to set a current limit from the API + } + + @Override + public void updateInputs(WristIOInputsAutoLogged inputs) { + inputs.positionRad = motor.getPosition().getValueAsDouble() * 2 * Math.PI * WristConstants.gearRatio; + inputs.velocityRadS = motor.getVelocity().getValueAsDouble() * 2 * Math.PI * WristConstants.gearRatio; + inputs.appliedVolts = motor.getMotorVoltage().getValueAsDouble(); + inputs.statorCurrent = motor.getStatorCurrent().getValueAsDouble(); + } + + @Override + public void setBrakeMode(boolean brake) { + motor.setControl(brake ? new StaticBrake() : new CoastOut()); + } + + @Override + public void setOuput(double volts) { + motor.setControl(new VoltageOut(volts)); + } + + @Override + public void setSensorPosition(double position) { + motor.setPosition(position / (2 * Math.PI) / WristConstants.gearRatio); + + } + +}