diff --git a/assets/Robot_Potato/config.json b/assets/Robot_Potato/config.json new file mode 100644 index 0000000..6a99b75 --- /dev/null +++ b/assets/Robot_Potato/config.json @@ -0,0 +1,30 @@ +{ + "name": "Potato", + "rotations": [ + { + "axis": "x", + "degrees": 90 + } + ], + "position": [ + 0, + 0, + 0 + ], + "cameras": [], + "components": [ + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + } + ], + "zeroedPositions": [ + 0, + 0, + 0 + ] + } + ] +} \ No newline at end of file diff --git a/assets/Robot_Potato/model.glb b/assets/Robot_Potato/model.glb new file mode 100644 index 0000000..14b0137 Binary files /dev/null and b/assets/Robot_Potato/model.glb differ diff --git a/assets/Robot_Potato/model_0.glb b/assets/Robot_Potato/model_0.glb new file mode 100644 index 0000000..684e33d Binary files /dev/null and b/assets/Robot_Potato/model_0.glb differ diff --git a/src/main/java/com/team973/frc2025/Robot.java b/src/main/java/com/team973/frc2025/Robot.java index 04ee4c4..0cacacc 100644 --- a/src/main/java/com/team973/frc2025/Robot.java +++ b/src/main/java/com/team973/frc2025/Robot.java @@ -5,6 +5,7 @@ package com.team973.frc2025; import com.team973.frc2025.subsystems.DriveController; +import com.team973.frc2025.subsystems.turret.TurretIO; import com.team973.lib.devices.GreyPigeonIO; import com.team973.lib.util.AllianceCache; import com.team973.lib.util.Joystick; @@ -21,25 +22,32 @@ public class Robot extends TimedRobot { private final GreyPigeonIO m_pigeon = m_subsystemManager.getPigeon(); private final DriveController m_driveController = m_subsystemManager.getDriveController(); + private final TurretIO m_turret = m_subsystemManager.getTurret(); private final Joystick m_driverStick = new Joystick(0, Joystick.Type.XboxController, m_logger.subLogger("driverStick")); + private final Joystick m_coDriverStick = + new Joystick(1, Joystick.Type.XboxController, m_logger.subLogger("coDriverStick")); private void syncSensors() { m_driveController.syncSensors(); + m_turret.syncSensors(); } private void updateSubsystems() { m_driveController.update(); + m_turret.update(); } private void resetSubsystems() { m_driveController.reset(); + m_turret.reset(); } private void log() { m_subsystemManager.log(); m_driveController.log(); + m_turret.log(); } private void updateJoysticks() { @@ -95,6 +103,18 @@ public void teleopPeriodic() { allianceScalar * m_driverStick.getLeftXAxis() * 0.7, m_driverStick.getRightXAxis() * 0.8); + if (m_coDriverStick.getAButtonPressed()) { + m_turret.setTargetPreset(TurretIO.Preset.One); + } else if (m_coDriverStick.getBButtonPressed()) { + m_turret.setTargetPreset(TurretIO.Preset.Two); + } else if (m_coDriverStick.getXButtonPressed()) { + m_turret.setTargetPreset(TurretIO.Preset.Three); + } else if (m_coDriverStick.getYButtonPressed()) { + m_turret.setState(TurretIO.State.Manual); + } + + m_turret.setManualInput(m_coDriverStick.getLeftYAxis()); + updateSubsystems(); } diff --git a/src/main/java/com/team973/frc2025/shared/RobotInfo.java b/src/main/java/com/team973/frc2025/shared/RobotInfo.java index ed18f58..c84ca12 100644 --- a/src/main/java/com/team973/frc2025/shared/RobotInfo.java +++ b/src/main/java/com/team973/frc2025/shared/RobotInfo.java @@ -18,6 +18,39 @@ public class RobotInfo { public static final DriveInfo DRIVE_INFO = new DriveInfo(); + public static final TurretInfo TURRET_INFO = new TurretInfo(); + + public static class TurretInfo { + public final int MOTOR_ID = 20; + + public final double MOTOR_GEAR_RATIO = ((5.0 / 1.0) * (128.0 / 10.0)); + + public final double TURRET_KS = 0.0; + public final double TURRET_KV = 0.0; + public final double TURRET_KA = 0.0; + public final double TURRET_KP = 2.0; + public final double TURRET_KI = 0.0; + public final double TURRET_KD = 0.0; + + public final double TURRET_MOTION_MAGIC_CRUISE_VELOCITY = 10.0; + public final double TURRET_MOTION_MAGIC_ACCELERATION = 10.0; + public final double TURRET_MOTION_MAGIC_JERK = 0.0; + + public final double TURRET_VOLTAGE_CLOSED_LOOP_RAMP_PERIOD = 0.0; + + public final double STATOR_CURRENT_LIMIT = 60.0; + public final double SUPPLY_CURRENT_LIMIT = 40.0; + + public final double PEAK_FORWARD_VOLTAGE = 12.0; + public final double PEAK_REVERSE_VOLTAGE = -12.0; + + public final double MANUAL_INPUT_TO_VOLTS = 12.0; + + public final double PRESET_ONE = 0.0; + public final double PRESET_TWO = 90.0; + public final double PRESET_THREE = -150.0; + } + public static class DriveInfo { public final int STATUS_SIGNAL_FREQUENCY = 200; diff --git a/src/main/java/com/team973/frc2025/subsystems/turret/Turret.java b/src/main/java/com/team973/frc2025/subsystems/turret/Turret.java new file mode 100644 index 0000000..2941ffa --- /dev/null +++ b/src/main/java/com/team973/frc2025/subsystems/turret/Turret.java @@ -0,0 +1,125 @@ +package com.team973.frc2025.subsystems.turret; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.team973.frc2025.shared.RobotInfo; +import com.team973.lib.devices.GreyTalonFX; +import com.team973.lib.util.Logger; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; + +public class Turret extends TurretIO { + protected static final RobotInfo.TurretInfo m_turretInfo = RobotInfo.TURRET_INFO; + + private final Logger m_logger; + + protected final GreyTalonFX m_motor; + + private double m_targetPostionDeg; + private double m_manualInput; + + public Turret(Logger logger) { + m_logger = logger; + m_motor = + new GreyTalonFX( + m_turretInfo.MOTOR_ID, RobotInfo.CANIVORE_CANBUS, m_logger.subLogger("motorRight")); + + TalonFXConfiguration motorConfig = new TalonFXConfiguration(); + + motorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + motorConfig.Slot0.kS = m_turretInfo.TURRET_KS; + motorConfig.Slot0.kV = m_turretInfo.TURRET_KV; + motorConfig.Slot0.kA = m_turretInfo.TURRET_KA; + motorConfig.Slot0.kP = m_turretInfo.TURRET_KP; + motorConfig.Slot0.kI = m_turretInfo.TURRET_KI; + motorConfig.Slot0.kD = m_turretInfo.TURRET_KD; + + motorConfig.MotionMagic.MotionMagicCruiseVelocity = + m_turretInfo.TURRET_MOTION_MAGIC_CRUISE_VELOCITY; + motorConfig.MotionMagic.MotionMagicAcceleration = m_turretInfo.TURRET_MOTION_MAGIC_ACCELERATION; + motorConfig.MotionMagic.MotionMagicJerk = m_turretInfo.TURRET_MOTION_MAGIC_JERK; + + motorConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = + m_turretInfo.TURRET_VOLTAGE_CLOSED_LOOP_RAMP_PERIOD; + + motorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + motorConfig.CurrentLimits.StatorCurrentLimit = m_turretInfo.STATOR_CURRENT_LIMIT; + motorConfig.CurrentLimits.StatorCurrentLimitEnable = true; + motorConfig.CurrentLimits.SupplyCurrentLimit = m_turretInfo.SUPPLY_CURRENT_LIMIT; + motorConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + + motorConfig.Voltage.PeakForwardVoltage = m_turretInfo.PEAK_FORWARD_VOLTAGE; + motorConfig.Voltage.PeakReverseVoltage = m_turretInfo.PEAK_REVERSE_VOLTAGE; + + m_motor.setConfig(motorConfig); + m_motor.setPosition(0.0); + + m_targetPostionDeg = Preset.One.getPositionDeg(); + m_manualInput = 0.0; + } + + @Override + public Pose3d getPose() { + return new Pose3d( + new Translation3d(), + new Rotation3d( + 0, 0, Math.toRadians(motorRotationsToDeg(m_motor.getPosition().getValueAsDouble())))); + } + + @Override + public GreyTalonFX getMotor() { + return m_motor; + } + + public double degToMotorRotations(double deg) { + return (deg / 360.0) * m_turretInfo.MOTOR_GEAR_RATIO; + } + + private double motorRotationsToDeg(double motorPostion) { + return (motorPostion / m_turretInfo.MOTOR_GEAR_RATIO) * 360.0; + } + + @Override + public void setTargetPreset(Preset preset) { + m_targetPostionDeg = preset.getPositionDeg(); + setState(State.ClosedLoop); + } + + @Override + public void setManualInput(double input) { + m_manualInput = input; + } + + @Override + public double getTargetPositionMotorRot() { + return degToMotorRotations(m_targetPostionDeg); + } + + @Override + public double getManualInput() { + return m_manualInput; + } + + @Override + public void syncSensors() {} + + @Override + public void log() { + double motorRot = m_motor.getPosition().getValueAsDouble(); + + m_motor.log(); + + m_logger.log("currentPostionDeg", motorRotationsToDeg(motorRot)); + m_logger.log("targetPostionDeg", m_targetPostionDeg); + m_logger.log("manualInput", m_manualInput); + + m_logger.log("state", getState().toString()); + } + + @Override + public void reset() {} +} diff --git a/src/main/java/com/team973/frc2025/subsystems/turret/TurretIO.java b/src/main/java/com/team973/frc2025/subsystems/turret/TurretIO.java new file mode 100644 index 0000000..7a860e0 --- /dev/null +++ b/src/main/java/com/team973/frc2025/subsystems/turret/TurretIO.java @@ -0,0 +1,63 @@ +package com.team973.frc2025.subsystems.turret; + +import com.team973.frc2025.shared.RobotInfo; +import com.team973.lib.devices.GreyTalonFX; +import com.team973.lib.util.StateMap; +import com.team973.lib.util.Subsystem; +import edu.wpi.first.math.geometry.Pose3d; + +public abstract class TurretIO extends Subsystem { + private final StateMap m_stateMap; + + protected static final RobotInfo.TurretInfo m_turretInfo = RobotInfo.TURRET_INFO; + + public enum State { + ClosedLoop, + Manual, + Off + } + + public enum Preset { + One(m_turretInfo.PRESET_ONE), + Two(m_turretInfo.PRESET_TWO), + Three(m_turretInfo.PRESET_THREE); + + private final double m_positionDeg; + + private Preset(double positionDeg) { + m_positionDeg = positionDeg; + } + + public double getPositionDeg() { + return m_positionDeg; + } + } + + @SuppressWarnings("unchecked") + public TurretIO() { + super(State.Off); + + m_stateMap = + new StateMap<>( + State.class, + new StateMap.Entry<>(State.ClosedLoop, new TurretStates.ClosedLoop(this)), + new StateMap.Entry<>(State.Manual, new TurretStates.Manual(this)), + new StateMap.Entry<>(State.Off, new TurretStates.Off(this))); + } + + public StateMap getStateMap() { + return m_stateMap; + } + + public abstract Pose3d getPose(); + + public abstract GreyTalonFX getMotor(); + + public abstract void setTargetPreset(Preset preset); + + public abstract void setManualInput(double volts); + + public abstract double getTargetPositionMotorRot(); + + public abstract double getManualInput(); +} diff --git a/src/main/java/com/team973/frc2025/subsystems/turret/TurretSim.java b/src/main/java/com/team973/frc2025/subsystems/turret/TurretSim.java new file mode 100644 index 0000000..75f353c --- /dev/null +++ b/src/main/java/com/team973/frc2025/subsystems/turret/TurretSim.java @@ -0,0 +1,62 @@ +package com.team973.frc2025.subsystems.turret; + +import com.ctre.phoenix6.sim.TalonFXSimState; +import com.team973.lib.util.Logger; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; + +public class TurretSim extends Turret { + private final SingleJointedArmSim m_sim; + private final TalonFXSimState m_motorSimState; + + private double m_lastPoseRad; + + public TurretSim(Logger logger) { + super(logger); + + m_sim = + new SingleJointedArmSim( + DCMotor.getKrakenX60(1), + m_turretInfo.MOTOR_GEAR_RATIO, + 0.02, + 0.0, + Math.toRadians(-165.0), + Math.toRadians(165.0), + false, + Math.toRadians(0.0)); + + m_motorSimState = m_motor.getSimState(); + m_lastPoseRad = m_sim.getAngleRads(); + } + + @Override + public Pose3d getPose() { + return new Pose3d(new Translation3d(), new Rotation3d(0, 0, m_sim.getAngleRads())); + } + + @Override + public void syncSensors() { + m_motorSimState.addRotorPosition( + (((m_sim.getAngleRads() - m_lastPoseRad) / (2 * Math.PI)) * m_turretInfo.MOTOR_GEAR_RATIO)); + m_lastPoseRad = m_sim.getAngleRads(); + + m_motorSimState.setRotorVelocity(m_sim.getVelocityRadPerSec()); + + m_motorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); + + super.syncSensors(); + } + + @Override + public void update() { + super.update(); + + m_sim.setInputVoltage(m_motorSimState.getMotorVoltage()); + + m_sim.update(0.02); + } +} diff --git a/src/main/java/com/team973/frc2025/subsystems/turret/TurretStates.java b/src/main/java/com/team973/frc2025/subsystems/turret/TurretStates.java new file mode 100644 index 0000000..324e713 --- /dev/null +++ b/src/main/java/com/team973/frc2025/subsystems/turret/TurretStates.java @@ -0,0 +1,63 @@ +package com.team973.frc2025.subsystems.turret; + +import com.team973.frc2025.shared.RobotInfo; +import com.team973.lib.devices.GreyTalonFX.ControlMode; +import com.team973.lib.util.SubsystemState; + +public class TurretStates { + private abstract static class TurretState implements SubsystemState { + protected final TurretIO m_turret; + + private TurretState(TurretIO turret) { + m_turret = turret; + } + } + + public static class ClosedLoop extends TurretState { + public ClosedLoop(TurretIO turret) { + super(turret); + } + + public void init() {} + + public void run() { + m_turret + .getMotor() + .setControl(ControlMode.MotionMagicVoltage, m_turret.getTargetPositionMotorRot()); + } + + public void exit() {} + } + + public static class Manual extends TurretState { + public Manual(TurretIO turret) { + super(turret); + } + + public void init() {} + + public void run() { + m_turret + .getMotor() + .setControl( + ControlMode.VoltageOut, + m_turret.getManualInput() * RobotInfo.TURRET_INFO.MANUAL_INPUT_TO_VOLTS); + } + + public void exit() {} + } + + public static class Off extends TurretState { + public Off(TurretIO turret) { + super(turret); + } + + public void init() {} + + public void run() { + m_turret.getMotor().setControl(ControlMode.DutyCycleOut, 0); + } + + public void exit() {} + } +} diff --git a/src/main/java/com/team973/lib/util/SubsystemManager.java b/src/main/java/com/team973/lib/util/SubsystemManager.java index 16ef22e..287c5f4 100644 --- a/src/main/java/com/team973/lib/util/SubsystemManager.java +++ b/src/main/java/com/team973/lib/util/SubsystemManager.java @@ -2,7 +2,9 @@ import com.team973.frc2025.Robot; import com.team973.frc2025.subsystems.DriveController; +import com.team973.frc2025.subsystems.turret.TurretIO; import com.team973.lib.devices.GreyPigeonIO; +import edu.wpi.first.math.geometry.Pose3d; public abstract class SubsystemManager { private final Logger m_logger; @@ -27,5 +29,9 @@ public static SubsystemManager init(Logger logger) { public abstract DriveController getDriveController(); - public void log() {} + public abstract TurretIO getTurret(); + + public void log() { + m_logger.log("components", new Pose3d[] {getTurret().getPose()}); + } } diff --git a/src/main/java/com/team973/lib/util/SubsystemManagerReal.java b/src/main/java/com/team973/lib/util/SubsystemManagerReal.java index 8dd2b0c..78f3560 100644 --- a/src/main/java/com/team973/lib/util/SubsystemManagerReal.java +++ b/src/main/java/com/team973/lib/util/SubsystemManagerReal.java @@ -3,12 +3,15 @@ import com.team973.frc2025.shared.RobotInfo; import com.team973.frc2025.subsystems.DriveController; import com.team973.frc2025.subsystems.swerve.SwerveModule; +import com.team973.frc2025.subsystems.turret.Turret; +import com.team973.frc2025.subsystems.turret.TurretIO; import com.team973.lib.devices.GreyPigeon; import com.team973.lib.devices.GreyPigeonIO; public class SubsystemManagerReal extends SubsystemManager { private final GreyPigeonIO m_pigeon; private final DriveController m_driveController; + private final TurretIO m_turret; public SubsystemManagerReal(Logger logger) { super(logger); @@ -33,6 +36,8 @@ public SubsystemManagerReal(Logger logger) { new SwerveModule( 3, RobotInfo.DRIVE_INFO.BACK_RIGHT_CONSTANTS, driveLogger.subLogger("swerve/mod3")), m_pigeon); + + m_turret = new Turret(logger.subLogger("turret")); } public GreyPigeonIO getPigeon() { @@ -42,4 +47,8 @@ public GreyPigeonIO getPigeon() { public DriveController getDriveController() { return m_driveController; } + + public TurretIO getTurret() { + return m_turret; + } } diff --git a/src/main/java/com/team973/lib/util/SubsystemManagerSim.java b/src/main/java/com/team973/lib/util/SubsystemManagerSim.java index 1d22df3..e2fb67b 100644 --- a/src/main/java/com/team973/lib/util/SubsystemManagerSim.java +++ b/src/main/java/com/team973/lib/util/SubsystemManagerSim.java @@ -3,6 +3,8 @@ import com.team973.frc2025.shared.RobotInfo; import com.team973.frc2025.subsystems.DriveController; import com.team973.frc2025.subsystems.swerve.SwerveModuleSim; +import com.team973.frc2025.subsystems.turret.TurretIO; +import com.team973.frc2025.subsystems.turret.TurretSim; import com.team973.lib.devices.GreyPigeonIO; import com.team973.lib.devices.GreyPigeonSim; import org.ironmaple.simulation.SimulatedArena; @@ -12,6 +14,7 @@ public class SubsystemManagerSim extends SubsystemManager { private final SwerveDriveSimulation m_swerveDriveSimulation; private final GreyPigeonIO m_pigeon; private final DriveController m_driveController; + private final TurretIO m_turret; public SubsystemManagerSim(Logger logger) { super(logger); @@ -56,6 +59,8 @@ public SubsystemManagerSim(Logger logger) { RobotInfo.DRIVE_INFO.BACK_RIGHT_CONSTANTS, driveLogger.subLogger("swerve/mod3")), m_pigeon); + + m_turret = new TurretSim(logger.subLogger("turret")); } public GreyPigeonIO getPigeon() { @@ -66,6 +71,10 @@ public DriveController getDriveController() { return m_driveController; } + public TurretIO getTurret() { + return m_turret; + } + @Override public void log() { super.log();