Merge pull request #38 from technototes/braelyn-10-22-horizontal_slid…

…es_subsystem

fixed increment and decrement on wrist for horizontal slides. should …

Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/HorizontalSlidesSubsystem.java

@@ -2,14 +2,15 @@
 
 import com.acmerobotics.dashboard.config.Config;
 import com.technototes.library.hardware.servo.Servo;
+import com.technototes.library.logger.Log;
 import com.technototes.library.logger.Loggable;
 import com.technototes.library.subsystem.Subsystem;
 import org.firstinspires.ftc.sixteen750.Hardware;
 
 @Config
 public class HorizontalSlidesSubsystem implements Subsystem, Loggable {
 
-    //slides servo - outstretched, retracted, transfer/neutral?
+    //slides servo (link servo) - outstretched, retracted, transfer/neutral?
     //wrist servo - transfer, pickup, neutral, wall pickup for specimen
     //claw servo - drop (open), pickup (long and short)(close)
 
@@ -27,10 +28,13 @@ public class HorizontalSlidesSubsystem implements Subsystem, Loggable {
     public static double ClawWristServoPickup = 0.05;
     public static double ClawWristServoIncrement = 0.555;
 
-    public Servo clawwristServo;
+    @Log(name = "wristTarget")
+    public double wristTargetPos;
+    public Servo wristServo;
     public Servo clawServo;
     public Servo linkServo;
 
+
     private boolean isHardware;
 
     public HorizontalSlidesSubsystem(Hardware hw) {
@@ -80,11 +84,18 @@ public void ClawWristServoTransfer() {
 
     public void ClawWristServoIncrement() {
         // the arm's position to score
-        clawwristServo.setPosition(ClawWristServoIncrement + 0.05);
+        setWristPos(wristTargetPos + WristServoIncrement);
     }
 
     public void ClawWristServoDecrement() {
         // the arm's position to score
-        clawwristServo.setPosition(ClawWristServoIncrement - 0.05);
+        setWristPos(wristTargetPos - WristServoIncrement);
+    }
+
+    private void setWristPos(double w) {
+        if (wristServo != null) {
+            wristServo.setPosition(w);
+            wristTargetPos = w;
+        }
     }
 }