Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 9 additions & 0 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -224,5 +224,14 @@ public static class ShooterConstants
public static final Per<VoltageUnit, AngularAccelerationUnit> ROTOR_KA = Volts.of(0.0).per(RotationsPerSecondPerSecond);
public static final Per<VoltageUnit, AngularVelocityUnit> ROTOR_KP = Volts.of(0.025).per(RPM);
public static final Per<VoltageUnit, AngularAccelerationUnit> ROTOR_KD = Volts.of(0.00005).per(RPM.per(Second));

// Flywheel
public static final Current FLYWHEEL_CURRENT_LIMIT = Amps.of(80);
public static final Voltage FLYWHEEL_KS = Volts.of(0.0);
public static final Per<VoltageUnit, AngularVelocityUnit> FLYWHEEL_KV = Volts.of(12.0).div(RPM.of(6784.0));
public static final Per<VoltageUnit, AngularAccelerationUnit> FLYWHEEL_KA = Volts.of(0.0).per(RotationsPerSecondPerSecond);
public static final Per<VoltageUnit, AngularVelocityUnit> FLYWHEEL_KP = Volts.of(0.00015).per(RPM);
public static final Per<VoltageUnit, AngularAccelerationUnit> FLYWHEEL_KD = Volts.of(0.0).per(RPM.per(Second));
public static final AngularVelocity FLYWHEEL_TOLERANCE = RPM.of(50);
}
}
14 changes: 7 additions & 7 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@
import frc.robot.subsystems.drive.Drive;
import frc.robot.subsystems.drive.TunerConstants;
import frc.robot.subsystems.intake.Intake;
import frc.robot.subsystems.shooter.rotor.Rotor;
import frc.robot.subsystems.shooter.flywheel.Flywheel;
import frc.robot.util.MeasureUtil;

@Logged
Expand All @@ -36,7 +36,7 @@ public class RobotContainer
// private Dimensionless _driveMultiplier = DriveConstants.FULL_SPEED_SCALE;

// Shooter subsystem components
private final Rotor _rotor = new Rotor();
private final Flywheel _flywheel = new Flywheel();

public RobotContainer()
{
Expand Down Expand Up @@ -86,11 +86,11 @@ private void configureButtonBindings()
_operator.povDown().onTrue(_intake.getRetractCmd());
_operator.povUp().onTrue(_intake.getExtendCmd());

// Rotor testing commands
_operator.a().onTrue(_rotor.commands.stop());
_operator.y().onTrue(_rotor.commands.setRate(RPM.of(80)));
_operator.b().whileTrue(_rotor.commands.run(RPM.of(100)));
_operator.x().whileTrue(_rotor.commands.run(RPM.of(60)));
// Flywheel testing commands
_operator.a().onTrue(_flywheel.commands.stop());
_operator.y().onTrue(_flywheel.commands.setRate(RPM.of(4800)));
_operator.b().whileTrue(_flywheel.commands.run(RPM.of(5400)));
_operator.x().whileTrue(_flywheel.commands.run(RPM.of(3600)));
}

public Command getAutonomousCommand()
Expand Down
136 changes: 136 additions & 0 deletions src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,136 @@
package frc.robot.subsystems.shooter.flywheel;

import static edu.wpi.first.units.Units.RPM;

import edu.wpi.first.epilogue.Logged;
import edu.wpi.first.epilogue.NotLogged;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.ShooterConstants;
import frc.robot.subsystems.shooter.flywheel.FlywheelIO.FlywheelIOInputs;

