9 files changed, 309 insertions, 77 deletions

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 017fa74..1ff70bb 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -3,31 +3,73 @@ package frc.robot;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 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;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.util.IMUGyro;
 
-public class Constants 
+public class Constants
 {
-    public static final CommandXboxController controller = new CommandXboxController(0);
+    public static final GenericHID controller = new GenericHID(0);
 
     public static final double DriveMaxSpeed = 5; // m/s
     public static final double DriveMaxAngular = Math.PI; // rad/s?
 
+    // for Drivetrain
     public static final double WheelDiameter = 0.1524; // in meters
     public static final double TrackWidth = 0.685;  // in meters
-
     public static final double EncoderResolution = 4096 / 24;
-    
+
     public static final IMUGyro gyro = new IMUGyro(new I2C(Port.kOnboard, 0x68));
 
+    public static final TrapezoidProfile.Constraints motor_constraints = 
+        new TrapezoidProfile.Constraints(DriveMaxSpeed, 4.5);
+    public static final SimpleMotorFeedforward motor_feedforward = new SimpleMotorFeedforward(1.1, 3.5);
+    // public static final SimpleMotorFeedforward motor_feedforward = new SimpleMotorFeedforward(0, 0, 0);
+
     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 Encoder encoderL = new Encoder(0, 1);
-    public static final Encoder encoderR = new Encoder(2, 3);
+    public static final Encoder encoderL = new Encoder(6, 7); 
+    public static final Encoder encoderR = new Encoder(4, 5);
+
+    // for Intake
+    public static final CANSparkMax intakeT = null;//new CANSparkMax(41, MotorType.kBrushless);
+    public static final CANSparkMax intakeD = null;//new CANSparkMax(42, MotorType.kBrushless);
+    // for Shooter
+    public static final CANSparkMax shooterT = null;//new CANSparkMax(61, MotorType.kBrushless);
+    public static final CANSparkMax shooterD = null;//new CANSparkMax(62, MotorType.kBrushless);
+
+    // for Arm]
+    public static final double ArmGearReduction = 60;
+
+    public static final ArmFeedforward arm_feedforward = new ArmFeedforward(0, 0, 0);
+    public static final TrapezoidProfile.Constraints arm_constraints = 
+        new TrapezoidProfile.Constraints(0, 0);
+
+    public static final double ArmkP = 0.0;
+    public static final double ArmkI = 0.0;
+    public static final double ArmkD = 0.0;
+
+    public static final CANSparkMax armL = null;//new CANSparkMax(51, MotorType.kBrushless);
+    public static final CANSparkMax armR = null;//new CANSparkMax(52, MotorType.kBrushless);
+
+    // for Chain 
+    public static final double ChainEncoderScale = 2600;
+
+    public static final TrapezoidProfile.Constraints chain_constraints = 
+        new TrapezoidProfile.Constraints(0, 0);
+
+    public static final double ChainkP = 0.0;
+    public static final double ChainkI = 0.0;
+    public static final double ChainkD = 0.0;
+    
+    public static final CANSparkMax chain = null;//new CANSparkMax(70, MotorType.kBrushed);
+    public static final Encoder chain_encoder = null;//new Encoder(3, 4);
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index bdaae21..0a5123a 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,17 +4,23 @@
 
 package frc.robot;
 
-import edu.wpi.first.wpilibj.TimedRobot;
+import org.littletonrobotics.junction.LoggedRobot;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
+
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
-public class Robot extends TimedRobot {
+public class Robot extends LoggedRobot {
   private Command m_autonomousCommand;
 
   private RobotContainer m_robotContainer;
 
   @Override
   public void robotInit() {
+    Logger.addDataReceiver(new NT4Publisher());
+    Logger.start();
+
     m_robotContainer = new RobotContainer();
   }
 
@@ -54,22 +60,8 @@ public class Robot extends TimedRobot {
     }
   }
 
-  // GenericHID controller = Constants.controller;
-  // CANSparkMax shooter_top = new CANSparkMax(21, MotorType.kBrushless);
-  // CANSparkMax shooter_bottom = new CANSparkMax(22, MotorType.kBrushless);
-  // CANSparkMax intakeL = new CANSparkMax(31, MotorType.kBrushless);
-  // CANSparkMax intakeR = new CANSparkMax(32, MotorType.kBrushless);
-
   @Override
-  public void teleopPeriodic() 
-  {
-    // shooter_top.setInverted(true);
-    // shooter_bottom.setInverted(true);
-    // shooter_top.set(controller.getRawAxis(1));
-    // shooter_bottom.set(controller.getRawAxis(3));
-    // intakeL.set(controller.getRawAxis(1));
-    // intakeR.set(controller.getRawAxis(3));
-  }
+  public void teleopPeriodic() {}
 
   @Override
   public void teleopExit() {}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index ed6afe9..cbaceea 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,64 +4,68 @@
 
 package frc.robot;
 
-import edu.wpi.first.math.geometry.Pose2d;
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.subsystems.Drivetrain;
 
 public class RobotContainer 
 {
     private final Drivetrain m_drive = new Drivetrain();
 
-    private final CommandXboxController m_controller = Constants.controller;
+    private final GenericHID m_controller = Constants.controller;
 
     public RobotContainer() 
     {
         configureBindings();
     }
 
-    private double deadzone(double low, double high, double val) {
+    private double processInput(double val) 
+    {  
+        double input = deadzone(-0.06, 0.06, val);
+        return input * input * input;
+    }
+
+    private double deadzone(double low, double high, double val)
+    {
         if(val > low && val < high) return 0.0;
-        return val;
+        return ((val > 0.0) ? val - high : val - low);
     }
 
     private void configureBindings() 
     {
-        m_drive.setDefaultCommand(m_drive.run(() -> 
-        m_drive.tank(
-            deadzone(-0.09, 0.09, m_controller.getLeftY()) * Constants.DriveMaxSpeed,
-            deadzone(-0.09, 0.09, m_controller.getRightY()) * Constants.DriveMaxSpeed
-        )));
+         m_drive.setDefaultCommand(m_drive.run(() -> 
+         m_drive.tank(
+             processInput(m_controller.getRawAxis(2)) * Constants.DriveMaxSpeed,
+             processInput(m_controller.getRawAxis(1)) * Constants.DriveMaxSpeed
+         )));
 
-        // debug controls
-        m_controller.a().onTrue(Commands.runOnce(() -> {
-            Pose2d pose = m_drive.pose2d();
-            System.out.println("x: " + pose.getX() +" y: "+ pose.getY());
-            System.out.println("yaw: " + Constants.gyro.toRotation2d().getDegrees());
-        })); 
-        m_controller.b().onTrue(Commands.runOnce(() -> {
-            System.out.println("Reset");
-            m_drive.reset();
-        })); 
+        // m_drive.setDefaultCommand(m_drive.run(() -> {
+        //     Logger.recordOutput("inputL", processInput(m_controller.getRawAxis(2)));
+        //     Logger.recordOutput("inputR", processInput(m_controller.getRawAxis(1)));
+        // }));
+        // m_controller.b().onTrue(Commands.runOnce(() -> {
+        //     m_drive.reset();
+        // })); 
 
-        m_controller.x().and(m_controller.leftBumper())
-            .onTrue(Commands.sequence(Commands.runOnce(() -> 
-                    System.out.println("Quasistatic Forward")),
-                m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward)));
-        m_controller.x().and(m_controller.rightBumper())
-            .onTrue(Commands.sequence(Commands.runOnce(() -> 
-                    System.out.println("Quasistatic Reverse")),
-                m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse)));
-        m_controller.y().and(m_controller.leftBumper())
-            .onTrue(Commands.sequence(Commands.runOnce(() -> 
-                    System.out.println("Dynamic Forward")),
-                m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward)));
-        m_controller.y().and(m_controller.rightBumper())
-            .onTrue(Commands.sequence(Commands.runOnce(() -> 
-                    System.out.println("Dynamic Reverse")),
-                m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse)));
+        // m_controller.x().and(m_controller.leftBumper())
+        //     .onTrue(Commands.sequence(Commands.runOnce(() -> 
+        //             System.out.println("Quasistatic Forward")),
+        //         m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward)));
+        // m_controller.x().and(m_controller.rightBumper())
+        //     .onTrue(Commands.sequence(Commands.runOnce(() -> 
+        //             System.out.println("Quasistatic Reverse")),
+        //         m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse)));
+        // m_controller.y().and(m_controller.leftBumper())
+        //     .onTrue(Commands.sequence(Commands.runOnce(() -> 
+        //             System.out.println("Dynamic Forward")),
+        //         m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward)));
+        // m_controller.y().and(m_controller.rightBumper())
+        //     .onTrue(Commands.sequence(Commands.runOnce(() -> 
+        //             System.out.println("Dynamic Reverse")),
+        //         m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse)));
     }
 
     public Command getAutonomousCommand() 
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());
+    }
+}