-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
13 changed files
with
363 additions
and
17 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,7 @@ | ||
| package frc.robot; | ||
|
|
||
| public class RobotMap { | ||
| public static class IntakeIDs { | ||
| public static final int ROLLERS = 10; | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,57 @@ | ||
| // 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.subsystems.intake; | ||
|
|
||
| import edu.wpi.first.math.controller.SimpleMotorFeedforward; | ||
| import edu.wpi.first.wpilibj2.command.SubsystemBase; | ||
| import frc.robot.Constants; | ||
| import org.littletonrobotics.junction.Logger; | ||
|
|
||
| public class Intake extends SubsystemBase { | ||
| /** Creates a new Intake. */ | ||
| private final IntakeRollerIO roller; | ||
|
|
||
| private final IntakeRollerIOInputsAutoLogged rInputs = new IntakeRollerIOInputsAutoLogged(); | ||
|
|
||
| private final SimpleMotorFeedforward ffModel; | ||
|
|
||
| public Intake(IntakeRollerIO roller) { | ||
| switch (Constants.currentMode) { | ||
| case REAL: | ||
| ffModel = new SimpleMotorFeedforward(0, 0); | ||
| break; | ||
| case REPLAY: | ||
| ffModel = new SimpleMotorFeedforward(0, 0); | ||
| break; | ||
| case SIM: | ||
| ffModel = new SimpleMotorFeedforward(0, 0.8); | ||
| break; | ||
| default: | ||
| ffModel = new SimpleMotorFeedforward(0, 0); | ||
| break; | ||
| } | ||
|
|
||
| this.roller = roller; | ||
|
|
||
| // make this a constant | ||
| roller.configurePID(0.5, 0, 0); | ||
| } | ||
|
|
||
| public void runRollers(double velocity) { | ||
| roller.setVelocity(velocity, ffModel.calculate(velocity)); | ||
| } | ||
|
|
||
| public void stopRollers() { | ||
| roller.stop(); | ||
| } | ||
|
|
||
| @Override | ||
| public void periodic() { | ||
| // This method will be called once per scheduler run | ||
| roller.updateInputs(rInputs); | ||
|
|
||
| Logger.processInputs("Intake", rInputs); | ||
| } | ||
| } |
26 changes: 26 additions & 0 deletions
26
src/main/java/frc/robot/subsystems/intake/IntakeRollerIO.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,26 @@ | ||
| // 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.subsystems.intake; | ||
|
|
||
| import org.littletonrobotics.junction.AutoLog; | ||
|
|
||
| /** Add your docs here. */ | ||
| public interface IntakeRollerIO { | ||
| @AutoLog | ||
| public static class IntakeRollerIOInputs { | ||
| public double rollerVelocity = 0.0; | ||
|
|
||
| public double appliedVolts = 0.0; | ||
| public double currentAmps = 0.0; | ||
| } | ||
|
|
||
| public default void updateInputs(IntakeRollerIOInputs inputs) {} | ||
|
|
||
| public default void setVelocity(double velocity, double ffVolts) {} | ||
|
|
||
| public default void stop() {} | ||
|
|
||
| public default void configurePID(double kP, double kI, double kD) {} | ||
| } |
49 changes: 49 additions & 0 deletions
49
src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSim.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,49 @@ | ||
| package frc.robot.subsystems.intake; | ||
|
|
||
| import edu.wpi.first.math.MathUtil; | ||
| import edu.wpi.first.math.controller.PIDController; | ||
| import edu.wpi.first.math.system.plant.DCMotor; | ||
| import edu.wpi.first.wpilibj.simulation.DCMotorSim; | ||
|
|
||
| public class IntakeRollerIOSim implements IntakeRollerIO { | ||
| private final DCMotor simGearbox = DCMotor.getNeoVortex(1); | ||
| private DCMotorSim sim = new DCMotorSim(simGearbox, 1, 0.7); | ||
| private PIDController pid = new PIDController(0.2, 0, 0); | ||
|
|
||
| private boolean closedLoop = false; | ||
| private double ffVolts = 0.0; | ||
| private double appliedVolts = 0.0; | ||
|
|
||
| @Override | ||
| public void updateInputs(IntakeRollerIOInputs inputs) { | ||
| if (closedLoop) { | ||
| appliedVolts = | ||
| MathUtil.clamp(pid.calculate(sim.getAngularVelocityRPM()) + ffVolts, -12.0, 12.0); | ||
| sim.setInputVoltage(appliedVolts); | ||
| } | ||
|
|
||
| sim.update(0.02); | ||
| inputs.rollerVelocity = sim.getAngularVelocityRPM(); | ||
| inputs.rollerVelocity = sim.getAngularVelocityRPM(); | ||
| inputs.appliedVolts = appliedVolts; | ||
| inputs.currentAmps = sim.getCurrentDrawAmps(); | ||
| } | ||
|
|
||
| @Override | ||
| public void setVelocity(double velocity, double ffVolts) { | ||
| closedLoop = true; | ||
| pid.setSetpoint(velocity); | ||
| this.ffVolts = ffVolts; | ||
| } | ||
|
|
||
| @Override | ||
| public void stop() { | ||
| closedLoop = false; | ||
| setVelocity(0, 0); | ||
| } | ||
|
|
||
| @Override | ||
| public void configurePID(double kP, double kI, double kD) { | ||
| pid.setPID(kP, kI, kD); | ||
| } | ||
| } |
51 changes: 51 additions & 0 deletions
51
src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSparkFlex.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,51 @@ | ||
| package frc.robot.subsystems.intake; | ||
|
|
||
| import com.revrobotics.CANSparkBase.ControlType; | ||
| import com.revrobotics.CANSparkBase.IdleMode; | ||
| import com.revrobotics.CANSparkFlex; | ||
| import com.revrobotics.CANSparkLowLevel.MotorType; | ||
| import com.revrobotics.SparkPIDController; | ||
| import com.revrobotics.SparkPIDController.ArbFFUnits; | ||
| import frc.robot.Constants; | ||
|
|
||
| public class IntakeRollerIOSparkFlex implements IntakeRollerIO { | ||
| private final CANSparkFlex rollers; | ||
| private final SparkPIDController pid; | ||
|
|
||
| public IntakeRollerIOSparkFlex(int id) { | ||
| rollers = new CANSparkFlex(id, MotorType.kBrushless); | ||
| rollers.restoreFactoryDefaults(); | ||
| rollers.setIdleMode(IdleMode.kCoast); | ||
|
|
||
| rollers.setSmartCurrentLimit(Constants.IntakeConstants.ROLLER_CURRENT_LIMIT); | ||
| rollers.setCANTimeout(250); | ||
| rollers.burnFlash(); | ||
|
|
||
| pid = rollers.getPIDController(); | ||
| } | ||
|
|
||
| @Override | ||
| public void updateInputs(IntakeRollerIOInputs inputs) { | ||
| inputs.rollerVelocity = rollers.getEncoder().getVelocity(); | ||
|
|
||
| inputs.appliedVolts = rollers.getBusVoltage(); | ||
| inputs.currentAmps = rollers.getOutputCurrent(); | ||
| } | ||
|
|
||
| @Override | ||
| public void setVelocity(double velocity, double ffVolts) { | ||
| pid.setReference(velocity, ControlType.kVelocity, 0, ffVolts, ArbFFUnits.kVoltage); | ||
| } | ||
|
|
||
| @Override | ||
| public void stop() { | ||
| rollers.stopMotor(); | ||
| } | ||
|
|
||
| @Override | ||
| public void configurePID(double kP, double kI, double kD) { | ||
| pid.setP(kP, 0); | ||
| pid.setI(kI, 0); | ||
| pid.setD(kD, 0); | ||
| } | ||
| } |
Oops, something went wrong.