diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fbfbd85..d7d67c7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -128,9 +128,10 @@ public static final class ArmElevatorConstants { public static final int kCurrentLimit = 30; public static final int initialExtension = 0; // TODO TUNE - public static final int maxExtension = 1000; // TODO TUNE + public static double minExtension = 0; + public static double maxExtension = 16; // Inches - public static final double armElevatorConversion = 1; // TODO TUNE + // public static final double armElevatorConversion = 1; // ArmElevator PID constants public static final double kP = 0.1; // TODO TUNE @@ -140,9 +141,6 @@ public static final class ArmElevatorConstants { // Arm Elevator Feedforward characterization constants 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 - } public static final class Presets { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 50feec0..60cdd1e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.ControlCommands.ArmCommands; +import frc.robot.commands.ControlCommands.ArmElevatorCommands; import frc.robot.commands.ControlCommands.DriveCommands; import frc.robot.commands.ControlCommands.WristCommands; import frc.robot.commands.FeedForwardCharacterization; @@ -220,6 +221,9 @@ private void configureButtonBindings() { sWrist.setDefaultCommand( WristCommands.joystickDrive(sWrist, () -> operatorController.getLeftY())); + sArmElevator.setDefaultCommand( + ArmElevatorCommands.triggerDrive(sArmElevator, () -> operatorController.getLeftTriggerAxis(), () -> operatorController.getRightTriggerAxis())); + operatorController .a() .whileTrue( diff --git a/src/main/java/frc/robot/commands/ControlCommands/ArmCommands.java b/src/main/java/frc/robot/commands/ControlCommands/ArmCommands.java index 08f5d11..335fa6e 100644 --- a/src/main/java/frc/robot/commands/ControlCommands/ArmCommands.java +++ b/src/main/java/frc/robot/commands/ControlCommands/ArmCommands.java @@ -1,4 +1,4 @@ -// Copyright 2021-2024 FRC 6328 +// Copyright 2021-2024 FRC 6328, FRC 5829 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/commands/ControlCommands/ArmElevatorCommands.java b/src/main/java/frc/robot/commands/ControlCommands/ArmElevatorCommands.java new file mode 100644 index 0000000..4c36f74 --- /dev/null +++ b/src/main/java/frc/robot/commands/ControlCommands/ArmElevatorCommands.java @@ -0,0 +1,47 @@ +// Copyright 2021-2024 FRC 6328, FRC 5829 +// 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.commands.ControlCommands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.armElevator.ArmElevator; +import java.util.function.DoubleSupplier; + +public class ArmElevatorCommands { + private static final double DEADBAND = 0.3; + private static final double MAXINCHESPERSECOND = 3; + + private ArmElevatorCommands() {} + + /** Wrist command using one axis of a joystick (controlling wrist velocity). */ + public static Command triggerDrive( + 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); + + // Calcaulate new rotational velocity + double rotationalVelocity = stickMagnitude * MAXINCHESPERSECOND; + + // Send command to wrist subsystem to run wrist + armElevator.runTargetVelocity(rotationalVelocity); + }, + armElevator); + } +} diff --git a/src/main/java/frc/robot/commands/ControlCommands/WristCommands.java b/src/main/java/frc/robot/commands/ControlCommands/WristCommands.java index b3d5566..720f465 100644 --- a/src/main/java/frc/robot/commands/ControlCommands/WristCommands.java +++ b/src/main/java/frc/robot/commands/ControlCommands/WristCommands.java @@ -1,4 +1,4 @@ -// Copyright 2021-2024 FRC 6328 +// Copyright 2021-2024 FRC 6328, FRC 5829 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java index de2cbe4..f92f999 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java @@ -22,6 +22,7 @@ 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 @@ -42,7 +43,7 @@ public class ArmIOSparkMax implements ArmIO { private final SparkPIDController pid = leftMotor.getPIDController(); - private double targetAngle = 0; // Radians, just a default value. + private double targetAngle = 0.0; // Radians, just a default value. public ArmIOSparkMax() { leftMotor.restoreFactoryDefaults(); @@ -118,52 +119,4 @@ public void configurePID(double kP, double kI, double kD) { pid.setD(kD, 0); pid.setFF(0, 0); } - - // TODO: The following commented code is from the ArmSubsystem - see if it needs to be integrated - // here - /* - public void setRotation(double value) { - armHeight = value; - } - - public void setDegrees(double degrees) { - double encoderunits = - Convert.angleToEncoderPos(degrees, kArmGearRatio, Encoder.RevRelativeEncoder); - armHeight = encoderunits; - } - - public double getAngle() { - final double rawRevs = mRightArmEncoder.getPosition(); - final double theta = - Convert.encoderPosToAngle(rawRevs, kArmGearRatio, Encoder.RevRelativeEncoder); - return ArmConstants.startingAngle - theta; - } - - public void resetEncoderValue() { - armHeight = 0; - mRightArmEncoder.setPosition(armHeight); - } - - public void drive(double pct) { - - // MathUtil.clamp(armHeight, ArmConstants.minimumHeight, getMaximumRotation()); - armHeight += pct * ArmConstants.armConversion; - } - - public boolean isFinished() { - return Math.abs(mRightArmEncoder.getPosition() - armHeight) < Presets.ArmThreshold; - } - - @Override - public void periodic() { - mRightArmPIDController.setReference(armHeight, CANSparkMax.ControlType.kPosition, 0); - // SmartDashboard.putNumber( - // "Arm Error", - // Convert.encoderPosToAngle( - // mRightArmEncoder.getPosition() - armHeight, kArmGearRatio, - // Encoder.RevRelativeEncoder)); - // SmartDashboard.putNumber("Arm angle", -(this.getAngle() - Arm.startingAngle)); - } - */ - } diff --git a/src/main/java/frc/robot/subsystems/armElevator/ArmElevator.java b/src/main/java/frc/robot/subsystems/armElevator/ArmElevator.java index 1174576..815712b 100644 --- a/src/main/java/frc/robot/subsystems/armElevator/ArmElevator.java +++ b/src/main/java/frc/robot/subsystems/armElevator/ArmElevator.java @@ -14,7 +14,6 @@ package frc.robot.subsystems.armElevator; import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ArmElevatorConstants; import frc.robot.Constants.EnvironmentalConstants; @@ -59,13 +58,19 @@ public void runVolts(double volts) { io.setVoltage(volts); } - /** Run closed loop at the specified velocity. */ - public void runVelocity(double velocityRPM) { - var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); - io.setVelocity(velocityRadPerSec, ffModel.calculate(velocityRadPerSec)); + // /** Run closed loop at the specified velocity. */ + // public void runVelocity(double velocityRPM) { + // var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); + // io.setVelocity(velocityRadPerSec, ffModel.calculate(velocityRadPerSec)); - // Log ArmElevator setpoint - Logger.recordOutput("ArmElevator/SetpointRPM", velocityRPM); + // // Log ArmElevator setpoint + // Logger.recordOutput("ArmElevator/SetpointRPM", velocityRPM); + // } + + public void runTargetVelocity(double velocityInchesPerSecond) { + io.setTargetDistance(inputs.targetDistance + + 0.02 // TODO cycle time needed + * velocityInchesPerSecond); } /** Stops the ArmElevator. */ @@ -79,12 +84,12 @@ public double getTargetDistance() { } // get the current position of the elevator - @AutoLogOutput(key = "ArmElevator/PositionMeters") + @AutoLogOutput(key = "ArmElevator/PositionInches") public double getPositionMeters() { - return inputs.positionMeters; + return inputs.positionInches; } - public void setTargetDistance(double distanceMeters) { - io.setTargetDistance(distanceMeters); - } + // public void setTargetDistance(double distanceInches) { + // io.setTargetDistance(distanceInches); + // } } diff --git a/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIO.java b/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIO.java index ea11a31..86a7f26 100644 --- a/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIO.java @@ -18,7 +18,7 @@ public interface ArmElevatorIO { @AutoLog public static class ArmElevatorIOInputs { - public double positionMeters = 0.0; + public double positionInches = 0.0; public double velocityMetersPerSec = 0.0; public double targetDistance = 0.0; public double[] currentAmps = new double[] {}; @@ -30,14 +30,12 @@ public default void updateInputs(ArmElevatorIOInputs inputs) {} /** Run open loop at the specified voltage. */ public default void setVoltage(double volts) {} - /** Run closed loop at the specified velocity. */ - public default void setVelocity(double velocityRadPerSec, double ffVolts) {} - /** Stop in open loop. */ public default void stop() {} /** Set velocity PID constants. */ public default void configurePID(double kP, double kI, double kD) {} + /** In inches */ 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 e3392bd..10d924a 100644 --- a/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSim.java @@ -41,7 +41,7 @@ public void updateInputs(ArmElevatorIOInputs inputs) { sim.update(0.02); inputs.targetDistance = 0.0; - inputs.positionMeters = sim.getPositionMeters(); + inputs.positionInches = sim.getPositionMeters(); inputs.currentAmps = new double[] {sim.getCurrentDrawAmps()}; } @@ -52,12 +52,12 @@ public void setVoltage(double volts) { sim.setInputVoltage(volts); } - @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { - closedLoop = true; - pid.setSetpoint(velocityRadPerSec); - this.ffVolts = ffVolts; - } + // @Override + // public void setVelocity(double velocityRadPerSec, double ffVolts) { + // closedLoop = true; + // pid.setSetpoint(velocityRadPerSec); + // this.ffVolts = ffVolts; + // } @Override public void stop() { diff --git a/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSparkMax.java b/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSparkMax.java index 7145d83..b61811b 100644 --- a/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSparkMax.java @@ -18,6 +18,8 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; import com.revrobotics.SparkPIDController.ArbFFUnits; + +import edu.wpi.first.math.MathUtil; import frc.robot.Constants.ArmElevatorConstants; /** @@ -42,9 +44,9 @@ public ArmElevatorIOSparkMax() { motor.setCANTimeout(250); - // motor.setInverted(false); + motor.setInverted(true); - motor.enableVoltageCompensation(12.0); + // motor.enableVoltageCompensation(12.0); motor.setSmartCurrentLimit(ArmElevatorConstants.kCurrentLimit); @@ -55,7 +57,7 @@ public ArmElevatorIOSparkMax() { @Override public void updateInputs(ArmElevatorIOInputs inputs) { - inputs.positionMeters = encoder.getPosition() / GEAR_RATIO * PULLEY_CIRCUMFERENCE; + inputs.positionInches = encoder.getPosition() / GEAR_RATIO * PULLEY_CIRCUMFERENCE; inputs.velocityMetersPerSec = encoder.getVelocity() / GEAR_RATIO * PULLEY_CIRCUMFERENCE; inputs.targetDistance = targetDistance; inputs.currentAmps = new double[] {motor.getOutputCurrent()}; @@ -67,15 +69,15 @@ public void setVoltage(double volts) { motor.setVoltage(volts); } - @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { + // @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() { @@ -91,16 +93,13 @@ public void configurePID(double kP, double kI, double kD) { } @Override - public void setTargetDistance(double distanceMeters) { - targetDistance = distanceMeters; + public void setTargetDistance(double distanceInches) { + targetDistance = MathUtil.clamp(distanceInches, ArmElevatorConstants.minExtension, ArmElevatorConstants.maxExtension); pid.setReference( - distanceMeters / PULLEY_CIRCUMFERENCE * GEAR_RATIO, + targetDistance / 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/armElevator/ArmElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorSubsystem.java deleted file mode 100644 index eae2ba7..0000000 --- a/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorSubsystem.java +++ /dev/null @@ -1,123 +0,0 @@ -// Copyright 2016-2024 FRC 5829 -// -// 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. - -// ! Reference file from last year -/* -package frc.robot.subsystems.MechanicalParts; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkMaxPIDController; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.ArmElevator; -import frc.robot.Constants.Presets; -import frc.robot.RobotContainer; -import frc.util.math.Convert; -import frc.util.math.Convert.Encoder; - -public class ArmElevatorSubsystem extends SubsystemBase { - - private CANSparkMax mArmMotor; - - public final RelativeEncoder mArmEncoder; - - private final SparkMaxPIDController mArmPIDController; - - public double armExtent; - private final double kArmGearRatio = (1.0 / 9.0) / (48.0 / 34.0); - private final double kDiameter = 1.5; - - public ArmElevatorSubsystem() { - - mArmMotor = new CANSparkMax(ArmElevator.kArmMotorId, MotorType.kBrushless); - mArmMotor.restoreFactoryDefaults(); - - // Current limit - mArmMotor.setSmartCurrentLimit(ArmElevator.kCurrentLimit); - - armExtent = ArmElevator.initialExtent; - - mArmEncoder = mArmMotor.getEncoder(); - - mArmPIDController = mArmMotor.getPIDController(); - - // mArmPIDController.setP(ArmElevator.kP); - // mArmPIDController.setI(ArmElevator.kI); - // mArmPIDController.setD(ArmElevator.kD); - mArmPIDController.setP(0.08); - mArmPIDController.setI(0); - mArmPIDController.setD(0); - - mArmPIDController.setOutputRange(-0.6, 0.6); - - // mRightArmPIDController.setFeedbackDevice(mRightArmEncoder); - - } - - public void setExtent(double value) { - armExtent = value; - } - - public void setExtentInches(double inches) { - double sensorUnits = - Convert.distanceToEncoderPos(inches, kArmGearRatio, kDiameter, Encoder.RevRelativeEncoder); - setExtent(sensorUnits); - } - - public double getExtent() { - final double rawRevs = mArmEncoder.getPosition(); - System.out.println(rawRevs); - final double extent = - Convert.encoderPosToDistance(rawRevs, kArmGearRatio, kDiameter, Encoder.RevRelativeEncoder); - System.out.println(extent); - return extent; - } - - public void resetEncoderValue() { - armExtent = 0; - mArmEncoder.setPosition(armExtent); - } - - public void drive(double pct) { - if (!RobotContainer.getResetPosMode()) { - armExtent = MathUtil.clamp(armExtent, ArmElevator.minimumExtent, ArmElevator.maximumExtend); - } - // MathUtil.clamp(armHeight, ArmConstants.minimumHeight, getMaximumRotation()); - armExtent += pct; - } - - public boolean atTargetExtent() { - return Math.abs(mArmEncoder.getPosition() - armExtent) < Presets.ArmThreshold; - } - - @Override - public void periodic() { - mArmPIDController.setReference(armExtent, CANSparkMax.ControlType.kPosition); - SmartDashboard.putNumber( - "Arm Error", - Convert.encoderPosToDistance( - mArmEncoder.getPosition() - armExtent, - kArmGearRatio, - kDiameter, - Encoder.RevRelativeEncoder)); - SmartDashboard.putNumber("Arm extent", mArmEncoder.getPosition()); - SmartDashboard.putNumber("Arm Target Extent", this.armExtent); - } - - public void stop() { - mArmMotor.set(0); - } -} -*/