diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Hardware.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Hardware.java index 3ca9669..7a254fb 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Hardware.java +++ b/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 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... diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Setup.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Setup.java index ab52333..0f0b1d1 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Setup.java +++ b/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 diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/KidShampoo.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/KidShampoo.java new file mode 100644 index 0000000..d027bef --- /dev/null +++ b/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); + } +}