diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java index b7ead0d..5589efb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java @@ -68,10 +68,10 @@ public class DriveConstants { * small and gradually increase them later after everything is working. All distance units are * inches. */ - public static double MAX_VEL = 50; - public static double MAX_ACCEL = 50; - public static double MAX_ANG_VEL = Math.toRadians(60); - public static double MAX_ANG_ACCEL = Math.toRadians(60); + public static double MAX_VEL = 80; + public static double MAX_ACCEL = 80; + public static double MAX_ANG_VEL = Math.toRadians(360); + public static double MAX_ANG_ACCEL = Math.toRadians(660); /* * Adjust the orientations here to match your robot. See the FTC SDK documentation for details. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java index 3d43da9..e750a9d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java @@ -105,10 +105,13 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr DriveConstants.LOGO_FACING_DIR, DriveConstants.USB_FACING_DIR)); imu.initialize(parameters); + leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + + this.leftFront.setDirection(DcMotor.Direction.REVERSE); this.rightFront.setDirection(DcMotor.Direction.FORWARD); this.leftRear.setDirection(DcMotor.Direction.REVERSE); @@ -329,8 +332,8 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr this.setWeightedDrivePower( new Pose2d( - gamepad1.left_stick_y * -1 * speedScale, - gamepad1.left_stick_x * -1 * speedScale, + gamepad1.left_stick_y * speedScale, + gamepad1.left_stick_x * speedScale, -gamepad1.right_stick_x * turnScale )); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TurnTest.java index 56ff661..2874259 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TurnTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TurnTest.java @@ -1,8 +1,10 @@ package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.opmode; import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; +import com.arcrobotics.ftclib.gamepad.GamepadEx; +import com.arcrobotics.ftclib.gamepad.GamepadKeys; 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.teamcode.hardware.roadrunner.drive.MecanumDrive; @@ -11,10 +13,10 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; * This is a simple routine to test turning capabilities. */ @Config -@Disabled @Autonomous(group = "drive") public class TurnTest extends LinearOpMode { public static double ANGLE = 90; // deg + public static double DISTANCE = 24; @Override public void runOpMode() throws InterruptedException { @@ -24,6 +26,25 @@ public class TurnTest extends LinearOpMode { if (isStopRequested()) return; - drive.turn(Math.toRadians(ANGLE)); + GamepadEx controller = new GamepadEx(this.gamepad1); + boolean zig = false; + while (opModeIsActive() && !isStopRequested()) { + controller.readButtons(); + if (controller.wasJustPressed(GamepadKeys.Button.A)) { + drive.turn(Math.toRadians(ANGLE)); + } + + if (controller.wasJustPressed(GamepadKeys.Button.B)) { + TrajectoryBuilder builder = drive.trajectoryBuilder(drive.getPoseEstimate()); + if (zig) { + zig = false; + builder.forward(DISTANCE); + } else { + zig = true; + builder.forward(-DISTANCE); + } + drive.followTrajectory(builder.build()); + } + } } }