Skip to content
This repository has been archived by the owner on May 19, 2024. It is now read-only.

Commit

Permalink
feat(intake): Add motion profiling for the pivot.
Browse files Browse the repository at this point in the history
  • Loading branch information
haydenheroux committed Feb 21, 2024
1 parent 6926066 commit 738a764
Show file tree
Hide file tree
Showing 4 changed files with 85 additions and 14 deletions.
6 changes: 5 additions & 1 deletion src/main/java/frc/robot/intake/Intake.java
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -42,6 +44,8 @@ public class Intake extends Subsystem {
private Intake() {
pivotMotor = IntakeFactory.createPivotMotor();
rollerMotor = IntakeFactory.createRollerMotor();

pivotMotor.setPosition(PivotMotorConstants.MAXIMUM_ANGLE.getRotations());
}

/**
Expand Down Expand Up @@ -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");

Expand Down
44 changes: 42 additions & 2 deletions src/main/java/frc/robot/intake/IntakeConstants.java
Original file line number Diff line number Diff line change
@@ -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;
Expand All @@ -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

Expand Down
47 changes: 37 additions & 10 deletions src/main/java/frc/robot/intake/PivotMotorIOTalonSRX.java
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/intake/RollerMotorIOSparkMax.java
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down

0 comments on commit 738a764

Please sign in to comment.