changes

7 files changed, 157 insertions, 54 deletions

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);