From 7baee4061727ca09738ffffb971652b3357216ec Mon Sep 17 00:00:00 2001 From: Iooob <94081903+Iooob@users.noreply.github.com> Date: Sat, 18 Jan 2025 00:57:21 -0500 Subject: [PATCH] Created Turntable subsystem + command - Need to setup simulator --- src/main/java/frc/robot/Constants.java | 2 + src/main/java/frc/robot/RobotContainer.java | 4 + .../frc/robot/commands/TurntableCommand.java | 39 ++++ .../robot/subsystems/ElevatorSubsystem.java | 10 +- .../robot/subsystems/TurntableSubsystem.java | 209 ++++++++++++++++++ 5 files changed, 259 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/commands/TurntableCommand.java create mode 100644 src/main/java/frc/robot/subsystems/TurntableSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d6c01b9..75d29b7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -35,6 +35,8 @@ public static final class CANConstants { public static final int MOTORLIFTERRIGHTID = 42; /// Elevator Motors public static final int MOTORELEVATORID = 51; + /// Turntable Motor + public static final int MOTORTURNTABLEID = 61; } public static final Mode simMode = Mode.SIM; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fc0fd23..86aba18 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,6 +24,7 @@ import frc.robot.commands.FlywheelCommand; import frc.robot.commands.LifterCommand; import frc.robot.commands.StraightCommand; +import frc.robot.commands.TurntableCommand; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.CameraSubsystem; import frc.robot.subsystems.DriveSubsystem; @@ -31,6 +32,7 @@ import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.FlywheelSubsystem; import frc.robot.subsystems.LifterSubsystem; +import frc.robot.subsystems.TurntableSubsystem; import frc.robot.subsystems.UltrasonicSubsystem; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -66,6 +68,7 @@ public class RobotContainer { private final DriverCameraSubsystem m_DriverCameraSubsystem = new DriverCameraSubsystem(); private final ElevatorSubsystem m_ElevatorSubsystem = new ElevatorSubsystem(); private final ArmSubsystem m_ArmSubsystem = new ArmSubsystem(); + private final TurntableSubsystem m_TurntableSubsystem = new TurntableSubsystem(); // The robots commands are defined here.. // private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem); @@ -79,6 +82,7 @@ public class RobotContainer { private final LifterCommand m_RightLifterCommand = new LifterCommand(m_rightLifterSubsystem); private final ElevatorCommand m_ElevatorCommand = new ElevatorCommand(m_ElevatorSubsystem); private final ArmCommand m_ArmCommand = new ArmCommand(m_ArmSubsystem); + private final TurntableCommand m_TurntableCommand = new TurntableCommand(m_TurntableSubsystem); // EX: these commands are used by autonomous only // private final AimAmpCommand m_AimAmpCommand = new AimAmpCommand(m_armSubsystem, diff --git a/src/main/java/frc/robot/commands/TurntableCommand.java b/src/main/java/frc/robot/commands/TurntableCommand.java new file mode 100644 index 0000000..c7148ad --- /dev/null +++ b/src/main/java/frc/robot/commands/TurntableCommand.java @@ -0,0 +1,39 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.TurntableSubsystem; + +/** An Turntable command that uses the Turntable subsystem. */ +public class TurntableCommand extends Command { + private final TurntableSubsystem m_TurntableSubsystem; + + /** + * Create a new TurntableCommand. + * + * @param t_Subsystem The subsystem used by this command. + */ + public TurntableCommand(TurntableSubsystem t_Subsystem) { + m_TurntableSubsystem = t_Subsystem; + + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(t_Subsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index c087694..4f51fae 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -26,21 +26,21 @@ import frc.robot.DriveConstants; public class ElevatorSubsystem extends SubsystemBase { - // Motor + // Declare Motor private final SparkMax m_Motor; - // Simulated Motor + // Declare Simulated Motor private final DCMotor m_simGearbox; private final SparkMaxSim m_simMotor; - // Motor Configs + // Instantiate Motor Configs private final SparkMaxConfig m_MotorConfig = new SparkMaxConfig(); // Declare PID private final SparkClosedLoopController m_ElevatorMainPIDController; // Declare Encoder private RelativeEncoder m_ElevatorEncoder; - // Simulated Encoder + // Declare Simulated Encoder private final SparkRelativeEncoderSim m_ElevatorEncoderSim; - // Elevator Physics Engine + // Declare Elevator Physics Engine private final ElevatorSim m_ElevatorSim; // TODO: Update to accurate values diff --git a/src/main/java/frc/robot/subsystems/TurntableSubsystem.java b/src/main/java/frc/robot/subsystems/TurntableSubsystem.java new file mode 100644 index 0000000..e19b625 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/TurntableSubsystem.java @@ -0,0 +1,209 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.Volts; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.sim.SparkMaxSim; +import com.revrobotics.sim.SparkRelativeEncoderSim; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.BatterySim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.simulation.LinearSystemSim; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants.CANConstants; +import frc.robot.DriveConstants; + +public class TurntableSubsystem extends SubsystemBase { + // Decalare Motor + private final SparkMax m_Motor; + // Declare Simulated Motor + private final DCMotor m_simGearbox; + private final SparkMaxSim m_simMotor; + + // Declare Motor Configs + private final SparkMaxConfig m_MotorConfig = new SparkMaxConfig(); + // Declare PID + private final SparkClosedLoopController m_TurntableMainPIDController; + // Declare Encoder + private RelativeEncoder m_TurntableEncoder; + // Declare Simulated Encoder + private final SparkRelativeEncoderSim m_TurntableEncoderSim; + // Declare Turntable Physics Engine + private final LinearSystemSim m_TurntableSim; + + // TODO: Update to accurate values + private final double kP, kI, kD, kIz, kMaxOutput, kMinOutput; + // general drive constants + // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2 + // https://sciencing.com/convert-rpm-linear-speed-8232280.html + private final double kGearRatio = 16; // TBD + // basically converted from rotations to to radians to then meters using the wheel diameter. + // the diameter is already *2 so we don't need to multiply by 2 again. + private final double kPositionConversionRatio = (Math.PI * 2) / kGearRatio; + private final double kVelocityConversionRatio = kPositionConversionRatio / 60; + + // setup feedforward + private final double kS = 0.1; // Static Gain (Volts) + private final double kV = 0.1; // Velocity Volts/(rad/s) + private final double kA = 0.1; // Acceleration Volts/(rad/s^2) + + // other constants + private final double kMaxAngleRads = 1.0; // TODO: Update + private final double kMinAngleRads = 0.01; + private final double kStartingAngleRads = kMinAngleRads + 0.01; + private final double kTurntableLengthMeters = 0.1; + private final double kjKgMetersSquared = + 0.1; // The moment of inertia of the Turntable; can be calculated from CAD software. + + SimpleMotorFeedforward m_TurntableFeedforward = new SimpleMotorFeedforward(kS, kV, kA); + + // setup trapezoidal motion profile + private final double kMaxVelocity = 0.2; // R/S + private final double kMaxAcceleration = 0.1; // R/S^2 + private final double kAllowedClosedLoopError = 0.05; // Radians + + // setup SysID for auto profiling + private final SysIdRoutine m_sysIdRoutine; + + // current limit + private final int k_CurrentLimit = 60; + + public TurntableSubsystem() { + // Create Turntable motor + m_Motor = new SparkMax(CANConstants.MOTORTURNTABLEID, SparkMax.MotorType.kBrushless); + + // Create Simulated Motors + m_simGearbox = DCMotor.getNEO(1); + m_simMotor = new SparkMaxSim(m_Motor, m_simGearbox); + + // Create Simulated encoder + m_TurntableEncoderSim = m_simMotor.getRelativeEncoderSim(); + + // Create Simulated Physics Engine + m_TurntableSim = + new LinearSystemSim( + m_simGearbox, + kGearRatio, + kjKgMetersSquared, + kTurntableLengthMeters, + kMinAngleRads, + kMaxAngleRads, + true, + kStartingAngleRads, + 0.01, + 0.001); + + // Set idle mode to coast + m_MotorConfig.idleMode(IdleMode.kBrake); + // Set current limit + m_MotorConfig.smartCurrentLimit(k_CurrentLimit); + + // Connect to built in PID controller + m_TurntableMainPIDController = m_Motor.getClosedLoopController(); + + // Allow us to read the encoder + m_TurntableEncoder = m_Motor.getEncoder(); + + m_MotorConfig.encoder.positionConversionFactor(kPositionConversionRatio); + m_MotorConfig.encoder.velocityConversionFactor(kVelocityConversionRatio); + + // PID coefficients + kP = 0.0; + kI = 0; + kD = 0; + kIz = 0; + kMaxOutput = 0.8; + kMinOutput = -0.8; + // set PID coefficients + m_MotorConfig.closedLoop.pid(kP, kI, kD, DriveConstants.kDrivetrainPositionPIDSlot); + m_MotorConfig.closedLoop.iZone(kIz, DriveConstants.kDrivetrainPositionPIDSlot); + m_MotorConfig.closedLoop.outputRange( + kMinOutput, kMaxOutput, DriveConstants.kDrivetrainPositionPIDSlot); + // Smart Control Config + m_MotorConfig.closedLoop.maxMotion.maxVelocity( + kMaxVelocity, DriveConstants.kDrivetrainPositionPIDSlot); + m_MotorConfig.closedLoop.maxMotion.maxAcceleration( + kMaxAcceleration, DriveConstants.kDrivetrainPositionPIDSlot); + m_MotorConfig.closedLoop.maxMotion.allowedClosedLoopError( + kAllowedClosedLoopError, DriveConstants.kDrivetrainPositionPIDSlot); + // setup SysID for auto profiling + m_sysIdRoutine = + new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (voltage) -> this.setVoltage(voltage), + null, // No log consumer, since data is recorded by URCL + this)); + + m_Motor.configure( + m_MotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + public void setVoltage(Voltage voltage) { + m_Motor.setVoltage(voltage.in(Volts)); + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutine.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutine.dynamic(direction); + } + + /* + * Move Turntable to a specific angle + */ + public void SetAngle(double radians) { + m_TurntableMainPIDController.setReference( + radians, + SparkBase.ControlType.kMAXMotionPositionControl, + DriveConstants.kDrivetrainPositionPIDSlot, + m_TurntableFeedforward.calculate(radians, m_TurntableEncoder.getVelocity())); + } + + /* + * Reset the Turntable + */ + public void ResetTurntable() { + SetAngle(kStartingAngleRads); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + // Update the simulation of our Turntable, set inputs + m_TurntableSim.setInput(m_simMotor.getAppliedOutput() * RobotController.getBatteryVoltage()); + + // update simulation (20ms) + m_TurntableSim.update(0.020); + + // Iterate PID loops + m_simMotor.iterate(m_TurntableSim.getVelocityRadPerSec(), RoboRioSim.getVInVoltage(), 0.02); + + // add load to battery + RoboRioSim.setVInVoltage( + BatterySim.calculateDefaultBatteryLoadedVoltage(m_TurntableSim.getCurrentDrawAmps())); + + // update encoder + m_TurntableEncoderSim.setPosition(m_TurntableSim.getAngleRads()); + m_TurntableEncoderSim.setVelocity(m_TurntableSim.getVelocityRadPerSec()); + } +}