diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 531294e..02b2d2a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -25,10 +25,14 @@ public final class Constants { public static final double armP = 21.0; public static final double armI = 0; public static final double armD = 0.015; - public static final double armS = 0; - public static final double armG = 0; - public static final double armV = 0; - public static final double armA = 0; + public static final double armS = 0; // stiction gain + public static final double armG = 0; // gravity gain + public static final double armV = 0; // velocity gain + public static final double armA = 0; // acceleration gain + public static final int armMaxVelRadPerSec = 0; + public static final int armMaxAccelRadPerSec = 0; + public static final int armDistPerPulse = 0; + public static final int armOffsetRad = 0; public static final int armLeadPort = 21; public static final int armFollowerPort = 26; public static final int armEncoderPort = 3; diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 99e4304..2fba4fe 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -8,20 +8,38 @@ import com.revrobotics.CANSparkMax; 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.util.datalog.DataLog; import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.util.datalog.StringLogEntry; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DutyCycleEncoder; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; -import java.util.function.DoubleSupplier; -/** Our Arm Subsystem */ public class Arm extends SubsystemBase { + + private TrapezoidProfile m_profile; + private TrapezoidProfile.State m_goal = new TrapezoidProfile.State(); + private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State(); + private PIDController m_pid; + private final CANSparkMax m_motor = new CANSparkMax(Constants.armLeadPort, MotorType.kBrushless); + private DataLog m_log = DataLogManager.getLog(); + private DoubleLogEntry log_trapezoid_goal = new DoubleLogEntry(m_log, "/arm/trapezoid/goal"); + private DoubleLogEntry log_pid_output = new DoubleLogEntry(m_log, "/arm/pid/output"); + private DoubleLogEntry log_pid_setpoint = new DoubleLogEntry(m_log, "/arm/pid/setpoint"); + private DoubleLogEntry log_ff_position_setpoint = + new DoubleLogEntry(m_log, "/arm/ff/position_setpoint"); + private DoubleLogEntry log_ff_velocity_setpoint = + new DoubleLogEntry(m_log, "/arm/ff/velocity_setpoint"); + private DoubleLogEntry log_ff_output = new DoubleLogEntry(m_log, "/arm/ff/output"); + private StringLogEntry log_setpoint = new StringLogEntry(m_log, "/arm/setpoint"); + private final DutyCycleEncoder m_encoder = new DutyCycleEncoder(Constants.armEncoderPort); + private ArmFeedforward m_feedforward = + new ArmFeedforward( + Constants.armS, Constants.armG, + Constants.armV, Constants.armA); + public enum Setpoint { kAmp, kIntake, @@ -29,104 +47,112 @@ public enum Setpoint { kStowed } - private final CANSparkMax m_primaryMotor = - new CANSparkMax(Constants.armLeadPort, MotorType.kBrushless); - private final CANSparkMax m_followerMotor = - new CANSparkMax(Constants.armFollowerPort, MotorType.kBrushless); - private final DutyCycleEncoder m_encoder = new DutyCycleEncoder(Constants.armEncoderPort); - - private final PIDController m_pid = - new PIDController(Constants.armP, Constants.armI, Constants.armD); - private final ArmFeedforward m_feedforward = - new ArmFeedforward(Constants.armS, Constants.armG, Constants.armV, Constants.armA); - - private final DataLog m_log = DataLogManager.getLog(); - private final DoubleLogEntry log_pid_output = new DoubleLogEntry(m_log, "/arm/pid/output"); - private final DoubleLogEntry log_pid_setpoint = new DoubleLogEntry(m_log, "/arm/pid/setpoint"); - private final DoubleLogEntry log_ff_position_setpoint = - new DoubleLogEntry(m_log, "/arm/ff/position_setpoint"); - private final DoubleLogEntry log_ff_velocity_setpoint = - new DoubleLogEntry(m_log, "/arm/ff/velocity_setpoint"); - private final DoubleLogEntry log_ff_output = new DoubleLogEntry(m_log, "/arm/ff/output"); - private final StringLogEntry log_setpoint = new StringLogEntry(m_log, "/arm/setpoint"); + private void useOutput(double output, Double position, Double velocity) { + // Calculate the feedforward from the sepoint + double feedforward = m_feedforward.calculate(position, velocity); - public final Trigger m_atSetpoint = new Trigger(m_pid::atSetpoint); + log_trapezoid_goal.append(m_goal.position); + log_trapezoid_goal.append(m_goal.velocity); + log_pid_output.append(output); + log_pid_setpoint.append(m_pid.getSetpoint()); + log_ff_output.append(feedforward); + log_ff_position_setpoint.append(position); + log_ff_velocity_setpoint.append(velocity); - /** Creates a new {@link Arm} {@link edu.wpi.first.wpilibj2.command.Subsystem}. */ - public Arm() { - m_followerMotor.follow(m_primaryMotor); - } - - /** Achieves and maintains speed for the primary motor. */ - private Command achievePosition(double position) { - return Commands.run( - () -> { - var pid_output = m_pid.calculate(m_encoder.getAbsolutePosition() * 2 * Math.PI, position); - log_pid_output.append(pid_output); - log_pid_setpoint.append(m_pid.getSetpoint()); - var ff_output = m_feedforward.calculate(position, (5676 / 250)); - log_ff_output.append(ff_output); - log_ff_position_setpoint.append(position); - log_ff_velocity_setpoint.append((5676 / 250)); - m_primaryMotor.setVoltage(-1 * ff_output + pid_output); - }); - } - - public Command stop() { - return runOnce(() -> m_primaryMotor.set(0)); + // Add the feedforward to the PID output to get the motor output + m_motor.setVoltage(output + feedforward); } /** - * Moves the arm to the specified position then stops. + * Sets the goal for the ProfiledPIDController. * - * @param position The desired position. - * @return a {@link Command} to get to the desired position. + * @param goal The desired goal state. */ - private Command moveToPosition(double position) { - return achievePosition(position) - .until( - () -> - m_pid.atSetpoint() - && ((m_encoder.getAbsolutePosition() * 2 * Math.PI) - position) < 0.001); + private void setGoal(TrapezoidProfile.State goal) { + m_goal = goal; } /** - * Holds the arm at the current position setpoint. + * Sets the goal for the ProfiledPIDController. * - * @return A {@link Command} to hold the position at the setpoint. + * @param goal The desired goal position. */ - public Command maintain() { - return achievePosition(m_pid.getSetpoint()); + private void setGoal(double goal) { + m_goal = new TrapezoidProfile.State(goal, 0); } /** - * Allows for manual control of the arm using a joystick. + * Gets the goal for the ProfiledPIDController. * - * @param speed The speed from the joystick input. - * @return A {@link Command} to control the arm manually. + * @return The goal. */ - public Command manualControl(DoubleSupplier speed) { - return Commands.run(() -> m_primaryMotor.set(speed.getAsDouble()), this); + private TrapezoidProfile.State getGoal() { + return m_goal; } - /* - * Makes the arm go to a setpoint from the {@link Setpoint} enum - * - * @param setpoint The setpoint to go to, a {@link Setpoint} + private double getPeriod() { + return m_pid.getPeriod(); + } + + /** + * Returns the next output of the PID controller. * - * @return A {@link Command} to go to the setpoint + * @param measurement The current measurement of the process variable. + * @param goal The new goal of the controller. + * @return The controller's next output. */ - public Command goToSetpoint(Setpoint setpoint) { + private double calculate(double measurement) { + + m_setpoint = m_profile.calculate(getPeriod(), m_setpoint, m_goal); + return m_pid.calculate(measurement, m_setpoint.position); + } + + private double calculate(double measurement, TrapezoidProfile.State goal) { + setGoal(goal); + return calculate(measurement); + } + + private double calculate(double measurement, double goal) { + setGoal(goal); + return calculate(measurement); + } + + public Arm() { + + m_pid = new PIDController(Constants.armP, Constants.armI, Constants.armD); + new TrapezoidProfile( + new TrapezoidProfile.Constraints( + Constants.armMaxVelRadPerSec, Constants.armMaxAccelRadPerSec)); + + // Start arm at rest in neutral position + setGoal(Constants.armOffsetRad); + } + + public double getMeasurement() { + return m_encoder.getDistance() + Constants.armOffsetRad; + } + + public void goToSetpoint(Setpoint setpoint) { double position = 0; log_setpoint.append(setpoint.name()); switch (setpoint) { - case kAmp -> position = 5.34; - case kIntake -> position = 3.7; - case kSpeaker -> position = 3.7; - case kStowed -> position = 3.7; - } + case kAmp: + position = 5.34; + break; + + case kIntake: + position = 3.7; + break; - return moveToPosition(position); + case kSpeaker: + position = 3.7; + break; + + case kStowed: + position = 3.7; + break; + } } } +;