From ca5b59c0cbe4238acfb580a3816002515816e81a Mon Sep 17 00:00:00 2001 From: Ishan1522 Date: Fri, 20 Dec 2024 17:57:42 -0500 Subject: [PATCH] Fun arm things --- .../java/frc/robot/subsystems/arm/Arm.java | 12 ++++ .../robot/subsystems/arm/ArmConstants.java | 15 +++++ .../robot/subsystems/arm/ArmInterface.java | 20 +++++++ .../frc/robot/subsystems/arm/PhysicalArm.java | 31 ++++++++++ .../subsystems/arm/SimulatedElevator.java | 21 +++++++ .../robot/subsystems/elevator/Elevator.java | 3 + .../elevator/ElevatorConstants.java | 3 + .../elevator/ElevatorInterface.java | 8 +++ .../subsystems/elevator/PhysicalElevator.java | 4 +- .../elevator/SimulatedElevator.java | 58 +++++++++---------- 10 files changed, 144 insertions(+), 31 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/arm/Arm.java create mode 100644 src/main/java/frc/robot/subsystems/arm/ArmConstants.java create mode 100644 src/main/java/frc/robot/subsystems/arm/ArmInterface.java create mode 100644 src/main/java/frc/robot/subsystems/arm/PhysicalArm.java create mode 100644 src/main/java/frc/robot/subsystems/arm/SimulatedElevator.java diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java new file mode 100644 index 0000000..6dfec1f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -0,0 +1,12 @@ +package frc.robot.subsystems.arm; + +import org.littletonrobotics.junction.AutoLog; + +public class Arm { + private ArmInterface armInterface; + private ArmInputsAutoLogged input = new ArmInputsAutoLogged(); + + public Arm(ArmInterface armInterface) { + this.armInterface = armInterface; + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java new file mode 100644 index 0000000..78dedfb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -0,0 +1,15 @@ +package frc.robot.subsystems.arm; + +public class ArmConstants { + public static final int ARM_MOTOR_ID = 0 - 9; + + public static final double ARM_P = 0 - 9; + public static final double ARM_I = 0 - 9; + public static final double ARM_D = 0 - 9; + + public static final double ARM_GEAR_RATIO = 0 - 9; + public static final double ARM_INERTIA_MASS = 0 - 9; + public static final double ARM_LENGTH_METERS = 0 - 9; + public static final double MINIMUM_ANGLE_RADIANS = 0 - 9; + public static final double MAXIMUM_ANGLE_RADIANS = 0 - 9; +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmInterface.java b/src/main/java/frc/robot/subsystems/arm/ArmInterface.java new file mode 100644 index 0000000..0f61537 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmInterface.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.arm; + +import org.littletonrobotics.junction.AutoLog; + +public interface ArmInterface { + + @AutoLog + public static class ArmInputs { + public double angle = 0.0; + } + + public default void updateInputs(ArmInputs inputs) {} + + public default void setAngle(double angle) {} + + public default double getAngle() { + return 0.0; + } +} + \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/arm/PhysicalArm.java b/src/main/java/frc/robot/subsystems/arm/PhysicalArm.java new file mode 100644 index 0000000..214fe02 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/PhysicalArm.java @@ -0,0 +1,31 @@ +// 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.arm; + +import org.littletonrobotics.junction.AutoLog; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +public class PhysicalArm implements ArmInterface { + private TalonFX armMotor = new TalonFX(ArmConstants.ARM_MOTOR_ID); + + public PhysicalArm() { + TalonFXConfiguration armConfig = new TalonFXConfiguration(); + + armConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + armConfig.Slot0.kP = ArmConstants.ARM_P; + armConfig.Slot0.kI = ArmConstants.ARM_I; + armConfig.Slot0.kD = ArmConstants.ARM_D; + + armMotor.getConfigurator().apply(armConfig); + } + + @Override + public void setAngle(double angle) { + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/SimulatedElevator.java b/src/main/java/frc/robot/subsystems/arm/SimulatedElevator.java new file mode 100644 index 0000000..a87ad84 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/SimulatedElevator.java @@ -0,0 +1,21 @@ +// 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.arm; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; + +/** Add your docs here. */ +public class SimulatedElevator implements ArmInterface { + private SingleJointedArmSim armSim = new SingleJointedArmSim(DCMotor.getFalcon500(1), ArmConstants.ARM_GEAR_RATIO, ArmConstants.ARM_INERTIA_MASS, ArmConstants.ARM_LENGTH_METERS, ArmConstants.MINIMUM_ANGLE_RADIANS, ArmConstants.MAXIMUM_ANGLE_RADIANS, false, 0, null); + private PIDController pidSim; + + public SimulatedElevator() { + pidSim = new PIDController(ArmConstants.ARM_P, ArmConstants.ARM_I, ArmConstants.ARM_D); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index a894a76..a01530d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -1,3 +1,6 @@ +/** + * This subsystem is an elevator that uses PID for its position. + */ package frc.robot.subsystems.elevator; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index 5856e6c..c3f8da2 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -1,6 +1,9 @@ package frc.robot.subsystems.elevator; public class ElevatorConstants { + public static final int ELEVATOR_LEADER_MOTOR_ID = 0 - 9; + public static final int ELEVATOR_FOLLOWER_MOTOR_ID = 0 - 9; + public static final double ELEVATOR_P = 0 - 9; public static final double ELEVATOR_I = 0 - 9; public static final double ELEVATOR_D = 0 - 9; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java index e7bb047..d68dd27 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java @@ -11,8 +11,16 @@ public static class ElevatorInputs { public double followerMotorPosition = 0.0; } + /** + * Updates inputs for elevator for AdvantageKit to log + * @param inputs values related to the elevator + */ public default void updateInputs(ElevatorInputs inputs) {} + /** + * Gets the current position of the elevator + * @return + */ public default double getElevatorPosition() { return 0.0; } diff --git a/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java b/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java index 541f450..c83160d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java @@ -12,8 +12,8 @@ /** Add your docs here. */ public class PhysicalElevator { - private TalonFX leaderMotor = new TalonFX(0); - private TalonFX followerMotor = new TalonFX(0); + private TalonFX leaderMotor = new TalonFX(ElevatorConstants.ELEVATOR_LEADER_MOTOR_ID); + private TalonFX followerMotor = new TalonFX(ElevatorConstants.ELEVATOR_FOLLOWER_MOTOR_ID); StatusSignal leaderPosition; StatusSignal followerPosition; diff --git a/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java b/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java index a9d0384..734c0af 100644 --- a/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java @@ -6,33 +6,33 @@ /** Add your docs here. */ public class SimulatedElevator implements ElevatorInterface { - private ElevatorSim simulatedElevator = new ElevatorSim(DCMotor.getFalcon500(2), ElevatorConstants.ELEVATOR_GEAR_RATIO, ElevatorConstants.ELEVATOR_CARRIAGE_MASS, ElevatorConstants.DRUM_RADIUS, ElevatorConstants.MIN_HEIGHT, ElevatorConstants.MAX_HEIGHT, true, 0.0); - private PIDController simPID; - private double currentVolts; - - public SimulatedElevator() { - simPID = new PIDController(ElevatorConstants.ELEVATOR_P, ElevatorConstants.ELEVATOR_I, ElevatorConstants.ELEVATOR_D); - } - - public void updateInputs(ElevatorInputs inputs) { - inputs.leaderMotorPosition = getElevatorPosition(); - inputs.followerMotorPosition = getElevatorPosition(); - } - - public void setElevatorPosition(double position) { - simulatedElevator.setState(simPID.calculate(position), ElevatorConstants.VELOCITY_METERS_PER_SECOND); - } - - public double getElevatorPosition() { - return simulatedElevator.getPositionMeters(); - } - - public void setVolts(double volts) { - currentVolts = simPID.calculate(volts); - simulatedElevator.setInputVoltage(currentVolts); - } - - public double getVolts() { - return currentVolts; - } + private ElevatorSim elevatorSim = new ElevatorSim(DCMotor.getFalcon500(2), ElevatorConstants.ELEVATOR_GEAR_RATIO, ElevatorConstants.ELEVATOR_CARRIAGE_MASS, ElevatorConstants.DRUM_RADIUS, ElevatorConstants.MIN_HEIGHT, ElevatorConstants.MAX_HEIGHT, true, 0.0); + private PIDController simPID; + private double currentVolts; + + public SimulatedElevator() { + simPID = new PIDController(ElevatorConstants.ELEVATOR_P, ElevatorConstants.ELEVATOR_I, ElevatorConstants.ELEVATOR_D); + } + + public void updateInputs(ElevatorInputs inputs) { + inputs.leaderMotorPosition = getElevatorPosition(); + inputs.followerMotorPosition = getElevatorPosition(); + } + + public void setElevatorPosition(double position) { + elevatorSim.setState(simPID.calculate(position), ElevatorConstants.VELOCITY_METERS_PER_SECOND); + } + + public double getElevatorPosition() { + return elevatorSim.getPositionMeters(); + } + + public void setVolts(double volts) { + currentVolts = simPID.calculate(volts); + elevatorSim.setInputVoltage(currentVolts); + } + + public double getVolts() { + return currentVolts; + } }