From afba8731b39909db8a86c2d8a3a2a5b01adcaf98 Mon Sep 17 00:00:00 2001 From: NoahLacks63 Date: Wed, 15 Jan 2025 02:04:25 +0000 Subject: [PATCH] added a few manual controls for arm and elevator --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/constants/ArmConstants.java | 2 + .../robot/constants/ElevatorConstants.java | 17 +++-- src/main/java/frc/robot/subsystems/Arm.java | 31 ++++++++- .../java/frc/robot/subsystems/Elevator.java | 66 +++++++++++++++---- 5 files changed, 99 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f3d7d3d..296213b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -78,7 +78,7 @@ public class RobotContainer { ); elevator.setDefaultCommand( - elevator.goToSetpoint(0, 1) + elevator.runElevator(operator::getLeftY) ); indexer.setDefaultCommand( diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java index 1dfe976..f222b5f 100644 --- a/src/main/java/frc/robot/constants/ArmConstants.java +++ b/src/main/java/frc/robot/constants/ArmConstants.java @@ -5,4 +5,6 @@ public class ArmConstants { public static final int kCANcoderID = 0; public static final double kEncoderConversionFactor = 0; + + public static final double kArmMaxVelocity = 0; } diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index deb4332..1662fb3 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -11,20 +11,29 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; public class ElevatorConstants { - public static final int kElevatorMotorID = 0; + public static final int kElevatorMotor1ID = 0; + public static final int kElevatorMotor2ID = 0; + + public static final int kTopLimitSwitchID = 0; + public static final int kBottomLimitSwitchID = 0; public static final double kEncoderConversionFactor = 0; public static final int kMotorAmpsMax = 0; - public static final double kPIDControllerP = 0; - public static final double kPIDControllerI = 0; - public static final double kPIDControllerD = 0; + public static final double kPositionControllerP = 0; + public static final double kPositionControllerI = 0; + public static final double kPositionControllerD = 0; + + 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; public static final double kFeedForwardV = 0; + public static final double kElevatorMaxVelocity = 0; // 1, 7, 10 are the defaults for these, change as necessary public static final double kSysIDRampRate = 1; diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 66fb977..eeaf5ab 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -1,5 +1,7 @@ package frc.robot.subsystems; +import java.util.function.DoubleSupplier; + import com.ctre.phoenix6.hardware.CANcoder; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType; @@ -15,7 +17,9 @@ public class Arm extends SubsystemBase { private CANcoder canCoder; - private PIDController pidController; + private PIDController positionController; + private PIDController velocityController; + private ArmFeedforward feedForward; public Arm() { @@ -27,9 +31,26 @@ public class Arm extends SubsystemBase { canCoder = new CANcoder(ArmConstants.kCANcoderID); } + //manual command that keeps ouput speed consistent no matter the direction + public Command runArm(DoubleSupplier speed) { + return run(() -> { + double realSpeedTarget = speed.getAsDouble() * ArmConstants.kArmMaxVelocity; + + double voltsOut = velocityController.calculate( + rotationsToRadians(canCoder.getVelocity().getValueAsDouble()), + realSpeedTarget + ) + feedForward.calculate( + rotationsToRadians(canCoder.getPosition().getValueAsDouble()), + canCoder.getVelocity().getValueAsDouble() + ); + + armMotor.setVoltage(voltsOut); + }); + } + public Command goToSetpoint(double setpoint, double timeout) { return run(() -> { - double voltsOut = pidController.calculate( + double voltsOut = positionController.calculate( canCoder.getPosition().getValueAsDouble(), setpoint ) + feedForward.calculate( @@ -38,6 +59,10 @@ public class Arm extends SubsystemBase { ); armMotor.setVoltage(voltsOut); - }).until(pidController::atSetpoint).withTimeout(timeout); + }).until(positionController::atSetpoint).withTimeout(timeout); + } + + protected double rotationsToRadians(double rotations) { + return rotations * 2 * Math.PI; } } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index b111743..319608d 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -1,5 +1,7 @@ package frc.robot.subsystems; +import java.util.function.DoubleSupplier; + import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.PersistMode; @@ -8,22 +10,33 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.ElevatorConstants; public class Elevator extends SubsystemBase { protected SparkMax elevatorMotor1; + protected SparkMax elevatorMotor2; protected RelativeEncoder encoder; - private PIDController pidController; + private DigitalInput topLimitSwitch; + private DigitalInput bottomLimitSwitch; + + private PIDController positionController; + private PIDController velocityController; private ElevatorFeedforward feedForward; public Elevator() { elevatorMotor1 = new SparkMax( - ElevatorConstants.kElevatorMotorID, + ElevatorConstants.kElevatorMotor1ID, + MotorType.kBrushless + ); + + elevatorMotor2 = new SparkMax( + ElevatorConstants.kElevatorMotor2ID, MotorType.kBrushless ); @@ -33,13 +46,32 @@ public class Elevator extends SubsystemBase { PersistMode.kPersistParameters ); + elevatorMotor2.configure( + ElevatorConstants.motorConfig.follow(ElevatorConstants.kElevatorMotor1ID), + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); encoder = elevatorMotor1.getEncoder(); - pidController = new PIDController( - ElevatorConstants.kPIDControllerP, - ElevatorConstants.kPIDControllerI, - ElevatorConstants.kPIDControllerD + topLimitSwitch = new DigitalInput( + ElevatorConstants.kTopLimitSwitchID + ); + + bottomLimitSwitch = new DigitalInput( + ElevatorConstants.kBottomLimitSwitchID + ); + + positionController = new PIDController( + ElevatorConstants.kPositionControllerP, + ElevatorConstants.kPositionControllerI, + ElevatorConstants.kPositionControllerD + ); + + velocityController = new PIDController( + ElevatorConstants.kVelocityControllerP, + ElevatorConstants.kVelocityControllerI, + ElevatorConstants.kVelocityControllerD ); feedForward = new ElevatorFeedforward( @@ -49,20 +81,32 @@ public class Elevator extends SubsystemBase { ); } - public Command runElevator(double speed) { + //manual command that keeps ouput speed consistent no matter the direction + public Command runElevator(DoubleSupplier speed) { return run(() -> { - elevatorMotor1.set(speed); - }); + double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kElevatorMaxVelocity; + + double voltsOut = velocityController.calculate( + encoder.getVelocity(), + realSpeedTarget + ) + feedForward.calculate(realSpeedTarget); + + elevatorMotor1.setVoltage(voltsOut); + }).until(() -> topLimitSwitch.get() || bottomLimitSwitch.get()); } + + //go to setpoint command public Command goToSetpoint(double setpoint, double timeout) { return run(() -> { - double voltsOut = pidController.calculate( + double voltsOut = positionController.calculate( encoder.getPosition(), setpoint ) + feedForward.calculate(0); elevatorMotor1.setVoltage(voltsOut); - }).until(pidController::atSetpoint).withTimeout(timeout); + }).until( + () -> positionController.atSetpoint() || topLimitSwitch.get() || bottomLimitSwitch.get() + ).withTimeout(timeout); } }