minor subsystem changesHEADmaster

8 files changed, 38 insertions, 49 deletions

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index f6b7a4b..ecbe1bd 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -9,8 +9,8 @@ import frc.robot.util.IMUGyro;
 
 public class Constants
 {
-    public static final GenericHID controller1 = new GenericHID(1);
-    public static final GenericHID controller2 = new GenericHID(0);
+    public static final GenericHID controller1 = new GenericHID(0);
+    public static final GenericHID controller2 = new GenericHID(1);
 
     public static final double InputSmoothing = 0.1;
 
@@ -72,7 +72,7 @@ public class Constants
     public static final int armR_ID = 42;
 
     // for Chain 
-    public static final double ChainEncoderScale = 3000;
+    public static final double ChainEncoderScale = 3200;
 
     public static final double ChainEpsilon = 0.05;
 
@@ -90,6 +90,6 @@ public class Constants
     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};
+    public static final int[] hookL_PINS = {0, 1};
+    public static final int[] hookR_PINS = {2, 3};
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 9a6241a..40a0180 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -35,8 +35,9 @@ public class RobotContainer
         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)))));
+                Commands.none().withTimeout(2000).andThen(m_intake.run(() -> m_intake.atPercentage(1)).withTimeout(3000))));
         SmartDashboard.putData(m_chooser);
+
         configureBindings();
     }
 
@@ -68,19 +69,20 @@ public class RobotContainer
 
     private void configureBindings() 
     {   
-        m_shooter.setDefaultCommand(m_shooter.run(() -> m_shooter.atPercentage(0.0)));
+        m_shooter.setDefaultCommand(m_shooter.run(() -> m_shooter.atPercentage(-0.2)));
         m_intake.setDefaultCommand(m_intake.run(() -> m_intake.atPercentage(0.0)));
         m_hooks.setDefaultCommand(m_hooks.run(() -> m_hooks.atPercentage(
             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.setDefaultCommands(
+            m_mech.m_arm.run(() -> m_mech.m_arm.incrementAngle(
+                deadzone(-0.05, 0.05, m_controller2.getRawAxis(2)) * 0.05)) ,
+            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(() -> {
@@ -88,8 +90,8 @@ public class RobotContainer
                     m_controller1.getRawAxis(2),
                     m_controller1.getRawAxis(1));
             m_drive.tank(
-                inputs[0] * Constants.DriveMaxSpeed,
-                inputs[1] * Constants.DriveMaxSpeed);
+                -inputs[1] * Constants.DriveMaxSpeed,   // reversed inputs
+                -inputs[0] * Constants.DriveMaxSpeed);
         }));
 
 
@@ -102,12 +104,12 @@ public class RobotContainer
 
         // new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.followTrajectory(trajectory));
 
-        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_intake.run(() -> m_intake.atPercentage(-0.1)));
+        new Trigger(() -> m_controller1.getRawAxis(5) > 0).whileTrue(m_intake.run(() -> m_intake.shoot()));
+        new Trigger(() -> m_controller1.getRawAxis(6) > 0).whileTrue(m_shooter.run(() -> m_shooter.atPercentage(1)));
+        new Trigger(() -> m_controller1.getRawAxis(7) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(1)));
+        new Trigger(() -> m_controller2.getRawButton(2)).whileTrue(m_intake.run(() -> m_intake.atPercentage(0.2)));
 
-        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));
diff --git a/src/main/java/frc/robot/commands/IntakeIntermediate.java b/src/main/java/frc/robot/commands/IntakeIntermediate.java
index e8ea7ee..3f3cc67 100644
--- a/src/main/java/frc/robot/commands/IntakeIntermediate.java
+++ b/src/main/java/frc/robot/commands/IntakeIntermediate.java
@@ -3,18 +3,19 @@ package frc.robot.commands;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.Chain;
+import frc.robot.subsystems.MainMech;
 
 public class IntakeIntermediate extends Command 
 {
     public Arm m_arm;
     public Chain m_chain;
 
-    public IntakeIntermediate(Arm arm, Chain chain)
+    public IntakeIntermediate(MainMech mech)
     { 
-        m_arm = arm;
-        m_chain = chain;
+        m_arm = mech.m_arm;
+        m_chain = mech.m_chain;
 
-        addRequirements(arm, chain);
+        addRequirements(mech, m_arm, m_chain);
     }
 
     @Override
diff --git a/src/main/java/frc/robot/subsystems/Chain.java b/src/main/java/frc/robot/subsystems/Chain.java
index 3ee7108..0e67c7e 100644
--- a/src/main/java/frc/robot/subsystems/Chain.java
+++ b/src/main/java/frc/robot/subsystems/Chain.java
@@ -36,7 +36,7 @@ public class Chain extends SubsystemBase
         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));
+        m_motor.setVoltage(MathUtil.clamp(pid_out, -11, 11));
     }
 
     public boolean isAtPosition(double pos)
diff --git a/src/main/java/frc/robot/subsystems/Hooks.java b/src/main/java/frc/robot/subsystems/Hooks.java
index 6e42322..76844e1 100644
--- a/src/main/java/frc/robot/subsystems/Hooks.java
+++ b/src/main/java/frc/robot/subsystems/Hooks.java
@@ -6,7 +6,6 @@ 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;
 
diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java
index 036aae0..1237977 100644
--- a/src/main/java/frc/robot/subsystems/Intake.java
+++ b/src/main/java/frc/robot/subsystems/Intake.java
@@ -36,9 +36,9 @@ public class Intake extends SubsystemBase
     public void shoot() 
     {
         if (Math.abs(m_intakeD_encoder.getVelocity()) > 2000) {
-            m_intakeT.set(-0.5);
+            m_intakeT.set(-0.3);
         }
-        m_intakeD.set(-0.5);
+        m_intakeD.set(-0.3);
     }
 
     @Override
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();
diff --git a/src/main/java/frc/robot/util/MechState.java b/src/main/java/frc/robot/util/MechState.java
index f0092f0..a3ede22 100644
--- a/src/main/java/frc/robot/util/MechState.java
+++ b/src/main/java/frc/robot/util/MechState.java
@@ -9,7 +9,7 @@ public class MechState {
     }
     // TODO
     public boolean isValid() { 
-        return 30 > (Math.cos(m_arm_pos * Math.PI / 180) * 60 - m_arm_pos * 80);
-        // return true;
-     }
+        // return 30 > (Math.cos(m_arm_pos * Math.PI / 180) * 60 - m_arm_pos * 80);
+        return true;
+    }
 }