Merge branch 'master' into vision

ftc16072 · Nov 12, 2024 · fab4f8e · fab4f8e
2 parents c8fe692 + 1560192
commit fab4f8e
Show file tree

Hide file tree

Showing 13 changed files with 283 additions and 111 deletions.
diff --git a/...ava/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator.java b/...ava/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();

diff --git a/...e/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/DoubleReverse4Bar.java b/...e/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/DoubleReverse4Bar.java
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeSlides.java b/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);
+    }
+
+
+}
+
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java
@@ -0,0 +1,105 @@
+package org.firstinspires.ftc.teamcode.ftc16072.Mechanisms;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.DigitalChannel;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.TouchSensor;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.ftc16072.Tests.QQTest;
+import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestServo;
+import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestSwitch;
+import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestTwoMotors;
+import org.firstinspires.ftc.teamcode.ftc16072.Util.PIDFController;
+
+import java.util.Arrays;
+import java.util.List;
+
+public class ScoreArm extends QQMechanism{
+    public static final double TEST_SPEED = 0.55;
+    public static double TEST_SPEED_OFF = 0.4;
+    DcMotor leftMotor;
+    DcMotor rightMotor;
+    TouchSensor limitSwitch;
+    public static double kP = 0.01;
+    public static double kI = 0.0;
+    public static double kD = 0.0;
+    public static double kF = 0;
+    public static double max =  0.8;
+    public static double min = -0.8;
+
+    public int currentPos;
+    public int desiredPos;
+    public double motorPower;
+
+    static int INTAKE_POSITION = 0;
+    static int SCORING_POSITION = 350;
+    static int PLACING_POSITION = 750;
+
+    public Telemetry telemetry;
+
+
+    PIDFController pidfController = new PIDFController(kP,kI,kD,kF,max,min);
+
+    @Override
+    public void init(HardwareMap hwMap) {
+        leftMotor = hwMap.get(DcMotor.class, "left_score_arm_motor");
+        rightMotor = hwMap.get(DcMotor.class, "right_score_arm_motor");
+        limitSwitch = hwMap.get(TouchSensor.class, "score_arm_switch");
+        rightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+        leftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        rightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        leftMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        rightMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        leftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        rightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+    }
+
+    public void goToIntake(){
+        desiredPos = INTAKE_POSITION;
+    }
+    public void goToScoring(){
+        desiredPos = SCORING_POSITION;
+    }
+    public void goToPlace(){
+        desiredPos = PLACING_POSITION;
+    }
+    public void manualPositionChange(double changeAmount){
+        desiredPos += changeAmount;
+    }
+
+
+
+
+    @Override
+    public void update(){
+        if(limitSwitch.isPressed()) {
+            leftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+            leftMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+            rightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+            rightMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+            if (desiredPos < 0){
+                desiredPos = 0;
+            }
+        }
+        currentPos = (leftMotor.getCurrentPosition() + rightMotor.getCurrentPosition())/2;//average left and right speeds
+        double motorPower = pidfController.calculate(desiredPos,currentPos);
+        this.motorPower = motorPower;
+        leftMotor.setPower(motorPower);
+        rightMotor.setPower(motorPower);
+        telemetry.addData("curerent pos",currentPos);
+        telemetry.addData("desired pos",desiredPos);
+        telemetry.addData("motor power",motorPower);
+
+    }
+
+    @Override
+    public List<QQTest> getTests() {
+        return Arrays.asList(
+                new TestTwoMotors("score arm up", TEST_SPEED,leftMotor,rightMotor),
+                new TestTwoMotors("score arm down", -TEST_SPEED, leftMotor,rightMotor),
+                new TestSwitch("Arm limit switch", limitSwitch)
+        );
+    }
+}
diff --git a/...Code/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/BlueCloseParkAuto.java b/...Code/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/BlueCloseParkAuto.java
@@ -19,7 +19,8 @@ public void loop(){
                 step = 2;
             }if(step == 2){
                 telemetry.addData("parked", 0);
-                robot.arm.goToGround();
+                robot.scoreArm.goToIntake();
+
             }
         }
     }

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/BlueFarParkAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/BlueFarParkAuto.java
@@ -19,7 +19,7 @@ public void loop(){
                 step = 2;
             }if(step == 2){
                 telemetry.addData("parked", 0);
-                robot.arm.goToGround();
+                robot.scoreArm.goToIntake();
             }
         }
     }

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/CloseParkAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/CloseParkAuto.java
@@ -32,7 +32,7 @@ else if(step == 1){
             if(doneDriving){
                 step = 3;
             }if(step == 3){
-                robot.arm.goToGround();
+                robot.scoreArm.goToIntake();
             }
         }
     }

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/FarParkAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/FarParkAuto.java
@@ -31,7 +31,7 @@ else if(step == 1){
             if(doneDriving){
                 step = 3;
             }if(step == 3){
-                robot.arm.goToGround();
+                robot.scoreArm.goToIntake();
             }
         }
     }