diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 6c4bc55..f5203a2 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -22,6 +22,6 @@ android { dependencies { implementation project(':FtcRobotController') annotationProcessor files('lib/OpModeAnnotationProcessor.jar') - implementation 'org.openftc:easyopencv:1.5.3' + implementation 'org.openftc:easyopencv:1.5.2' implementation 'org.openftc:apriltag:1.1.0' } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/ConceptVuforiaNavigation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/ConceptVuforiaNavigation.java index f263c2d..d0b4652 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/ConceptVuforiaNavigation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/ConceptVuforiaNavigation.java @@ -5,6 +5,7 @@ import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XZY; import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -42,6 +43,7 @@ import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XZY; import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC; +@Disabled public class ConceptVuforiaNavigation { /* Copyright (c) 2019 FIRST. All rights reserved. * diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/ObjectDetectionTeamMadeObjects.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/ObjectDetectionTeamMadeObjects.java index 3988828..3f9638f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/ObjectDetectionTeamMadeObjects.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/ObjectDetectionTeamMadeObjects.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.opModes.autos; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.robotcore.external.tfod.Recognition; @@ -20,6 +21,7 @@ import java.util.List; +@Disabled @com.qualcomm.robotcore.eventloop.opmode.Autonomous(name="MechAuto CV Parking Custom", group="Autos") public class ObjectDetectionTeamMadeObjects extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/OpenCVWithImages.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/OpenCVWithImages.java index 3a4287e..9e36219 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/OpenCVWithImages.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/OpenCVWithImages.java @@ -2,6 +2,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; @@ -30,6 +31,7 @@ @Autonomous(name="OpenCV and Feild", group="Autos") +@Disabled public class OpenCVWithImages extends LinearOpMode { OpenCvCamera camera; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/autoScoring.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/autoScoring.java index 86e02cd..fb6049f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/autoScoring.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/autoScoring.java @@ -6,6 +6,7 @@ import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XZY; import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -33,7 +34,7 @@ import java.util.ArrayList; import java.util.List; -@TeleOp(name="XAutoScoring", group ="Autos") +@Autonomous(name="XAutoScoring") public class autoScoring extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/givenExampleCodeOpenCV.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/givenExampleCodeOpenCV.java index 2a68800..bc482f1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/givenExampleCodeOpenCV.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/givenExampleCodeOpenCV.java @@ -22,6 +22,7 @@ package org.firstinspires.ftc.teamcode.opModes.autos; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -36,6 +37,7 @@ import java.util.ArrayList; +@Disabled @com.qualcomm.robotcore.eventloop.opmode.Autonomous(name="Given Open CV", group="Autos") public class givenExampleCodeOpenCV extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/demos/PIDArmDemo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/demos/PIDArmDemo.java index d5dee77..d28488b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/demos/PIDArmDemo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/demos/PIDArmDemo.java @@ -2,6 +2,7 @@ import androidx.annotation.NonNull; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; @@ -16,7 +17,9 @@ import org.firstinspires.ftc.teamcode.util.Data.TelemetryData; + @com.qualcomm.robotcore.eventloop.opmode.TeleOp(name="Arm PID Demo", group="TeleOps") +@Disabled public class PIDArmDemo extends LinearOpMode { @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/teleOp/XTeleOpAbsolute.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/teleOp/XTeleOpAbsolute.java index 0c403ee..4731aed 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/teleOp/XTeleOpAbsolute.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/teleOp/XTeleOpAbsolute.java @@ -44,6 +44,8 @@ public void runOpMode() { waitForStart(); + NavFunctions.updateHeading(nav); + drivePID.target = nav.heading; while(opModeIsActive()){ @@ -59,50 +61,71 @@ public void runOpMode() { NavFunctions.updateDt(nav); NavFunctions.updateHeading(nav); - - TelemetryData opModeTelemetry = new TelemetryData("OpMode"); telemetry.addChild(opModeTelemetry); - //change target angle with input - if(r.length() >= Constants.TURN_CUTOFF){ - float mod = Constants.defaultXTurnSpeed - + ((Constants.maxXTurnSpeed-Constants.defaultXTurnSpeed)*this.gamepad1.left_trigger); - drivePID.target = PIDFunctions.angleWrap(drivePID.target - (r.x * mod)); + + + + + + float PIDOut = 0; + { + //change target angle with input + if (r.length() >= Constants.TURN_CUTOFF) { + if (!gamepad1.left_bumper) { + + float mod = Constants.defaultXTurnSpeed + + ((Constants.maxXTurnSpeed - Constants.defaultXTurnSpeed) * this.gamepad1.left_trigger); + drivePID.target = PIDFunctions.angleWrap(drivePID.target - (r.x * mod)); + } else { + + drivePID.target = PIDFunctions.angleWrap(r.getAngleDeg() - 90); + } + + + } + PIDOut = PIDFunctions.updatePIDAngular( + drivePID, + nav.heading, + (float) nav.dt); + + opModeTelemetry.addChild(new TelemetryData("Angle", nav.heading)); + opModeTelemetry.addChild(new TelemetryData("PID power", PIDOut)); + opModeTelemetry.addChild(new TelemetryData("Target Angle", drivePID.target)); } - opModeTelemetry.addChild(new TelemetryData("Target Angle", drivePID.target)); - float PIDOut = PIDFunctions.updatePIDAngular( - drivePID, - nav.heading, - (float)nav.dt); - opModeTelemetry.addChild(new TelemetryData("Angle", nav.heading)); - opModeTelemetry.addChild(new TelemetryData("PID power", PIDOut)); + { + V2f driveCmd = l; + V2f dpad = new V2f( + (gamepad1.dpad_right?1:0) - (gamepad1.dpad_left?1:0), + (gamepad1.dpad_up?1:0) - (gamepad1.dpad_down?1:0) + ); + if(dpad.length() > 0) { + driveCmd = dpad; + } - //multiplier for drive speed. - float mod = Constants.defaultXDriveSpeed - + ((1-Constants.defaultXDriveSpeed)*this.gamepad1.right_trigger); - //float mod = 1.0f; - telemetry.addChild("Percent", mod); - V2f in = new V2f( - l.x * mod, - l.y * mod); - in = in.rotated(-nav.heading); - opModeTelemetry.addChild(new TelemetryData("Drive", in)); + //multiplier for drive speed. + float mod = Constants.defaultXDriveSpeed + + ((1-Constants.defaultXDriveSpeed)*this.gamepad1.right_trigger); - DriveFunctions.setPower( - drive, - in, - PIDOut); + V2f in = new V2f( driveCmd.x * mod, driveCmd.y * mod); + in = in.rotated(-nav.heading); + + DriveFunctions.setPower( + drive, + in, + PIDOut); + } @@ -129,8 +152,6 @@ public void runOpMode() { - - arm.liftMotor.setPower(liftSpeed); telemetry.addChild("Lift pos", pos); telemetry.addChild("Lift speed", liftSpeed);