-
Notifications
You must be signed in to change notification settings - Fork 0
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Intake and indexer #19
Changes from 9 commits
a491418
61e7def
33b1517
3f0b2e5
671b55a
32361a9
b906548
bdc5270
94dcca9
952ed9b
c554380
08023a3
3195a97
5181091
ee8c739
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,30 @@ | ||
package org.team1540.robot2024.commands.indexer; | ||
|
||
import edu.wpi.first.wpilibj2.command.Command; | ||
import org.team1540.robot2024.subsystems.indexer.Indexer; | ||
|
||
public class IntakeCommand extends Command { | ||
|
||
private final Indexer indexer; | ||
|
||
public IntakeCommand(Indexer indexer) { | ||
this.indexer = indexer; | ||
addRequirements(indexer); | ||
} | ||
|
||
@Override | ||
public void initialize() { | ||
indexer.setIntakePercent(0.4); | ||
} | ||
|
||
|
||
@Override | ||
public boolean isFinished() { | ||
return indexer.noteInIndexer(); | ||
} | ||
|
||
@Override | ||
public void end(boolean interrupted) { | ||
indexer.setIntakePercent(0); | ||
} | ||
} |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,60 @@ | ||
package org.team1540.robot2024.subsystems.indexer; | ||
|
||
import edu.wpi.first.math.controller.SimpleMotorFeedforward; | ||
import edu.wpi.first.wpilibj2.command.Command; | ||
import edu.wpi.first.wpilibj2.command.Commands; | ||
import edu.wpi.first.wpilibj2.command.SubsystemBase; | ||
import org.littletonrobotics.junction.Logger; | ||
import org.team1540.robot2024.util.LoggedTunableNumber; | ||
import static org.team1540.robot2024.Constants.Indexer.*; | ||
|
||
|
||
public class Indexer extends SubsystemBase { | ||
private final IndexerIO indexerIO; | ||
private final IndexerIOInputsAutoLogged indexerInputs = new IndexerIOInputsAutoLogged(); | ||
private final boolean TUNING = true; | ||
private final LoggedTunableNumber kP = new LoggedTunableNumber("Indexer/kP", FEEDER_KP); | ||
private final LoggedTunableNumber kI = new LoggedTunableNumber("Indexer/kI", FEEDER_KI); | ||
private final LoggedTunableNumber kD = new LoggedTunableNumber("Indexer/kD", FEEDER_KD); | ||
|
||
|
||
public Indexer(IndexerIO indexerIO) { | ||
this.indexerIO = indexerIO; | ||
} | ||
|
||
public void periodic() { | ||
indexerIO.updateInputs(indexerInputs); | ||
Logger.processInputs("Indexer", indexerInputs); | ||
if (TUNING) { | ||
if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode()) || kD.hasChanged(hashCode())) { | ||
indexerIO.configurePID(kP.get(), kI.get(), kD.get()); | ||
} | ||
} | ||
} | ||
|
||
public void setIntakePercent(double percent) { | ||
indexerIO.setIntakeVoltage(percent * 12.0); | ||
} | ||
|
||
public boolean noteInIndexer() { | ||
AdleyJ marked this conversation as resolved.
Show resolved
Hide resolved
|
||
return indexerInputs.noteInIntake; | ||
} | ||
|
||
public Command feedToAmp() { | ||
return Commands.sequence( | ||
Commands.runOnce(() -> indexerIO.setFeederVelocity(-600), this), | ||
Commands.waitSeconds(5), | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. This could wait for X time after the note stops being seen ( There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. For sequences though I might make them their own classes |
||
Commands.runOnce(() -> indexerIO.setFeederVelocity(0), this) | ||
); | ||
} | ||
|
||
public Command feedToShooter() { | ||
return Commands.sequence( | ||
Commands.runOnce(() -> indexerIO.setFeederVelocity(1200), this), | ||
Commands.waitSeconds(1), | ||
Commands.runOnce(() -> indexerIO.setFeederVelocity(0), this) | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. I might make this not just end after a second. This will only be used in other sequences anyway so maybe just having it return the command to set feeder velocity to 1200 would be good |
||
); | ||
} | ||
|
||
|
||
} |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,33 @@ | ||
package org.team1540.robot2024.subsystems.indexer; | ||
|
||
import org.littletonrobotics.junction.AutoLog; | ||
|
||
public interface IndexerIO { | ||
|
||
@AutoLog | ||
class IndexerIOInputs { | ||
public double intakeVoltage; | ||
public double intakeCurrentAmps; | ||
public double intakeVelocityRPM; | ||
public double feederVoltage; | ||
public double feederCurrentAmps; | ||
public double feederVelocityRPM; | ||
public boolean noteInIntake = false; | ||
public double setpointRPM; | ||
public double feederPositionError; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. The setpoint logging should happen in the subsystem, not the IO inputs. Also, why are you logging the position error? Isn't the feeder only velocity controlled? |
||
} | ||
|
||
/** | ||
* Updates the set of loggable inputs. | ||
*/ | ||
default void updateInputs(IndexerIOInputs inputs) {} | ||
|
||
default void setIntakeVoltage(double volts) {} | ||
|
||
default void setFeederVoltage(double volts) {} | ||
|
||
default void setFeederVelocity(double velocity) {} | ||
|
||
default void configurePID(double p, double i, double d) {} | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. This should probably be called |
||
|
||
} |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,66 @@ | ||
package org.team1540.robot2024.subsystems.indexer; | ||
|
||
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; | ||
import edu.wpi.first.wpilibj.simulation.SimDeviceSim; | ||
import org.team1540.robot2024.Constants; | ||
|
||
import static org.team1540.robot2024.Constants.Indexer.*; | ||
|
||
public class IndexerIOSim implements IndexerIO { | ||
|
||
|
||
private final DCMotorSim intakeSim = new DCMotorSim(DCMotor.getNEO(1), INTAKE_GEAR_RATIO,0.025); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. what is 0.025 doing here? (it should probably be in constants file) |
||
private final DCMotorSim feederSim = new DCMotorSim(DCMotor.getNEO(1), FEEDER_GEAR_RATIO,0.025); | ||
// private final SimDeviceSim beamBreakSim = new SimDeviceSim("Indexer Beam Break"); | ||
private final PIDController feederSimPID = new PIDController(FEEDER_KP, FEEDER_KI,FEEDER_KD); | ||
private boolean isClosedLoop = true; | ||
private double intakeVoltage = 0.0; | ||
private double feederVoltage = 0.0; | ||
private double feederVelocityRPS = 0.0; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. This should probably called |
||
|
||
@Override | ||
public void updateInputs(IndexerIOInputs inputs) { | ||
if (isClosedLoop) { | ||
feederVoltage = MathUtil.clamp(feederSimPID.calculate(feederSim.getAngularVelocityRPM() / 60, feederVelocityRPS), -12.0, 12.0); | ||
} | ||
intakeSim.setInputVoltage(intakeVoltage); | ||
feederSim.setInputVoltage(feederVoltage); | ||
intakeSim.update(Constants.LOOP_PERIOD_SECS); | ||
feederSim.update(Constants.LOOP_PERIOD_SECS); | ||
inputs.intakeCurrentAmps = intakeSim.getCurrentDrawAmps(); | ||
inputs.intakeVoltage = intakeVoltage; | ||
inputs.intakeVelocityRPM = intakeSim.getAngularVelocityRPM(); | ||
inputs.feederCurrentAmps = feederSim.getCurrentDrawAmps(); | ||
inputs.feederVoltage = feederVoltage; | ||
inputs.feederVelocityRPM = feederSim.getAngularVelocityRPM(); | ||
// inputs.noteInIntake = beamBreakSim.getBoolean("Indexer Beam Break").get(); | ||
inputs.setpointRPM = feederSimPID.getSetpoint() * 60; | ||
inputs.feederPositionError = feederSimPID.getPositionError(); | ||
} | ||
|
||
@Override | ||
public void setIntakeVoltage(double volts) { | ||
intakeVoltage = MathUtil.clamp(volts, -12.0, 12.0); | ||
} | ||
|
||
@Override | ||
public void configurePID(double p, double i, double d) { | ||
feederSimPID.setPID(p, i, d); | ||
} | ||
|
||
public void setFeederVoltage(double volts) { | ||
isClosedLoop = false; | ||
feederVoltage = MathUtil.clamp(volts, -12.0, 12.0); | ||
} | ||
|
||
|
||
public void setFeederVelocity(double velocity) { | ||
isClosedLoop = true; | ||
feederSimPID.reset(); | ||
feederVelocityRPS = velocity / 60; | ||
} | ||
|
||
} |
rutmanz marked this conversation as resolved.
Show resolved
Hide resolved
|
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,68 @@ | ||
package org.team1540.robot2024.subsystems.indexer; | ||
|
||
import com.revrobotics.*; | ||
import edu.wpi.first.math.controller.SimpleMotorFeedforward; | ||
import edu.wpi.first.wpilibj.DigitalInput; | ||
|
||
import static org.team1540.robot2024.Constants.Indexer.*; | ||
|
||
|
||
public class IndexerIOSparkMax implements IndexerIO { | ||
private final CANSparkMax intakeMotor = new CANSparkMax(INTAKE_ID, CANSparkLowLevel.MotorType.kBrushless); | ||
private final CANSparkMax feederMotor = new CANSparkMax(FEEDER_ID, CANSparkLowLevel.MotorType.kBrushless); | ||
private final DigitalInput indexerBeamBreak = new DigitalInput(0); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. constants file for digitalinput id |
||
private final SparkPIDController feederPID; | ||
private final SimpleMotorFeedforward feederFF = new SimpleMotorFeedforward(FEEDER_KS, FEEDER_KV); | ||
private double setpointRPM; | ||
|
||
|
||
public IndexerIOSparkMax() { | ||
intakeMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); | ||
intakeMotor.enableVoltageCompensation(12.0); | ||
intakeMotor.setSmartCurrentLimit(30); | ||
|
||
feederMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); | ||
feederMotor.enableVoltageCompensation(12.0); | ||
feederMotor.setSmartCurrentLimit(30); | ||
|
||
feederPID = feederMotor.getPIDController(); | ||
feederPID.setP(FEEDER_KP, 0); | ||
feederPID.setI(FEEDER_KI, 0); | ||
feederPID.setD(FEEDER_KD, 0); | ||
} | ||
|
||
@Override | ||
public void updateInputs(IndexerIOInputs inputs) { | ||
inputs.intakeCurrentAmps = intakeMotor.getOutputCurrent(); | ||
inputs.intakeVoltage = intakeMotor.getBusVoltage() * intakeMotor.getAppliedOutput(); | ||
inputs.intakeVelocityRPM = intakeMotor.getEncoder().getVelocity(); | ||
inputs.feederCurrentAmps = feederMotor.getOutputCurrent(); | ||
inputs.feederVoltage = feederMotor.getBusVoltage() * feederMotor.getAppliedOutput(); | ||
inputs.feederVelocityRPM = feederMotor.getEncoder().getVelocity(); | ||
inputs.noteInIntake = indexerBeamBreak.get(); | ||
inputs.setpointRPM = setpointRPM; | ||
inputs.feederPositionError = setpointRPM - feederMotor.getEncoder().getVelocity(); | ||
} | ||
|
||
@Override | ||
public void setIntakeVoltage(double volts) { | ||
intakeMotor.setVoltage(volts); | ||
} | ||
|
||
@Override | ||
public void setFeederVoltage(double volts) { | ||
feederMotor.setVoltage(volts); | ||
} | ||
|
||
@Override | ||
public void setFeederVelocity(double velocity) { | ||
setpointRPM = velocity; | ||
feederPID.setReference( | ||
velocity / FEEDER_GEAR_RATIO, // Should this be multiplied? | ||
CANSparkBase.ControlType.kVelocity, | ||
0, | ||
feederFF.calculate(velocity), | ||
SparkPIDController.ArbFFUnits.kVoltage | ||
); | ||
} | ||
} |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
There's already a constant called
tuningMode
in the constants file, use that instead of this