From 738a764d6601ec85a6c4f98f79720c88bbfb2223 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Tue, 20 Feb 2024 19:08:42 -0500 Subject: [PATCH] feat(intake): Add motion profiling for the pivot. --- src/main/java/frc/robot/intake/Intake.java | 6 ++- .../frc/robot/intake/IntakeConstants.java | 44 ++++++++++++++++- .../robot/intake/PivotMotorIOTalonSRX.java | 47 +++++++++++++++---- .../robot/intake/RollerMotorIOSparkMax.java | 2 +- 4 files changed, 85 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index a8955bc..6a42cb7 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -2,12 +2,14 @@ import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.MedianFilter; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.lib.Subsystem; import frc.lib.Telemetry; +import frc.robot.intake.IntakeConstants.PivotMotorConstants; import frc.robot.intake.IntakeConstants.RollerMotorConstants; import frc.robot.intake.PivotMotorIO.PivotMotorIOValues; import frc.robot.intake.RollerMotorIO.RollerMotorIOValues; @@ -42,6 +44,8 @@ public class Intake extends Subsystem { private Intake() { pivotMotor = IntakeFactory.createPivotMotor(); rollerMotor = IntakeFactory.createRollerMotor(); + + pivotMotor.setPosition(PivotMotorConstants.MAXIMUM_ANGLE.getRotations()); } /** @@ -69,7 +73,7 @@ public void periodic() { public void addToShuffleboard(ShuffleboardTab tab) { ShuffleboardLayout pivot = Telemetry.addColumn(tab, "Pivot"); - pivot.addDouble("Position (rot)", () -> pivotMotorValues.positionRotations); + pivot.addDouble("Position (deg)", () -> Units.rotationsToDegrees(pivotMotorValues.positionRotations)); ShuffleboardLayout roller = Telemetry.addColumn(tab, "Roller"); diff --git a/src/main/java/frc/robot/intake/IntakeConstants.java b/src/main/java/frc/robot/intake/IntakeConstants.java index 51a2fc1..6431648 100644 --- a/src/main/java/frc/robot/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/intake/IntakeConstants.java @@ -1,18 +1,55 @@ package frc.robot.intake; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import frc.lib.CAN; import frc.lib.MotorCurrentLimits; +import frc.lib.MotionProfileCalculator; /** Constants for the intake subsystem. */ public class IntakeConstants { - public static final double INTAKE_ROLLER_RADIUS = 0.5 * Units.inchesToMeters(1.375); + /** Constants for the pivot motor. */ + public static class PivotMotorConstants { + /** Pivot motor's CAN identifier. */ + public static final CAN CAN = new CAN(38); + + /** If true, invert the pivot motor. */ + public static final boolean IS_INVERTED = false; + + /** Gearing between the pivot sensor and the pivot. */ + public static final double SENSOR_GEARING = 16.0 / 18.0; // TODO flip? + + /** Gearing between the motor and the pivot. */ + public static final double MOTOR_GEARING = 49 * SENSOR_GEARING; + + /** Pivot motor's maximum voltage. */ + public static final double MAXIMUM_VOLTAGE = 4; + + /** Pivot motor's minimum angle. */ + public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(0); + + /** Pivot motor's maximum angle. */ + public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(90); + + /** Maximum speed of the pivot in rotations per second. */ + public static final double MAXIMUM_SPEED = 0.5; + + /** Maximum acceleration of the pivot in rotations per second per second. */ + public static final double MAXIMUM_ACCELERATION = MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.1); + + /** Maximum speed and acceleration of the pivot. */ + public static final TrapezoidProfile.Constraints CONSTRAINTS = new TrapezoidProfile.Constraints(MAXIMUM_SPEED, MAXIMUM_ACCELERATION); + + /** Motion profile of the pivot using constraints. */ + public static final TrapezoidProfile MOTION_PROFILE = new TrapezoidProfile(CONSTRAINTS); + } /** Constants for the roller motor. */ public static class RollerMotorConstants { /** Roller motor's CAN identifier. */ - public static final CAN ID = new CAN(4); + public static final CAN CAN = new CAN(4); /** If true, invert the roller motor. */ public static final boolean IS_INVERTED = false; @@ -33,6 +70,9 @@ public static class RollerMotorConstants { /** Current limits for the roller motor. */ public static final MotorCurrentLimits CURRENT_LIMITS = new MotorCurrentLimits(40); + /** Radius of the roller in meters. */ + public static final double INTAKE_ROLLER_RADIUS = 0.5 * Units.inchesToMeters(1.375); + /** Size of the current spike when intaking a note in amps. */ public static final double NOTE_CURRENT = 18.0; // TODO diff --git a/src/main/java/frc/robot/intake/PivotMotorIOTalonSRX.java b/src/main/java/frc/robot/intake/PivotMotorIOTalonSRX.java index 7c396e6..20cd9ba 100644 --- a/src/main/java/frc/robot/intake/PivotMotorIOTalonSRX.java +++ b/src/main/java/frc/robot/intake/PivotMotorIOTalonSRX.java @@ -1,36 +1,59 @@ package frc.robot.intake; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import frc.robot.RobotConstants; +import edu.wpi.first.math.MathUtil; +import frc.robot.intake.IntakeConstants.PivotMotorConstants; +/** Pivot motor using a Talon SRX. */ public class PivotMotorIOTalonSRX implements PivotMotorIO { + /** Hardware Talon SRX. */ private final TalonSRX talonSRX; public PivotMotorIOTalonSRX() { - int canId = 38; - - talonSRX = new TalonSRX(canId); + talonSRX = new TalonSRX(PivotMotorConstants.CAN.id()); } @Override public void configure() { - boolean inverted = false; + talonSRX.setInverted(PivotMotorConstants.IS_INVERTED); - talonSRX.setInverted(inverted); + talonSRX.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); } @Override public void update(PivotMotorIOValues values) { - // TODO - values.positionRotations = 0.0; + values.positionRotations = getPivotPosition(); + } + + /** + * Gets the position of the pivot in rotations. + * + * @return the position of the pivot in rotations. + */ + private double getPivotPosition() { + double rotations = talonSRX.getSelectedSensorPosition() / 2048; + + return rotations / PivotMotorConstants.SENSOR_GEARING; + } + + /** + * Setes the position of the pivot in rotations. + * + * @param positionRotations the position of the pivot in rotations. + */ + private void setPivotPosition(double positionRotations) { + double units = positionRotations * PivotMotorConstants.SENSOR_GEARING * 2048; + + talonSRX.setSelectedSensorPosition(units); } @Override public void setPosition(double positionRotations) { - // TODO + setPivotPosition(positionRotations); } @Override @@ -40,7 +63,11 @@ public void setSetpoint(double positionRotations) { @Override public void setVoltage(double volts) { - talonSRX.set(TalonSRXControlMode.PercentOutput, volts / RobotConstants.BATTERY_VOLTAGE); + volts = MathUtil.clamp(volts, -PivotMotorConstants.MAXIMUM_VOLTAGE, PivotMotorConstants.MAXIMUM_VOLTAGE); + + double percent = volts / talonSRX.getBusVoltage(); + + talonSRX.set(TalonSRXControlMode.PercentOutput, percent); } @Override diff --git a/src/main/java/frc/robot/intake/RollerMotorIOSparkMax.java b/src/main/java/frc/robot/intake/RollerMotorIOSparkMax.java index 0e9f135..ce5b28b 100644 --- a/src/main/java/frc/robot/intake/RollerMotorIOSparkMax.java +++ b/src/main/java/frc/robot/intake/RollerMotorIOSparkMax.java @@ -12,7 +12,7 @@ public class RollerMotorIOSparkMax implements RollerMotorIO { private final CANSparkMax sparkMax; public RollerMotorIOSparkMax() { - sparkMax = new CANSparkMax(RollerMotorConstants.ID.id(), MotorType.kBrushless); + sparkMax = new CANSparkMax(RollerMotorConstants.CAN.id(), MotorType.kBrushless); } @Override