package frc.robot.subsystems; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.commands.IntakeIntermediate; import frc.robot.util.MechState; public class MainMech extends SubsystemBase { public final Arm m_arm = new Arm(); public final Chain m_chain = new Chain(); public enum MechCmd { INTAKE, SPEAKER, AMP, DEFAULT, MANUAL } private double deadzone(double low, double high, double val) { if(val > low && val < high) return 0.0; return ((val > 0.0) ? val - high : val - low); } public MechCmd current_state = MechCmd.SPEAKER; public Command m_manual_cmd = this.run(() -> { }); public MechState states[] = { new MechState(0.95, -40), new MechState(0, -42), new MechState(0.95, 54.5), new MechState(0.5, -20), new MechState(0.5, 20) }; public MainMech() { } public void setDefaultCommands(Command arm_command, Command chain_command) { m_arm.setDefaultCommand(arm_command); m_chain.setDefaultCommand(chain_command); } public Command command(MechState state) { return Commands.parallel( m_arm.run(() -> m_arm.atAngleDegrees(state.m_arm_pos)), m_chain.run(() -> m_chain.atPercentage(state.m_chain_pos))); } public Command toState(MechCmd state) { MechState next = states[state.ordinal()]; if (next.isValid()) { Command next_cmd = command(next); if(state == MechCmd.INTAKE || current_state == MechCmd.INTAKE) { current_state = state; return new IntakeIntermediate(m_arm, m_chain).andThen(next_cmd); } if (state == MechCmd.MANUAL) { current_state = state; double arm_pos = m_arm.pos() + deadzone(-0.05, 0.05, Constants.controller2.getRawAxis(2)); double chain_pos = Constants.controller2.getRawAxis(0) * (-0.5) + 0.5; MechState manual_state = new MechState(chain_pos, arm_pos); if (manual_state.isValid()) { return command(manual_state); } return Commands.none(); } current_state = state; return next_cmd; } return Commands.none(); } @Override public void periodic() { } }