From 69f4a79b6dae7b98001f8cbb2650b50828acf565 Mon Sep 17 00:00:00 2001 From: 24cirinom <112493487+24cirinom@users.noreply.github.com> Date: Mon, 19 Feb 2024 16:37:35 -0600 Subject: [PATCH] arm elevator --- src/main/java/frc/robot/Constants.java | 6 +- .../subsystems/armElevator/ArmElevator.java | 18 ++-- .../subsystems/armElevator/ArmElevatorIO.java | 11 ++- .../armElevator/ArmElevatorIOSim.java | 4 +- .../armElevator/ArmElevatorIOSparkMax.java | 42 ++++++--- .../subsystems/climber/ClimberIOSparkMax.java | 1 - .../flywheel/BottomFlywheelIOSparkMax.java | 86 ------------------- 7 files changed, 53 insertions(+), 115 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/flywheel/BottomFlywheelIOSparkMax.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2ece5c9..a71d8a3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -128,7 +128,7 @@ public static final class ArmElevatorConstants { public static final int kCurrentLimit = 30; public static final int initialExtension = 0; // TODO TUNE - public static final int maxExtension = 1; // TODO TUNE + public static final int maxExtension = 1000; // TODO TUNE public static final double armElevatorConversion = 1; // TODO TUNE @@ -138,8 +138,8 @@ public static final class ArmElevatorConstants { public static final double kD = 0.0; // TODO TUNE // Arm Elevator Feedforward characterization constants - public static final double ks = 0.10; // TODO TUNE - public static final double kv = 0.05; // TODO TUNE + public static final double ks = 0; // TODO TUNE + public static final double kv = 0; // TODO TUNE // public static final double ArmThreshold = 0.1; // TODO TUNE diff --git a/src/main/java/frc/robot/subsystems/armElevator/ArmElevator.java b/src/main/java/frc/robot/subsystems/armElevator/ArmElevator.java index a24add0..1174576 100644 --- a/src/main/java/frc/robot/subsystems/armElevator/ArmElevator.java +++ b/src/main/java/frc/robot/subsystems/armElevator/ArmElevator.java @@ -73,14 +73,18 @@ public void stop() { io.stop(); } - /** Returns the current velocity in RPM. */ - @AutoLogOutput - public double getVelocityRPM() { - return Units.radiansPerSecondToRotationsPerMinute(inputs.velocityRadPerSec); + @AutoLogOutput(key = "ArmElevator/TargetDistance") + public double getTargetDistance() { + return inputs.targetDistance; } - /** Returns the current velocity in radians per second. */ - public double getCharacterizationVelocity() { - return inputs.velocityRadPerSec; + // get the current position of the elevator + @AutoLogOutput(key = "ArmElevator/PositionMeters") + public double getPositionMeters() { + return inputs.positionMeters; + } + + public void setTargetDistance(double distanceMeters) { + io.setTargetDistance(distanceMeters); } } diff --git a/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIO.java b/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIO.java index 16814d7..626cdde 100644 --- a/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIO.java @@ -18,10 +18,9 @@ public interface ArmElevatorIO { @AutoLog public static class ArmElevatorIOInputs { - public double position = 0.0; - public double targetPosition = 0.0; - public double appliedVolts = 0.0; - public double velocityRadPerSec = 0.0; + public double positionMeters = 0.0; + public double velocityMetersPerSec = 0.0; + public double targetDistance = 0.0; public double[] currentAmps = new double[] {}; } @@ -39,4 +38,8 @@ public default void stop() {} /** Set velocity PID constants. */ public default void configurePID(double kP, double kI, double kD) {} + + public default void setTargetDistance(double distance) {} + + } diff --git a/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSim.java b/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSim.java index 9e021c5..e3392bd 100644 --- a/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSim.java @@ -40,8 +40,8 @@ public void updateInputs(ArmElevatorIOInputs inputs) { sim.update(0.02); - inputs.position = 0.0; - inputs.appliedVolts = appliedVolts; + inputs.targetDistance = 0.0; + inputs.positionMeters = sim.getPositionMeters(); inputs.currentAmps = new double[] {sim.getCurrentDrawAmps()}; } diff --git a/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSparkMax.java b/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSparkMax.java index b347ae2..6d62816 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.util.Units; import frc.robot.Constants.ArmElevatorConstants; /** @@ -26,9 +25,12 @@ * "CANSparkFlex". */ public class ArmElevatorIOSparkMax implements ArmElevatorIO { - private static final double GEAR_RATIO = 52.0 / 34.0; // May be reciprocal + 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_CIRCUMFERENCE = + Math.PI * PULLEY_DIAMETER; // pulley circumference - public double targetPosition = 0.0; + private double targetDistance = 0.0; private final CANSparkMax motor = new CANSparkMax(ArmElevatorConstants.kArmElevatorMotorId, MotorType.kBrushless); @@ -53,10 +55,12 @@ public ArmElevatorIOSparkMax() { @Override public void updateInputs(ArmElevatorIOInputs inputs) { - inputs.position = encoder.getPosition() / GEAR_RATIO; - inputs.targetPosition = targetPosition; - inputs.appliedVolts = motor.getAppliedOutput() * motor.getBusVoltage(); + inputs.positionMeters = encoder.getPosition() / GEAR_RATIO * PULLEY_CIRCUMFERENCE; + inputs.velocityMetersPerSec = encoder.getVelocity() / GEAR_RATIO * PULLEY_CIRCUMFERENCE; + inputs.targetDistance = targetDistance; inputs.currentAmps = new double[] {motor.getOutputCurrent()}; + + System.out.println(encoder.getPosition() / GEAR_RATIO * PULLEY_CIRCUMFERENCE); } @Override @@ -66,12 +70,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 @@ -86,4 +90,18 @@ public void configurePID(double kP, double kI, double kD) { pid.setD(kD, 0); pid.setFF(0, 0); } + + @Override + public void setTargetDistance(double distanceInches) { + targetDistance = distanceMeters; + pid.setReference( + distanceMeters / PULLEY_CIRCUMFERENCE * GEAR_RATIO, + ControlType.kPosition, + 0, + 0, + ArbFFUnits.kVoltage); + + // targetDistance = MathUtil.clamp(targetDistance, ArmConstants.minimumDistance, + // ArmConstants.maximumDistance); + } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkMax.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkMax.java index 669d5d7..5063294 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkMax.java @@ -59,7 +59,6 @@ public ClimberIOSparkMax() { leftMotor.setSmartCurrentLimit(ClimberConstants.kCurrentLimit); rightMotor.setSmartCurrentLimit(ClimberConstants.kCurrentLimit); - // leftMotor.burnFlash(); // rightMotor.burnFlash(); } diff --git a/src/main/java/frc/robot/subsystems/flywheel/BottomFlywheelIOSparkMax.java b/src/main/java/frc/robot/subsystems/flywheel/BottomFlywheelIOSparkMax.java deleted file mode 100644 index 9aa0601..0000000 --- a/src/main/java/frc/robot/subsystems/flywheel/BottomFlywheelIOSparkMax.java +++ /dev/null @@ -1,86 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.flywheel; - -import com.revrobotics.CANSparkBase.ControlType; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkPIDController; -import com.revrobotics.SparkPIDController.ArbFFUnits; -import edu.wpi.first.math.util.Units; -import frc.robot.Constants.FlywheelConstants; - -/** - * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with - * "CANSparkFlex". - */ -public class BottomFlywheelIOSparkMax implements FlywheelIO { - private static final double GEAR_RATIO = 52.0 / 34.0; // May be recipricol - - private final CANSparkMax leader = - new CANSparkMax(FlywheelConstants.kBottomFlywheelSparkMaxCanId, MotorType.kBrushless); - private final RelativeEncoder encoder = leader.getEncoder(); - private final SparkPIDController pid = leader.getPIDController(); - - public BottomFlywheelIOSparkMax() { - leader.restoreFactoryDefaults(); - - leader.setCANTimeout(250); - - leader.setInverted(false); - - leader.enableVoltageCompensation(12.0); - leader.setSmartCurrentLimit(30); - - leader.burnFlash(); - } - - @Override - public void updateInputs(FlywheelIOInputs inputs) { - inputs.positionRad = Units.rotationsToRadians(encoder.getPosition() / GEAR_RATIO); - inputs.velocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / GEAR_RATIO); - inputs.appliedVolts = leader.getAppliedOutput() * leader.getBusVoltage(); - inputs.currentAmps = new double[] {leader.getOutputCurrent()}; - } - - @Override - public void setVoltage(double volts) { - leader.setVoltage(volts); - } - - @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { - pid.setReference( - Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO, - ControlType.kVelocity, - 0, - ffVolts, - ArbFFUnits.kVoltage); - } - - @Override - public void stop() { - leader.stopMotor(); - } - - @Override - public void configurePID(double kP, double kI, double kD) { - pid.setP(kP, 0); - pid.setI(kI, 0); - pid.setD(kD, 0); - pid.setFF(0, 0); - } -}