/*
* Class containing the business logic for how the flywheel component of the
* shooter subsystem works
*/
@Logged
public class Flywheel extends SubsystemBase
{
// The inputs measured from the flywheel
@Logged
private final FlywheelIOInputs _inputs;

// The physical layer (motors and sensors) the business logic interacts with
@NotLogged
private final FlywheelIO _io;

// Provides a clean way for other classes to instantiate commands that run on
// this object
@NotLogged
public final FlywheelCommandFactory commands = new FlywheelCommandFactory();

// The target rate of rotation for the flywheel
@Logged
private AngularVelocity _targetRate;

// Creates an instance of a flywheel
public Flywheel()
{
// Inputs is just a container for variables. In can always be initialized using
// the default constructor
_inputs = new FlywheelIOInputs();

// Instantiate an appropriate physical layer object based on if we're running on
// a real robot or in simulation
if (RobotBase.isReal())
{
_io = new FlywheelIOReal();
}
else
{
_io = new FlywheelIOSim();
}

// Initialize private non-final member variables
_targetRate = RPM.zero();
}

// Executes every 20ms. Used to measure inputs from the physical layer
@Override
public void periodic()
{
_io.updateInputs(_inputs);
}

// Gets the current rate of rotation of the flywheel
public AngularVelocity getRate()
{
// Since the flywheel is made up of two motors, we'll take the average of both
// motors' speeds. The difference should be negligible since they're
// mechanically linked on the same output shaft.
return _inputs.leftMotorRotationRate.plus(_inputs.rightMotorRotationRate).div(2);
}

// Gets the current target rate of rotation for the flywheel
public AngularVelocity getTargetRate()
{
return _targetRate;
}

// Determines if the flywheel is currently spinning at the target angular
// velocity. Only returns true if the flywheel is actually on.
public boolean atSpeed()
{
return _targetRate.gt(RPM.zero()) && getRate().isNear(_targetRate, ShooterConstants.FLYWHEEL_TOLERANCE);
}

// Sets the target rate of rotation for the flywheel
public void setRate(AngularVelocity rate)
{
if (rate.lte(RPM.zero()))
{
stop();
return;
}

_targetRate = rate;
_io.setRate(_targetRate);
}

// Commands the flywheel to stop rotating
public void stop()
{
_targetRate = RPM.zero();
_io.stop();
}

/************
* Commands *
************/

// Nested helper class responsible for creating commands that act on the
// enclosing Flywheel object instance
public class FlywheelCommandFactory
{
// Creates a command to set the angular rate of rotation of the flywheel
public Command setRate(AngularVelocity rate)
{
return runOnce(() -> Flywheel.this.setRate(rate));
}

// Creates a command to stop the rotation of the flywheel
public Command stop()
{
return runOnce(Flywheel.this::stop);
}

// Creates a command to set the angular rate of rotation of the flywheel. When
// the command ends, the flywheel stops rotating
public Command run(AngularVelocity rate)
{
return startEnd(() -> Flywheel.this.setRate(rate), Flywheel.this::stop);
}
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,59 @@
package frc.robot.subsystems.shooter.flywheel;

import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.RPM;
import static edu.wpi.first.units.Units.Volts;

import edu.wpi.first.epilogue.Logged;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Current;
import edu.wpi.first.units.measure.Voltage;

/*
* Interface for objects that represent the physical layer (motors and sensors)
* of the flywheel sub-component of the shooter subsystem.
*/
public interface FlywheelIO
{
/*
* A container for the sensor inputs we can expect back from the flywheel each
* timestep.
*/
@Logged
public static class FlywheelIOInputs
{
// The voltage currently applied to the left flywheel motor
@Logged
public Voltage leftMotorAppliedVoltage = Volts.zero();

// The amount of current the left flywheel motor is currently drawing
@Logged
public Current leftMotorCurrentDraw = Amps.zero();

// The current rotational velocity of the left flywheel motor
@Logged
public AngularVelocity leftMotorRotationRate = RPM.zero();

// The voltage currently applied to the right flywheel motor
@Logged
public Voltage rightMotorAppliedVoltage = Volts.zero();

// The amound of current the right flywheel motor is currently drawing
@Logged
public Current rightMotorCurrentDraw = Amps.zero();

// The current rotational velocity of the right flywheel motor
@Logged
public AngularVelocity rightMotorRotationRate = RPM.zero();
}

// Allows the FlywheelIO object to read sensor inputs. Updated values are loaded
// into the provided "inputs" parameter.
public void updateInputs(FlywheelIOInputs inputs);

// Sets the desired angular rate of rotation of the flywheel
public void setRate(AngularVelocity rate);

// Commands the flywheel to stop rotating
public void stop();
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,122 @@
package frc.robot.subsystems.shooter.flywheel;

import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.RPM;
import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond;
import static edu.wpi.first.units.Units.Second;
import static edu.wpi.first.units.Units.Volts;

import com.revrobotics.PersistMode;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.ResetMode;
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkFlex;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.config.SparkFlexConfig;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;

import edu.wpi.first.units.measure.AngularVelocity;
import frc.robot.Constants.CANConstants;
import frc.robot.Constants.GeneralConstants;
import frc.robot.Constants.ShooterConstants;

/*
* A real implementation of the physical layer (motors and sensors) of the
* flywheel sub-component of the shooter subsystem.
*/
public class FlywheelIOReal implements FlywheelIO
{
// The left flywheel motor
private final SparkFlex _leftFlywheelMotor;

// The right flywheel motor
private final SparkFlex _rightFlywheelMotor;

// The closed loop controller for running the flywheel at a specified velocity
private final SparkClosedLoopController _pid;

// The encoder built into the left flywheel motor
private final RelativeEncoder _leftFlywheelEncoder;

// The encoder built into the right flywheel motor
private final RelativeEncoder _rightFlywheelEncoder;

public FlywheelIOReal()
{
// Instantiate the motors
_leftFlywheelMotor = new SparkFlex(CANConstants.FLYWHEEL_LEAD, MotorType.kBrushless);
_rightFlywheelMotor = new SparkFlex(CANConstants.FLYWHEEL_FOLLOW, MotorType.kBrushless);

// Set the CAN timeout high for initial configuration
_leftFlywheelMotor.setCANTimeout(250); // Milliseconds
_rightFlywheelMotor.setCANTimeout(250); // Milliseconds

// Start configuring the motors
var config = new SparkFlexConfig();

// Motor configurations related to general motor output
config.inverted(false);
config.idleMode(IdleMode.kCoast);
config.smartCurrentLimit((int)ShooterConstants.FLYWHEEL_CURRENT_LIMIT.in(Amps));
config.voltageCompensation(GeneralConstants.MOTOR_VOLTAGE.in(Volts));

// Motor configurations related to closed loop gains
config.closedLoop.feedForward.kG(0.0);
config.closedLoop.feedForward.kS(ShooterConstants.FLYWHEEL_KS.in(Volts));
config.closedLoop.feedForward.kV(ShooterConstants.FLYWHEEL_KV.in(Volts.per(RPM)));
config.closedLoop.feedForward.kA(ShooterConstants.FLYWHEEL_KA.in(Volts.per(RotationsPerSecondPerSecond)));
config.closedLoop.p(ShooterConstants.FLYWHEEL_KP.in(Volts.per(RPM)));
config.closedLoop.i(0.0);
config.closedLoop.d(ShooterConstants.FLYWHEEL_KD.in(Volts.per(RPM.per(Second))));

// Configure the leader motor
_leftFlywheelMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);

// Provide an additional configuration for the follower motor
config.follow(_leftFlywheelMotor);

// Configure the follower motor
_rightFlywheelMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);

// Get the encoders
_leftFlywheelEncoder = _leftFlywheelMotor.getEncoder();
_rightFlywheelEncoder = _leftFlywheelMotor.getEncoder();

// The closed loop controller
_pid = _leftFlywheelMotor.getClosedLoopController();

// Reset the CAN timeouts
_leftFlywheelMotor.setCANTimeout(0);
_rightFlywheelMotor.setCANTimeout(0);
}

// Allows the FlywheelIOReal object to read sensor inputs. Updated values are
// loaded into the provided "inputs" parameter.
@Override
public void updateInputs(FlywheelIOInputs inputs)
{
inputs.leftMotorAppliedVoltage = Volts.of(_leftFlywheelMotor.getBusVoltage()).times(_leftFlywheelMotor.getAppliedOutput());
inputs.leftMotorCurrentDraw = Amps.of(_leftFlywheelMotor.getOutputCurrent());
inputs.leftMotorRotationRate = RPM.of(_leftFlywheelEncoder.getVelocity());

inputs.rightMotorAppliedVoltage = Volts.of(_rightFlywheelMotor.getBusVoltage()).times(_rightFlywheelMotor.getAppliedOutput());
inputs.rightMotorCurrentDraw = Amps.of(_rightFlywheelMotor.getOutputCurrent());
inputs.rightMotorRotationRate = RPM.of(_rightFlywheelEncoder.getVelocity());
}

// Sets the desired angular rate of rotation of the flywheel.
@Override
public void setRate(AngularVelocity rate)
{
_pid.setSetpoint(rate.in(RPM), ControlType.kVelocity, ClosedLoopSlot.kSlot0);
}

// Commands the flywheel to stop rotating.
@Override
public void stop()
{
_leftFlywheelMotor.setVoltage(Volts.zero());
}
}
Loading