6 files changed, 210 insertions, 16 deletions

diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
new file mode 100644
index 0000000..dca7ebd
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -0,0 +1,59 @@
+package frc.robot.subsystems;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class Arm extends SubsystemBase 
+{
+    private final CANSparkMax m_armL = Constants.armL;
+    private final CANSparkMax m_armR = Constants.armR;
+
+    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 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);
+    
+    public Arm()
+    {   
+        m_armR.setInverted(true);
+
+        m_encoderL.setPositionConversionFactor(2*Math.PI / Constants.ArmGearReduction);
+        m_encoderR.setPositionConversionFactor(2*Math.PI / Constants.ArmGearReduction);
+    }
+
+    // in rad
+    public void atAngle(double angle)
+    {
+        voltage(
+            m_pidL.calculate(m_encoderL.getPosition())
+            + m_feedforward.calculate(m_pidL.getSetpoint().position, m_pidR.getSetpoint().velocity),
+
+            m_pidR.calculate(m_encoderR.getPosition())
+            + m_feedforward.calculate(m_pidR.getSetpoint().position, m_pidR.getSetpoint().velocity)
+        );
+    }
+
+
+    public void voltage(double leftVolts, double rightVolts)
+    {
+        m_armL.setVoltage(leftVolts);
+        m_armR.setVoltage(rightVolts);
+    }
+
+    @Override
+    public void periodic() 
+    {
+        Logger.recordOutput(getName() + "/positionL", m_encoderL.getPosition());
+        Logger.recordOutput(getName() + "/positionR", m_encoderR.getPosition());
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/Chain.java b/src/main/java/frc/robot/subsystems/Chain.java
new file mode 100644
index 0000000..2485739
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Chain.java
@@ -0,0 +1,44 @@
+package frc.robot.subsystems;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.revrobotics.CANSparkMax;
+
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+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 ProfiledPIDController m_pid = new ProfiledPIDController(Constants.ChainkP, Constants.ChainkI, Constants.ChainkD, Constants.chain_constraints);
+
+    public Chain()
+    {
+        // m_motor.setInverted(true);
+        // m_encoder.setReverseDirection(true);
+
+        m_encoder.setDistancePerPulse(1.0/Constants.ChainEncoderScale);
+        m_encoder.reset();
+    }
+
+    public void reset() 
+    {
+        m_encoder.reset();
+    }
+
+    // perc between 0 and 1;
+    public void atPercentage(double perc) 
+    {
+        m_motor.setVoltage(m_pid.calculate(m_encoder.getDistance(), perc));
+    }
+
+    @Override
+    public void periodic() 
+    { 
+        Logger.recordOutput(getName() + "/position", m_encoder.getDistance());
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 557c7e3..5756650 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -2,14 +2,14 @@ package frc.robot.subsystems;
 
 import com.revrobotics.CANSparkMax;
 
-import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
 import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
-import edu.wpi.first.math.proto.Trajectory;
+import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.units.Distance;
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.units.Velocity;
@@ -25,6 +25,8 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import static edu.wpi.first.units.MutableMeasure.mutable;
 import static edu.wpi.first.units.Units.Volts;
 
+import org.littletonrobotics.junction.Logger;
+
 import static edu.wpi.first.units.Units.Meters;
 import static edu.wpi.first.units.Units.MetersPerSecond;
 
@@ -43,10 +45,10 @@ public class Drivetrain extends SubsystemBase
     private Encoder m_encoderL = Constants.encoderL;
     private Encoder m_encoderR = Constants.encoderR;
 
-    private SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1.1, 3.16, 0.17);
+    private SimpleMotorFeedforward m_feedforward = Constants.motor_feedforward; 
 
-    private final PIDController m_pidL = new PIDController(1, 0, 0);
-    private final PIDController m_pidR = new PIDController(1, 0, 0);
+    private ProfiledPIDController m_pidL = new ProfiledPIDController(0.2, 0, 0, Constants.motor_constraints);
+    private ProfiledPIDController m_pidR = new ProfiledPIDController(0.2, 0, 0, Constants.motor_constraints);
     
     private DifferentialDriveKinematics m_kinematics = 
         new DifferentialDriveKinematics(Constants.TrackWidth);
@@ -89,7 +91,7 @@ public class Drivetrain extends SubsystemBase
         m_motorR_follower.follow(m_motorR_leader);
         m_motorL_follower.follow(m_motorL_leader);
         
-        m_motorR_leader.setInverted(true);
+        m_motorL_leader.setInverted(true);
         m_encoderL.setReverseDirection(true);
 
         m_encoderL.setDistancePerPulse(Math.PI * Constants.WheelDiameter / Constants.EncoderResolution);
@@ -119,6 +121,9 @@ public class Drivetrain extends SubsystemBase
 
     public void speed(DifferentialDriveWheelSpeeds speeds) 
     {
+        Logger.recordOutput(getName() + "/leftSpeed", speeds.leftMetersPerSecond);
+        Logger.recordOutput(getName() + "/rightSpeed", speeds.rightMetersPerSecond);
+
         voltage(
             m_pidL.calculate(m_encoderL.getRate(), speeds.leftMetersPerSecond)
               + m_feedforward.calculate(speeds.leftMetersPerSecond),
@@ -129,6 +134,9 @@ public class Drivetrain extends SubsystemBase
 
     public void voltage(double leftVolts, double rightVolts)
     {
+        Logger.recordOutput(getName() + "/leftVolts", leftVolts);
+        Logger.recordOutput(getName() + "/rightVolts", rightVolts);
+
         m_motorL_leader.setVoltage(leftVolts);
         m_motorR_leader.setVoltage(rightVolts);
     }
@@ -159,34 +167,42 @@ public class Drivetrain extends SubsystemBase
             m_gyro.toRotation2d(),
             m_encoderL.getDistance(),
             m_encoderR.getDistance());
-    }
 
-    @Override
-    public void simulationPeriodic() {
-        // This method will be called once per scheduler run during simulation
+        Logger.recordOutput(getName() + "/gyro", m_gyro.toRotation2d().getDegrees());
+        Logger.recordOutput(getName() + "/pose", pose2d());
+
+        Logger.recordOutput(getName() + "/encoderL", m_encoderL.getRate());
+        Logger.recordOutput(getName() + "/encoderR", m_encoderR.getRate());
     }
 
-    public Command trajectory(Trajectory trajectory)
+    public Command followTrajectory(Trajectory trajectory)
     {
-        // TOOD: Implement
-        // return this.runOnce(...)   
+        // RamseteCommand ramsete = new RamseteCommand(
+        //     trajectory, this::pose2d, 
+        //     new RamseteController(0, 0), 
+        //     m_feedforward, m_kinematics, 
+        //     () -> new DifferentialDriveWheelSpeeds(m_encoderL.getRate(), m_encoderR.getRate()), 
+        //     m_pidR, m_pidL, this::voltage, this);
+
+        // return Commands.runOnce(() -> this.reset())
+        //                .andThen(ramsete)
+        //                .andThen(Commands.runOnce(() -> voltage(0, 0)));
         return Commands.none();
     }
 
     public Command sysIdQausistatic(SysIdRoutine.Direction direction)
     {
         return Commands.race(m_sysIdRoutine.quasistatic(direction),
-            Commands.runOnce(() -> {
+            this.runOnce(() -> {
                 while(m_encoderL.getDistance() < 6 && 
                       m_encoderR.getDistance() < 6);
             }));
-        
     }
 
     public Command sysIdDynamic(SysIdRoutine.Direction direction)
     {
         return Commands.race(m_sysIdRoutine.dynamic(direction),
-        Commands.runOnce(() -> {
+        this.runOnce(() -> {
             while(m_encoderL.getDistance() < 6 && 
                   m_encoderR.getDistance() < 6);
         }));
diff --git a/src/main/java/frc/robot/subsystems/Hooks.java b/src/main/java/frc/robot/subsystems/Hooks.java
new file mode 100644
index 0000000..eb9c4f7
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Hooks.java
@@ -0,0 +1,8 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Hooks extends SubsystemBase 
+{
+    
+}
diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java
new file mode 100644
index 0000000..85cbbf8
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Intake.java
@@ -0,0 +1,34 @@
+package frc.robot.subsystems;
+
+import org.littletonrobotics.junction.Logger;
+import com.revrobotics.CANSparkMax;
+
+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 Intake() { }
+
+    public void atPercentage(double perc) 
+    {
+        atPercentage(perc, perc); 
+    }
+
+    public void atPercentage(double upPerc, double downPerc) 
+    {
+        m_intakeT.set(upPerc);
+        m_intakeD.set(downPerc);
+    }
+
+    @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
new file mode 100644
index 0000000..f171049
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Shooter.java
@@ -0,0 +1,33 @@
+package frc.robot.subsystems;
+
+import org.littletonrobotics.junction.Logger;
+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 Shooter() { }
+
+    public void atPercentage(double perc)
+    {
+        atPercentage(perc, perc);
+    }
+
+    public void atPercentage(double upPerc, double downPerc)
+    {
+        m_shooterT.set(upPerc);
+        m_shooterD.set(downPerc);
+    }
+
+    @Override
+    public void periodic() 
+    {     
+        Logger.recordOutput(getName() + "/speedT", m_shooterT.getEncoder().getVelocity());
+        Logger.recordOutput(getName() + "/speedD", m_shooterD.getEncoder().getVelocity());
+    }
+}