From bdff6587b0ff4fd59bd2778d899779c72f30913d Mon Sep 17 00:00:00 2001 From: Justin Date: Thu, 11 Jan 2024 20:24:14 -0600 Subject: [PATCH] Gotta love PID --- .../roadrunner/drive/DriveConstants.java | 4 +- .../roadrunner/drive/SampleMecanumDrive.java | 6 +-- .../roadrunner/drive/opmode/Spinning.java | 46 +++++++++++++++++++ 3 files changed, 51 insertions(+), 5 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/Spinning.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java index f3ca168..d0e73d1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java @@ -68,8 +68,8 @@ public class DriveConstants { */ public static double MAX_VEL = 50; public static double MAX_ACCEL = 50; - public static double MAX_ANG_VEL = Math.toRadians(120); - public static double MAX_ANG_ACCEL = Math.toRadians(120); + public static double MAX_ANG_VEL = 15; + public static double MAX_ANG_ACCEL = 8; public static double encoderTicksToInches(double ticks) { return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java index bb3081f..7cf0d43 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java @@ -39,8 +39,8 @@ import java.util.List; @Config public class SampleMecanumDrive extends MecanumDrive { - public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(8, 0, 0); - public static PIDCoefficients HEADING_PID = new PIDCoefficients(8, 0, 0); + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(7, 0, 1); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(6.8, 0, 0); public static double LATERAL_MULTIPLIER = 1; @@ -65,7 +65,7 @@ public class SampleMecanumDrive extends MecanumDrive { super(DriveConstants.kV, DriveConstants.kA, DriveConstants.kStatic, DriveConstants.TRACK_WIDTH, DriveConstants.TRACK_WIDTH, LATERAL_MULTIPLIER); follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID, - new Pose2d(0.5, 0.5, Math.toRadians(5)), 0.5); + new Pose2d(0.5, 0.5, Math.toRadians(1)), 0.5); LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/Spinning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/Spinning.java new file mode 100644 index 0000000..86ee087 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/Spinning.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; + +/* + * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base + * classes). The robot drives back and forth in a straight line indefinitely. Utilization of the + * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer + * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're + * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once + * you've successfully connected, start the program, and your robot will begin moving forward and + * backward. You should observe the target position (green) and your pose estimate (blue) and adjust + * your follower PID coefficients such that you follow the target position as accurately as possible. + * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID. + * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID. + * These coefficients can be tuned live in dashboard. + * + * This opmode is designed as a convenient, coarse tuning for the follower PID coefficients. It + * is recommended that you use the FollowerPIDTuner opmode for further fine tuning. + */ +//@Config +@Autonomous(group = "drive") +public class Spinning extends LinearOpMode { + + public static double ANGLE = 50; + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + waitForStart(); + + while (opModeIsActive() && !isStopRequested()) { + drive.turn(Math.toRadians(90)); + sleep(1000); + drive.turn(Math.toRadians(-90)); + sleep(1000); + + } + } +} \ No newline at end of file