diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a94a93f..c21e002 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; @@ -45,6 +46,8 @@ import frc.robot.subsystems.shooter.HoodIOSim; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterSubsystem; +import frc.robot.subsystems.shooter.TurretIO; +import frc.robot.subsystems.shooter.TurretIOSim; import frc.robot.subsystems.shooter.TurretSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread; @@ -277,7 +280,12 @@ public Robot() { case COMP: indexer = new SpindexerSubsystem(); intake = new LintakeSubsystem(); - shooter = new TurretSubsystem(); + shooter = + new TurretSubsystem( + ROBOT_MODE == RobotMode.REAL + ? new TurretIO(TurretIO.getTurretConfiguration(), canivore) + : new TurretIOSim(TurretIO.getTurretConfiguration(), canivore) + ); climber = new ClimberSubsystem(); // TODO climber break; } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index cd4ab04..ea0187a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -119,6 +119,10 @@ public Command spit() { }); // TODO: TUNE HOOD POS AND FLYWHEEL VELOCITY } + public Command setHoodPositionCommand(Supplier hoodPosition) { + return this.run(() -> hoodIO.setHoodPosition(hoodPosition.get())); + } + @Override public void periodic() { hoodIO.updateInputs(hoodInputs); @@ -135,7 +139,7 @@ public Command runHoodSysid() { .until( () -> hoodInputs.hoodPositionRotations.getDegrees() - > (HOOD_MAX_ROTATION.getDegrees() - 5)), // Stop before endstop + > (HOOD_MAX_ROTATION.getDegrees() - 5)), hoodSysid .quasistatic(Direction.kReverse) .until( diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java new file mode 100644 index 0000000..a871211 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java @@ -0,0 +1,123 @@ +// 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.shooter; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; +import org.littletonrobotics.junction.AutoLog; + +/** Add your docs here. */ +public class TurretIO { + @AutoLog + public static class TurretIOInputs { + public Rotation2d turretPositionRotations = new Rotation2d(); + public double turretStatorCurrentAmps = 0.0; + public double velocityRotationsPerSec = 0.0; + public double turretSupplyCurrentAmps = 0.0; + public double turretVoltage = 0.0; + public double turretTempC = 0.0; + } + + protected TalonFX turretMotor; + private final BaseStatusSignal turretPositionRotations; + private final StatusSignal turretVoltage; + private final StatusSignal turretStatorCurrent; + private final StatusSignal turretSupplyCurrent; + private final StatusSignal turretTemp; + private final StatusSignal turretAngularVelocity; + private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); + private PositionVoltage positionVoltage = new PositionVoltage(0.0).withEnableFOC(true); + private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true); + + public TurretIO(TalonFXConfiguration talonFXConfiguration, CANBus canbus) { + turretMotor = new TalonFX(11, canbus); + turretMotor.getConfigurator().apply(TurretIO.getTurretConfiguration()); + turretPositionRotations = turretMotor.getPosition(); + turretAngularVelocity = turretMotor.getVelocity(); + turretVoltage = turretMotor.getMotorVoltage(); + turretStatorCurrent = turretMotor.getStatorCurrent(); + turretSupplyCurrent = turretMotor.getSupplyCurrent(); + turretTemp = turretMotor.getDeviceTemp(); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + turretPositionRotations, + turretAngularVelocity, + turretVoltage, + turretStatorCurrent, + turretSupplyCurrent, + turretTemp); + turretMotor.optimizeBusUtilization(); + } + + public static TalonFXConfiguration getTurretConfiguration() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + config.Feedback.SensorToMechanismRatio = TurretSubsystem.TURRET_GEAR_RATIO; + + // config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; Potentially need, maybe not tho. + + config.Slot0.kS = 0.0; + config.Slot0.kG = 0.0; + config.Slot0.kV = 1.1; + config.Slot0.kP = 5.0; + config.Slot0.kD = 0.0; + + config.CurrentLimits.StatorCurrentLimit = 80.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = 60.0; + + return config; + } + + public void setTurretVoltage(double turretVoltage) { + turretMotor.setControl(voltageOut.withOutput(turretVoltage)); + } + + public void setTurretPosition(Rotation2d turretPosition) { + turretMotor.setControl(positionVoltage.withPosition(turretPosition.getRotations())); + } + + public void setTurretVelocity(double turretVelocity) { + turretMotor.setControl(velocityVoltage.withVelocity(turretVelocity)); + } + + public void updateInputs(TurretIOInputs inputs) { + BaseStatusSignal.refreshAll( + turretPositionRotations, + turretVoltage, + turretStatorCurrent, + turretSupplyCurrent, + turretTemp); + + inputs.turretPositionRotations = + Rotation2d.fromRadians(turretPositionRotations.getValueAsDouble()); + inputs.turretVoltage = turretVoltage.getValueAsDouble(); + inputs.turretStatorCurrentAmps = turretStatorCurrent.getValueAsDouble(); + inputs.turretSupplyCurrentAmps = turretSupplyCurrent.getValueAsDouble(); + inputs.turretTempC = turretTemp.getValueAsDouble(); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java b/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java new file mode 100644 index 0000000..850f20f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java @@ -0,0 +1,46 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.sim.ChassisReference; +import com.ctre.phoenix6.sim.TalonFXSimState; +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +public class TurretIOSim extends TurretIO{ + DCMotorSim turretphysicssSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX44Foc(1), 0.001, TurretSubsystem.TURRET_GEAR_RATIO), + DCMotor.getKrakenX44Foc(1)); + +TalonFXSimState motorSim; +final double simLoopPeriod = 0.002; +Notifier simNotifier; +double lastSimTime = 0.0; + +public TurretIOSim(TalonFXConfiguration configuration, CANBus canBus){ + super(configuration, canBus); +motorSim = turretMotor.getSimState(); +motorSim.setMotorType(MotorType.KrakenX44); +motorSim.Orientation = ChassisReference.Clockwise_Positive; +simNotifier = new Notifier(() -> { + double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - lastSimTime; + lastSimTime = currentTime; +motorSim.setSupplyVoltage(RobotController.getBatteryVoltage()); +turretphysicssSim.setInputVoltage(motorSim.getMotorVoltage()); +turretphysicssSim.update(deltaTime); +motorSim.setRawRotorPosition(turretphysicssSim.getAngularPositionRotations() * turretphysicssSim.getGearing()); +motorSim.setRotorVelocity(turretphysicssSim.getAngularVelocityRPM()/60 * turretphysicssSim.getGearing()); + +}); +simNotifier.startPeriodic(simLoopPeriod); + +} +} diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 685b95a..be3cc2f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -5,42 +5,69 @@ package frc.robot.subsystems.shooter; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; + /** Pivoting hooded shooter (turret). !! COMP !! */ public class TurretSubsystem extends SubsystemBase implements Shooter { + public static double TURRET_GEAR_RATIO = (12.0 / 42.0) * (16.0 / 32.0) * (10.0 / 85.0); + public TurretIO turretIO; + public TurretIOInputsAutoLogged turretIOInputs = new TurretIOInputsAutoLogged(); + /** Creates a new TurretSubsystem. */ - public TurretSubsystem() {} + public TurretSubsystem(TurretIO turretIO) { + this.turretIO = turretIO; + } @Override public void periodic() { + turretIO.updateInputs(turretIOInputs); + Logger.processInputs("Shooter/Turret", turretIOInputs); // This method will be called once per scheduler run } @Override public Command shoot(Supplier robotPoseSupplier) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'shoot'"); + return this.run( + () -> { + turretIO.setTurretPosition(new Rotation2d()); + // TODO:Find the reall number + + }); } @Override public Command feed(Supplier robotPoseSupplier, Supplier feedTarget) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'feed'"); + return this.run( + () -> { + turretIO.setTurretPosition(new Rotation2d()); + // TODO:Find the reall number + + }); } @Override public Command rest() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'rest'"); + return this.run( + () -> { + turretIO.setTurretPosition(new Rotation2d()); + // TODO:Find the reall number + + }); } @Override public Command spit() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'spit'"); + return this.run( + () -> { + turretIO.setTurretPosition(new Rotation2d()); + // TODO:Find the reall number + + }); } @Override @@ -63,7 +90,11 @@ public Command zeroHood() { @Override public Command testShoot() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'testShoot'"); + return this.run( + () -> { + turretIO.setTurretPosition(new Rotation2d()); + // TODO:Find the reall number + + }); } }