Merge pull request #14 from Team-4536/drive-improvements

Drive improvements
Team-4536 · Feb 8, 2023 · e9bb3a5 · e9bb3a5
2 parents 3bb3d5b + 0eccadb
commit e9bb3a5
Show file tree

Hide file tree

Showing 8 changed files with 132 additions and 33 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/functions/PIDFunctions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/functions/PIDFunctions.java
@@ -26,6 +26,23 @@ public static float updatePIDAngular(PIDData d, float pos, float dt){
         return out;
     }
 
+    public static float updatePID(PIDData d, float pos, float dt){
+
+        // calculate the error in degrees
+        float error = d.target - pos;
+
+        // rate of change of the error
+        float derivative = (error - d.prevErr) * dt;
+
+        // sum of all error over time
+        d.iSum += error * dt;
+
+        float out = (d.Kp * error) + (d.Ki * d.iSum) + (d.Kd * derivative);
+
+        d.prevErr = error;
+
+        return out;
+    }
 
 
     //                                                                      V these degree symbols are cool

diff --git a/.../src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/ConceptVuforiaNavigation.java b/.../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/...ain/java/org/firstinspires/ftc/teamcode/opModes/autos/ObjectDetectionTeamMadeObjects.java b/...ain/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
@@ -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
@@ -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 {
 
 
@@ -183,7 +184,6 @@ public class autoScoring extends LinearOpMode {
                 float x = pos.get(0);
                 float y = pos.get(1);
 
-                //NTS: run starting w/ the bot facing the wall!
                 V2f rel = new V2f(x - (-1232), y - (-1984));
                 TelemetryData r = new TelemetryData("Relative");
                 r.addChild("x", rel.x);

diff --git a/...de/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/givenExampleCodeOpenCV.java b/...de/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
@@ -0,0 +1,53 @@
+package org.firstinspires.ftc.teamcode.opModes.demos;
+
+import androidx.annotation.NonNull;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.functions.NavFunctions;
+import org.firstinspires.ftc.teamcode.functions.PIDFunctions;
+import org.firstinspires.ftc.teamcode.functions.TelemetryFunctions;
+import org.firstinspires.ftc.teamcode.util.Constants;
+import org.firstinspires.ftc.teamcode.util.Data.ArmData;
+import org.firstinspires.ftc.teamcode.util.Data.DriveData;
+import org.firstinspires.ftc.teamcode.util.Data.NavData;
+import org.firstinspires.ftc.teamcode.util.Data.PIDData;
+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
+    public void runOpMode() {
+
+        TelemetryData telemetry = new TelemetryData("State");
+        NavData nav = new NavData(this.hardwareMap);
+        PIDData armPID = new PIDData(0.01f, 0.0f, 0.0f);
+        @NonNull DcMotor liftMotor = this.hardwareMap.get(DcMotor.class, "lift_motor");
+
+
+        // Constants.initArm(arm, c);
+        TelemetryFunctions.sendTelemetry(this.telemetry, telemetry);
+
+
+        waitForStart();
+        while (opModeIsActive()) {
+
+            NavFunctions.updateDt(nav);
+            armPID.target += this.gamepad1.left_stick_y * 10 * nav.dt;
+
+            float pidOut = PIDFunctions.updatePID(armPID, liftMotor.getCurrentPosition(), (float)nav.dt);
+            liftMotor.setPower(pidOut);
+            telemetry.addChild("Arm encoder", liftMotor.getCurrentPosition());
+            telemetry.addChild("Target pos", armPID.target);
+            telemetry.addChild("pidOut", pidOut);
+
+        }
+    }
+
+}
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
@@ -38,12 +38,14 @@ public void runOpMode() {
         TelemetryData c = new TelemetryData();
         telemetry.addChild(c);
         c.title = "yg;uirguhlsrtguh;sgbrhj;sreguh;srge;ioj";
-        Constants.initArm(arm, c);
+        // Constants.initArm(arm, c);
         TelemetryFunctions.sendTelemetry(this.telemetry, telemetry);
 
 
 
         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);