From 47b46bfe4d95e59305d16f714869f6ca74db9449 Mon Sep 17 00:00:00 2001 From: NolanKnight Date: Thu, 18 Dec 2025 20:36:04 -0800 Subject: [PATCH 1/5] elevator subsystem WIP --- .../com/team973/frc2025/shared/RobotInfo.java | 27 +++++ .../frc2025/subsystems/elevator/Elevator.java | 114 ++++++++++++++++++ .../subsystems/elevator/ElevatorIO.java | 40 ++++++ .../subsystems/elevator/ElevatorSim.java | 60 +++++++++ .../subsystems/elevator/ElevatorStates.java | 56 +++++++++ 5 files changed, 297 insertions(+) create mode 100644 src/main/java/com/team973/frc2025/subsystems/elevator/Elevator.java create mode 100644 src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorIO.java create mode 100644 src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorSim.java create mode 100644 src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorStates.java diff --git a/src/main/java/com/team973/frc2025/shared/RobotInfo.java b/src/main/java/com/team973/frc2025/shared/RobotInfo.java index ed18f58..7c92094 100644 --- a/src/main/java/com/team973/frc2025/shared/RobotInfo.java +++ b/src/main/java/com/team973/frc2025/shared/RobotInfo.java @@ -17,6 +17,33 @@ public class RobotInfo { public static final String ROBORIO_CANBUS = ""; public static final DriveInfo DRIVE_INFO = new DriveInfo(); + public static final ElevatorInfo ELEVATOR_INFO = new ElevatorInfo(); + + public static class ElevatorInfo { + public final int MOTOR_ID = 20; + + public final double MOTOR_GEAR_RATIO = 10.0 / 56.0; + public final double MOTOR_ROT_TO_HEIGHT_METERS = MOTOR_GEAR_RATIO * 5.0 * 36.0 * 100.0; + + public final double ELEVATOR_KS = 0.0; + public final double ELEVATOR_KV = 0.15; + public final double ELEVATOR_KA = 0.01; + public final double ELEVATOR_KP = 4.0; + public final double ELEVATOR_KI = 0.0; + public final double ELEVATOR_KD = 0.0; + + public final double ELEVATOR_MOTION_MAGIC_CRUISE_VELOCITY = 65.0; + public final double ELEVATOR_MOTION_MAGIC_ACCELERATION = 390.0; + public final double ELEVATOR_MOTION_MAGIC_JERK = 2000.0; + + public final double ELEVATOR_VOLTAGE_CLOSED_LOOP_RAMP_PERIOD = 0.00; + + 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 static class DriveInfo { public final int STATUS_SIGNAL_FREQUENCY = 200; diff --git a/src/main/java/com/team973/frc2025/subsystems/elevator/Elevator.java b/src/main/java/com/team973/frc2025/subsystems/elevator/Elevator.java new file mode 100644 index 0000000..221e179 --- /dev/null +++ b/src/main/java/com/team973/frc2025/subsystems/elevator/Elevator.java @@ -0,0 +1,114 @@ +package com.team973.frc2025.subsystems.elevator; + +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.Conversions; +import com.team973.lib.util.Logger; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; + +public class Elevator extends ElevatorIO { + private final RobotInfo.ElevatorInfo m_elevatorInfo; + + private final Logger m_logger; + + protected final GreyTalonFX m_motor; + + private double m_targetPostionHeightMeters; + + public Elevator(Logger logger) { + m_logger = logger; + m_elevatorInfo = RobotInfo.ELEVATOR_INFO; + m_motor = + new GreyTalonFX( + m_elevatorInfo.MOTOR_ID, RobotInfo.CANIVORE_CANBUS, m_logger.subLogger("motorRight")); + + TalonFXConfiguration motorConfig = new TalonFXConfiguration(); + + motorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + motorConfig.Slot0.kS = m_elevatorInfo.ELEVATOR_KS; + motorConfig.Slot0.kV = m_elevatorInfo.ELEVATOR_KV; + motorConfig.Slot0.kA = m_elevatorInfo.ELEVATOR_KA; + motorConfig.Slot0.kP = m_elevatorInfo.ELEVATOR_KP; + motorConfig.Slot0.kI = m_elevatorInfo.ELEVATOR_KI; + motorConfig.Slot0.kD = m_elevatorInfo.ELEVATOR_KD; + + motorConfig.MotionMagic.MotionMagicCruiseVelocity = + m_elevatorInfo.ELEVATOR_MOTION_MAGIC_CRUISE_VELOCITY; + motorConfig.MotionMagic.MotionMagicAcceleration = + m_elevatorInfo.ELEVATOR_MOTION_MAGIC_ACCELERATION; + motorConfig.MotionMagic.MotionMagicJerk = m_elevatorInfo.ELEVATOR_MOTION_MAGIC_JERK; + + motorConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = + m_elevatorInfo.ELEVATOR_VOLTAGE_CLOSED_LOOP_RAMP_PERIOD; + + motorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + motorConfig.CurrentLimits.StatorCurrentLimit = m_elevatorInfo.STATOR_CURRENT_LIMIT; + motorConfig.CurrentLimits.StatorCurrentLimitEnable = true; + motorConfig.CurrentLimits.SupplyCurrentLimit = m_elevatorInfo.SUPPLY_CURRENT_LIMIT; + motorConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + + motorConfig.Voltage.PeakForwardVoltage = m_elevatorInfo.PEAK_FORWARD_VOLTAGE; + motorConfig.Voltage.PeakReverseVoltage = m_elevatorInfo.PEAK_REVERSE_VOLTAGE; + + m_motor.setConfig(motorConfig); + m_motor.setPosition(0.0); + } + + @Override + public Pose3d getPose() { + return new Pose3d( + 0, + 0, + motorRotationsToHeightMeters(m_motor.getPosition().getValueAsDouble()) + * Conversions.Distance.METERS_PER_INCH, + new Rotation3d()); + } + + @Override + public GreyTalonFX getMotor() { + return m_motor; + } + + public double heightMetersToMotorRotations(double postionHeight) { + return postionHeight / m_elevatorInfo.MOTOR_ROT_TO_HEIGHT_METERS; + } + + private double motorRotationsToHeightMeters(double motorPostion) { + return motorPostion * m_elevatorInfo.MOTOR_ROT_TO_HEIGHT_METERS; + } + + @Override + public void setTargetPostion(double targetPostionHeightMeters) { + m_targetPostionHeightMeters = targetPostionHeightMeters; + setState(State.ClosedLoop); + } + + @Override + public double getTargetPositionMotorRot() { + return heightMetersToMotorRotations(m_targetPostionHeightMeters); + } + + @Override + public void syncSensors() {} + + @Override + public void log() { + double motorRot = m_motor.getPosition().getValueAsDouble(); + + m_motor.log(); + + m_logger.log("currentPostionHeightInches", motorRotationsToHeightMeters(motorRot)); + m_logger.log("targetPostionHeightInches", m_targetPostionHeightMeters); + + m_logger.log("state", getState().toString()); + } + + @Override + public void reset() {} +} diff --git a/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorIO.java b/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorIO.java new file mode 100644 index 0000000..efa247c --- /dev/null +++ b/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorIO.java @@ -0,0 +1,40 @@ +package com.team973.frc2025.subsystems.elevator; + +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 ElevatorIO extends Subsystem { + private final StateMap m_stateMap; + + public enum State { + ClosedLoop, + Manual, + Off + } + + @SuppressWarnings("unchecked") + public ElevatorIO() { + super(State.Off); + + m_stateMap = + new StateMap<>( + State.class, + new StateMap.Entry<>(State.ClosedLoop, new ElevatorStates.ClosedLoop(this)), + new StateMap.Entry<>(State.Manual, new ElevatorStates.Manual(this)), + new StateMap.Entry<>(State.Off, new ElevatorStates.Off(this))); + } + + public StateMap getStateMap() { + return m_stateMap; + } + + public abstract Pose3d getPose(); + + public abstract GreyTalonFX getMotor(); + + public abstract void setTargetPostion(double targetPostionHeightinches); + + public abstract double getTargetPositionMotorRot(); +} diff --git a/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorSim.java b/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorSim.java new file mode 100644 index 0000000..4e294de --- /dev/null +++ b/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorSim.java @@ -0,0 +1,60 @@ +package com.team973.frc2025.subsystems.elevator; + +import com.ctre.phoenix6.sim.TalonFXSimState; +import com.team973.frc2025.shared.RobotInfo; +import com.team973.lib.util.Conversions; +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; + +public class ElevatorSim extends Elevator { + private final edu.wpi.first.wpilibj.simulation.ElevatorSim m_sim; + private final TalonFXSimState m_motorSimState; + + private final double m_motorRotToElevatorHeightMeters; + private double m_lastPoseMeters; + + public ElevatorSim(Logger logger) { + super(logger); + + m_motorRotToElevatorHeightMeters = RobotInfo.ELEVATOR_INFO.MOTOR_ROT_TO_HEIGHT_METERS; + + m_sim = + new edu.wpi.first.wpilibj.simulation.ElevatorSim( + DCMotor.getKrakenX60(1), + 1.0 / RobotInfo.ELEVATOR_INFO.MOTOR_GEAR_RATIO, + 5.0, + 0.029, + 0.0, + Conversions.Distance.METERS_PER_INCH * 30.0, + true, + 0.0); + + m_motorSimState = m_motor.getSimState(); + m_lastPoseMeters = m_sim.getPositionMeters(); + } + + @Override + public Pose3d getPose() { + return new Pose3d(0, 0, m_sim.getPositionMeters(), new Rotation3d()); + } + + @Override + public void syncSensors() { + m_motorSimState.addRotorPosition( + (m_sim.getPositionMeters() - m_lastPoseMeters) / m_motorRotToElevatorHeightMeters); + m_lastPoseMeters = m_sim.getPositionMeters(); + + 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/elevator/ElevatorStates.java b/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorStates.java new file mode 100644 index 0000000..5b50078 --- /dev/null +++ b/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorStates.java @@ -0,0 +1,56 @@ +package com.team973.frc2025.subsystems.elevator; + +import com.team973.lib.devices.GreyTalonFX.ControlMode; +import com.team973.lib.util.SubsystemState; + +public class ElevatorStates { + private abstract static class ElevatorState implements SubsystemState { + protected final ElevatorIO m_elevator; + + private ElevatorState(ElevatorIO elevator) { + m_elevator = elevator; + } + } + + public static class ClosedLoop extends ElevatorState { + public ClosedLoop(ElevatorIO elevator) { + super(elevator); + } + + public void init() {} + + public void run() { + m_elevator + .getMotor() + .setControl(ControlMode.MotionMagicVoltage, m_elevator.getTargetPositionMotorRot(), 0); + } + + public void exit() {} + } + + public static class Manual extends ElevatorState { + public Manual(ElevatorIO elevator) { + super(elevator); + } + + public void init() {} + + public void run() {} + + public void exit() {} + } + + public static class Off extends ElevatorState { + public Off(ElevatorIO elevator) { + super(elevator); + } + + public void init() {} + + public void run() { + m_elevator.getMotor().setControl(ControlMode.DutyCycleOut, 0, 0); + } + + public void exit() {} + } +} From 88c9840c2d1cf38c982984d3631f0f9d242e2077 Mon Sep 17 00:00:00 2001 From: NolanKnight Date: Thu, 8 Jan 2026 20:24:46 -0800 Subject: [PATCH 2/5] elevator in subsystem manager --- src/main/java/com/team973/lib/util/SubsystemManager.java | 8 +++++++- .../java/com/team973/lib/util/SubsystemManagerReal.java | 9 +++++++++ .../java/com/team973/lib/util/SubsystemManagerSim.java | 9 +++++++++ 3 files changed, 25 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/team973/lib/util/SubsystemManager.java b/src/main/java/com/team973/lib/util/SubsystemManager.java index 16ef22e..96e40cb 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.elevator.ElevatorIO; 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 ElevatorIO getElevator(); + + public void log() { + m_logger.log("components", new Pose3d[] {getElevator().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..50a04aa 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.elevator.Elevator; +import com.team973.frc2025.subsystems.elevator.ElevatorIO; 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 ElevatorIO m_elevator; 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_elevator = new Elevator(logger.subLogger("elevator")); } public GreyPigeonIO getPigeon() { @@ -42,4 +47,8 @@ public GreyPigeonIO getPigeon() { public DriveController getDriveController() { return m_driveController; } + + public ElevatorIO getElevator() { + return m_elevator; + } } diff --git a/src/main/java/com/team973/lib/util/SubsystemManagerSim.java b/src/main/java/com/team973/lib/util/SubsystemManagerSim.java index 1d22df3..1d80e47 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.elevator.ElevatorIO; +import com.team973.frc2025.subsystems.elevator.ElevatorSim; 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 ElevatorIO m_elevator; 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_elevator = new ElevatorSim(logger.subLogger("elevator")); } public GreyPigeonIO getPigeon() { @@ -66,6 +71,10 @@ public DriveController getDriveController() { return m_driveController; } + public ElevatorIO getElevator() { + return m_elevator; + } + @Override public void log() { super.log(); From 577c0a1fbac38a0fed545f03093dd31ed0233558 Mon Sep 17 00:00:00 2001 From: NolanKnight Date: Thu, 8 Jan 2026 20:37:47 -0800 Subject: [PATCH 3/5] robot model --- assets/Robot_Potato/config.json | 30 ++++++++++++++++++++++++++++++ assets/Robot_Potato/model.glb | Bin 0 -> 8852 bytes assets/Robot_Potato/model_0.glb | Bin 0 -> 8148 bytes 3 files changed, 30 insertions(+) create mode 100644 assets/Robot_Potato/config.json create mode 100644 assets/Robot_Potato/model.glb create mode 100644 assets/Robot_Potato/model_0.glb diff --git a/assets/Robot_Potato/config.json b/assets/Robot_Potato/config.json new file mode 100644 index 0000000..ca3ce71 --- /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 0000000000000000000000000000000000000000..043773cb99943e1824dbc4e90a27b8b9592747aa GIT binary patch literal 8852 zcmdT|33yyp75)Ph+$cK=795_k)F$KXlY}NEP1BN^CSwwiQVL8aFO$K{On5UXO-m59 zpyGnXDyYR3)S@hcqNr^Z+(oE}6;T8QEh_F>#r;3`zWe57ZKkxJ2;V*DzvrI&-*e7A z_f5WLhkBPD0AP~^TsH?;-qY2&-b!aOcBxc!N><~l^;X}=z<}+XowF~o8bh^K-)PzH z8W*ODd_jD5{6;dMj%eY$shVI##3AHEb6Gtu1t0(iZ5oGlPZVP%%3iNCp#uRsHswV37`) zbSabWw}V9|yV|O?*4R!dS7dFhNHD~?y|(P6c^~VomXY$HqNTOV$z^kebU9bZ21=u) zvYnUXI(yD+NwozQuMDJmTLbN#t+j!0FcM0t4cT-dw~jj$H!ZEje4e*QYOANWEfr`< zMPiXaTYpYm0+I6ytu1To+Ls2p?MyJ`q%);pU0q$bJ?z*eZc{{obfG_x@*9c+oEQ5? zGUdREbYWy5ohgqv5@dT}py=fFuG3O0+UJuj77S5FLg83lI8+yoPJLu_2}{T74h^Nt zc7I@*UCi5MXOuhHJ7#~Vp)MRduMjysJT=K#CEE!su?KPmQnZxIPOfjHY?rt_9E@v{ zL|v%9E*!VU#@G*D=lfrR_X;}fLbg0;HAce;Hp7H>A=4fn(9cH^W^a|#PF9{wG}e%a zR9bl!WEJ|o;R}c2e31U&6459v{&=F&x;j}mQ3h|aOGX0ouzC6=~{jWqS0hH5luvr;aD<}jK%Bu zHAqCF4T(@R8Bf;7>m$hoP4%%veKHz}M3eR5czrx6-wuZ17<$7P3P!?_a5NH%CKC1W zhFCPgo9i!*^bOfPtbV`MSawEim4RIvw8w>?N;~C1SmJkbd3O97cRB5~h9uG1)xDyn zgV$JVrMi0Bd)vD@d2Ku;SLo02OUVyir9A3F^F_J#*w|`0w!n!#HO~Olx?4T>cyTIa z_`y(X3b|3bJ456d3p_RtSlr%;El2HWDjs=H)6<7-X*T%8V=p33DZ3y0eYo!VXsmgN zx0$^LZ#eX7!@sqHQ%!=m&)qEND*FVicw8S3GX`Sq?_v|b@^N#S$$#tix$~O!-c++6 zyRiwsicNeNlZ=Iuiw`$T46fb7oD&P@=7oMNd@>jN1iaPw3!lOClNc1gA4?1zO|c^q zicj!#HI%bBCdK3P_wV&_dbxA2#)ikOkK!~uvL@J#*W@U%8~eE2SyRvM!+w81CT~jl zlTUItwIv>3A2%NHF?lKM)=K5Ynyj$x*Lkoa^Rbf36b_1P5mZNg=I ze0!U9UaCJmEP0!J{a9)&cH!}3sjHt;Fo*Q8KOTjp|0|q+to_N)y<*=X!{BW)_t_1m z-Fs8?*O~b9dKU_dUGwO({Fvu> z{(@y~VmuRle08R8r5|Lkd)H!D{jBrzz6mO3?K3_Xldt$|tiB;G9{(Mi+Q;NO{vB0v z_w3DvUt$#O`nYy;cm6vStiNf@ky!N!Yd*D)@EU)?ZoYB{e#{!_d-vDPhbQ!{e(%D; zbIor7vG*IHuyDFQDo%qdIqL6#wvYQw+(s_>ZVQjWqTM(ry^C>n-(GdD%OhNdb5C|T z%lNBv-My3SZvB%k3AmaFJ~E_DX{Vv`!<)y#o8`CIrVRl zXP0w~&m=7OB5%d#!*izl&GPzI`N-cw{QFJeD=yuz$d4J9=Q9B}3%~f&?qU6Rpyu~u zT^s+~(eIHlli$lxVfU>m?EgEh5@$-CtK-yI$EM%)1kBhx9}nx;w7(y_cc}h0@MB$D zFHZ7!IeKyGc(mQUXY-w*&-DJ4nopls#b<}9x#FzYbuK1%_g=hnwSD|vsVD8tQlAR; ze3aegA)eW8YUsw}aZa{-b?`XFhyNVJ|1Mvo_5Um0-*G?N>Z9`A?z^*njH}7_a&`9Z zbtjy{KAHvIo4eYZJyqqf$XClf;A(gcYc+eS@tvtI-r4F-^i1DR^>|;!^uOTQS7&G0&-jinu7B!A!?|}@M`AekJ}zCmiyml^cvP)t#{25r z>3FB9cdlb*4ZZkh#=^h1---RPx7t{$uFov*$?zII z{TnwPrhlueXEnTX(zDKgmVIfeRPFulZ~y9RI@Zx4Z&&3#8(i(3kyGBT>eDK)iN|~Q z?%qju!>w;*&;8`)XyTFFr^Pim00*Lm12_b8a1airJ_yglbMYL?=Tpzc3vejqVbn+9 zC>%+7H1)AK952K%oE=ViH1#n=$6_9i$BS_S*7I{3^UczxAWq|r5 zSX?=YBY>CT6r4DLK&uxBY`Ak5-~L4 zbTnciWfSVL2+fquXyE@%v^Nkpp%pDSgYpb4#u6;WGRkFWLp#pIa?0g+B|4Ep7dr4N z>J=QFlpWM(p&Pw;HF~JerWV;v>-o44=ima$^N}XH5GyIuI2Y&9axTX?$YM2TS&nm& z;aVT%O6q>t7{DryEM>pKS^BKvYKAgH+aO+pA<7}-aS`z#avXUSP=bSDj*C%Ls7RZG zGS*_0aug%DgnA9-TI$QN9_uJCrG72-I*buthRd-5S5RJo*WnG+uczEVeHGq_D=FVZ z{TAvg@n+(ya5dhBw^F{HdL!O}YbdY5JMk{O8`okJ$41JHv}~g7TD%wU!F81Hqkca& z<9f>L@d11gAHjzyKScd8+=v?}Z@@?Kacsd&lsDlMxD~hIllT6W_sc8*ZV# zo$_YtTeyBJ(t-Gx9|L!9sAxEBxLdw7uYL3|(IrM@3KsPD&p zobRN3fcie7dnxbd>{s|Dp2RNvnrIitlXwEZz|SdnQ9nWZEx;ir_3Q~#LyQ9Qzx z$0;A@>=F7rLHQWhe!}r6euv-SDg2iDDf}LP!XGLBK)oA(!JqM0%H8-op2puO|3Upv d{0IN0{1gwOH3w23Tr;QUAj(6i{|i*$OM(CZ literal 0 HcmV?d00001 diff --git a/assets/Robot_Potato/model_0.glb b/assets/Robot_Potato/model_0.glb new file mode 100644 index 0000000000000000000000000000000000000000..8161c2d53f20da7794c9f6b621694a51b068fdc0 GIT binary patch literal 8148 zcmdT|3v?V+8U70efl|R%MGML><vaNyV^ z=YHQe|NQsA|NZZOC+8&B+cEbfB6|9KqV1;;&2MjOT`jAbj8-ffW>KzNvRdvcb$4s# z!o0Rpu2Xz+SHGpTb$1svi$7+JLZ6{)y4BI&r^$8EU?}F3Ge$|r>rgo4lMCu9gi!*) zkP?gff&nEQRiZvT*T!n1%kVz0^Y=Ls%a0jZ7^# z_SB9Q4w)Z!r(t)e(J1_#aP1Ar=44tPz~0m%FVku{r|FuhT81gt$!&d_E;S_KHm6bQ z&@w%`(QD-TrRqRTT9VaP1PnN2)M7@>Y5~K{Et7rn3e7C$4HQZa1r$7Mt1MH+@vN4U zC98*rg|*Gh=kmI0<@KCY>@Qkcfi2hCJ};SSlxBBIsg4GzskOl;1p^_anor27dj1;B z;IUz8FbV}6CPK^Y9gQg|nF>WhQe!sHT%^!ay&<{E-!xZBYnedGR5QhZ-|tUreWq5# zG#*G&^{kZg8jNl{8`)CEl3G-~)U9T$lF5>6(z^|_V9%P7YH6xMut-1whLm8$A5{Fo z@aRj1=b&_`Zf~z@X<6xV%_wM=*^ik@AR^`~wf)M-jHj3oM?qad(5POeP)6Bfu zYkTNw0UKk+D_MK?b(t-irS|sbGijrg>(OyktXaF%qSk4$*H#XrjDkzR-77E$?MrXzME)( za&`E577TXkk%47wSsf4dss$v#-#f)~mS;}I489lq9am14&P)Yy1|&&3dm7Q~rdHZ9 zbtpdR;pK@jUk^;o6DAb>}&+#_jm2cv%m{^~fT3 zt+IZj99;WR>!w5;zLpb*`#5WPbKu&T@>u-LjZNIAtcT)juf0AY>}7pidp)az%-S7pM{$n>~5{R8Zo_lcQyB6agLl-#eK?sV)vo2d;7DTo3}r0fAk@b$^0?b8{gH! zXOueEzhwL^e)`{+vcBD8G`}1_?I~NYd-WgjN4ME}pZ3rALetjGH~lMqL9MOh-LJ%V z{AUTDd*IKn#BbYeYqriKR@r_?u{HA_`s-ru&*Ch4z}77O^p&<|>+YUoYnIQ|(`?Q1 z>^suQ=N^4G#kq{fy@;II*@^lyE$Yv-s6W%9{!EMdGcD@Rw5Y$Zv-%5vR)3~N{h1c^ zXIj*sX;FWsMg5s(_4n3XoXcoFw*MP_)9g529}&OY&)z%e-VwLwS>JkVD74-4_FQkw zqy5KI&C4|zSqJBP;+_lJucr?6VvpE+z33~ZMPD&3`ig1MS4@k(Vp{Z-usipf`#blV z$8qj8kMG=TzOHkxc|Oj)=HDNue_W0Hxn|!brbYfti~N}u`71|T$G;LihWHh&?}^7USyM?4ULVi=Chv$B03TaPvOx82{nk8!B&s#kbNPU!u;cHeOQf7|m3wxj+3o}Acv>H@Ck z4+Ro`nK+AU2v{INSDyXz>B~$C`>+} zk3K|x3?E$z4gdo*lR|hqgA_c?q$+R_7=*P776osrpfBY!tEquzQ#HlFwHPs~qZ+6> zj9C<=%Q()$QxaaYsh&Pe31AW&$J;1H@Dv9o!4aq^erpk@8kPvnqq%fBwa`bXncBdu zl%_e<2yDcgG~PD>n`l08KD^TKN&!>!QD8fK)9{-IY{8o}JX)v&_B1V^ENQe5tkD%1 z-N1$5MNm2572r;&9^fMIVyHZ@6TAfKW5C7WrBELSE&(rt>IL>v0k{-T%fSV@5<>^- zWB`}r=_;^6Dn=jhDsUIn)j$=Tfii(z;38R60+wh6t)x}d5A3IF=o7S>t_2Q&KSiIU zHNfk@H_&HjEq#tYOY7-ox(RBKuBT4}*MbM>MtBU;I`AMo)&V~czd^bMJP5xHz#HH@ zh?(nwH-oBK;NJibgI@r?NQZzg(651q=pgu4z{B9@fd}b1 P;Pdn>{So{u#&h&PfgDN$ literal 0 HcmV?d00001 From 6021d0a8c9e18560930967b946ee487e6894d241 Mon Sep 17 00:00:00 2001 From: NolanKnight Date: Thu, 8 Jan 2026 20:38:26 -0800 Subject: [PATCH 4/5] elevator controls and sim WIP --- src/main/java/com/team973/frc2025/Robot.java | 28 +++++++++++++++---- .../com/team973/frc2025/shared/RobotInfo.java | 5 ++++ .../frc2025/subsystems/elevator/Elevator.java | 9 ++---- .../subsystems/elevator/ElevatorIO.java | 21 +++++++++++++- .../subsystems/elevator/ElevatorSim.java | 12 +++++--- 5 files changed, 59 insertions(+), 16 deletions(-) diff --git a/src/main/java/com/team973/frc2025/Robot.java b/src/main/java/com/team973/frc2025/Robot.java index 04ee4c4..2be854e 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.elevator.ElevatorIO; 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 ElevatorIO m_elevator = m_subsystemManager.getElevator(); 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_elevator.syncSensors(); } private void updateSubsystems() { m_driveController.update(); + m_elevator.update(); } private void resetSubsystems() { m_driveController.reset(); + m_elevator.reset(); } private void log() { m_subsystemManager.log(); m_driveController.log(); + m_elevator.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_elevator.setTargetPreset(ElevatorIO.Preset.One); + } else if (m_coDriverStick.getBButtonPressed()) { + m_elevator.setTargetPreset(ElevatorIO.Preset.Two); + } else if (m_coDriverStick.getXButtonPressed()) { + m_elevator.setTargetPreset(ElevatorIO.Preset.Three); + } else if (m_coDriverStick.getYButtonPressed()) { + m_elevator.setState(ElevatorIO.State.Manual); + } + + // m_elevator.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 7c92094..74eb8f4 100644 --- a/src/main/java/com/team973/frc2025/shared/RobotInfo.java +++ b/src/main/java/com/team973/frc2025/shared/RobotInfo.java @@ -2,6 +2,7 @@ import static edu.wpi.first.units.Units.Inches; +import com.team973.lib.util.Conversions; import com.team973.lib.util.SwerveModuleConfig; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -43,6 +44,10 @@ public static class ElevatorInfo { public final double PEAK_FORWARD_VOLTAGE = 12.0; public final double PEAK_REVERSE_VOLTAGE = -12.0; + + public final double PRESET_ONE = 0.0; + public final double PRESET_TWO = 15.0 * Conversions.Distance.METERS_PER_INCH; + public final double PRESET_THREE = 30.0 * Conversions.Distance.METERS_PER_INCH; } public static class DriveInfo { diff --git a/src/main/java/com/team973/frc2025/subsystems/elevator/Elevator.java b/src/main/java/com/team973/frc2025/subsystems/elevator/Elevator.java index 221e179..62ff2ca 100644 --- a/src/main/java/com/team973/frc2025/subsystems/elevator/Elevator.java +++ b/src/main/java/com/team973/frc2025/subsystems/elevator/Elevator.java @@ -11,8 +11,6 @@ import edu.wpi.first.math.geometry.Rotation3d; public class Elevator extends ElevatorIO { - private final RobotInfo.ElevatorInfo m_elevatorInfo; - private final Logger m_logger; protected final GreyTalonFX m_motor; @@ -21,7 +19,6 @@ public class Elevator extends ElevatorIO { public Elevator(Logger logger) { m_logger = logger; - m_elevatorInfo = RobotInfo.ELEVATOR_INFO; m_motor = new GreyTalonFX( m_elevatorInfo.MOTOR_ID, RobotInfo.CANIVORE_CANBUS, m_logger.subLogger("motorRight")); @@ -75,7 +72,7 @@ public GreyTalonFX getMotor() { return m_motor; } - public double heightMetersToMotorRotations(double postionHeight) { + protected double heightMetersToMotorRotations(double postionHeight) { return postionHeight / m_elevatorInfo.MOTOR_ROT_TO_HEIGHT_METERS; } @@ -84,8 +81,8 @@ private double motorRotationsToHeightMeters(double motorPostion) { } @Override - public void setTargetPostion(double targetPostionHeightMeters) { - m_targetPostionHeightMeters = targetPostionHeightMeters; + public void setTargetPreset(Preset preset) { + m_targetPostionHeightMeters = preset.getHeightMeters(); setState(State.ClosedLoop); } diff --git a/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorIO.java b/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorIO.java index efa247c..ee5c851 100644 --- a/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorIO.java @@ -1,5 +1,6 @@ package com.team973.frc2025.subsystems.elevator; +import com.team973.frc2025.shared.RobotInfo; import com.team973.lib.devices.GreyTalonFX; import com.team973.lib.util.StateMap; import com.team973.lib.util.Subsystem; @@ -8,12 +9,30 @@ public abstract class ElevatorIO extends Subsystem { private final StateMap m_stateMap; + protected static final RobotInfo.ElevatorInfo m_elevatorInfo = RobotInfo.ELEVATOR_INFO; + public enum State { ClosedLoop, Manual, Off } + public enum Preset { + One(m_elevatorInfo.PRESET_ONE), + Two(m_elevatorInfo.PRESET_TWO), + Three(m_elevatorInfo.PRESET_THREE); + + private final double m_heightMeters; + + private Preset(double heightMeters) { + m_heightMeters = heightMeters; + } + + public double getHeightMeters() { + return m_heightMeters; + } + } + @SuppressWarnings("unchecked") public ElevatorIO() { super(State.Off); @@ -34,7 +53,7 @@ public StateMap getStateMap() { public abstract GreyTalonFX getMotor(); - public abstract void setTargetPostion(double targetPostionHeightinches); + public abstract void setTargetPreset(Preset preset); public abstract double getTargetPositionMotorRot(); } diff --git a/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorSim.java b/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorSim.java index 4e294de..3f2473a 100644 --- a/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorSim.java +++ b/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorSim.java @@ -7,19 +7,17 @@ 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.wpilibj.RobotController; public class ElevatorSim extends Elevator { private final edu.wpi.first.wpilibj.simulation.ElevatorSim m_sim; private final TalonFXSimState m_motorSimState; - private final double m_motorRotToElevatorHeightMeters; private double m_lastPoseMeters; public ElevatorSim(Logger logger) { super(logger); - m_motorRotToElevatorHeightMeters = RobotInfo.ELEVATOR_INFO.MOTOR_ROT_TO_HEIGHT_METERS; - m_sim = new edu.wpi.first.wpilibj.simulation.ElevatorSim( DCMotor.getKrakenX60(1), @@ -43,9 +41,15 @@ public Pose3d getPose() { @Override public void syncSensors() { m_motorSimState.addRotorPosition( - (m_sim.getPositionMeters() - m_lastPoseMeters) / m_motorRotToElevatorHeightMeters); + heightMetersToMotorRotations(m_sim.getPositionMeters() - m_lastPoseMeters)); + m_lastPoseMeters = m_sim.getPositionMeters(); + m_motorSimState.setRotorVelocity( + heightMetersToMotorRotations(m_sim.getVelocityMetersPerSecond())); + + m_motorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); + super.syncSensors(); } From 0b4fdfd40493d9bb393d33c2fd96fcd95b81ac3b Mon Sep 17 00:00:00 2001 From: NolanKnight Date: Fri, 9 Jan 2026 22:05:59 -0800 Subject: [PATCH 5/5] added manual controls and debugged sim --- src/main/java/com/team973/frc2025/Robot.java | 2 +- .../com/team973/frc2025/shared/RobotInfo.java | 4 +++- .../frc2025/subsystems/elevator/Elevator.java | 19 +++++++++++++++++-- .../subsystems/elevator/ElevatorIO.java | 4 ++++ .../subsystems/elevator/ElevatorStates.java | 11 +++++++++-- 5 files changed, 34 insertions(+), 6 deletions(-) diff --git a/src/main/java/com/team973/frc2025/Robot.java b/src/main/java/com/team973/frc2025/Robot.java index 2be854e..710e274 100644 --- a/src/main/java/com/team973/frc2025/Robot.java +++ b/src/main/java/com/team973/frc2025/Robot.java @@ -113,7 +113,7 @@ public void teleopPeriodic() { m_elevator.setState(ElevatorIO.State.Manual); } - // m_elevator.setManualInput(m_coDriverStick.getLeftYAxis()); + m_elevator.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 74eb8f4..ae2838b 100644 --- a/src/main/java/com/team973/frc2025/shared/RobotInfo.java +++ b/src/main/java/com/team973/frc2025/shared/RobotInfo.java @@ -23,8 +23,10 @@ public class RobotInfo { public static class ElevatorInfo { public final int MOTOR_ID = 20; + public final double MANUAL_INPUT_TO_VOLTS = 12.0; + public final double MOTOR_GEAR_RATIO = 10.0 / 56.0; - public final double MOTOR_ROT_TO_HEIGHT_METERS = MOTOR_GEAR_RATIO * 5.0 * 36.0 * 100.0; + public final double MOTOR_ROT_TO_HEIGHT_METERS = MOTOR_GEAR_RATIO * 5.0 * 36.0 * (1.0 / 100.0); public final double ELEVATOR_KS = 0.0; public final double ELEVATOR_KV = 0.15; diff --git a/src/main/java/com/team973/frc2025/subsystems/elevator/Elevator.java b/src/main/java/com/team973/frc2025/subsystems/elevator/Elevator.java index 62ff2ca..3d86f65 100644 --- a/src/main/java/com/team973/frc2025/subsystems/elevator/Elevator.java +++ b/src/main/java/com/team973/frc2025/subsystems/elevator/Elevator.java @@ -16,6 +16,7 @@ public class Elevator extends ElevatorIO { protected final GreyTalonFX m_motor; private double m_targetPostionHeightMeters; + private double m_manualInput; public Elevator(Logger logger) { m_logger = logger; @@ -55,6 +56,9 @@ public Elevator(Logger logger) { m_motor.setConfig(motorConfig); m_motor.setPosition(0.0); + + m_targetPostionHeightMeters = Preset.One.getHeightMeters(); + m_manualInput = 0.0; } @Override @@ -86,11 +90,21 @@ public void setTargetPreset(Preset preset) { setState(State.ClosedLoop); } + @Override + public void setManualInput(double input) { + m_manualInput = input; + } + @Override public double getTargetPositionMotorRot() { return heightMetersToMotorRotations(m_targetPostionHeightMeters); } + @Override + public double getManualInput() { + return m_manualInput; + } + @Override public void syncSensors() {} @@ -100,8 +114,9 @@ public void log() { m_motor.log(); - m_logger.log("currentPostionHeightInches", motorRotationsToHeightMeters(motorRot)); - m_logger.log("targetPostionHeightInches", m_targetPostionHeightMeters); + m_logger.log("currentPostionHeightMeters", motorRotationsToHeightMeters(motorRot)); + m_logger.log("targetPostionHeightMeters", m_targetPostionHeightMeters); + m_logger.log("manualInput", m_manualInput); m_logger.log("state", getState().toString()); } diff --git a/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorIO.java b/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorIO.java index ee5c851..445497a 100644 --- a/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorIO.java @@ -55,5 +55,9 @@ public StateMap getStateMap() { public abstract void setTargetPreset(Preset preset); + public abstract void setManualInput(double input); + public abstract double getTargetPositionMotorRot(); + + public abstract double getManualInput(); } diff --git a/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorStates.java b/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorStates.java index 5b50078..499182d 100644 --- a/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorStates.java +++ b/src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorStates.java @@ -1,5 +1,6 @@ package com.team973.frc2025.subsystems.elevator; +import com.team973.frc2025.shared.RobotInfo; import com.team973.lib.devices.GreyTalonFX.ControlMode; import com.team973.lib.util.SubsystemState; @@ -22,7 +23,7 @@ public void init() {} public void run() { m_elevator .getMotor() - .setControl(ControlMode.MotionMagicVoltage, m_elevator.getTargetPositionMotorRot(), 0); + .setControl(ControlMode.MotionMagicVoltage, m_elevator.getTargetPositionMotorRot()); } public void exit() {} @@ -35,7 +36,13 @@ public Manual(ElevatorIO elevator) { public void init() {} - public void run() {} + public void run() { + m_elevator + .getMotor() + .setControl( + ControlMode.VoltageOut, + m_elevator.getManualInput() * RobotInfo.ELEVATOR_INFO.MANUAL_INPUT_TO_VOLTS); + } public void exit() {} }