changes

12 files changed, 261 insertions, 104 deletions

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index efff2b8..f6b7a4b 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -1,12 +1,7 @@
 package frc.robot;
 
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.CANSparkLowLevel.MotorType;
-
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.I2C;
 import edu.wpi.first.wpilibj.I2C.Port;
@@ -14,8 +9,8 @@ import frc.robot.util.IMUGyro;
 
 public class Constants
 {
-    public static final GenericHID controller = new GenericHID(0);
-    public static final GenericHID controller2 = new GenericHID(1);
+    public static final GenericHID controller1 = new GenericHID(1);
+    public static final GenericHID controller2 = new GenericHID(0);
 
     public static final double InputSmoothing = 0.1;
 
@@ -35,21 +30,23 @@ public class Constants
 
     public static final TrapezoidProfile.Constraints motor_constraints = 
         new TrapezoidProfile.Constraints(DriveMaxSpeed, DriveMaxAcceleration);
-    public static final SimpleMotorFeedforward motor_feedforward = new SimpleMotorFeedforward(0.54082, 2.4705);
 
-    public static final CANSparkMax motorL_leader = new CANSparkMax(11, MotorType.kBrushed);
-    public static final CANSparkMax motorL_follower = new CANSparkMax(12, MotorType.kBrushed);
-    public static final CANSparkMax motorR_leader = new CANSparkMax(13, MotorType.kBrushed);
-    public static final CANSparkMax motorR_follower = new CANSparkMax(14, MotorType.kBrushed);
+    public static final double MotorkS = 0.54082;
+    public static final double MotorkV = 2.4705;
+
+    public static final int motorL_leader_ID = 11;
+    public static final int motorL_follower_ID = 12;
+    public static final int motorR_leader_ID = 13;
+    public static final int motorR_follower_ID = 14;
 
-    public static final Encoder encoderL = new Encoder(6, 7); 
-    public static final Encoder encoderR = new Encoder(4, 5);
+    public static final int[] encoderL_PINS = {6, 7};
+    public static final int[] encoderR_PINS = {4, 5};
 
     // for Intake
     public static final int intakeT_ID = 32;
     public static final int intakeD_ID = 31;
 
-    // for Shooter    CommandScheduler.getInstance().run();
+    // for Shooter
 
     public static final int shooterT_ID = 21;
     public static final int shooterD_ID = 22;
@@ -92,4 +89,7 @@ public class Constants
     // Hooks
     public static final int hookL_ID = 62;
     public static final int hookR_ID = 61;
+
+    public static final int[] hookL_PINS = {0, 0};
+    public static final int[] hookR_PINS = {0, 0};
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 2fc88fa..f7d7c3f 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -22,7 +22,8 @@ public class Robot extends LoggedRobot {
     Logger.addDataReceiver(new NT4Publisher());
     Logger.start();
 
-    CameraServer.startAutomaticCapture();
+    CameraServer.startAutomaticCapture(0);
+    CameraServer.startAutomaticCapture(1);
 
     m_robotContainer = new RobotContainer();
   }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 88e493c..9a6241a 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,32 +4,39 @@
 
 package frc.robot;
 
-import com.fasterxml.jackson.databind.deser.std.ContainerDeserializerBase;
-
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.subsystems.Arm;
-import frc.robot.subsystems.Chain;
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Hooks;
 import frc.robot.subsystems.Intake;
+import frc.robot.subsystems.MainMech;
 import frc.robot.subsystems.Shooter;
+import frc.robot.subsystems.MainMech.MechCmd;
 
 public class RobotContainer 
 {
     private final Drivetrain m_drive = new Drivetrain();
-    private final Chain m_chain = new Chain();
-    private final Arm m_arm = new Arm();
+    private final MainMech m_mech = new MainMech();
     private final Intake m_intake = new Intake();
     private final Shooter m_shooter = new Shooter();
     private final Hooks m_hooks = new Hooks();
     
-    private final GenericHID m_controller = Constants.controller;
+    private final GenericHID m_controller1 = Constants.controller1;
+    private final GenericHID m_controller2 = Constants.controller2;
+    SendableChooser<Command> m_chooser = new SendableChooser<>();
 
     public RobotContainer() 
     {
+        m_chooser.setDefaultOption("None", Commands.none());
+        m_chooser.addOption("Shoot", 
+            Commands.parallel(
+                m_shooter.run(() -> m_shooter.atPercentage(1)), 
+                Commands.none().withTimeout(500).andThen(m_intake.run(() -> m_intake.atPercentage(1)))));
+        SmartDashboard.putData(m_chooser);
         configureBindings();
     }
 
@@ -63,21 +70,23 @@ public class RobotContainer
     {   
         m_shooter.setDefaultCommand(m_shooter.run(() -> m_shooter.atPercentage(0.0)));
         m_intake.setDefaultCommand(m_intake.run(() -> m_intake.atPercentage(0.0)));
-        m_arm.setDefaultCommand(m_arm.run(() -> m_arm.incrementAngleDegrees(
-            deadzone(-0.05, 0.05, Constants.controller2.getRawAxis(2)))));
         m_hooks.setDefaultCommand(m_hooks.run(() -> m_hooks.atPercentage(
-            deadzone(-0.1, 0.1, Constants.controller2.getRawAxis(4)), 
-            deadzone(-0.1, 0.1, Constants.controller2.getRawAxis(5)))));
-        m_chain.setDefaultCommand(m_chain.run(() -> 
-            m_chain.atPercentage(
-                Constants.controller2.getRawAxis(0)
-                * (-0.5) + 0.5
-            )));
+            deadzone(-0.1, 0.1, m_controller2.getRawAxis(4)), 
+            deadzone(-0.1, 0.1, m_controller2.getRawAxis(5)))));
+
+        // m_mech.setDefaultCommands(
+        //     m_mech.m_arm.run(() -> m_mech.m_arm.incrementAngle(
+        //         deadzone(-0.05, 0.05, m_controller2.getRawAxis(2)) * 0.3)) ,
+        //     m_mech.m_chain.run(() -> m_mech.m_chain.atPercentage(
+        //         m_controller2.getRawAxis(0)
+        //         * (-0.5) + 0.5))
+        // );
+        m_mech.setDefaultCommand(m_mech.run(() -> m_mech.toState(MechCmd.MANUAL).execute()));
 
         m_drive.setDefaultCommand(m_drive.run(() -> {
             double[] inputs = correctInputs(
-                    m_controller.getRawAxis(2),
-                    m_controller.getRawAxis(1));
+                    m_controller1.getRawAxis(2),
+                    m_controller1.getRawAxis(1));
             m_drive.tank(
                 inputs[0] * Constants.DriveMaxSpeed,
                 inputs[1] * Constants.DriveMaxSpeed);
@@ -93,21 +102,18 @@ public class RobotContainer
 
         // new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.followTrajectory(trajectory));
 
-        new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(0.3)));
-        new Trigger(() -> m_controller.getRawAxis(4) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(-0.1)));
-        new Trigger(() -> m_controller.getRawAxis(7) > 0).whileTrue(m_shooter.run(() -> m_shooter.atPercentage(1)));
-        new Trigger(() -> m_controller.getRawAxis(5) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(1)));
-
-        new Trigger(() -> Constants.controller2.getRawAxis(6) > 0).whileTrue(
-            Commands.parallel(m_arm.run(() -> m_arm.atAngleDegrees(0)), 
-                Commands.waitUntil(() -> m_arm.hasClearedAngle(-20)).andThen(m_chain.run(() -> m_chain.atPercentage(0.9)))
-                .until(() -> m_chain.isAtPosition(0.9)).andThen(Commands.run(() -> m_arm.atAngleDegrees((-40))))));//.whileTrue(
-            // Commands.parallel(m_arm.run(() -> m_arm.atAngleDegrees(-30)), Commands.waitUntil(() -> m_arm.hasClearedAngleDegree(-20))
-            //     .andThen(() ->
-            //         m_chain.run(() -> m_chain.atPercentage(0.9)).until(() ->
-            //             m_chain.atPosition(0)).andThen(m_arm.run(() -> m_arm.atAngleDegrees(-40))))
-            // ));
-            // Commands.parallel(arm (chain at 0.waitUntil(arm > -30)).andThen());
+        new Trigger(() -> m_controller1.getRawAxis(6) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(0.3)));
+        new Trigger(() -> m_controller1.getRawAxis(4) > 0).whileTrue(m_intake.run(() -> m_intake.shoot()));
+        new Trigger(() -> m_controller1.getRawAxis(7) > 0).whileTrue(m_shooter.run(() -> m_shooter.atPercentage(1)));
+        new Trigger(() -> m_controller1.getRawAxis(5) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(0.6)));
+
+        new Trigger(() -> m_controller2.getRawButton(1)).whileTrue(m_mech.toState(MechCmd.MANUAL));
+        new Trigger(() -> m_controller2.getRawAxis(6) > 0).whileTrue(m_mech.toState(MechCmd.INTAKE));
+        new Trigger(() -> m_controller2.getRawAxis(7) > 0).whileTrue(m_mech.toState(MechCmd.DEFAULT));
+        new Trigger(() -> m_controller2.getRawButton(4)).whileTrue(m_mech.toState(MechCmd.AMP));
+        new Trigger(() -> m_controller2.getRawButton(3)).whileTrue(m_mech.toState(MechCmd.SPEAKER));
+
+
         // new Trigger(() -> m_controller.getRawAxis(4) > 0).whileTrue(m_drive.sysIdQausistatic(Direction.kReverse));
         // new Trigger(() -> m_controller.getRawAxis(7) > 0).whileTrue(m_drive.sysIdDynamic(Direction.kForward));
         // new Trigger(() -> m_controller.getRawAxis(5) > 0).whileTrue(m_drive.sysIdDynamic(Direction.kReverse));
