From ab4e805e20c5beef08398d5952cca80d076f8faa Mon Sep 17 00:00:00 2001 From: Varun Damarla <57463486+damarlavarun2004@users.noreply.github.com> Date: Sun, 28 Mar 2021 12:26:09 -0400 Subject: [PATCH 1/8] Added javadoc --- .../ftc/teamcode/API/Config/Constants.java | 16 ++++++++ .../ftc/teamcode/API/InitRobot.java | 5 ++- .../ftc/teamcode/API/Movement.java | 7 +++- .../firstinspires/ftc/teamcode/API/Robot.java | 40 ++++++++++++++++--- .../ftc/teamcode/API/SampleMecanumDrive.java | 5 +++ .../ftc/teamcode/API/Sensor.java | 17 +++++++- .../ftc/teamcode/API/StateMachine.java | 12 ++++++ .../teamcode/OpMode/Autonomous/PowerShot.java | 4 ++ .../OpMode/TeleOp/ColorSensorTelemetry.java | 4 ++ 9 files changed, 99 insertions(+), 11 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java index b97e438..22387c0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java @@ -57,14 +57,30 @@ public class Constants { public static double MAX_ANG_VEL = Math.toRadians(180.0); public static double MAX_ANG_ACCEL = Math.toRadians(180.0); + + /** + * Returns encoder ticks in converted inches + * @param ticks number of encoder ticks as stated by the encoder + * @return encoder ticks converted to inches + */ public static double encoderTicksToInches(double ticks) { return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; } + /** + * Returns rpm in converted velocity + * @param rpm revolutions per minute of the motor + * @return velocity of the robot using the rpm information and some math + */ public static double rpmToVelocity(double rpm) { return rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS / 60.0; } + /** + * Returns ticks per second in converted velocity + * @param ticksPerSecond ticks per second information as listed on the encoder + * @return + */ public static double getMotorVelocityF(double ticksPerSecond) { // see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx return 32767 / ticksPerSecond; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java index 6829c68..8e20441 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java @@ -30,7 +30,10 @@ public class InitRobot { private static SmartMotor fl; private static SmartMotor fr; - // TODO: JavaDoc + /** + * Initializes the robot as per information stored in the passed LinearOpMode + * @param l LinearOpMode object consisting of all the robot's information + */ public static void init(@NotNull LinearOpMode l) { /* * ####### ###### diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java index b84489a..838640c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java @@ -175,7 +175,7 @@ public void moveIntake(double intakePower) { /** * Opens the grabber based on a boolean assignment - * @param command tue to open + * @param command true to open */ public void grabWobble(boolean command) { if (command) { @@ -185,7 +185,10 @@ public void grabWobble(boolean command) { } } - + /** + * Pushes the disk for launch based on a boolean assignment + * @param command true to push the disk + */ public void launch(boolean command) { if (command) { Objects.requireNonNull(servos.get(Naming.SERVO_LAUNCHER)).setPosition(LAUNCHER_OPEN); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Robot.java index 2992434..d6b9a84 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Robot.java @@ -41,7 +41,13 @@ public class Robot { public static void whiteLine(String sensor, double power) { driveToColor(sensor, power, Sensor.Colors.WHITE); } - + + /** + * Stops the robot once it detects the desired color + * @param sensor name of the color sensor as noted in Naming + * @param power amount of power to be delivered + * @param targetColor the specific color as noted in Sensor that the robot must detect + */ public static void driveToColor(String sensor, double power, Sensor.Colors targetColor) { while (true) { Sensor.Colors color = Sensor.getRGB(sensor); @@ -53,7 +59,12 @@ public static void driveToColor(String sensor, double power, Sensor.Colors targe } } } - + + /** + * Shoots the desired number of disks based on desired voltage autonomously + * @param count number of disks to be shot + * @param volts amount of power assigned to shoot the disk + */ public static void shootAuto(int count, double volts) { // Power up flywheel movement.moveFlywheel(volts/Robot.sensor.getBatteryVoltage(linearOpMode.hardwareMap.voltageSensor)); @@ -64,11 +75,20 @@ public static void shootAuto(int count, double volts) { movement.launch(false); } } - + + /** + * Shoots the desired number of disks based on assigned voltage autonomously + * @param count number of disks to be shot + */ public static void shootAuto(int count) { shootAuto(count, 10.00); } - + + /** + * Moves the robot arm autonomously to interact with the wobble goal + * @param sensor name of the color sensor to assist arm movement + * @param arm name of the arm motor that is moving the arm + */ public static void moveArm(String sensor, String arm) { SmartMotor armMotor = Movement.getMotor(arm); Sensor.Colors target; @@ -99,6 +119,12 @@ public static void moveArm(String sensor, String arm) { } } + /** + * Moves the robot arm based on boolean assignment to interact with the wobble goal + * @param command true to open the arm + * @param sensor name of the color sensor to assist arm movement + * @param arm name of the arm motor that is moving the arm + */ public static void moveArm(boolean command, String sensor, String arm) { SmartMotor armMotor = Movement.getMotor(arm); Sensor.Colors target; @@ -124,10 +150,12 @@ public static void moveArm(boolean command, String sensor, String arm) { armMotor.setPower(0); } - + /** + * Pulls the arm out and drop the wobble goal + */ public static void wobbleDrop() { Robot.moveArm(true, Naming.COLOR_SENSOR_ARM, Naming.MOTOR_WOBBLE_ARM); - Robot.movement.grabWobble(true); + Robot.movement.grabWobble(true); // TODO modify this function because it's confusing linearOpMode.sleep(500); Robot.moveArm(false, Naming.COLOR_SENSOR_ARM, Naming.MOTOR_WOBBLE_ARM); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java index ff5194a..564cac0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java @@ -176,6 +176,11 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap)); } + /** + * Builds a trajectory for the robot to move according to in autonomous + * @param startPose starting position of the robot on the field + * @return the trajectory the robot must follow + */ public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) { return new TrajectoryBuilder(startPose, velConstraint, accelConstraint); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Sensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Sensor.java index 58e3ce0..7d739eb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Sensor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Sensor.java @@ -273,6 +273,11 @@ public void calibrateGyro(String id) { ReadWriteFile.writeFile(file, calibrationData.serialize()); } + /** + * Converts RGB to HSV values for color sensor detection + * @param rgb color stored in RGB that is to be converted + * @return the color converted to HSV + */ public static double[] RGBtoHSV(double[] rgb) { double[] hsv = new double[] {0, 0, 0}; double max = Math.max(Math.max(rgb[0], rgb[1]), rgb[2]); @@ -306,7 +311,11 @@ public static double[] RGBtoHSV(double[] rgb) { return hsv; } - // Computes the current battery voltage + /** + * Computes the current battery voltage + * @param voltageSensors all the powers of the sensors in the robot + * @return the current battery voltage of the robot + */ public double getBatteryVoltage(HardwareMap.DeviceMapping voltageSensors) { double result = Double.POSITIVE_INFINITY; for (VoltageSensor sensor : voltageSensors) { @@ -350,7 +359,11 @@ public static void initTfod() { tfod.setZoom(2, 16.0/9.0); } } - + + /** + * Returns the number of disks the robot sees using the camera + * @return the number of disks detected (none, zero, or four) + */ public static Disks detectDisks() { for (int i = 0; i<1000000; i++) { if (Robot.linearOpMode.isStopRequested()) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StateMachine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StateMachine.java index a8ce154..7895261 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StateMachine.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StateMachine.java @@ -1,17 +1,29 @@ package org.firstinspires.ftc.teamcode.API; +/** + * State machine to ease synchronize and prevents bugs with robot movements + */ public class StateMachine { public enum State { ARMIN, ARMOUT, DRIVE } + private static State currentState; + /** + * Returns what the robot is doing at the current instant + * @return the state of the robot's current action + */ public State getCurrentState() { return currentState; } + /** + * Modifies the state of the robot based on the user's command + * @param state the new state of robot based on the user's command + */ public void setCurrentState(State state) { currentState = state; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/PowerShot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/PowerShot.java index 5ff19a6..588f595 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/PowerShot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/PowerShot.java @@ -23,6 +23,10 @@ public class PowerShot extends LinearOpMode { public static double DRIVEY = 15; public static double TURNFUDGE = 180.0/150; @Override + + /** + * Drives the robot to the appropriate spot to shoot the powershot pegs down and parks it at the white line + */ public void runOpMode() throws InterruptedException { InitRobot.init(this); SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ColorSensorTelemetry.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ColorSensorTelemetry.java index d94962b..1559697 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ColorSensorTelemetry.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ColorSensorTelemetry.java @@ -21,6 +21,10 @@ @TeleOp(name="Color Sensor Telemetry", group="99-test") public class ColorSensorTelemetry extends LinearOpMode { @Override + + /** + * Drives the robot to the white line until the color is detected or user presses stop + */ public void runOpMode() throws InterruptedException { InitRobot.init(this); From 289becbded30916748082b9a6c34a476510baec7 Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Sun, 28 Mar 2021 18:53:16 -0400 Subject: [PATCH 2/8] Add hook intake --- .../ftc/teamcode/API/Config/Naming.java | 3 +++ .../ftc/teamcode/API/InitRobot.java | 4 ++++ .../ftc/teamcode/API/Movement.java | 21 +++++++++++++++++++ .../teamcode/OpMode/TeleOp/TeleOpMain.java | 12 +++++++++++ .../ftc/teamcode/OpMode/TeleOp/ZeroServo.java | 6 +++--- 5 files changed, 43 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java index 5c0d4bd..24b3388 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java @@ -15,6 +15,9 @@ public class Naming { public static final String SERVO_WOBBLE_GRABBER = "wobbleGrabber"; public static final String SERVO_LAUNCHER = "launcher"; + + public static final String SERVO_HOOK_1 = "hook1"; + public static final String SERVO_HOOK_2 = "hook2"; public static final String COLOR_SENSOR_PARK = "parkSensor"; public static final String COLOR_SENSOR_ARM = "armSensor"; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java index 8e20441..0c726f9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java @@ -76,12 +76,16 @@ public static void init(@NotNull LinearOpMode l) { // Get servos SmartServo wobbleGrabber = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_WOBBLE_GRABBER)); SmartServo launcher = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_LAUNCHER)); + SmartServo hook1 = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_HOOK_1)); + SmartServo hook2 = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_HOOK_2)); // Add servos into the list HashMap servos = new HashMap<>(); //servos.put(Naming.SERVO_WOBBLE_ARM, wobbleArm); servos.put(Naming.SERVO_WOBBLE_GRABBER, wobbleGrabber); servos.put(Naming.SERVO_LAUNCHER, launcher); + servos.put(Naming.SERVO_HOOK_1, hook1); + servos.put(Naming.SERVO_HOOK_2, hook2); // Add CRServos into the list HashMap crServos = new HashMap<>(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java index 838640c..24d059a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java @@ -49,6 +49,11 @@ public class Movement { private final static double WOBBLE_RELEASE = 0; private final static double LAUNCHER_OPEN = 0.6; private final static double LAUNCHER_CLOSE = 1; + private final static double HOOK_OPEN = 1; + private final static double HOOK_CLOSE = 0.4; + private final static double HOOK_IN = 0.6; + private final static double HOOK_OUT = 0; + public static HashMap motors; public static HashMap servos; @@ -196,4 +201,20 @@ public void launch(boolean command) { Objects.requireNonNull(servos.get(Naming.SERVO_LAUNCHER)).setPosition(LAUNCHER_CLOSE); } } + + public void hook(boolean command) { + if (command) { + Objects.requireNonNull(servos.get(Naming.SERVO_HOOK_2)).setPosition(HOOK_CLOSE); + } else { + Objects.requireNonNull(servos.get(Naming.SERVO_HOOK_2)).setPosition(HOOK_OPEN); + } + } + + public void extendHook(boolean command) { + if (command) { + Objects.requireNonNull(servos.get(Naming.SERVO_HOOK_1)).setPosition(HOOK_OUT); + } else { + Objects.requireNonNull(servos.get(Naming.SERVO_HOOK_1)).setPosition(HOOK_IN); + } + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java index 4655b85..4a6736d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java @@ -115,6 +115,18 @@ public void runOpMode() { } Robot.movement.launch(gamepad2.right_bumper); + + if (gamepad2.dpad_down) { + Robot.movement.extendHook(true); + } else if (gamepad2.dpad_up) { + Robot.movement.extendHook(false); + } + + if (gamepad2.dpad_right) { + Robot.movement.hook(true); + } else if (gamepad2.dpad_left) { + Robot.movement.hook(false); + } telemetry.addData("Back Left", blPower); telemetry.addData("Back Right", brPower); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ZeroServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ZeroServo.java index 15a2bb5..ffd0982 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ZeroServo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ZeroServo.java @@ -26,7 +26,7 @@ associated documentation files (the "Software"), to deal in the Software without public class ZeroServo extends LinearOpMode { @Override public void runOpMode() { - Servo zeroServo = hardwareMap.get(Servo.class, Naming.SERVO_WOBBLE_GRABBER); + Servo zeroServo = hardwareMap.get(Servo.class, Naming.SERVO_HOOK_2); telemetry.addData("Status", "Initialized"); telemetry.update(); @@ -42,9 +42,9 @@ public void runOpMode() { waitForStart(); while(opModeIsActive()) { if (gamepad2.a) { - zeroServo.setPosition(0.3); + zeroServo.setPosition(1); } else if (gamepad2.b) { - zeroServo.setPosition(0); + zeroServo.setPosition(0.4); } } } From 6012d17165e6e66e8f13c9406c91dd9985fdeaa7 Mon Sep 17 00:00:00 2001 From: VarunDamarla <57463486+VarunDamarla@users.noreply.github.com> Date: Wed, 31 Mar 2021 18:34:01 -0400 Subject: [PATCH 3/8] Added javadoc for hook functions --- .idea/misc.xml | 2 +- .../org/firstinspires/ftc/teamcode/API/Movement.java | 12 ++++++++++-- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/.idea/misc.xml b/.idea/misc.xml index 06ebe6f..103c049 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,6 +1,6 @@ - + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java index 24d059a..0c0df84 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java @@ -201,7 +201,11 @@ public void launch(boolean command) { Objects.requireNonNull(servos.get(Naming.SERVO_LAUNCHER)).setPosition(LAUNCHER_CLOSE); } } - + + /** + * Pushes the disk into the intake based on a boolean assignment + * @param command true to push the disk + */ public void hook(boolean command) { if (command) { Objects.requireNonNull(servos.get(Naming.SERVO_HOOK_2)).setPosition(HOOK_CLOSE); @@ -209,7 +213,11 @@ public void hook(boolean command) { Objects.requireNonNull(servos.get(Naming.SERVO_HOOK_2)).setPosition(HOOK_OPEN); } } - + + /** + * Opens the hook to push the disk into the intake based on a boolean assignment + * @param command true to open the hook + */ public void extendHook(boolean command) { if (command) { Objects.requireNonNull(servos.get(Naming.SERVO_HOOK_1)).setPosition(HOOK_OUT); From fe144208f6da08fa52e1227d9e7f725a6505c93d Mon Sep 17 00:00:00 2001 From: VarunDamarla <57463486+VarunDamarla@users.noreply.github.com> Date: Wed, 31 Mar 2021 18:46:21 -0400 Subject: [PATCH 4/8] Added javadoc --- .../org/firstinspires/ftc/teamcode/API/HW/SmartMotor.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/HW/SmartMotor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/HW/SmartMotor.java index 03cdc41..0a7f98c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/HW/SmartMotor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/HW/SmartMotor.java @@ -35,6 +35,10 @@ public class SmartMotor { // Motor configuration @Getter @Setter double powerModifier = 1; + /** + * Creates a SmartMotor object based on motor object + * @param motor motor object that is being "converted" into a SmartMotor + */ public SmartMotor(DcMotorEx motor) { this.motor = motor; power = 0; From 8cdffa641573e82a0752852434107ae9c074b7e8 Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Thu, 15 Apr 2021 20:41:25 -0400 Subject: [PATCH 5/8] Tune PID coeffs --- .gitignore | 2 +- .idea/misc.xml | 2 +- .../ftc/teamcode/API/Config/Constants.java | 10 +++++----- .../org/firstinspires/ftc/teamcode/API/InitRobot.java | 8 ++++---- .../ftc/teamcode/API/SampleMecanumDrive.java | 2 +- .../teamcode/API/StandardTrackingWheelLocalizer.java | 8 ++++---- .../ftc/teamcode/OpMode/Autonomous/SplineTest.java | 2 +- 7 files changed, 17 insertions(+), 17 deletions(-) diff --git a/.gitignore b/.gitignore index 674138c..97a31c4 100644 --- a/.gitignore +++ b/.gitignore @@ -86,4 +86,4 @@ lint/outputs/ lint/tmp/ # lint/reports/ -VuForiaKey.java +.SpaceVim.d diff --git a/.idea/misc.xml b/.idea/misc.xml index 103c049..06ebe6f 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,6 +1,6 @@ - + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java index 22387c0..1689382 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java @@ -8,7 +8,7 @@ public class Constants { /* * These are motor constants that should be listed online for your motors. */ - public static final double TICKS_PER_REV = 4096; + public static final double TICKS_PER_REV = 8192; public static final double MAX_RPM = 340; /* @@ -32,7 +32,7 @@ public class Constants { */ public static double WHEEL_RADIUS = 2; // in public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed - public static double TRACK_WIDTH = 15.5; // in + public static double TRACK_WIDTH = 14.4; // in /* * These are the feedforward parameters used to model the drive motor behavior. If you are using @@ -40,9 +40,9 @@ public class Constants { * motor encoders or have elected not to use them for velocity control, these values should be * empirically tuned. */ - public static double kV = 1.0 / rpmToVelocity(MAX_RPM); + public static double kV = 0.0170; public static double kA = 0; - public static double kStatic = 0; + public static double kStatic = 0.07; /* * These values are used to generate the trajectories for you robot. To ensure proper operation, @@ -53,7 +53,7 @@ public class Constants { * forces acceleration-limited profiling). All distance units are inches. */ public static double MAX_VEL = 30.0; - public static double MAX_ACCEL = 20.0; + public static double MAX_ACCEL = 17.5; public static double MAX_ANG_VEL = Math.toRadians(180.0); public static double MAX_ANG_ACCEL = Math.toRadians(180.0); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java index 0c726f9..c200974 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java @@ -116,10 +116,10 @@ public static void init(@NotNull LinearOpMode l) { fr.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); } - bl.setPowerModifier(Constants.MOTOR_BL_POWERMOD); - br.setPowerModifier(Constants.MOTOR_BR_POWERMOD); - fl.setPowerModifier(Constants.MOTOR_FL_POWERMOD); - fr.setPowerModifier(Constants.MOTOR_FR_POWERMOD); + //bl.setPowerModifier(Constants.MOTOR_BL_POWERMOD); + //br.setPowerModifier(Constants.MOTOR_BR_POWERMOD); + //fl.setPowerModifier(Constants.MOTOR_FL_POWERMOD); + //fr.setPowerModifier(Constants.MOTOR_FR_POWERMOD); // Get color sensors SmartColorSensor parkSensor = new SmartColorSensor((NormalizedColorSensor)l.hardwareMap.get(ColorSensor.class, Naming.COLOR_SENSOR_PARK)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java index 564cac0..6297a57 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java @@ -62,7 +62,7 @@ */ @Config public class SampleMecanumDrive extends MecanumDrive { - public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0); + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0.1); public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0); public static double LATERAL_MULTIPLIER = 1; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java index e2be2e5..bbd0295 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java @@ -28,12 +28,12 @@ */ @Config public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer { - public static double TICKS_PER_REV = 4096; - public static double WHEEL_RADIUS = 0.7; // in + public static double TICKS_PER_REV = 8192; + public static double WHEEL_RADIUS = 0.8; // in public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed - public static double LATERAL_DISTANCE = 19.81; // in; distance between the left and right wheels - public static double FORWARD_OFFSET = 4.8; // in; offset of the lateral wheel + public static double LATERAL_DISTANCE = 15.9; // in; distance between the left and right wheels + public static double FORWARD_OFFSET = 6.0; // in; offset of the lateral wheel private static Encoder leftEncoder, rightEncoder, lateralEncoder; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/SplineTest.java index 99c1144..7a9410a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/SplineTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/SplineTest.java @@ -24,7 +24,7 @@ public void runOpMode() throws InterruptedException { if (isStopRequested()) return; Trajectory traj = drive.trajectoryBuilder(new Pose2d()) - .splineTo(new Vector2d(30, 30), 0) + .splineTo(new Vector2d(30, -30), 0) .build(); drive.followTrajectory(traj); From 880d0b2ddf05b23e776010d0e4bc39576ef76611 Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Fri, 16 Apr 2021 20:52:12 -0400 Subject: [PATCH 6/8] Update PID, fix odometry wheel direction --- .../firstinspires/ftc/teamcode/API/SampleMecanumDrive.java | 4 ++-- .../ftc/teamcode/API/StandardTrackingWheelLocalizer.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java index 6297a57..19fe08b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java @@ -62,8 +62,8 @@ */ @Config public class SampleMecanumDrive extends MecanumDrive { - public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0.1); - public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0); + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0.5, 0.5, 1); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(0.5, 0.5, 1); public static double LATERAL_MULTIPLIER = 1; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java index bbd0295..77032fc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java @@ -51,7 +51,7 @@ public StandardTrackingWheelLocalizer(HardwareMap hardwareMap) { // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) leftEncoder.setDirection(Encoder.Direction.FORWARD); rightEncoder.setDirection(Encoder.Direction.REVERSE); - lateralEncoder.setDirection(Encoder.Direction.FORWARD); + lateralEncoder.setDirection(Encoder.Direction.REVERSE); } public static double encoderTicksToInches(double ticks) { From 1e10239a3c50444fcd060c4137966797aa8c4821 Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Sun, 18 Apr 2021 10:03:24 -0400 Subject: [PATCH 7/8] Update teleop --- .../ftc/teamcode/OpMode/TeleOp/TeleOpMain.java | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java index 4a6736d..56ce42a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java @@ -97,34 +97,34 @@ public void runOpMode() { } // Moving the wobble - if (gamepad2.x) { + if (gamepad1.x) { if (Sensor.getRGB(Naming.COLOR_SENSOR_ARM) != Sensor.Colors.RED) { Robot.state.setCurrentState(StateMachine.State.ARMOUT); } - } else if (gamepad2.y) { + } else if (gamepad1.y) { if (Sensor.getRGB(Naming.COLOR_SENSOR_ARM) != Sensor.Colors.BLUE) { Robot.state.setCurrentState(StateMachine.State.ARMIN); } } Robot.moveArm(Naming.COLOR_SENSOR_ARM, Naming.MOTOR_WOBBLE_ARM); - if (gamepad2.a) { + if (gamepad1.a) { Robot.movement.grabWobble(true); - } else if (gamepad2.b) { + } else if (gamepad1.b) { Robot.movement.grabWobble(false); } Robot.movement.launch(gamepad2.right_bumper); - if (gamepad2.dpad_down) { + if (gamepad2.a) { Robot.movement.extendHook(true); - } else if (gamepad2.dpad_up) { + } else if (gamepad2.y) { Robot.movement.extendHook(false); } - if (gamepad2.dpad_right) { + if (gamepad2.b) { Robot.movement.hook(true); - } else if (gamepad2.dpad_left) { + } else if (gamepad2.x) { Robot.movement.hook(false); } From 3804d9aa01b5f27e00c0f72eaede9dba7edb3a7e Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Sat, 24 Apr 2021 18:15:56 -0400 Subject: [PATCH 8/8] League qualifier adjustments! --- .../ftc/teamcode/API/Config/Constants.java | 4 +- .../ftc/teamcode/API/SampleMecanumDrive.java | 6 +- .../API/StandardTrackingWheelLocalizer.java | 2 +- .../OpMode/Autonomous/WobbleRRShot.java | 87 +++++++++++++++++++ .../OpMode/Autonomous/WobbleShot.java | 53 +++++------ 5 files changed, 121 insertions(+), 31 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WobbleRRShot.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java index 1689382..582aa04 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java @@ -1,9 +1,11 @@ package org.firstinspires.ftc.teamcode.API.Config; +import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.hardware.PIDFCoefficients; import org.firstinspires.ftc.teamcode.API.Sensor; +@Config public class Constants { /* * These are motor constants that should be listed online for your motors. @@ -55,7 +57,7 @@ public class Constants { public static double MAX_VEL = 30.0; public static double MAX_ACCEL = 17.5; public static double MAX_ANG_VEL = Math.toRadians(180.0); - public static double MAX_ANG_ACCEL = Math.toRadians(180.0); + public static double MAX_ANG_ACCEL = Math.toRadians(90.0); /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java index 19fe08b..e7501da 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java @@ -62,8 +62,8 @@ */ @Config public class SampleMecanumDrive extends MecanumDrive { - public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0.5, 0.5, 1); - public static PIDCoefficients HEADING_PID = new PIDCoefficients(0.5, 0.5, 1); + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0.6, 0.7, 1.2); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(0.65, 0.5, 1.2); public static double LATERAL_MULTIPLIER = 1; @@ -134,7 +134,7 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { } // TODO: adjust the names of the following hardware devices to match your configuration - imu = Sensor.getGyro(Naming.GYRO_0); + imu = Sensor.getGyro(Naming.GYRO_1); BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS; imu.initialize(parameters); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java index 77032fc..4564017 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java @@ -29,7 +29,7 @@ @Config public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer { public static double TICKS_PER_REV = 8192; - public static double WHEEL_RADIUS = 0.8; // in + public static double WHEEL_RADIUS = 0.8; // in. bumped to more than actuality because we want to make sure that it doesn't overshoot public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed public static double LATERAL_DISTANCE = 15.9; // in; distance between the left and right wheels diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WobbleRRShot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WobbleRRShot.java new file mode 100644 index 0000000..19cf6f6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WobbleRRShot.java @@ -0,0 +1,87 @@ +package org.firstinspires.ftc.teamcode.OpMode.Autonomous; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.API.Config.Naming; +import org.firstinspires.ftc.teamcode.API.InitRobot; +import org.firstinspires.ftc.teamcode.API.Robot; +import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.API.Sensor; + +/* + * This is an autonomous program to shoot the power shots, drop off the wobble goals, and park + */ +@Config +@Autonomous(name="Wobble RR Shot", group = "00-drive") +public class WobbleRRShot extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + InitRobot.init(this); + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + Sensor.initVuforia(Naming.WEBCAM_0); + Sensor.initTfod(); + + Pose2d position = new Pose2d(); + + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + telemetry.addLine(); + telemetry.addData(">", "Press start"); + telemetry.update(); + + waitForStart(); + if (isStopRequested()) return; + + Sensor.Disks disks = Sensor.detectDisks(); + + telemetry.addData("Detected:", disks); + telemetry.update(); + // Go to the white line + //Robot.whiteLine(Naming.COLOR_SENSOR_PARK, 0.4); + drive.followTrajectory(drive.trajectoryBuilder(position).forward(81).build()); + + drive.turn(0.2); + + // Shoot the first disk + Robot.shootAuto( 1, 9.75); + drive.turn(-0.25); + // Shoot again + Robot.shootAuto(1); + drive.turn(-0.30); + // And again + Robot.shootAuto(1); + // Go back to the white line + Robot.whiteLine(Naming.COLOR_SENSOR_PARK, 0.4); + Robot.movement.moveFlywheel(0); + + drive.turn(-Math.PI); // Roughly 90 degrees + Robot.driveToColor(Naming.COLOR_SENSOR_PARK, 0.4, Sensor.Colors.RED); + + if (disks == Sensor.Disks.ONE) { + drive.followTrajectory(drive.trajectoryBuilder(new Pose2d()).strafeLeft(40).build()); + Robot.moveArm(true, Naming.COLOR_SENSOR_ARM, Naming.MOTOR_WOBBLE_ARM); + Robot.wobbleDrop(); + drive.followTrajectory(drive.trajectoryBuilder(new Pose2d()).strafeRight(40).build()); + } else if (disks == Sensor.Disks.NONE) { + drive.turn(-Math.PI/2); // Roughly 90 degrees + Robot.wobbleDrop(); + } else { + Robot.movement.move1x4(-0.4); + sleep(400); + Robot.movement.move1x4(0); + drive.turn(-Math.PI/2); // Roughly 90 degrees + Robot.driveToColor(Naming.COLOR_SENSOR_PARK, -0.4, Sensor.Colors.RED); + Robot.movement.move1x4(-0.4); + sleep(600); + Robot.movement.move1x4(0); + Robot.driveToColor(Naming.COLOR_SENSOR_PARK, -0.4, Sensor.Colors.RED); + Robot.wobbleDrop(); + Robot.whiteLine(Naming.COLOR_SENSOR_PARK, 0.4); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WobbleShot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WobbleShot.java index 736ddb5..7eaa88c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WobbleShot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WobbleShot.java @@ -10,19 +10,17 @@ import org.firstinspires.ftc.teamcode.API.Config.Naming; import org.firstinspires.ftc.teamcode.API.InitRobot; +import org.firstinspires.ftc.teamcode.API.Movement; import org.firstinspires.ftc.teamcode.API.Robot; import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; import org.firstinspires.ftc.teamcode.API.Sensor; /* - * This is an autonomous program to shoot the power shots, drop off the wobble goals, and park + * This is a simple program to reach the white line */ @Config -@Autonomous(name="Wobble Shot", group = "00-drive") +@Autonomous(name="Wobble Shot", group = "drive") public class WobbleShot extends LinearOpMode { - public static double DRIVEFUDGE = 60.0/42; - public static double DRIVEY = 15; - public static double TURNFUDGE = 180.0/150; @Override public void runOpMode() throws InterruptedException { InitRobot.init(this); @@ -47,44 +45,47 @@ public void runOpMode() throws InterruptedException { // Go to the white line Robot.whiteLine(Naming.COLOR_SENSOR_PARK, 0.4); - // Move so we are in the launch zone - Trajectory moveback = drive.trajectoryBuilder(new Pose2d()) - .back(DRIVEY*DRIVEFUDGE) - .build(); - drive.followTrajectory(moveback); - drive.turn(0.2*TURNFUDGE); + // Move back so we are in the launch zone + drive.followTrajectory(drive.trajectoryBuilder(new Pose2d()).back(7).build()); + // Get in position to shoot... + drive.turn(Math.toRadians(2)); // Shoot the first disk - Robot.shootAuto( 1, 9.75); - drive.turn(-0.25*TURNFUDGE); - // Shoot again + Robot.shootAuto( 1); + // Adjust and shoot + drive.turn(Math.toRadians(-7)); Robot.shootAuto(1); - drive.turn(-0.30*TURNFUDGE); // And again - Robot.shootAuto(1); - // Go back to the white line - Robot.whiteLine(Naming.COLOR_SENSOR_PARK, 0.4); + drive.turn(Math.toRadians(-5)); + Robot.shootAuto(1, 9.75); + // Make sure that the flywheel is unpowered Robot.movement.moveFlywheel(0); - drive.turn(-1.80*TURNFUDGE); // Roughly 90 degrees + // Go back to the white line and park + Robot.whiteLine(Naming.COLOR_SENSOR_PARK, 0.4); + + drive.turn(Math.toRadians(-80)); Robot.driveToColor(Naming.COLOR_SENSOR_PARK, 0.4, Sensor.Colors.RED); if (disks == Sensor.Disks.ONE) { - drive.followTrajectory(drive.trajectoryBuilder(new Pose2d()).strafeLeft(40).build()); - Robot.moveArm(true, Naming.COLOR_SENSOR_ARM, Naming.MOTOR_WOBBLE_ARM); + Robot.movement.move4x4(-0.4, 0.4, 0.4, -0.4); + sleep(750); + Robot.movement.move1x4(0); Robot.wobbleDrop(); - drive.followTrajectory(drive.trajectoryBuilder(new Pose2d()).strafeRight(40).build()); + Robot.movement.move4x4(0.4, -0.4, -0.4, 0.4); + sleep(750); + Robot.movement.move1x4(0); } else if (disks == Sensor.Disks.NONE) { - drive.turn(-2*TURNFUDGE); // Roughly 90 degrees + drive.turn(Math.toRadians(-90)); // Roughly 90 degrees Robot.wobbleDrop(); } else { Robot.movement.move1x4(-0.4); - sleep(400); + sleep(200); Robot.movement.move1x4(0); - drive.turn(-2*TURNFUDGE); // Roughly 90 degrees + drive.turn(Math.toRadians(-90)); // Roughly 90 degrees Robot.driveToColor(Naming.COLOR_SENSOR_PARK, -0.4, Sensor.Colors.RED); Robot.movement.move1x4(-0.4); - sleep(600); + sleep(500); Robot.movement.move1x4(0); Robot.driveToColor(Naming.COLOR_SENSOR_PARK, -0.4, Sensor.Colors.RED); Robot.wobbleDrop();