npc coding fr

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/HorizontalSlidePIDTuner.java

@@ -0,0 +1,68 @@
+package org.firstinspires.ftc.teamcode.common;
+
+
+import com.acmerobotics.dashboard.config.Config;
+import com.arcrobotics.ftclib.controller.PIDController;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+
+
+import org.firstinspires.ftc.teamcode.Robot;
+
+
+import java.util.List;
+
+@Config
+@TeleOp
+
+public class HorizontalSlidePIDTuner extends OpMode {
+
+    public DcMotorEx liftMotorOne;
+    public PIDController liftController;
+    private Robot robot;
+
+    public static double slideP = 0;
+    public static double slideI = 0;
+    public static double slideD = 0;
+    public static double SLIDE_TICKS_PER_INCH = 2 * Math.PI * 0.764445002 / 537.7;
+    public static double targetPosition = 0;
+    public List<LynxModule> controllers;
+
+
+    @Override
+    public void init(){
+        liftController = new PIDController(slideP, slideI, slideD);
+        //this.robot.reset();
+        liftMotorOne = hardwareMap.get(DcMotorEx.class, "liftOne");
+        //liftEncoder = hardwareMap.get(DcMotorEx.class, "LB");
+        liftMotorOne.setDirection(DcMotorSimple.Direction.REVERSE);
+        //liftEncoder.setDirection(DcMotorSimple.Direction.REVERSE);
+        controllers = hardwareMap.getAll(LynxModule.class);
+        for(LynxModule module : controllers){
+            module.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+        }
+
+    }
+    @Override
+    public void loop(){
+        liftController.setPID(slideP, slideI, slideD);
+        double liftPosition = liftMotorOne.getCurrentPosition() * SLIDE_TICKS_PER_INCH;
+
+        double liftPower = liftController.calculate(liftPosition, targetPosition);
+        liftMotorOne.setPower(liftPower);
+
+        telemetry.addData("Lift Position, Motor 1", liftPosition);
+        telemetry.addLine();
+
+        telemetry.addData("Lift Target", targetPosition);
+        telemetry.addData("Lift Power", liftPower);
+        telemetry.update();
+
+        for(LynxModule module : controllers){
+            module.clearBulkCache();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/LinearSlidePIDTuner.java

@@ -30,12 +30,9 @@ public class LinearSlidePIDTuner extends OpMode {
 
 
     private Robot robot;
-
-
     public static double slideP = 0;
     public static double slideI = 0;
     public static double slideD = 0;
-    public static double slideKg = 0;
     public static double SLIDE_TICKS_PER_INCH = 2 * Math.PI * 0.764445002 / 537.7;
     public static double targetPosition = 0;
     public List<LynxModule> controllers;
@@ -67,7 +64,6 @@ public void loop(){
         double pid = liftController.calculate(liftPosition2, targetPosition);
         double liftPower = pid + slideKg;
 
-
         liftMotorOne.setPower(liftPower);
         liftMotorTwo.setPower(liftPower);
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HorizontalLiftSubsystem.java

@@ -7,28 +7,50 @@
 import com.arcrobotics.ftclib.controller.PIDController;
 import com.arcrobotics.ftclib.hardware.motors.MotorEx;
 import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.util.ElapsedTime;
 
 public class HorizontalLiftSubsystem extends SubsystemBase {
-    public final MotorEx motor;
+    public final MotorEx horizontalLift;
     private final PIDController controller;
     private MotionProfile profile;
     public MotionState state;
-    public static double P = 0.17;
-    public static double I = 0;
-    public static double D = 0;
-    public double currentLiftPosition;
-    public double targetLiftPosition;
+
+    public static double P = 0.17, I = 0.0, D = 0.0;
+    private final ElapsedTime timer, voltageTimer;
+    private double liftPosition, targetLiftPosition;
+
     public double liftPower;
-    private final double slidesTickPerInch = 2 * Math.PI * 0.701771654 / 145.1;
+    private final double SLIDE_TICK = 2 * Math.PI * 0.701771654 / 145.1;
     private boolean isAuto = false;
     public HorizontalLiftSubsystem(HardwareMap hardwareMap, boolean isAuto){
-        motor = new MotorEx(hardwareMap, "horizontalMotor");
+        horizontalLift = new MotorEx(hardwareMap, "horizontalMotor");
         controller = new PIDController(P, I, D);
         controller.setPID(P, I, D);
         this.isAuto = isAuto;
 
+
         profile = MotionProfileGenerator.generateSimpleMotionProfile(new MotionState(1, 0),
                 new MotionState(0,0), 0, 0);
         // values are not final
+        timer = new ElapsedTime(); voltageTimer = new ElapsedTime();
+    }
+
+    public void read(){
+        liftPosition = horizontalLift.encoder.getPosition() * SLIDE_TICK;
+    }
+    public void write(){
+        horizontalLift.set(liftPower);
     }
+
+    public void setTargetLiftPosition(double v){targetLiftPosition = v;}
+
+    public double getLiftPosition(){return liftPosition;}
+
+    public void newProfile(double targetPos, double max_v, double max_a){
+        profile = MotionProfileGenerator.generateSimpleMotionProfile(
+                new MotionState(liftPosition, 0),
+                new MotionState(targetPos, 0), max_v, max_a
+        );
+    }
+
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/VerticalLiftSubsystem.java

@@ -40,5 +40,7 @@ public VerticalLiftSubsystem(HardwareMap hardwareMap, boolean isAuto){
         lift2.setInverted(true);
         lift2.encoder.setDirection(MotorEx.Direction.REVERSE);
 
+        liftController = new PIDController(P, I, D);
+
     }
 }