Added secondary intake motor, cleaned up subsystem

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotContoller.java

@@ -10,37 +10,19 @@
 import com.pedropathing.geometry.Pose;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
-import org.firstinspires.ftc.library.command.Command;
 import org.firstinspires.ftc.library.command.CommandOpMode;
-import org.firstinspires.ftc.library.command.CommandScheduler;
 import org.firstinspires.ftc.library.command.RunCommand;
-import org.firstinspires.ftc.library.command.button.Trigger;
 import org.firstinspires.ftc.library.gamepad.GamepadEx;
-import org.firstinspires.ftc.library.gamepad.GamepadKeys;
-import org.firstinspires.ftc.robotcore.external.Supplier;
-import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.autonomous.Auto;
 import org.firstinspires.ftc.teamcode.autonomous.Location;
-import org.firstinspires.ftc.teamcode.command_factories.IntakeFactory;
-import org.firstinspires.ftc.teamcode.command_factories.ShooterFactory;
-import org.firstinspires.ftc.teamcode.command_factories.TransferFactory;
-import org.firstinspires.ftc.teamcode.command_factories.TurretFactory;
-import org.firstinspires.ftc.teamcode.commands.ConstrainedFlashCommand;
-import org.firstinspires.ftc.teamcode.commands.TeleopMecanum;
-import org.firstinspires.ftc.teamcode.commands.TurretPositionSetpoint;
 import org.firstinspires.ftc.teamcode.constants.GlobalConstants;
-import org.firstinspires.ftc.teamcode.constants.LEDConstants;
-import org.firstinspires.ftc.teamcode.constants.ShooterConstants;
-import org.firstinspires.ftc.teamcode.constants.TransferConstants;
-import org.firstinspires.ftc.teamcode.constants.TurretConstants;
 import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
 import org.firstinspires.ftc.teamcode.subsystems.Intake;
 import org.firstinspires.ftc.teamcode.subsystems.LED;
 import org.firstinspires.ftc.teamcode.subsystems.Shooter;
 import org.firstinspires.ftc.teamcode.subsystems.Transfer;
 import org.firstinspires.ftc.teamcode.subsystems.Turret;
 import org.firstinspires.ftc.teamcode.subsystems.Vision;
-import org.firstinspires.ftc.library.math.geometry.Pose2d;
 import org.firstinspires.ftc.teamcode.utilities.SavedConfiguration;
 
 import java.util.Optional;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopBindings.java

@@ -6,7 +6,6 @@
 import org.firstinspires.ftc.teamcode.command_factories.IntakeFactory;
 import org.firstinspires.ftc.teamcode.command_factories.ShooterFactory;
 import org.firstinspires.ftc.teamcode.command_factories.TransferFactory;
-import org.firstinspires.ftc.teamcode.commands.ConstrainedFlashCommand;
 import org.firstinspires.ftc.teamcode.commands.TeleopMecanum;
 import org.firstinspires.ftc.teamcode.constants.LEDConstants;
 import org.firstinspires.ftc.teamcode.constants.ShooterConstants;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestingOpMode.java

@@ -5,18 +5,14 @@
 import com.bylazar.lights.PanelsLights;
 import com.bylazar.telemetry.PanelsTelemetry;
 import com.bylazar.telemetry.TelemetryManager;
+
 import com.pedropathing.follower.Follower;
 import com.pedropathing.geometry.Pose;
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+
 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.Servo;
 
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.constants.IntakeConstants;
 import org.firstinspires.ftc.teamcode.constants.LEDConstants;
-import org.firstinspires.ftc.teamcode.constants.TransferConstants;
 import org.firstinspires.ftc.teamcode.constants.TurretConstants;
 import org.firstinspires.ftc.teamcode.pedropathing.Constants;
 import org.firstinspires.ftc.teamcode.subsystems.Intake;
@@ -35,8 +31,6 @@ public class TestingOpMode extends OpMode {
 
     public Follower follower;
 
-    public boolean toogle = false;
-
     @IgnoreConfigurable
     private TelemetryManager telemetryManager;
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/IntakeFactory.java

@@ -20,4 +20,16 @@ public static Command openLoopSetpointCommand(Intake intake, DoubleSupplier setp
             intake.setOpenLoopSetpoint(setpoint.getAsDouble());
         }).withName("Intake Open Loop");
     }
