diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/odo/DriveForwardPID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/odo/DriveForwardPID.java index b8449424b83a..31657523a3f7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/odo/DriveForwardPID.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/odo/DriveForwardPID.java @@ -14,6 +14,13 @@ public class DriveForwardPID { public static final double MIN_SPEED_INITIAL = 0.3; // was 0.25 public static final double MIN_SPEED_FINAL = 0.25; // was 0.15 public static final double acceptableError = .50; // in + + public static final double MAX_SPEED_D = 0.3; // drive forward/reverse + public static final double RAMPS_UP_D = 6; // drive forward/reverse + public static final double RAMPS_DOWN_D = 8; // drive forward/reverse + public static final double MIN_SPEED_INITIAL_D = 0.25; // drive forward/reverse + public static final double MIN_SPEED_FINAL_D = 0.15; // drive forward/reverse + public static final double acceptableError_D = 1.0; // drive forward/reverse private final DcMotor paraRight; private final DcMotor paraLeft; private final DcMotor perp; @@ -25,12 +32,22 @@ public static double rampDown(double distToTarget) { else return (MAX_SPEED - MIN_SPEED_FINAL) * (distToTarget / RAMPS_DOWN) + MIN_SPEED_FINAL; } + public static double rampDownForward(double distToTarget) { + if (distToTarget <= 0) return 0.0; + if (distToTarget >= RAMPS_DOWN_D) return MAX_SPEED_D; + else return (MAX_SPEED_D - MIN_SPEED_FINAL_D) * (distToTarget / RAMPS_DOWN_D) + MIN_SPEED_FINAL_D; + } // TODO: Migrate to Kotlin impl public static double rampUp(double distTravel) { if (distTravel <= 0) return MIN_SPEED_INITIAL; if (distTravel >= RAMPS_UP) return MAX_SPEED; return (MAX_SPEED - MIN_SPEED_INITIAL) * (distTravel / RAMPS_UP) + MIN_SPEED_INITIAL; } + public static double rampUpForward(double distTravel) { + if (distTravel <= 0) return MIN_SPEED_INITIAL_D; + if (distTravel >= RAMPS_UP_D) return MAX_SPEED_D; + return (MAX_SPEED_D - MIN_SPEED_INITIAL_D) * (distTravel / RAMPS_UP_D) + MIN_SPEED_INITIAL_D; + } public DriveForwardPID(RobotConfiguration robot) { this.robot = robot; @@ -88,7 +105,7 @@ public void DriveForward(double target, Telemetry telemetry, double timeout) { double ldist = LeftOdoDist() - lbase; double avg = (rdist + ldist) / 2.0; - if (avg > target - acceptableError) break; + if (avg > target - acceptableError_D) break; double error = rdist - ldist; double currentTime = stopwatch.time(); @@ -97,7 +114,7 @@ public void DriveForward(double target, Telemetry telemetry, double timeout) { previousTime = currentTime; double correction = kp * error + ki * overall_error; - double speed = Math.min(rampUp(avg), rampDown(target - avg)); + double speed = Math.min(rampUpForward(avg), rampDownForward(target - avg)); driveMotors.frontLeft.setPower(speed + correction); driveMotors.backLeft.setPower(speed + correction); driveMotors.frontRight.setPower(speed - correction); @@ -129,7 +146,7 @@ public void DriveReverse(double target, Telemetry telemetry, double timeout) { double ldist = lbase - LeftOdoDist(); double avg = (rdist + ldist) / 2.0; - if (avg > target - acceptableError) break; + if (avg > target - acceptableError_D) break; double error = rdist - ldist; @@ -138,15 +155,18 @@ public void DriveReverse(double target, Telemetry telemetry, double timeout) { overall_error += error * dt; double correction = kp * error + ki * overall_error; - double speed = -Math.min(rampUp(avg), rampDown(target - avg)); + double speed = -Math.min(rampUpForward(avg), rampDownForward(target - avg)); driveMotors.frontLeft.setPower(speed - correction); driveMotors.backLeft.setPower(speed - correction); driveMotors.frontRight.setPower(speed + correction); driveMotors.backRight.setPower(speed + correction); + //telemetry.addData("Target: ", target); + } driveMotors.setAll(0); sumOfError = overall_error; + } public void strafeRight(double target, Telemetry telemetry) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DriveForward.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DriveForward.java index c6910211fcf4..d4f01c337d05 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DriveForward.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DriveForward.java @@ -24,7 +24,7 @@ public void runOpMode() { waitForStart(); if (!opModeIsActive()) return; - pidDrive.strafeLeft(24, telemetry); + pidDrive.DriveForward(24, telemetry); // pidTurn.TurnRobot(360, telemetry); // sleep(1000); // pidTurn.TurnRobot(-360, telemetry); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAuto.java index 2ce642b24684..95a24ea5b3bf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAuto.java @@ -258,11 +258,12 @@ public void runOpMode() throws InterruptedException { turnPID.TurnRobot(modifier * 100.0, telemetry); forwardPID.DriveReverse(19.0, telemetry, 5.0); // Left: 9.5", Center: 15", Right: 21.5" + Task navigateInFrontOfTag; scheduler.task(tracker.getTaskFactory()); scheduler.task(trackerTagUpdate.updateTool(tracker)); - Task navigateInFrontOfTag; + switch (result) { case Left: navigateInFrontOfTag = newOdo.strafeLeft( @@ -293,7 +294,6 @@ public void runOpMode() throws InterruptedException { // ewwwww scheduler.runToCompletion(this::opModeIsActive); - // robot.liftLeft().setTargetPosition(1000); // encoder ticks diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyBlueRightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyBlueRightTest.java new file mode 100644 index 000000000000..d14d1d085f8f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyBlueRightTest.java @@ -0,0 +1,71 @@ +package org.firstinspires.ftc.teamcode.opmodes; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraName; +import org.firstinspires.ftc.teamcode.Var; +import org.firstinspires.ftc.teamcode.configurations.RobotConfiguration; +import org.firstinspires.ftc.teamcode.odo.DriveForwardPID; +import org.firstinspires.ftc.teamcode.odo.TurnPID; + +@Autonomous +public class TwentyBlueRightTest extends LinearOpMode{ + private DriveForwardPID forwardPID; + private TurnPID turnPID; + private RobotConfiguration robot; + + @Override + public void runOpMode() throws InterruptedException { + RobotConfiguration robot = RobotConfiguration.currentConfiguration().invoke(hardwareMap); + robot.clearEncoders(); + DriveForwardPID drivePID = new DriveForwardPID(robot); + TurnPID turnPID = new TurnPID(robot); + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + robot = RobotConfiguration.currentConfiguration().invoke(hardwareMap); + waitForStart(); + if (!opModeIsActive()) return; + /*Left spike marker BlueRightAuto + /drivePID.DriveReverse(21.0, telemetry); + turnPID.TurnRobot(45.0, telemetry); + drivePID.DriveReverse(3.0, telemetry); + turnPID.TurnRobot(-45.0, telemetry); + drivePID.strafeLeft(2, telemetry); + drivePID.DriveReverse(25, telemetry); + sleep(250); + turnPID.TurnRobot(95, telemetry); + drivePID.DriveReverse(80, telemetry); + drivePID.strafeRight(27, telemetry);*/ + + /*Center spike marker BlueRightAuto + drivePID.DriveReverse(27, telemetry); + drivePID.strafeLeft(7, telemetry); + drivePID.DriveReverse(24, telemetry); + turnPID.TurnRobot(95, telemetry); + drivePID.DriveReverse(87, telemetry); + drivePID.strafeRight(22, telemetry);*/ + + //Right spike marker BlueRightAuto + drivePID.DriveReverse(16.0, telemetry); + turnPID.TurnRobot(-60.0, telemetry); + turnPID.TurnRobot(60, telemetry); + drivePID.strafeRight(4.5, telemetry); + drivePID.DriveReverse(34, telemetry); + sleep(250); + turnPID.TurnRobot(90, telemetry); + drivePID.DriveReverse(78, telemetry); + drivePID.strafeRight(20.5, telemetry); + + while (opModeIsActive()) { + telemetry.addData("LEFT ", drivePID.LeftOdoDist()); + telemetry.addData("RIGHT", drivePID.RightOdoDist()); + telemetry.update(); + sleep(20); + } + + + + } +}