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
18 changes: 18 additions & 0 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,10 @@
import static edu.wpi.first.units.Units.Percent;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond;
import static edu.wpi.first.units.Units.Second;
import static edu.wpi.first.units.Units.Seconds;
import static edu.wpi.first.units.Units.Value;
import static edu.wpi.first.units.Units.Volts;

import java.util.List;
Expand All @@ -28,7 +31,10 @@
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.units.AngleUnit;
import edu.wpi.first.units.AngularAccelerationUnit;
import edu.wpi.first.units.AngularVelocityUnit;
import edu.wpi.first.units.DistanceUnit;
import edu.wpi.first.units.VoltageUnit;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Current;
Expand Down Expand Up @@ -207,4 +213,16 @@ public static class SimulationConstants
public static final Distance EXTENDED_DISTANCE = Inches.of(10);
public static final Distance RETRACTED_DISTANCE = Inches.of(0);
}

public static class ShooterConstants
{
// Rotor
public static final Current ROTOR_CURRENT_LIMIT = Amps.of(80);
public static final Dimensionless ROTOR_GEAR_RATIO = Value.of(36).div(Value.of(1)); // 36:1
public static final Voltage ROTOR_KS = Volts.of(0.0);
public static final Per<VoltageUnit, AngularVelocityUnit> ROTOR_KV = Volts.of(12.0).div(RPM.of(6000.0).div(ROTOR_GEAR_RATIO));
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));
}
}
11 changes: 11 additions & 0 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
// the WPILib BSD license file in the root directory of this project.
package frc.robot;

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

import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
Expand All @@ -19,6 +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.util.MeasureUtil;

@Logged
Expand All @@ -33,6 +35,9 @@ public class RobotContainer
private final Autos _autos = new Autos(_drive, _intake);
// private Dimensionless _driveMultiplier = DriveConstants.FULL_SPEED_SCALE;

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

public RobotContainer()
{
configureDefaultCommands();
Expand Down Expand Up @@ -80,6 +85,12 @@ private void configureButtonBindings()
_operator.rightTrigger().whileTrue(_intake.jiggle());
_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)));
}

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

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.subsystems.shooter.rotor.RotorIO.RotorIOInputs;

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

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

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

// Creates an instance of a rotor
public Rotor()
{
// Inputs is just a container for variables. It can always be initialized using
// the default constructor
_inputs = new RotorIOInputs();

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

// 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 rotor motor
public AngularVelocity getRate()
{
return _inputs.rotationRate;
}

// Sets the desired angular rate of rotation of the rotor
public void setRate(AngularVelocity rate)
{
// If a non-positive rate is requested, turn off the rotor
if (rate.lte(RPM.zero()))
{
stop();
return;
}

_io.setRate(rate);
}

// Commands the rotor to stop rotating
public void stop()
{
_io.stop();
}

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

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

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

// Creates a command to set the angular rate of rotation of the rotor. When the
// command ends, the rotor stops rotating
public Command run(AngularVelocity rate)
{
return startEnd(() -> Rotor.this.setRate(rate), Rotor.this::stop);
}
}
}
47 changes: 47 additions & 0 deletions src/main/java/frc/robot/subsystems/shooter/rotor/RotorIO.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
package frc.robot.subsystems.shooter.rotor;

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 rotor sub-component of the shooter subsystem.
*/
public interface RotorIO
{
/*
* A container for the sensor inputs we can expect back from the rotor motor
* each timestep.
*/
@Logged
public static class RotorIOInputs
{
// The voltage the rotor is currently applying
@Logged
public Voltage appliedVoltage = Volts.zero();

// The amount of current the rotor is currently drawing
@Logged
public Current currentDraw = Amps.zero();

// The current rotational velocity of the rotor
@Logged
public AngularVelocity rotationRate = RPM.zero();
}

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

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

// Commands the rotor to stop rotating
public void stop();
}
89 changes: 89 additions & 0 deletions src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOReal.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,89 @@
package frc.robot.subsystems.shooter.rotor;

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.Value;
import static edu.wpi.first.units.Units.Volts;

import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;

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

/*
* A real implementation of the physical layer (motors and sensors) of the rotor
* sub-component of the shooter subsystem.
*/
public class RotorIOReal implements RotorIO
{
// The rotor motor
private final TalonFX _rotorMotor;

// The request object for running the rotor at a specified velocity
private final VelocityVoltage _velocityRequest;

// Constructs a RotorIOReal instance
public RotorIOReal()
{
// Initialize the motor
_rotorMotor = new TalonFX(CANConstants.ROTOR_MOTOR);

// Set up the velocity request for rotating at a desired rate
_velocityRequest = new VelocityVoltage(RPM.zero()).withSlot(0);

// Start configuring the motor
var config = new TalonFXConfiguration();

// Motor configurations related to current limits
config.CurrentLimits.StatorCurrentLimit = ShooterConstants.ROTOR_CURRENT_LIMIT.in(Amps);
config.CurrentLimits.StatorCurrentLimitEnable = true;

// Motor configurations related to general motor output
config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;

// Motor configurations related to closed loop gains
config.Slot0.kG = 0.0;
config.Slot0.kS = ShooterConstants.ROTOR_KS.in(Volts);
config.Slot0.kV = ShooterConstants.ROTOR_KV.in(Volts.per(RPM));
config.Slot0.kA = ShooterConstants.ROTOR_KA.in(Volts.per(RotationsPerSecondPerSecond));
config.Slot0.kP = ShooterConstants.ROTOR_KP.in(Volts.per(RPM));
config.Slot0.kI = 0.0;
config.Slot0.kD = ShooterConstants.ROTOR_KD.in(Volts.per(RPM.per(Second)));

// Set the gear ratio of the motor
config.Feedback.SensorToMechanismRatio = ShooterConstants.ROTOR_GEAR_RATIO.in(Value);

// Apply the configuration to the motor
_rotorMotor.getConfigurator().apply(config);
}

// Allows the RotorIOReal object to read sensor inputs. Updated
// values are loaded into the provided "inputs" parameter.
public void updateInputs(RotorIOInputs inputs)
{
// Save off the new inputs as measured by the motor
inputs.appliedVoltage = _rotorMotor.getMotorVoltage().getValue();
inputs.currentDraw = _rotorMotor.getStatorCurrent().getValue();
inputs.rotationRate = _rotorMotor.getVelocity().getValue();
}

// Sets the desired angular rate of rotation of the rotor
public void setRate(AngularVelocity rate)
{
_rotorMotor.setControl(_velocityRequest.withVelocity(rate));
}

// Commands the rotor to stop rotating
public void stop()
{
_rotorMotor.setVoltage(0);
}
}
Loading