@@ -121,6 +127,6 @@ public class RobotContainer
 
     public Command getAutonomousCommand() 
     {
-        return Commands.print("No autonomous command configured");
+        return m_chooser.getSelected();
     }
 }
diff --git a/src/main/java/frc/robot/commands/IntakeIntermediate.java b/src/main/java/frc/robot/commands/IntakeIntermediate.java
new file mode 100644
index 0000000..e8ea7ee
--- /dev/null
+++ b/src/main/java/frc/robot/commands/IntakeIntermediate.java
@@ -0,0 +1,32 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Arm;
+import frc.robot.subsystems.Chain;
+
+public class IntakeIntermediate extends Command 
+{
+    public Arm m_arm;
+    public Chain m_chain;
+
+    public IntakeIntermediate(Arm arm, Chain chain)
+    { 
+        m_arm = arm;
+        m_chain = chain;
+
+        addRequirements(arm, chain);
+    }
+
+    @Override
+    public void execute()
+    {
+        m_arm.atAngleDegrees(-10);
+        m_chain.atPercentage(1);
+    }
+
+    @Override
+    public boolean isFinished()
+    {
+        return m_arm.isAtAngleDegree(-10) && m_chain.isAtPosition(1);
+    }    
+}
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 75e8ad2..0b8814d 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -8,7 +8,6 @@ import com.revrobotics.CANSparkLowLevel.MotorType;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
-import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.units.Angle;
 import edu.wpi.first.units.Measure;
