diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index f07dac4..7319fd6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java new file mode 100644 index 0000000..7d80708 --- /dev/null +++ b/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(); + } + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java index cb5fc64..98faf1d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java +++ b/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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java new file mode 100644 index 0000000..eac72e0 --- /dev/null +++ b/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); + + } + } +} \ No newline at end of file