From 9a6487a36885d147365af8056ba86a8b62de4b57 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Thu, 4 Jan 2024 20:56:23 -0600 Subject: [PATCH] Updated Vexy/Yellow minibot (disabled code in autonomous drive states) --- .../minibots/yellow/YellowMinibot.java | 182 ++++++++++-------- .../minibots/yellow/states/Pilot.java | 107 +++++++--- .../minibots/yellow/states/Rotate.java | 62 +++--- .../minibots/yellow/states/StrafeMove.java | 34 ++-- .../minibots/yellow/states/TankMove.java | 76 ++++---- 5 files changed, 268 insertions(+), 193 deletions(-) diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/YellowMinibot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/YellowMinibot.java index a7ff840..498ede1 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/YellowMinibot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/YellowMinibot.java @@ -3,8 +3,14 @@ package dev.cyberarm.minibots.yellow; import com.arcrobotics.ftclib.hardware.motors.MotorEx; import com.arcrobotics.ftclib.hardware.motors.MotorGroup; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.hardware.rev.RevTouchSensor; import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.TouchSensor; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; @@ -14,46 +20,88 @@ import dev.cyberarm.engine.V2.CyberarmEngine; public class YellowMinibot { private final CyberarmEngine engine; - public final TimeCraftersConfiguration config; - public MotorEx leftFront, rightFront, leftBack, rightBack; - public MotorGroup left, right; - public IMU imu; - public CRServo droneLauncher; - public final double wheelCircumference, distancePerTick, imuAngleOffset, initialFacing; + public TimeCraftersConfiguration config; + public final DcMotorEx backLeft, frontLeft, frontRight, backRight; + public final IMU imu; + public final Servo leftClaw, rightClaw, droneLatch; + public final CRServo arm; + public final TouchSensor armEndStopLeft, armEndStopRight; + + public double DRIVETRAIN_WHEEL_DIAMETER_MM = 90.0; + public int DRIVETRAIN_MOTOR_TICKS = 4; + public double DRIVETRAIN_MOTOR_GEAR_RATIO = 72.0; + public double DRIVETRAIN_MAX_VELOCITY_MM = 305.0; + public double LEFT_CLAW_CLOSED_POSITION = 0.0; + public double LEFT_CLAW_OPEN_POSITION = 0.0; + public double RIGHT_CLAW_CLOSED_POSITION = 0.0; + public double RIGHT_CLAW_OPEN_POSITION = 0.0; + public double DRONE_LATCH_INITIAL_POSITION = 0.0; + public double DRONE_LATCH_LAUNCH_POSITION = 0.0; + public YellowMinibot(CyberarmEngine engine) { this.engine = engine; - this.config = new TimeCraftersConfiguration("Vexy_2023"); + reloadConfig(); - leftFront = new MotorEx(engine.hardwareMap, "leftFront"); - rightFront = new MotorEx(engine.hardwareMap, "rightFront"); + backLeft = (DcMotorEx) engine.hardwareMap.dcMotor.get("backLeft"); + frontLeft = (DcMotorEx) engine.hardwareMap.dcMotor.get("frontLeft"); + frontRight = (DcMotorEx) engine.hardwareMap.dcMotor.get("frontRight"); + backRight = (DcMotorEx) engine.hardwareMap.dcMotor.get("backRight"); - leftBack = new MotorEx(engine.hardwareMap, "leftBack"); - rightBack = new MotorEx(engine.hardwareMap, "rightBack"); + backLeft.setDirection(DcMotorSimple.Direction.FORWARD); + frontLeft.setDirection(DcMotorSimple.Direction.FORWARD); + frontRight.setDirection(DcMotorSimple.Direction.FORWARD); + backRight.setDirection(DcMotorSimple.Direction.FORWARD); - wheelCircumference = Math.PI * (50.0 * 2); //wheelRadius * wheelRadius; - distancePerTick = 288 / wheelCircumference; // raw motor encoder * gear ratio + backLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + frontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + backRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - leftFront.setDistancePerPulse(distancePerTick); - rightFront.setDistancePerPulse(distancePerTick); - leftBack.setDistancePerPulse(distancePerTick); - rightBack.setDistancePerPulse(distancePerTick); + backLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - left = new MotorGroup(leftFront, leftBack); - right = new MotorGroup(rightFront, rightBack); + leftClaw = engine.hardwareMap.servo.get("leftClaw"); /// Port 0 + rightClaw = engine.hardwareMap.servo.get("rightClaw"); /// Port 1 + arm = engine.hardwareMap.crservo.get("arm"); /// Port 2 + arm.setDirection(DcMotorSimple.Direction.REVERSE); + droneLatch = engine.hardwareMap.servo.get("droneLatch"); /// Port 3 - imu = engine.hardwareMap.get(IMU.class, "imu"); + armEndStopLeft = engine.hardwareMap.touchSensor.get("armEndStopLeft"); + armEndStopRight = engine.hardwareMap.touchSensor.get("armEndStopRight"); + + imu = engine.hardwareMap.get(IMU.class, "imu"); // Embedded | IC2 port 0 IMU.Parameters parameters = new IMU.Parameters( new RevHubOrientationOnRobot( RevHubOrientationOnRobot.LogoFacingDirection.UP, RevHubOrientationOnRobot.UsbFacingDirection.RIGHT)); imu.initialize(parameters); + } - imuAngleOffset = 0; - initialFacing = facing(); + public void reloadConfig() { + this.config = new TimeCraftersConfiguration("cyberarm_Vexy_Yellow"); - droneLauncher = engine.hardwareMap.crservo.get("droneLauncher"); /// Port 5 + loadConstants(); + } + + public void loadConstants() { + DRIVETRAIN_WHEEL_DIAMETER_MM = config.variable("Robot", "Drivetrain_Tuning", "wheel_diameter_mm").value(); + DRIVETRAIN_MOTOR_TICKS = config.variable("Robot", "Drivetrain_Tuning", "motor_ticks").value(); + DRIVETRAIN_MOTOR_GEAR_RATIO = config.variable("Robot", "Drivetrain_Tuning", "motor_gear_ratio").value(); + DRIVETRAIN_MAX_VELOCITY_MM = config.variable("Robot", "Drivetrain_Tuning", "max_velocity_mm").value(); + + LEFT_CLAW_CLOSED_POSITION = config.variable("Robot", "LeftClaw_Tuning", "open_position").value(); + LEFT_CLAW_OPEN_POSITION = config.variable("Robot", "LeftClaw_Tuning", "closed_position").value(); + + + RIGHT_CLAW_OPEN_POSITION = config.variable("Robot", "RightClaw_Tuning", "open_position").value(); + RIGHT_CLAW_CLOSED_POSITION = config.variable("Robot", "RightClaw_Tuning", "closed_position").value(); + + DRONE_LATCH_INITIAL_POSITION = config.variable("Robot", "DroneLatch_Tuning", "initial_position").value(); + DRONE_LATCH_LAUNCH_POSITION = config.variable("Robot", "DroneLatch_Tuning", "launch_position").value(); } public void standardTelemetry() { @@ -61,35 +109,45 @@ public class YellowMinibot { engine.telemetry.addLine("Motors"); engine.telemetry.addData( - "Left Front", + "Front Left", "Power: %.2f, Current: %.2f mAmp, Position: %d", - leftFront.motorEx.getPower(), - leftFront.motorEx.getCurrent(CurrentUnit.MILLIAMPS), - leftFront.motorEx.getCurrentPosition()); + frontLeft.getPower(), + frontLeft.getCurrent(CurrentUnit.MILLIAMPS), + frontLeft.getCurrentPosition()); engine.telemetry.addData( - "Right Front", + "Front Right", "Power: %.2f, Current: %.2f mAmp, Position: %d", - rightFront.motorEx.getPower(), - rightFront.motorEx.getCurrent(CurrentUnit.MILLIAMPS), - rightFront.motorEx.getCurrentPosition()); + frontRight.getPower(), + frontRight.getCurrent(CurrentUnit.MILLIAMPS), + frontRight.getCurrentPosition()); engine.telemetry.addData( - "Left Back", + "Back Left", "Power: %.2f, Current: %.2f mAmp, Position: %d", - leftBack.motorEx.getPower(), - leftBack.motorEx.getCurrent(CurrentUnit.MILLIAMPS), - leftBack.motorEx.getCurrentPosition()); + backLeft.getPower(), + backLeft.getCurrent(CurrentUnit.MILLIAMPS), + backLeft.getCurrentPosition()); engine.telemetry.addData( - "Right Back", + "Back Right", "Power: %.2f, Current: %.2f mAmp, Position: %d", - rightBack.motorEx.getPower(), - rightBack.motorEx.getCurrent(CurrentUnit.MILLIAMPS), - rightBack.motorEx.getCurrentPosition()); + backRight.getPower(), + backRight.getCurrent(CurrentUnit.MILLIAMPS), + backRight.getCurrentPosition()); engine.telemetry.addLine(); + engine.telemetry.addLine("Servos"); - engine.telemetry.addData("droneLauncher", droneLauncher.getPower()); + engine.telemetry.addData("left claw", leftClaw.getPosition()); + engine.telemetry.addData("right claw", rightClaw.getPosition()); + engine.telemetry.addData("arm", arm.getPower()); + engine.telemetry.addData("droneLatch", droneLatch.getPosition()); engine.telemetry.addLine(); + + engine.telemetry.addData("arm endstop left", armEndStopLeft.isPressed()); + engine.telemetry.addData("arm endstop right", armEndStopRight.isPressed()); + + engine.telemetry.addLine(); + } public void teleopTelemetry() { @@ -97,46 +155,4 @@ public class YellowMinibot { engine.telemetry.addLine(); } - - public double distanceMM(int ticks) { - return distancePerTick * ticks; - } - - - public double initialFacing() { - return initialFacing; - } - - public double facing() { - double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); - - return (((imuDegrees + 360.0) % 360.0) + imuAngleOffset) % 360.0; - } - - public double heading() { - return AngleUnit.normalizeRadians(-facing() * Math.PI / 180.0); -// return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); - } - - public double turnRate() { - return imu.getRobotAngularVelocity(AngleUnit.DEGREES).yRotationRate; // NOTE: UNTESTED - } - - public boolean isBetween(double value, double min, double max) { - return value >= min && value <= max; - } - - // Adapted from: https://github.com/gosu/gosu/blob/980d64de2ce52e4b16fdd5cb9c9e11c8bbb80671/src/Math.cpp#L38 - public double angleDiff(double from, double to) { - double value = (to - from + 180); - - double fmod = (value - 0.0) % (360.0 - 0.0); - - return (fmod < 0 ? fmod + 360.0 : fmod + 0.0) - 180; - } - - public double lerp(double min, double max, double t) - { - return min + (max - min) * t; - } -} +} \ No newline at end of file diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Pilot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Pilot.java index 6117c15..1591be6 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Pilot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Pilot.java @@ -1,40 +1,43 @@ package dev.cyberarm.minibots.yellow.states; +import com.qualcomm.robotcore.hardware.Gamepad; + import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.engine.V2.Utilities; import dev.cyberarm.minibots.yellow.YellowMinibot; public class Pilot extends CyberarmState { final YellowMinibot robot; + private boolean leftClawOpen, rightClawOpen; public Pilot(YellowMinibot robot) { this.robot = robot; + + this.leftClawOpen = false; + this.rightClawOpen = false; } @Override public void exec() { - /// --- IMU Reset --- /// - if (engine.gamepad1.guide) { - robot.imu.resetYaw(); - } + drivetrain(); + armController(); + clawControllers(); + droneLatchController(); + } - /// --- DRONE --- /// - if (engine.gamepad1.y) { - robot.droneLauncher.setPower(1.0); - } else if (engine.gamepad1.a) { - robot.droneLauncher.setPower(0.0); - } - - /// --- DRIVE --- /// + private void drivetrain() + { // robot.left.set(engine.gamepad1.left_stick_y); // robot.right.set(engine.gamepad1.right_stick_y); // https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html#field-centric - double y = engine.gamepad1.left_stick_y; // Remember, Y stick value is reversed; - double x = -engine.gamepad1.left_stick_x; - double rx = -engine.gamepad1.right_stick_x; + double y = -engine.gamepad1.left_stick_y; // Remember, Y stick value is reversed; + double x = engine.gamepad1.left_stick_x; + double rx = engine.gamepad1.right_stick_x; double botHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); @@ -53,16 +56,72 @@ public class Pilot extends CyberarmState { double frontRightPower = (rotY - rotX - rx) / denominator; double backRightPower = (rotY + rotX - rx) / denominator; -// frontLeftPower *= 24.0; -// backLeftPower *= 24.0; -// frontRightPower *= 24.0; -// backRightPower *= 24.0; + double velocity = Utilities.unitToTicks( + robot.DRIVETRAIN_MOTOR_TICKS, + robot.DRIVETRAIN_MOTOR_GEAR_RATIO, + robot.DRIVETRAIN_WHEEL_DIAMETER_MM, + DistanceUnit.MM, + robot.DRIVETRAIN_MAX_VELOCITY_MM); - double maxPower = 1.0; + robot.backLeft.setVelocity(frontLeftPower * velocity); + robot.frontLeft.setVelocity(backLeftPower * velocity); + robot.frontRight.setVelocity(frontRightPower * velocity); + robot.backRight.setVelocity(backRightPower * velocity); + } - robot.leftFront.motorEx.setPower(frontLeftPower * maxPower); - robot.leftBack.motorEx.setPower(backLeftPower * maxPower); - robot.rightFront.motorEx.setPower(frontRightPower * maxPower); - robot.rightBack.motorEx.setPower(backRightPower * maxPower); + private void armController() + { + double armPower = engine.gamepad1.right_trigger - engine.gamepad1.left_trigger; + if (armPower > 0) { + if (robot.armEndStopLeft.isPressed() || robot.armEndStopRight.isPressed()) + armPower = 0; + } + + robot.arm.setPower(armPower); + } + + private void clawControllers() + { + robot.leftClaw.setPosition(leftClawOpen ? robot.LEFT_CLAW_OPEN_POSITION : robot.LEFT_CLAW_CLOSED_POSITION); + robot.rightClaw.setPosition(rightClawOpen ? robot.RIGHT_CLAW_OPEN_POSITION : robot.RIGHT_CLAW_CLOSED_POSITION); + } + + private void droneLatchController() + {} + + @Override + public void telemetry() { + robot.standardTelemetry(); + robot.teleopTelemetry(); + } + + @Override + public void buttonDown(Gamepad gamepad, String button) { + if (gamepad == engine.gamepad1) + { + switch (button) { + case "guide": + /// --- IMU Reset --- /// + robot.imu.resetYaw(); + break; + case "start": + robot.reloadConfig(); + break; + case "left_bumper": + leftClawOpen = !leftClawOpen; + break; + case "right_bumper": + rightClawOpen = !rightClawOpen; + break; + case "y": + /// DRONE LATCH /// + robot.droneLatch.setPosition(robot.DRONE_LATCH_LAUNCH_POSITION); + break; + case "a": + /// DRONE LATCH /// + robot.droneLatch.setPosition(robot.DRONE_LATCH_INITIAL_POSITION); + break; + } + } } } diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Rotate.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Rotate.java index 3594496..f0a7e8e 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Rotate.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Rotate.java @@ -47,40 +47,40 @@ public class Rotate extends CyberarmState { @Override public void exec() { - if (Math.abs(robot.angleDiff(robot.facing(), heading)) <= tolerance) { - robot.left.set(0); - robot.right.set(0); - - setHasFinished(true); - - return; - } - - if (runTime() >= timeoutMS) { - robot.left.set(0); - robot.right.set(0); - - setHasFinished(true); - } - - double angleDiff = robot.angleDiff(robot.facing() + robot.turnRate(), heading); - double velocity = robot.lerp(minVelocity, maxVelocity, Range.clip(Math.abs(angleDiff) / 90.0, 0.0, 1.0)); - - if (angleDiff > 0) { - robot.left.set(-velocity); - robot.right.set(velocity); - } else { - robot.left.set(velocity); - robot.right.set(-velocity); - } +// if (Math.abs(robot.angleDiff(robot.facing(), heading)) <= tolerance) { +// robot.left.set(0); +// robot.right.set(0); +// +// setHasFinished(true); +// +// return; +// } +// +// if (runTime() >= timeoutMS) { +// robot.left.set(0); +// robot.right.set(0); +// +// setHasFinished(true); +// } +// +// double angleDiff = robot.angleDiff(robot.facing() + robot.turnRate(), heading); +// double velocity = robot.lerp(minVelocity, maxVelocity, Range.clip(Math.abs(angleDiff) / 90.0, 0.0, 1.0)); +// +// if (angleDiff > 0) { +// robot.left.set(-velocity); +// robot.right.set(velocity); +// } else { +// robot.left.set(velocity); +// robot.right.set(-velocity); +// } } @Override public void telemetry() { - engine.telemetry.addLine(); - engine.telemetry.addData("Robot Heading", robot.facing()); - engine.telemetry.addData("Robot Target Heading", heading); - engine.telemetry.addData("Robot Angle Diff", robot.angleDiff(robot.facing() + robot.turnRate(), heading)); - engine.telemetry.addData("Robot Turn Rate", robot.turnRate()); +// engine.telemetry.addLine(); +// engine.telemetry.addData("Robot Heading", robot.facing()); +// engine.telemetry.addData("Robot Target Heading", heading); +// engine.telemetry.addData("Robot Angle Diff", robot.angleDiff(robot.facing() + robot.turnRate(), heading)); +// engine.telemetry.addData("Robot Turn Rate", robot.turnRate()); } } diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/StrafeMove.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/StrafeMove.java index 95bdfe7..3d5d661 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/StrafeMove.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/StrafeMove.java @@ -38,26 +38,26 @@ public class StrafeMove extends CyberarmState { @Override public void start() { - robot.leftFront.setTargetDistance(distanceMM); - - robot.left.setPositionTolerance(tolerance); - robot.right.setPositionTolerance(tolerance); - - double motorVelocity = (distanceMM < 0 ? velocity * -1 : velocity); - - robot.leftFront.set(motorVelocity); - robot.rightFront.set(-motorVelocity); - - robot.leftBack.set(-motorVelocity); - robot.rightBack.set(motorVelocity); +// robot.leftFront.setTargetDistance(distanceMM); +// +// robot.left.setPositionTolerance(tolerance); +// robot.right.setPositionTolerance(tolerance); +// +// double motorVelocity = (distanceMM < 0 ? velocity * -1 : velocity); +// +// robot.leftFront.set(motorVelocity); +// robot.rightFront.set(-motorVelocity); +// +// robot.leftBack.set(-motorVelocity); +// robot.rightBack.set(motorVelocity); } @Override public void exec() { - if (robot.left.atTargetPosition() || Math.abs(robot.leftFront.getDistance()) >= Math.abs(distanceMM) || runTime() >= timeoutMS) { - robot.left.set(0); - robot.right.set(0); - setHasFinished(true); - } +// if (robot.left.atTargetPosition() || Math.abs(robot.leftFront.getDistance()) >= Math.abs(distanceMM) || runTime() >= timeoutMS) { +// robot.left.set(0); +// robot.right.set(0); +// setHasFinished(true); +// } } } diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/TankMove.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/TankMove.java index e797bfe..12ffe22 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/TankMove.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/TankMove.java @@ -43,51 +43,51 @@ public class TankMove extends CyberarmState { @Override public void start() { - robot.left.resetEncoder(); - robot.right.resetEncoder(); - - robot.left.setTargetDistance(leftDistanceMM); - robot.right.setTargetDistance(rightDistanceMM); - - robot.left.setPositionTolerance(tolerance); - robot.right.setPositionTolerance(tolerance); - - robot.left.set(leftVelocity); - robot.right.set(rightVelocity); +// robot.left.resetEncoder(); +// robot.right.resetEncoder(); +// +// robot.left.setTargetDistance(leftDistanceMM); +// robot.right.setTargetDistance(rightDistanceMM); +// +// robot.left.setPositionTolerance(tolerance); +// robot.right.setPositionTolerance(tolerance); +// +// robot.left.set(leftVelocity); +// robot.right.set(rightVelocity); } @Override public void exec() { - if (robot.left.atTargetPosition() || Math.abs(robot.leftFront.getDistance()) >= Math.abs(leftDistanceMM)) { - robot.left.set(0); - } - if (robot.right.atTargetPosition() || Math.abs(robot.rightFront.getDistance()) >= Math.abs(leftDistanceMM)) { - robot.right.set(0); - } - - if ( - (robot.left.atTargetPosition() && robot.right.atTargetPosition()) || - (Math.abs(robot.leftFront.getDistance()) >= Math.abs(leftDistanceMM) && robot.right.atTargetPosition() || Math.abs(robot.rightFront.getDistance()) >= Math.abs(leftDistanceMM)) || - runTime() >= timeoutMS) { - robot.left.set(0); - robot.right.set(0); - setHasFinished(true); - } +// if (robot.left.atTargetPosition() || Math.abs(robot.leftFront.getDistance()) >= Math.abs(leftDistanceMM)) { +// robot.left.set(0); +// } +// if (robot.right.atTargetPosition() || Math.abs(robot.rightFront.getDistance()) >= Math.abs(leftDistanceMM)) { +// robot.right.set(0); +// } +// +// if ( +// (robot.left.atTargetPosition() && robot.right.atTargetPosition()) || +// (Math.abs(robot.leftFront.getDistance()) >= Math.abs(leftDistanceMM) && robot.right.atTargetPosition() || Math.abs(robot.rightFront.getDistance()) >= Math.abs(leftDistanceMM)) || +// runTime() >= timeoutMS) { +// robot.left.set(0); +// robot.right.set(0); +// setHasFinished(true); +// } } @Override public void telemetry() { - engine.telemetry.addLine(); - - engine.telemetry.addData("Left distance", robot.leftFront.getDistance()); - engine.telemetry.addData("Left position", robot.left.getPositions().get(0)); - engine.telemetry.addData("Left speed", robot.left.getSpeeds().get(0)); - engine.telemetry.addData("Left velocity", robot.left.getVelocity()); - engine.telemetry.addLine(); - - engine.telemetry.addData("Right distance", robot.rightFront.getDistance()); - engine.telemetry.addData("Right position", robot.right.getPositions().get(0)); - engine.telemetry.addData("Right speed", robot.right.getSpeeds().get(0)); - engine.telemetry.addData("Right velocity", robot.right.getVelocity()); +// engine.telemetry.addLine(); +// +// engine.telemetry.addData("Left distance", robot.leftFront.getDistance()); +// engine.telemetry.addData("Left position", robot.left.getPositions().get(0)); +// engine.telemetry.addData("Left speed", robot.left.getSpeeds().get(0)); +// engine.telemetry.addData("Left velocity", robot.left.getVelocity()); +// engine.telemetry.addLine(); +// +// engine.telemetry.addData("Right distance", robot.rightFront.getDistance()); +// engine.telemetry.addData("Right position", robot.right.getPositions().get(0)); +// engine.telemetry.addData("Right speed", robot.right.getSpeeds().get(0)); +// engine.telemetry.addData("Right velocity", robot.right.getVelocity()); } }