@@ -16,7 +15,6 @@ import edu.wpi.first.units.MutableMeasure;
 import edu.wpi.first.units.Velocity;
 import edu.wpi.first.units.Voltage;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.PIDCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
@@ -105,15 +103,9 @@ public class Arm extends SubsystemBase
         m_pidR.reset(Constants.arm_initial_position);
     }
 
-
-    public void incrementAngleDegrees(double amount) {
-        incrementAngle(amount * Math.PI / 180.0);
-    }
-
-    public void incrementAngle(double amount) {
-        atAngle(m_setpoint + amount);
+    public double pos() {
+        return Math.min(m_encoderL.getPosition(), m_encoderR.getPosition()) * 180 / Math.PI;
     }
-    
 
     // in rad
     public void atAngle(double angle)
@@ -130,26 +122,36 @@ public class Arm extends SubsystemBase
             m_feedforward.calculate(m_pidR.getSetpoint().position, m_pidR.getSetpoint().velocity)
         );
     }
+
+    public void voltage(double leftVoltsPID, double leftVoltsFFW, double rightVoltsPID, double rightVoltsFFW)
+    {   
+        Logger.recordOutput(getName() + "/leftPID", leftVoltsPID);
+        Logger.recordOutput(getName() + "/rightPID", rightVoltsPID);
+        Logger.recordOutput(getName() + "/leftFFW", leftVoltsFFW);
+        Logger.recordOutput(getName() + "/rightFFW", rightVoltsFFW);
+
+        m_armL.setVoltage(MathUtil.clamp(leftVoltsPID + leftVoltsFFW, -4, 4));
+        m_armR.setVoltage(MathUtil.clamp(rightVoltsPID + rightVoltsFFW, -4, 4));
+    }
+
     // in deg
