teleopv4, added drivetrain center of mass correction and removed unused brake feature

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java

@@ -1,7 +1,6 @@
 package org.firstinspires.ftc.teamcode.teleop;
 
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
 import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH;
 import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX;
 import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY;
@@ -13,7 +12,6 @@ import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.acmerobotics.roadrunner.Pose2d;
 import com.arcrobotics.ftclib.controller.PIDFController;
 import com.pedropathing.follower.Follower;
 import com.pedropathing.geometry.Pose;
@@ -22,9 +20,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.teamcode.constants.Color;
-import org.firstinspires.ftc.teamcode.constants.ServoPositions;
 import org.firstinspires.ftc.teamcode.constants.StateEnums;
-import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
 import org.firstinspires.ftc.teamcode.utils.Drivetrain;
 import org.firstinspires.ftc.teamcode.utils.Flywheel;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java

@@ -0,0 +1,49 @@
+package org.firstinspires.ftc.teamcode.teleop;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.utilsv2.Drivetrain;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+@TeleOp
+@Config
+public class TeleopV4 extends LinearOpMode {
+    Robot robot;
+    Drivetrain drivetrain;
+    MultipleTelemetry TELE;
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        robot = new Robot(hardwareMap);
+
+        TELE = new MultipleTelemetry(
+                FtcDashboard.getInstance().getTelemetry(), telemetry
+        );
+
+        drivetrain = new Drivetrain(robot, TELE);
+
+        drivetrain.setTelemetry(true);
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()){
+
+            //Drivetrain
+
+            drivetrain.drive(
+                    -gamepad1.right_stick_y,
+                    gamepad1.right_stick_x,
+                    gamepad1.left_stick_x
+            );
+
+            TELE.update();
+        }
+
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java

@@ -22,7 +22,7 @@ public class Drivetrain {
     private final ProfileAccelConstraint ACCEL_CONSTRAINT = new ProfileAccelConstraint(-Math.abs(60), 200);
 
 
-    public Drivetrain (Robot rob, Follower follower){
+    public Drivetrain(Robot rob, Follower follower){
 
         this.robot = rob;
         this.follower = follower;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java

@@ -0,0 +1,98 @@
+package org.firstinspires.ftc.teamcode.utilsv2;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+@Config
+public class Drivetrain {
+
+    Robot robot;
+
+    MultipleTelemetry telemetry;
+
+    private static final double DEADZONE = 0.15;
+    private static final double AXIS_SNAP_THRESHOLD = 0.12;
+
+    private static final double STRAFE_MULTIPLIER = 1.2;
+
+    public static double FORWARD_ROTATION_CORRECTION = 0.03;
+    public static double STRAFE_ROTATION_CORRECTION = -0.03;
+
+    private boolean tele = false;
+
+    public Drivetrain(Robot rob, MultipleTelemetry TELE) {
+
+        this.robot = rob;
+
+        robot.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        robot.frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        robot.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        robot.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+
+        this.telemetry = TELE;
+    }
+
+    public void setTelemetry(boolean input) {
+        tele = input;
+    }
+
+    public void drive(double y, double x, double rx) {
+
+        boolean snappedForward = false;
+        boolean snappedStrafe = false;
+
+        if (Math.abs(y) < DEADZONE) y = 0;
+        if (Math.abs(x) < DEADZONE) x = 0;
+        if (Math.abs(rx) < DEADZONE) rx = 0;
+
+        if (Math.abs(x) < AXIS_SNAP_THRESHOLD) {
+            x = 0;
+            snappedForward = true;
+        }
+
+        if (Math.abs(y) < AXIS_SNAP_THRESHOLD) {
+            y = 0;
+            snappedStrafe = true;
+        }
+
+        x *= STRAFE_MULTIPLIER;
+
+        double correctionRX = 0;
+
+        if (rx == 0) {
+            correctionRX += (y * FORWARD_ROTATION_CORRECTION);
+            correctionRX += (x * STRAFE_ROTATION_CORRECTION);
+
+            rx += correctionRX;
+        }
+
+        double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
+
+        double frontLeftPower = (y + x + rx) / denominator;
+        double backLeftPower = (y - x + rx) / denominator;
+        double frontRightPower = (y - x - rx) / denominator;
+        double backRightPower = (y + x - rx) / denominator;
+
+        robot.frontLeft.setPower(frontLeftPower);
+        robot.backLeft.setPower(backLeftPower);
+        robot.frontRight.setPower(frontRightPower);
+        robot.backRight.setPower(backRightPower);
+
+        if (tele) {
+
+            telemetry.addData("Forward Snap", snappedForward);
+            telemetry.addData("Strafe Snap", snappedStrafe);
+
+            telemetry.addData("Correction RX", correctionRX);
+
+            telemetry.addData("FL", frontLeftPower);
+            telemetry.addData("BL", backLeftPower);
+            telemetry.addData("FR", frontRightPower);
+            telemetry.addData("BR", backRightPower);
+
+        }
+    }
+}