From 07041a5b6d6913467fe4a5f7db527f2304030c42 Mon Sep 17 00:00:00 2001 From: fatema Date: Mon, 6 Oct 2025 20:46:24 -0700 Subject: [PATCH 1/3] hello --- src/main/java/frc/robot/Constants.java | 2 + .../robot/subsystems/ElevatorSubsystem2.java | 141 ++++++++++++++++++ 2 files changed, 143 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/ElevatorSubsystem2.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7ef43561..f99ca445 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -895,6 +895,8 @@ public static class ElevatorConstants { public static final double MOTIONMAGIC_MAX_ACCELERATION = 250; public static double SENSOR_OFFSET = 0.11; + public static double minHeight; + public static double maxHeight; // public static final double MOTIONMAGIC_KG = 0.28; public static final double CRUISE_VELOCITY = 6.0; // To-do public static final double ACCELERATION = 6.0; // To-do diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem2.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem2.java new file mode 100644 index 00000000..2960a22f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem2.java @@ -0,0 +1,141 @@ + +// 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; + +import com.ctre.phoenix6.configs.MotionMagicConfigs; //import statements +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.Slot1Configs; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.CANrange; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import dev.doglog.DogLog; +import edu.wpi.first.math.filter.LinearFilter; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Constants.ElevatorConstants; +import frc.robot.util.LoggedTalonFX; + +// import frc.robot.Constants.Elevator.ElevatorPositions; + +public class ElevatorSubsystem2 extends SubsystemBase { //class declaration + private TalonFX motor1; // motors + master holder + target height declaration + private TalonFX motor2; + public TalonFX master; + public double targetHeight; + + private MotionMagicConfigs mmc; + + private final MotionMagicVoltage controlRequest = new MotionMagicVoltage(0); //motion magic control request + + public ElevatorSubsystem2() { //constructor + motor1 = //motor 1 initialization + new LoggedTalonFX( + "subsystems/Elevator/motor1", + ElevatorConstants.MOTOR1_PORT, + Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); + motor2 = //motor 2 initialization + new LoggedTalonFX( + "subsystems/Elevator/motor2", + ElevatorConstants.MOTOR2_PORT, + Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); + + Follower follower = new Follower(ElevatorConstants.MOTOR1_PORT, false); //setting motor 2 as the follower to motor 1(master) + motor2.setControl(follower); + + + Slot1Configs s1c = //slot 1 configs + new Slot1Configs() + // .withKP(ElevatorConstants.S1C_KP) + // .withKI(ElevatorConstants.S1C_KI) + // .withKD(ElevatorConstants.S1C_KD) + .withKS(ElevatorConstants.S0C_KS) + .withKG(ElevatorConstants.S0C_KG) + .withKA(ElevatorConstants.S0C_KA) + .withKV(ElevatorConstants.S0C_KV) + .withGravityType(GravityTypeValue.Elevator_Static) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + + Slot0Configs s0c = //slot 0 configs + PID + new Slot0Configs() + .withKP(ElevatorConstants.S0C_KP) + .withKI(ElevatorConstants.S0C_KI) + .withKD(ElevatorConstants.S0C_KD) + .withKS(ElevatorConstants.S0C_KS) + .withKG(ElevatorConstants.S0C_KG) + .withKA(ElevatorConstants.S0C_KA) + .withKV(ElevatorConstants.S0C_KV) + .withGravityType(GravityTypeValue.Elevator_Static) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + + ((LoggedTalonFX) motor1).updateCurrentLimits( //current limits of motor 1 and 2 + ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); + ((LoggedTalonFX) motor2).updateCurrentLimits( + ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); + + TalonFXConfigurator m1Config = motor1.getConfigurator(); + TalonFXConfigurator m2Config = motor2.getConfigurator(); + + m1Config.apply(s0c); + m2Config.apply(s0c); + m1Config.apply(s1c); + m2Config.apply(s1c); + + MotorOutputConfigs moc = new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake); + + // apply MotionMagic to motors + mmc = new MotionMagicConfigs(); + mmc.MotionMagicCruiseVelocity = ElevatorConstants.MOTIONMAGIC_MAX_VELOCITY; + mmc.MotionMagicAcceleration = ElevatorConstants.MOTIONMAGIC_MAX_ACCELERATION; + + m1Config.apply(mmc); + m2Config.apply(mmc); + + m1Config.apply(moc); + m2Config.apply(moc); + + master = motor1; //set motor 1 as master + } + + public void setPosition(double height) { //set position method + if (height < Constants.ElevatorConstants.minHeight) height = Constants.ElevatorConstants.minHeight; + if (height > Constants.ElevatorConstants.maxHeight) height = Constants.ElevatorConstants.maxHeight; + + master.setControl( //set master (motor 1) to height w control + controlRequest + .withPosition( + height + * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS + / ElevatorConstants.CARRAIGE_UPDUCTION) + .withSlot(0)); + DogLog.log( //log + "subsystems/Elevator/elevatorSetpoint(rot)", + height + * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS + / ElevatorConstants.CARRAIGE_UPDUCTION); + } + + public void resetElevatorPositionToZero() { //zero position + master.setPosition(0); + } + + public double getHeight() { // getter method for height + return master.getRotorPosition().getValueAsDouble()* ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS/ ElevatorConstants.CARRAIGE_UPDUCTION; + } + + public void periodic() { //periodic -- repeats every 20 ms + DogLog.log( + "subsystems/Elevator/currentheight", + getHeight()); + } +} \ No newline at end of file From 88974b60f6a8d416fa4242e24728d37e05278afa Mon Sep 17 00:00:00 2001 From: fatema Date: Wed, 8 Oct 2025 20:20:41 -0700 Subject: [PATCH 2/3] change comment --- src/main/java/frc/robot/subsystems/ElevatorSubsystem2.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem2.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem2.java index 2960a22f..43178daf 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem2.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem2.java @@ -138,4 +138,5 @@ public void periodic() { //periodic -- repeats every 20 ms "subsystems/Elevator/currentheight", getHeight()); } -} \ No newline at end of file +} +//change \ No newline at end of file From 8f5544a2612c6bfb8fe8f9094e1bde36b9c2d790 Mon Sep 17 00:00:00 2001 From: fatema Date: Wed, 8 Oct 2025 20:34:27 -0700 Subject: [PATCH 3/3] changed names --- .../robot/commandGroups/ApplicationMD4.java | 52 +++ .../robot/subsystems/ElevatorSubsystem.java | 361 +++++------------- .../robot/subsystems/ElevatorSubsystem1.java | 329 ++++++++++++++++ .../robot/subsystems/ElevatorSubsystem2.java | 142 ------- .../frc/robot/subsystems/LedSubsystem.java | 1 + 5 files changed, 469 insertions(+), 416 deletions(-) create mode 100644 src/main/java/frc/robot/commandGroups/ApplicationMD4.java create mode 100644 src/main/java/frc/robot/subsystems/ElevatorSubsystem1.java delete mode 100644 src/main/java/frc/robot/subsystems/ElevatorSubsystem2.java diff --git a/src/main/java/frc/robot/commandGroups/ApplicationMD4.java b/src/main/java/frc/robot/commandGroups/ApplicationMD4.java new file mode 100644 index 00000000..276ca431 --- /dev/null +++ b/src/main/java/frc/robot/commandGroups/ApplicationMD4.java @@ -0,0 +1,52 @@ +package frc.robot.commandGroups; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.ElevatorConstants.ElevatorPositions; +import frc.robot.commands.ElevatorCommands.SetElevatorLevel; + +/* + * + * + * Task Card: + * You will be creating a CommandGroup that does the following: + * 1. Intakes the Coral + * 2. Elevator goes to L4 + * 3. Elevator goes to L2 + * 4. Elevator goes to L3 + * 5. Elevator goes to L2 + * 6. Elevator Shoots AND Spins the Funnel at the same time. + * When the Shooting ENDS stop the Funnel + * 7. Elevator goes to L4 + * 8. AS Elevator lowers down to L2 spin both the Funnel and Shooter + * + * + * NOTES: + * * Auto-intaking has been disabled + * * Auto-eject has been disabled + * --> This means you have to be careful with trying to feed a coral + * --> when there is already another coral in the robot + * + * * INVESTIGATE ALL OF THE RELEVANT COMMANDS AND THEIR END BEHAVIORS + * * BEFORE YOU BEGIN CREATING THE COMMAND GROUP + * + * Relevant Commands: + * SetElevatorLevel <-- takes care of Elevator raising to certain hegiht + * ShootTootsieSlide <-- Shoots the coral (spins the shooter) + * RunFunnelOutCommand <-- spins the funnel + * + * + * * STARTING HINT: This class must extend either SequentialCommandGroup or + * * ParallelCommandGroup + * + * Good luck and refer to the MD.4 Resources, me or your Software + * mentors if you have any questions or difficulties! + * + * After you make your CommandGroup, check RobotContainer.java + * Lines 359-360 and you will bind your CommandGroup to the + * custom controller board left L1 button and test that way + * + */ + +public class ApplicationMD4 extends SequentialCommandGroup { + addCommands(new SetElevatorLevel(ElevatorPositions.L4, true), new SetElevatorLevel(ElevatorPositions.L3, true), f); +} diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index a00efcb2..6596d451 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -1,10 +1,11 @@ -// Copyright (c) FIRST and other WPILib contributors. + +// 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; -import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; //import statements import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.Slot1Configs; @@ -14,6 +15,7 @@ import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANrange; +import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; @@ -22,73 +24,41 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Constants.ElevatorConstants; -import frc.robot.Constants.ElevatorConstants.ElevatorPositions; import frc.robot.util.LoggedTalonFX; -// import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class ElevatorSubsystem extends SubsystemBase { - private static ElevatorSubsystem instance; - - private LoggedTalonFX motor1; - private LoggedTalonFX motor2; - public LoggedTalonFX master; +// import frc.robot.Constants.Elevator.ElevatorPositions; - private LinearFilter elevatorFilter; - private double currentHeightToF; - private boolean elevatorZeroed; +public class ElevatorSubsystem extends SubsystemBase { //class declaration + private TalonFX motor1; // motors + master holder + target height declaration + private TalonFX motor2; + public TalonFX master; + public double targetHeight; - private MotionMagicConfigs mmc; - private ElevatorPositions currentLevel; - private CANrange distance; // Time of Flight (ToF) sensor + private MotionMagicConfigs mmc; - private final MotionMagicVoltage controlRequest = new MotionMagicVoltage(0); - private final TorqueCurrentFOC torqueRequest = new TorqueCurrentFOC(0); - private final VelocityVoltage velocityRequest = new VelocityVoltage(0); + private final MotionMagicVoltage controlRequest = new MotionMagicVoltage(0); //motion magic control request - private ElevatorSubsystem() { - // Initialize motors - elevatorZeroed = false; - elevatorFilter = LinearFilter.singlePoleIIR(0.1, 0.02); - - distance = - new CANrange( - ElevatorConstants.CANRANGE_PORT, Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); - - motor1 = + public ElevatorSubsystem() { //constructor + motor1 = //motor 1 initialization new LoggedTalonFX( "subsystems/Elevator/motor1", ElevatorConstants.MOTOR1_PORT, Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); - motor2 = + motor2 = //motor 2 initialization new LoggedTalonFX( "subsystems/Elevator/motor2", ElevatorConstants.MOTOR2_PORT, Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); - currentLevel = ElevatorPositions.Intake; + Follower follower = new Follower(ElevatorConstants.MOTOR1_PORT, false); //setting motor 2 as the follower to motor 1(master) + motor2.setControl(follower); - // Set up motor followers and deal with inverted motors - Follower follower = new Follower(ElevatorConstants.MOTOR1_PORT, false); - motor2.setControl(follower); - Slot1Configs s1c = + Slot1Configs s1c = //slot 1 configs new Slot1Configs() - .withKP(ElevatorConstants.S1C_KP) - .withKI(ElevatorConstants.S1C_KI) - .withKD(ElevatorConstants.S1C_KD) - .withKS(ElevatorConstants.S0C_KS) - .withKG(ElevatorConstants.S0C_KG) - .withKA(ElevatorConstants.S0C_KA) - .withKV(ElevatorConstants.S0C_KV) - .withGravityType(GravityTypeValue.Elevator_Static) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - - Slot0Configs s0c = - new Slot0Configs() - .withKP(ElevatorConstants.S0C_KP) - .withKI(ElevatorConstants.S0C_KI) - .withKD(ElevatorConstants.S0C_KD) + // .withKP(ElevatorConstants.S1C_KP) + // .withKI(ElevatorConstants.S1C_KI) + // .withKD(ElevatorConstants.S1C_KD) .withKS(ElevatorConstants.S0C_KS) .withKG(ElevatorConstants.S0C_KG) .withKA(ElevatorConstants.S0C_KA) @@ -96,234 +66,77 @@ private ElevatorSubsystem() { .withGravityType(GravityTypeValue.Elevator_Static) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - motor1.updateCurrentLimits( - ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); - motor2.updateCurrentLimits( - ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); - - TalonFXConfigurator m1Config = motor1.getConfigurator(); - TalonFXConfigurator m2Config = motor2.getConfigurator(); - - m1Config.apply(s0c); - m2Config.apply(s0c); - m1Config.apply(s1c); - m2Config.apply(s1c); - - MotorOutputConfigs moc = new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake); - - // Apply MotionMagic to motors - mmc = new MotionMagicConfigs(); - mmc.MotionMagicCruiseVelocity = ElevatorConstants.MOTIONMAGIC_MAX_VELOCITY; - mmc.MotionMagicAcceleration = ElevatorConstants.MOTIONMAGIC_MAX_ACCELERATION; - - m1Config.apply(mmc); - m2Config.apply(mmc); - - m1Config.apply(moc); - m2Config.apply(moc); - - master = motor1; - currentHeightToF = elevatorFilter.calculate(getToFDistance()); - resetPositionFiltered(); - } - - // instance for elevator subsystem - public static ElevatorSubsystem getInstance() { - if (instance == null) { - instance = new ElevatorSubsystem(); + Slot0Configs s0c = //slot 0 configs + PID + new Slot0Configs() + .withKP(ElevatorConstants.S0C_KP) + .withKI(ElevatorConstants.S0C_KI) + .withKD(ElevatorConstants.S0C_KD) + .withKS(ElevatorConstants.S0C_KS) + .withKG(ElevatorConstants.S0C_KG) + .withKA(ElevatorConstants.S0C_KA) + .withKV(ElevatorConstants.S0C_KV) + .withGravityType(GravityTypeValue.Elevator_Static) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + + ((LoggedTalonFX) motor1).updateCurrentLimits( //current limits of motor 1 and 2 + ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); + ((LoggedTalonFX) motor2).updateCurrentLimits( + ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); + + TalonFXConfigurator m1Config = motor1.getConfigurator(); + TalonFXConfigurator m2Config = motor2.getConfigurator(); + + m1Config.apply(s0c); + m2Config.apply(s0c); + m1Config.apply(s1c); + m2Config.apply(s1c); + + MotorOutputConfigs moc = new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake); + + // apply MotionMagic to motors + mmc = new MotionMagicConfigs(); + mmc.MotionMagicCruiseVelocity = ElevatorConstants.MOTIONMAGIC_MAX_VELOCITY; + mmc.MotionMagicAcceleration = ElevatorConstants.MOTIONMAGIC_MAX_ACCELERATION; + + m1Config.apply(mmc); + m2Config.apply(mmc); + + m1Config.apply(moc); + m2Config.apply(moc); + + master = motor1; //set motor 1 as master } - return instance; - } - - public boolean tofIsConnected() { - return distance.isConnected(); - } - public void resetPositionFiltered() { - master.setPosition( - currentHeightToF * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); - DogLog.log( - "subsystems/Elevator/resetElevatorPosition", - currentHeightToF * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); - } - - public void resetPosition() { - if (tofIsConnected()) { - master.setPosition( - this.getToFDistance() - * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); - DogLog.log( - "subsystems/Elevator/resetElevatorPosition", - this.getToFDistance() - * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); + public void setPosition(double height) { //set position method + if (height < Constants.ElevatorConstants.minHeight) height = Constants.ElevatorConstants.minHeight; + if (height > Constants.ElevatorConstants.maxHeight) height = Constants.ElevatorConstants.maxHeight; + + master.setControl( //set master (motor 1) to height w control + controlRequest + .withPosition( + height + * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS + / ElevatorConstants.CARRAIGE_UPDUCTION) + .withSlot(0)); + DogLog.log( //log + "subsystems/Elevator/elevatorSetpoint(rot)", + height + * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS + / ElevatorConstants.CARRAIGE_UPDUCTION); } - } - - public void resetPosition(double posInHeight) { - // TODO: add constant to convert distance to encoder values - master.setPosition( - posInHeight * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); - DogLog.log( - "subsystems/Elevator/resetElevatorPosition", - posInHeight * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); - } - // Hardstop Zeroing functions: - public void moveElevatorNegative() { - master.setControl(velocityRequest.withVelocity(-5).withSlot(1)); - } - - public void reduceCurrentLimits() { - master.updateCurrentLimits(30, 10); - } - - public void resetCurrentLimits() { - master.updateCurrentLimits( - Constants.ElevatorConstants.STATOR_CURRENT_LIMIT, - Constants.ElevatorConstants.SUPPLY_CURRENT_LIMIT); - } - - public void resetElevatorPositionToZero() { - master.setPosition(0); - // master.setControl(controlRequest.withPosition(master.getPosition().getValueAsDouble()).withSlot(0)); - // master.setPosition(0); - // master.setControl(controlRequest.withPosition(0).withSlot(0)); - // master.setPosition(0); - } - - public boolean checkCurrent() { - double Supplycurrent = Math.abs(master.getSupplyCurrent().getValue().magnitude()); - double Statorcurrent = Math.abs(master.getStatorCurrent().getValue().magnitude()); - DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/supply", Supplycurrent); - DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/stator", Statorcurrent); - - if (Supplycurrent > 1.0 && Statorcurrent > 20) { - return true; + public void resetElevatorPositionToZero() { //zero position + master.setPosition(0); } - return false; - } - - public double getError() { - return currentLevel.height - * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS - / Constants.ElevatorConstants.CARRAIGE_UPDUCTION - - master.getPosition().getValueAsDouble(); - } - - public ElevatorPositions getLevel() { - return currentLevel; - } - - public boolean atIntake() { - return currentLevel.equals(ElevatorPositions.Intake); - } - - public void elevateTo(ElevatorPositions level) { - this.currentLevel = level; - this.setPosition(level.height); - } - - public void setPosition(double height) { - master.setControl( - controlRequest - .withPosition( - height - * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS - / ElevatorConstants.CARRAIGE_UPDUCTION) - .withSlot(0)); - DogLog.log( - "subsystems/Elevator/elevatorSetpoint(rot)", - height - * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS - / ElevatorConstants.CARRAIGE_UPDUCTION); - } - - public void ElevatorTorqueMode() { - DogLog.log("subsystems/Elevator/usingTorqueMode", true); - master.setControl(torqueRequest.withOutput(Constants.ElevatorConstants.ELEVATOR_TORQUE)); - // .withMaxAbsDutyCycle(Constants.ElevatorConstants.ELEVATOR_DUTY_CYCLE)); - } - // TODO: ONLY FOR DEBUGGING - public void testElevator(double height) { - this.setPosition(height); - } - - public boolean isAtPosition() { - return (Math.abs(getError()) <= ElevatorConstants.SETPOINT_TOLERANCE); - } - - public boolean canFunnelTransferCoralToScoring() { - return this.getLevel().equals(Constants.ElevatorConstants.ElevatorPositions.Intake) - && this.getError() < Constants.ElevatorConstants.MAX_POSITIONAL_ERROR; - } - - public double getToFDistance() { - // 0.11 is the sensor offset - DogLog.log( - "subsystems/Elevator/ToF/DistanceNoOffset", distance.getDistance().getValueAsDouble()); - return distance.getDistance().getValueAsDouble() - Constants.ElevatorConstants.SENSOR_OFFSET; - } - - public boolean isElevatorZeroed() { - return elevatorZeroed; - } - - public void elevatorHasBeenZeroed() { - elevatorZeroed = true; - } - - @Override - public void periodic() { - currentHeightToF = elevatorFilter.calculate(getToFDistance()); - // Time of Flight Sensor - DogLog.log("subsystems/Elevator/getError", getError()); - DogLog.log("subsystems/Elevator/ToF/Distance", getToFDistance()); - DogLog.log("subsystems/Elevator/ToF/Connected", distance.isConnected()); - DogLog.log("subsystems/Elevator/ToF/LinearFilterDistance", currentHeightToF); - - DogLog.log("subsystems/Elevator/isAtPosition", this.isAtPosition()); - DogLog.log("subsystems/Elevator/targetPosition", currentLevel.getPosition()); - DogLog.log("subsystems/Elevator/targetHeightDist", currentLevel.getHeight()); - DogLog.log( - "subsystems/Elevator/targetHeightRot", - currentLevel.getHeight() - * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS - / Constants.ElevatorConstants.CARRAIGE_UPDUCTION); - DogLog.log( - "subsystems/Elevator/currentHeightDist", - master.getPosition().getValueAsDouble() - * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_ROTATIONS_TO_DISTANCE - * Constants.ElevatorConstants.CARRAIGE_UPDUCTION); - DogLog.log("subsystems/Elevator/currentHeightRot", master.getPosition().getValueAsDouble()); - DogLog.log( - "subsystems/Elevator/command", - this.getCurrentCommand() == null ? "NOTHING" : this.getCurrentCommand().getName()); - DogLog.log( - "subsystems/Elevator/resetPositionBoolean", - this.isAtPosition() && this.getLevel().equals(ElevatorPositions.Intake)); - DogLog.log( - "subsystems/Elevator/targetisIntake", this.getLevel().equals(ElevatorPositions.Intake)); - DogLog.log("subsystems/Elevator/targetLevel", this.getLevel().toString()); - DogLog.log( - "subsystems/Elevator/closedLoopError", master.getClosedLoopError().getValueAsDouble()); - DogLog.log( - "subsystems/Elevator/elevatorProfile", master.getClosedLoopReference().getValueAsDouble()); - } - - @Override - public void simulationPeriodic() { - // Simulate encoder behavior based on motor speed - double simulatedSpeed = master.getVelocity().getValueAsDouble(); - double currentPosition = master.getPosition().getValueAsDouble(); - - // Update simulated position based on speed (simplified example) - double newPosition = currentPosition + simulatedSpeed * 0.02; // Assuming a 20ms loop - master.setPosition( - newPosition); // Alarming to have this since running this on the robot will lead to + public double getHeight() { // getter method for height + return master.getRotorPosition().getValueAsDouble()* ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS/ ElevatorConstants.CARRAIGE_UPDUCTION; + } - // Log simulation data for debugging - DogLog.log("Simulated Position", newPosition); - DogLog.log("Simulated Speed", simulatedSpeed); - } + public void periodic() { //periodic -- repeats every 20 ms + DogLog.log( + "subsystems/Elevator/currentheight", + getHeight()); + } } +//change \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem1.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem1.java new file mode 100644 index 00000000..88c039d8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem1.java @@ -0,0 +1,329 @@ +// 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; + +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.Slot1Configs; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.CANrange; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import dev.doglog.DogLog; +import edu.wpi.first.math.filter.LinearFilter; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Constants.ElevatorConstants; +import frc.robot.Constants.ElevatorConstants.ElevatorPositions; +import frc.robot.util.LoggedTalonFX; + +// import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ElevatorSubsystem1 extends SubsystemBase { + private static ElevatorSubsystem instance; + + private LoggedTalonFX motor1; + private LoggedTalonFX motor2; + public LoggedTalonFX master; + + private LinearFilter elevatorFilter; + private double currentHeightToF; + private boolean elevatorZeroed; + + private MotionMagicConfigs mmc; + private ElevatorPositions currentLevel; + private CANrange distance; // Time of Flight (ToF) sensor + + private final MotionMagicVoltage controlRequest = new MotionMagicVoltage(0); + private final TorqueCurrentFOC torqueRequest = new TorqueCurrentFOC(0); + private final VelocityVoltage velocityRequest = new VelocityVoltage(0); + + private ElevatorSubsystem1() { + // Initialize motors + elevatorZeroed = false; + elevatorFilter = LinearFilter.singlePoleIIR(0.1, 0.02); + + distance = + new CANrange( + ElevatorConstants.CANRANGE_PORT, Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); + + motor1 = + new LoggedTalonFX( + "subsystems/Elevator/motor1", + ElevatorConstants.MOTOR1_PORT, + Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); + motor2 = + new LoggedTalonFX( + "subsystems/Elevator/motor2", + ElevatorConstants.MOTOR2_PORT, + Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); + + currentLevel = ElevatorPositions.Intake; + + // Set up motor followers and deal with inverted motors + Follower follower = new Follower(ElevatorConstants.MOTOR1_PORT, false); + motor2.setControl(follower); + + Slot1Configs s1c = + new Slot1Configs() + .withKP(ElevatorConstants.S1C_KP) + .withKI(ElevatorConstants.S1C_KI) + .withKD(ElevatorConstants.S1C_KD) + .withKS(ElevatorConstants.S0C_KS) + .withKG(ElevatorConstants.S0C_KG) + .withKA(ElevatorConstants.S0C_KA) + .withKV(ElevatorConstants.S0C_KV) + .withGravityType(GravityTypeValue.Elevator_Static) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + + Slot0Configs s0c = + new Slot0Configs() + .withKP(ElevatorConstants.S0C_KP) + .withKI(ElevatorConstants.S0C_KI) + .withKD(ElevatorConstants.S0C_KD) + .withKS(ElevatorConstants.S0C_KS) + .withKG(ElevatorConstants.S0C_KG) + .withKA(ElevatorConstants.S0C_KA) + .withKV(ElevatorConstants.S0C_KV) + .withGravityType(GravityTypeValue.Elevator_Static) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + + motor1.updateCurrentLimits( + ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); + motor2.updateCurrentLimits( + ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); + + TalonFXConfigurator m1Config = motor1.getConfigurator(); + TalonFXConfigurator m2Config = motor2.getConfigurator(); + + m1Config.apply(s0c); + m2Config.apply(s0c); + m1Config.apply(s1c); + m2Config.apply(s1c); + + MotorOutputConfigs moc = new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake); + + // Apply MotionMagic to motors + mmc = new MotionMagicConfigs(); + mmc.MotionMagicCruiseVelocity = ElevatorConstants.MOTIONMAGIC_MAX_VELOCITY; + mmc.MotionMagicAcceleration = ElevatorConstants.MOTIONMAGIC_MAX_ACCELERATION; + + m1Config.apply(mmc); + m2Config.apply(mmc); + + m1Config.apply(moc); + m2Config.apply(moc); + + master = motor1; + currentHeightToF = elevatorFilter.calculate(getToFDistance()); + resetPositionFiltered(); + } + + // instance for elevator subsystem + public static ElevatorSubsystem getInstance() { + if (instance == null) { + instance = new ElevatorSubsystem(); + } + return instance; + } + + public boolean tofIsConnected() { + return distance.isConnected(); + } + + public void resetPositionFiltered() { + master.setPosition( + currentHeightToF * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); + DogLog.log( + "subsystems/Elevator/resetElevatorPosition", + currentHeightToF * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); + } + + public void resetPosition() { + if (tofIsConnected()) { + master.setPosition( + this.getToFDistance() + * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); + DogLog.log( + "subsystems/Elevator/resetElevatorPosition", + this.getToFDistance() + * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); + } + } + + public void resetPosition(double posInHeight) { + // TODO: add constant to convert distance to encoder values + master.setPosition( + posInHeight * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); + DogLog.log( + "subsystems/Elevator/resetElevatorPosition", + posInHeight * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); + } + + // Hardstop Zeroing functions: + public void moveElevatorNegative() { + master.setControl(velocityRequest.withVelocity(-5).withSlot(1)); + } + + public void reduceCurrentLimits() { + master.updateCurrentLimits(30, 10); + } + + public void resetCurrentLimits() { + master.updateCurrentLimits( + Constants.ElevatorConstants.STATOR_CURRENT_LIMIT, + Constants.ElevatorConstants.SUPPLY_CURRENT_LIMIT); + } + + public void resetElevatorPositionToZero() { + master.setPosition(0); + // master.setControl(controlRequest.withPosition(master.getPosition().getValueAsDouble()).withSlot(0)); + // master.setPosition(0); + // master.setControl(controlRequest.withPosition(0).withSlot(0)); + // master.setPosition(0); + } + + public boolean checkCurrent() { + double Supplycurrent = Math.abs(master.getSupplyCurrent().getValue().magnitude()); + double Statorcurrent = Math.abs(master.getStatorCurrent().getValue().magnitude()); + DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/supply", Supplycurrent); + DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/stator", Statorcurrent); + + if (Supplycurrent > 1.0 && Statorcurrent > 20) { + return true; + } + return false; + } + + public double getError() { + return currentLevel.height + * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS + / Constants.ElevatorConstants.CARRAIGE_UPDUCTION + - master.getPosition().getValueAsDouble(); + } + + public ElevatorPositions getLevel() { + return currentLevel; + } + + public boolean atIntake() { + return currentLevel.equals(ElevatorPositions.Intake); + } + + public void elevateTo(ElevatorPositions level) { + this.currentLevel = level; + this.setPosition(level.height); + } + + public void setPosition(double height) { + master.setControl( + controlRequest + .withPosition( + height + * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS + / ElevatorConstants.CARRAIGE_UPDUCTION) + .withSlot(0)); + DogLog.log( + "subsystems/Elevator/elevatorSetpoint(rot)", + height + * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS + / ElevatorConstants.CARRAIGE_UPDUCTION); + } + + public void ElevatorTorqueMode() { + DogLog.log("subsystems/Elevator/usingTorqueMode", true); + master.setControl(torqueRequest.withOutput(Constants.ElevatorConstants.ELEVATOR_TORQUE)); + // .withMaxAbsDutyCycle(Constants.ElevatorConstants.ELEVATOR_DUTY_CYCLE)); + } + + // TODO: ONLY FOR DEBUGGING + public void testElevator(double height) { + this.setPosition(height); + } + + public boolean isAtPosition() { + return (Math.abs(getError()) <= ElevatorConstants.SETPOINT_TOLERANCE); + } + + public boolean canFunnelTransferCoralToScoring() { + return this.getLevel().equals(Constants.ElevatorConstants.ElevatorPositions.Intake) + && this.getError() < Constants.ElevatorConstants.MAX_POSITIONAL_ERROR; + } + + public double getToFDistance() { + // 0.11 is the sensor offset + DogLog.log( + "subsystems/Elevator/ToF/DistanceNoOffset", distance.getDistance().getValueAsDouble()); + return distance.getDistance().getValueAsDouble() - Constants.ElevatorConstants.SENSOR_OFFSET; + } + + public boolean isElevatorZeroed() { + return elevatorZeroed; + } + + public void elevatorHasBeenZeroed() { + elevatorZeroed = true; + } + + @Override + public void periodic() { + currentHeightToF = elevatorFilter.calculate(getToFDistance()); + // Time of Flight Sensor + DogLog.log("subsystems/Elevator/getError", getError()); + DogLog.log("subsystems/Elevator/ToF/Distance", getToFDistance()); + DogLog.log("subsystems/Elevator/ToF/Connected", distance.isConnected()); + DogLog.log("subsystems/Elevator/ToF/LinearFilterDistance", currentHeightToF); + + DogLog.log("subsystems/Elevator/isAtPosition", this.isAtPosition()); + DogLog.log("subsystems/Elevator/targetPosition", currentLevel.getPosition()); + DogLog.log("subsystems/Elevator/targetHeightDist", currentLevel.getHeight()); + DogLog.log( + "subsystems/Elevator/targetHeightRot", + currentLevel.getHeight() + * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS + / Constants.ElevatorConstants.CARRAIGE_UPDUCTION); + DogLog.log( + "subsystems/Elevator/currentHeightDist", + master.getPosition().getValueAsDouble() + * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_ROTATIONS_TO_DISTANCE + * Constants.ElevatorConstants.CARRAIGE_UPDUCTION); + DogLog.log("subsystems/Elevator/currentHeightRot", master.getPosition().getValueAsDouble()); + DogLog.log( + "subsystems/Elevator/command", + this.getCurrentCommand() == null ? "NOTHING" : this.getCurrentCommand().getName()); + DogLog.log( + "subsystems/Elevator/resetPositionBoolean", + this.isAtPosition() && this.getLevel().equals(ElevatorPositions.Intake)); + DogLog.log( + "subsystems/Elevator/targetisIntake", this.getLevel().equals(ElevatorPositions.Intake)); + DogLog.log("subsystems/Elevator/targetLevel", this.getLevel().toString()); + DogLog.log( + "subsystems/Elevator/closedLoopError", master.getClosedLoopError().getValueAsDouble()); + DogLog.log( + "subsystems/Elevator/elevatorProfile", master.getClosedLoopReference().getValueAsDouble()); + } + + @Override + public void simulationPeriodic() { + // Simulate encoder behavior based on motor speed + double simulatedSpeed = master.getVelocity().getValueAsDouble(); + double currentPosition = master.getPosition().getValueAsDouble(); + + // Update simulated position based on speed (simplified example) + double newPosition = currentPosition + simulatedSpeed * 0.02; // Assuming a 20ms loop + master.setPosition( + newPosition); // Alarming to have this since running this on the robot will lead to + + // Log simulation data for debugging + DogLog.log("Simulated Position", newPosition); + DogLog.log("Simulated Speed", simulatedSpeed); + } +} diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem2.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem2.java deleted file mode 100644 index 43178daf..00000000 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem2.java +++ /dev/null @@ -1,142 +0,0 @@ - -// 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; - -import com.ctre.phoenix6.configs.MotionMagicConfigs; //import statements -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.Slot1Configs; -import com.ctre.phoenix6.configs.TalonFXConfigurator; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.controls.TorqueCurrentFOC; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.CANrange; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.GravityTypeValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; -import dev.doglog.DogLog; -import edu.wpi.first.math.filter.LinearFilter; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.Constants.ElevatorConstants; -import frc.robot.util.LoggedTalonFX; - -// import frc.robot.Constants.Elevator.ElevatorPositions; - -public class ElevatorSubsystem2 extends SubsystemBase { //class declaration - private TalonFX motor1; // motors + master holder + target height declaration - private TalonFX motor2; - public TalonFX master; - public double targetHeight; - - private MotionMagicConfigs mmc; - - private final MotionMagicVoltage controlRequest = new MotionMagicVoltage(0); //motion magic control request - - public ElevatorSubsystem2() { //constructor - motor1 = //motor 1 initialization - new LoggedTalonFX( - "subsystems/Elevator/motor1", - ElevatorConstants.MOTOR1_PORT, - Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); - motor2 = //motor 2 initialization - new LoggedTalonFX( - "subsystems/Elevator/motor2", - ElevatorConstants.MOTOR2_PORT, - Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); - - Follower follower = new Follower(ElevatorConstants.MOTOR1_PORT, false); //setting motor 2 as the follower to motor 1(master) - motor2.setControl(follower); - - - Slot1Configs s1c = //slot 1 configs - new Slot1Configs() - // .withKP(ElevatorConstants.S1C_KP) - // .withKI(ElevatorConstants.S1C_KI) - // .withKD(ElevatorConstants.S1C_KD) - .withKS(ElevatorConstants.S0C_KS) - .withKG(ElevatorConstants.S0C_KG) - .withKA(ElevatorConstants.S0C_KA) - .withKV(ElevatorConstants.S0C_KV) - .withGravityType(GravityTypeValue.Elevator_Static) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - - Slot0Configs s0c = //slot 0 configs + PID - new Slot0Configs() - .withKP(ElevatorConstants.S0C_KP) - .withKI(ElevatorConstants.S0C_KI) - .withKD(ElevatorConstants.S0C_KD) - .withKS(ElevatorConstants.S0C_KS) - .withKG(ElevatorConstants.S0C_KG) - .withKA(ElevatorConstants.S0C_KA) - .withKV(ElevatorConstants.S0C_KV) - .withGravityType(GravityTypeValue.Elevator_Static) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - - ((LoggedTalonFX) motor1).updateCurrentLimits( //current limits of motor 1 and 2 - ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); - ((LoggedTalonFX) motor2).updateCurrentLimits( - ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); - - TalonFXConfigurator m1Config = motor1.getConfigurator(); - TalonFXConfigurator m2Config = motor2.getConfigurator(); - - m1Config.apply(s0c); - m2Config.apply(s0c); - m1Config.apply(s1c); - m2Config.apply(s1c); - - MotorOutputConfigs moc = new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake); - - // apply MotionMagic to motors - mmc = new MotionMagicConfigs(); - mmc.MotionMagicCruiseVelocity = ElevatorConstants.MOTIONMAGIC_MAX_VELOCITY; - mmc.MotionMagicAcceleration = ElevatorConstants.MOTIONMAGIC_MAX_ACCELERATION; - - m1Config.apply(mmc); - m2Config.apply(mmc); - - m1Config.apply(moc); - m2Config.apply(moc); - - master = motor1; //set motor 1 as master - } - - public void setPosition(double height) { //set position method - if (height < Constants.ElevatorConstants.minHeight) height = Constants.ElevatorConstants.minHeight; - if (height > Constants.ElevatorConstants.maxHeight) height = Constants.ElevatorConstants.maxHeight; - - master.setControl( //set master (motor 1) to height w control - controlRequest - .withPosition( - height - * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS - / ElevatorConstants.CARRAIGE_UPDUCTION) - .withSlot(0)); - DogLog.log( //log - "subsystems/Elevator/elevatorSetpoint(rot)", - height - * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS - / ElevatorConstants.CARRAIGE_UPDUCTION); - } - - public void resetElevatorPositionToZero() { //zero position - master.setPosition(0); - } - - public double getHeight() { // getter method for height - return master.getRotorPosition().getValueAsDouble()* ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS/ ElevatorConstants.CARRAIGE_UPDUCTION; - } - - public void periodic() { //periodic -- repeats every 20 ms - DogLog.log( - "subsystems/Elevator/currentheight", - getHeight()); - } -} -//change \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/LedSubsystem.java b/src/main/java/frc/robot/subsystems/LedSubsystem.java index 1133138c..3f492e1a 100644 --- a/src/main/java/frc/robot/subsystems/LedSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LedSubsystem.java @@ -133,3 +133,4 @@ public void periodic() { DogLog.log("Subsystem/LED/LEDstate", this.currentState.name); } } +//comment