diff --git a/src/main/java/frc/robot/config/RobotConfig.java b/src/main/java/frc/robot/config/RobotConfig.java index 9ae0d817..a44dfaff 100644 --- a/src/main/java/frc/robot/config/RobotConfig.java +++ b/src/main/java/frc/robot/config/RobotConfig.java @@ -79,8 +79,8 @@ public static class ArmConstants { public static double maxAngleInDegrees = 90.0; public static double minAngleInDegrees = 0.0; - public static double maxVelocityInDegreesPerSecond = 90; - public static double maxAccelerationInDegreesPerSecondSquared = 720; + public static double maxVelocityInDegreesPerSecond = 45; + public static double maxAccelerationInDegreesPerSecondSquared = 120; public static double intakeAngleInDegrees = 1; public static double ejectAngleInDegrees = 15; diff --git a/src/main/java/frc/robot/config/RobotConfigInferno.java b/src/main/java/frc/robot/config/RobotConfigInferno.java index 76edd291..6dcc2363 100644 --- a/src/main/java/frc/robot/config/RobotConfigInferno.java +++ b/src/main/java/frc/robot/config/RobotConfigInferno.java @@ -87,8 +87,8 @@ public RobotConfigInferno() { ArmConstants.ffKv = 6.18; ArmConstants.ffKa = 0.04; - ArmConstants.maxVelocityInDegreesPerSecond = 90; - ArmConstants.maxAccelerationInDegreesPerSecondSquared = 720; + ArmConstants.maxVelocityInDegreesPerSecond = 45; + ArmConstants.maxAccelerationInDegreesPerSecondSquared = 120; ArmConstants.pidMaxOutput = 6.0; ArmConstants.pidMinOutput = -5.0; diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index 8e20b4cd..be5532f0 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -2,26 +2,30 @@ import static edu.wpi.first.units.Units.Volts; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.config.RobotConfig.ArmConstants; import frc.robot.util.DevilBotState; import frc.robot.util.DevilBotState.State; import frc.robot.util.LoggedTunableNumber; +import frc.robot.util.TrapezoidProfileSubsystem2876; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -public class ArmSubsystem extends SubsystemBase implements Arm { +public class ArmSubsystem extends TrapezoidProfileSubsystem2876 implements Arm { private final ArmIO io; private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); - private ArmFeedforward feedforward; + private ArmFeedforward feedforward = + new ArmFeedforward( + ArmConstants.ffKs, ArmConstants.ffKg, ArmConstants.ffKv, ArmConstants.ffKa); private final SysIdRoutine sysId; private final double positionDegreeMax = ArmConstants.maxAngleInDegrees; private final double positionDegreeMin = ArmConstants.minAngleInDegrees; @@ -55,6 +59,10 @@ public class ArmSubsystem extends SubsystemBase implements Arm { private double kG, kV, kA, kS; public ArmSubsystem(ArmIO io) { + super( + new TrapezoidProfile.Constraints( + ArmConstants.maxVelocityInDegreesPerSecond, + ArmConstants.maxAccelerationInDegreesPerSecondSquared)); this.io = io; armKp.initDefault(ArmConstants.pidKp); @@ -85,6 +93,26 @@ public ArmSubsystem(ArmIO io) { new SysIdRoutine.Mechanism((voltage) -> runVoltage(voltage.in(Volts)), null, this)); io.setBrakeMode(true); + disable(); + } + + @Override + public void useState(TrapezoidProfile.State setpoint) { + double ff = feedforward.calculate(setpoint.position, 0); + + // Use feedforward + HW velocity PID (ignore SW PID) + io.setPosition(setpoint.position, ff); + + Logger.recordOutput("Arm/setAngle/setpointDegrees", setpoint.position); + Logger.recordOutput("Arm/setAngle/ffVolts", ff); + + // System.out.println("pos: " + setpoint.position); + // System.out.println("vel: " + setpoint.velocity); + } + + @Override + public TrapezoidProfile.State getMeasurement() { + return new TrapezoidProfile.State(getRelativeAngle(), getVelocity()); } @Override @@ -109,7 +137,10 @@ public double getTargetAngle() { // sets of the angle of the arm @Override - public void setAngle(double degrees, double velocityDegreesPerSecond) { + public void setAngle(double degrees) { + degrees = + MathUtil.clamp(degrees, ArmConstants.minAngleInDegrees, ArmConstants.maxAngleInDegrees); + Logger.recordOutput("Arm/setAngle/requestedAngleDegress", degrees); // Don't try to set position if absolute encoder is broken/missing. if (isAbsoluteEncoderConnected() == false) { @@ -146,13 +177,10 @@ public void setAngle(double degrees, double velocityDegreesPerSecond) { double deltaDegrees = this.targetDegrees - getAngle(); this.targetRelativeDegrees = getRelativeAngle() + deltaDegrees; - double ff = feedforward.calculate(this.targetDegrees, this.targetVelocityDegreesPerSecond); - Logger.recordOutput("Arm/setAngle/setpointDegrees", this.targetRelativeDegrees); - Logger.recordOutput("Arm/setAngle/ffVolts", ff); - // Set the position reference with feedforward voltage - io.setPosition(this.targetDegrees, ff); + setGoal(this.targetDegrees); + enable(); } @Override @@ -171,6 +199,7 @@ && getAngle() < ArmConstants.maxAngleInDegrees + 10) { // Sets the voltage to volts. the volts value is -12 to 12 public void runVoltage(double volts) { targetVoltage = voltageSafety(volts); + disable(); io.setVoltage(targetVoltage); } @@ -191,6 +220,7 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { @Override public void periodic() { + super.periodic(); if (armKp.hasChanged(hashCode()) || armKd.hasChanged(hashCode()) || armOutputMin.hasChanged(hashCode()) @@ -225,10 +255,6 @@ public void periodic() { io.resetRelativeEncoder(getAngle()); } - if (Math.abs(inputs.velocityInDegrees) < 0.1) { - io.resetRelativeEncoder(getAngle()); - } - if (isLimitHigh() && inputs.appliedVolts > 0) { // TODO: turn off voltage or stop pid io.setVoltage(0); diff --git a/src/main/java/frc/robot/util/TrapezoidProfileSubsystem2876.java b/src/main/java/frc/robot/util/TrapezoidProfileSubsystem2876.java new file mode 100644 index 00000000..38217534 --- /dev/null +++ b/src/main/java/frc/robot/util/TrapezoidProfileSubsystem2876.java @@ -0,0 +1,122 @@ +// 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.util; + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** + * A subsystem that generates and runs trapezoidal motion profiles automatically. The user specifies + * how to use the current state of the motion profile by overriding the `useState` method. + * + *

This class is provided by the NewCommands VendorDep + */ +public abstract class TrapezoidProfileSubsystem2876 extends SubsystemBase { + private final double m_period; + private final TrapezoidProfile m_profile; + + private TrapezoidProfile.State m_state; + private TrapezoidProfile.State m_goal; + + private boolean m_enabled = true; + + /** + * Creates a new TrapezoidProfileSubsystem2876. + * + * @param constraints The constraints (maximum velocity and acceleration) for the profiles. + * @param initialPosition The initial position of the controlled mechanism when the subsystem is + * constructed. + * @param period The period of the main robot loop, in seconds. + */ + public TrapezoidProfileSubsystem2876( + TrapezoidProfile.Constraints constraints, double initialPosition, double period) { + requireNonNullParam(constraints, "constraints", "TrapezoidProfileSubsystem2876"); + m_profile = new TrapezoidProfile(constraints); + m_state = new TrapezoidProfile.State(initialPosition, 0); + setGoal(initialPosition); + m_period = period; + } + + /** + * Creates a new TrapezoidProfileSubsystem2876. + * + * @param constraints The constraints (maximum velocity and acceleration) for the profiles. + * @param initialPosition The initial position of the controlled mechanism when the subsystem is + * constructed. + */ + public TrapezoidProfileSubsystem2876( + TrapezoidProfile.Constraints constraints, double initialPosition) { + this(constraints, initialPosition, 0.02); + } + + /** + * Creates a new TrapezoidProfileSubsystem2876. + * + * @param constraints The constraints (maximum velocity and acceleration) for the profiles. + */ + public TrapezoidProfileSubsystem2876(TrapezoidProfile.Constraints constraints) { + this(constraints, 0, 0.02); + } + + @Override + public void periodic() { + m_state = m_profile.calculate(m_period, m_state, m_goal); + if (m_enabled) { + useState(m_state); + } + } + + /** + * Sets the goal state for the subsystem. + * + * @param goal The goal state for the subsystem's motion profile. + */ + public final void setGoal(TrapezoidProfile.State goal) { + m_goal = goal; + } + + /** + * Sets the goal state for the subsystem. Goal velocity assumed to be zero. + * + * @param goal The goal position for the subsystem's motion profile. + */ + public final void setGoal(double goal) { + setGoal(new TrapezoidProfile.State(goal, 0)); + } + + /** Enable the TrapezoidProfileSubsystem2876's output. */ + public void enable() { + if (false == m_enabled) { + // DevilBotz 2876: Update the *current* state before starting the motion + // profile to ensure smooth motion to the target in case the + // position/velocity changed *outside* of this subsystem. + // + // E.g. robot was disabled and the arm position changed due to gravity + m_state = getMeasurement(); + } + m_enabled = true; + } + + /** Disable the TrapezoidProfileSubsystem2876's output. */ + public void disable() { + m_enabled = false; + } + + /** + * Users should override this to consume the current state of the motion profile. + * + * @param state The current state of the motion profile. + */ + protected abstract void useState(TrapezoidProfile.State state); + + /** + * Returns the measurement of the process variable used by the TrapezoidProfileSubsystem2876 + * + * @return the measurement of the process variable + */ + protected abstract TrapezoidProfile.State getMeasurement(); +}