-    public void atAngleDegrees(double angle) 
+    public void incrementAngleDegrees(double amount) 
     {
-        atAngle(angle * Math.PI / 180.0);
+        incrementAngle(amount * Math.PI / 180.0);
     }
 
     // in rad
-    public boolean hasClearedAngle(double angle) 
+    public void incrementAngle(double amount) 
     {
-        if(m_encoderL.getPosition() > angle
-        && m_encoderR.getPosition() > angle) 
-            return true;
-
-        return false;
+        atAngle(m_setpoint + amount);
     }
 
-    public boolean hasClearedAngleDegree(double angle)
+    // in deg
+    public void atAngleDegrees(double angle) 
     {
-        return hasClearedAngle(angle * Math.PI/180);
+        atAngle(angle * Math.PI / 180.0);
     }
+
     // in rad
     public boolean isAtAngle(double angle) 
     {
@@ -160,21 +162,27 @@ public class Arm extends SubsystemBase
         return false;
     }
 
+    // in deg
     public boolean isAtAngleDegree(double angle)
     {
         return isAtAngle(angle * Math.PI/180);
     }
 
-    public void voltage(double leftVoltsPID, double leftVoltsFFW, double rightVoltsPID, double rightVoltsFFW)
-    {   
-        Logger.recordOutput(getName() + "/leftPID", leftVoltsPID);
-        Logger.recordOutput(getName() + "/rightPID", rightVoltsPID);
-        Logger.recordOutput(getName() + "/leftFFW", leftVoltsFFW);
-        Logger.recordOutput(getName() + "/rightFFW", rightVoltsFFW);
+    // in rad
+    // public boolean hasClearedAngle(double angle) 
+    // {
+    //     if(m_encoderL.getPosition() > angle
+    //     && m_encoderR.getPosition() > angle) 
+    //         return true;
+
+    //     return false;
+    // }
+
+    // public boolean hasClearedAngleDegree(double angle)
+    // {
+    //     return hasClearedAngle(angle * Math.PI/180);
+    // }
 
-        m_armL.setVoltage(MathUtil.clamp(leftVoltsPID + leftVoltsFFW, -4, 4));
-        m_armR.setVoltage(MathUtil.clamp(rightVoltsPID + rightVoltsFFW, -4, 4));
-    }
 
     @Override
     public void periodic() 
@@ -183,6 +191,7 @@ public class Arm extends SubsystemBase
         Logger.recordOutput(getName() + "/positionR", m_encoderR.getPosition() * 180/Math.PI);
     }
 
