orks

6 files changed, 216 insertions, 28 deletions

diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index dca7ebd..75e8ad2 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -4,56 +4,192 @@ import org.littletonrobotics.junction.Logger;
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
+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;
+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;
 
+import static edu.wpi.first.units.MutableMeasure.mutable;
+import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.Seconds;
+
 public class Arm extends SubsystemBase 
 {
-    private final CANSparkMax m_armL = Constants.armL;
-    private final CANSparkMax m_armR = Constants.armR;
+    // rad
+    private double m_setpoint = Constants.arm_initial_position;
+    private final CANSparkMax m_armL = new CANSparkMax(Constants.armL_ID, MotorType.kBrushless);
+    private final CANSparkMax m_armR = new CANSparkMax(Constants.armR_ID, MotorType.kBrushless);
 
     private final RelativeEncoder m_encoderL = m_armL.getEncoder();
     private final RelativeEncoder m_encoderR = m_armR.getEncoder();
 
-    private final ArmFeedforward m_feedforward = Constants.arm_feedforward;
+    private ArmFeedforward m_feedforward = new ArmFeedforward(Constants.ArmkS, Constants.ArmkG, Constants.ArmkV);
 
-    private final ProfiledPIDController m_pidL = new ProfiledPIDController(Constants.ArmkP, Constants.ArmkI, Constants.ArmkD, Constants.arm_constraints);
-    private final ProfiledPIDController m_pidR = new ProfiledPIDController(Constants.ArmkP, Constants.ArmkI, Constants.ArmkD, Constants.arm_constraints);
+    private ProfiledPIDController m_pidL = new ProfiledPIDController(Constants.ArmkP, Constants.ArmkI, Constants.ArmkD, Constants.arm_constraints);
+    private ProfiledPIDController m_pidR = new ProfiledPIDController(Constants.ArmkP, Constants.ArmkI, Constants.ArmkD, Constants.arm_constraints);
     
+    // SYSID
+    private final MutableMeasure<Voltage> m_voltage = mutable(Volts.of(0));
+    private final MutableMeasure<Angle> m_angle = mutable(Radians.of(0));
+    private final MutableMeasure<Velocity<Angle>> m_velocity = mutable(RadiansPerSecond.of(0));
+
+    private final SysIdRoutine m_sysIdRoutine = new SysIdRoutine(
+    new SysIdRoutine.Config(
+        Volts.per(Seconds).of(0.5),
+        Volts.of(3),
+        Seconds.of(10)),
+    new SysIdRoutine.Mechanism(
+        (Measure<Voltage> volts) -> {
+            m_armL.setVoltage(volts.in(Volts));
+            m_armR.setVoltage(volts.in(Volts));
+        },
+        log -> {
+            log.motor("arm-left")
+                .voltage(
+                    m_voltage.mut_replace(
+                        m_armL.getAppliedOutput() * m_armL.getBusVoltage(), Volts)
+                )
+                .angularPosition(m_angle.mut_replace(m_encoderL.getPosition(), Radians))
+                .angularVelocity(m_velocity.mut_replace(m_encoderL.getVelocity(), RadiansPerSecond));
+            log.motor("arm-right")
+
+                .voltage(
+                    m_voltage.mut_replace(
+                        m_armR.getAppliedOutput() * m_armR.getBusVoltage(), Volts)
+                )
+                .angularPosition(m_angle.mut_replace(m_encoderR.getPosition(), Radians))
+                .angularVelocity(m_velocity.mut_replace(m_encoderR.getVelocity(), RadiansPerSecond));
+            
+        }, this));
+
     public Arm()
     {   
+        m_armL.setInverted(false);
         m_armR.setInverted(true);
 
         m_encoderL.setPositionConversionFactor(2*Math.PI / Constants.ArmGearReduction);
         m_encoderR.setPositionConversionFactor(2*Math.PI / Constants.ArmGearReduction);
+        reset();
+    }
+
+    // public Arm(double kP, double kI, double kD, double kS, double kG, double kV)
+    // {   
+    //     this();
+
+    //     m_feedforward = new ArmFeedforward(kS, kG, kV);
+    //     m_pidL = new ProfiledPIDController(kP, kI, kD, Constants.arm_constraints);
+    //     m_pidR = new ProfiledPIDController(kP, kI, kD, Constants.arm_constraints);
+    // }
+
+    public void reset()
+    {
+        m_setpoint = Constants.arm_initial_position;
+        m_encoderL.setPosition(Constants.arm_initial_position);
+        m_encoderR.setPosition(Constants.arm_initial_position);
+
+        m_pidL.reset(Constants.arm_initial_position);
+        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);
+    }
+    
+
     // in rad
     public void atAngle(double angle)
     {
+        m_setpoint = angle;
+        m_pidL.setGoal(m_setpoint);
+        m_pidR.setGoal(m_setpoint);
+
         voltage(
-            m_pidL.calculate(m_encoderL.getPosition())
-            + m_feedforward.calculate(m_pidL.getSetpoint().position, m_pidR.getSetpoint().velocity),
+            m_pidL.calculate(m_encoderL.getPosition()),
+            m_feedforward.calculate(m_pidL.getSetpoint().position, m_pidL.getSetpoint().velocity),
 
-            m_pidR.calculate(m_encoderR.getPosition())
-            + m_feedforward.calculate(m_pidR.getSetpoint().position, m_pidR.getSetpoint().velocity)
+            m_pidR.calculate(m_encoderR.getPosition()),
+            m_feedforward.calculate(m_pidR.getSetpoint().position, m_pidR.getSetpoint().velocity)
         );
     }
