diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b84afd1..abcfd3d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,7 +18,6 @@ import frc.robot.match_metadata.*; import frc.robot.misc.exceptions.UnknownTargetRobotException; import frc.robot.superstructure.SuperstructureSubsystem; -import frc.robot.superstructure.arm.*; import frc.robot.superstructure.commands.ArmDownAndShootCommand; import frc.robot.superstructure.commands.ArmDownAndSnarfCommand; import frc.robot.superstructure.commands.ArmUpAndShootCommand; @@ -51,7 +50,6 @@ public class RobotContainer { private final UpperHubVisionSubsystem upperVisionSubsystem; private final CargoVisionSubsystem cargoVisionSubsystem; private final Swiffer swiffer; - private final Arm arm; private final SuperstructureSubsystem superstructureSubsystem; private final Lights lights; private final Localization localization; @@ -72,7 +70,6 @@ public RobotContainer() { if (Constants.getMode() == Constants.Mode.REPLAY) { matchMetadataSubsystem = new MatchMetadataSubsystem(new MatchMetadataIOReplay()); lights = new Lights(new LightsIOReplay()); - arm = new Arm(new ArmIOReplay(), lights); swiffer = new Swiffer(new SwifferIOReplay(), lights); imuSubsystem = new ImuSubsystem(new ImuIOReplay()); upperVisionSubsystem = new UpperHubVisionSubsystem(new UpperHubVisionIOReplay()); @@ -90,7 +87,6 @@ public RobotContainer() { case COMP_BOT: matchMetadataSubsystem = new MatchMetadataSubsystem(new MatchMetadataIOFms()); lights = new Lights(new LightsIOReplay()); - arm = new Arm(new ArmIONeos(), lights); swiffer = new Swiffer(new SwifferIOFalcon500(), lights); imuSubsystem = new ImuSubsystem(new ImuIOAdis16470()); upperVisionSubsystem = new UpperHubVisionSubsystem(new UpperHubVisionIOReplay()); @@ -107,7 +103,6 @@ public RobotContainer() { case TEST_2020_BOT: matchMetadataSubsystem = new MatchMetadataSubsystem(new MatchMetadataIOFms()); lights = new Lights(new LightsIORoborio()); - arm = new Arm(new ArmIOReplay(), lights); swiffer = new Swiffer(new SwifferIOReplay(), lights); imuSubsystem = new ImuSubsystem(new ImuIOReplay()); upperVisionSubsystem = new UpperHubVisionSubsystem(new UpperHubVisionIOReplay()); @@ -124,7 +119,6 @@ public RobotContainer() { case SIM_BOT: matchMetadataSubsystem = new MatchMetadataSubsystem(new MatchMetadataIOSim()); lights = new Lights(new LightsIOSim()); - arm = new Arm(new ArmIOSimNeos(), lights); swiffer = new Swiffer(new SwifferIOSimFalcon500(), lights); imuSubsystem = new ImuSubsystem(new ImuIOSim()); upperVisionSubsystem = new UpperHubVisionSubsystem(new UpperHubVisionIOSim()); @@ -143,7 +137,7 @@ public RobotContainer() { } } - superstructureSubsystem = new SuperstructureSubsystem(swiffer, arm, lights); + superstructureSubsystem = new SuperstructureSubsystem(swiffer, lights); localization = new Localization(driveSubsystem, cargoVisionSubsystem, imuSubsystem); autonomousChooser = @@ -165,7 +159,6 @@ private void initLogging() { SmartDashboard.putData(upperVisionSubsystem); SmartDashboard.putData(cargoVisionSubsystem); SmartDashboard.putData(swiffer); - SmartDashboard.putData(arm); SmartDashboard.putData(superstructureSubsystem); SmartDashboard.putData(lights); } diff --git a/src/main/java/frc/robot/superstructure/SuperstructureSubsystem.java b/src/main/java/frc/robot/superstructure/SuperstructureSubsystem.java index b715ec6..765e282 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureSubsystem.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureSubsystem.java @@ -5,20 +5,17 @@ package frc.robot.superstructure; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.superstructure.arm.Arm; import frc.robot.superstructure.commands.ArmUpAndStopCommand; import frc.robot.superstructure.lights.Lights; import frc.robot.superstructure.swiffer.Swiffer; public class SuperstructureSubsystem extends SubsystemBase { public final Swiffer swiffer; - public final Arm arm; public final Lights lights; /** Creates a new SuperstructureSubsystem. */ - public SuperstructureSubsystem(Swiffer swiffer, Arm arm, Lights lights) { + public SuperstructureSubsystem(Swiffer swiffer, Lights lights) { this.swiffer = swiffer; - this.arm = arm; this.lights = lights; setDefaultCommand( @@ -30,6 +27,5 @@ public SuperstructureSubsystem(Swiffer swiffer, Arm arm, Lights lights) { @Override public void periodic() { swiffer.periodic(); - arm.periodic(); } } diff --git a/src/main/java/frc/robot/superstructure/arm/Arm.java b/src/main/java/frc/robot/superstructure/arm/Arm.java deleted file mode 100644 index 79438b5..0000000 --- a/src/main/java/frc/robot/superstructure/arm/Arm.java +++ /dev/null @@ -1,112 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.superstructure.arm; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.superstructure.arm.ArmIO.Inputs; -import frc.robot.superstructure.lights.Lights; -import frc.robot.superstructure.swiffer.Swiffer; -import org.littletonrobotics.junction.Logger; - -public class Arm extends SubsystemBase { - /** Mass of arm in kilograms. */ - public static final double ARM_MASS = Swiffer.MASS + (4776.833 / 1e3); - - /** Length of arm in meters. */ - public static final double ARM_LENGTH = 838.20 / 1e3; - - /** Moment of inertia (Iyy in Fusion 360) in kg m^2. */ - public static final double MOMENT_OF_INERTIA = 3.084; - - /** Gear ratio of motor. */ - public static final double GEARING = 300.0 / 7.0; - - /** The starting positon of the arm. */ - // Arm starts in the up position - public static final ArmPosition STARTING_POSITION = ArmPosition.UP; - - private final Lights lights; - - private final ArmIO io; - private final Inputs inputs = new Inputs(); - - private ArmPosition desiredPosition = STARTING_POSITION; - private double desiredVoltageVolts = 0; - - /** Creates a new Arm. */ - public Arm(ArmIO io, Lights lights) { - this.io = io; - this.lights = lights; - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - - io.updateInputs(inputs); - Logger.getInstance().processInputs("Arm", inputs); - - if (DriverStation.isEnabled()) { - doPositionControlLoop(); - } - - final var isAtGoal = atGoal(); - Logger.getInstance().recordOutput("Arm/Goal/Position", desiredPosition.toString()); - Logger.getInstance().recordOutput("Arm/Goal/AtGoal", isAtGoal); - Logger.getInstance().recordOutput("Arm/Position", getPosition().toString()); - Logger.getInstance().recordOutput("Arm/DesiredVoltageVolts", desiredVoltageVolts); - - lights.setSubsystemState(desiredPosition, isAtGoal); - } - - /** Set the desired position of the arm. */ - public void setDesiredPosition(ArmPosition position) { - desiredPosition = position; - } - - /** Check if the arm is at the provided position. */ - public boolean atPosition(ArmPosition position) { - return getPosition() == position; - } - - private void doPositionControlLoop() { - if (atGoal()) { - desiredVoltageVolts = 0; - } else { - switch (desiredPosition) { - case UP: - desiredVoltageVolts = 2.75; - break; - case DOWN: - desiredVoltageVolts = -2; - break; - default: - throw new IllegalArgumentException("Invalid desired position"); - } - } - - io.setVoltage(desiredVoltageVolts); - } - - private boolean atGoal() { - return atPosition(desiredPosition); - } - - private ArmPosition getPosition() { - if (inputs.upwardLimitSwitchEnabled) { - if (inputs.downwardLimitSwitchEnabled) { - // Both limit switches should never be enabled - return ArmPosition.UNKNOWN; - } - - return ArmPosition.UP; - } else if (inputs.downwardLimitSwitchEnabled) { - return ArmPosition.DOWN; - } else { - return ArmPosition.UNKNOWN; - } - } -} diff --git a/src/main/java/frc/robot/superstructure/arm/ArmIO.java b/src/main/java/frc/robot/superstructure/arm/ArmIO.java deleted file mode 100644 index d5846fb..0000000 --- a/src/main/java/frc/robot/superstructure/arm/ArmIO.java +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.superstructure.arm; - -import frc.robot.misc.SubsystemIO; -import org.littletonrobotics.junction.LogTable; -import org.littletonrobotics.junction.inputs.LoggableInputs; - -interface ArmIO extends SubsystemIO { - public class Inputs implements LoggableInputs { - public double appliedVolts = 0; - public double currentAmps = 0; - public double tempCelcius = 0; - public double positionRadians = 0; - public double velocityRadiansPerSecond = 0; - public boolean downwardLimitSwitchEnabled = false; - public boolean upwardLimitSwitchEnabled = false; - - public void toLog(LogTable table) { - table.put("VoltageVolts", appliedVolts); - table.put("CurrentAmps", currentAmps); - table.put("TempCelcius", tempCelcius); - table.put("PositionRadians", positionRadians); - table.put("VelocityRadiansPerSecond", velocityRadiansPerSecond); - table.put("DownwardLimitSwitchEnabled", downwardLimitSwitchEnabled); - table.put("UpwardLimitSwitchEnabled", upwardLimitSwitchEnabled); - } - - public void fromLog(LogTable table) { - appliedVolts = table.getDouble("VoltageVolts", appliedVolts); - currentAmps = table.getDouble("CurrentAmps", currentAmps); - tempCelcius = table.getDouble("TempCelcius", tempCelcius); - positionRadians = table.getDouble("PositionRadians", positionRadians); - velocityRadiansPerSecond = - table.getDouble("VelocityRadiansPerSecond", velocityRadiansPerSecond); - downwardLimitSwitchEnabled = - table.getBoolean("DownwardLimitSwitchEnabled", downwardLimitSwitchEnabled); - upwardLimitSwitchEnabled = - table.getBoolean("UpwardLimitSwitchEnabled", upwardLimitSwitchEnabled); - } - } - - /** - * Sets the output voltage of the arm's motor. A positive voltage will raise the arm and a - * negative voltage will lower the it. That means that positive voltage and a positive arm angle - * are directly related. - */ - public void setVoltage(double volts); -} diff --git a/src/main/java/frc/robot/superstructure/arm/ArmIONeos.java b/src/main/java/frc/robot/superstructure/arm/ArmIONeos.java deleted file mode 100644 index 2b53202..0000000 --- a/src/main/java/frc/robot/superstructure/arm/ArmIONeos.java +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.superstructure.arm; - -import com.ctre.phoenix.sensors.AbsoluteSensorRange; -import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenix.sensors.SensorInitializationStrategy; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel; -import com.revrobotics.SparkMaxLimitSwitch; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.util.Units; -import frc.robot.Constants; -import frc.robot.Constants.TargetRobot; -import frc.robot.misc.exceptions.UnsupportedSubsystemException; -import frc.robot.misc.util.GearingConverter; - -public class ArmIONeos implements ArmIO { - /** Initial encoder position after gearing. */ - static final Rotation2d INITIAL_ENCODER_POSITION = - new Rotation2d(Arm.STARTING_POSITION.stateForSimulation.position); - - protected static final GearingConverter GEARING_CONVERTER = new GearingConverter(60.0 / 15.0); - - protected final CANSparkMax motor; - protected final CANCoder encoder; - - protected final SparkMaxLimitSwitch downwardLimitSwitch; - protected final SparkMaxLimitSwitch upwardLimitSwitch; - - public ArmIONeos() { - switch (Constants.getRobot()) { - case COMP_BOT: - case SIM_BOT: - motor = new CANSparkMax(6, CANSparkMaxLowLevel.MotorType.kBrushless); - encoder = new CANCoder(3); - - downwardLimitSwitch = motor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - upwardLimitSwitch = motor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - break; - default: - throw new UnsupportedSubsystemException(this); - } - - if (Constants.getRobot() == TargetRobot.COMP_BOT) { - motor.setInverted(true); - } - - encoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToZero); - encoder.configSensorDirection(false); - encoder.configAbsoluteSensorRange(AbsoluteSensorRange.Signed_PlusMinus180); - encoder.setPosition(0); - } - - protected static DCMotor getMotorSim() { - return DCMotor.getNEO(1); - } - - @Override - public void updateInputs(Inputs inputs) { - inputs.appliedVolts = motor.getAppliedOutput(); - inputs.currentAmps = motor.getOutputCurrent(); - inputs.tempCelcius = motor.getMotorTemperature(); - inputs.positionRadians = - Rotation2d.fromDegrees(GEARING_CONVERTER.beforeToAfterGearing(encoder.getPosition())) - .plus(INITIAL_ENCODER_POSITION) - .getRadians(); - inputs.velocityRadiansPerSecond = Units.degreesToRadians(encoder.getVelocity()); - inputs.downwardLimitSwitchEnabled = downwardLimitSwitch.isPressed(); - inputs.upwardLimitSwitchEnabled = upwardLimitSwitch.isPressed(); - } - - @Override - public void setVoltage(double volts) { - if ((volts < 0 && downwardLimitSwitch.isPressed()) - || (volts > 0 && upwardLimitSwitch.isPressed())) { - motor.setVoltage(0); - } - - motor.setVoltage(volts); - } -} diff --git a/src/main/java/frc/robot/superstructure/arm/ArmIOReplay.java b/src/main/java/frc/robot/superstructure/arm/ArmIOReplay.java deleted file mode 100644 index 7c4239a..0000000 --- a/src/main/java/frc/robot/superstructure/arm/ArmIOReplay.java +++ /dev/null @@ -1,17 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.superstructure.arm; - -public class ArmIOReplay implements ArmIO { - @Override - public void updateInputs(Inputs inputs) { - // Intentionally left empty - } - - @Override - public void setVoltage(double volts) { - // Intentionally left empty - } -} diff --git a/src/main/java/frc/robot/superstructure/arm/ArmIOSimNeos.java b/src/main/java/frc/robot/superstructure/arm/ArmIOSimNeos.java deleted file mode 100644 index 1d45cfb..0000000 --- a/src/main/java/frc/robot/superstructure/arm/ArmIOSimNeos.java +++ /dev/null @@ -1,115 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.superstructure.arm; - -import com.revrobotics.REVPhysicsSim; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; -import frc.robot.Constants; -import frc.robot.misc.util.sensors.SensorUnitConverter; -import org.littletonrobotics.junction.Logger; - -public class ArmIOSimNeos extends ArmIONeos implements ArmIO { - /** Height in meters of the tower the arm is attached to. */ - private static final double TOWER_HEIGHT = 330.20 / 1e3; - - /** The arm tower's angle relative to the floor. */ - private static final Rotation2d TOWER_ANGLE = Rotation2d.fromDegrees(90); - - private final SingleJointedArmSim sim = - new SingleJointedArmSim( - getMotorSim(), - Arm.GEARING, - Arm.ARM_LENGTH, - Arm.MOMENT_OF_INERTIA, - // This assumes that the DOWN position has an angle less than the UP position - ArmPosition.DOWN.stateForSimulation.position, - ArmPosition.UP.stateForSimulation.position, - Arm.ARM_MASS, - true); - private final Mechanism2d arm2d = - new Mechanism2d(Arm.ARM_LENGTH * 1.5, TOWER_HEIGHT + Arm.ARM_LENGTH); - private final MechanismRoot2d armPivot = - arm2d.getRoot("ArmPivot", (Arm.ARM_LENGTH * 1.5) / 4, (TOWER_HEIGHT + Arm.ARM_LENGTH) / 4); - private final MechanismLigament2d armTower = - armPivot.append(new MechanismLigament2d("ArmTower", TOWER_HEIGHT, TOWER_ANGLE.getDegrees())); - private final MechanismLigament2d arm = - armTower.append(new MechanismLigament2d("Arm", Arm.ARM_LENGTH, 0)); - - public ArmIOSimNeos() { - REVPhysicsSim.getInstance().addSparkMax(motor, DCMotor.getNEO(1)); - - SmartDashboard.putData("Arm Sim", arm2d); - - armTower.setColor(new Color8Bit(Color.kBlue)); - arm.setColor(new Color8Bit(Color.kYellow)); - - sim.setState( - VecBuilder.fill( - Arm.STARTING_POSITION.stateForSimulation.position, - Arm.STARTING_POSITION.stateForSimulation.velocity)); - - // TODO: Use CAD for drawing accurate line widths - } - - @Override - public void updateInputs(Inputs inputs) { - sim.setInputVoltage(motor.getAppliedOutput()); - - sim.update(Constants.PERIOD_SECONDS); - - final var positionRadians = sim.getAngleRads(); - final var velocityRadiansPerSecond = sim.getVelocityRadPerSec(); - - final var encoderSim = encoder.getSimCollection(); - - encoderSim.setRawPosition( - (int) - Math.round( - SensorUnitConverter.cancoder.radiansToSensorUnits( - INITIAL_ENCODER_POSITION.getRadians() + positionRadians))); - - final var velocitySensorUnits = - SensorUnitConverter.cancoder.radiansPerSecondToSensorUnitsPer100ms( - velocityRadiansPerSecond); - encoderSim.setVelocity(velocitySensorUnits); - - Logger.getInstance().recordOutput("Arm/Sim/VelocityRadiansPerSecond", velocityRadiansPerSecond); - Logger.getInstance().recordOutput("Arm/Sim/PositionRadians", positionRadians); - - arm.setAngle(Units.radiansToDegrees(positionRadians) - TOWER_ANGLE.getDegrees()); - - super.updateInputs(inputs); - - if (motor.getInverted()) { - // A hack to make the desired voltage look the same as the actual voltage - - inputs.appliedVolts *= -1; - } - - inputs.upwardLimitSwitchEnabled = positionRadians >= ArmPosition.UP.stateForSimulation.position; - inputs.downwardLimitSwitchEnabled = - positionRadians <= ArmPosition.DOWN.stateForSimulation.position; - } - - @Override - public void setVoltage(double volts) { - if (motor.getInverted()) { - // REV simulation software doesn't invert voltage, even when the motor is inverted - volts *= -1; - } - - super.setVoltage(volts); - } -} diff --git a/src/main/java/frc/robot/superstructure/arm/ArmPosition.java b/src/main/java/frc/robot/superstructure/arm/ArmPosition.java deleted file mode 100644 index 4be5444..0000000 --- a/src/main/java/frc/robot/superstructure/arm/ArmPosition.java +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.superstructure.arm; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.TrapezoidProfile; - -/** - * The goal positions for the arm to be in. Angles are CCW+. When the arm is level with the floor it - * is at a rotation of 0. When the arm is perpendicular with the floor it is at a rotation of 90deg - * (not -90deg or 270deg, that would be CW+). - */ -public enum ArmPosition { - UP(Rotation2d.fromDegrees(40)), - DOWN(Rotation2d.fromDegrees(-10)), - UNKNOWN(null); - - final TrapezoidProfile.State stateForSimulation; - - ArmPosition(Rotation2d angle) { - this.stateForSimulation = - angle == null ? null : new TrapezoidProfile.State(angle.getRadians(), 0); - } -} diff --git a/src/main/java/frc/robot/superstructure/arm/characterization-instructions.txt b/src/main/java/frc/robot/superstructure/arm/characterization-instructions.txt deleted file mode 100644 index fd8e3b8..0000000 --- a/src/main/java/frc/robot/superstructure/arm/characterization-instructions.txt +++ /dev/null @@ -1,8 +0,0 @@ -This is necessary until this is merged: https://github.com/wpilibsuite/sysid/issues/416 - -1. Use a block to hold the arm parallel to the floor -2. In Phoenix Tuner use BootToZero and power cycle the robot -3. Power cycle the robot (sets the encoder to 0) -4. Deploy SysID -5. Remove the block and allow the arm to fully lower -6. Run the characterization routine diff --git a/src/main/java/frc/robot/superstructure/arm/commands/ArmCommand.java b/src/main/java/frc/robot/superstructure/arm/commands/ArmCommand.java deleted file mode 100644 index 9372d73..0000000 --- a/src/main/java/frc/robot/superstructure/arm/commands/ArmCommand.java +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.superstructure.arm.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.superstructure.arm.Arm; -import frc.robot.superstructure.arm.ArmPosition; - -/** - * A command to move the arm to a desired position. Finishes after the arm has first met the desired - * position. - */ -public class ArmCommand extends CommandBase { - private final Arm arm; - private final ArmPosition goalPosition; - - /** Creates a new ArmCommand. */ - public ArmCommand(Arm arm, ArmPosition goalPosition) { - this.arm = arm; - this.goalPosition = goalPosition; - - addRequirements(arm); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - arm.setDesiredPosition(goalPosition); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - // This exits the command when the arm is at the desired position. This does not turn off the - // arm, it will continue to maintain the desired position set by this command. Arguably this - // command should never be "finished", but it's more convenient to do it like this. - return arm.atPosition(goalPosition); - } -} diff --git a/src/main/java/frc/robot/superstructure/commands/ArmDownAndShootCommand.java b/src/main/java/frc/robot/superstructure/commands/ArmDownAndShootCommand.java index 220acd4..54fba8f 100644 --- a/src/main/java/frc/robot/superstructure/commands/ArmDownAndShootCommand.java +++ b/src/main/java/frc/robot/superstructure/commands/ArmDownAndShootCommand.java @@ -7,8 +7,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.superstructure.SuperstructureSubsystem; -import frc.robot.superstructure.arm.ArmPosition; -import frc.robot.superstructure.arm.commands.ArmCommand; import frc.robot.superstructure.swiffer.SwifferMode; import frc.robot.superstructure.swiffer.commands.SwifferCommand; @@ -24,14 +22,12 @@ public ArmDownAndShootCommand(SuperstructureSubsystem superstructure) { // the lights that are are preparing to shoot. new InstantCommand( () -> superstructure.lights.setSubsystemState(SwifferMode.SHOOTING, false)), - // Arm down - new ArmCommand(superstructure.arm, ArmPosition.DOWN), // Shoot all cargo after the arm is down // Since the default superstructure mode is to lift the arm up and since this command is run // continuously, adding an end condition here can cause the arm to jump up briefly as the // command ends and restarts. We rely on the copilot to know when to cancel this command. new SwifferCommand(superstructure.swiffer, SwifferMode.SHOOTING)); - addRequirements(superstructure, superstructure.arm, superstructure.swiffer); + addRequirements(superstructure, superstructure.swiffer); } } diff --git a/src/main/java/frc/robot/superstructure/commands/ArmDownAndSnarfCommand.java b/src/main/java/frc/robot/superstructure/commands/ArmDownAndSnarfCommand.java index f4161b4..45343df 100644 --- a/src/main/java/frc/robot/superstructure/commands/ArmDownAndSnarfCommand.java +++ b/src/main/java/frc/robot/superstructure/commands/ArmDownAndSnarfCommand.java @@ -6,8 +6,6 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.robot.superstructure.SuperstructureSubsystem; -import frc.robot.superstructure.arm.ArmPosition; -import frc.robot.superstructure.arm.commands.ArmCommand; import frc.robot.superstructure.swiffer.SwifferMode; import frc.robot.superstructure.swiffer.commands.SwifferCommand; @@ -16,11 +14,9 @@ public class ArmDownAndSnarfCommand extends ParallelCommandGroup { /** Creates a new ArmDownAndSnarfCommand. */ public ArmDownAndSnarfCommand(SuperstructureSubsystem superstructure) { addCommands( - // Start lowering the arm - new ArmCommand(superstructure.arm, ArmPosition.DOWN), // While that's happening we begin spinning up the swiffer new SwifferCommand(superstructure.swiffer, SwifferMode.SNARFING)); - addRequirements(superstructure, superstructure.arm, superstructure.swiffer); + addRequirements(superstructure, superstructure.swiffer); } } diff --git a/src/main/java/frc/robot/superstructure/commands/ArmUpAndShootCommand.java b/src/main/java/frc/robot/superstructure/commands/ArmUpAndShootCommand.java index 6e63343..a456525 100644 --- a/src/main/java/frc/robot/superstructure/commands/ArmUpAndShootCommand.java +++ b/src/main/java/frc/robot/superstructure/commands/ArmUpAndShootCommand.java @@ -7,8 +7,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.superstructure.SuperstructureSubsystem; -import frc.robot.superstructure.arm.ArmPosition; -import frc.robot.superstructure.arm.commands.ArmCommand; import frc.robot.superstructure.swiffer.SwifferMode; import frc.robot.superstructure.swiffer.commands.SwifferCommand; @@ -27,12 +25,10 @@ public ArmUpAndShootCommand(SuperstructureSubsystem superstructure) { // the lights that are are preparing to shoot. new InstantCommand( () -> superstructure.lights.setSubsystemState(SwifferMode.SHOOTING, false)), - // Arm up - new ArmCommand(superstructure.arm, ArmPosition.UP), // Shoot all cargo after the arm is up new SwifferCommand(superstructure.swiffer, SwifferMode.SHOOTING) .withTimeout(SHOOT_DURATION)); - addRequirements(superstructure, superstructure.arm, superstructure.swiffer); + addRequirements(superstructure, superstructure.swiffer); } } diff --git a/src/main/java/frc/robot/superstructure/commands/ArmUpAndStopCommand.java b/src/main/java/frc/robot/superstructure/commands/ArmUpAndStopCommand.java index b2486f6..d28475f 100644 --- a/src/main/java/frc/robot/superstructure/commands/ArmUpAndStopCommand.java +++ b/src/main/java/frc/robot/superstructure/commands/ArmUpAndStopCommand.java @@ -6,8 +6,6 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.robot.superstructure.SuperstructureSubsystem; -import frc.robot.superstructure.arm.ArmPosition; -import frc.robot.superstructure.arm.commands.ArmCommand; import frc.robot.superstructure.swiffer.SwifferMode; import frc.robot.superstructure.swiffer.commands.SwifferCommand; @@ -17,11 +15,10 @@ public class ArmUpAndStopCommand extends ParallelCommandGroup { public ArmUpAndStopCommand(SuperstructureSubsystem superstructure) { addCommands( // Arm up - new ArmCommand(superstructure.arm, ArmPosition.UP), // Stop the flywheel new SwifferCommand(superstructure.swiffer, SwifferMode.STOPPED) .until(() -> superstructure.swiffer.atGoal(SwifferMode.STOPPED))); - addRequirements(superstructure, superstructure.arm, superstructure.swiffer); + addRequirements(superstructure, superstructure.swiffer); } } diff --git a/src/main/java/frc/robot/superstructure/lights/Lights.java b/src/main/java/frc/robot/superstructure/lights/Lights.java index 2ccc11a..f61fac1 100644 --- a/src/main/java/frc/robot/superstructure/lights/Lights.java +++ b/src/main/java/frc/robot/superstructure/lights/Lights.java @@ -8,8 +8,6 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.superstructure.arm.Arm; -import frc.robot.superstructure.arm.ArmPosition; import frc.robot.superstructure.lights.LightsIO.Inputs; import frc.robot.superstructure.swiffer.Swiffer; import frc.robot.superstructure.swiffer.SwifferMode; @@ -30,7 +28,6 @@ public class Lights extends SubsystemBase { private SwifferMode swifferState = SwifferMode.STOPPED; private boolean swifferAtGoal = true; - private ArmPosition armState = ArmPosition.UP; private boolean armAtGoal = true; private Color currentColor = Color.kBlack; @@ -66,14 +63,13 @@ public void setSubsystemState(SwifferMode swifferState, boolean atGoal) { } /** Sets the {@link Arm arm}'s state. */ - public void setSubsystemState(ArmPosition armState, boolean atGoal) { - this.armState = armState; + public void setSubsystemState(boolean atGoal) { this.armAtGoal = atGoal; } private void chooseColor() { Color color; - LightsMode lightsMode; + LightsMode lightsMode = LightsMode.BLINK_FAST; switch (swifferState) { case STOPPED: @@ -91,30 +87,6 @@ private void chooseColor() { return; } - switch (armState) { - case UP: - switch (swifferState) { - case STOPPED: - lightsMode = armAtGoal && swifferAtGoal ? LightsMode.BLINK_SLOW : LightsMode.BLINK_FAST; - break; - case SHOOTING: - lightsMode = armAtGoal ? LightsMode.BLINK_FAST : LightsMode.BLINK_SLOW; - break; - default: - // Should never happen - setColorForError(); - return; - } - break; - case DOWN: - lightsMode = armAtGoal && swifferAtGoal ? LightsMode.BLINK_FAST : LightsMode.BLINK_SLOW; - break; - default: - // Should never happen - setColorForError(); - return; - } - setColor(color, lightsMode); Logger.getInstance()