diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2f295cc..cad1500 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -150,6 +150,7 @@ public RobotContainer() { will.rightBumper(), will.leftBumper())); + // FIXME: This error is here to kind of guide you... armSubsystem.setDefaultCommand( new ArmManualCommand( armSubsystem, @@ -321,13 +322,15 @@ private void configureButtonBindings() { jasonLayer .off(jason.b()) + // FIXME: This error is here to kind of guide you... .onTrue(new ArmPositionCommand(armSubsystem, Arm.Setpoints.SHELF_INTAKE)) .whileTrue( new ForceOuttakeSubsystemModeCommand(outtakeSubsystem, OuttakeSubsystem.Modes.INTAKE)); - // reset + // Reset arm position jasonLayer .off(jason.y()) + // FIXME: This error is here to kind of guide you... .onTrue(new ArmPositionCommand(armSubsystem, Arm.Setpoints.STOWED)) .onTrue(new IntakeCommand(intakeSubsystem, IntakeSubsystem.Modes.STOWED)); jason.start().onTrue(new SetZeroModeCommand(armSubsystem)); @@ -344,23 +347,19 @@ private void configureButtonBindings() { ? IntakeSubsystem.Modes.INTAKE_LOW : IntakeSubsystem.Modes.INTAKE)); - jason - .povUp() - .onTrue( - new IntakeCommand(intakeSubsystem, IntakeSubsystem.Modes.OUTTAKE) - .alongWith(new ArmPositionCommand(armSubsystem, Arm.Setpoints.HANDOFF))); - jason.start().onTrue(new ZeroIntakeCommand(intakeSubsystem)); jason .back() .whileTrue( new IntakeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE) + // FIXME: This error is here to kind of guide you... .alongWith(new ArmPositionCommand(armSubsystem, Arm.Setpoints.HANDOFF)) .alongWith( new ForceOuttakeSubsystemModeCommand( outtakeSubsystem, OuttakeSubsystem.Modes.OFF))) .onFalse( + // FIXME: This error is here to kind of guide you... new ArmPositionCommand(armSubsystem, Arm.Setpoints.STOWED) .alongWith(new IntakeCommand(intakeSubsystem, IntakeSubsystem.Modes.STOWED))); @@ -448,7 +447,6 @@ private void setupAutonomousCommands() { final Map eventMap = Map.of( "stow arm", - new ArmPositionCommand(armSubsystem, Constants.Arm.Setpoints.STOWED), "zero everything", (new SetZeroModeCommand(armSubsystem)) .alongWith(new ZeroIntakeCommand(intakeSubsystem)), @@ -491,6 +489,8 @@ public void end(boolean interrupted) { outtakeSubsystem, armSubsystem, Constants.SCORE_STEP_MAP.get(NodeType.CUBE.atHeight(Height.MID)).subList(0, 1)), + + // FIXME: How can we get this working again? "outtake", new ScoreCommand(outtakeSubsystem, armSubsystem, drivingCubeOuttake.subList(1, 2), 1) .andThen( diff --git a/src/main/java/frc/robot/commands/ArmManualCommand.java b/src/main/java/frc/robot/commands/ArmManualCommand.java index 692d1aa..d4e9e8a 100644 --- a/src/main/java/frc/robot/commands/ArmManualCommand.java +++ b/src/main/java/frc/robot/commands/ArmManualCommand.java @@ -6,19 +6,13 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.ArmSubsystem; -import java.util.function.DoubleSupplier; public class ArmManualCommand extends CommandBase { ArmSubsystem armSubsystem; - DoubleSupplier angleSupplier; - DoubleSupplier extensionSupplier; /** Creates a new AngleArmCommand. */ - public ArmManualCommand( - ArmSubsystem armSubsystem, DoubleSupplier angleSupplier, DoubleSupplier extensionSupplier) { + public ArmManualCommand(ArmSubsystem armSubsystem) { this.armSubsystem = armSubsystem; - this.angleSupplier = angleSupplier; - this.extensionSupplier = extensionSupplier; addRequirements(armSubsystem); } @@ -28,15 +22,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() { - - double angleOutput = angleSupplier.getAsDouble(); - double extensionOutput = extensionSupplier.getAsDouble(); - - armSubsystem.setTargetPosition( - armSubsystem.getTargetAngleDegrees() + angleOutput, - armSubsystem.getTargetExtensionInches() + extensionOutput); - } + public void execute() {} // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/frc/robot/commands/ArmPositionCommand.java b/src/main/java/frc/robot/commands/ArmPositionCommand.java index a3ab8f1..a64b711 100644 --- a/src/main/java/frc/robot/commands/ArmPositionCommand.java +++ b/src/main/java/frc/robot/commands/ArmPositionCommand.java @@ -6,30 +6,19 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.ArmSubsystem; -import frc.robot.subsystems.ArmSubsystem.ArmState; public class ArmPositionCommand extends CommandBase { private ArmSubsystem subsystem; - private final double desiredAngle; - private final double targetExtension; /** Creates a new ArmPositionCommand. */ - public ArmPositionCommand(ArmSubsystem subsystem, double desiredAngle, double targetExtension) { + public ArmPositionCommand(ArmSubsystem subsystem) { this.subsystem = subsystem; - this.desiredAngle = desiredAngle; - this.targetExtension = targetExtension; // Use addRequirements() here to declare subsystem dependencies. addRequirements(subsystem); } - public ArmPositionCommand(ArmSubsystem subsystem, ArmState armState) { - this(subsystem, armState.angle(), armState.extension()); - } - // Called when the command is initially scheduled. @Override - public void initialize() { - subsystem.setTargetPosition(desiredAngle, targetExtension); - } + public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override @@ -42,6 +31,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return subsystem.atTarget(); + return false; } } diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java index 9bc089c..29d1e47 100644 --- a/src/main/java/frc/robot/commands/GroundPickupCommand.java +++ b/src/main/java/frc/robot/commands/GroundPickupCommand.java @@ -5,8 +5,6 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants.Arm; -import frc.robot.Constants.Arm.Setpoints; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.OuttakeSubsystem; @@ -24,20 +22,6 @@ public GroundPickupCommand( Supplier modeSupplier) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); - addCommands( - new SetOuttakeModeCommand(outtakeSubsystem, OuttakeSubsystem.Modes.INTAKE) - .deadlineWith( - new IntakeCommand(intakeSubsystem, modeSupplier) - .alongWith(new ArmPositionCommand(armSubsystem, Setpoints.HANDOFF))) - .andThen( - new ArmPositionCommand(armSubsystem, Arm.Setpoints.STOWED) - .alongWith(new IntakeCommand(intakeSubsystem, IntakeSubsystem.Modes.STOWED)))); - } - - public GroundPickupCommand( - IntakeSubsystem intakeSubsystem, - OuttakeSubsystem outtakeSubsystem, - ArmSubsystem armSubsystem) { - this(intakeSubsystem, outtakeSubsystem, armSubsystem, () -> IntakeSubsystem.Modes.INTAKE); + addCommands(); } } diff --git a/src/main/java/frc/robot/commands/ScoreCommand.java b/src/main/java/frc/robot/commands/ScoreCommand.java index 2fa8df0..c1dbaba 100644 --- a/src/main/java/frc/robot/commands/ScoreCommand.java +++ b/src/main/java/frc/robot/commands/ScoreCommand.java @@ -70,10 +70,13 @@ private Command createStep(ScoreStep scoreStep) { var armState = scoreStep.armState(); var outtakeState = scoreStep.outtakeState(); if (armState.isPresent() && outtakeState.isPresent()) { - return new ArmPositionCommand(armSubsystem, armState.get()) + // FIXME: we need a working armposition command here + return new ArmPositionCommand(armSubsystem) .deadlineWith(new SetOuttakeModeCommand(outtakeSubsystem, outtakeState.get())); } else if (armState.isPresent()) { - return new ArmPositionCommand(armSubsystem, armState.get()); + + // FIXME: we need a working armposition command here + return new ArmPositionCommand(armSubsystem); } else if (outtakeState.isPresent()) { return new SetOuttakeModeCommand(outtakeSubsystem, outtakeState.get()); } else { diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 573c929..6536a9e 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -4,312 +4,33 @@ package frc.robot.subsystems; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; -import com.ctre.phoenix.motorcontrol.can.TalonFX; -import com.ctre.phoenix.sensors.AbsoluteSensorRange; -import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenix.sensors.MagnetFieldStrength; -import com.ctre.phoenix.sensors.SensorInitializationStrategy; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.Arm; -import frc.robot.Constants.Arm.Thresholds; import frc.robot.Constants.Config; -import frc.util.Util; /** Add your docs here. */ public class ArmSubsystem extends SubsystemBase { - // Put methods for controlling this subsystem - // here. Call these from Commands. - - // TO DO: ADD VOLTAGE LIMITERS!!! - - private final TalonFX angleMotor; - private final PIDController angleController; - private final CANCoder angleEncoder; - private double targetAngleDegrees; // measured in degrees - - private final TalonFX extensionMotor; - private final PIDController extensionController; - private double targetExtensionInches = 0; - - // gross members for logging to shuffleboard motor powers - private double extensionOutput = 0; - private double angleOutput = 0; private final ShuffleboardTab tab = Shuffleboard.getTab("Arm"); - // stator limits - private LinearFilter filter; - private double filterOutput; + // FIXME: Is that what you need/want?? public static record ArmState(double angle, double extension) {} + // stator limits + private LinearFilter filter; + public ArmSubsystem() { filter = LinearFilter.movingAverage(35); - this.angleMotor = new TalonFX(Arm.Ports.ARM_MOTOR_PORT); - extensionMotor = new TalonFX(Arm.Ports.TELESCOPING_MOTOR_PORT); - - angleMotor.setNeutralMode(NeutralMode.Brake); - extensionMotor.setNeutralMode(NeutralMode.Brake); - - extensionMotor.configFactoryDefault(); - extensionMotor.setInverted(false); - angleMotor.setInverted(true); - - extensionMotor.configForwardSoftLimitThreshold( - inchesLengthToTicks(Arm.Setpoints.Extensions.MAX_EXTENSION), 20); // this is the top - extensionMotor.configReverseSoftLimitThreshold( - inchesLengthToTicks(Arm.Setpoints.Extensions.MIN_EXTENSION), - 20); // this is the bottom limit - - extensionMotor.configForwardSoftLimitEnable(true, 20); - extensionMotor.configReverseSoftLimitEnable(true, 20); - - angleController = new PIDController(.019, 0, 0); - extensionController = new PIDController(Arm.ExtensionGains.BASE_P, 0, 0); - angleController.setTolerance(Thresholds.Angles.EPSILON); - extensionController.setTolerance(Thresholds.Extensions.EPSILON); - - angleEncoder = new CANCoder(Arm.Ports.ENCODER_PORT); - - angleEncoder.configFactoryDefault(); - - angleEncoder.configSensorInitializationStrategy( - SensorInitializationStrategy.BootToAbsolutePosition); - angleEncoder.configAbsoluteSensorRange(AbsoluteSensorRange.Signed_PlusMinus180); - - targetAngleDegrees = Arm.Setpoints.STOWED.angle(); - - var config = - new StatorCurrentLimitConfiguration( - true /*enable*/, - 50 /* current limit */, - 5 /* threshold */, - .1 /*time in seconds to trip*/); - - angleMotor.configStatorCurrentLimit(config); - - angleEncoder.configMagnetOffset(Arm.ANGULAR_OFFSET); - - angleEncoder.configSensorDirection(true); - - angleEncoder.setPositionToAbsolute(10); // ms - - extensionMotor.setSelectedSensorPosition(0); - - if (Config.SHOW_SHUFFLEBOARD_DEBUG_DATA) { - tab.addDouble("current angle", this::getAngle); - tab.addDouble("Desired Angle", () -> targetAngleDegrees); - tab.add("Angle Arm PID", angleController); - tab.add("Telescoping Arm PID", extensionController); - tab.addNumber("current telescope PID P term", () -> extensionController.getP()); - tab.addNumber("Current Extension", this::getCurrentExtensionInches); - tab.addNumber("Target Extension", () -> targetExtensionInches); - tab.addNumber("extension error", () -> targetExtensionInches - getCurrentExtensionInches()); - tab.addNumber("Telescoping PID Output", () -> extensionOutput); - tab.addBoolean( - "Current or Target Angle within Unsafe Threshold", - this::currentOrTargetAnglePassesUnsafeRange); - tab.addNumber("Arm Gravity Offset", this::computeArmGravityOffset); - tab.addNumber("Angle Output", () -> angleOutput); - tab.addNumber("Angle Error", () -> targetAngleDegrees - getAngle()); - tab.addBoolean("At target", this::atTarget); - tab.addString("Current Mode", () -> mode.toString()); - tab.addNumber("Stator current", this.extensionMotor::getStatorCurrent); - tab.addNumber("filtered stator current", () -> this.filterOutput); - } - } - - public enum Modes { - DRIVETOPOS, - ZERO - } - - // current mode - private Modes mode = Modes.DRIVETOPOS; - - public Modes getMode() { - return mode; - } - - public void setZeroMode() { - extensionMotor.configForwardSoftLimitEnable(false, 20); - extensionMotor.configReverseSoftLimitEnable(false, 20); - mode = Modes.ZERO; - targetExtensionInches = Arm.Setpoints.Extensions.MIN_EXTENSION; - } - - /* methods for angle arm control */ - public double getAngle() { - return angleEncoder.getAbsolutePosition(); - } - - public void setTargetAngleDegrees(double targetAngleDegrees) { - mode = Modes.DRIVETOPOS; - this.targetAngleDegrees = - MathUtil.clamp( - targetAngleDegrees, - Arm.Thresholds.Angles.BACKWARD_ANGLE_LIMIT, - Arm.Thresholds.Angles.FORWARD_ANGLE_LIMIT); - } - - public double getTargetAngleDegrees() { - return targetAngleDegrees; - } - - public double getTargetExtensionInches() { - return targetExtensionInches; - } - - /* methods for telescoping arm control */ - public void setTargetExtensionInches(double targetExtensionInches) { - mode = Modes.DRIVETOPOS; - this.targetExtensionInches = - MathUtil.clamp( - targetExtensionInches, - Arm.Setpoints.Extensions.MIN_EXTENSION, - Arm.Setpoints.Extensions.MAX_EXTENSION); - } - - public double getCurrentExtensionInches() { - return ticksLengthToInches(extensionMotor.getSelectedSensorPosition()); - } - - private static double inchesLengthToTicks(double extension) { - return (extension / Arm.SPOOL_CIRCUMFERENCE) * Arm.TELESCOPING_ARM_GEAR_RATIO * Arm.TICKS; - } - - private static double ticksLengthToInches(double ticks) { - return ((ticks / Arm.TICKS) / Arm.TELESCOPING_ARM_GEAR_RATIO) * Arm.SPOOL_CIRCUMFERENCE; - } - - public void setTargetPosition(double targetAngleDegrees, double targetExtensionInches) { - setTargetAngleDegrees(targetAngleDegrees); - setTargetExtensionInches(targetExtensionInches); - } - - public void setTargetPosition(ArmState targetState) { - setTargetPosition(targetState.angle, targetState.extension); - } - - /* safety methods */ - private boolean withinAngleRange(double angle) { - return angle < Arm.Thresholds.Angles.FORWARD_UNSAFE_EXTENSION_ANGLE_THRESHOLD - && angle > Arm.Thresholds.Angles.BACKWARD_UNSAFE_EXTENSION_ANGLE_THRESHOLD; - } - - private boolean currentOrTargetAnglePassesUnsafeRange() { - // if the current angle is within the unsafe range - return withinAngleRange(getAngle()) - || - // if the target angle is within the unsafe range, return true - withinAngleRange(targetAngleDegrees) - // if the signs are opposite, arm must pass through bottom - || Math.signum(targetAngleDegrees) != Math.signum(getAngle()); - } - - // Return true if encoder is bad - private boolean encoderIsBad() { - return angleEncoder.getMagnetFieldStrength() == MagnetFieldStrength.Invalid_Unknown - || angleEncoder.getMagnetFieldStrength() == MagnetFieldStrength.BadRange_RedLED; - } - - // Add the gravity offset as a function of sine - private double computeArmGravityOffset() { - return Math.sin(Math.toRadians(getAngle())) * Arm.GRAVITY_CONTROL_PERCENT; - } - - private boolean extensionIsRetracted() { - return getCurrentExtensionInches() < Arm.Thresholds.Extensions.FULLY_RETRACTED_INCHES_THRESHOLD; - } - - private double computeIntermediateAngleGoal() { - if (!extensionIsRetracted() && currentOrTargetAnglePassesUnsafeRange()) { - // set the intermediate angle to the lowest safe angle on the same side as we are currently on - if (getAngle() < 0) { - return Arm.Thresholds.Angles.BACKWARD_UNSAFE_EXTENSION_ANGLE_THRESHOLD; - } - return Arm.Thresholds.Angles.FORWARD_UNSAFE_EXTENSION_ANGLE_THRESHOLD; - } - return targetAngleDegrees; - } - - private double computeIntermediateExtensionGoal() { - if (currentOrTargetAnglePassesUnsafeRange()) { - return Arm.Setpoints.Extensions.MIN_EXTENSION; - } - return targetExtensionInches; - } - - public boolean atTarget() { - return Util.epsilonEquals(getAngle(), targetAngleDegrees, Thresholds.Angles.EPSILON) - && Util.epsilonEquals( - getCurrentExtensionInches(), targetExtensionInches, Thresholds.Extensions.EPSILON); - // && extensionController.atSetpoint() - // && angleController.atSetpoint(); + // Add debug table + if (Config.SHOW_SHUFFLEBOARD_DEBUG_DATA) {} } @Override - public void periodic() { - this.filterOutput = this.filter.calculate(this.extensionMotor.getStatorCurrent()); - - extensionController.setP( - Arm.ExtensionGains.BASE_P - + Arm.ExtensionGains.MAX_ADDITIONAL_P - * Math.pow( - MathUtil.clamp( - getCurrentExtensionInches() / Arm.Setpoints.Extensions.MAX_EXTENSION, 0, 1), - 3)); - - switch (mode) { - case DRIVETOPOS: - driveToPosPeriodic(); - break; - case ZERO: - zeroPeriodic(); - break; - } - } - - public void zeroPeriodic() { - angleMotor.set(ControlMode.PercentOutput, computeArmGravityOffset()); - extensionMotor.set(ControlMode.PercentOutput, Arm.ZERO_RETRACTION_PERCENT); - if (filterOutput > Arm.EXTENSION_STATOR_LIMIT) { - extensionMotor.setSelectedSensorPosition(0); - mode = Modes.DRIVETOPOS; - extensionMotor.configForwardSoftLimitEnable(true, 20); - extensionMotor.configReverseSoftLimitEnable(true, 20); - } - } - - public void driveToPosPeriodic() { - - double currentAngle = getAngle(); - // Add the gravity offset as a function of sine - final double armGravityOffset = computeArmGravityOffset(); - - extensionOutput = - extensionController.calculate( - getCurrentExtensionInches(), computeIntermediateExtensionGoal()); - - angleOutput = angleController.calculate(currentAngle, computeIntermediateAngleGoal()); - - if (encoderIsBad()) { - angleMotor.set(ControlMode.PercentOutput, 0); - } else { - angleMotor.set( - ControlMode.PercentOutput, MathUtil.clamp(angleOutput + armGravityOffset, -.7, .7)); - } - extensionMotor.set(ControlMode.PercentOutput, MathUtil.clamp(extensionOutput, -.5, .5)); - } + public void periodic() {} }