Merge pull request #23 from ftc16072/intake_slides_code

Intake slides code

FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator.java

@@ -110,7 +110,7 @@ public void runOpMode()
         ColorBlobLocatorProcessor colorLocator = new ColorBlobLocatorProcessor.Builder()
                 .setTargetColorRange(ColorRange.BLUE)         // use a predefined color match
                 .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY)    // exclude blobs inside blobs
-                .setRoi(ImageRegion.asUnityCenterCoordinates(-0.5, 0.5, 0.5, -0.5))  // search central 1/4 of camera view
+                .setRoi(ImageRegion.asUnityCenterCoordinates(-0.3, 0.3, 0.3, -0.3))  // search central 1/4 of camera view
                 .setDrawContours(true)                        // Show contours on the Stream Preview
                 .setBlurSize(5)                               // Smooth the transitions between different colors in image
                 .build();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeSlides.java

@@ -0,0 +1,111 @@
+package org.firstinspires.ftc.teamcode.ftc16072.Mechanisms;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.ftc16072.Tests.QQTest;
+
+
+import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestTwoMotors;
+import org.firstinspires.ftc.teamcode.ftc16072.Util.PIDFController;
+
+import java.util.Arrays;
+import java.util.List;
+
+
+@Config
+public class IntakeSlides extends QQMechanism {
+    //HAVE TO CHANGE ALL VALUES LATER
+    public static final double TEST_SPEED = 0.55;
+    private static final int SlIDES_POSITION_SAFETY_BACK = -50;
+    private static final int FULL_EXTENSION_POSITION = 1480;
+    public static final int SLIDES_EXTENSION_BOUNDARY = FULL_EXTENSION_POSITION+10;
+    private static final int HALF_EXTENSION_POSITION = 740;
+    private static final int START_POSITION = 0;
+
+    private DcMotor rightIntakeSlideMotor;
+    private DcMotor leftIntakeSlideMotor;
+
+
+    public int currentPos;
+    public int desiredPos;
+    public double motorPower;
+
+    public static double kP = 0.01;
+    public static double kI = 0.0;
+    public static double kD = 0.0;
+    public static double kF = 0.4;
+    public static double max =  0.8;
+    public static double min = -max;
+    public Telemetry telemetry;
+
+    PIDFController pidfController = new PIDFController(kP,kI,kD,kF,max,min);
+
+
+    @Override
+    public void init(HardwareMap hwMap) {
+
+        rightIntakeSlideMotor = hwMap.get(DcMotor.class, "right_intake_slide_motor");
+        rightIntakeSlideMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+        rightIntakeSlideMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        rightIntakeSlideMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        rightIntakeSlideMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+        leftIntakeSlideMotor = hwMap.get(DcMotor.class, "left_intake_slide_motor");
+        leftIntakeSlideMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        leftIntakeSlideMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        leftIntakeSlideMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+    }
+    public void setPosition(int desiredPos){
+        this.desiredPos = desiredPos;
+    }
+    public void manualPositionChange(int changeAmount){
+        desiredPos += changeAmount;
+    }
+    @Override
+    public void update(){
+        currentPos = (leftIntakeSlideMotor.getCurrentPosition() + rightIntakeSlideMotor.getCurrentPosition())/2;//average left and right speeds
+        double motorPower = pidfController.calculate(desiredPos,currentPos);
+        this.motorPower = motorPower;
+        leftIntakeSlideMotor.setPower(motorPower);
+        rightIntakeSlideMotor.setPower(motorPower);
+        telemetry.addData("Current", currentPos);
+        telemetry.addData("Desired", desiredPos);
+        telemetry.addData("Motor Power", motorPower);
+
+        if(currentPos <= SlIDES_POSITION_SAFETY_BACK){
+            setPosition(SlIDES_POSITION_SAFETY_BACK);
+        }
+
+        if(currentPos > SLIDES_EXTENSION_BOUNDARY){
+            setPosition(SLIDES_EXTENSION_BOUNDARY);
+        }
+    }
+
+
+
+    @Override
+    public List<QQTest> getTests() {
+        return Arrays.asList(
+                new TestTwoMotors("slides out", TEST_SPEED,leftIntakeSlideMotor,rightIntakeSlideMotor),
+                new TestTwoMotors("slides in", -TEST_SPEED,leftIntakeSlideMotor,rightIntakeSlideMotor)
+        );
+    }
+    public void fullExtension(){
+        setPosition(FULL_EXTENSION_POSITION);
+    }
+    public void halfExtension(){
+        setPosition(HALF_EXTENSION_POSITION);
+    }
+    public void startPosition(){
+        setPosition(START_POSITION);
+    }
+
+
+}
+
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java

@@ -14,6 +14,7 @@ public class NoTurnTeleop extends QQOpMode{
     public void init(){
         isPlacing = false;
         super.init();
+        robot.intakeSlides.telemetry = telemetry;
         robot.scoreArm.telemetry = telemetry;
     }
     public void loop(){
@@ -38,14 +39,16 @@ public void loop(){
             robot.claw.wristStart();
         }else if (gamepad1.dpad_left) {
             robot.claw.wristEnd();
-        }else if (gamepad1.dpad_up){
+        }else if (gamepad1.dpad_down){
             robot.scoreArm.manualPositionChange(MANUAL_CHANGE);
         }else if (gamepad1.dpad_down){
             robot.scoreArm.manualPositionChange(-MANUAL_CHANGE);
         }
         if (gamepad1.y && gamepad1.dpad_right){
+
             robot.controlHub.resetGyro();
         }
+
         if (gamepad1.right_trigger > TRIGGER_THRESHOLD) {
             robot.claw.close();
         }else if (gamepad1.right_bumper){
@@ -63,5 +66,20 @@ public void loop(){
         }else{
             nav.driveFieldRelative(forward, left, rotate);
         }
+        if(gamepad2.x){
+            robot.intakeSlides.fullExtension();
+        }
+        if(gamepad2.y){
+            robot.intakeSlides.halfExtension();
+        }
+        if(gamepad2.a){
+            robot.intakeSlides.startPosition();
+        }
+        if(gamepad2.dpad_up){
+            robot.intakeSlides.manualPositionChange(5);
+        }
+        if(gamepad2.dpad_down){
+            robot.intakeSlides.manualPositionChange(-5);
+        }
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java

@@ -4,6 +4,7 @@
 
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Claw;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.ControlHub;
+import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.IntakeSlides;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.MecanumDrive;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.OpticalTrackingOdometrySensor;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.QQMechanism;
@@ -20,14 +21,20 @@ public class Robot {
     public Claw claw;
     //public DoubleReverse4Bar doubleReverse4Bar;
    // public Slides slides;
+
+
+    public IntakeSlides intakeSlides;
+
     public ScoreArm scoreArm;
+
     List<QQMechanism> mechanisms;
 
     public Robot() {
         mecanumDrive = new MecanumDrive();
         controlHub = new ControlHub();
         otos = new OpticalTrackingOdometrySensor();
         claw = new Claw();
+        intakeSlides = new IntakeSlides();
         //doubleReverse4Bar = new DoubleReverse4Bar();
         //slides = new Slides();
         scoreArm = new ScoreArm();
@@ -38,8 +45,10 @@ public Robot() {
                 // otos,
                 claw,
                // slides,
-                claw,
+                //doubleReverse4Bar,
+                intakeSlides,
                 scoreArm);
+
     }
     public void init(HardwareMap hwMap) {
         for (QQMechanism mechanism : mechanisms) {