CR Servo Extension

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoAccepting.java

@@ -23,6 +23,6 @@ public void initialize() {
 
     @Override
     public boolean isFinished() {
-        return true;
+        return this.extendo.isFinished();
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoReady.java

@@ -23,6 +23,6 @@ public void initialize() {
 
     @Override
     public boolean isFinished() {
-        return true;
+        return this.extendo.isFinished();
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoScore.java

@@ -23,6 +23,6 @@ public void initialize() {
 
     @Override
     public boolean isFinished() {
-        return true;
+        return this.extendo.isFinished();
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftSlam.java

@@ -9,7 +9,7 @@ public class LiftSlam extends CommandBase {
     private final MultipleTelemetry telemetry;
     private final Lift lift;
 
-    public LiftSlam(final MultipleTelemetry telemetry, final Lift lift){
+    public LiftSlam(final MultipleTelemetry telemetry, final Lift lift) {
         this.telemetry = telemetry;
 
         this.lift = lift;
@@ -18,7 +18,7 @@ public LiftSlam(final MultipleTelemetry telemetry, final Lift lift){
 
     @Override
     public void initialize() {
-        this.lift.setTarget(this.lift.getTarget() - 4 * Lift.INCREMENT);
+        this.lift.setTarget(this.lift.getTarget() - Lift.SLAM);
     }
 
     @Override

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ExtendoTest.java

@@ -33,10 +33,17 @@ public void runOpMode() throws InterruptedException {
 
         super.waitForStart();
 
+        this.extendo.startThread(this);
         while (opModeIsActive()) {
             super.resetCycle();
             CommandScheduler.getInstance().run();
 
+            super.multipleTelemetry.addData("Position", this.extendo.getPosition());
+            super.multipleTelemetry.addData("Target", this.extendo.getTarget());
+            super.multipleTelemetry.addData("Error", this.extendo.getError());
+
+            this.extendo.setConstants();
+
             super.logCycles();
             super.multipleTelemetry.update();
         }

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

@@ -2,33 +2,69 @@
 
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.arcrobotics.ftclib.command.CommandOpMode;
 import com.arcrobotics.ftclib.command.SubsystemBase;
+import com.qualcomm.robotcore.hardware.AnalogInput;
+import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.HardwareMap;
-import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.teamcode.utils.PIDController;
 
 @Config
 public class Extendo extends SubsystemBase {
-    public static double ACCEPTING = 0.55;
-    public static double SCORE = 0.55;
-    public static double READY = 0.1;
-    public static double INCREMENT = 0.05;
+    public static double kP = 0.015;
+    public static double kI = 0.0001;
+    public static double kD = 0.0001;
+    public static double OFFSET = 306.0;
+
+    public static int ALLOWED_ERROR = 15;
+    public static int SCORE = 0;
+    public static int READY = 100;
+    public static int ACCEPTING = 350;
 
     private final MultipleTelemetry telemetry;
-    private final Servo extendo;
+
+    public final AnalogInput encoder;
+    private final CRServo extendo;
+    private final PIDController controller;
 
     public Extendo(final HardwareMap hwMap, final MultipleTelemetry telemetry) {
         this.telemetry = telemetry;
+        this.controller = new PIDController(Extendo.kP, Extendo.kI, Extendo.kD);
+
+        this.extendo = hwMap.get(CRServo.class, "extendo");
+        this.encoder = hwMap.get(AnalogInput.class, "encoder");
+    }
+
+    public synchronized double getPosition() {
+        return Extendo.OFFSET - (this.encoder.getVoltage() / 3.3 * 360);
+    }
 
-        this.extendo = hwMap.get(Servo.class, "extendo");
+    public void startThread(CommandOpMode opMode) {
+        while (opMode.opModeIsActive())
+            synchronized (this.extendo) {
+                double power = this.controller.calculate(this.getPosition());
+                this.extendo.setPower(power);
+            }
+    }
 
-        this.extendo.setPosition(Extendo.READY);
+    public void setConstants() {
+        this.controller.setCoefficients(Extendo.kP, Extendo.kI, Extendo.kD);
     }
 
-    public double getPosition() {
-        return this.extendo.getPosition();
+    public double getTarget() {
+        return this.controller.getTarget();
     }
 
     public void setPosition(double position) {
-        this.extendo.setPosition(position);
+        this.controller.setTarget(position);
+    }
+
+    public double getError() {
+        return this.controller.getLastError();
+    }
+
+    public boolean isFinished() {
+        return this.controller.isFinished();
     }
 }

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

@@ -18,13 +18,15 @@ public class Lift extends SubsystemBase {
     public static double LOW_VOLTAGE = 12.0;
     public static double ALLOWED_ERROR = 15;
 
-    public static double ACCEPTING = 50;
     public static double LOW_BASKET = 1500;
     public static double HIGH_BASKET = 2800;
 
     public static double LOW_RUNG = 1000;
     public static double HIGH_RUNG = 2100;
+
+    public static double ACCEPTING = 50;
     public static double INCREMENT = 250;
+    public static double SLAM = 750;
 
     public static double kP = 0.0015;
     public static double kI = 0.0006;
@@ -60,7 +62,8 @@ public Lift(final HardwareMap hwMap, final MultipleTelemetry telemetry) {
         this.controller = new PIDController(kP, kI, kD);
         this.controller.setAllowedError(Lift.ALLOWED_ERROR);
 
-        this.reset();
+        // TODO: Make this work.
+        // this.reset();
     }
 
     public void startThread(CommandOpMode opMode) {
@@ -123,13 +126,17 @@ public double getError() {
     }
 
     public void setConstants() {
-        this.controller.setCoefficients(kP, kI, kD, kG);
+        this.controller.setCoefficients(kP, kI, kD);
     }
 
     public void reset() {
-        setTarget(-999999999);
+        this.setTarget(-999999999);
         while (this.voltage.getVoltage() > Lift.LOW_VOLTAGE) {
         }
-        setTarget(0);
+
+        this.leftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        this.rightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        this.setTarget(0);
     }
 }