+    // in deg
+    public void atAngleDegrees(double angle) 
+    {
+        atAngle(angle * Math.PI / 180.0);
+    }
+
+    // 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);
+    }
+    // in rad
+    public boolean isAtAngle(double angle) 
+    {
+        if(Math.abs(m_encoderL.getPosition() - angle) < Constants.ArmEpsilon
+        && Math.abs(m_encoderR.getPosition() - angle) < Constants.ArmEpsilon) 
+            return true;
 
+        return false;
+    }
 
-    public void voltage(double leftVolts, double rightVolts)
+    public boolean isAtAngleDegree(double angle)
     {
-        m_armL.setVoltage(leftVolts);
-        m_armR.setVoltage(rightVolts);
+        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);
+
+        m_armL.setVoltage(MathUtil.clamp(leftVoltsPID + leftVoltsFFW, -4, 4));
+        m_armR.setVoltage(MathUtil.clamp(rightVoltsPID + rightVoltsFFW, -4, 4));
     }
 
     @Override
     public void periodic() 
     {
-        Logger.recordOutput(getName() + "/positionL", m_encoderL.getPosition());
-        Logger.recordOutput(getName() + "/positionR", m_encoderR.getPosition());
+        Logger.recordOutput(getName() + "/positionL", m_encoderL.getPosition() * 180/Math.PI);
+        Logger.recordOutput(getName() + "/positionR", m_encoderR.getPosition() * 180/Math.PI);
+    }
+
+    public Command sysIdQausistatic(SysIdRoutine.Direction direction)
+    {
+        return m_sysIdRoutine.quasistatic(direction);
+    }
+
+    public Command sysIdDynamic(SysIdRoutine.Direction direction)
+    {
+        return m_sysIdRoutine.dynamic(direction);
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/Chain.java b/src/main/java/frc/robot/subsystems/Chain.java
index 2485739..40235fe 100644
--- a/src/main/java/frc/robot/subsystems/Chain.java
+++ b/src/main/java/frc/robot/subsystems/Chain.java
@@ -3,7 +3,9 @@ package frc.robot.subsystems;
 import org.littletonrobotics.junction.Logger;
 
 import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkLowLevel.MotorType;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -11,15 +13,16 @@ import frc.robot.Constants;
 
 public class Chain extends SubsystemBase 
 {
-    private final CANSparkMax m_motor = Constants.chain;
-    private final Encoder m_encoder = Constants.chain_encoder;
+    private final CANSparkMax m_motor = new CANSparkMax(Constants.chain_ID, MotorType.kBrushed);
+    private final Encoder m_encoder = new Encoder(Constants.chain_encoder_PINS[0], Constants.chain_encoder_PINS[1]);
 
     private final ProfiledPIDController m_pid = new ProfiledPIDController(Constants.ChainkP, Constants.ChainkI, Constants.ChainkD, Constants.chain_constraints);
 
     public Chain()
     {
+        // TODO: figure out invertedness
+
         // m_motor.setInverted(true);
-        // m_encoder.setReverseDirection(true);
 
         m_encoder.setDistancePerPulse(1.0/Constants.ChainEncoderScale);
         m_encoder.reset();
@@ -33,7 +36,18 @@ public class Chain extends SubsystemBase
     // perc between 0 and 1;
     public void atPercentage(double perc) 
     {
-        m_motor.setVoltage(m_pid.calculate(m_encoder.getDistance(), 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));
+    }
+
+    public boolean isAtPosition(double pos)
+    {
+        if(Math.abs(pos - m_encoder.getDistance()) < Constants.ChainEpsilon)
+            return true;
+        return false;
     }
 
     @Override
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 5f34119..aee9597 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -13,7 +13,6 @@ import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
 import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
 import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
-import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint;
 import edu.wpi.first.units.Distance;
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.units.Velocity;
diff --git a/src/main/java/frc/robot/subsystems/Hooks.java b/src/main/java/frc/robot/subsystems/Hooks.java
index eb9c4f7..b6c26c5 100644
--- a/src/main/java/frc/robot/subsystems/Hooks.java
+++ b/src/main/java/frc/robot/subsystems/Hooks.java
@@ -1,8 +1,31 @@
 package frc.robot.subsystems;
 
+import java.lang.constant.Constable;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+
+import frc.robot.Constants;
+
+
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Hooks extends SubsystemBase 
 {
-    
+    private final CANSparkMax m_hookL = new CANSparkMax(Constants.hookL_ID, MotorType.kBrushed);
+    private final CANSparkMax m_hookR = new CANSparkMax(Constants.hookR_ID, MotorType.kBrushed);
+
+    public Hooks() 
+    { 
+        // TODO: possibly invert to move forward upon positive votage
+        // TODO: invert one hook
+        m_hookL.setInverted(true);
+    }
+
+    public void atPercentage(double percL, double percR)
+    {
+        m_hookL.set(percL);
+        m_hookR.set(percR);
+    }
+
 }
diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java
index 85cbbf8..41620bb 100644
--- a/src/main/java/frc/robot/subsystems/Intake.java
+++ b/src/main/java/frc/robot/subsystems/Intake.java
@@ -1,17 +1,25 @@
 package frc.robot.subsystems;
 
 import org.littletonrobotics.junction.Logger;
+
+import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 
 public class Intake extends SubsystemBase 
 {
-    private final CANSparkMax m_intakeT = Constants.intakeT;
-    private final CANSparkMax m_intakeD = Constants.intakeD;
+    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 RelativeEncoder m_intakeD_encoder = m_intakeD.getEncoder();
 
-    public Intake() { }
+    public Intake() 
+    {
+        m_intakeT.setInverted(true);
+        m_intakeD.setInverted(true);
+    }
 
     public void atPercentage(double perc) 
     {
@@ -23,12 +31,18 @@ public class Intake extends SubsystemBase
         m_intakeT.set(upPerc);
         m_intakeD.set(downPerc);
     }
+    public void shoot() 
+    {
+        if (m_intakeD_encoder.getVelocity() > 2000) {
+            m_intakeT.set(1);
+        }
+        m_intakeD.set(1);
+    }
 
     @Override
     public void periodic()
     {
         Logger.recordOutput(getName() + "/speedT", m_intakeT.getEncoder().getVelocity());
         Logger.recordOutput(getName() + "/speedD", m_intakeD.getEncoder().getVelocity());
-
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java
index f171049..677073c 100644
--- a/src/main/java/frc/robot/subsystems/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/Shooter.java
@@ -1,17 +1,19 @@
 package frc.robot.subsystems;
 
 import org.littletonrobotics.junction.Logger;
+
+import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 
 public class Shooter extends SubsystemBase 
-{
-    private final CANSparkMax m_shooterT = Constants.shooterT;
-    private final CANSparkMax m_shooterD = Constants.shooterD;
+{   
+    public static final CANSparkMax m_shooterT = new CANSparkMax(Constants.shooterT_ID, MotorType.kBrushless);
+    public static final CANSparkMax m_shooterD = new CANSparkMax(Constants.shooterD_ID, MotorType.kBrushless);
 
-    public Shooter() { }
+    public Shooter() {}
 
     public void atPercentage(double perc)
     {