Added lift configurations

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

@@ -0,0 +1,72 @@
+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.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+
+@Config
+@Autonomous(name = "Lift Motor Subsystem - PID Test")
+public class LiftMotorSubsystem extends LinearOpMode {
+
+    private DcMotorEx liftSlideLeft;
+    private DcMotorEx liftSlideRight;
+    public static double kp = 0.0015, ki = 0, kd = 0;
+    private double lastError = 0;
+    private double integralSum = 0;
+    public static int targetPosition = 0;
+    private final FtcDashboard dashboard = FtcDashboard.getInstance();
+    private ElapsedTime timer = new ElapsedTime();
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        TelemetryPacket packet = new TelemetryPacket();
+        dashboard.setTelemetryTransmissionInterval(25);
+
+        liftSlideLeft = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_LEFT_MOTOR);
+        liftSlideRight = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_RIGHT_MOTOR);
+
+        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);
+
+        waitForStart();
+
+        while(opModeIsActive()) {
+            double power = calculatePower(targetPosition, liftSlideLeft.getCurrentPosition());
+
+            packet.put("Power", power);
+            packet.put("Position", liftSlideLeft.getCurrentPosition());
+            packet.put("Error", lastError);
+            packet.put("Seconds", timer.seconds());
+
+            liftSlideLeft.setPower(power);
+            liftSlideRight.setPower(power);
+
+            dashboard.sendTelemetryPacket(packet);
+        }
+    }
+
+    private double calculatePower(int targetPosition, int currentPosition) {
+        // reference is targetPosition, state is currentPosition
+        double error = targetPosition - currentPosition;
+        integralSum += error * timer.seconds();
+        double derivative = (error - lastError) / timer.seconds();
+        lastError = error;
+        timer.reset();
+        return (error * kp) + (derivative * kd) + (integralSum * ki);
+    }
+
+}