+    // Sysid
     public Command sysIdQausistatic(SysIdRoutine.Direction direction)
     {
         return m_sysIdRoutine.quasistatic(direction);
diff --git a/src/main/java/frc/robot/subsystems/Chain.java b/src/main/java/frc/robot/subsystems/Chain.java
index 40235fe..3ee7108 100644
--- a/src/main/java/frc/robot/subsystems/Chain.java
+++ b/src/main/java/frc/robot/subsystems/Chain.java
@@ -20,10 +20,6 @@ public class Chain extends SubsystemBase
 
     public Chain()
     {
-        // TODO: figure out invertedness
-
-        // m_motor.setInverted(true);
-
         m_encoder.setDistancePerPulse(1.0/Constants.ChainEncoderScale);
         m_encoder.reset();
     }
@@ -36,9 +32,9 @@ public class Chain extends SubsystemBase
     // perc between 0 and 1;
     public void atPercentage(double perc) 
     {
-        // System.out.println(perc);
         Logger.recordOutput(getName() + "/perc", perc);
         double pid_out = m_pid.calculate(m_encoder.getDistance(), perc);
+
         Logger.recordOutput(getName() + "/pidOUt", pid_out);
         m_motor.setVoltage(MathUtil.clamp(pid_out, -6, 6));
     }
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index aee9597..66d1ba6 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -1,6 +1,7 @@
 package frc.robot.subsystems;
 
 import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkLowLevel.MotorType;
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
@@ -40,15 +41,15 @@ public class Drivetrain extends SubsystemBase
 {
     private IMUGyro m_gyro = Constants.gyro;
 
-    private CANSparkMax m_motorL_leader = Constants.motorL_leader;
-    private CANSparkMax m_motorL_follower = Constants.motorL_follower;
-    private CANSparkMax m_motorR_leader = Constants.motorR_leader;
-    private CANSparkMax m_motorR_follower = Constants.motorR_follower;
+    private CANSparkMax m_motorL_leader = new CANSparkMax(Constants.motorL_leader_ID, MotorType.kBrushed);
+    private CANSparkMax m_motorL_follower = new CANSparkMax(Constants.motorL_follower_ID, MotorType.kBrushed);
+    private CANSparkMax m_motorR_leader = new CANSparkMax(Constants.motorR_leader_ID, MotorType.kBrushed);
+    private CANSparkMax m_motorR_follower = new CANSparkMax(Constants.motorR_follower_ID, MotorType.kBrushed);
 
-    private Encoder m_encoderL = Constants.encoderL;
-    private Encoder m_encoderR = Constants.encoderR;
+    private Encoder m_encoderL = new Encoder(Constants.encoderL_PINS[0], Constants.encoderL_PINS[1]);
+    private Encoder m_encoderR = new Encoder(Constants.encoderR_PINS[0], Constants.encoderR_PINS[1]);
 
-    private SimpleMotorFeedforward m_feedforward = Constants.motor_feedforward; 
+    private SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(Constants.MotorkS, Constants.MotorkV);
 
     private final double kP = 0.363;
 
@@ -116,8 +117,8 @@ public class Drivetrain extends SubsystemBase
 
         m_odometry = new DifferentialDriveOdometry(
             m_gyro.toRotation2d(),
-            Constants.encoderL.getDistance(),
-            Constants.encoderR.getDistance());
+            m_encoderL.getDistance(),
+            m_encoderR.getDistance());
     }
 
     // speed - linear velocity in m/s
@@ -168,8 +169,8 @@ public class Drivetrain extends SubsystemBase
 
         m_odometry = new DifferentialDriveOdometry(
             m_gyro.toRotation2d(),
-            Constants.encoderL.getDistance(),
-            Constants.encoderR.getDistance());
+            m_encoderL.getDistance(),
+            m_encoderR.getDistance());
     }    
 
     @Override
diff --git a/src/main/java/frc/robot/subsystems/Hooks.java b/src/main/java/frc/robot/subsystems/Hooks.java
index b6c26c5..6e42322 100644
--- a/src/main/java/frc/robot/subsystems/Hooks.java
+++ b/src/main/java/frc/robot/subsystems/Hooks.java
@@ -1,25 +1,29 @@
 package frc.robot.subsystems;
 
-import java.lang.constant.Constable;
+import org.littletonrobotics.junction.Logger;
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
 import frc.robot.Constants;
