diff --git a/assets/Robot_Potato/config.json b/assets/Robot_Potato/config.json new file mode 100644 index 0000000..130f5dd --- /dev/null +++ b/assets/Robot_Potato/config.json @@ -0,0 +1,34 @@ +{ + "name": "Potato", + "rotations": [ + { + "axis": "x", + "degrees": 90 + } + ], + "position": [ + 0, + 0, + 0 + ], + "cameras": [], + "components": [ + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "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..76f7848 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..ab6db94 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..e726275 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.arm.ArmIO; import com.team973.lib.devices.GreyPigeonIO; import com.team973.lib.util.AllianceCache; import com.team973.lib.util.Joystick; @@ -21,29 +22,37 @@ public class Robot extends TimedRobot { private final GreyPigeonIO m_pigeon = m_subsystemManager.getPigeon(); private final DriveController m_driveController = m_subsystemManager.getDriveController(); + private final ArmIO m_arm = m_subsystemManager.getArm(); 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_arm.syncSensors(); } private void updateSubsystems() { m_driveController.update(); + m_arm.update(); } private void resetSubsystems() { m_driveController.reset(); + m_arm.reset(); } private void log() { m_subsystemManager.log(); m_driveController.log(); + m_arm.log(); } private void updateJoysticks() { m_driverStick.update(); + m_coDriverStick.update(); } public Robot() { @@ -53,6 +62,7 @@ public Robot() { @Override public void robotPeriodic() { + syncSensors(); log(); updateJoysticks(); } @@ -71,8 +81,6 @@ public void teleopInit() { @Override public void teleopPeriodic() { - syncSensors(); - double allianceScalar = 1.0; if (AllianceCache.Get().get() == Alliance.Red) { // Our gyroscope is blue-centric meaning that facing away from the alliance wall @@ -95,6 +103,18 @@ public void teleopPeriodic() { allianceScalar * m_driverStick.getLeftXAxis() * 0.7, m_driverStick.getRightXAxis() * 0.8); + if (m_coDriverStick.getAButtonPressed()) { + m_arm.setTargetPreset(ArmIO.Preset.One); + } else if (m_coDriverStick.getBButtonPressed()) { + m_arm.setTargetPreset(ArmIO.Preset.Two); + } else if (m_coDriverStick.getXButtonPressed()) { + m_arm.setTargetPreset(ArmIO.Preset.Three); + } else if (m_coDriverStick.getYButtonPressed()) { + m_arm.setState(ArmIO.State.Manual); + } + + m_arm.setManualInput(m_coDriverStick.getLeftYAxis()); + updateSubsystems(); } @@ -102,9 +122,7 @@ public void teleopPeriodic() { public void disabledInit() {} @Override - public void disabledPeriodic() { - syncSensors(); - } + public void disabledPeriodic() {} @Override public void testInit() {} diff --git a/src/main/java/com/team973/frc2025/shared/RobotInfo.java b/src/main/java/com/team973/frc2025/shared/RobotInfo.java index ed18f58..6922af1 100644 --- a/src/main/java/com/team973/frc2025/shared/RobotInfo.java +++ b/src/main/java/com/team973/frc2025/shared/RobotInfo.java @@ -18,6 +18,40 @@ public class RobotInfo { public static final DriveInfo DRIVE_INFO = new DriveInfo(); + public static final ArmInfo ARM_INFO = new ArmInfo(); + + public static class ArmInfo { + public final int MOTOR_ID = 20; + + public final double MOTOR_GEAR_RATIO = (10.0 / 50.0) * (20.0 / 68.0) * (17.0 / 84.0); + public final double LENGTH_METERS = 0.5461; + + public final double ARM_KS = 0.0; + public final double ARM_KV = 0.0; + public final double ARM_KA = 0.0; + public final double ARM_KP = 2.0; + public final double ARM_KI = 0.0; + public final double ARM_KD = 0.0; + + public final double ARM_MOTION_MAGIC_CRUISE_VELOCITY = 10.0; + public final double ARM_MOTION_MAGIC_ACCELERATION = 10.0; + public final double ARM_MOTION_MAGIC_JERK = 0.0; + + public final double ARM_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 = 60.0; + public final double PRESET_THREE = -60.0; + } + public static class DriveInfo { public final int STATUS_SIGNAL_FREQUENCY = 200; diff --git a/src/main/java/com/team973/frc2025/subsystems/arm/Arm.java b/src/main/java/com/team973/frc2025/subsystems/arm/Arm.java new file mode 100644 index 0000000..0e05f09 --- /dev/null +++ b/src/main/java/com/team973/frc2025/subsystems/arm/Arm.java @@ -0,0 +1,123 @@ +package com.team973.frc2025.subsystems.arm; + +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 Arm extends ArmIO { + protected static final RobotInfo.ArmInfo m_armInfo = RobotInfo.ARM_INFO; + + protected final Logger m_logger; + + protected final GreyTalonFX m_motor; + + private double m_targetPostionDeg; + private double m_manualInput; + + public Arm(Logger logger) { + m_logger = logger; + m_motor = + new GreyTalonFX( + m_armInfo.MOTOR_ID, RobotInfo.CANIVORE_CANBUS, m_logger.subLogger("motorRight")); + + TalonFXConfiguration motorConfig = new TalonFXConfiguration(); + + motorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + motorConfig.Slot0.kS = m_armInfo.ARM_KS; + motorConfig.Slot0.kV = m_armInfo.ARM_KV; + motorConfig.Slot0.kA = m_armInfo.ARM_KA; + motorConfig.Slot0.kP = m_armInfo.ARM_KP; + motorConfig.Slot0.kI = m_armInfo.ARM_KI; + motorConfig.Slot0.kD = m_armInfo.ARM_KD; + + motorConfig.MotionMagic.MotionMagicCruiseVelocity = m_armInfo.ARM_MOTION_MAGIC_CRUISE_VELOCITY; + motorConfig.MotionMagic.MotionMagicAcceleration = m_armInfo.ARM_MOTION_MAGIC_ACCELERATION; + motorConfig.MotionMagic.MotionMagicJerk = m_armInfo.ARM_MOTION_MAGIC_JERK; + + motorConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = + m_armInfo.ARM_VOLTAGE_CLOSED_LOOP_RAMP_PERIOD; + + motorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + motorConfig.CurrentLimits.StatorCurrentLimit = m_armInfo.STATOR_CURRENT_LIMIT; + motorConfig.CurrentLimits.StatorCurrentLimitEnable = true; + motorConfig.CurrentLimits.SupplyCurrentLimit = m_armInfo.SUPPLY_CURRENT_LIMIT; + motorConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + + motorConfig.Voltage.PeakForwardVoltage = m_armInfo.PEAK_FORWARD_VOLTAGE; + motorConfig.Voltage.PeakReverseVoltage = m_armInfo.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; + } + + private double degToMotorRotations(double deg) { + return (deg / 360.0) / m_armInfo.MOTOR_GEAR_RATIO; + } + + private double motorRotationsToDeg(double motorPostion) { + return (motorPostion * m_armInfo.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() { + m_motor.log(); + + m_logger.log( + "currentPostionDeg", motorRotationsToDeg(m_motor.getPosition().getValueAsDouble())); + 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/arm/ArmIO.java b/src/main/java/com/team973/frc2025/subsystems/arm/ArmIO.java new file mode 100644 index 0000000..d440e07 --- /dev/null +++ b/src/main/java/com/team973/frc2025/subsystems/arm/ArmIO.java @@ -0,0 +1,63 @@ +package com.team973.frc2025.subsystems.arm; + +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 ArmIO extends Subsystem { + private final StateMap m_stateMap; + + protected static final RobotInfo.ArmInfo m_armInfo = RobotInfo.ARM_INFO; + + public enum State { + ClosedLoop, + Manual, + Off + } + + public enum Preset { + One(m_armInfo.PRESET_ONE), + Two(m_armInfo.PRESET_TWO), + Three(m_armInfo.PRESET_THREE); + + private final double m_positionDeg; + + private Preset(double positionDeg) { + m_positionDeg = positionDeg; + } + + public double getPositionDeg() { + return m_positionDeg; + } + } + + @SuppressWarnings("unchecked") + public ArmIO() { + super(State.Off); + + m_stateMap = + new StateMap<>( + State.class, + new StateMap.Entry<>(State.ClosedLoop, new ArmStates.ClosedLoop(this)), + new StateMap.Entry<>(State.Manual, new ArmStates.Manual(this)), + new StateMap.Entry<>(State.Off, new ArmStates.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/arm/ArmSim.java b/src/main/java/com/team973/frc2025/subsystems/arm/ArmSim.java new file mode 100644 index 0000000..3f09a51 --- /dev/null +++ b/src/main/java/com/team973/frc2025/subsystems/arm/ArmSim.java @@ -0,0 +1,84 @@ +package com.team973.frc2025.subsystems.arm; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; + +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.system.plant.DCMotor; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; + +public class ArmSim extends Arm { + private final SingleJointedArmSim m_sim; + private final TalonFXSimState m_motorSimState; + + private final Angle initialAngle; + private Angle previousAngle; + + public ArmSim(Logger logger) { + super(logger); + + initialAngle = Degrees.of(90); + previousAngle = initialAngle; + + DCMotor krakenX44 = new DCMotor(18.0, 4.05, 275, 1.4, 7530 * 2.0 * Math.PI / 60.0, 2); + + m_sim = + new SingleJointedArmSim( + krakenX44, + 1.0 / m_armInfo.MOTOR_GEAR_RATIO, + 0.2, + m_armInfo.LENGTH_METERS, + Math.toRadians(-15.0), + Math.toRadians(195.0), + true, + initialAngle.in(Radians)); + + m_motorSimState = m_motor.getSimState(); + } + + @Override + public Pose3d getPose() { + return new Pose3d(0, -0.05, 0.3, new Rotation3d(-m_sim.getAngleRads(), 0, 0)); + } + + @Override + public void log() { + super.log(); + + m_logger.log("simAngleRads", m_sim.getAngleRads()); + } + + @Override + public void syncSensors() { + Angle armAngle = Radians.of(m_sim.getAngleRads()); + AngularVelocity armAngularVelocity = RadiansPerSecond.of(m_sim.getVelocityRadPerSec()); + + Angle diffAngle = armAngle.minus(previousAngle); + previousAngle = armAngle; + + Angle rotorDiffAngle = diffAngle.div(m_armInfo.MOTOR_GEAR_RATIO); + AngularVelocity rotorAngularVelocity = armAngularVelocity.div(m_armInfo.MOTOR_GEAR_RATIO); + + m_motorSimState.addRotorPosition(rotorDiffAngle); + m_motorSimState.setRotorVelocity(rotorAngularVelocity); + m_motorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); + + m_sim.update(0.02); + + super.syncSensors(); + } + + @Override + public void update() { + super.update(); + + m_sim.setInputVoltage(m_motorSimState.getMotorVoltage()); + } +} diff --git a/src/main/java/com/team973/frc2025/subsystems/arm/ArmStates.java b/src/main/java/com/team973/frc2025/subsystems/arm/ArmStates.java new file mode 100644 index 0000000..51926fc --- /dev/null +++ b/src/main/java/com/team973/frc2025/subsystems/arm/ArmStates.java @@ -0,0 +1,63 @@ +package com.team973.frc2025.subsystems.arm; + +import com.team973.frc2025.shared.RobotInfo; +import com.team973.lib.devices.GreyTalonFX.ControlMode; +import com.team973.lib.util.SubsystemState; + +public class ArmStates { + private abstract static class ArmState implements SubsystemState { + protected final ArmIO m_arm; + + private ArmState(ArmIO arm) { + m_arm = arm; + } + } + + public static class ClosedLoop extends ArmState { + public ClosedLoop(ArmIO arm) { + super(arm); + } + + public void init() {} + + public void run() { + m_arm + .getMotor() + .setControl(ControlMode.MotionMagicVoltage, m_arm.getTargetPositionMotorRot()); + } + + public void exit() {} + } + + public static class Manual extends ArmState { + public Manual(ArmIO arm) { + super(arm); + } + + public void init() {} + + public void run() { + m_arm + .getMotor() + .setControl( + ControlMode.VoltageOut, + m_arm.getManualInput() * RobotInfo.ARM_INFO.MANUAL_INPUT_TO_VOLTS); + } + + public void exit() {} + } + + public static class Off extends ArmState { + public Off(ArmIO turret) { + super(turret); + } + + public void init() {} + + public void run() { + m_arm.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..6554c83 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.arm.ArmIO; 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 ArmIO getArm(); + + public void log() { + m_logger.log("components", new Pose3d[] {getArm().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..4d3e9cc 100644 --- a/src/main/java/com/team973/lib/util/SubsystemManagerReal.java +++ b/src/main/java/com/team973/lib/util/SubsystemManagerReal.java @@ -2,6 +2,8 @@ import com.team973.frc2025.shared.RobotInfo; import com.team973.frc2025.subsystems.DriveController; +import com.team973.frc2025.subsystems.arm.Arm; +import com.team973.frc2025.subsystems.arm.ArmIO; import com.team973.frc2025.subsystems.swerve.SwerveModule; import com.team973.lib.devices.GreyPigeon; import com.team973.lib.devices.GreyPigeonIO; @@ -9,6 +11,7 @@ public class SubsystemManagerReal extends SubsystemManager { private final GreyPigeonIO m_pigeon; private final DriveController m_driveController; + private final ArmIO m_arm; 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_arm = new Arm(logger.subLogger("arm")); } public GreyPigeonIO getPigeon() { @@ -42,4 +47,8 @@ public GreyPigeonIO getPigeon() { public DriveController getDriveController() { return m_driveController; } + + public ArmIO getArm() { + return m_arm; + } } diff --git a/src/main/java/com/team973/lib/util/SubsystemManagerSim.java b/src/main/java/com/team973/lib/util/SubsystemManagerSim.java index 1d22df3..037cdcf 100644 --- a/src/main/java/com/team973/lib/util/SubsystemManagerSim.java +++ b/src/main/java/com/team973/lib/util/SubsystemManagerSim.java @@ -2,6 +2,8 @@ import com.team973.frc2025.shared.RobotInfo; import com.team973.frc2025.subsystems.DriveController; +import com.team973.frc2025.subsystems.arm.ArmIO; +import com.team973.frc2025.subsystems.arm.ArmSim; import com.team973.frc2025.subsystems.swerve.SwerveModuleSim; import com.team973.lib.devices.GreyPigeonIO; import com.team973.lib.devices.GreyPigeonSim; @@ -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 ArmIO m_arm; 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_arm = new ArmSim(logger.subLogger("arm")); } public GreyPigeonIO getPigeon() { @@ -66,6 +71,10 @@ public DriveController getDriveController() { return m_driveController; } + public ArmIO getArm() { + return m_arm; + } + @Override public void log() { super.log();