From 70dbca83d41ea132de4a747e3fc6ca3ee674575b Mon Sep 17 00:00:00 2001 From: 24cirinom <112493487+24cirinom@users.noreply.github.com> Date: Mon, 19 Feb 2024 18:21:58 -0600 Subject: [PATCH] working armelevator --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/RobotContainer.java | 5 ++++- .../ControlCommands/ArmElevatorCommands.java | 11 +++++----- .../robot/subsystems/arm/ArmIOSparkMax.java | 1 - .../subsystems/armElevator/ArmElevator.java | 8 ++++--- .../armElevator/ArmElevatorIOSparkMax.java | 22 +++++++++---------- .../subsystems/wrist/WristIOSparkMax.java | 2 +- 7 files changed, 29 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d7d67c7..35a3a97 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -131,10 +131,10 @@ public static final class ArmElevatorConstants { public static double minExtension = 0; public static double maxExtension = 16; // Inches - // public static final double armElevatorConversion = 1; + // public static final double armElevatorConversion = 1; // ArmElevator PID constants - public static final double kP = 0.1; // TODO TUNE + public static final double kP = 1; // TODO TUNE public static final double kI = 0.0; // TODO TUNE public static final double kD = 0.0; // TODO TUNE diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 60cdd1e..77416cd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -222,7 +222,10 @@ private void configureButtonBindings() { WristCommands.joystickDrive(sWrist, () -> operatorController.getLeftY())); sArmElevator.setDefaultCommand( - ArmElevatorCommands.triggerDrive(sArmElevator, () -> operatorController.getLeftTriggerAxis(), () -> operatorController.getRightTriggerAxis())); + ArmElevatorCommands.triggerDrive( + sArmElevator, + () -> operatorController.getLeftTriggerAxis(), + () -> operatorController.getRightTriggerAxis())); operatorController .a() diff --git a/src/main/java/frc/robot/commands/ControlCommands/ArmElevatorCommands.java b/src/main/java/frc/robot/commands/ControlCommands/ArmElevatorCommands.java index 4c36f74..2532f78 100644 --- a/src/main/java/frc/robot/commands/ControlCommands/ArmElevatorCommands.java +++ b/src/main/java/frc/robot/commands/ControlCommands/ArmElevatorCommands.java @@ -27,14 +27,15 @@ private ArmElevatorCommands() {} /** Wrist command using one axis of a joystick (controlling wrist velocity). */ public static Command triggerDrive( - ArmElevator armElevator, - DoubleSupplier leftTriggerSupplier, - DoubleSupplier rightTriggerSupplier) { + ArmElevator armElevator, + DoubleSupplier leftTriggerSupplier, + DoubleSupplier rightTriggerSupplier) { return Commands.run( () -> { // Apply deadband (i.e. min DEADBAND max 1.0) - double stickMagnitude = MathUtil.applyDeadband(leftTriggerSupplier.getAsDouble(), DEADBAND) - + -MathUtil.applyDeadband(rightTriggerSupplier.getAsDouble(), DEADBAND); + double stickMagnitude = + MathUtil.applyDeadband(leftTriggerSupplier.getAsDouble(), DEADBAND) + + -MathUtil.applyDeadband(rightTriggerSupplier.getAsDouble(), DEADBAND); // Calcaulate new rotational velocity double rotationalVelocity = stickMagnitude * MAXINCHESPERSECOND; diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java index f92f999..9275569 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java @@ -22,7 +22,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.util.Units; import frc.robot.Constants.ArmConstants; -import frc.robot.commands.ControlCommands.ArmCommands; /** * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with diff --git a/src/main/java/frc/robot/subsystems/armElevator/ArmElevator.java b/src/main/java/frc/robot/subsystems/armElevator/ArmElevator.java index 5059686..4114604 100644 --- a/src/main/java/frc/robot/subsystems/armElevator/ArmElevator.java +++ b/src/main/java/frc/robot/subsystems/armElevator/ArmElevator.java @@ -31,6 +31,7 @@ public ArmElevator(ArmElevatorIO io) { // Switch constants based on mode (the physics simulator is treated as a // separate robot with different tuning) + switch (EnvironmentalConstants.currentMode) { case REAL: io.configurePID(ArmElevatorConstants.kP, ArmElevatorConstants.kI, ArmElevatorConstants.kD); @@ -69,9 +70,10 @@ public void runVolts(double volts) { // } public void runTargetVelocity(double velocityInchesPerSecond) { - io.setTargetDistance(inputs.targetDistance - + 0.02 // TODO cycle time needed - * velocityInchesPerSecond); + io.setTargetDistance( + inputs.targetDistance + + 0.02 // TODO cycle time needed + * velocityInchesPerSecond); } /** Stops the ArmElevator. */ diff --git a/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSparkMax.java b/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSparkMax.java index b61811b..db03f52 100644 --- a/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSparkMax.java @@ -18,7 +18,6 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; import com.revrobotics.SparkPIDController.ArbFFUnits; - import edu.wpi.first.math.MathUtil; import frc.robot.Constants.ArmElevatorConstants; @@ -28,7 +27,7 @@ */ public class ArmElevatorIOSparkMax implements ArmElevatorIO { private static final double GEAR_RATIO = 40.0; // gear ratio - private static final double PULLEY_DIAMETER = 2; // pulley diameter in meters + private static final double PULLEY_DIAMETER = 2.0; // pulley diameter in meters private static final double PULLEY_CIRCUMFERENCE = Math.PI * PULLEY_DIAMETER; // pulley circumference @@ -50,7 +49,7 @@ public ArmElevatorIOSparkMax() { motor.setSmartCurrentLimit(ArmElevatorConstants.kCurrentLimit); - pid.setOutputRange(0, ArmElevatorConstants.maxExtension); + // pid.setOutputRange(0, ArmElevatorConstants.maxExtension); // motor.burnFlash(); } @@ -61,7 +60,6 @@ public void updateInputs(ArmElevatorIOInputs inputs) { inputs.velocityMetersPerSec = encoder.getVelocity() / GEAR_RATIO * PULLEY_CIRCUMFERENCE; inputs.targetDistance = targetDistance; inputs.currentAmps = new double[] {motor.getOutputCurrent()}; - } @Override @@ -71,12 +69,12 @@ public void setVoltage(double volts) { // @Override // public void setVelocity(double velocityRadPerSec, double ffVolts) { - // pid.setReference( - // Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO, - // ControlType.kVelocity, - // 0, - // ffVolts, - // ArbFFUnits.kVoltage); + // pid.setReference( + // Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO, + // ControlType.kVelocity, + // 0, + // ffVolts, + // ArbFFUnits.kVoltage); // } @Override @@ -94,7 +92,9 @@ public void configurePID(double kP, double kI, double kD) { @Override public void setTargetDistance(double distanceInches) { - targetDistance = MathUtil.clamp(distanceInches, ArmElevatorConstants.minExtension, ArmElevatorConstants.maxExtension); + targetDistance = + MathUtil.clamp( + distanceInches, ArmElevatorConstants.minExtension, ArmElevatorConstants.maxExtension); pid.setReference( targetDistance / PULLEY_CIRCUMFERENCE * GEAR_RATIO, ControlType.kPosition, diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOSparkMax.java b/src/main/java/frc/robot/subsystems/wrist/WristIOSparkMax.java index b08cb58..51a7c50 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIOSparkMax.java @@ -41,7 +41,7 @@ public WristIOSparkMax() { motor.setCANTimeout(250); - encoder.setPosition(WristConstants.initialAngle); + // encoder.setPosition(WristConstants.initialAngle); // motor.enableVoltageCompensation(12.0);