minor subsystem changesHEADmaster

1 files changed, 4 insertions, 17 deletions

diff --git a/src/main/java/frc/robot/subsystems/MainMech.java b/src/main/java/frc/robot/subsystems/MainMech.java
index 1e31892..ac2eff3 100644
--- a/src/main/java/frc/robot/subsystems/MainMech.java
+++ b/src/main/java/frc/robot/subsystems/MainMech.java
@@ -32,7 +32,7 @@ public class MainMech extends SubsystemBase
     });
 
     public MechState states[] = {
-        new MechState(0.95, -40),
+        new MechState(0.8, -48),
         new MechState(0, -42),
         new MechState(0.95, 54.5),
         new MechState(0.5, -20),
@@ -49,9 +49,11 @@ public class MainMech extends SubsystemBase
 
     public Command command(MechState state) {
         
-        return Commands.parallel(  
+        Command c = Commands.parallel(  
             m_arm.run(() -> m_arm.atAngleDegrees(state.m_arm_pos)), 
             m_chain.run(() -> m_chain.atPercentage(state.m_chain_pos)));
+        c.addRequirements(this);
+        return c;
     }
 
     public Command toState(MechCmd state) 
@@ -59,21 +61,6 @@ public class MainMech extends SubsystemBase
         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();