From 3dcbb50c539d4d0a94a5a36633e0ab3a476b20af Mon Sep 17 00:00:00 2001 From: Robert Teal Date: Mon, 1 Jan 2024 19:07:48 -0600 Subject: [PATCH] tweaked auto and slides --- .../ftc/teamcode/hardware/Robot.java | 1 - .../ftc/teamcode/hardware/Slides.java | 18 ++++--- .../opmode/autonomous/RedBackStageAuto.java | 49 +++++++++++++++---- 3 files changed, 50 insertions(+), 18 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java index 2c68684..f9db286 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java @@ -75,7 +75,6 @@ public class Robot { case(0): macroStartTime = runTime; arm.setDoor(OPEN); - slides.setTarget(slides.getTarget()+70); macroState++; break; case(1): diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java index 5ce0598..3a56e08 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java @@ -5,18 +5,20 @@ import com.arcrobotics.ftclib.controller.PIDController; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; @Config public class Slides { private DcMotor slide; private DcMotor slide2; - public static double p = 0.0014; - public static double i = 0.02; - public static double d = 0; - public static double f = 0.01; +// public static double p = 0.0014; +// public static double i = 0.02; +// public static double d = 0; +// public static double f = 0.01; + public static PIDFCoefficients coefficients = new PIDFCoefficients(0.0014,0.02,0,0.01); public static double pTolerance = 20; - public static PIDController controller = new PIDController(p, i, d); + private PIDController controller = new PIDController(coefficients.p, coefficients.i, coefficients.d); public static int targetMin = -10; public static int targetMax = 830; @@ -102,15 +104,15 @@ public class Slides { // slide2.setPower(0); // } else { double pid, ff; - controller.setPID(p, i, d); + controller.setPID(coefficients.p, coefficients.i, coefficients.d); controller.setTolerance(pTolerance); pid = controller.calculate(slide.getCurrentPosition(), target); - ff = f; + ff = coefficients.f; slide.setPower(pid + ff); pid = controller.calculate(slide2.getCurrentPosition(), target); - ff = f; + ff = coefficients.f; slide2.setPower(pid + ff); // } // } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java index a454644..546b006 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.opmode.autonomous; import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; import com.acmerobotics.roadrunner.trajectory.Trajectory; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -20,6 +21,10 @@ public class RedBackStageAuto extends AutoBase { public Trajectory parkRobot2; public Trajectory parkRobot3; + public Trajectory stackrun1b1; + public Trajectory stackrun1b2; + public Trajectory stackrun1b3; + @Override public void createTrajectories() { // set start position @@ -41,6 +46,10 @@ public class RedBackStageAuto extends AutoBase { Pose2d park2 = new Pose2d(48, -12, Math.toRadians(180)); Pose2d park3 = new Pose2d(48, -12, Math.toRadians(180)); + Pose2d stack_1x1 = new Pose2d(-56, -12, Math.toRadians(180)); + Pose2d stack_2x1 = new Pose2d(-56, -12, Math.toRadians(180)); + Pose2d stack_3x1 = new Pose2d(-56, -12, Math.toRadians(180)); + // create trajectories scorePurple1 = robot.drive.trajectoryBuilder(start) .lineToLinearHeading(drop1) @@ -71,6 +80,19 @@ public class RedBackStageAuto extends AutoBase { parkRobot3 = robot.drive.trajectoryBuilder(scoreYellow3.end()) .lineToLinearHeading(park3) .build(); + + stackrun1b1 = robot.drive.trajectoryBuilder(scoreYellow1.end()) + .splineToConstantHeading(new Vector2d(30, -12), Math.toRadians(0)) + .lineToLinearHeading(stack_1x1) + .build(); + stackrun1b2 = robot.drive.trajectoryBuilder(scoreYellow2.end()) + .splineToConstantHeading(new Vector2d(30, -12), Math.toRadians(0)) + .lineToLinearHeading(stack_2x1) + .build(); + stackrun1b3 = robot.drive.trajectoryBuilder(scoreYellow1.end()) + .splineToConstantHeading(new Vector2d(30, -12), Math.toRadians(0)) + .lineToLinearHeading(stack_1x1) + .build(); } @Override @@ -117,21 +139,30 @@ public class RedBackStageAuto extends AutoBase { case 4: robot.resetMacro(0, getRuntime()); if (robot.macroState >= 2){ - robot.drive.followTrajectoryAsync(teamPropLocation==1?parkRobot1:(teamPropLocation==2?parkRobot2:parkRobot3)); + // robot.drive.followTrajectoryAsync(teamPropLocation==1?parkRobot1:(teamPropLocation==2?parkRobot2:parkRobot3)); + robot.drive.followTrajectoryAsync(stackrun1b2); macroState++; } break; - // PARK ROBOT + case 5: - // reset macro' - if (robot.macroState != 0) { - robot.resetMacro(0, getRuntime()); - } - // if macro and drive are done, end auto - if (robot.macroState == 0 && !robot.drive.isBusy()) { - macroState=-1; + if(!robot.drive.isBusy()){ + macroState =-1; } + + //macroState++; break; + // PARK ROBOT +// case 6: +// // reset macro' +// if (robot.macroState != 0) { +// robot.resetMacro(0, getRuntime()); +// } +// // if macro and drive are done, end auto +// if (robot.macroState == 0 && !robot.drive.isBusy()) { +// macroState=-1; +// } +// break; } } } \ No newline at end of file