-
-
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Hooks extends SubsystemBase 
 {
+    // initial angle 237
     private final CANSparkMax m_hookL = new CANSparkMax(Constants.hookL_ID, MotorType.kBrushed);
     private final CANSparkMax m_hookR = new CANSparkMax(Constants.hookR_ID, MotorType.kBrushed);
 
+    private final Encoder m_hookL_encoder = new Encoder(Constants.hookL_PINS[0], Constants.hookL_PINS[1]);
+    private final Encoder m_hookR_encoder = new Encoder(Constants.hookR_PINS[0], Constants.hookR_PINS[1]);
+    
     public Hooks() 
     { 
         // TODO: possibly invert to move forward upon positive votage
-        // TODO: invert one hook
         m_hookL.setInverted(true);
+        m_hookL_encoder.setReverseDirection(true);
     }
 
     public void atPercentage(double percL, double percR)
@@ -28,4 +32,10 @@ public class Hooks extends SubsystemBase
         m_hookR.set(percR);
     }
 
+    @Override
+    public void periodic()
+    {
+        Logger.recordOutput(getName() + "/encoderL", m_hookL_encoder.getDistance());
+        Logger.recordOutput(getName() + "/encoderR", m_hookR_encoder.getDistance());
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java
index 41620bb..036aae0 100644
--- a/src/main/java/frc/robot/subsystems/Intake.java
+++ b/src/main/java/frc/robot/subsystems/Intake.java
@@ -11,8 +11,8 @@ import frc.robot.Constants;
 
 public class Intake extends SubsystemBase 
 {
-    public static final CANSparkMax m_intakeT = new CANSparkMax(Constants.intakeT_ID, MotorType.kBrushless);
-    public static final CANSparkMax m_intakeD = new CANSparkMax(Constants.intakeD_ID, MotorType.kBrushless);
+    private final CANSparkMax m_intakeT = new CANSparkMax(Constants.intakeT_ID, MotorType.kBrushless);
+    private final CANSparkMax m_intakeD = new CANSparkMax(Constants.intakeD_ID, MotorType.kBrushless);
     private final RelativeEncoder m_intakeD_encoder = m_intakeD.getEncoder();
 
     public Intake() 
@@ -26,17 +26,19 @@ public class Intake extends SubsystemBase
         atPercentage(perc, perc); 
     }
 
+    // perc is between 0.0 and 1.0
     public void atPercentage(double upPerc, double downPerc) 
     {
         m_intakeT.set(upPerc);
         m_intakeD.set(downPerc);
     }
+
     public void shoot() 
     {
-        if (m_intakeD_encoder.getVelocity() > 2000) {
-            m_intakeT.set(1);
+        if (Math.abs(m_intakeD_encoder.getVelocity()) > 2000) {
+            m_intakeT.set(-0.5);
         }
-        m_intakeD.set(1);
+        m_intakeD.set(-0.5);
     }
 
     @Override
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() { }
+}
diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java
index 677073c..273e5bd 100644
--- a/src/main/java/frc/robot/subsystems/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/Shooter.java
@@ -15,6 +15,7 @@ public class Shooter extends SubsystemBase
 
     public Shooter() {}
 
+    // perc is betwen 0.0 and 1.0
     public void atPercentage(double perc)
     {
         atPercentage(perc, perc);
diff --git a/src/main/java/frc/robot/util/MechState.java b/src/main/java/frc/robot/util/MechState.java
new file mode 100644
index 0000000..f0092f0
--- /dev/null
+++ b/src/main/java/frc/robot/util/MechState.java
@@ -0,0 +1,15 @@
+package frc.robot.util;
+
+public class MechState {
+    public double m_chain_pos;
+    public double m_arm_pos;
+    public MechState(double chain_pos, double arm_pos) {
+        m_chain_pos = chain_pos;
+        m_arm_pos = arm_pos;
+    }
+    // TODO
+    public boolean isValid() { 
+        return 30 > (Math.cos(m_arm_pos * Math.PI / 180) * 60 - m_arm_pos * 80);
+        // return true;
+     }
+}