Kid Shampoo Initial Subsystem work

Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Hardware.java

@@ -2,10 +2,10 @@
 
 import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
-import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.ServoController;
+import com.technototes.library.hardware.motor.CRServo;
 import com.technototes.library.hardware.motor.EncodedMotor;
 import com.technototes.library.hardware.motor.Motor;
 import com.technototes.library.hardware.sensor.IMU;
@@ -23,6 +23,8 @@ public class Hardware implements Loggable {
     public IMU imu;
     public EncodedMotor<DcMotorEx> fl, fr, rl, rr;
     public MotorEncoder odoF, odoR;
+    public Servo retainer, jaw;
+    public CRServo intake;
 
     /* Put other hardware here! */
 
@@ -43,6 +45,11 @@ public Hardware(HardwareMap hwmap) {
             odoR = new MotorEncoder(Setup.HardwareNames.ODOR);
             odoF = new MotorEncoder(Setup.HardwareNames.ODOF);
         }
+        if (Setup.Connected.KIDSSHAMPOOSUBSYSTEM) {
+            intake = new CRServo(Setup.HardwareNames.INTAKE);
+            retainer = new Servo(Setup.HardwareNames.RETAINER);
+            jaw = new Servo(Setup.HardwareNames.JAW);
+        }
     }
 
     // We can read the voltage from the different hubs for fun...

Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Setup.java

@@ -10,6 +10,7 @@ public static class Connected {
         public static boolean DRIVEBASE = true;
         public static boolean ODOSUBSYSTEM = true;
         public static boolean SAFETYSUBSYSTEM = true;
+        public static boolean KIDSSHAMPOOSUBSYSTEM = true;
     }
 
     @Config
@@ -22,6 +23,9 @@ public static class HardwareNames {
         public static String IMU = "imu";
         public static String ODOF = "odof";
         public static String ODOR = "odor";
+        public static String INTAKE = "intake";
+        public static String RETAINER = "retainer";
+        public static String JAW = "jaw";
     }
 
     @Config

Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/KidShampoo.java

@@ -0,0 +1,60 @@
+package org.firstinspires.ftc.twenty403.subsystems;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.technototes.library.hardware.motor.CRServo;
+import com.technototes.library.hardware.servo.Servo;
+import com.technototes.library.logger.Loggable;
+import com.technototes.library.subsystem.Subsystem;
+import org.firstinspires.ftc.twenty403.Hardware;
+
+@Config
+public class KidShampoo implements Subsystem, Loggable {
+
+    private Servo retainer, jaw;
+    private CRServo intake;
+
+    public static double RETAINER_OPEN_POSITION = -.2;
+    public static double RETAINER_EAT_POSITION = -.1;
+    public static double RETAINER_CLOSE_POSITION = .1;
+
+    public static double JAW_BITE_POSITION = .1;
+
+    public static double JAW_RELEASE_POSITION = -.1;
+    public static double INTAKE_SLURP = -.1;
+
+    public static double INTAKE_SPIT = .1;
+
+    public KidShampoo(Hardware hw) {
+        intake = hw.intake;
+        retainer = hw.retainer;
+        jaw = hw.jaw;
+    }
+
+    public void openRetainer() {
+        retainer.setPosition(RETAINER_OPEN_POSITION);
+    }
+
+    public void eatRetainer() {
+        retainer.setPosition(RETAINER_EAT_POSITION);
+    }
+
+    public void closeRetainer() {
+        retainer.setPosition(RETAINER_CLOSE_POSITION);
+    }
+
+    public void biteJaw() {
+        jaw.setPosition(JAW_BITE_POSITION);
+    }
+
+    public void releaseJaw() {
+        jaw.setPosition(JAW_RELEASE_POSITION);
+    }
+
+    public void slurpIntake() {
+        intake.setPower(INTAKE_SLURP);
+    }
+
+    public void spitIntake() {
+        intake.setPower(INTAKE_SPIT);
+    }
+}