Dual Motor Slide Subsystem

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java

@@ -0,0 +1,131 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR;
+import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+public class DualMotorSliderSubsystem {
+
+    /*
+    liftSlideLeft - Left Motor for Dual Linear Slide as a DcMotorEx object
+    Currently, the value of (liftSlideLeft) is null because we haven't assigned a value (object) to it yet.
+    It expects an object of type "DcMotorEx".
+     */
+    private DcMotorEx liftSlideLeft;
+
+    /*
+    liftSlideRight - Right Motor for Dual Linear Slide as a DcMotorEx object
+    Currently, the value of (liftSlideRight) is null because we haven't assigned a value (object) to it yet.
+    It expects an object of type "DcMotorEx".
+     */
+    private DcMotorEx liftSlideRight;
+
+    /*
+    targetPosition - Variable that holds target position of slides.
+    */
+    private int targetPosition = 0;
+
+    /*
+    getTargetPosition/setTargetPosition - Best practice to "hide" (private) targetPosition and,
+                                          instead, use a "setter" to set the target position value or
+                                          a "getter" to get the target position value.
+    */
+    public void setTargetPosition(int value) {
+        targetPosition = value;
+    }
+    private int getTargetPosition() { return targetPosition; }
+
+    /*
+    PID - Proportional/Integral/Derivative Values
+
+    For a dual motor linear slide, we only tune the P - Proportion.
+    The Proportion variable (kp) answers the question "how fast do we want to get to our destination?"
+
+    It's the only value we set because the variable ki and kd deal with how to handle when we're off the path.
+    Since we're going straight, we don't need to worry about.
+    */
+    public final static double kp = 0.0015, ki = 0, kd = 0;
+
+    /*
+    lastError/integralSum/timer - These 3 variables are placeholders in determining how much
+                                  power to send to both motors.
+    */
+    private double lastError = 0;
+    private double integralSum = 0;
+    private ElapsedTime timer = new ElapsedTime();
+
+    public DualMotorSliderSubsystem(HardwareMap hardwareMap) {
+        /*
+        liftSlideLeft/liftSlideRight - Now, we are assigning a value of DcMotorEx to each variable
+
+        We "assign" the object DcMotorEx to liftSlideLeft and liftSlideRight and "link them" to the
+        driver hub configuration name that matches the motor on the slide.
+         */
+        liftSlideLeft = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_LEFT_MOTOR);
+        liftSlideRight = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_RIGHT_MOTOR);
+    }
+
+    public void init() {
+        /*
+        Initialize the motors with the following settings (assuming slide is at the very bottom position):
+
+        - Reset the encoders to be zero
+        - When the motor stops moving (zero power), brake.  This means we can't move the motors, not even gravity.
+        - Sets the motor to run without the encoder.  This doesn't mean we won't use the encoder values (because we will).
+          It just means to not FULLY depend on them, we will just prefer to use motor power instead.
+         */
+        liftSlideLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        liftSlideLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        liftSlideLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        liftSlideRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        liftSlideRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        liftSlideRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+    }
+
+    /*
+    To calculate power, we send the targetPosition value (how high we want the slider motors to
+    be based on motor 'ticks') to calculatePower function.
+
+    For every loop that occurs, it will constantly calculate power.  So long as we're far away from
+    our "target" position, we will get as much power as possible.  The closer we get, the lower the
+    power we will receive.
+    */
+    public void update() {
+        double power = calculatePower();
+        liftSlideLeft.setPower(power);
+        liftSlideRight.setPower(power);
+    }
+
+    /*
+    Calculating power - To calculate the power, we determine the proportion, derivative and
+    integral of our closed loop system.
+
+    For more information, please visit:
+    - Introduction to Closed Loop System:
+      - https://www.ctrlaltftc.com/introduction-to-closed-loop-control
+    - The PID controller:
+      - https://www.ctrlaltftc.com/the-pid-controller
+    */
+    private double calculatePower() {
+        double error = getTargetPosition() - liftSlideLeft.getCurrentPosition();
+        integralSum += error * timer.seconds();
+        double derivative = (error - lastError) / timer.seconds();
+        lastError = error;
+        timer.reset();
+        return (error * kp) + (derivative * kd) + (integralSum * ki);
+    }
+
+    public void toLowBucketPosition() {
+        setTargetPosition(1500);
+    }
+
+    public void toHighBucketPosition() {
+        setTargetPosition(3000);
+    }
+
+}