From 56980d3772caf6236c61ff03b2017c81ed1fdf87 Mon Sep 17 00:00:00 2001 From: Tylr-J42 Date: Sat, 8 Feb 2025 03:27:59 -0500 Subject: [PATCH] removed velocity controllers on position mechanisms and added controller PID for elevator --- src/main/java/frc/robot/RobotContainer.java | 12 +++---- .../robot/constants/ElevatorConstants.java | 35 +++++++++++++------ .../java/frc/robot/subsystems/Elevator.java | 26 ++++++++++++-- .../java/frc/robot/subsystems/Vision.java | 7 +++- 4 files changed, 60 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 543a420..3feda2b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -89,17 +89,17 @@ public class RobotContainer { ) ); - elevator.setDefaultCommand( - elevator.runAssistedElevator(operator::getLeftY) - ); + //elevator.setDefaultCommand( + //elevator.runAssistedElevator(operator::getLeftY) + // ); manipulator.setDefaultCommand( manipulator.defaultCommand() ); - manipulatorPivot.setDefaultCommand( - manipulatorPivot.runAssistedPivot(operator::getRightY) - ); + //manipulatorPivot.setDefaultCommand( + // manipulatorPivot.runAssistedPivot(operator::getRightY) + //); //Driver inputs driver.start().whileTrue( diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index e401bd3..ee9dbe4 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -14,38 +14,42 @@ public class ElevatorConstants { public static final int kElevatorMotor1ID = 0; public static final int kElevatorMotor2ID = 0; - public static final int kTopLimitSwitchID = 0; public static final int kBottomLimitSwitchID = 0; - // 60/12 gearing multiplied by circumference of sprocket multiplied by 2 for carriage position - public static final double kEncoderConversionFactor = 11/60 * Math.PI * 1.75 * 2; + // 60/11 gearing multiplied by circumference of sprocket multiplied by 2 for carriage position + public static final double kEncoderConversionFactor = 11/60 * (22*0.25) * 2; - public static final int kMotorAmpsMax = 40; + public static final int kCurrentLimit = 40; public static final double kPositionControllerP = 0; public static final double kPositionControllerI = 0; public static final double kPositionControllerD = 0; + + public static final double kAllowedError = 0.25; + /* public static final double kVelocityControllerP = 0; public static final double kVelocityControllerI = 0; public static final double kVelocityControllerD = 0; + */ public static final double kFeedForwardS = 0; - public static final double kFeedForwardG = 0.41; // calculated value - public static final double kFeedForwardV = 1.75; // calculated value + public static final double kFeedForwardG = 0.53; // calculated value + public static final double kFeedForwardV = 4.78; // calculated value public static final double kMaxVelocity = 0; + public static final double kMaxAcceleration = 0; public static final double kCoralIntakePosition = 0; public static final double kL1Position = 0; - public static final double kL2Position = 0; - public static final double kL3Position = 0; - public static final double kL4Position = 0; + public static final double kL2Position = 14.5; + public static final double kL3Position = 29.0; + public static final double kL4Position = 53.0; public static final double kL2AlgaePosition = 0; public static final double kL3AlgaePosition = 0; /**The position of the top of the elevator brace */ public static final double kBracePosition = 0; - public static final double kMaxHeight = 0; + public static final double kMaxHeight = 53.0; // 1, 7, 10 are the defaults for these, change as necessary public static final double kSysIDRampRate = 1; @@ -66,10 +70,19 @@ public class ElevatorConstants { static { motorConfig - .smartCurrentLimit(kMotorAmpsMax) + .smartCurrentLimit(kCurrentLimit) .idleMode(kIdleMode); motorConfig.encoder .positionConversionFactor(kEncoderConversionFactor) .velocityConversionFactor(kEncoderConversionFactor / 60.0); + motorConfig.closedLoop + .p(kPositionControllerP) + .i(kPositionControllerI) + .d(kPositionControllerD) + .velocityFF(0.0); // keep at zero for position pid + motorConfig.closedLoop.maxMotion + .maxAcceleration(kMaxAcceleration) + .maxVelocity(kMaxVelocity) + .allowedClosedLoopError(kAllowedError); } } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index b7c647e..be7ccde 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -3,6 +3,9 @@ package frc.robot.subsystems; import java.util.function.DoubleSupplier; import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; @@ -21,12 +24,14 @@ public class Elevator extends SubsystemBase { protected SparkMax elevatorMotor1; protected SparkMax elevatorMotor2; + private SparkClosedLoopController elevatorClosedLoop; + protected RelativeEncoder encoder; private DigitalInput bottomLimitSwitch; private PIDController positionController; - private PIDController velocityController; + //private PIDController velocityController; private ElevatorFeedforward feedForward; @@ -41,6 +46,8 @@ public class Elevator extends SubsystemBase { MotorType.kBrushless ); + elevatorClosedLoop = elevatorMotor1.getClosedLoopController(); + elevatorMotor1.configure( ElevatorConstants.motorConfig, ResetMode.kResetSafeParameters, @@ -65,11 +72,13 @@ public class Elevator extends SubsystemBase { ElevatorConstants.kPositionControllerD ); + /* velocityController = new PIDController( ElevatorConstants.kVelocityControllerP, ElevatorConstants.kVelocityControllerI, ElevatorConstants.kVelocityControllerD ); + */ feedForward = new ElevatorFeedforward( ElevatorConstants.kFeedForwardS, @@ -123,7 +132,7 @@ public class Elevator extends SubsystemBase { * * @param speed How fast the elevator moves * @return Sets motor voltage to move the elevator relative to the speed parameter - */ + * public Command runAssistedElevator(DoubleSupplier speed) { return run(() -> { double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kMaxVelocity; @@ -137,6 +146,7 @@ public class Elevator extends SubsystemBase { }).until( () -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kMaxHeight); } + */ /** * Moves the elevator to a target destination (setpoint). @@ -153,6 +163,16 @@ public class Elevator extends SubsystemBase { ElevatorConstants.kMaxHeight ); + return run(() -> { + + elevatorClosedLoop.setReference(clampedSetpoint, + SparkBase.ControlType.kMAXMotionPositionControl, + ClosedLoopSlot.kSlot0, + feedForward.calculate(0) + ); + + }); +/* if (clampedSetpoint == 0) { return run(() -> { double voltsOut = positionController.calculate( @@ -181,6 +201,8 @@ public class Elevator extends SubsystemBase { () -> positionController.atSetpoint() || bottomLimitSwitch.get() ).withTimeout(timeout); } + */ + } /** diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 72a16a0..e1fe524 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -11,6 +11,8 @@ public class Vision{ private DoubleSubscriber cam1ClosestTag; private BooleanSubscriber cam1TagDetected; + private DoubleSubscriber cam1TimeStamp; + private DoubleSubscriber framerate; public Vision(){ @@ -22,7 +24,6 @@ public class Vision{ cam1ClosestTag = visionTable.getDoubleTopic("cam1ClosestTag").subscribe(0.0); cam1TagDetected = visionTable.getBooleanTopic("cam1_visible").subscribe(false); - // cam2TagDetected = visionTable.getBooleanTopic("cam2_visible").subscribe(false); framerate = visionTable.getDoubleTopic("fps").subscribe(0.0); } @@ -39,6 +40,10 @@ public class Vision{ return cam1TagDetected.get(); } + public double getCam1TimeStamp(){ + return cam1GlobalPose.getLastChange(); + } + public double getFPS(){ return framerate.get(); }