1 files changed, 84 insertions, 0 deletions
diff --git a/src/main/java/frc/robot/subsystems/MainMech.java b/src/main/java/frc/robot/subsystems/MainMech.java
new file mode 100644
index 0000000..1e31892
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/MainMech.java
@@ -0,0 +1,84 @@
+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() { }
+}