+
+    public static Command setFrontIntakeOpenLoopSetpointCommand(Intake intake, DoubleSupplier setpoint) {
+        return Commands.runOnce(() -> {
+            intake.setFrontMotorOpenLoop(setpoint.getAsDouble());
+        }).withName("Intake Open Loop");
+    }
+
+    public static Command setRearIntakeOpenLoopSetpointCommand(Intake intake, DoubleSupplier setpoint) {
+        return Commands.runOnce(() -> {
+            intake.setRearMotorOpenLoop(setpoint.getAsDouble());
+        }).withName("Intake Open Loop");
+    }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/LEDFactory.java

@@ -14,10 +14,22 @@
 import java.util.function.LongSupplier;
 
 public class LEDFactory {
-    public static Command constantColorCommand(LED led, LEDConstants.ColorValue setpoint) {
+    public static Command setConstantColorCommand(LED led, LEDConstants.ColorValue setpoint) {
         return Commands.runOnce(() -> {
             led.setSolid(setpoint);
         });
     }
+
+    public static Command setStandardBlinkingCommand(LED led, LEDConstants.ColorValue setpoint, LongSupplier intervalMs) {
+        return Commands.runOnce(() -> {
+            led.setDefaultSimpleBlink(setpoint, intervalMs.getAsLong());
+        });
+    }
+
+    public static Command setAdvancedStandardBlinkingCommand(LED led, LEDConstants.ColorValue setpointA, LEDConstants.ColorValue setpointB, LongSupplier intervalMs) {
+        return Commands.runOnce(() -> {
+            led.setSimpleBlink(setpointA, setpointB, intervalMs.getAsLong());
+        });
+    }
 }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/IntakeConstants.java

@@ -5,11 +5,15 @@
 @Configurable
 public class IntakeConstants {
     public static final String kSubsystemName = "Intake ";
-    public static final String intakeMotorID = "intakeMotor";
+    public static final String rearIntakeMotorID = "intakeMotor";
+    public static final String frontIntakeMotorID = "frontIntakeMotor";
 
     public static final double intakeMaximumRPM = 435;
     public static final double intakeMotorCPR = 384.5;
 
+    public static final double frontIntakeRatio = (96 / 24.00);
+    public static final double rearIntakeRatio = 1;
+
     public static double kP = 0.001;
     public static double kI = 0.0;
     public static double kD = 0.0;

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

@@ -15,7 +15,8 @@
 import org.firstinspires.ftc.teamcode.constants.IntakeConstants;
 
 public class Intake extends SubsystemBase {
-    private DcMotorEx intakeMotor;
+    private DcMotorEx frontIntakeMotor;
+    private DcMotorEx rearIntakeMotor;
 
     private PIDFController velocityPIDFController;
     private SimpleMotorFeedforward velocityFeedforward;
@@ -24,9 +25,14 @@ public class Intake extends SubsystemBase {
     static TelemetryManager telemetryM;
 
     public Intake(HardwareMap hMap, TelemetryManager telemetryM) {
-        intakeMotor = hMap.get(DcMotorEx.class, IntakeConstants.intakeMotorID);
-        intakeMotor.setDirection(DcMotorSimple.Direction.REVERSE);
-        intakeMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        frontIntakeMotor = hMap.get(DcMotorEx.class, IntakeConstants.frontIntakeMotorID);
+        rearIntakeMotor = hMap.get(DcMotorEx.class, IntakeConstants.rearIntakeMotorID);
+
+        frontIntakeMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+        rearIntakeMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        frontIntakeMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        rearIntakeMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
         velocityPIDFController = new PIDFController(IntakeConstants.kP, IntakeConstants.kI, IntakeConstants.kD, IntakeConstants.kF);
         velocityFeedforward = new SimpleMotorFeedforward(IntakeConstants.kS, IntakeConstants.kV, IntakeConstants.kA);
@@ -36,8 +42,10 @@ public Intake(HardwareMap hMap, TelemetryManager telemetryM) {
 
     @Override
     public void periodic() {
-        telemetryM.addData(IntakeConstants.kSubsystemName + "Current Velocity", getVelocity());
-        telemetryM.addData(IntakeConstants.kSubsystemName + "Current Open Loop", intakeMotor.getPower());
+        telemetryM.addData("Front " + IntakeConstants.kSubsystemName + "Current Velocity", getFrontVelocity());
+        telemetryM.addData("Front " + IntakeConstants.kSubsystemName + "Current Open Loop", frontIntakeMotor.getPower());
+        telemetryM.addData("Rear " + IntakeConstants.kSubsystemName + "Current Velocity", getRearVelocity());
+        telemetryM.addData("Rear " + IntakeConstants.kSubsystemName + "Current Open Loop", rearIntakeMotor.getPower());
     }
 
     public void setVelocitySetpoint(double targetRPM) {
@@ -50,15 +58,38 @@ public void setVelocitySetpoint(double targetRPM) {
             velocityFeedforward.setCoefficient(IntakeConstants.kS, IntakeConstants.kV, IntakeConstants.kA);
         }
 
-        intakeMotor.setPower(velocityPIDFController.calculate(getVelocity(), targetRPM) + velocityFeedforward.calculate(targetRPM));
+        frontIntakeMotor.setPower(velocityPIDFController.calculate(getFrontVelocity(), targetRPM) + velocityFeedforward.calculate(targetRPM));
+        rearIntakeMotor.setPower(velocityPIDFController.calculate(getRearVelocity(), targetRPM) + velocityFeedforward.calculate(targetRPM));
+    }
+
+    public void setFrontMotorOpenLoop(double speed) {
+        telemetryM.addData("Front" + IntakeConstants.kSubsystemName + "Setpoint Open Loop", speed);
+        frontIntakeMotor.setPower(speed);
+    }
+
+    public void setRearMotorOpenLoop(double speed) {
+        telemetryM.addData("Rear" + IntakeConstants.kSubsystemName + "Setpoint Open Loop", speed);
+        rearIntakeMotor.setPower(speed);
     }
 
+
     public void setOpenLoopSetpoint(double speed) {
-        telemetryM.addData(IntakeConstants.kSubsystemName + "Setpoint Open Loop", speed);
-        intakeMotor.setPower(speed);
+        setFrontMotorOpenLoop(speed);
+        setRearMotorOpenLoop(speed);
+    }
+
+    public void setEqualOpenLoopSetpoint(double rearSpeed) {
+        double frontSpeed = rearSpeed;
+
+        setFrontMotorOpenLoop(frontSpeed);
+        setRearMotorOpenLoop(rearSpeed);
+    }
+
+    public double getFrontVelocity() {
+        return ((frontIntakeMotor.getVelocity() / IntakeConstants.intakeMotorCPR) * IntakeConstants.frontIntakeRatio) * 60;
     }
 
-    public double getVelocity() {
-        return (intakeMotor.getVelocity() / IntakeConstants.intakeMotorCPR) * 60;
+    public double getRearVelocity() {
+        return ((rearIntakeMotor.getVelocity() / IntakeConstants.intakeMotorCPR) * IntakeConstants.rearIntakeRatio) * 60;
     }
 }

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

@@ -22,6 +22,9 @@ public class Transfer extends SubsystemBase {
     @IgnoreConfigurable
     static TelemetryManager telemetryM;
 
+    private boolean isBlockerEngaged = true;
+    private boolean isKickerEngaged = false;
+
     public Transfer(HardwareMap hMap, TelemetryManager telemetryM) {
         kickerServo = hMap.get(Servo.class, TransferConstants.kickerServoID);
         blockerServo = hMap.get(Servo.class, TransferConstants.blockerServoID);
@@ -45,8 +48,15 @@ public void periodic() {
     }
 
     public void onInitialization(boolean initKicker, boolean initBlocker) {
-        if(initKicker) kickerServo.setPosition(TransferConstants.kickerIdlePosition);
-        if(initBlocker) blockerServo.setPosition(TransferConstants.blockerIdlePosition);
+        if(initKicker) {
+            kickerServo.setPosition(TransferConstants.kickerIdlePosition);
+            isKickerEngaged = false;
+        }
+
+        if(initBlocker) {
+            blockerServo.setPosition(TransferConstants.blockerIdlePosition);
+            isBlockerEngaged = true;
+        }
     }
 
     public void setKickerPosition(double position) {
@@ -68,4 +78,12 @@ public double firstCSDistance() {
     public double secondCSDistance() {
         return secondColorSensor.getDistance(DistanceUnit.INCH);
     }
+
+    public boolean isBlockerEngaged() {
+        return isBlockerEngaged;
+    }
+
+    public boolean isKickerEngaged() {
+        return isKickerEngaged;
+    }
 }