From e978a0073e2ee2dfcabbf4cf898e048191e336d7 Mon Sep 17 00:00:00 2001 From: soh226 <85522920+soh226@users.noreply.github.com> Date: Thu, 24 Feb 2022 18:09:09 -0800 Subject: [PATCH] All recent codes Recent code updated after meet #3 - 2/24/2024 --- .../teamcode/AutoBlueCarouselWarehouse.java | 979 ++++++++------- .../ftc/teamcode/AutoBlueForward.java | 1060 +++++++++-------- .../ftc/teamcode/AutoBlueForwardNoHub.java | 567 +++++++++ .../ftc/teamcode/AutoBlueWarehouse.java | 1026 ++++++++-------- .../ftc/teamcode/AutoBlueWarehouseNoHub.java | 541 +++++++++ .../ftc/teamcode/AutoRedForward.java | 1055 ++++++++-------- .../ftc/teamcode/AutoRedForwardNoHub.java | 565 +++++++++ .../ftc/teamcode/AutoRedWarehouse.java | 1036 ++++++++-------- .../ftc/teamcode/AutoRedWarehouseNoHub.java | 550 +++++++++ .../ftc/teamcode/Manual_Complete_Blue.java | 237 ++++ .../ftc/teamcode/Manual_Complete_Red.java | 195 +++ .../firstinspires/ftc/teamcode/NFMyRobot.java | 261 ++-- .../org/firstinspires/ftc/teamcode/readme.md | 2 +- 13 files changed, 5512 insertions(+), 2562 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueForwardNoHub.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueWarehouseNoHub.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedForwardNoHub.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedWarehouseNoHub.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Manual_Complete_Blue.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Manual_Complete_Red.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueCarouselWarehouse.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueCarouselWarehouse.java index 3238a3a7afb7..13ec9f4cdd16 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueCarouselWarehouse.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueCarouselWarehouse.java @@ -1,491 +1,490 @@ -/* Copyright (c) 2017 FIRST. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted (subject to the limitations in the disclaimer below) provided that - * the following conditions are met: - * - * Redistributions of source code must retain the above copyright notice, this list - * of conditions and the following disclaimer. - * - * Redistributions in binary form must reproduce the above copyright notice, this - * list of conditions and the following disclaimer in the documentation and/or - * other materials provided with the distribution. - * - * Neither the name of FIRST nor the names of its contributors may be used to endorse or - * promote products derived from this software without specific prior written permission. - * - * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS - * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.util.ElapsedTime; -import com.qualcomm.robotcore.util.RobotLog; - -/** - * - * The desired path in this example is: - * - Drive forward for 48 inches - * - Spin right for 12 Inches - * - Drive Backwards for 24 inches - * - Stop and close the claw. - * - * The code is written using a method called: encoderDrive(speed, leftInches, rightInches, timeoutS) - * that performs the actual movement. - * This methods assumes that each movement is relative to the last stopping place. - * There are other ways to perform encoder based moves, but this method is probably the simplest. - * This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile - * - * Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name. - * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list - */ - - -@Autonomous(name="Autonomous: AutoBlueCarouselWarehouse", group="Autonomous") -//@Disabled -public class AutoBlueCarouselWarehouse extends LinearOpMode { - - public enum Direction { - FORWARD, - BACKWARD, - RIGHT, - LEFT, - F_RIGHT_DIG, - F_LEFT_DIG, - B_RIGHT_DIG, - B_LEFT_DIG, - CLOCK_WISE_TURN, - ANTI_CLOCK_WISE_TURN - } - public enum TEST_MODE { - WORKING1, - WORKING2, - WORKING3, - WORKING4, - TEST1, - TEST2, - TEST3, - TEST4 - } - - /* Declare OpMode members. */ - NFMyRobot robot = new NFMyRobot(); // Use NF my Robot h/w - - private final ElapsedTime runtime = new ElapsedTime(); - - //Encoder produced TICK COUNTS per revolution - static final double COUNTS_PER_MOTOR_REV = 537.7; // eg: TETRIX Motor Encoder - 1440, REV Hex Motors: 2240 - static final double DRIVE_GEAR_REDUCTION = 1; //2.0; // This is < 1.0 if geared UP - static final double WHEEL_DIAMETER_INCHES = 3.7; // For figuring circumference - static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / - (WHEEL_DIAMETER_INCHES * 3.1428); - static final double COUNTS_FULL_TURN = 72; - static final int ENCODER_COUNT_BEFORE_STOP = 140; //slow down before 3" - - static TEST_MODE TEST_RUN_TYPE = TEST_MODE.WORKING1; - - //static final double DRIVE_SPEED = 1; - //static final double TURN_SPEED = 0.5; - - // Declare our motors - // Make sure your ID's match your configuration - //DcMotor motorFrontLeft = null; - //DcMotor motorBackLeft = null; - //DcMotor motorFrontRight = null; - //DcMotor motorBackRight = null; - - int DuckCount; - double distance; - - @Override - public void runOpMode() /*throws InterruptedException*/ { - Direction dir; - distance = 0; - DuckCount = 0; - /* - * Initialize the drive system variables. - * The init() method of the hardware class does all the work here - */ - robot.init(hardwareMap); - runtime.reset(); - - // Send telemetry message to signify robot waiting; - telemetry.addData("Status", "Starting Autorun"); //Auto run - telemetry.update(); - - // Reset Encoder - robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - - // Set Encoder - robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.motorBackRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - - robot.motorBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - robot.motorBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - robot.motorFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - robot.motorFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - // Send telemetry message to indicate successful Encoder reset - telemetry.addData("Path0", "Position: Front L:%7d R:%7d, Back L:%7d R:%7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - telemetry.update(); - - // Reverse the right side motors - robot.motorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); - robot.motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE); - - // Wait for the game to start (driver presses PLAY) - waitForStart(); - - if (isStopRequested()) return; - - if (TEST_RUN_TYPE == TEST_MODE.WORKING1) - { - myEncoderDrive(Direction.RIGHT, 0.1, 1.5, 2000); - myEncoderTurn(0.2, 5); - myEncoderDrive(Direction.BACKWARD, 0.50, 22, 2000); - myEncoderDrive(Direction.BACKWARD, 0.35, 8, 2000); - spinCarousel(2000,500); - myEncoderDrive(Direction.FORWARD, 0.35, 8, 2000); - myEncoderTurn(0.2, -10); - myEncoderDrive(Direction.LEFT, 0.1, 2.75, 2000); - myEncoderDrive(Direction.FORWARD, 0.6 , 92.5, 10000); - myEncoderDrive(Direction.RIGHT, 0.6, 30, 5000); - myEncoderDrive(Direction.FORWARD, 0.5, 12, 2000); - - - }; - - sleep(100); // pause for servos to move - - telemetry.addData("Path", "Complete"); - telemetry.update(); - } - - public Direction moveRobot(double x1, double y1, double currOrient, - double x2, double y2, double newOrient, double speed) - { - double angle; - double turn_angle; - Direction direction = Direction.FORWARD; - - // angle will return Anti-clockwise value [-90, 90] - // Example A(-12,50), B(-60, 60). Anglex2) { - // Go backward to (x2,y2) - direction = Direction.BACKWARD; - } else if (x2>x1) { - // Go Forward to (x2, y2) - direction = Direction.FORWARD; - } else if (y1>y2) { - // Go Backward to (x2, y2) - direction = Direction.BACKWARD; - } else { - // Go Forward to (x2, y2) - direction = Direction.FORWARD; - } - - myEncoderDrive(direction, speed, distance, 1000); - myEncoderTurn(speed, newOrient - angle); - - /* - // Move 60+ degree Anti-CLOCKWISE - if (turn_angle < -180) { - turn_angle = turn_angle + 180; - myEncoderTurn(0.4, turn_angle); - myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); - myEncoderTurn(0.4, newOrient - angle -180); - } else if(turn_angle > 180) { - turn_angle = turn_angle - 180; - myEncoderTurn(0.4, turn_angle); - myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); - myEncoderTurn(0.4, newOrient - angle + 180); - } else { - myEncoderTurn(0.4, turn_angle); - // Move FWD - myEncoderDrive(Direction.FORWARD, 0.75, distance, 1000); - myEncoderTurn(0.4, newOrient - angle); - } - */ - return direction; - } - - // Run Carousel - public void spinCarousel(double timeout, long sleeptime) - { - runtime.reset(); - - robot.motorCarouselSpin.setPower(0.66); - while (opModeIsActive() && - runtime.milliseconds() < timeout) - { - telemetry.addData("NFAuto", "Running Carousel for Duck for %f milisec", timeout); - telemetry.update(); - } - robot.motorCarouselSpin.setPower(0); - //sleep(sleeptime); - DuckCount++; - RobotLog.ii("NFAuto", "Number of Duck delivered: %d, timeout %f", DuckCount, timeout); - } - - public void myEncoderTurn(double speed, double degree) - { - double inch_cnt; - - if (Math.abs(degree) < 1) return; - - // Adjust to turn less than 180 - if (degree < -180) { - degree = degree + 360; - } - if (degree > 180) { - degree = degree - 360; - } - - inch_cnt = degree*(COUNTS_FULL_TURN/360); - RobotLog.ii("Input:myEncoderTurn", "Speed/Degree/inch_cnt, %f, %f %f", - speed, degree, inch_cnt); - - if (degree >= 0) { - myEncoderDrive(Direction.ANTI_CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); - } else { - myEncoderDrive(Direction.CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); - } - } - - public void myEncoderDrive(Direction direction, double speed, double Inches, - double timeoutS) { //SensorsToUse sensors_2_use) - int newFrontLeftTarget = 0; - int newFrontRightTarget = 0; - int newBackLeftTarget = 0; - int newBackRightTarget = 0; - int remainingDistance; - int cnt = 0; - double new_speed = 0; - - - RobotLog.ii("Input", "Enter - myEncoderDrive - speed=%f, Inches=%f, timeout=%f", - speed, Inches, timeoutS); - telemetry.addData("Path1", "Enter - myEncoderDrive - speed=%f," + - " Inches=%f, timeout=%f", speed, Inches, timeoutS); - telemetry.update(); - - // Turn off ENCODER - // Reset Encoder beginning to see it gets better. - robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - - RobotLog.ii("Path0", "Starting Position: Front L:%7d R:%7d, Back L:%7d R:%7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - - // Ensure that the op mode is still active - if (opModeIsActive() && !isStopRequested()) { - - // Determine new target position, and pass to motor controller - if (direction == Direction.BACKWARD) { - //Go forward - RobotLog.ii("NFusion", "Moving BACKWARD.... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else if (direction == Direction.FORWARD) { - //Go backward - RobotLog.ii("NFusion", "Moving FORWARD..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.LEFT) { - //Move Right - RobotLog.ii("NFusion", "Moving LEFT..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else if (direction == Direction.RIGHT) { - //Move Left - RobotLog.ii("NFusion", "Moving RIGHT..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.F_RIGHT_DIG) { - //Forward Left Diagonal - RobotLog.ii("NFusion", "Moving F_RIGHT_DIG ..... [%f inches]..", Inches); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else if (direction == Direction.F_LEFT_DIG) { - //Move Forward Right Diagonal - RobotLog.ii("NFusion", "Moving F_LEFT_DIG..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else if (direction == Direction.B_RIGHT_DIG){ - //Move Backward Left Diagonal - RobotLog.ii("NFusion", "Moving B_RIGHT_DIG..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.B_LEFT_DIG) { - //Backward Right Diagonal - RobotLog.ii("NFusion", "Moving B_LEFT_DIG ..... [%f inches]..", Inches); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.ANTI_CLOCK_WISE_TURN) { - // Turn Clock Wise - RobotLog.ii("NFusion", "Turn Anti Clockwise ..... [%f ms]..", timeoutS); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.CLOCK_WISE_TURN) { - // Turn Clock Wise - RobotLog.ii("NFusion", "Turn Clockwise ..... [%f ms]..", timeoutS); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else { - // Do Not move - speed = 0; - } - - // Set the Encoder to the target position. - robot.motorFrontLeft.setTargetPosition(newFrontLeftTarget); - robot.motorFrontRight.setTargetPosition(newFrontRightTarget); - robot.motorBackLeft.setTargetPosition(newBackLeftTarget); - robot.motorBackRight.setTargetPosition(newBackRightTarget); - - // Turn On RUN_TO_POSITION - robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); - robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); - robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); - robot.motorBackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); - - // Set power for the motors. - robot.motorFrontLeft.setPower(Math.abs(speed)); - robot.motorFrontRight.setPower(Math.abs(speed)); - robot.motorBackLeft.setPower(Math.abs(speed)); - robot.motorBackRight.setPower(Math.abs(speed)); - - // reset the timeout time and start motion. - runtime.reset(); - - RobotLog.ii("NFusion", "Final Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", - newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); - - while (opModeIsActive() && - (runtime.milliseconds() < timeoutS) && - ((robot.motorFrontLeft.isBusy() || robot.motorFrontRight.isBusy()) && - (robot.motorBackLeft.isBusy() || robot.motorBackRight.isBusy()))) { - // Display it for the driver every 10 ms - if(runtime.milliseconds() > cnt * 30 ) { // Print every 30 ms - RobotLog.ii("NFusion", "Current Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - telemetry.addData("Path1", "Running to: FL: %7d FR: %7d BL: %7d BR: %7d", - newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); - telemetry.addData("Path2", "Running at: FL: %7d FR: %7d BL: %7d BR: %7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - telemetry.update(); - cnt++; - } - // Reduce the Speed before stopping - remainingDistance = Math.abs(newFrontLeftTarget-robot.motorFrontLeft.getCurrentPosition()); - if (remainingDistance < 10) - { - remainingDistance = Math.abs(newFrontRightTarget-robot.motorFrontRight.getCurrentPosition()); - } - if ((remainingDistance < ENCODER_COUNT_BEFORE_STOP) && (remainingDistance >= 10)){ - new_speed = Math.abs(speed)*(remainingDistance/(float)ENCODER_COUNT_BEFORE_STOP); - robot.motorFrontLeft.setPower(new_speed); - robot.motorFrontRight.setPower(new_speed); - robot.motorBackLeft.setPower(new_speed); - robot.motorBackRight.setPower(new_speed); - if((cnt % 5) == 0) { // skip 5 cnt and print - RobotLog.ii("NFusion", "Remaining Dist: %7d, Speed %f, new Speed %f", - remainingDistance, speed, new_speed); - } - } - } - - //RobotLog.ii("NFusion", "Motor Encoder Status FL: %d, FR: %d, BL: %d, BR: %d", - // motorFrontLeft.isBusy(), - // motorFrontRight.isBusy(), - // motorBackLeft.isBusy(), - //motorBackRight.isBusy()); - - // Stop all motion; - robot.motorFrontLeft.setPower(0); - robot.motorFrontRight.setPower(0); - robot.motorBackLeft.setPower(0); - robot.motorBackRight.setPower(0); - - //sleep(500); // optional pause after each move - - // Turn off ENCODER - // Reset Encoder - //robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - //robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - //robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - //robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - - RobotLog.ii("Path0", "Exit - myEncoderDrive Last Position: Front L:%7d R:%7d, Back L:%7d R:%7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - - //telemetry.addData("Status", "Movement Distance: %7d", - // distance_traveled); - //telemetry.update(); - - } - } +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.RobotLog; + +/** + * + * The desired path in this example is: + * - Drive forward for 48 inches + * - Spin right for 12 Inches + * - Drive Backwards for 24 inches + * - Stop and close the claw. + * + * The code is written using a method called: encoderDrive(speed, leftInches, rightInches, timeoutS) + * that performs the actual movement. + * This methods assumes that each movement is relative to the last stopping place. + * There are other ways to perform encoder based moves, but this method is probably the simplest. + * This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile + * + * Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list + */ + + +@Autonomous(name="Autonomous: AutoBlueCarouselWarehouse", group="Autonomous") +//@Disabled +public class AutoBlueCarouselWarehouse extends LinearOpMode { + + public enum Direction { + FORWARD, + BACKWARD, + RIGHT, + LEFT, + F_RIGHT_DIG, + F_LEFT_DIG, + B_RIGHT_DIG, + B_LEFT_DIG, + CLOCK_WISE_TURN, + ANTI_CLOCK_WISE_TURN + } + public enum TEST_MODE { + WORKING1, + WORKING2, + WORKING3, + WORKING4, + TEST1, + TEST2, + TEST3, + TEST4 + } + + /* Declare OpMode members. */ + NFMyRobot robot = new NFMyRobot(); // Use NF my Robot h/w + + private final ElapsedTime runtime = new ElapsedTime(); + + //Encoder produced TICK COUNTS per revolution + static final double COUNTS_PER_MOTOR_REV = 537.7; // eg: TETRIX Motor Encoder - 1440, REV Hex Motors: 2240 + static final double DRIVE_GEAR_REDUCTION = 1; //2.0; // This is < 1.0 if geared UP + static final double WHEEL_DIAMETER_INCHES = 3.7; // For figuring circumference + static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / + (WHEEL_DIAMETER_INCHES * 3.1428); + static final double COUNTS_FULL_TURN = 72; + static final int ENCODER_COUNT_BEFORE_STOP = 140; //slow down before 3" + + static TEST_MODE TEST_RUN_TYPE = TEST_MODE.WORKING1; + + //static final double DRIVE_SPEED = 1; + //static final double TURN_SPEED = 0.5; + + // Declare our motors + // Make sure your ID's match your configuration + //DcMotor motorFrontLeft = null; + //DcMotor motorBackLeft = null; + //DcMotor motorFrontRight = null; + //DcMotor motorBackRight = null; + + int DuckCount; + double distance; + + @Override + public void runOpMode() /*throws InterruptedException*/ { + Direction dir; + distance = 0; + DuckCount = 0; + /* + * Initialize the drive system variables. + * The init() method of the hardware class does all the work here + */ + robot.init(hardwareMap); + runtime.reset(); + + // Send telemetry message to signify robot waiting; + telemetry.addData("Status", "Starting Autorun"); //Auto run + telemetry.update(); + + // Reset Encoder + robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + // Set Encoder + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + robot.motorBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + // Send telemetry message to indicate successful Encoder reset + telemetry.addData("Path0", "Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.update(); + + // Reverse the right side motors + robot.motorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); + robot.motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE); + + // Wait for the game to start (driver presses PLAY) + waitForStart(); + + if (isStopRequested()) return; + + if (TEST_RUN_TYPE == TEST_MODE.WORKING1) + { + myEncoderDrive(Direction.RIGHT, 0.1, 1.5, 2000); + myEncoderTurn(0.2, 6); + myEncoderDrive(Direction.BACKWARD, 0.50, 22, 2000); + myEncoderDrive(Direction.BACKWARD, 0.20, 6, 2000); + spinCarousel(2500,500); + myEncoderDrive(Direction.FORWARD, 0.35, 24, 2000); + myEncoderTurn(0.2, -3); + myEncoderDrive(Direction.LEFT, 0.1, 2.75, 2000); + myEncoderDrive(Direction.FORWARD, 0.6 , 72, 10000); + myEncoderDrive(Direction.RIGHT, 0.6, 30, 5000); + myEncoderDrive(Direction.FORWARD, 0.5, 12, 2000); + + }; + + sleep(100); // pause for servos to move + + telemetry.addData("Path", "Complete"); + telemetry.update(); + } + + public Direction moveRobot(double x1, double y1, double currOrient, + double x2, double y2, double newOrient, double speed) + { + double angle; + double turn_angle; + Direction direction = Direction.FORWARD; + + // angle will return Anti-clockwise value [-90, 90] + // Example A(-12,50), B(-60, 60). Anglex2) { + // Go backward to (x2,y2) + direction = Direction.BACKWARD; + } else if (x2>x1) { + // Go Forward to (x2, y2) + direction = Direction.FORWARD; + } else if (y1>y2) { + // Go Backward to (x2, y2) + direction = Direction.BACKWARD; + } else { + // Go Forward to (x2, y2) + direction = Direction.FORWARD; + } + + myEncoderDrive(direction, speed, distance, 1000); + myEncoderTurn(speed, newOrient - angle); + + /* + // Move 60+ degree Anti-CLOCKWISE + if (turn_angle < -180) { + turn_angle = turn_angle + 180; + myEncoderTurn(0.4, turn_angle); + myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle -180); + } else if(turn_angle > 180) { + turn_angle = turn_angle - 180; + myEncoderTurn(0.4, turn_angle); + myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle + 180); + } else { + myEncoderTurn(0.4, turn_angle); + // Move FWD + myEncoderDrive(Direction.FORWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle); + } + */ + return direction; + } + + // Run Carousel + public void spinCarousel(double timeout, long sleeptime) + { + runtime.reset(); + + robot.motorCarouselSpin.setPower(0.55); + while (opModeIsActive() && + runtime.milliseconds() < timeout) + { + telemetry.addData("NFAuto", "Running Carousel for Duck for %f milisec", timeout); + telemetry.update(); + } + robot.motorCarouselSpin.setPower(0); + //sleep(sleeptime); + DuckCount++; + RobotLog.ii("NFAuto", "Number of Duck delivered: %d, timeout %f", DuckCount, timeout); + } + + public void myEncoderTurn(double speed, double degree) + { + double inch_cnt; + + if (Math.abs(degree) < 1) return; + + // Adjust to turn less than 180 + if (degree < -180) { + degree = degree + 360; + } + if (degree > 180) { + degree = degree - 360; + } + + inch_cnt = degree*(COUNTS_FULL_TURN/360); + RobotLog.ii("Input:myEncoderTurn", "Speed/Degree/inch_cnt, %f, %f %f", + speed, degree, inch_cnt); + + if (degree >= 0) { + myEncoderDrive(Direction.ANTI_CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); + } else { + myEncoderDrive(Direction.CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); + } + } + + public void myEncoderDrive(Direction direction, double speed, double Inches, + double timeoutS) { //SensorsToUse sensors_2_use) + int newFrontLeftTarget = 0; + int newFrontRightTarget = 0; + int newBackLeftTarget = 0; + int newBackRightTarget = 0; + int remainingDistance; + int cnt = 0; + double new_speed = 0; + + + RobotLog.ii("Input", "Enter - myEncoderDrive - speed=%f, Inches=%f, timeout=%f", + speed, Inches, timeoutS); + telemetry.addData("Path1", "Enter - myEncoderDrive - speed=%f," + + " Inches=%f, timeout=%f", speed, Inches, timeoutS); + telemetry.update(); + + // Turn off ENCODER + // Reset Encoder beginning to see it gets better. + robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + RobotLog.ii("Path0", "Starting Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + + // Ensure that the op mode is still active + if (opModeIsActive() && !isStopRequested()) { + + // Determine new target position, and pass to motor controller + if (direction == Direction.BACKWARD) { + //Go forward + RobotLog.ii("NFusion", "Moving BACKWARD.... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.FORWARD) { + //Go backward + RobotLog.ii("NFusion", "Moving FORWARD..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.LEFT) { + //Move Right + RobotLog.ii("NFusion", "Moving LEFT..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.RIGHT) { + //Move Left + RobotLog.ii("NFusion", "Moving RIGHT..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.F_RIGHT_DIG) { + //Forward Left Diagonal + RobotLog.ii("NFusion", "Moving F_RIGHT_DIG ..... [%f inches]..", Inches); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.F_LEFT_DIG) { + //Move Forward Right Diagonal + RobotLog.ii("NFusion", "Moving F_LEFT_DIG..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.B_RIGHT_DIG){ + //Move Backward Left Diagonal + RobotLog.ii("NFusion", "Moving B_RIGHT_DIG..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.B_LEFT_DIG) { + //Backward Right Diagonal + RobotLog.ii("NFusion", "Moving B_LEFT_DIG ..... [%f inches]..", Inches); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.ANTI_CLOCK_WISE_TURN) { + // Turn Clock Wise + RobotLog.ii("NFusion", "Turn Anti Clockwise ..... [%f ms]..", timeoutS); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.CLOCK_WISE_TURN) { + // Turn Clock Wise + RobotLog.ii("NFusion", "Turn Clockwise ..... [%f ms]..", timeoutS); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else { + // Do Not move + speed = 0; + } + + // Set the Encoder to the target position. + robot.motorFrontLeft.setTargetPosition(newFrontLeftTarget); + robot.motorFrontRight.setTargetPosition(newFrontRightTarget); + robot.motorBackLeft.setTargetPosition(newBackLeftTarget); + robot.motorBackRight.setTargetPosition(newBackRightTarget); + + // Turn On RUN_TO_POSITION + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + // Set power for the motors. + robot.motorFrontLeft.setPower(Math.abs(speed)); + robot.motorFrontRight.setPower(Math.abs(speed)); + robot.motorBackLeft.setPower(Math.abs(speed)); + robot.motorBackRight.setPower(Math.abs(speed)); + + // reset the timeout time and start motion. + runtime.reset(); + + RobotLog.ii("NFusion", "Final Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", + newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); + + while (opModeIsActive() && + (runtime.milliseconds() < timeoutS) && + ((robot.motorFrontLeft.isBusy() || robot.motorFrontRight.isBusy()) && + (robot.motorBackLeft.isBusy() || robot.motorBackRight.isBusy()))) { + // Display it for the driver every 10 ms + if(runtime.milliseconds() > cnt * 30 ) { // Print every 30 ms + RobotLog.ii("NFusion", "Current Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.addData("Path1", "Running to: FL: %7d FR: %7d BL: %7d BR: %7d", + newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); + telemetry.addData("Path2", "Running at: FL: %7d FR: %7d BL: %7d BR: %7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.update(); + cnt++; + } + // Reduce the Speed before stopping + remainingDistance = Math.abs(newFrontLeftTarget-robot.motorFrontLeft.getCurrentPosition()); + if (remainingDistance < 10) + { + remainingDistance = Math.abs(newFrontRightTarget-robot.motorFrontRight.getCurrentPosition()); + } + if ((remainingDistance < ENCODER_COUNT_BEFORE_STOP) && (remainingDistance >= 10)){ + new_speed = Math.abs(speed)*(remainingDistance/(float)ENCODER_COUNT_BEFORE_STOP); + robot.motorFrontLeft.setPower(new_speed); + robot.motorFrontRight.setPower(new_speed); + robot.motorBackLeft.setPower(new_speed); + robot.motorBackRight.setPower(new_speed); + if((cnt % 5) == 0) { // skip 5 cnt and print + RobotLog.ii("NFusion", "Remaining Dist: %7d, Speed %f, new Speed %f", + remainingDistance, speed, new_speed); + } + } + } + + //RobotLog.ii("NFusion", "Motor Encoder Status FL: %d, FR: %d, BL: %d, BR: %d", + // motorFrontLeft.isBusy(), + // motorFrontRight.isBusy(), + // motorBackLeft.isBusy(), + //motorBackRight.isBusy()); + + // Stop all motion; + robot.motorFrontLeft.setPower(0); + robot.motorFrontRight.setPower(0); + robot.motorBackLeft.setPower(0); + robot.motorBackRight.setPower(0); + + //sleep(500); // optional pause after each move + + // Turn off ENCODER + // Reset Encoder + //robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + RobotLog.ii("Path0", "Exit - myEncoderDrive Last Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + + //telemetry.addData("Status", "Movement Distance: %7d", + // distance_traveled); + //telemetry.update(); + + } + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueForward.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueForward.java index ecc6f8e16731..f17ce1345df4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueForward.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueForward.java @@ -1,492 +1,570 @@ -/* Copyright (c) 2017 FIRST. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted (subject to the limitations in the disclaimer below) provided that - * the following conditions are met: - * - * Redistributions of source code must retain the above copyright notice, this list - * of conditions and the following disclaimer. - * - * Redistributions in binary form must reproduce the above copyright notice, this - * list of conditions and the following disclaimer in the documentation and/or - * other materials provided with the distribution. - * - * Neither the name of FIRST nor the names of its contributors may be used to endorse or - * promote products derived from this software without specific prior written permission. - * - * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS - * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.util.ElapsedTime; -import com.qualcomm.robotcore.util.RobotLog; -import org.firstinspires.ftc.teamcode.NFMyRobot; - -/** - * - * The desired path in this example is: - * - Drive forward for 48 inches - * - Spin right for 12 Inches - * - Drive Backwards for 24 inches - * - Stop and close the claw. - * - * The code is written using a method called: encoderDrive(speed, leftInches, rightInches, timeoutS) - * that performs the actual movement. - * This methods assumes that each movement is relative to the last stopping place. - * There are other ways to perform encoder based moves, but this method is probably the simplest. - * This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile - * - * Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name. - * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list - */ - - -@Autonomous(name="Autonomous: AutoBlueForward", group="Autonomous") -//@Disabled -public class AutoBlueForward extends LinearOpMode { - - public enum Direction { - FORWARD, - BACKWARD, - RIGHT, - LEFT, - F_RIGHT_DIG, - F_LEFT_DIG, - B_RIGHT_DIG, - B_LEFT_DIG, - CLOCK_WISE_TURN, - ANTI_CLOCK_WISE_TURN - } - public enum TEST_MODE { - WORKING1, - WORKING2, - WORKING3, - WORKING4, - TEST1, - TEST2, - TEST3, - TEST4 - } - - /* Declare OpMode members. */ - NFMyRobot robot = new NFMyRobot(); // Use NF my Robot h/w - - private final ElapsedTime runtime = new ElapsedTime(); - - //Encoder produced TICK COUNTS per revolution - static final double COUNTS_PER_MOTOR_REV = 537.7; // eg: TETRIX Motor Encoder - 1440, REV Hex Motors: 2240 - static final double DRIVE_GEAR_REDUCTION = 1; //2.0; // This is < 1.0 if geared UP - static final double WHEEL_DIAMETER_INCHES = 3.7; // For figuring circumference - static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / - (WHEEL_DIAMETER_INCHES * 3.1428); - static final double COUNTS_FULL_TURN = 72; - static final int ENCODER_COUNT_BEFORE_STOP = 140; //slow down before 3" - - static TEST_MODE TEST_RUN_TYPE = TEST_MODE.WORKING1; - - //static final double DRIVE_SPEED = 1; - //static final double TURN_SPEED = 0.5; - - // Declare our motors - // Make sure your ID's match your configuration - //DcMotor motorFrontLeft = null; - //DcMotor motorBackLeft = null; - //DcMotor motorFrontRight = null; - //DcMotor motorBackRight = null; - - int DuckCount; - double distance; - - @Override - public void runOpMode() /*throws InterruptedException*/ { - Direction dir; - distance = 0; - DuckCount = 0; - /* - * Initialize the drive system variables. - * The init() method of the hardware class does all the work here - */ - robot.init(hardwareMap); - runtime.reset(); - - // Send telemetry message to signify robot waiting; - telemetry.addData("Status", "Starting Autorun"); //Auto run - telemetry.update(); - - // Reset Encoder - robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - - // Set Encoder - robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.motorBackRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - - robot.motorBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - robot.motorBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - robot.motorFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - robot.motorFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - // Send telemetry message to indicate successful Encoder reset - telemetry.addData("Path0", "Position: Front L:%7d R:%7d, Back L:%7d R:%7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - telemetry.update(); - - // Reverse the right side motors - robot.motorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); - robot.motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE); - - // Wait for the game to start (driver presses PLAY) - waitForStart(); - - if (isStopRequested()) return; - - if (TEST_RUN_TYPE == TEST_MODE.WORKING1) - { - myEncoderDrive(Direction.RIGHT, 0.1, 1.5, 2000); - myEncoderTurn(0.2, 5); - myEncoderDrive(Direction.BACKWARD, 0.50, 22, 2000); - myEncoderDrive(Direction.BACKWARD, 0.35, 7, 2000); - spinCarousel(2000,500); - myEncoderDrive(Direction.FORWARD, 0.35, 8, 2000); - myEncoderTurn(0.2, -10); - myEncoderDrive(Direction.RIGHT, 0.35, 29, 2000); - myEncoderDrive(Direction.BACKWARD, 0.4, 12, 2000); - - }; - - sleep(100); // pause for servos to move - - telemetry.addData("Path", "Complete"); - telemetry.update(); - } - //currOrient = Current Orientation (rotation) - // x1 current x of robot; y1 current y of robot - // x2 new x of robot; y2 new y of robot (by new I mean targeted x and y) - // speed is just speed - public Direction moveRobot(double x1, double y1, double currOrient, - double x2, double y2, double newOrient, double speed) - { - double angle; - double turn_angle; - Direction direction = Direction.FORWARD; - - // angle will return Anti-clockwise value [-90, 90] - // Example A(-12,50), B(-60, 60). Anglex2) { - // Go backward to (x2,y2) - direction = Direction.BACKWARD; - } else if (x2>x1) { - // Go Forward to (x2, y2) - direction = Direction.FORWARD; - } else if (y1>y2) { - // Go Backward to (x2, y2) - direction = Direction.BACKWARD; - } else { - // Go Forward to (x2, y2) - direction = Direction.FORWARD; - } - - myEncoderDrive(direction, speed, distance, 1000); - myEncoderTurn(speed, newOrient - angle); - - /* - // Move 60+ degree Anti-CLOCKWISE - if (turn_angle < -180) { - turn_angle = turn_angle + 180; - myEncoderTurn(0.4, turn_angle); - myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); - myEncoderTurn(0.4, newOrient - angle -180); - } else if(turn_angle > 180) { - turn_angle = turn_angle - 180; - myEncoderTurn(0.4, turn_angle); - myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); - myEncoderTurn(0.4, newOrient - angle + 180); - } else { - myEncoderTurn(0.4, turn_angle); - // Move FWD - myEncoderDrive(Direction.FORWARD, 0.75, distance, 1000); - myEncoderTurn(0.4, newOrient - angle); - } - */ - return direction; - } - - // Run Carousel - public void spinCarousel(double timeout, long sleeptime) - { - runtime.reset(); - - robot.motorCarouselSpin.setPower(0.8); - while (opModeIsActive() && - runtime.milliseconds() < timeout) - { - telemetry.addData("NFAuto", "Running Carousel for Duck for %f milisec", timeout); - telemetry.update(); - } - robot.motorCarouselSpin.setPower(0); - //sleep(sleeptime); - DuckCount++; - RobotLog.ii("NFAuto", "Number of Duck delivered: %d, timeout %f", DuckCount, timeout); - } - - public void myEncoderTurn(double speed, double degree) - { - double inch_cnt; - - if (Math.abs(degree) < 1) return; - - // Adjust to turn less than 180 - if (degree < -180) { - degree = degree + 360; - } - if (degree > 180) { - degree = degree - 360; - } - - inch_cnt = degree*(COUNTS_FULL_TURN/360); - RobotLog.ii("Input:myEncoderTurn", "Speed/Degree/inch_cnt, %f, %f %f", - speed, degree, inch_cnt); - - if (degree >= 0) { - myEncoderDrive(Direction.ANTI_CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); - } else { - myEncoderDrive(Direction.CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); - } - } - - public void myEncoderDrive(Direction direction, double speed, double Inches, - double timeoutS) { //SensorsToUse sensors_2_use) - int newFrontLeftTarget = 0; - int newFrontRightTarget = 0; - int newBackLeftTarget = 0; - int newBackRightTarget = 0; - int remainingDistance; - int cnt = 0; - double new_speed = 0; - - - RobotLog.ii("Input", "Enter - myEncoderDrive - speed=%f, Inches=%f, timeout=%f", - speed, Inches, timeoutS); - telemetry.addData("Path1", "Enter - myEncoderDrive - speed=%f," + - " Inches=%f, timeout=%f", speed, Inches, timeoutS); - telemetry.update(); - - // Turn off ENCODER - // Reset Encoder beginning to see it gets better. - robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - - RobotLog.ii("Path0", "Starting Position: Front L:%7d R:%7d, Back L:%7d R:%7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - - // Ensure that the op mode is still active - if (opModeIsActive() && !isStopRequested()) { - - // Determine new target position, and pass to motor controller - if (direction == Direction.BACKWARD) { - //Go forward - RobotLog.ii("NFusion", "Moving BACKWARD.... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else if (direction == Direction.FORWARD) { - //Go backward - RobotLog.ii("NFusion", "Moving FORWARD..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.LEFT) { - //Move Right - RobotLog.ii("NFusion", "Moving LEFT..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else if (direction == Direction.RIGHT) { - //Move Left - RobotLog.ii("NFusion", "Moving RIGHT..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.F_RIGHT_DIG) { - //Forward Left Diagonal - RobotLog.ii("NFusion", "Moving F_RIGHT_DIG ..... [%f inches]..", Inches); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else if (direction == Direction.F_LEFT_DIG) { - //Move Forward Right Diagonal - RobotLog.ii("NFusion", "Moving F_LEFT_DIG..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else if (direction == Direction.B_RIGHT_DIG){ - //Move Backward Left Diagonal - RobotLog.ii("NFusion", "Moving B_RIGHT_DIG..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.B_LEFT_DIG) { - //Backward Right Diagonal - RobotLog.ii("NFusion", "Moving B_LEFT_DIG ..... [%f inches]..", Inches); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.ANTI_CLOCK_WISE_TURN) { - // Turn Clock Wise - RobotLog.ii("NFusion", "Turn Anti Clockwise ..... [%f ms]..", timeoutS); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.CLOCK_WISE_TURN) { - // Turn Clock Wise - RobotLog.ii("NFusion", "Turn Clockwise ..... [%f ms]..", timeoutS); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else { - // Do Not move - speed = 0; - } - - // Set the Encoder to the target position. - robot.motorFrontLeft.setTargetPosition(newFrontLeftTarget); - robot.motorFrontRight.setTargetPosition(newFrontRightTarget); - robot.motorBackLeft.setTargetPosition(newBackLeftTarget); - robot.motorBackRight.setTargetPosition(newBackRightTarget); - - // Turn On RUN_TO_POSITION - robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); - robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); - robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); - robot.motorBackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); - - // Set power for the motors. - robot.motorFrontLeft.setPower(Math.abs(speed)); - robot.motorFrontRight.setPower(Math.abs(speed)); - robot.motorBackLeft.setPower(Math.abs(speed)); - robot.motorBackRight.setPower(Math.abs(speed)); - - // reset the timeout time and start motion. - runtime.reset(); - - RobotLog.ii("NFusion", "Final Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", - newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); - - while (opModeIsActive() && - (runtime.milliseconds() < timeoutS) && - ((robot.motorFrontLeft.isBusy() || robot.motorFrontRight.isBusy()) && - (robot.motorBackLeft.isBusy() || robot.motorBackRight.isBusy()))) { - // Display it for the driver every 10 ms - if(runtime.milliseconds() > cnt * 30 ) { // Print every 30 ms - RobotLog.ii("NFusion", "Current Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - telemetry.addData("Path1", "Running to: FL: %7d FR: %7d BL: %7d BR: %7d", - newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); - telemetry.addData("Path2", "Running at: FL: %7d FR: %7d BL: %7d BR: %7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - telemetry.update(); - cnt++; - } - // Reduce the Speed before stopping - remainingDistance = Math.abs(newFrontLeftTarget-robot.motorFrontLeft.getCurrentPosition()); - if (remainingDistance < 10) - { - remainingDistance = Math.abs(newFrontRightTarget-robot.motorFrontRight.getCurrentPosition()); - } - if ((remainingDistance < ENCODER_COUNT_BEFORE_STOP) && (remainingDistance >= 10)){ - new_speed = Math.abs(speed)*(remainingDistance/(float)ENCODER_COUNT_BEFORE_STOP); - robot.motorFrontLeft.setPower(new_speed); - robot.motorFrontRight.setPower(new_speed); - robot.motorBackLeft.setPower(new_speed); - robot.motorBackRight.setPower(new_speed); - if((cnt % 5) == 0) { // skip 5 cnt and print - RobotLog.ii("NFusion", "Remaining Dist: %7d, Speed %f, new Speed %f", - remainingDistance, speed, new_speed); - } - } - } - - //RobotLog.ii("NFusion", "Motor Encoder Status FL: %d, FR: %d, BL: %d, BR: %d", - // motorFrontLeft.isBusy(), - // motorFrontRight.isBusy(), - // motorBackLeft.isBusy(), - //motorBackRight.isBusy()); - - // Stop all motion; - robot.motorFrontLeft.setPower(0); - robot.motorFrontRight.setPower(0); - robot.motorBackLeft.setPower(0); - robot.motorBackRight.setPower(0); - - //sleep(500); // optional pause after each move - - // Turn off ENCODER - // Reset Encoder - //robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - //robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - //robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - //robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - - RobotLog.ii("Path0", "Exit - myEncoderDrive Last Position: Front L:%7d R:%7d, Back L:%7d R:%7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - - //telemetry.addData("Status", "Movement Distance: %7d", - // distance_traveled); - //telemetry.update(); - - } - } +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.RobotLog; +import org.firstinspires.ftc.teamcode.NFMyRobot; + +/** + * + * The desired path in this example is: + * - Drive forward for 48 inches + * - Spin right for 12 Inches + * - Drive Backwards for 24 inches + * - Stop and close the claw. + * + * The code is written using a method called: encoderDrive(speed, leftInches, rightInches, timeoutS) + * that performs the actual movement. + * This methods assumes that each movement is relative to the last stopping place. + * There are other ways to perform encoder based moves, but this method is probably the simplest. + * This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile + * + * Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list + */ + + +@Autonomous(name="Autonomous: AutoBlueForward", group="Autonomous") +//@Disabled +public class AutoBlueForward extends LinearOpMode { + + public enum Direction { + FORWARD, + BACKWARD, + RIGHT, + LEFT, + F_RIGHT_DIG, + F_LEFT_DIG, + B_RIGHT_DIG, + B_LEFT_DIG, + CLOCK_WISE_TURN, + ANTI_CLOCK_WISE_TURN + } + public enum TEST_MODE { + WORKING1, + WORKING2, + WORKING3, + WORKING4, + TEST1, + TEST2, + TEST3, + TEST4 + } + + /* Declare OpMode members. */ + NFMyRobot robot = new NFMyRobot(); // Use NF my Robot h/w + + private final ElapsedTime runtime = new ElapsedTime(); + + //Encoder produced TICK COUNTS per revolution + static final double COUNTS_PER_MOTOR_REV = 537.7; // eg: TETRIX Motor Encoder - 1440, REV Hex Motors: 2240 + static final double DRIVE_GEAR_REDUCTION = 1; //2.0; // This is < 1.0 if geared UP + static final double WHEEL_DIAMETER_INCHES = 3.7; // For figuring circumference + static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / + (WHEEL_DIAMETER_INCHES * 3.1428); + static final double COUNTS_FULL_TURN = 72; + static final int ENCODER_COUNT_BEFORE_STOP = 140; //slow down before 3" + static final int ARM_FULL_TURN_CNT = 537; + static TEST_MODE TEST_RUN_TYPE = TEST_MODE.WORKING3; + + //static final double DRIVE_SPEED = 1; + //static final double TURN_SPEED = 0.5; + + // Declare our motors + // Make sure your ID's match your configuration + //DcMotor motorFrontLeft = null; + //DcMotor motorBackLeft = null; + //DcMotor motorFrontRight = null; + //DcMotor motorBackRight = null; + + int DuckCount; + double distance; + + @Override + public void runOpMode() /*throws InterruptedException*/ { + Direction dir; + distance = 0; + DuckCount = 0; + /* + * Initialize the drive system variables. + * The init() method of the hardware class does all the work here + */ + robot.init(hardwareMap); + runtime.reset(); + + // Send telemetry message to signify robot waiting; + telemetry.addData("Status", "Starting Autorun"); //Auto run + telemetry.update(); + + // Reset Encoder + robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + // Set Encoder + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + robot.motorBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + // Send telemetry message to indicate successful Encoder reset + telemetry.addData("Path0", "Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.update(); + + // Reverse the right side motors + robot.motorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); + robot.motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE); + + // Wait for the game to start (driver presses PLAY) + waitForStart(); + + if (isStopRequested()) return; + + if (TEST_RUN_TYPE == TEST_MODE.WORKING1) + { + myEncoderDrive(Direction.RIGHT, 0.1, 1.5, 2000); + myEncoderTurn(0.2, 6); + myEncoderDrive(Direction.BACKWARD, 0.50, 22, 2000); + myEncoderDrive(Direction.BACKWARD, 0.25, 6, 2000); + spinCarousel(2500,0.50); + myEncoderDrive(Direction.FORWARD, 0.35, 8, 2000); + myEncoderTurn(0.2, -5); + myEncoderDrive(Direction.RIGHT, 0.35, 29, 2000); + myEncoderDrive(Direction.BACKWARD, 0.4, 12, 2000); + } + + if (TEST_RUN_TYPE == TEST_MODE.WORKING2) + { + myEncoderDrive(Direction.RIGHT, 0.25, 6, 2000); + myEncoderDrive(Direction.BACKWARD, 0.50, 24, 2000); + myEncoderTurn(0.2, -45); + myEncoderDrive(Direction.RIGHT, 0.25, 10, 2000); + myEncoderDrive(Direction.BACKWARD, 0.15, 10, 850); + spinCarousel(2500,0.50); + myEncoderDrive(Direction.FORWARD, 0.25, 2, 2000); + myEncoderTurn(0.2, 45); + myEncoderDrive(Direction.RIGHT, 0.35, 22, 2000); + myEncoderDrive(Direction.BACKWARD, 0.25, 3, 2000); + } + + if (TEST_RUN_TYPE == TEST_MODE.WORKING3) + { + myEncoderDrive(Direction.RIGHT, 0.35, 16, 2000); + myArmMove(0.15, 0.35, 2200); + myEncoderTurn(0.2, -67.5); + myEncoderDrive(Direction.FORWARD, 0.25, 12, 2000); + sleep(300); + //ARM Move and Claw Open: + robot.servo_1.setPosition(0.75); + robot.servo_2.setPosition(0.25); + sleep(350); + myEncoderDrive(Direction.BACKWARD, 0.50, 19.5, 2000); + myEncoderTurn(0.2, 67.5); + myEncoderDrive(Direction.BACKWARD, 0.50, 22, 2000); + robot.servo_1.setPosition(1); + robot.servo_2.setPosition(0); + myEncoderTurn(0.25, -45); + myEncoderDrive(Direction.RIGHT, 0.25, 8, 2000); + myEncoderDrive(Direction.BACKWARD, 0.15, 8, 870); + spinCarousel(2800,0.60); + myEncoderDrive(Direction.FORWARD, 0.25, 3, 2000); + myEncoderTurn(0.2, 45); + myEncoderDrive(Direction.RIGHT, 0.35, 20, 2000); + myEncoderDrive(Direction.BACKWARD, 0.25, 3, 2000); + myArmMove(0.15, -0.20, 1500); + } + + // Set all Motors without Encoder for Manual Run. + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + //sleep(100); // pause for servos to move + telemetry.addData("Path", "Complete"); + telemetry.update(); + } + + // Run Carousel + public void spinCarousel(double timeout, double speed) + { + runtime.reset(); + + robot.motorCarouselSpin.setPower(speed); + while (opModeIsActive() && + runtime.milliseconds() < timeout) + { + telemetry.addData("NFAuto", "Running Carousel for Duck for %f milisec", timeout); + telemetry.update(); + } + robot.motorCarouselSpin.setPower(0); + //sleep(sleeptime); + DuckCount++; + RobotLog.ii("NFAuto", "Number of Duck delivered: %d, timeout %f", DuckCount, timeout); + } + + public void myEncoderTurn(double speed, double degree) + { + double inch_cnt; + + if (Math.abs(degree) < 1) return; + + // Adjust to turn less than 180 + if (degree < -180) { + degree = degree + 360; + } + if (degree > 180) { + degree = degree - 360; + } + + inch_cnt = degree*(COUNTS_FULL_TURN/360); + RobotLog.ii("Input:myEncoderTurn", "Speed/Degree/inch_cnt, %f, %f %f", + speed, degree, inch_cnt); + + if (degree >= 0) { + myEncoderDrive(Direction.ANTI_CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); + } else { + myEncoderDrive(Direction.CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); + } + } + + public void myArmMove(double speed, double rotation, double timeoutS) { //SensorsToUse sensors_2_use) + int newArmPosition = 0; + + // Reset Encoder beginning to see it gets better. + robot.motorArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + newArmPosition = robot.motorArm.getCurrentPosition() + (int) (rotation * ARM_FULL_TURN_CNT); + // Set the Encoder to the target position. + robot.motorArm.setTargetPosition(newArmPosition); + // Turn On RUN_TO_POSITION + robot.motorArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + // Set power for the motors. + robot.motorArm.setPower(Math.abs(speed)); + // reset the timeout time and start motion. + runtime.reset(); + + while (opModeIsActive() && + (runtime.milliseconds() < timeoutS) && + (robot.motorArm.isBusy())) + { + RobotLog.ii("NFusion", "Arm Current Pos: %7d, Target Pos: %7d", + robot.motorArm.getCurrentPosition(), newArmPosition); + } + + robot.motorArm.setPower(0); + sleep(50); + } + public void myEncoderDrive(Direction direction, double speed, double Inches, + double timeoutS) { //SensorsToUse sensors_2_use) + int newFrontLeftTarget = 0; + int newFrontRightTarget = 0; + int newBackLeftTarget = 0; + int newBackRightTarget = 0; + int remainingDistance; + int cnt = 0; + double new_speed = 0; + + + RobotLog.ii("Input", "Enter - myEncoderDrive - speed=%f, Inches=%f, timeout=%f", + speed, Inches, timeoutS); + telemetry.addData("Path1", "Enter - myEncoderDrive - speed=%f," + + " Inches=%f, timeout=%f", speed, Inches, timeoutS); + telemetry.update(); + + // Turn off ENCODER + // Reset Encoder beginning to see it gets better. + robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + RobotLog.ii("Path0", "Starting Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + + // Ensure that the op mode is still active + if (opModeIsActive() && !isStopRequested()) { + + // Determine new target position, and pass to motor controller + if (direction == Direction.BACKWARD) { + //Go forward + RobotLog.ii("NFusion", "Moving BACKWARD.... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.FORWARD) { + //Go backward + RobotLog.ii("NFusion", "Moving FORWARD..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.LEFT) { + //Move Right + RobotLog.ii("NFusion", "Moving LEFT..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.RIGHT) { + //Move Left + RobotLog.ii("NFusion", "Moving RIGHT..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.F_RIGHT_DIG) { + //Forward Left Diagonal + RobotLog.ii("NFusion", "Moving F_RIGHT_DIG ..... [%f inches]..", Inches); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.F_LEFT_DIG) { + //Move Forward Right Diagonal + RobotLog.ii("NFusion", "Moving F_LEFT_DIG..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.B_RIGHT_DIG){ + //Move Backward Left Diagonal + RobotLog.ii("NFusion", "Moving B_RIGHT_DIG..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.B_LEFT_DIG) { + //Backward Right Diagonal + RobotLog.ii("NFusion", "Moving B_LEFT_DIG ..... [%f inches]..", Inches); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.ANTI_CLOCK_WISE_TURN) { + // Turn Clock Wise + RobotLog.ii("NFusion", "Turn Anti Clockwise ..... [%f ms]..", timeoutS); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.CLOCK_WISE_TURN) { + // Turn Clock Wise + RobotLog.ii("NFusion", "Turn Clockwise ..... [%f ms]..", timeoutS); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else { + // Do Not move + speed = 0; + } + + // Set the Encoder to the target position. + robot.motorFrontLeft.setTargetPosition(newFrontLeftTarget); + robot.motorFrontRight.setTargetPosition(newFrontRightTarget); + robot.motorBackLeft.setTargetPosition(newBackLeftTarget); + robot.motorBackRight.setTargetPosition(newBackRightTarget); + + // Turn On RUN_TO_POSITION + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + // Set power for the motors. + robot.motorFrontLeft.setPower(Math.abs(speed)); + robot.motorFrontRight.setPower(Math.abs(speed)); + robot.motorBackLeft.setPower(Math.abs(speed)); + robot.motorBackRight.setPower(Math.abs(speed)); + + // reset the timeout time and start motion. + runtime.reset(); + + RobotLog.ii("NFusion", "Final Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", + newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); + + while (opModeIsActive() && + (runtime.milliseconds() < timeoutS) && + ((robot.motorFrontLeft.isBusy() || robot.motorFrontRight.isBusy()) && + (robot.motorBackLeft.isBusy() || robot.motorBackRight.isBusy()))) { + // Display it for the driver every 10 ms + if(runtime.milliseconds() > cnt * 30 ) { // Print every 30 ms + RobotLog.ii("NFusion", "Current Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.addData("Path1", "Running to: FL: %7d FR: %7d BL: %7d BR: %7d", + newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); + telemetry.addData("Path2", "Running at: FL: %7d FR: %7d BL: %7d BR: %7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.update(); + cnt++; + } + // Reduce the Speed before stopping + remainingDistance = Math.abs(newFrontLeftTarget-robot.motorFrontLeft.getCurrentPosition()); + if (remainingDistance < 10) + { + remainingDistance = Math.abs(newFrontRightTarget-robot.motorFrontRight.getCurrentPosition()); + } + if ((remainingDistance < ENCODER_COUNT_BEFORE_STOP) && (remainingDistance >= 10)){ + new_speed = Math.abs(speed)*(remainingDistance/(float)ENCODER_COUNT_BEFORE_STOP); + robot.motorFrontLeft.setPower(new_speed); + robot.motorFrontRight.setPower(new_speed); + robot.motorBackLeft.setPower(new_speed); + robot.motorBackRight.setPower(new_speed); + if((cnt % 5) == 0) { // skip 5 cnt and print + RobotLog.ii("NFusion", "Remaining Dist: %7d, Speed %f, new Speed %f", + remainingDistance, speed, new_speed); + } + } + } + + //RobotLog.ii("NFusion", "Motor Encoder Status FL: %d, FR: %d, BL: %d, BR: %d", + // motorFrontLeft.isBusy(), + // motorFrontRight.isBusy(), + // motorBackLeft.isBusy(), + //motorBackRight.isBusy()); + + // Stop all motion; + robot.motorFrontLeft.setPower(0); + robot.motorFrontRight.setPower(0); + robot.motorBackLeft.setPower(0); + robot.motorBackRight.setPower(0); + + //sleep(500); // optional pause after each move + + // Turn off ENCODER + // Reset Encoder + //robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + RobotLog.ii("Path0", "Exit - myEncoderDrive Last Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + + //telemetry.addData("Status", "Movement Distance: %7d", + // distance_traveled); + //telemetry.update(); + + } + } + + //currOrient = Current Orientation (rotation) + // x1 current x of robot; y1 current y of robot + // x2 new x of robot; y2 new y of robot (by new I mean targeted x and y) + // speed is just speed + public Direction moveRobot(double x1, double y1, double currOrient, + double x2, double y2, double newOrient, double speed) + { + double angle; + double turn_angle; + Direction direction = Direction.FORWARD; + + // angle will return Anti-clockwise value [-90, 90] + // Example A(-12,50), B(-60, 60). Anglex2) { + // Go backward to (x2,y2) + direction = Direction.BACKWARD; + } else if (x2>x1) { + // Go Forward to (x2, y2) + direction = Direction.FORWARD; + } else if (y1>y2) { + // Go Backward to (x2, y2) + direction = Direction.BACKWARD; + } else { + // Go Forward to (x2, y2) + direction = Direction.FORWARD; + } + + myEncoderDrive(direction, speed, distance, 1000); + myEncoderTurn(speed, newOrient - angle); + + /* + // Move 60+ degree Anti-CLOCKWISE + if (turn_angle < -180) { + turn_angle = turn_angle + 180; + myEncoderTurn(0.4, turn_angle); + myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle -180); + } else if(turn_angle > 180) { + turn_angle = turn_angle - 180; + myEncoderTurn(0.4, turn_angle); + myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle + 180); + } else { + myEncoderTurn(0.4, turn_angle); + // Move FWD + myEncoderDrive(Direction.FORWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle); + } + */ + return direction; + } + } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueForwardNoHub.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueForwardNoHub.java new file mode 100644 index 000000000000..ff44ab4833ad --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueForwardNoHub.java @@ -0,0 +1,567 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.RobotLog; + +/** + * + * The desired path in this example is: + * - Drive forward for 48 inches + * - Spin right for 12 Inches + * - Drive Backwards for 24 inches + * - Stop and close the claw. + * + * The code is written using a method called: encoderDrive(speed, leftInches, rightInches, timeoutS) + * that performs the actual movement. + * This methods assumes that each movement is relative to the last stopping place. + * There are other ways to perform encoder based moves, but this method is probably the simplest. + * This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile + * + * Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list + */ + + +@Autonomous(name="Autonomous: AutoBlueForwardNoHub", group="Autonomous") +//@Disabled +public class AutoBlueForwardNoHub extends LinearOpMode { + + public enum Direction { + FORWARD, + BACKWARD, + RIGHT, + LEFT, + F_RIGHT_DIG, + F_LEFT_DIG, + B_RIGHT_DIG, + B_LEFT_DIG, + CLOCK_WISE_TURN, + ANTI_CLOCK_WISE_TURN + } + public enum TEST_MODE { + WORKING1, + WORKING2, + WORKING3, + WORKING4, + TEST1, + TEST2, + TEST3, + TEST4 + } + + /* Declare OpMode members. */ + NFMyRobot robot = new NFMyRobot(); // Use NF my Robot h/w + + private final ElapsedTime runtime = new ElapsedTime(); + + //Encoder produced TICK COUNTS per revolution + static final double COUNTS_PER_MOTOR_REV = 537.7; // eg: TETRIX Motor Encoder - 1440, REV Hex Motors: 2240 + static final double DRIVE_GEAR_REDUCTION = 1; //2.0; // This is < 1.0 if geared UP + static final double WHEEL_DIAMETER_INCHES = 3.7; // For figuring circumference + static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / + (WHEEL_DIAMETER_INCHES * 3.1428); + static final double COUNTS_FULL_TURN = 72; + static final int ENCODER_COUNT_BEFORE_STOP = 140; //slow down before 3" + static final int ARM_FULL_TURN_CNT = 537; + static TEST_MODE TEST_RUN_TYPE = TEST_MODE.WORKING2; + + //static final double DRIVE_SPEED = 1; + //static final double TURN_SPEED = 0.5; + + // Declare our motors + // Make sure your ID's match your configuration + //DcMotor motorFrontLeft = null; + //DcMotor motorBackLeft = null; + //DcMotor motorFrontRight = null; + //DcMotor motorBackRight = null; + + int DuckCount; + double distance; + + @Override + public void runOpMode() /*throws InterruptedException*/ { + Direction dir; + distance = 0; + DuckCount = 0; + /* + * Initialize the drive system variables. + * The init() method of the hardware class does all the work here + */ + robot.init(hardwareMap); + runtime.reset(); + + // Send telemetry message to signify robot waiting; + telemetry.addData("Status", "Starting Autorun"); //Auto run + telemetry.update(); + + // Reset Encoder + robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + // Set Encoder + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + robot.motorBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + // Send telemetry message to indicate successful Encoder reset + telemetry.addData("Path0", "Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.update(); + + // Reverse the right side motors + robot.motorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); + robot.motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE); + + // Wait for the game to start (driver presses PLAY) + waitForStart(); + + if (isStopRequested()) return; + + if (TEST_RUN_TYPE == TEST_MODE.WORKING1) + { + myEncoderDrive(Direction.RIGHT, 0.1, 1.5, 2000); + myEncoderTurn(0.2, 6); + myEncoderDrive(Direction.BACKWARD, 0.50, 22, 2000); + myEncoderDrive(Direction.BACKWARD, 0.25, 6, 2000); + spinCarousel(2500,0.50); + myEncoderDrive(Direction.FORWARD, 0.35, 8, 2000); + myEncoderTurn(0.2, -5); + myEncoderDrive(Direction.RIGHT, 0.35, 29, 2000); + myEncoderDrive(Direction.BACKWARD, 0.4, 12, 2000); + } + + if (TEST_RUN_TYPE == TEST_MODE.WORKING2) + { + myEncoderDrive(Direction.RIGHT, 0.25, 6, 2000); + myEncoderDrive(Direction.BACKWARD, 0.50, 24, 2000); + myEncoderTurn(0.2, -45); + myEncoderDrive(Direction.RIGHT, 0.25, 10, 2000); + myEncoderDrive(Direction.BACKWARD, 0.15, 10, 850); + spinCarousel(2500,0.50); + myEncoderDrive(Direction.FORWARD, 0.25, 2, 2000); + myEncoderTurn(0.2, 45); + myEncoderDrive(Direction.RIGHT, 0.35, 22, 2000); + myEncoderDrive(Direction.BACKWARD, 0.25, 3, 2000); + } + + if (TEST_RUN_TYPE == TEST_MODE.WORKING3) + { + myEncoderDrive(Direction.RIGHT, 0.35, 16, 2000); + myArmMove(0.15, 0.35, 2200); + myEncoderTurn(0.2, -67.5); + myEncoderDrive(Direction.FORWARD, 0.25, 12, 2000); + //ARM Move and Claw Open: + robot.servo_1.setPosition(1); + robot.servo_2.setPosition(0); + sleep(350); + myEncoderDrive(Direction.BACKWARD, 0.50, 20, 2000); + myEncoderTurn(0.2, 67.5); + //myEncoderDrive(Direction.RIGHT, 0.25, 6, 2000); + myEncoderDrive(Direction.BACKWARD, 0.50, 23, 2000); + robot.servo_1.setPosition(0.75); + robot.servo_2.setPosition(0.25); + myEncoderTurn(0.2, -45); + myEncoderDrive(Direction.RIGHT, 0.25, 8, 2000); + myEncoderDrive(Direction.BACKWARD, 0.15, 8, 870); + spinCarousel(2800,0.60); + myEncoderDrive(Direction.FORWARD, 0.25, 3, 2000); + myEncoderTurn(0.2, 45); + myEncoderDrive(Direction.RIGHT, 0.35, 20, 2000); + myEncoderDrive(Direction.BACKWARD, 0.25, 3, 2000); + } + + // Set all Motors without Encoder for Manual Run. + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + //sleep(100); // pause for servos to move + telemetry.addData("Path", "Complete"); + telemetry.update(); + } + + // Run Carousel + public void spinCarousel(double timeout, double speed) + { + runtime.reset(); + + robot.motorCarouselSpin.setPower(speed); + while (opModeIsActive() && + runtime.milliseconds() < timeout) + { + telemetry.addData("NFAuto", "Running Carousel for Duck for %f milisec", timeout); + telemetry.update(); + } + robot.motorCarouselSpin.setPower(0); + //sleep(sleeptime); + DuckCount++; + RobotLog.ii("NFAuto", "Number of Duck delivered: %d, timeout %f", DuckCount, timeout); + } + + public void myEncoderTurn(double speed, double degree) + { + double inch_cnt; + + if (Math.abs(degree) < 1) return; + + // Adjust to turn less than 180 + if (degree < -180) { + degree = degree + 360; + } + if (degree > 180) { + degree = degree - 360; + } + + inch_cnt = degree*(COUNTS_FULL_TURN/360); + RobotLog.ii("Input:myEncoderTurn", "Speed/Degree/inch_cnt, %f, %f %f", + speed, degree, inch_cnt); + + if (degree >= 0) { + myEncoderDrive(Direction.ANTI_CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); + } else { + myEncoderDrive(Direction.CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); + } + } + + public void myArmMove(double speed, double rotation, double timeoutS) { //SensorsToUse sensors_2_use) + int newArmPosition = 0; + + // Reset Encoder beginning to see it gets better. + robot.motorArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + newArmPosition = robot.motorArm.getCurrentPosition() + (int) (rotation * ARM_FULL_TURN_CNT); + // Set the Encoder to the target position. + robot.motorArm.setTargetPosition(newArmPosition); + // Turn On RUN_TO_POSITION + robot.motorArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + // Set power for the motors. + robot.motorArm.setPower(Math.abs(speed)); + // reset the timeout time and start motion. + runtime.reset(); + + while (opModeIsActive() && + (runtime.milliseconds() < timeoutS) && + (robot.motorArm.isBusy())) + { + RobotLog.ii("NFusion", "Arm Current Pos: %7d, Target Pos: %7d", + robot.motorArm.getCurrentPosition(), newArmPosition); + } + + robot.motorArm.setPower(0); + sleep(50); + } + public void myEncoderDrive(Direction direction, double speed, double Inches, + double timeoutS) { //SensorsToUse sensors_2_use) + int newFrontLeftTarget = 0; + int newFrontRightTarget = 0; + int newBackLeftTarget = 0; + int newBackRightTarget = 0; + int remainingDistance; + int cnt = 0; + double new_speed = 0; + + + RobotLog.ii("Input", "Enter - myEncoderDrive - speed=%f, Inches=%f, timeout=%f", + speed, Inches, timeoutS); + telemetry.addData("Path1", "Enter - myEncoderDrive - speed=%f," + + " Inches=%f, timeout=%f", speed, Inches, timeoutS); + telemetry.update(); + + // Turn off ENCODER + // Reset Encoder beginning to see it gets better. + robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + RobotLog.ii("Path0", "Starting Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + + // Ensure that the op mode is still active + if (opModeIsActive() && !isStopRequested()) { + + // Determine new target position, and pass to motor controller + if (direction == Direction.BACKWARD) { + //Go forward + RobotLog.ii("NFusion", "Moving BACKWARD.... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.FORWARD) { + //Go backward + RobotLog.ii("NFusion", "Moving FORWARD..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.LEFT) { + //Move Right + RobotLog.ii("NFusion", "Moving LEFT..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.RIGHT) { + //Move Left + RobotLog.ii("NFusion", "Moving RIGHT..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.F_RIGHT_DIG) { + //Forward Left Diagonal + RobotLog.ii("NFusion", "Moving F_RIGHT_DIG ..... [%f inches]..", Inches); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.F_LEFT_DIG) { + //Move Forward Right Diagonal + RobotLog.ii("NFusion", "Moving F_LEFT_DIG..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.B_RIGHT_DIG){ + //Move Backward Left Diagonal + RobotLog.ii("NFusion", "Moving B_RIGHT_DIG..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.B_LEFT_DIG) { + //Backward Right Diagonal + RobotLog.ii("NFusion", "Moving B_LEFT_DIG ..... [%f inches]..", Inches); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.ANTI_CLOCK_WISE_TURN) { + // Turn Clock Wise + RobotLog.ii("NFusion", "Turn Anti Clockwise ..... [%f ms]..", timeoutS); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.CLOCK_WISE_TURN) { + // Turn Clock Wise + RobotLog.ii("NFusion", "Turn Clockwise ..... [%f ms]..", timeoutS); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else { + // Do Not move + speed = 0; + } + + // Set the Encoder to the target position. + robot.motorFrontLeft.setTargetPosition(newFrontLeftTarget); + robot.motorFrontRight.setTargetPosition(newFrontRightTarget); + robot.motorBackLeft.setTargetPosition(newBackLeftTarget); + robot.motorBackRight.setTargetPosition(newBackRightTarget); + + // Turn On RUN_TO_POSITION + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + // Set power for the motors. + robot.motorFrontLeft.setPower(Math.abs(speed)); + robot.motorFrontRight.setPower(Math.abs(speed)); + robot.motorBackLeft.setPower(Math.abs(speed)); + robot.motorBackRight.setPower(Math.abs(speed)); + + // reset the timeout time and start motion. + runtime.reset(); + + RobotLog.ii("NFusion", "Final Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", + newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); + + while (opModeIsActive() && + (runtime.milliseconds() < timeoutS) && + ((robot.motorFrontLeft.isBusy() || robot.motorFrontRight.isBusy()) && + (robot.motorBackLeft.isBusy() || robot.motorBackRight.isBusy()))) { + // Display it for the driver every 10 ms + if(runtime.milliseconds() > cnt * 30 ) { // Print every 30 ms + RobotLog.ii("NFusion", "Current Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.addData("Path1", "Running to: FL: %7d FR: %7d BL: %7d BR: %7d", + newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); + telemetry.addData("Path2", "Running at: FL: %7d FR: %7d BL: %7d BR: %7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.update(); + cnt++; + } + // Reduce the Speed before stopping + remainingDistance = Math.abs(newFrontLeftTarget-robot.motorFrontLeft.getCurrentPosition()); + if (remainingDistance < 10) + { + remainingDistance = Math.abs(newFrontRightTarget-robot.motorFrontRight.getCurrentPosition()); + } + if ((remainingDistance < ENCODER_COUNT_BEFORE_STOP) && (remainingDistance >= 10)){ + new_speed = Math.abs(speed)*(remainingDistance/(float)ENCODER_COUNT_BEFORE_STOP); + robot.motorFrontLeft.setPower(new_speed); + robot.motorFrontRight.setPower(new_speed); + robot.motorBackLeft.setPower(new_speed); + robot.motorBackRight.setPower(new_speed); + if((cnt % 5) == 0) { // skip 5 cnt and print + RobotLog.ii("NFusion", "Remaining Dist: %7d, Speed %f, new Speed %f", + remainingDistance, speed, new_speed); + } + } + } + + //RobotLog.ii("NFusion", "Motor Encoder Status FL: %d, FR: %d, BL: %d, BR: %d", + // motorFrontLeft.isBusy(), + // motorFrontRight.isBusy(), + // motorBackLeft.isBusy(), + //motorBackRight.isBusy()); + + // Stop all motion; + robot.motorFrontLeft.setPower(0); + robot.motorFrontRight.setPower(0); + robot.motorBackLeft.setPower(0); + robot.motorBackRight.setPower(0); + + //sleep(500); // optional pause after each move + + // Turn off ENCODER + // Reset Encoder + //robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + RobotLog.ii("Path0", "Exit - myEncoderDrive Last Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + + //telemetry.addData("Status", "Movement Distance: %7d", + // distance_traveled); + //telemetry.update(); + + } + } + + //currOrient = Current Orientation (rotation) + // x1 current x of robot; y1 current y of robot + // x2 new x of robot; y2 new y of robot (by new I mean targeted x and y) + // speed is just speed + public Direction moveRobot(double x1, double y1, double currOrient, + double x2, double y2, double newOrient, double speed) + { + double angle; + double turn_angle; + Direction direction = Direction.FORWARD; + + // angle will return Anti-clockwise value [-90, 90] + // Example A(-12,50), B(-60, 60). Anglex2) { + // Go backward to (x2,y2) + direction = Direction.BACKWARD; + } else if (x2>x1) { + // Go Forward to (x2, y2) + direction = Direction.FORWARD; + } else if (y1>y2) { + // Go Backward to (x2, y2) + direction = Direction.BACKWARD; + } else { + // Go Forward to (x2, y2) + direction = Direction.FORWARD; + } + + myEncoderDrive(direction, speed, distance, 1000); + myEncoderTurn(speed, newOrient - angle); + + /* + // Move 60+ degree Anti-CLOCKWISE + if (turn_angle < -180) { + turn_angle = turn_angle + 180; + myEncoderTurn(0.4, turn_angle); + myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle -180); + } else if(turn_angle > 180) { + turn_angle = turn_angle - 180; + myEncoderTurn(0.4, turn_angle); + myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle + 180); + } else { + myEncoderTurn(0.4, turn_angle); + // Move FWD + myEncoderDrive(Direction.FORWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle); + } + */ + return direction; + } + +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueWarehouse.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueWarehouse.java index d3692dd6a90b..965c331695e8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueWarehouse.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueWarehouse.java @@ -1,482 +1,546 @@ -/* Copyright (c) 2017 FIRST. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted (subject to the limitations in the disclaimer below) provided that - * the following conditions are met: - * - * Redistributions of source code must retain the above copyright notice, this list - * of conditions and the following disclaimer. - * - * Redistributions in binary form must reproduce the above copyright notice, this - * list of conditions and the following disclaimer in the documentation and/or - * other materials provided with the distribution. - * - * Neither the name of FIRST nor the names of its contributors may be used to endorse or - * promote products derived from this software without specific prior written permission. - * - * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS - * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.util.ElapsedTime; -import com.qualcomm.robotcore.util.RobotLog; - -/** - * - * The desired path in this example is: - * - Drive forward for 48 inches - * - Spin right for 12 Inches - * - Drive Backwards for 24 inches - * - Stop and close the claw. - * - * The code is written using a method called: encoderDrive(speed, leftInches, rightInches, timeoutS) - * that performs the actual movement. - * This methods assumes that each movement is relative to the last stopping place. - * There are other ways to perform encoder based moves, but this method is probably the simplest. - * This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile - * - * Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name. - * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list - */ - - -@Autonomous(name="Autonomous: AutoBlueWarehouse", group="Autonomous") -//@Disabled -public class AutoBlueWarehouse extends LinearOpMode { - - public enum Direction { - FORWARD, - BACKWARD, - RIGHT, - LEFT, - F_RIGHT_DIG, - F_LEFT_DIG, - B_RIGHT_DIG, - B_LEFT_DIG, - CLOCK_WISE_TURN, - ANTI_CLOCK_WISE_TURN - } - public enum TEST_MODE { - WORKING1, - WORKING2, - WORKING3, - WORKING4, - TEST1, - TEST2, - TEST3, - TEST4 - } - - /* Declare OpMode members. */ - NFMyRobot robot = new NFMyRobot(); // Use NF my Robot h/w - - private final ElapsedTime runtime = new ElapsedTime(); - - //Encoder produced TICK COUNTS per revolution - static final double COUNTS_PER_MOTOR_REV = 537.7; // eg: TETRIX Motor Encoder - 1440, REV Hex Motors: 2240 - static final double DRIVE_GEAR_REDUCTION = 1; //2.0; // This is < 1.0 if geared UP - static final double WHEEL_DIAMETER_INCHES = 3.7; // For figuring circumference - static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / - (WHEEL_DIAMETER_INCHES * 3.1428); - static final double COUNTS_FULL_TURN = 72; - static final int ENCODER_COUNT_BEFORE_STOP = 140; //slow down before 3" - - static TEST_MODE TEST_RUN_TYPE = TEST_MODE.WORKING1; - - //static final double DRIVE_SPEED = 1; - //static final double TURN_SPEED = 0.5; - - // Declare our motors - // Make sure your ID's match your configuration - //DcMotor motorFrontLeft = null; - //DcMotor motorBackLeft = null; - //DcMotor motorFrontRight = null; - //DcMotor motorBackRight = null; - - int DuckCount; - double distance; - - @Override - public void runOpMode() /*throws InterruptedException*/ { - Direction dir; - distance = 0; - DuckCount = 0; - /* - * Initialize the drive system variables. - * The init() method of the hardware class does all the work here - */ - robot.init(hardwareMap); - runtime.reset(); - - // Send telemetry message to signify robot waiting; - telemetry.addData("Status", "Starting Autorun"); //Auto run - telemetry.update(); - - // Reset Encoder - robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - - // Set Encoder - robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.motorBackRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - - robot.motorBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - robot.motorBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - robot.motorFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - robot.motorFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - // Send telemetry message to indicate successful Encoder reset - telemetry.addData("Path0", "Position: Front L:%7d R:%7d, Back L:%7d R:%7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - telemetry.update(); - - // Reverse the right side motors - robot.motorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); - robot.motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE); - - // Wait for the game to start (driver presses PLAY) - waitForStart(); - - if (isStopRequested()) return; - - if (TEST_RUN_TYPE == TEST_MODE.WORKING1) - { - myEncoderDrive(Direction.FORWARD, 0.75, 63.5, 10000); - myEncoderDrive(Direction.RIGHT, 0.6, 30, 5000); - myEncoderDrive(Direction.FORWARD, 0.5, 12, 2000); - - }; - - sleep(100); // pause for servos to move - - telemetry.addData("Path", "Complete"); - telemetry.update(); - } - - public Direction moveRobot(double x1, double y1, double currOrient, - double x2, double y2, double newOrient, double speed) - { - double angle; - double turn_angle; - Direction direction = Direction.FORWARD; - - // angle will return Anti-clockwise value [-90, 90] - // Example A(-12,50), B(-60, 60). Anglex2) { - // Go backward to (x2,y2) - direction = Direction.BACKWARD; - } else if (x2>x1) { - // Go Forward to (x2, y2) - direction = Direction.FORWARD; - } else if (y1>y2) { - // Go Backward to (x2, y2) - direction = Direction.BACKWARD; - } else { - // Go Forward to (x2, y2) - direction = Direction.FORWARD; - } - - myEncoderDrive(direction, speed, distance, 1000); - myEncoderTurn(speed, newOrient - angle); - - /* - // Move 60+ degree Anti-CLOCKWISE - if (turn_angle < -180) { - turn_angle = turn_angle + 180; - myEncoderTurn(0.4, turn_angle); - myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); - myEncoderTurn(0.4, newOrient - angle -180); - } else if(turn_angle > 180) { - turn_angle = turn_angle - 180; - myEncoderTurn(0.4, turn_angle); - myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); - myEncoderTurn(0.4, newOrient - angle + 180); - } else { - myEncoderTurn(0.4, turn_angle); - // Move FWD - myEncoderDrive(Direction.FORWARD, 0.75, distance, 1000); - myEncoderTurn(0.4, newOrient - angle); - } - */ - return direction; - } - - // Run Carousel - public void spinCarousel(double timeout, long sleeptime) - { - runtime.reset(); - - robot.motorCarouselSpin.setPower(-0.8); - while (opModeIsActive() && - runtime.milliseconds() < timeout) - { - telemetry.addData("NFAuto", "Running Carousel for Duck for %f milisec", timeout); - telemetry.update(); - } - robot.motorCarouselSpin.setPower(0); - //sleep(sleeptime); - DuckCount++; - RobotLog.ii("NFAuto", "Number of Duck delivered: %d, timeout %f", DuckCount, timeout); - } - - public void myEncoderTurn(double speed, double degree) - { - double inch_cnt; - - if (Math.abs(degree) < 1) return; - - // Adjust to turn less than 180 - if (degree < -180) { - degree = degree + 360; - } - if (degree > 180) { - degree = degree - 360; - } - - inch_cnt = degree*(COUNTS_FULL_TURN/360); - RobotLog.ii("Input:myEncoderTurn", "Speed/Degree/inch_cnt, %f, %f %f", - speed, degree, inch_cnt); - - if (degree >= 0) { - myEncoderDrive(Direction.ANTI_CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); - } else { - myEncoderDrive(Direction.CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); - } - } - - public void myEncoderDrive(Direction direction, double speed, double Inches, - double timeoutS) { //SensorsToUse sensors_2_use) - int newFrontLeftTarget = 0; - int newFrontRightTarget = 0; - int newBackLeftTarget = 0; - int newBackRightTarget = 0; - int remainingDistance; - int cnt = 0; - double new_speed = 0; - - - RobotLog.ii("Input", "Enter - myEncoderDrive - speed=%f, Inches=%f, timeout=%f", - speed, Inches, timeoutS); - telemetry.addData("Path1", "Enter - myEncoderDrive - speed=%f," + - " Inches=%f, timeout=%f", speed, Inches, timeoutS); - telemetry.update(); - - // Turn off ENCODER - // Reset Encoder beginning to see it gets better. - robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - - RobotLog.ii("Path0", "Starting Position: Front L:%7d R:%7d, Back L:%7d R:%7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - - // Ensure that the op mode is still active - if (opModeIsActive() && !isStopRequested()) { - - // Determine new target position, and pass to motor controller - if (direction == Direction.BACKWARD) { - //Go forward - RobotLog.ii("NFusion", "Moving BACKWARD.... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else if (direction == Direction.FORWARD) { - //Go backward - RobotLog.ii("NFusion", "Moving FORWARD..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.LEFT) { - //Move Right - RobotLog.ii("NFusion", "Moving LEFT..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else if (direction == Direction.RIGHT) { - //Move Left - RobotLog.ii("NFusion", "Moving RIGHT..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.F_RIGHT_DIG) { - //Forward Left Diagonal - RobotLog.ii("NFusion", "Moving F_RIGHT_DIG ..... [%f inches]..", Inches); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else if (direction == Direction.F_LEFT_DIG) { - //Move Forward Right Diagonal - RobotLog.ii("NFusion", "Moving F_LEFT_DIG..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else if (direction == Direction.B_RIGHT_DIG){ - //Move Backward Left Diagonal - RobotLog.ii("NFusion", "Moving B_RIGHT_DIG..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.B_LEFT_DIG) { - //Backward Right Diagonal - RobotLog.ii("NFusion", "Moving B_LEFT_DIG ..... [%f inches]..", Inches); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.ANTI_CLOCK_WISE_TURN) { - // Turn Clock Wise - RobotLog.ii("NFusion", "Turn Anti Clockwise ..... [%f ms]..", timeoutS); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.CLOCK_WISE_TURN) { - // Turn Clock Wise - RobotLog.ii("NFusion", "Turn Clockwise ..... [%f ms]..", timeoutS); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else { - // Do Not move - speed = 0; - } - - // Set the Encoder to the target position. - robot.motorFrontLeft.setTargetPosition(newFrontLeftTarget); - robot.motorFrontRight.setTargetPosition(newFrontRightTarget); - robot.motorBackLeft.setTargetPosition(newBackLeftTarget); - robot.motorBackRight.setTargetPosition(newBackRightTarget); - - // Turn On RUN_TO_POSITION - robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); - robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); - robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); - robot.motorBackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); - - // Set power for the motors. - robot.motorFrontLeft.setPower(Math.abs(speed)); - robot.motorFrontRight.setPower(Math.abs(speed)); - robot.motorBackLeft.setPower(Math.abs(speed)); - robot.motorBackRight.setPower(Math.abs(speed)); - - // reset the timeout time and start motion. - runtime.reset(); - - RobotLog.ii("NFusion", "Final Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", - newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); - - while (opModeIsActive() && - (runtime.milliseconds() < timeoutS) && - ((robot.motorFrontLeft.isBusy() || robot.motorFrontRight.isBusy()) && - (robot.motorBackLeft.isBusy() || robot.motorBackRight.isBusy()))) { - // Display it for the driver every 10 ms - if(runtime.milliseconds() > cnt * 30 ) { // Print every 30 ms - RobotLog.ii("NFusion", "Current Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - telemetry.addData("Path1", "Running to: FL: %7d FR: %7d BL: %7d BR: %7d", - newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); - telemetry.addData("Path2", "Running at: FL: %7d FR: %7d BL: %7d BR: %7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - telemetry.update(); - cnt++; - } - // Reduce the Speed before stopping - remainingDistance = Math.abs(newFrontLeftTarget-robot.motorFrontLeft.getCurrentPosition()); - if (remainingDistance < 10) - { - remainingDistance = Math.abs(newFrontRightTarget-robot.motorFrontRight.getCurrentPosition()); - } - if ((remainingDistance < ENCODER_COUNT_BEFORE_STOP) && (remainingDistance >= 10)){ - new_speed = Math.abs(speed)*(remainingDistance/(float)ENCODER_COUNT_BEFORE_STOP); - robot.motorFrontLeft.setPower(new_speed); - robot.motorFrontRight.setPower(new_speed); - robot.motorBackLeft.setPower(new_speed); - robot.motorBackRight.setPower(new_speed); - if((cnt % 5) == 0) { // skip 5 cnt and print - RobotLog.ii("NFusion", "Remaining Dist: %7d, Speed %f, new Speed %f", - remainingDistance, speed, new_speed); - } - } - } - - //RobotLog.ii("NFusion", "Motor Encoder Status FL: %d, FR: %d, BL: %d, BR: %d", - // motorFrontLeft.isBusy(), - // motorFrontRight.isBusy(), - // motorBackLeft.isBusy(), - //motorBackRight.isBusy()); - - // Stop all motion; - robot.motorFrontLeft.setPower(0); - robot.motorFrontRight.setPower(0); - robot.motorBackLeft.setPower(0); - robot.motorBackRight.setPower(0); - - //sleep(500); // optional pause after each move - - // Turn off ENCODER - // Reset Encoder - //robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - //robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - //robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - //robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - - RobotLog.ii("Path0", "Exit - myEncoderDrive Last Position: Front L:%7d R:%7d, Back L:%7d R:%7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - - //telemetry.addData("Status", "Movement Distance: %7d", - // distance_traveled); - //telemetry.update(); - - } - } +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.RobotLog; + +/** + * + * The desired path in this example is: + * - Drive forward for 48 inches + * - Spin right for 12 Inches + * - Drive Backwards for 24 inches + * - Stop and close the claw. + * + * The code is written using a method called: encoderDrive(speed, leftInches, rightInches, timeoutS) + * that performs the actual movement. + * This methods assumes that each movement is relative to the last stopping place. + * There are other ways to perform encoder based moves, but this method is probably the simplest. + * This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile + * + * Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list + */ + + +@Autonomous(name="Autonomous: AutoBlueWarehouse", group="Autonomous") +//@Disabled +public class AutoBlueWarehouse extends LinearOpMode { + + public enum Direction { + FORWARD, + BACKWARD, + RIGHT, + LEFT, + F_RIGHT_DIG, + F_LEFT_DIG, + B_RIGHT_DIG, + B_LEFT_DIG, + CLOCK_WISE_TURN, + ANTI_CLOCK_WISE_TURN + } + public enum TEST_MODE { + WORKING1, + WORKING2, + WORKING3, + WORKING4, + TEST1, + TEST2, + TEST3, + TEST4 + } + + /* Declare OpMode members. */ + NFMyRobot robot = new NFMyRobot(); // Use NF my Robot h/w + + private final ElapsedTime runtime = new ElapsedTime(); + + //Encoder produced TICK COUNTS per revolution + static final double COUNTS_PER_MOTOR_REV = 537.7; // eg: TETRIX Motor Encoder - 1440, REV Hex Motors: 2240 + static final double DRIVE_GEAR_REDUCTION = 1; //2.0; // This is < 1.0 if geared UP + static final double WHEEL_DIAMETER_INCHES = 3.7; // For figuring circumference + static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / + (WHEEL_DIAMETER_INCHES * 3.1428); + static final double COUNTS_FULL_TURN = 72; + static final int ENCODER_COUNT_BEFORE_STOP = 140; //slow down before 3" + static final int ARM_FULL_TURN_CNT = 537; + + static TEST_MODE TEST_RUN_TYPE = TEST_MODE.WORKING2; + + //static final double DRIVE_SPEED = 1; + //static final double TURN_SPEED = 0.5; + + // Declare our motors + // Make sure your ID's match your configuration + //DcMotor motorFrontLeft = null; + //DcMotor motorBackLeft = null; + //DcMotor motorFrontRight = null; + //DcMotor motorBackRight = null; + + int DuckCount; + double distance; + + @Override + public void runOpMode() /*throws InterruptedException*/ { + Direction dir; + distance = 0; + DuckCount = 0; + /* + * Initialize the drive system variables. + * The init() method of the hardware class does all the work here + */ + robot.init(hardwareMap); + runtime.reset(); + + // Send telemetry message to signify robot waiting; + telemetry.addData("Status", "Starting Autorun"); //Auto run + telemetry.update(); + + // Reset Encoder + robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + // Set Encoder + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + robot.motorBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + // Send telemetry message to indicate successful Encoder reset + telemetry.addData("Path0", "Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.update(); + + // Reverse the right side motors + robot.motorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); + robot.motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE); + + // Wait for the game to start (driver presses PLAY) + waitForStart(); + + if (isStopRequested()) return; + + if (TEST_RUN_TYPE == TEST_MODE.WORKING1) + { + myEncoderDrive(Direction.FORWARD, 0.75, 63.5, 10000); + myEncoderDrive(Direction.RIGHT, 0.6, 30, 5000); + myEncoderDrive(Direction.FORWARD, 0.5, 12, 2000); + } + + if (TEST_RUN_TYPE == TEST_MODE.WORKING2) + { + myEncoderDrive(Direction.LEFT, 0.25, 17, 2000); + myEncoderDrive(Direction.FORWARD, 0.20, 3, 2000); + myArmMove(0.15, 0.35, 2200); + myEncoderTurn(0.25, 75); + //myEncoderDrive(Direction.RIGHT, 0.35, 6, 2000); + myEncoderDrive(Direction.FORWARD, 0.20, 11.5, 2000); + sleep(300); + //ARM Move and Claw Open: + robot.servo_1.setPosition(0.75); + robot.servo_2.setPosition(0.25); + sleep(350); + myEncoderDrive(Direction.BACKWARD, 0.25, 12, 2000); + robot.servo_1.setPosition(1); + robot.servo_2.setPosition(0); + myEncoderTurn(0.25, -75); + myEncoderDrive(Direction.RIGHT, 0.20, 18, 2000); + myEncoderDrive(Direction.LEFT, 0.15, 0.5, 2000); + myEncoderDrive(Direction.BACKWARD, 0.50, 42, 2000); + myEncoderDrive(Direction.LEFT, 0.50, 23, 2000); + myEncoderTurn(0.25, -135); + myEncoderDrive(Direction.RIGHT, 0.20, 3, 2000); + //myEncoderDrive(Direction.BACKWARD, 0.25, 8, 2000); + myArmMove(0.15, 0.20, 1500); + }; + + + //sleep(100); // pause for servos to move + // Set all Motors without Encoder for Manual Run. + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + telemetry.addData("Path", "Complete"); + telemetry.update(); + } + + public void myArmMove(double speed, double rotation, double timeoutS) { //SensorsToUse sensors_2_use) + int newArmPosition = 0; + + // Reset Encoder beginning to see it gets better. + robot.motorArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + newArmPosition = robot.motorArm.getCurrentPosition() + (int) (rotation * ARM_FULL_TURN_CNT); + // Set the Encoder to the target position. + robot.motorArm.setTargetPosition(newArmPosition); + // Turn On RUN_TO_POSITION + robot.motorArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + // Set power for the motors. + robot.motorArm.setPower(Math.abs(speed)); + // reset the timeout time and start motion. + runtime.reset(); + + while (opModeIsActive() && + (runtime.milliseconds() < timeoutS) && + (robot.motorArm.isBusy())) + { + RobotLog.ii("NFusion", "Arm Current Pos: %7d, Target Pos: %7d", + robot.motorArm.getCurrentPosition(), newArmPosition); + } + + robot.motorArm.setPower(0); + //sleep(50); + } + + public Direction moveRobot(double x1, double y1, double currOrient, + double x2, double y2, double newOrient, double speed) + { + double angle; + double turn_angle; + Direction direction = Direction.FORWARD; + + // angle will return Anti-clockwise value [-90, 90] + // Example A(-12,50), B(-60, 60). Anglex2) { + // Go backward to (x2,y2) + direction = Direction.BACKWARD; + } else if (x2>x1) { + // Go Forward to (x2, y2) + direction = Direction.FORWARD; + } else if (y1>y2) { + // Go Backward to (x2, y2) + direction = Direction.BACKWARD; + } else { + // Go Forward to (x2, y2) + direction = Direction.FORWARD; + } + + myEncoderDrive(direction, speed, distance, 1000); + myEncoderTurn(speed, newOrient - angle); + + /* + // Move 60+ degree Anti-CLOCKWISE + if (turn_angle < -180) { + turn_angle = turn_angle + 180; + myEncoderTurn(0.4, turn_angle); + myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle -180); + } else if(turn_angle > 180) { + turn_angle = turn_angle - 180; + myEncoderTurn(0.4, turn_angle); + myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle + 180); + } else { + myEncoderTurn(0.4, turn_angle); + // Move FWD + myEncoderDrive(Direction.FORWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle); + } + */ + return direction; + } + + // Run Carousel + public void spinCarousel(double timeout, double speed) + { + runtime.reset(); + + robot.motorCarouselSpin.setPower(speed); + while (opModeIsActive() && + runtime.milliseconds() < timeout) + { + telemetry.addData("NFAuto", "Running Carousel for Duck for %f milisec", timeout); + telemetry.update(); + } + robot.motorCarouselSpin.setPower(0); + //sleep(sleeptime); + DuckCount++; + RobotLog.ii("NFAuto", "Number of Duck delivered: %d, timeout %f", DuckCount, timeout); + } + + public void myEncoderTurn(double speed, double degree) + { + double inch_cnt; + + if (Math.abs(degree) < 1) return; + + // Adjust to turn less than 180 + if (degree < -180) { + degree = degree + 360; + } + if (degree > 180) { + degree = degree - 360; + } + + inch_cnt = degree*(COUNTS_FULL_TURN/360); + RobotLog.ii("Input:myEncoderTurn", "Speed/Degree/inch_cnt, %f, %f %f", + speed, degree, inch_cnt); + + if (degree >= 0) { + myEncoderDrive(Direction.ANTI_CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); + } else { + myEncoderDrive(Direction.CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); + } + } + + public void myEncoderDrive(Direction direction, double speed, double Inches, + double timeoutS) { //SensorsToUse sensors_2_use) + int newFrontLeftTarget = 0; + int newFrontRightTarget = 0; + int newBackLeftTarget = 0; + int newBackRightTarget = 0; + int remainingDistance; + int cnt = 0; + double new_speed = 0; + + + RobotLog.ii("Input", "Enter - myEncoderDrive - speed=%f, Inches=%f, timeout=%f", + speed, Inches, timeoutS); + telemetry.addData("Path1", "Enter - myEncoderDrive - speed=%f," + + " Inches=%f, timeout=%f", speed, Inches, timeoutS); + telemetry.update(); + + // Turn off ENCODER + // Reset Encoder beginning to see it gets better. + robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + RobotLog.ii("Path0", "Starting Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + + // Ensure that the op mode is still active + if (opModeIsActive() && !isStopRequested()) { + + // Determine new target position, and pass to motor controller + if (direction == Direction.BACKWARD) { + //Go forward + RobotLog.ii("NFusion", "Moving BACKWARD.... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.FORWARD) { + //Go backward + RobotLog.ii("NFusion", "Moving FORWARD..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.LEFT) { + //Move Right + RobotLog.ii("NFusion", "Moving LEFT..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.RIGHT) { + //Move Left + RobotLog.ii("NFusion", "Moving RIGHT..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.F_RIGHT_DIG) { + //Forward Left Diagonal + RobotLog.ii("NFusion", "Moving F_RIGHT_DIG ..... [%f inches]..", Inches); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.F_LEFT_DIG) { + //Move Forward Right Diagonal + RobotLog.ii("NFusion", "Moving F_LEFT_DIG..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.B_RIGHT_DIG){ + //Move Backward Left Diagonal + RobotLog.ii("NFusion", "Moving B_RIGHT_DIG..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.B_LEFT_DIG) { + //Backward Right Diagonal + RobotLog.ii("NFusion", "Moving B_LEFT_DIG ..... [%f inches]..", Inches); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.ANTI_CLOCK_WISE_TURN) { + // Turn Clock Wise + RobotLog.ii("NFusion", "Turn Anti Clockwise ..... [%f ms]..", timeoutS); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.CLOCK_WISE_TURN) { + // Turn Clock Wise + RobotLog.ii("NFusion", "Turn Clockwise ..... [%f ms]..", timeoutS); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else { + // Do Not move + speed = 0; + } + + // Set the Encoder to the target position. + robot.motorFrontLeft.setTargetPosition(newFrontLeftTarget); + robot.motorFrontRight.setTargetPosition(newFrontRightTarget); + robot.motorBackLeft.setTargetPosition(newBackLeftTarget); + robot.motorBackRight.setTargetPosition(newBackRightTarget); + + // Turn On RUN_TO_POSITION + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + // Set power for the motors. + robot.motorFrontLeft.setPower(Math.abs(speed)); + robot.motorFrontRight.setPower(Math.abs(speed)); + robot.motorBackLeft.setPower(Math.abs(speed)); + robot.motorBackRight.setPower(Math.abs(speed)); + + // reset the timeout time and start motion. + runtime.reset(); + + RobotLog.ii("NFusion", "Final Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", + newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); + + while (opModeIsActive() && + (runtime.milliseconds() < timeoutS) && + ((robot.motorFrontLeft.isBusy() || robot.motorFrontRight.isBusy()) && + (robot.motorBackLeft.isBusy() || robot.motorBackRight.isBusy()))) { + // Display it for the driver every 10 ms + if(runtime.milliseconds() > cnt * 30 ) { // Print every 30 ms + RobotLog.ii("NFusion", "Current Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.addData("Path1", "Running to: FL: %7d FR: %7d BL: %7d BR: %7d", + newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); + telemetry.addData("Path2", "Running at: FL: %7d FR: %7d BL: %7d BR: %7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.update(); + cnt++; + } + // Reduce the Speed before stopping + remainingDistance = Math.abs(newFrontLeftTarget-robot.motorFrontLeft.getCurrentPosition()); + if (remainingDistance < 10) + { + remainingDistance = Math.abs(newFrontRightTarget-robot.motorFrontRight.getCurrentPosition()); + } + if ((remainingDistance < ENCODER_COUNT_BEFORE_STOP) && (remainingDistance >= 10)){ + new_speed = Math.abs(speed)*(remainingDistance/(float)ENCODER_COUNT_BEFORE_STOP); + robot.motorFrontLeft.setPower(new_speed); + robot.motorFrontRight.setPower(new_speed); + robot.motorBackLeft.setPower(new_speed); + robot.motorBackRight.setPower(new_speed); + if((cnt % 5) == 0) { // skip 5 cnt and print + RobotLog.ii("NFusion", "Remaining Dist: %7d, Speed %f, new Speed %f", + remainingDistance, speed, new_speed); + } + } + } + + //RobotLog.ii("NFusion", "Motor Encoder Status FL: %d, FR: %d, BL: %d, BR: %d", + // motorFrontLeft.isBusy(), + // motorFrontRight.isBusy(), + // motorBackLeft.isBusy(), + //motorBackRight.isBusy()); + + // Stop all motion; + robot.motorFrontLeft.setPower(0); + robot.motorFrontRight.setPower(0); + robot.motorBackLeft.setPower(0); + robot.motorBackRight.setPower(0); + + //sleep(500); // optional pause after each move + + // Turn off ENCODER + // Reset Encoder + //robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + RobotLog.ii("Path0", "Exit - myEncoderDrive Last Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + + //telemetry.addData("Status", "Movement Distance: %7d", + // distance_traveled); + //telemetry.update(); + + } + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueWarehouseNoHub.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueWarehouseNoHub.java new file mode 100644 index 000000000000..e86b4b349185 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueWarehouseNoHub.java @@ -0,0 +1,541 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.RobotLog; + +/** + * + * The desired path in this example is: + * - Drive forward for 48 inches + * - Spin right for 12 Inches + * - Drive Backwards for 24 inches + * - Stop and close the claw. + * + * The code is written using a method called: encoderDrive(speed, leftInches, rightInches, timeoutS) + * that performs the actual movement. + * This methods assumes that each movement is relative to the last stopping place. + * There are other ways to perform encoder based moves, but this method is probably the simplest. + * This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile + * + * Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list + */ + + +@Autonomous(name="Autonomous: AutoBlueWarehouseNoHub", group="Autonomous") +//@Disabled +public class AutoBlueWarehouseNoHub extends LinearOpMode { + + public enum Direction { + FORWARD, + BACKWARD, + RIGHT, + LEFT, + F_RIGHT_DIG, + F_LEFT_DIG, + B_RIGHT_DIG, + B_LEFT_DIG, + CLOCK_WISE_TURN, + ANTI_CLOCK_WISE_TURN + } + public enum TEST_MODE { + WORKING1, + WORKING2, + WORKING3, + WORKING4, + TEST1, + TEST2, + TEST3, + TEST4 + } + + /* Declare OpMode members. */ + NFMyRobot robot = new NFMyRobot(); // Use NF my Robot h/w + + private final ElapsedTime runtime = new ElapsedTime(); + + //Encoder produced TICK COUNTS per revolution + static final double COUNTS_PER_MOTOR_REV = 537.7; // eg: TETRIX Motor Encoder - 1440, REV Hex Motors: 2240 + static final double DRIVE_GEAR_REDUCTION = 1; //2.0; // This is < 1.0 if geared UP + static final double WHEEL_DIAMETER_INCHES = 3.7; // For figuring circumference + static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / + (WHEEL_DIAMETER_INCHES * 3.1428); + static final double COUNTS_FULL_TURN = 72; + static final int ENCODER_COUNT_BEFORE_STOP = 140; //slow down before 3" + static final int ARM_FULL_TURN_CNT = 537; + + static TEST_MODE TEST_RUN_TYPE = TEST_MODE.WORKING2; + + //static final double DRIVE_SPEED = 1; + //static final double TURN_SPEED = 0.5; + + // Declare our motors + // Make sure your ID's match your configuration + //DcMotor motorFrontLeft = null; + //DcMotor motorBackLeft = null; + //DcMotor motorFrontRight = null; + //DcMotor motorBackRight = null; + + int DuckCount; + double distance; + + @Override + public void runOpMode() /*throws InterruptedException*/ { + Direction dir; + distance = 0; + DuckCount = 0; + /* + * Initialize the drive system variables. + * The init() method of the hardware class does all the work here + */ + robot.init(hardwareMap); + runtime.reset(); + + // Send telemetry message to signify robot waiting; + telemetry.addData("Status", "Starting Autorun"); //Auto run + telemetry.update(); + + // Reset Encoder + robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + // Set Encoder + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + robot.motorBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + // Send telemetry message to indicate successful Encoder reset + telemetry.addData("Path0", "Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.update(); + + // Reverse the right side motors + robot.motorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); + robot.motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE); + + // Wait for the game to start (driver presses PLAY) + waitForStart(); + + if (isStopRequested()) return; + + if (TEST_RUN_TYPE == TEST_MODE.WORKING1) + { + myEncoderDrive(Direction.FORWARD, 0.75, 63.5, 10000); + myEncoderDrive(Direction.RIGHT, 0.6, 30, 5000); + myEncoderDrive(Direction.FORWARD, 0.5, 12, 2000); + } + + if (TEST_RUN_TYPE == TEST_MODE.WORKING2) + { + //myEncoderDrive(Direction.LEFT, 0.25, 17, 2000); + //myArmMove(0.15, 0.35, 2200); + //myEncoderTurn(0.25, 70); + //myEncoderDrive(Direction.RIGHT, 0.35, 6, 2000); + //myEncoderDrive(Direction.FORWARD, 0.20, 13, 2000); + //ARM Move and Claw Open: + //robot.servo_1.setPosition(0.75); + //robot.servo_2.setPosition(0.25); + //sleep(350); + //myEncoderDrive(Direction.BACKWARD, 0.25, 12, 2000); + //robot.servo_1.setPosition(1); + //robot.servo_2.setPosition(0); + //myEncoderTurn(0.25, -70); + //myEncoderDrive(Direction.RIGHT, 0.20, 21, 2000); + myEncoderDrive(Direction.LEFT, 0.15, 0.3, 2000); + myEncoderDrive(Direction.BACKWARD, 0.50, 39, 2000); + myEncoderDrive(Direction.LEFT, 0.50, 26, 2000); + myEncoderDrive(Direction.BACKWARD, 0.25, 8, 2000); + }; + + + //sleep(100); // pause for servos to move + // Set all Motors without Encoder for Manual Run. + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + telemetry.addData("Path", "Complete"); + telemetry.update(); + } + + public void myArmMove(double speed, double rotation, double timeoutS) { //SensorsToUse sensors_2_use) + int newArmPosition = 0; + + // Reset Encoder beginning to see it gets better. + robot.motorArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + newArmPosition = robot.motorArm.getCurrentPosition() + (int) (rotation * ARM_FULL_TURN_CNT); + // Set the Encoder to the target position. + robot.motorArm.setTargetPosition(newArmPosition); + // Turn On RUN_TO_POSITION + robot.motorArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + // Set power for the motors. + robot.motorArm.setPower(Math.abs(speed)); + // reset the timeout time and start motion. + runtime.reset(); + + while (opModeIsActive() && + (runtime.milliseconds() < timeoutS) && + (robot.motorArm.isBusy())) + { + RobotLog.ii("NFusion", "Arm Current Pos: %7d, Target Pos: %7d", + robot.motorArm.getCurrentPosition(), newArmPosition); + } + + robot.motorArm.setPower(0); + //sleep(50); + } + + public Direction moveRobot(double x1, double y1, double currOrient, + double x2, double y2, double newOrient, double speed) + { + double angle; + double turn_angle; + Direction direction = Direction.FORWARD; + + // angle will return Anti-clockwise value [-90, 90] + // Example A(-12,50), B(-60, 60). Anglex2) { + // Go backward to (x2,y2) + direction = Direction.BACKWARD; + } else if (x2>x1) { + // Go Forward to (x2, y2) + direction = Direction.FORWARD; + } else if (y1>y2) { + // Go Backward to (x2, y2) + direction = Direction.BACKWARD; + } else { + // Go Forward to (x2, y2) + direction = Direction.FORWARD; + } + + myEncoderDrive(direction, speed, distance, 1000); + myEncoderTurn(speed, newOrient - angle); + + /* + // Move 60+ degree Anti-CLOCKWISE + if (turn_angle < -180) { + turn_angle = turn_angle + 180; + myEncoderTurn(0.4, turn_angle); + myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle -180); + } else if(turn_angle > 180) { + turn_angle = turn_angle - 180; + myEncoderTurn(0.4, turn_angle); + myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle + 180); + } else { + myEncoderTurn(0.4, turn_angle); + // Move FWD + myEncoderDrive(Direction.FORWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle); + } + */ + return direction; + } + + // Run Carousel + public void spinCarousel(double timeout, double speed) + { + runtime.reset(); + + robot.motorCarouselSpin.setPower(speed); + while (opModeIsActive() && + runtime.milliseconds() < timeout) + { + telemetry.addData("NFAuto", "Running Carousel for Duck for %f milisec", timeout); + telemetry.update(); + } + robot.motorCarouselSpin.setPower(0); + //sleep(sleeptime); + DuckCount++; + RobotLog.ii("NFAuto", "Number of Duck delivered: %d, timeout %f", DuckCount, timeout); + } + + public void myEncoderTurn(double speed, double degree) + { + double inch_cnt; + + if (Math.abs(degree) < 1) return; + + // Adjust to turn less than 180 + if (degree < -180) { + degree = degree + 360; + } + if (degree > 180) { + degree = degree - 360; + } + + inch_cnt = degree*(COUNTS_FULL_TURN/360); + RobotLog.ii("Input:myEncoderTurn", "Speed/Degree/inch_cnt, %f, %f %f", + speed, degree, inch_cnt); + + if (degree >= 0) { + myEncoderDrive(Direction.ANTI_CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); + } else { + myEncoderDrive(Direction.CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); + } + } + + public void myEncoderDrive(Direction direction, double speed, double Inches, + double timeoutS) { //SensorsToUse sensors_2_use) + int newFrontLeftTarget = 0; + int newFrontRightTarget = 0; + int newBackLeftTarget = 0; + int newBackRightTarget = 0; + int remainingDistance; + int cnt = 0; + double new_speed = 0; + + + RobotLog.ii("Input", "Enter - myEncoderDrive - speed=%f, Inches=%f, timeout=%f", + speed, Inches, timeoutS); + telemetry.addData("Path1", "Enter - myEncoderDrive - speed=%f," + + " Inches=%f, timeout=%f", speed, Inches, timeoutS); + telemetry.update(); + + // Turn off ENCODER + // Reset Encoder beginning to see it gets better. + robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + RobotLog.ii("Path0", "Starting Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + + // Ensure that the op mode is still active + if (opModeIsActive() && !isStopRequested()) { + + // Determine new target position, and pass to motor controller + if (direction == Direction.BACKWARD) { + //Go forward + RobotLog.ii("NFusion", "Moving BACKWARD.... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.FORWARD) { + //Go backward + RobotLog.ii("NFusion", "Moving FORWARD..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.LEFT) { + //Move Right + RobotLog.ii("NFusion", "Moving LEFT..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.RIGHT) { + //Move Left + RobotLog.ii("NFusion", "Moving RIGHT..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.F_RIGHT_DIG) { + //Forward Left Diagonal + RobotLog.ii("NFusion", "Moving F_RIGHT_DIG ..... [%f inches]..", Inches); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.F_LEFT_DIG) { + //Move Forward Right Diagonal + RobotLog.ii("NFusion", "Moving F_LEFT_DIG..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.B_RIGHT_DIG){ + //Move Backward Left Diagonal + RobotLog.ii("NFusion", "Moving B_RIGHT_DIG..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.B_LEFT_DIG) { + //Backward Right Diagonal + RobotLog.ii("NFusion", "Moving B_LEFT_DIG ..... [%f inches]..", Inches); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.ANTI_CLOCK_WISE_TURN) { + // Turn Clock Wise + RobotLog.ii("NFusion", "Turn Anti Clockwise ..... [%f ms]..", timeoutS); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.CLOCK_WISE_TURN) { + // Turn Clock Wise + RobotLog.ii("NFusion", "Turn Clockwise ..... [%f ms]..", timeoutS); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else { + // Do Not move + speed = 0; + } + + // Set the Encoder to the target position. + robot.motorFrontLeft.setTargetPosition(newFrontLeftTarget); + robot.motorFrontRight.setTargetPosition(newFrontRightTarget); + robot.motorBackLeft.setTargetPosition(newBackLeftTarget); + robot.motorBackRight.setTargetPosition(newBackRightTarget); + + // Turn On RUN_TO_POSITION + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + // Set power for the motors. + robot.motorFrontLeft.setPower(Math.abs(speed)); + robot.motorFrontRight.setPower(Math.abs(speed)); + robot.motorBackLeft.setPower(Math.abs(speed)); + robot.motorBackRight.setPower(Math.abs(speed)); + + // reset the timeout time and start motion. + runtime.reset(); + + RobotLog.ii("NFusion", "Final Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", + newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); + + while (opModeIsActive() && + (runtime.milliseconds() < timeoutS) && + ((robot.motorFrontLeft.isBusy() || robot.motorFrontRight.isBusy()) && + (robot.motorBackLeft.isBusy() || robot.motorBackRight.isBusy()))) { + // Display it for the driver every 10 ms + if(runtime.milliseconds() > cnt * 30 ) { // Print every 30 ms + RobotLog.ii("NFusion", "Current Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.addData("Path1", "Running to: FL: %7d FR: %7d BL: %7d BR: %7d", + newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); + telemetry.addData("Path2", "Running at: FL: %7d FR: %7d BL: %7d BR: %7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.update(); + cnt++; + } + // Reduce the Speed before stopping + remainingDistance = Math.abs(newFrontLeftTarget-robot.motorFrontLeft.getCurrentPosition()); + if (remainingDistance < 10) + { + remainingDistance = Math.abs(newFrontRightTarget-robot.motorFrontRight.getCurrentPosition()); + } + if ((remainingDistance < ENCODER_COUNT_BEFORE_STOP) && (remainingDistance >= 10)){ + new_speed = Math.abs(speed)*(remainingDistance/(float)ENCODER_COUNT_BEFORE_STOP); + robot.motorFrontLeft.setPower(new_speed); + robot.motorFrontRight.setPower(new_speed); + robot.motorBackLeft.setPower(new_speed); + robot.motorBackRight.setPower(new_speed); + if((cnt % 5) == 0) { // skip 5 cnt and print + RobotLog.ii("NFusion", "Remaining Dist: %7d, Speed %f, new Speed %f", + remainingDistance, speed, new_speed); + } + } + } + + //RobotLog.ii("NFusion", "Motor Encoder Status FL: %d, FR: %d, BL: %d, BR: %d", + // motorFrontLeft.isBusy(), + // motorFrontRight.isBusy(), + // motorBackLeft.isBusy(), + //motorBackRight.isBusy()); + + // Stop all motion; + robot.motorFrontLeft.setPower(0); + robot.motorFrontRight.setPower(0); + robot.motorBackLeft.setPower(0); + robot.motorBackRight.setPower(0); + + //sleep(500); // optional pause after each move + + // Turn off ENCODER + // Reset Encoder + //robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + RobotLog.ii("Path0", "Exit - myEncoderDrive Last Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + + //telemetry.addData("Status", "Movement Distance: %7d", + // distance_traveled); + //telemetry.update(); + + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedForward.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedForward.java index 04697730e118..b725c2ad9550 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedForward.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedForward.java @@ -1,489 +1,568 @@ -/* Copyright (c) 2017 FIRST. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted (subject to the limitations in the disclaimer below) provided that - * the following conditions are met: - * - * Redistributions of source code must retain the above copyright notice, this list - * of conditions and the following disclaimer. - * - * Redistributions in binary form must reproduce the above copyright notice, this - * list of conditions and the following disclaimer in the documentation and/or - * other materials provided with the distribution. - * - * Neither the name of FIRST nor the names of its contributors may be used to endorse or - * promote products derived from this software without specific prior written permission. - * - * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS - * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.util.ElapsedTime; -import com.qualcomm.robotcore.util.RobotLog; -import org.firstinspires.ftc.teamcode.NFMyRobot; - -/** - * - * The desired path in this example is: - * - Drive forward for 48 inches - * - Spin right for 12 Inches - * - Drive Backwards for 24 inches - * - Stop and close the claw. - * - * The code is written using a method called: encoderDrive(speed, leftInches, rightInches, timeoutS) - * that performs the actual movement. - * This methods assumes that each movement is relative to the last stopping place. - * There are other ways to perform encoder based moves, but this method is probably the simplest. - * This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile - * - * Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name. - * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list - */ - - -@Autonomous(name="Autonomous: AutoRedForward", group="Autonomous") -//@Disabled -public class AutoRedForward extends LinearOpMode { - - public enum Direction { - FORWARD, - BACKWARD, - RIGHT, - LEFT, - F_RIGHT_DIG, - F_LEFT_DIG, - B_RIGHT_DIG, - B_LEFT_DIG, - CLOCK_WISE_TURN, - ANTI_CLOCK_WISE_TURN - } - public enum TEST_MODE { - WORKING1, - WORKING2, - WORKING3, - WORKING4, - TEST1, - TEST2, - TEST3, - TEST4 - } - - /* Declare OpMode members. */ - NFMyRobot robot = new NFMyRobot(); // Use NF my Robot h/w - - private final ElapsedTime runtime = new ElapsedTime(); - - //Encoder produced TICK COUNTS per revolution - static final double COUNTS_PER_MOTOR_REV = 537.7; // eg: TETRIX Motor Encoder - 1440, REV Hex Motors: 2240 - static final double DRIVE_GEAR_REDUCTION = 1; //2.0; // This is < 1.0 if geared UP - static final double WHEEL_DIAMETER_INCHES = 3.7; // For figuring circumference - static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / - (WHEEL_DIAMETER_INCHES * 3.1428); - static final double COUNTS_FULL_TURN = 72; - static final int ENCODER_COUNT_BEFORE_STOP = 140; //slow down before 3" - - static TEST_MODE TEST_RUN_TYPE = TEST_MODE.WORKING1; - - //static final double DRIVE_SPEED = 1; - //static final double TURN_SPEED = 0.5; - - // Declare our motors - // Make sure your ID's match your configuration - //DcMotor motorFrontLeft = null; - //DcMotor motorBackLeft = null; - //DcMotor motorFrontRight = null; - //DcMotor motorBackRight = null; - - int DuckCount; - double distance; - - @Override - public void runOpMode() /*throws InterruptedException*/ { - Direction dir; - distance = 0; - DuckCount = 0; - /* - * Initialize the drive system variables. - * The init() method of the hardware class does all the work here - */ - robot.init(hardwareMap); - runtime.reset(); - - // Send telemetry message to signify robot waiting; - telemetry.addData("Status", "Starting Autorun"); //Auto run - telemetry.update(); - - // Reset Encoder - robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - - // Set Encoder - robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.motorBackRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - - robot.motorBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - robot.motorBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - robot.motorFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - robot.motorFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - // Send telemetry message to indicate successful Encoder reset - telemetry.addData("Path0", "Position: Front L:%7d R:%7d, Back L:%7d R:%7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - telemetry.update(); - - // Reverse the right side motors - robot.motorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); - robot.motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE); - - // Wait for the game to start (driver presses PLAY) - waitForStart(); - - if (isStopRequested()) return; - - if (TEST_RUN_TYPE == TEST_MODE.WORKING1) - { - myEncoderDrive(Direction.LEFT, 0.1, 1.5, 2000); - myEncoderTurn(0.2, 5); - myEncoderDrive(Direction.BACKWARD, 0.50, 22, 2000); - myEncoderDrive(Direction.BACKWARD, 0.35, 10.5, 2000); - spinCarousel(2000,500); - myEncoderDrive(Direction.FORWARD, 0.35, 8, 2000); - myEncoderTurn(0.2, -10); - myEncoderDrive(Direction.LEFT, 0.35, 29, 2000); - myEncoderDrive(Direction.BACKWARD, 0.4, 12, 2000); - - }; - - sleep(100); // pause for servos to move - - telemetry.addData("Path", "Complete"); - telemetry.update(); - } - - public Direction moveRobot(double x1, double y1, double currOrient, - double x2, double y2, double newOrient, double speed) - { - double angle; - double turn_angle; - Direction direction = Direction.FORWARD; - - // angle will return Anti-clockwise value [-90, 90] - // Example A(-12,50), B(-60, 60). Anglex2) { - // Go backward to (x2,y2) - direction = Direction.BACKWARD; - } else if (x2>x1) { - // Go Forward to (x2, y2) - direction = Direction.FORWARD; - } else if (y1>y2) { - // Go Backward to (x2, y2) - direction = Direction.BACKWARD; - } else { - // Go Forward to (x2, y2) - direction = Direction.FORWARD; - } - - myEncoderDrive(direction, speed, distance, 1000); - myEncoderTurn(speed, newOrient - angle); - - /* - // Move 60+ degree Anti-CLOCKWISE - if (turn_angle < -180) { - turn_angle = turn_angle + 180; - myEncoderTurn(0.4, turn_angle); - myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); - myEncoderTurn(0.4, newOrient - angle -180); - } else if(turn_angle > 180) { - turn_angle = turn_angle - 180; - myEncoderTurn(0.4, turn_angle); - myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); - myEncoderTurn(0.4, newOrient - angle + 180); - } else { - myEncoderTurn(0.4, turn_angle); - // Move FWD - myEncoderDrive(Direction.FORWARD, 0.75, distance, 1000); - myEncoderTurn(0.4, newOrient - angle); - } - */ - return direction; - } - - // Run Carousel - public void spinCarousel(double timeout, long sleeptime) - { - runtime.reset(); - - robot.motorCarouselSpin.setPower(-0.8); - while (opModeIsActive() && - runtime.milliseconds() < timeout) - { - telemetry.addData("NFAuto", "Running Carousel for Duck for %f milisec", timeout); - telemetry.update(); - } - robot.motorCarouselSpin.setPower(0); - //sleep(sleeptime); - DuckCount++; - RobotLog.ii("NFAuto", "Number of Duck delivered: %d, timeout %f", DuckCount, timeout); - } - - public void myEncoderTurn(double speed, double degree) - { - double inch_cnt; - - if (Math.abs(degree) < 1) return; - - // Adjust to turn less than 180 - if (degree < -180) { - degree = degree + 360; - } - if (degree > 180) { - degree = degree - 360; - } - - inch_cnt = degree*(COUNTS_FULL_TURN/360); - RobotLog.ii("Input:myEncoderTurn", "Speed/Degree/inch_cnt, %f, %f %f", - speed, degree, inch_cnt); - - if (degree >= 0) { - myEncoderDrive(Direction.ANTI_CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); - } else { - myEncoderDrive(Direction.CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); - } - } - - public void myEncoderDrive(Direction direction, double speed, double Inches, - double timeoutS) { //SensorsToUse sensors_2_use) - int newFrontLeftTarget = 0; - int newFrontRightTarget = 0; - int newBackLeftTarget = 0; - int newBackRightTarget = 0; - int remainingDistance; - int cnt = 0; - double new_speed = 0; - - - RobotLog.ii("Input", "Enter - myEncoderDrive - speed=%f, Inches=%f, timeout=%f", - speed, Inches, timeoutS); - telemetry.addData("Path1", "Enter - myEncoderDrive - speed=%f," + - " Inches=%f, timeout=%f", speed, Inches, timeoutS); - telemetry.update(); - - // Turn off ENCODER - // Reset Encoder beginning to see it gets better. - robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - - RobotLog.ii("Path0", "Starting Position: Front L:%7d R:%7d, Back L:%7d R:%7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - - // Ensure that the op mode is still active - if (opModeIsActive() && !isStopRequested()) { - - // Determine new target position, and pass to motor controller - if (direction == Direction.BACKWARD) { - //Go forward - RobotLog.ii("NFusion", "Moving BACKWARD.... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else if (direction == Direction.FORWARD) { - //Go backward - RobotLog.ii("NFusion", "Moving FORWARD..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.LEFT) { - //Move Right - RobotLog.ii("NFusion", "Moving LEFT..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else if (direction == Direction.RIGHT) { - //Move Left - RobotLog.ii("NFusion", "Moving RIGHT..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.F_RIGHT_DIG) { - //Forward Left Diagonal - RobotLog.ii("NFusion", "Moving F_RIGHT_DIG ..... [%f inches]..", Inches); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else if (direction == Direction.F_LEFT_DIG) { - //Move Forward Right Diagonal - RobotLog.ii("NFusion", "Moving F_LEFT_DIG..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else if (direction == Direction.B_RIGHT_DIG){ - //Move Backward Left Diagonal - RobotLog.ii("NFusion", "Moving B_RIGHT_DIG..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.B_LEFT_DIG) { - //Backward Right Diagonal - RobotLog.ii("NFusion", "Moving B_LEFT_DIG ..... [%f inches]..", Inches); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.ANTI_CLOCK_WISE_TURN) { - // Turn Clock Wise - RobotLog.ii("NFusion", "Turn Anti Clockwise ..... [%f ms]..", timeoutS); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.CLOCK_WISE_TURN) { - // Turn Clock Wise - RobotLog.ii("NFusion", "Turn Clockwise ..... [%f ms]..", timeoutS); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else { - // Do Not move - speed = 0; - } - - // Set the Encoder to the target position. - robot.motorFrontLeft.setTargetPosition(newFrontLeftTarget); - robot.motorFrontRight.setTargetPosition(newFrontRightTarget); - robot.motorBackLeft.setTargetPosition(newBackLeftTarget); - robot.motorBackRight.setTargetPosition(newBackRightTarget); - - // Turn On RUN_TO_POSITION - robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); - robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); - robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); - robot.motorBackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); - - // Set power for the motors. - robot.motorFrontLeft.setPower(Math.abs(speed)); - robot.motorFrontRight.setPower(Math.abs(speed)); - robot.motorBackLeft.setPower(Math.abs(speed)); - robot.motorBackRight.setPower(Math.abs(speed)); - - // reset the timeout time and start motion. - runtime.reset(); - - RobotLog.ii("NFusion", "Final Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", - newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); - - while (opModeIsActive() && - (runtime.milliseconds() < timeoutS) && - ((robot.motorFrontLeft.isBusy() || robot.motorFrontRight.isBusy()) && - (robot.motorBackLeft.isBusy() || robot.motorBackRight.isBusy()))) { - // Display it for the driver every 10 ms - if(runtime.milliseconds() > cnt * 30 ) { // Print every 30 ms - RobotLog.ii("NFusion", "Current Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - telemetry.addData("Path1", "Running to: FL: %7d FR: %7d BL: %7d BR: %7d", - newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); - telemetry.addData("Path2", "Running at: FL: %7d FR: %7d BL: %7d BR: %7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - telemetry.update(); - cnt++; - } - // Reduce the Speed before stopping - remainingDistance = Math.abs(newFrontLeftTarget-robot.motorFrontLeft.getCurrentPosition()); - if (remainingDistance < 10) - { - remainingDistance = Math.abs(newFrontRightTarget-robot.motorFrontRight.getCurrentPosition()); - } - if ((remainingDistance < ENCODER_COUNT_BEFORE_STOP) && (remainingDistance >= 10)){ - new_speed = Math.abs(speed)*(remainingDistance/(float)ENCODER_COUNT_BEFORE_STOP); - robot.motorFrontLeft.setPower(new_speed); - robot.motorFrontRight.setPower(new_speed); - robot.motorBackLeft.setPower(new_speed); - robot.motorBackRight.setPower(new_speed); - if((cnt % 5) == 0) { // skip 5 cnt and print - RobotLog.ii("NFusion", "Remaining Dist: %7d, Speed %f, new Speed %f", - remainingDistance, speed, new_speed); - } - } - } - - //RobotLog.ii("NFusion", "Motor Encoder Status FL: %d, FR: %d, BL: %d, BR: %d", - // motorFrontLeft.isBusy(), - // motorFrontRight.isBusy(), - // motorBackLeft.isBusy(), - //motorBackRight.isBusy()); - - // Stop all motion; - robot.motorFrontLeft.setPower(0); - robot.motorFrontRight.setPower(0); - robot.motorBackLeft.setPower(0); - robot.motorBackRight.setPower(0); - - //sleep(500); // optional pause after each move - - // Turn off ENCODER - // Reset Encoder - //robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - //robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - //robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - //robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - - RobotLog.ii("Path0", "Exit - myEncoderDrive Last Position: Front L:%7d R:%7d, Back L:%7d R:%7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - - //telemetry.addData("Status", "Movement Distance: %7d", - // distance_traveled); - //telemetry.update(); - - } - } +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.RobotLog; +import org.firstinspires.ftc.teamcode.NFMyRobot; + +/** + * + * The desired path in this example is: + * - Drive forward for 48 inches + * - Spin right for 12 Inches + * - Drive Backwards for 24 inches + * - Stop and close the claw. + * + * The code is written using a method called: encoderDrive(speed, leftInches, rightInches, timeoutS) + * that performs the actual movement. + * This methods assumes that each movement is relative to the last stopping place. + * There are other ways to perform encoder based moves, but this method is probably the simplest. + * This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile + * + * Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list + */ + + +@Autonomous(name="Autonomous: AutoRedForward", group="Autonomous") +//@Disabled +public class AutoRedForward extends LinearOpMode { + + public enum Direction { + FORWARD, + BACKWARD, + RIGHT, + LEFT, + F_RIGHT_DIG, + F_LEFT_DIG, + B_RIGHT_DIG, + B_LEFT_DIG, + CLOCK_WISE_TURN, + ANTI_CLOCK_WISE_TURN + } + public enum TEST_MODE { + WORKING1, + WORKING2, + WORKING3, + WORKING4, + TEST1, + TEST2, + TEST3, + TEST4 + } + + /* Declare OpMode members. */ + NFMyRobot robot = new NFMyRobot(); // Use NF my Robot h/w + + private final ElapsedTime runtime = new ElapsedTime(); + + //Encoder produced TICK COUNTS per revolution + static final double COUNTS_PER_MOTOR_REV = 537.7; // eg: TETRIX Motor Encoder - 1440, REV Hex Motors: 2240 + static final double DRIVE_GEAR_REDUCTION = 1; //2.0; // This is < 1.0 if geared UP + static final double WHEEL_DIAMETER_INCHES = 3.7; // For figuring circumference + static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / + (WHEEL_DIAMETER_INCHES * 3.1428); + static final double COUNTS_FULL_TURN = 72; + static final int ENCODER_COUNT_BEFORE_STOP = 140; //slow down before 3" + static final int ARM_FULL_TURN_CNT = 537; + + static TEST_MODE TEST_RUN_TYPE = TEST_MODE.WORKING3; + + //static final double DRIVE_SPEED = 1; + //static final double TURN_SPEED = 0.5; + + // Declare our motors + // Make sure your ID's match your configuration + //DcMotor motorFrontLeft = null; + //DcMotor motorBackLeft = null; + //DcMotor motorFrontRight = null; + //DcMotor motorBackRight = null; + + int DuckCount; + double distance; + + @Override + public void runOpMode() /*throws InterruptedException*/ { + Direction dir; + distance = 0; + DuckCount = 0; + /* + * Initialize the drive system variables. + * The init() method of the hardware class does all the work here + */ + robot.init(hardwareMap); + runtime.reset(); + + // Send telemetry message to signify robot waiting; + telemetry.addData("Status", "Starting Autorun"); //Auto run + telemetry.update(); + + // Reset Encoder + robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + // Set Encoder + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + robot.motorBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + // Send telemetry message to indicate successful Encoder reset + telemetry.addData("Path0", "Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.update(); + + // Reverse the right side motors + robot.motorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); + robot.motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE); + + // Wait for the game to start (driver presses PLAY) + waitForStart(); + + if (isStopRequested()) return; + + if (TEST_RUN_TYPE == TEST_MODE.WORKING1) + { + myEncoderDrive(Direction.LEFT, 0.1, 4, 2000); + myEncoderTurn(0.2, 5.5); + myEncoderDrive(Direction.BACKWARD, 0.50, 25, 2000); + myEncoderDrive(Direction.BACKWARD, 0.20, 8, 2000); + spinCarousel(2500,0.50); + myEncoderDrive(Direction.FORWARD, 0.35, 8, 2000); + myEncoderTurn(0.2, -4); + myEncoderDrive(Direction.LEFT, 0.35, 29, 2000); + myEncoderDrive(Direction.BACKWARD, 0.4, 10, 2000); + + } + + if (TEST_RUN_TYPE == TEST_MODE.WORKING2) + { + myEncoderDrive(Direction.LEFT, 0.25, 6, 2000); + myEncoderDrive(Direction.BACKWARD, 0.50, 24, 2000); + myEncoderTurn(0.25, 50); + myEncoderDrive(Direction.LEFT, 0.25, 8, 2000); + myEncoderDrive(Direction.BACKWARD, 0.20, 8, 800); + spinCarousel(2500,-0.50); + myEncoderDrive(Direction.FORWARD, 0.35, 2, 2000); + myEncoderTurn(0.2, -50); + myEncoderDrive(Direction.LEFT, 0.35, 20, 2000); + myEncoderDrive(Direction.BACKWARD, 0.25, 3, 2000); + } + + if (TEST_RUN_TYPE == TEST_MODE.WORKING3) + { + myEncoderDrive(Direction.LEFT, 0.35, 16, 2000); + myArmMove(0.15, 0.35, 2200); + myEncoderTurn(0.2, 68); + myEncoderDrive(Direction.FORWARD, 0.25, 11.5, 2000); + sleep(300); + //ARM Move and Claw Open: + robot.servo_1.setPosition(0.75); + robot.servo_2.setPosition(0.25); + sleep(350); + myEncoderDrive(Direction.BACKWARD, 0.30, 20, 2000); + myEncoderTurn(0.2, -68); + myEncoderDrive(Direction.BACKWARD, 0.50, 23, 2000); + robot.servo_1.setPosition(1); + robot.servo_2.setPosition(0); + myEncoderTurn(0.25, 60); + myEncoderDrive(Direction.LEFT, 0.25, 8, 2000); + myEncoderDrive(Direction.BACKWARD, 0.20, 9, 870); + spinCarousel(2800,-0.60); + myEncoderDrive(Direction.FORWARD, 0.35, 2, 2000); + myEncoderTurn(0.2, -60); + myEncoderDrive(Direction.LEFT, 0.35, 20, 2000); + myEncoderDrive(Direction.BACKWARD, 0.25, 3, 2000); + myArmMove(0.15, -0.20, 1500); + } + //sleep(100); // pause for servos to move + + + // Set all Motors without Encoder for Manual Run. + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + telemetry.addData("Path", "Complete"); + telemetry.update(); + } + + public void myArmMove(double speed, double rotation, double timeoutS) { //SensorsToUse sensors_2_use) + int newArmPosition = 0; + + // Reset Encoder beginning to see it gets better. + robot.motorArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + newArmPosition = robot.motorArm.getCurrentPosition() + (int) (rotation * ARM_FULL_TURN_CNT); + // Set the Encoder to the target position. + robot.motorArm.setTargetPosition(newArmPosition); + // Turn On RUN_TO_POSITION + robot.motorArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + // Set power for the motors. + robot.motorArm.setPower(Math.abs(speed)); + // reset the timeout time and start motion. + runtime.reset(); + + while (opModeIsActive() && + (runtime.milliseconds() < timeoutS) && + (robot.motorArm.isBusy())) + { + RobotLog.ii("NFusion", "Arm Current Pos: %7d, Target Pos: %7d", + robot.motorArm.getCurrentPosition(), newArmPosition); + } + + robot.motorArm.setPower(0); + sleep(50); + } + + public Direction moveRobot(double x1, double y1, double currOrient, + double x2, double y2, double newOrient, double speed) + { + double angle; + double turn_angle; + Direction direction = Direction.FORWARD; + + // angle will return Anti-clockwise value [-90, 90] + // Example A(-12,50), B(-60, 60). Anglex2) { + // Go backward to (x2,y2) + direction = Direction.BACKWARD; + } else if (x2>x1) { + // Go Forward to (x2, y2) + direction = Direction.FORWARD; + } else if (y1>y2) { + // Go Backward to (x2, y2) + direction = Direction.BACKWARD; + } else { + // Go Forward to (x2, y2) + direction = Direction.FORWARD; + } + + myEncoderDrive(direction, speed, distance, 1000); + myEncoderTurn(speed, newOrient - angle); + + /* + // Move 60+ degree Anti-CLOCKWISE + if (turn_angle < -180) { + turn_angle = turn_angle + 180; + myEncoderTurn(0.4, turn_angle); + myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle -180); + } else if(turn_angle > 180) { + turn_angle = turn_angle - 180; + myEncoderTurn(0.4, turn_angle); + myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle + 180); + } else { + myEncoderTurn(0.4, turn_angle); + // Move FWD + myEncoderDrive(Direction.FORWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle); + } + */ + return direction; + } + + // Run Carousel + public void spinCarousel(double timeout, double speed) + { + runtime.reset(); + + robot.motorCarouselSpin.setPower(speed); + while (opModeIsActive() && + runtime.milliseconds() < timeout) + { + telemetry.addData("NFAuto", "Running Carousel for Duck for %f milisec", timeout); + telemetry.update(); + } + robot.motorCarouselSpin.setPower(0); + //sleep(sleeptime); + DuckCount++; + RobotLog.ii("NFAuto", "Number of Duck delivered: %d, timeout %f", DuckCount, timeout); + } + + public void myEncoderTurn(double speed, double degree) + { + double inch_cnt; + + if (Math.abs(degree) < 1) return; + + // Adjust to turn less than 180 + if (degree < -180) { + degree = degree + 360; + } + if (degree > 180) { + degree = degree - 360; + } + + inch_cnt = degree*(COUNTS_FULL_TURN/360); + RobotLog.ii("Input:myEncoderTurn", "Speed/Degree/inch_cnt, %f, %f %f", + speed, degree, inch_cnt); + + if (degree >= 0) { + myEncoderDrive(Direction.ANTI_CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); + } else { + myEncoderDrive(Direction.CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); + } + } + + public void myEncoderDrive(Direction direction, double speed, double Inches, + double timeoutS) { //SensorsToUse sensors_2_use) + int newFrontLeftTarget = 0; + int newFrontRightTarget = 0; + int newBackLeftTarget = 0; + int newBackRightTarget = 0; + int remainingDistance; + int cnt = 0; + double new_speed = 0; + + + RobotLog.ii("Input", "Enter - myEncoderDrive - speed=%f, Inches=%f, timeout=%f", + speed, Inches, timeoutS); + telemetry.addData("Path1", "Enter - myEncoderDrive - speed=%f," + + " Inches=%f, timeout=%f", speed, Inches, timeoutS); + telemetry.update(); + + // Turn off ENCODER + // Reset Encoder beginning to see it gets better. + robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + RobotLog.ii("Path0", "Starting Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + + // Ensure that the op mode is still active + if (opModeIsActive() && !isStopRequested()) { + + // Determine new target position, and pass to motor controller + if (direction == Direction.BACKWARD) { + //Go forward + RobotLog.ii("NFusion", "Moving BACKWARD.... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.FORWARD) { + //Go backward + RobotLog.ii("NFusion", "Moving FORWARD..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.LEFT) { + //Move Right + RobotLog.ii("NFusion", "Moving LEFT..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.RIGHT) { + //Move Left + RobotLog.ii("NFusion", "Moving RIGHT..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.F_RIGHT_DIG) { + //Forward Left Diagonal + RobotLog.ii("NFusion", "Moving F_RIGHT_DIG ..... [%f inches]..", Inches); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.F_LEFT_DIG) { + //Move Forward Right Diagonal + RobotLog.ii("NFusion", "Moving F_LEFT_DIG..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.B_RIGHT_DIG){ + //Move Backward Left Diagonal + RobotLog.ii("NFusion", "Moving B_RIGHT_DIG..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.B_LEFT_DIG) { + //Backward Right Diagonal + RobotLog.ii("NFusion", "Moving B_LEFT_DIG ..... [%f inches]..", Inches); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.ANTI_CLOCK_WISE_TURN) { + // Turn Clock Wise + RobotLog.ii("NFusion", "Turn Anti Clockwise ..... [%f ms]..", timeoutS); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.CLOCK_WISE_TURN) { + // Turn Clock Wise + RobotLog.ii("NFusion", "Turn Clockwise ..... [%f ms]..", timeoutS); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else { + // Do Not move + speed = 0; + } + + // Set the Encoder to the target position. + robot.motorFrontLeft.setTargetPosition(newFrontLeftTarget); + robot.motorFrontRight.setTargetPosition(newFrontRightTarget); + robot.motorBackLeft.setTargetPosition(newBackLeftTarget); + robot.motorBackRight.setTargetPosition(newBackRightTarget); + + // Turn On RUN_TO_POSITION + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + // Set power for the motors. + robot.motorFrontLeft.setPower(Math.abs(speed)); + robot.motorFrontRight.setPower(Math.abs(speed)); + robot.motorBackLeft.setPower(Math.abs(speed)); + robot.motorBackRight.setPower(Math.abs(speed)); + + // reset the timeout time and start motion. + runtime.reset(); + + RobotLog.ii("NFusion", "Final Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", + newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); + + while (opModeIsActive() && + (runtime.milliseconds() < timeoutS) && + ((robot.motorFrontLeft.isBusy() || robot.motorFrontRight.isBusy()) && + (robot.motorBackLeft.isBusy() || robot.motorBackRight.isBusy()))) { + // Display it for the driver every 10 ms + if(runtime.milliseconds() > cnt * 30 ) { // Print every 30 ms + RobotLog.ii("NFusion", "Current Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.addData("Path1", "Running to: FL: %7d FR: %7d BL: %7d BR: %7d", + newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); + telemetry.addData("Path2", "Running at: FL: %7d FR: %7d BL: %7d BR: %7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.update(); + cnt++; + } + // Reduce the Speed before stopping + remainingDistance = Math.abs(newFrontLeftTarget-robot.motorFrontLeft.getCurrentPosition()); + if (remainingDistance < 10) + { + remainingDistance = Math.abs(newFrontRightTarget-robot.motorFrontRight.getCurrentPosition()); + } + if ((remainingDistance < ENCODER_COUNT_BEFORE_STOP) && (remainingDistance >= 10)){ + new_speed = Math.abs(speed)*(remainingDistance/(float)ENCODER_COUNT_BEFORE_STOP); + robot.motorFrontLeft.setPower(new_speed); + robot.motorFrontRight.setPower(new_speed); + robot.motorBackLeft.setPower(new_speed); + robot.motorBackRight.setPower(new_speed); + if((cnt % 5) == 0) { // skip 5 cnt and print + RobotLog.ii("NFusion", "Remaining Dist: %7d, Speed %f, new Speed %f", + remainingDistance, speed, new_speed); + } + } + } + + //RobotLog.ii("NFusion", "Motor Encoder Status FL: %d, FR: %d, BL: %d, BR: %d", + // motorFrontLeft.isBusy(), + // motorFrontRight.isBusy(), + // motorBackLeft.isBusy(), + //motorBackRight.isBusy()); + + // Stop all motion; + robot.motorFrontLeft.setPower(0); + robot.motorFrontRight.setPower(0); + robot.motorBackLeft.setPower(0); + robot.motorBackRight.setPower(0); + + //sleep(500); // optional pause after each move + + // Turn off ENCODER + // Reset Encoder + //robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + RobotLog.ii("Path0", "Exit - myEncoderDrive Last Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + + //telemetry.addData("Status", "Movement Distance: %7d", + // distance_traveled); + //telemetry.update(); + + } + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedForwardNoHub.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedForwardNoHub.java new file mode 100644 index 000000000000..534df3561244 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedForwardNoHub.java @@ -0,0 +1,565 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.RobotLog; + +/** + * + * The desired path in this example is: + * - Drive forward for 48 inches + * - Spin right for 12 Inches + * - Drive Backwards for 24 inches + * - Stop and close the claw. + * + * The code is written using a method called: encoderDrive(speed, leftInches, rightInches, timeoutS) + * that performs the actual movement. + * This methods assumes that each movement is relative to the last stopping place. + * There are other ways to perform encoder based moves, but this method is probably the simplest. + * This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile + * + * Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list + */ + + +@Autonomous(name="Autonomous: AutoRedForward_No_Hub", group="Autonomous") +//@Disabled +public class AutoRedForwardNoHub extends LinearOpMode { + + public enum Direction { + FORWARD, + BACKWARD, + RIGHT, + LEFT, + F_RIGHT_DIG, + F_LEFT_DIG, + B_RIGHT_DIG, + B_LEFT_DIG, + CLOCK_WISE_TURN, + ANTI_CLOCK_WISE_TURN + } + public enum TEST_MODE { + WORKING1, + WORKING2, + WORKING3, + WORKING4, + TEST1, + TEST2, + TEST3, + TEST4 + } + + /* Declare OpMode members. */ + NFMyRobot robot = new NFMyRobot(); // Use NF my Robot h/w + + private final ElapsedTime runtime = new ElapsedTime(); + + //Encoder produced TICK COUNTS per revolution + static final double COUNTS_PER_MOTOR_REV = 537.7; // eg: TETRIX Motor Encoder - 1440, REV Hex Motors: 2240 + static final double DRIVE_GEAR_REDUCTION = 1; //2.0; // This is < 1.0 if geared UP + static final double WHEEL_DIAMETER_INCHES = 3.7; // For figuring circumference + static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / + (WHEEL_DIAMETER_INCHES * 3.1428); + static final double COUNTS_FULL_TURN = 72; + static final int ENCODER_COUNT_BEFORE_STOP = 140; //slow down before 3" + static final int ARM_FULL_TURN_CNT = 537; + + static TEST_MODE TEST_RUN_TYPE = TEST_MODE.WORKING2; + + //static final double DRIVE_SPEED = 1; + //static final double TURN_SPEED = 0.5; + + // Declare our motors + // Make sure your ID's match your configuration + //DcMotor motorFrontLeft = null; + //DcMotor motorBackLeft = null; + //DcMotor motorFrontRight = null; + //DcMotor motorBackRight = null; + + int DuckCount; + double distance; + + @Override + public void runOpMode() /*throws InterruptedException*/ { + Direction dir; + distance = 0; + DuckCount = 0; + /* + * Initialize the drive system variables. + * The init() method of the hardware class does all the work here + */ + robot.init(hardwareMap); + runtime.reset(); + + // Send telemetry message to signify robot waiting; + telemetry.addData("Status", "Starting Autorun"); //Auto run + telemetry.update(); + + // Reset Encoder + robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + // Set Encoder + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + robot.motorBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + // Send telemetry message to indicate successful Encoder reset + telemetry.addData("Path0", "Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.update(); + + // Reverse the right side motors + robot.motorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); + robot.motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE); + + // Wait for the game to start (driver presses PLAY) + waitForStart(); + + if (isStopRequested()) return; + + if (TEST_RUN_TYPE == TEST_MODE.WORKING1) + { + myEncoderDrive(Direction.LEFT, 0.1, 4, 2000); + myEncoderTurn(0.2, 5.5); + myEncoderDrive(Direction.BACKWARD, 0.50, 25, 2000); + myEncoderDrive(Direction.BACKWARD, 0.20, 8, 2000); + spinCarousel(2500,0.50); + myEncoderDrive(Direction.FORWARD, 0.35, 8, 2000); + myEncoderTurn(0.2, -4); + myEncoderDrive(Direction.LEFT, 0.35, 29, 2000); + myEncoderDrive(Direction.BACKWARD, 0.4, 10, 2000); + + } + + if (TEST_RUN_TYPE == TEST_MODE.WORKING2) + { + myEncoderDrive(Direction.LEFT, 0.25, 6, 2000); + myEncoderDrive(Direction.BACKWARD, 0.50, 24, 2000); + myEncoderTurn(0.25, 50); + myEncoderDrive(Direction.LEFT, 0.25, 8, 2000); + myEncoderDrive(Direction.BACKWARD, 0.20, 8, 800); + spinCarousel(2500,-0.50); + myEncoderDrive(Direction.FORWARD, 0.35, 2, 2000); + myEncoderTurn(0.2, -50); + myEncoderDrive(Direction.LEFT, 0.35, 20, 2000); + myEncoderDrive(Direction.BACKWARD, 0.25, 3, 2000); + } + + if (TEST_RUN_TYPE == TEST_MODE.WORKING3) + { + myEncoderDrive(Direction.LEFT, 0.35, 16, 2000); + myArmMove(0.15, 0.35, 2200); + myEncoderTurn(0.2, 68); + myEncoderDrive(Direction.FORWARD, 0.25, 12.5, 2000); + //ARM Move and Claw Open: + robot.servo_1.setPosition(0.75); + robot.servo_2.setPosition(0.25); + sleep(350); + myEncoderDrive(Direction.BACKWARD, 0.30, 20, 2000); + myEncoderTurn(0.2, -68); + myEncoderDrive(Direction.BACKWARD, 0.50, 23, 2000); + myEncoderTurn(0.25, 60); + robot.servo_1.setPosition(1); + robot.servo_2.setPosition(0); + myEncoderDrive(Direction.LEFT, 0.25, 8, 2000); + myEncoderDrive(Direction.BACKWARD, 0.20, 9, 870); + spinCarousel(2800,-0.60); + myEncoderDrive(Direction.FORWARD, 0.35, 2, 2000); + myEncoderTurn(0.2, -60); + myEncoderDrive(Direction.LEFT, 0.35, 20, 2000); + myEncoderDrive(Direction.BACKWARD, 0.25, 3, 2000); + } + //sleep(100); // pause for servos to move + + + // Set all Motors without Encoder for Manual Run. + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + telemetry.addData("Path", "Complete"); + telemetry.update(); + } + + public void myArmMove(double speed, double rotation, double timeoutS) { //SensorsToUse sensors_2_use) + int newArmPosition = 0; + + // Reset Encoder beginning to see it gets better. + robot.motorArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + newArmPosition = robot.motorArm.getCurrentPosition() + (int) (rotation * ARM_FULL_TURN_CNT); + // Set the Encoder to the target position. + robot.motorArm.setTargetPosition(newArmPosition); + // Turn On RUN_TO_POSITION + robot.motorArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + // Set power for the motors. + robot.motorArm.setPower(Math.abs(speed)); + // reset the timeout time and start motion. + runtime.reset(); + + while (opModeIsActive() && + (runtime.milliseconds() < timeoutS) && + (robot.motorArm.isBusy())) + { + RobotLog.ii("NFusion", "Arm Current Pos: %7d, Target Pos: %7d", + robot.motorArm.getCurrentPosition(), newArmPosition); + } + + robot.motorArm.setPower(0); + sleep(50); + } + + public Direction moveRobot(double x1, double y1, double currOrient, + double x2, double y2, double newOrient, double speed) + { + double angle; + double turn_angle; + Direction direction = Direction.FORWARD; + + // angle will return Anti-clockwise value [-90, 90] + // Example A(-12,50), B(-60, 60). Anglex2) { + // Go backward to (x2,y2) + direction = Direction.BACKWARD; + } else if (x2>x1) { + // Go Forward to (x2, y2) + direction = Direction.FORWARD; + } else if (y1>y2) { + // Go Backward to (x2, y2) + direction = Direction.BACKWARD; + } else { + // Go Forward to (x2, y2) + direction = Direction.FORWARD; + } + + myEncoderDrive(direction, speed, distance, 1000); + myEncoderTurn(speed, newOrient - angle); + + /* + // Move 60+ degree Anti-CLOCKWISE + if (turn_angle < -180) { + turn_angle = turn_angle + 180; + myEncoderTurn(0.4, turn_angle); + myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle -180); + } else if(turn_angle > 180) { + turn_angle = turn_angle - 180; + myEncoderTurn(0.4, turn_angle); + myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle + 180); + } else { + myEncoderTurn(0.4, turn_angle); + // Move FWD + myEncoderDrive(Direction.FORWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle); + } + */ + return direction; + } + + // Run Carousel + public void spinCarousel(double timeout, double speed) + { + runtime.reset(); + + robot.motorCarouselSpin.setPower(speed); + while (opModeIsActive() && + runtime.milliseconds() < timeout) + { + telemetry.addData("NFAuto", "Running Carousel for Duck for %f milisec", timeout); + telemetry.update(); + } + robot.motorCarouselSpin.setPower(0); + //sleep(sleeptime); + DuckCount++; + RobotLog.ii("NFAuto", "Number of Duck delivered: %d, timeout %f", DuckCount, timeout); + } + + public void myEncoderTurn(double speed, double degree) + { + double inch_cnt; + + if (Math.abs(degree) < 1) return; + + // Adjust to turn less than 180 + if (degree < -180) { + degree = degree + 360; + } + if (degree > 180) { + degree = degree - 360; + } + + inch_cnt = degree*(COUNTS_FULL_TURN/360); + RobotLog.ii("Input:myEncoderTurn", "Speed/Degree/inch_cnt, %f, %f %f", + speed, degree, inch_cnt); + + if (degree >= 0) { + myEncoderDrive(Direction.ANTI_CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); + } else { + myEncoderDrive(Direction.CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); + } + } + + public void myEncoderDrive(Direction direction, double speed, double Inches, + double timeoutS) { //SensorsToUse sensors_2_use) + int newFrontLeftTarget = 0; + int newFrontRightTarget = 0; + int newBackLeftTarget = 0; + int newBackRightTarget = 0; + int remainingDistance; + int cnt = 0; + double new_speed = 0; + + + RobotLog.ii("Input", "Enter - myEncoderDrive - speed=%f, Inches=%f, timeout=%f", + speed, Inches, timeoutS); + telemetry.addData("Path1", "Enter - myEncoderDrive - speed=%f," + + " Inches=%f, timeout=%f", speed, Inches, timeoutS); + telemetry.update(); + + // Turn off ENCODER + // Reset Encoder beginning to see it gets better. + robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + RobotLog.ii("Path0", "Starting Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + + // Ensure that the op mode is still active + if (opModeIsActive() && !isStopRequested()) { + + // Determine new target position, and pass to motor controller + if (direction == Direction.BACKWARD) { + //Go forward + RobotLog.ii("NFusion", "Moving BACKWARD.... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.FORWARD) { + //Go backward + RobotLog.ii("NFusion", "Moving FORWARD..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.LEFT) { + //Move Right + RobotLog.ii("NFusion", "Moving LEFT..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.RIGHT) { + //Move Left + RobotLog.ii("NFusion", "Moving RIGHT..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.F_RIGHT_DIG) { + //Forward Left Diagonal + RobotLog.ii("NFusion", "Moving F_RIGHT_DIG ..... [%f inches]..", Inches); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.F_LEFT_DIG) { + //Move Forward Right Diagonal + RobotLog.ii("NFusion", "Moving F_LEFT_DIG..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.B_RIGHT_DIG){ + //Move Backward Left Diagonal + RobotLog.ii("NFusion", "Moving B_RIGHT_DIG..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.B_LEFT_DIG) { + //Backward Right Diagonal + RobotLog.ii("NFusion", "Moving B_LEFT_DIG ..... [%f inches]..", Inches); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.ANTI_CLOCK_WISE_TURN) { + // Turn Clock Wise + RobotLog.ii("NFusion", "Turn Anti Clockwise ..... [%f ms]..", timeoutS); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.CLOCK_WISE_TURN) { + // Turn Clock Wise + RobotLog.ii("NFusion", "Turn Clockwise ..... [%f ms]..", timeoutS); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else { + // Do Not move + speed = 0; + } + + // Set the Encoder to the target position. + robot.motorFrontLeft.setTargetPosition(newFrontLeftTarget); + robot.motorFrontRight.setTargetPosition(newFrontRightTarget); + robot.motorBackLeft.setTargetPosition(newBackLeftTarget); + robot.motorBackRight.setTargetPosition(newBackRightTarget); + + // Turn On RUN_TO_POSITION + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + // Set power for the motors. + robot.motorFrontLeft.setPower(Math.abs(speed)); + robot.motorFrontRight.setPower(Math.abs(speed)); + robot.motorBackLeft.setPower(Math.abs(speed)); + robot.motorBackRight.setPower(Math.abs(speed)); + + // reset the timeout time and start motion. + runtime.reset(); + + RobotLog.ii("NFusion", "Final Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", + newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); + + while (opModeIsActive() && + (runtime.milliseconds() < timeoutS) && + ((robot.motorFrontLeft.isBusy() || robot.motorFrontRight.isBusy()) && + (robot.motorBackLeft.isBusy() || robot.motorBackRight.isBusy()))) { + // Display it for the driver every 10 ms + if(runtime.milliseconds() > cnt * 30 ) { // Print every 30 ms + RobotLog.ii("NFusion", "Current Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.addData("Path1", "Running to: FL: %7d FR: %7d BL: %7d BR: %7d", + newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); + telemetry.addData("Path2", "Running at: FL: %7d FR: %7d BL: %7d BR: %7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.update(); + cnt++; + } + // Reduce the Speed before stopping + remainingDistance = Math.abs(newFrontLeftTarget-robot.motorFrontLeft.getCurrentPosition()); + if (remainingDistance < 10) + { + remainingDistance = Math.abs(newFrontRightTarget-robot.motorFrontRight.getCurrentPosition()); + } + if ((remainingDistance < ENCODER_COUNT_BEFORE_STOP) && (remainingDistance >= 10)){ + new_speed = Math.abs(speed)*(remainingDistance/(float)ENCODER_COUNT_BEFORE_STOP); + robot.motorFrontLeft.setPower(new_speed); + robot.motorFrontRight.setPower(new_speed); + robot.motorBackLeft.setPower(new_speed); + robot.motorBackRight.setPower(new_speed); + if((cnt % 5) == 0) { // skip 5 cnt and print + RobotLog.ii("NFusion", "Remaining Dist: %7d, Speed %f, new Speed %f", + remainingDistance, speed, new_speed); + } + } + } + + //RobotLog.ii("NFusion", "Motor Encoder Status FL: %d, FR: %d, BL: %d, BR: %d", + // motorFrontLeft.isBusy(), + // motorFrontRight.isBusy(), + // motorBackLeft.isBusy(), + //motorBackRight.isBusy()); + + // Stop all motion; + robot.motorFrontLeft.setPower(0); + robot.motorFrontRight.setPower(0); + robot.motorBackLeft.setPower(0); + robot.motorBackRight.setPower(0); + + //sleep(500); // optional pause after each move + + // Turn off ENCODER + // Reset Encoder + //robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + RobotLog.ii("Path0", "Exit - myEncoderDrive Last Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + + //telemetry.addData("Status", "Movement Distance: %7d", + // distance_traveled); + //telemetry.update(); + + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedWarehouse.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedWarehouse.java index 4f69ebae18f4..638dd3ab2ee6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedWarehouse.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedWarehouse.java @@ -1,482 +1,556 @@ -/* Copyright (c) 2017 FIRST. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted (subject to the limitations in the disclaimer below) provided that - * the following conditions are met: - * - * Redistributions of source code must retain the above copyright notice, this list - * of conditions and the following disclaimer. - * - * Redistributions in binary form must reproduce the above copyright notice, this - * list of conditions and the following disclaimer in the documentation and/or - * other materials provided with the distribution. - * - * Neither the name of FIRST nor the names of its contributors may be used to endorse or - * promote products derived from this software without specific prior written permission. - * - * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS - * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.util.ElapsedTime; -import com.qualcomm.robotcore.util.RobotLog; - -/** - * - * The desired path in this example is: - * - Drive forward for 48 inches - * - Spin right for 12 Inches - * - Drive Backwards for 24 inches - * - Stop and close the claw. - * - * The code is written using a method called: encoderDrive(speed, leftInches, rightInches, timeoutS) - * that performs the actual movement. - * This methods assumes that each movement is relative to the last stopping place. - * There are other ways to perform encoder based moves, but this method is probably the simplest. - * This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile - * - * Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name. - * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list - */ - - -@Autonomous(name="Autonomous: AutoRedWarehouse", group="Autonomous") -//@Disabled -public class AutoRedWarehouse extends LinearOpMode { - - public enum Direction { - FORWARD, - BACKWARD, - RIGHT, - LEFT, - F_RIGHT_DIG, - F_LEFT_DIG, - B_RIGHT_DIG, - B_LEFT_DIG, - CLOCK_WISE_TURN, - ANTI_CLOCK_WISE_TURN - } - public enum TEST_MODE { - WORKING1, - WORKING2, - WORKING3, - WORKING4, - TEST1, - TEST2, - TEST3, - TEST4 - } - - /* Declare OpMode members. */ - NFMyRobot robot = new NFMyRobot(); // Use NF my Robot h/w - - private final ElapsedTime runtime = new ElapsedTime(); - - //Encoder produced TICK COUNTS per revolution - static final double COUNTS_PER_MOTOR_REV = 537.7; // eg: TETRIX Motor Encoder - 1440, REV Hex Motors: 2240 - static final double DRIVE_GEAR_REDUCTION = 1; //2.0; // This is < 1.0 if geared UP - static final double WHEEL_DIAMETER_INCHES = 3.7; // For figuring circumference - static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / - (WHEEL_DIAMETER_INCHES * 3.1428); - static final double COUNTS_FULL_TURN = 72; - static final int ENCODER_COUNT_BEFORE_STOP = 140; //slow down before 3" - - static TEST_MODE TEST_RUN_TYPE = TEST_MODE.WORKING1; - - //static final double DRIVE_SPEED = 1; - //static final double TURN_SPEED = 0.5; - - // Declare our motors - // Make sure your ID's match your configuration - //DcMotor motorFrontLeft = null; - //DcMotor motorBackLeft = null; - //DcMotor motorFrontRight = null; - //DcMotor motorBackRight = null; - - int DuckCount; - double distance; - - @Override - public void runOpMode() /*throws InterruptedException*/ { - Direction dir; - distance = 0; - DuckCount = 0; - /* - * Initialize the drive system variables. - * The init() method of the hardware class does all the work here - */ - robot.init(hardwareMap); - runtime.reset(); - - // Send telemetry message to signify robot waiting; - telemetry.addData("Status", "Starting Autorun"); //Auto run - telemetry.update(); - - // Reset Encoder - robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - - // Set Encoder - robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.motorBackRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - - robot.motorBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - robot.motorBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - robot.motorFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - robot.motorFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - // Send telemetry message to indicate successful Encoder reset - telemetry.addData("Path0", "Position: Front L:%7d R:%7d, Back L:%7d R:%7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - telemetry.update(); - - // Reverse the right side motors - robot.motorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); - robot.motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE); - - // Wait for the game to start (driver presses PLAY) - waitForStart(); - - if (isStopRequested()) return; - - if (TEST_RUN_TYPE == TEST_MODE.WORKING1) - { - myEncoderDrive(Direction.FORWARD, 0.75, 63.5, 10000); - myEncoderDrive(Direction.LEFT, 0.6, 30, 5000); - myEncoderDrive(Direction.FORWARD, 0.5, 12, 2000); - - }; - - sleep(100); // pause for servos to move - - telemetry.addData("Path", "Complete"); - telemetry.update(); - } - - public Direction moveRobot(double x1, double y1, double currOrient, - double x2, double y2, double newOrient, double speed) - { - double angle; - double turn_angle; - Direction direction = Direction.FORWARD; - - // angle will return Anti-clockwise value [-90, 90] - // Example A(-12,50), B(-60, 60). Anglex2) { - // Go backward to (x2,y2) - direction = Direction.BACKWARD; - } else if (x2>x1) { - // Go Forward to (x2, y2) - direction = Direction.FORWARD; - } else if (y1>y2) { - // Go Backward to (x2, y2) - direction = Direction.BACKWARD; - } else { - // Go Forward to (x2, y2) - direction = Direction.FORWARD; - } - - myEncoderDrive(direction, speed, distance, 1000); - myEncoderTurn(speed, newOrient - angle); - - /* - // Move 60+ degree Anti-CLOCKWISE - if (turn_angle < -180) { - turn_angle = turn_angle + 180; - myEncoderTurn(0.4, turn_angle); - myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); - myEncoderTurn(0.4, newOrient - angle -180); - } else if(turn_angle > 180) { - turn_angle = turn_angle - 180; - myEncoderTurn(0.4, turn_angle); - myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); - myEncoderTurn(0.4, newOrient - angle + 180); - } else { - myEncoderTurn(0.4, turn_angle); - // Move FWD - myEncoderDrive(Direction.FORWARD, 0.75, distance, 1000); - myEncoderTurn(0.4, newOrient - angle); - } - */ - return direction; - } - - // Run Carousel - public void spinCarousel(double timeout, long sleeptime) - { - runtime.reset(); - - robot.motorCarouselSpin.setPower(-0.8); - while (opModeIsActive() && - runtime.milliseconds() < timeout) - { - telemetry.addData("NFAuto", "Running Carousel for Duck for %f milisec", timeout); - telemetry.update(); - } - robot.motorCarouselSpin.setPower(0); - //sleep(sleeptime); - DuckCount++; - RobotLog.ii("NFAuto", "Number of Duck delivered: %d, timeout %f", DuckCount, timeout); - } - - public void myEncoderTurn(double speed, double degree) - { - double inch_cnt; - - if (Math.abs(degree) < 1) return; - - // Adjust to turn less than 180 - if (degree < -180) { - degree = degree + 360; - } - if (degree > 180) { - degree = degree - 360; - } - - inch_cnt = degree*(COUNTS_FULL_TURN/360); - RobotLog.ii("Input:myEncoderTurn", "Speed/Degree/inch_cnt, %f, %f %f", - speed, degree, inch_cnt); - - if (degree >= 0) { - myEncoderDrive(Direction.ANTI_CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); - } else { - myEncoderDrive(Direction.CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); - } - } - - public void myEncoderDrive(Direction direction, double speed, double Inches, - double timeoutS) { //SensorsToUse sensors_2_use) - int newFrontLeftTarget = 0; - int newFrontRightTarget = 0; - int newBackLeftTarget = 0; - int newBackRightTarget = 0; - int remainingDistance; - int cnt = 0; - double new_speed = 0; - - - RobotLog.ii("Input", "Enter - myEncoderDrive - speed=%f, Inches=%f, timeout=%f", - speed, Inches, timeoutS); - telemetry.addData("Path1", "Enter - myEncoderDrive - speed=%f," + - " Inches=%f, timeout=%f", speed, Inches, timeoutS); - telemetry.update(); - - // Turn off ENCODER - // Reset Encoder beginning to see it gets better. - robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - - RobotLog.ii("Path0", "Starting Position: Front L:%7d R:%7d, Back L:%7d R:%7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - - // Ensure that the op mode is still active - if (opModeIsActive() && !isStopRequested()) { - - // Determine new target position, and pass to motor controller - if (direction == Direction.BACKWARD) { - //Go forward - RobotLog.ii("NFusion", "Moving BACKWARD.... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else if (direction == Direction.FORWARD) { - //Go backward - RobotLog.ii("NFusion", "Moving FORWARD..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.LEFT) { - //Move Right - RobotLog.ii("NFusion", "Moving LEFT..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else if (direction == Direction.RIGHT) { - //Move Left - RobotLog.ii("NFusion", "Moving RIGHT..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.F_RIGHT_DIG) { - //Forward Left Diagonal - RobotLog.ii("NFusion", "Moving F_RIGHT_DIG ..... [%f inches]..", Inches); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else if (direction == Direction.F_LEFT_DIG) { - //Move Forward Right Diagonal - RobotLog.ii("NFusion", "Moving F_LEFT_DIG..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else if (direction == Direction.B_RIGHT_DIG){ - //Move Backward Left Diagonal - RobotLog.ii("NFusion", "Moving B_RIGHT_DIG..... [%f inches]..", Inches); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.B_LEFT_DIG) { - //Backward Right Diagonal - RobotLog.ii("NFusion", "Moving B_LEFT_DIG ..... [%f inches]..", Inches); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.ANTI_CLOCK_WISE_TURN) { - // Turn Clock Wise - RobotLog.ii("NFusion", "Turn Anti Clockwise ..... [%f ms]..", timeoutS); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - } else if (direction == Direction.CLOCK_WISE_TURN) { - // Turn Clock Wise - RobotLog.ii("NFusion", "Turn Clockwise ..... [%f ms]..", timeoutS); - newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); - newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); - } else { - // Do Not move - speed = 0; - } - - // Set the Encoder to the target position. - robot.motorFrontLeft.setTargetPosition(newFrontLeftTarget); - robot.motorFrontRight.setTargetPosition(newFrontRightTarget); - robot.motorBackLeft.setTargetPosition(newBackLeftTarget); - robot.motorBackRight.setTargetPosition(newBackRightTarget); - - // Turn On RUN_TO_POSITION - robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); - robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); - robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); - robot.motorBackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); - - // Set power for the motors. - robot.motorFrontLeft.setPower(Math.abs(speed)); - robot.motorFrontRight.setPower(Math.abs(speed)); - robot.motorBackLeft.setPower(Math.abs(speed)); - robot.motorBackRight.setPower(Math.abs(speed)); - - // reset the timeout time and start motion. - runtime.reset(); - - RobotLog.ii("NFusion", "Final Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", - newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); - - while (opModeIsActive() && - (runtime.milliseconds() < timeoutS) && - ((robot.motorFrontLeft.isBusy() || robot.motorFrontRight.isBusy()) && - (robot.motorBackLeft.isBusy() || robot.motorBackRight.isBusy()))) { - // Display it for the driver every 10 ms - if(runtime.milliseconds() > cnt * 30 ) { // Print every 30 ms - RobotLog.ii("NFusion", "Current Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - telemetry.addData("Path1", "Running to: FL: %7d FR: %7d BL: %7d BR: %7d", - newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); - telemetry.addData("Path2", "Running at: FL: %7d FR: %7d BL: %7d BR: %7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - telemetry.update(); - cnt++; - } - // Reduce the Speed before stopping - remainingDistance = Math.abs(newFrontLeftTarget-robot.motorFrontLeft.getCurrentPosition()); - if (remainingDistance < 10) - { - remainingDistance = Math.abs(newFrontRightTarget-robot.motorFrontRight.getCurrentPosition()); - } - if ((remainingDistance < ENCODER_COUNT_BEFORE_STOP) && (remainingDistance >= 10)){ - new_speed = Math.abs(speed)*(remainingDistance/(float)ENCODER_COUNT_BEFORE_STOP); - robot.motorFrontLeft.setPower(new_speed); - robot.motorFrontRight.setPower(new_speed); - robot.motorBackLeft.setPower(new_speed); - robot.motorBackRight.setPower(new_speed); - if((cnt % 5) == 0) { // skip 5 cnt and print - RobotLog.ii("NFusion", "Remaining Dist: %7d, Speed %f, new Speed %f", - remainingDistance, speed, new_speed); - } - } - } - - //RobotLog.ii("NFusion", "Motor Encoder Status FL: %d, FR: %d, BL: %d, BR: %d", - // motorFrontLeft.isBusy(), - // motorFrontRight.isBusy(), - // motorBackLeft.isBusy(), - //motorBackRight.isBusy()); - - // Stop all motion; - robot.motorFrontLeft.setPower(0); - robot.motorFrontRight.setPower(0); - robot.motorBackLeft.setPower(0); - robot.motorBackRight.setPower(0); - - //sleep(500); // optional pause after each move - - // Turn off ENCODER - // Reset Encoder - //robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - //robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - //robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - //robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - - RobotLog.ii("Path0", "Exit - myEncoderDrive Last Position: Front L:%7d R:%7d, Back L:%7d R:%7d", - robot.motorFrontLeft.getCurrentPosition(), - robot.motorFrontRight.getCurrentPosition(), - robot.motorBackLeft.getCurrentPosition(), - robot.motorBackRight.getCurrentPosition()); - - //telemetry.addData("Status", "Movement Distance: %7d", - // distance_traveled); - //telemetry.update(); - - } - } +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.RobotLog; + +/** + * + * The desired path in this example is: + * - Drive forward for 48 inches + * - Spin right for 12 Inches + * - Drive Backwards for 24 inches + * - Stop and close the claw. + * + * The code is written using a method called: encoderDrive(speed, leftInches, rightInches, timeoutS) + * that performs the actual movement. + * This methods assumes that each movement is relative to the last stopping place. + * There are other ways to perform encoder based moves, but this method is probably the simplest. + * This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile + * + * Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list + */ + + +@Autonomous(name="Autonomous: AutoRedWarehouse", group="Autonomous") +//@Disabled +public class AutoRedWarehouse extends LinearOpMode { + + public enum Direction { + FORWARD, + BACKWARD, + RIGHT, + LEFT, + F_RIGHT_DIG, + F_LEFT_DIG, + B_RIGHT_DIG, + B_LEFT_DIG, + CLOCK_WISE_TURN, + ANTI_CLOCK_WISE_TURN + } + public enum TEST_MODE { + WORKING1, + WORKING2, + WORKING3, + WORKING4, + TEST1, + TEST2, + TEST3, + TEST4 + } + + /* Declare OpMode members. */ + NFMyRobot robot = new NFMyRobot(); // Use NF my Robot h/w + + private final ElapsedTime runtime = new ElapsedTime(); + + //Encoder produced TICK COUNTS per revolution + static final double COUNTS_PER_MOTOR_REV = 537.7; // eg: TETRIX Motor Encoder - 1440, REV Hex Motors: 2240 + static final double DRIVE_GEAR_REDUCTION = 1; //2.0; // This is < 1.0 if geared UP + static final double WHEEL_DIAMETER_INCHES = 3.7; // For figuring circumference + static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / + (WHEEL_DIAMETER_INCHES * 3.1428); + static final double COUNTS_FULL_TURN = 72; + static final int ENCODER_COUNT_BEFORE_STOP = 140; //slow down before 3" + static final int ARM_FULL_TURN_CNT = 537; + + static TEST_MODE TEST_RUN_TYPE = TEST_MODE.WORKING2; + + //static final double DRIVE_SPEED = 1; + //static final double TURN_SPEED = 0.5; + + // Declare our motors + // Make sure your ID's match your configuration + //DcMotor motorFrontLeft = null; + //DcMotor motorBackLeft = null; + //DcMotor motorFrontRight = null; + //DcMotor motorBackRight = null; + + int DuckCount; + double distance; + + @Override + public void runOpMode() /*throws InterruptedException*/ { + Direction dir; + distance = 0; + DuckCount = 0; + /* + * Initialize the drive system variables. + * The init() method of the hardware class does all the work here + */ + robot.init(hardwareMap); + runtime.reset(); + + // Send telemetry message to signify robot waiting; + telemetry.addData("Status", "Starting Autorun"); //Auto run + telemetry.update(); + + // Reset Encoder + robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + // Set Encoder + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + robot.motorBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + // Send telemetry message to indicate successful Encoder reset + telemetry.addData("Path0", "Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.update(); + + // Reverse the right side motors + robot.motorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); + robot.motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE); + + // Wait for the game to start (driver presses PLAY) + waitForStart(); + + if (isStopRequested()) return; + + if (TEST_RUN_TYPE == TEST_MODE.WORKING1) + { + myEncoderDrive(Direction.LEFT, 0.1, 4, 2000); + myEncoderTurn(0.2, 5.5); + myEncoderDrive(Direction.BACKWARD, 0.50, 25, 2000); + myEncoderDrive(Direction.BACKWARD, 0.15, 8, 2000); + spinCarousel(2000,500); + myEncoderDrive(Direction.FORWARD, 0.35, 8, 2000); + myEncoderTurn(0.2, -3); + myEncoderDrive(Direction.RIGHT, 0.1, 3, 2000); + myEncoderDrive(Direction.FORWARD, 0.75, 88.5, 10000); + myEncoderDrive(Direction.LEFT, 0.6, 30, 5000); + myEncoderDrive(Direction.FORWARD, 0.5, 12, 2000); + + } + + if (TEST_RUN_TYPE == TEST_MODE.WORKING2) + { + myEncoderDrive(Direction.RIGHT, 0.25, 17, 2000); + myEncoderDrive(Direction.FORWARD, 0.20, 3, 2000); + myArmMove(0.15, 0.35, 2200); + myEncoderTurn(0.25, -75); + //myEncoderDrive(Direction.LEFT, 0.35, 6, 2000); + myEncoderDrive(Direction.FORWARD, 0.20, 11.5, 2000); + //ARM Move and Claw Open: + sleep(300); + robot.servo_1.setPosition(0.75); + robot.servo_2.setPosition(0.25); + sleep(350); + myEncoderDrive(Direction.BACKWARD, 0.25, 12, 2000); + robot.servo_1.setPosition(1); + robot.servo_2.setPosition(0); + myEncoderTurn(0.25, 75); + myEncoderDrive(Direction.LEFT, 0.20, 18, 2000); + myEncoderDrive(Direction.RIGHT, 0.15, 0.5, 2000); + myEncoderDrive(Direction.BACKWARD, 0.50, 42, 10000); + myEncoderDrive(Direction.RIGHT, 0.50, 23, 5000); + myEncoderTurn(0.25, 135); + myEncoderDrive(Direction.LEFT, 0.20, 3, 2000); + //myEncoderDrive(Direction.BACKWARD, 0.25, 8, 2000); + myArmMove(0.15, 0.20, 1500); + } + + //sleep(100); // pause for servos to move + + // Set all Motors without Encoder for Manual Run. + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + telemetry.addData("Path", "Complete"); + telemetry.update(); + } + + public void myArmMove(double speed, double rotation, double timeoutS) { //SensorsToUse sensors_2_use) + int newArmPosition = 0; + + // Reset Encoder beginning to see it gets better. + robot.motorArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + newArmPosition = robot.motorArm.getCurrentPosition() + (int) (rotation * ARM_FULL_TURN_CNT); + // Set the Encoder to the target position. + robot.motorArm.setTargetPosition(newArmPosition); + // Turn On RUN_TO_POSITION + robot.motorArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + // Set power for the motors. + robot.motorArm.setPower(Math.abs(speed)); + // reset the timeout time and start motion. + runtime.reset(); + + while (opModeIsActive() && + (runtime.milliseconds() < timeoutS) && + (robot.motorArm.isBusy())) + { + RobotLog.ii("NFusion", "Arm Current Pos: %7d, Target Pos: %7d", + robot.motorArm.getCurrentPosition(), newArmPosition); + } + + robot.motorArm.setPower(0); + //sleep(50); + } + + public Direction moveRobot(double x1, double y1, double currOrient, + double x2, double y2, double newOrient, double speed) + { + double angle; + double turn_angle; + Direction direction = Direction.FORWARD; + + // angle will return Anti-clockwise value [-90, 90] + // Example A(-12,50), B(-60, 60). Anglex2) { + // Go backward to (x2,y2) + direction = Direction.BACKWARD; + } else if (x2>x1) { + // Go Forward to (x2, y2) + direction = Direction.FORWARD; + } else if (y1>y2) { + // Go Backward to (x2, y2) + direction = Direction.BACKWARD; + } else { + // Go Forward to (x2, y2) + direction = Direction.FORWARD; + } + + myEncoderDrive(direction, speed, distance, 1000); + myEncoderTurn(speed, newOrient - angle); + + /* + // Move 60+ degree Anti-CLOCKWISE + if (turn_angle < -180) { + turn_angle = turn_angle + 180; + myEncoderTurn(0.4, turn_angle); + myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle -180); + } else if(turn_angle > 180) { + turn_angle = turn_angle - 180; + myEncoderTurn(0.4, turn_angle); + myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle + 180); + } else { + myEncoderTurn(0.4, turn_angle); + // Move FWD + myEncoderDrive(Direction.FORWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle); + } + */ + return direction; + } + + // Run Carousel + public void spinCarousel(double timeout, long sleeptime) + { + runtime.reset(); + + robot.motorCarouselSpin.setPower(-0.8); + while (opModeIsActive() && + runtime.milliseconds() < timeout) + { + telemetry.addData("NFAuto", "Running Carousel for Duck for %f milisec", timeout); + telemetry.update(); + } + robot.motorCarouselSpin.setPower(0); + //sleep(sleeptime); + DuckCount++; + RobotLog.ii("NFAuto", "Number of Duck delivered: %d, timeout %f", DuckCount, timeout); + } + + public void myEncoderTurn(double speed, double degree) + { + double inch_cnt; + + if (Math.abs(degree) < 1) return; + + // Adjust to turn less than 180 + if (degree < -180) { + degree = degree + 360; + } + if (degree > 180) { + degree = degree - 360; + } + + inch_cnt = degree*(COUNTS_FULL_TURN/360); + RobotLog.ii("Input:myEncoderTurn", "Speed/Degree/inch_cnt, %f, %f %f", + speed, degree, inch_cnt); + + if (degree >= 0) { + myEncoderDrive(Direction.ANTI_CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); + } else { + myEncoderDrive(Direction.CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); + } + } + + public void myEncoderDrive(Direction direction, double speed, double Inches, + double timeoutS) { //SensorsToUse sensors_2_use) + int newFrontLeftTarget = 0; + int newFrontRightTarget = 0; + int newBackLeftTarget = 0; + int newBackRightTarget = 0; + int remainingDistance; + int cnt = 0; + double new_speed = 0; + + + RobotLog.ii("Input", "Enter - myEncoderDrive - speed=%f, Inches=%f, timeout=%f", + speed, Inches, timeoutS); + telemetry.addData("Path1", "Enter - myEncoderDrive - speed=%f," + + " Inches=%f, timeout=%f", speed, Inches, timeoutS); + telemetry.update(); + + // Turn off ENCODER + // Reset Encoder beginning to see it gets better. + robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + RobotLog.ii("Path0", "Starting Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + + // Ensure that the op mode is still active + if (opModeIsActive() && !isStopRequested()) { + + // Determine new target position, and pass to motor controller + if (direction == Direction.BACKWARD) { + //Go forward + RobotLog.ii("NFusion", "Moving BACKWARD.... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.FORWARD) { + //Go backward + RobotLog.ii("NFusion", "Moving FORWARD..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.LEFT) { + //Move Right + RobotLog.ii("NFusion", "Moving LEFT..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.RIGHT) { + //Move Left + RobotLog.ii("NFusion", "Moving RIGHT..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.F_RIGHT_DIG) { + //Forward Left Diagonal + RobotLog.ii("NFusion", "Moving F_RIGHT_DIG ..... [%f inches]..", Inches); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.F_LEFT_DIG) { + //Move Forward Right Diagonal + RobotLog.ii("NFusion", "Moving F_LEFT_DIG..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.B_RIGHT_DIG){ + //Move Backward Left Diagonal + RobotLog.ii("NFusion", "Moving B_RIGHT_DIG..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.B_LEFT_DIG) { + //Backward Right Diagonal + RobotLog.ii("NFusion", "Moving B_LEFT_DIG ..... [%f inches]..", Inches); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.ANTI_CLOCK_WISE_TURN) { + // Turn Clock Wise + RobotLog.ii("NFusion", "Turn Anti Clockwise ..... [%f ms]..", timeoutS); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.CLOCK_WISE_TURN) { + // Turn Clock Wise + RobotLog.ii("NFusion", "Turn Clockwise ..... [%f ms]..", timeoutS); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else { + // Do Not move + speed = 0; + } + + // Set the Encoder to the target position. + robot.motorFrontLeft.setTargetPosition(newFrontLeftTarget); + robot.motorFrontRight.setTargetPosition(newFrontRightTarget); + robot.motorBackLeft.setTargetPosition(newBackLeftTarget); + robot.motorBackRight.setTargetPosition(newBackRightTarget); + + // Turn On RUN_TO_POSITION + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + // Set power for the motors. + robot.motorFrontLeft.setPower(Math.abs(speed)); + robot.motorFrontRight.setPower(Math.abs(speed)); + robot.motorBackLeft.setPower(Math.abs(speed)); + robot.motorBackRight.setPower(Math.abs(speed)); + + // reset the timeout time and start motion. + runtime.reset(); + + RobotLog.ii("NFusion", "Final Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", + newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); + + while (opModeIsActive() && + (runtime.milliseconds() < timeoutS) && + ((robot.motorFrontLeft.isBusy() || robot.motorFrontRight.isBusy()) && + (robot.motorBackLeft.isBusy() || robot.motorBackRight.isBusy()))) { + // Display it for the driver every 10 ms + if(runtime.milliseconds() > cnt * 30 ) { // Print every 30 ms + RobotLog.ii("NFusion", "Current Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.addData("Path1", "Running to: FL: %7d FR: %7d BL: %7d BR: %7d", + newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); + telemetry.addData("Path2", "Running at: FL: %7d FR: %7d BL: %7d BR: %7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.update(); + cnt++; + } + // Reduce the Speed before stopping + remainingDistance = Math.abs(newFrontLeftTarget-robot.motorFrontLeft.getCurrentPosition()); + if (remainingDistance < 10) + { + remainingDistance = Math.abs(newFrontRightTarget-robot.motorFrontRight.getCurrentPosition()); + } + if ((remainingDistance < ENCODER_COUNT_BEFORE_STOP) && (remainingDistance >= 10)){ + new_speed = Math.abs(speed)*(remainingDistance/(float)ENCODER_COUNT_BEFORE_STOP); + robot.motorFrontLeft.setPower(new_speed); + robot.motorFrontRight.setPower(new_speed); + robot.motorBackLeft.setPower(new_speed); + robot.motorBackRight.setPower(new_speed); + if((cnt % 5) == 0) { // skip 5 cnt and print + RobotLog.ii("NFusion", "Remaining Dist: %7d, Speed %f, new Speed %f", + remainingDistance, speed, new_speed); + } + } + } + + //RobotLog.ii("NFusion", "Motor Encoder Status FL: %d, FR: %d, BL: %d, BR: %d", + // motorFrontLeft.isBusy(), + // motorFrontRight.isBusy(), + // motorBackLeft.isBusy(), + //motorBackRight.isBusy()); + + // Stop all motion; + robot.motorFrontLeft.setPower(0); + robot.motorFrontRight.setPower(0); + robot.motorBackLeft.setPower(0); + robot.motorBackRight.setPower(0); + + //sleep(500); // optional pause after each move + + // Turn off ENCODER + // Reset Encoder + //robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + RobotLog.ii("Path0", "Exit - myEncoderDrive Last Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + + //telemetry.addData("Status", "Movement Distance: %7d", + // distance_traveled); + //telemetry.update(); + + } + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedWarehouseNoHub.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedWarehouseNoHub.java new file mode 100644 index 000000000000..3a6d7b3360ae --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedWarehouseNoHub.java @@ -0,0 +1,550 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.RobotLog; + +/** + * + * The desired path in this example is: + * - Drive forward for 48 inches + * - Spin right for 12 Inches + * - Drive Backwards for 24 inches + * - Stop and close the claw. + * + * The code is written using a method called: encoderDrive(speed, leftInches, rightInches, timeoutS) + * that performs the actual movement. + * This methods assumes that each movement is relative to the last stopping place. + * There are other ways to perform encoder based moves, but this method is probably the simplest. + * This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile + * + * Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list + */ + + +@Autonomous(name="Autonomous: AutoRedWarehouseNoHub", group="Autonomous") +//@Disabled +public class AutoRedWarehouseNoHub extends LinearOpMode { + + public enum Direction { + FORWARD, + BACKWARD, + RIGHT, + LEFT, + F_RIGHT_DIG, + F_LEFT_DIG, + B_RIGHT_DIG, + B_LEFT_DIG, + CLOCK_WISE_TURN, + ANTI_CLOCK_WISE_TURN + } + public enum TEST_MODE { + WORKING1, + WORKING2, + WORKING3, + WORKING4, + TEST1, + TEST2, + TEST3, + TEST4 + } + + /* Declare OpMode members. */ + NFMyRobot robot = new NFMyRobot(); // Use NF my Robot h/w + + private final ElapsedTime runtime = new ElapsedTime(); + + //Encoder produced TICK COUNTS per revolution + static final double COUNTS_PER_MOTOR_REV = 537.7; // eg: TETRIX Motor Encoder - 1440, REV Hex Motors: 2240 + static final double DRIVE_GEAR_REDUCTION = 1; //2.0; // This is < 1.0 if geared UP + static final double WHEEL_DIAMETER_INCHES = 3.7; // For figuring circumference + static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / + (WHEEL_DIAMETER_INCHES * 3.1428); + static final double COUNTS_FULL_TURN = 72; + static final int ENCODER_COUNT_BEFORE_STOP = 140; //slow down before 3" + static final int ARM_FULL_TURN_CNT = 537; + + static TEST_MODE TEST_RUN_TYPE = TEST_MODE.WORKING2; + + //static final double DRIVE_SPEED = 1; + //static final double TURN_SPEED = 0.5; + + // Declare our motors + // Make sure your ID's match your configuration + //DcMotor motorFrontLeft = null; + //DcMotor motorBackLeft = null; + //DcMotor motorFrontRight = null; + //DcMotor motorBackRight = null; + + int DuckCount; + double distance; + + @Override + public void runOpMode() /*throws InterruptedException*/ { + Direction dir; + distance = 0; + DuckCount = 0; + /* + * Initialize the drive system variables. + * The init() method of the hardware class does all the work here + */ + robot.init(hardwareMap); + runtime.reset(); + + // Send telemetry message to signify robot waiting; + telemetry.addData("Status", "Starting Autorun"); //Auto run + telemetry.update(); + + // Reset Encoder + robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + // Set Encoder + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + robot.motorBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.motorFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + // Send telemetry message to indicate successful Encoder reset + telemetry.addData("Path0", "Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.update(); + + // Reverse the right side motors + robot.motorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); + robot.motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE); + + // Wait for the game to start (driver presses PLAY) + waitForStart(); + + if (isStopRequested()) return; + + if (TEST_RUN_TYPE == TEST_MODE.WORKING1) + { + myEncoderDrive(Direction.LEFT, 0.1, 4, 2000); + myEncoderTurn(0.2, 5.5); + myEncoderDrive(Direction.BACKWARD, 0.50, 25, 2000); + myEncoderDrive(Direction.BACKWARD, 0.15, 8, 2000); + spinCarousel(2000,500); + myEncoderDrive(Direction.FORWARD, 0.35, 8, 2000); + myEncoderTurn(0.2, -3); + myEncoderDrive(Direction.RIGHT, 0.1, 3, 2000); + myEncoderDrive(Direction.FORWARD, 0.75, 88.5, 10000); + myEncoderDrive(Direction.LEFT, 0.6, 30, 5000); + myEncoderDrive(Direction.FORWARD, 0.5, 12, 2000); + + } + + if (TEST_RUN_TYPE == TEST_MODE.WORKING2) + { + //myEncoderDrive(Direction.RIGHT, 0.25, 17, 2000); + //myArmMove(0.15, 0.35, 2200); + //myEncoderTurn(0.25, -70); + //myEncoderDrive(Direction.LEFT, 0.35, 6, 2000); + //myEncoderDrive(Direction.FORWARD, 0.20, 12, 2000); + //ARM Move and Claw Open: + //robot.servo_1.setPosition(1); + //robot.servo_2.setPosition(0); + //sleep(350); + //myEncoderDrive(Direction.BACKWARD, 0.25, 12, 2000); + //robot.servo_1.setPosition(0.75); + //robot.servo_2.setPosition(0.25); + //myEncoderTurn(0.25, 70); + //myEncoderDrive(Direction.LEFT, 0.20, 22, 2000); + myEncoderDrive(Direction.RIGHT, 0.15, 0.3, 2000); + myEncoderDrive(Direction.BACKWARD, 0.50, 39, 10000); + myEncoderDrive(Direction.RIGHT, 0.50, 26, 5000); + myEncoderDrive(Direction.BACKWARD, 0.25, 8, 2000); + } + + //sleep(100); // pause for servos to move + + // Set all Motors without Encoder for Manual Run. + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.motorArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + telemetry.addData("Path", "Complete"); + telemetry.update(); + } + + public void myArmMove(double speed, double rotation, double timeoutS) { //SensorsToUse sensors_2_use) + int newArmPosition = 0; + + // Reset Encoder beginning to see it gets better. + robot.motorArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + newArmPosition = robot.motorArm.getCurrentPosition() + (int) (rotation * ARM_FULL_TURN_CNT); + // Set the Encoder to the target position. + robot.motorArm.setTargetPosition(newArmPosition); + // Turn On RUN_TO_POSITION + robot.motorArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + // Set power for the motors. + robot.motorArm.setPower(Math.abs(speed)); + // reset the timeout time and start motion. + runtime.reset(); + + while (opModeIsActive() && + (runtime.milliseconds() < timeoutS) && + (robot.motorArm.isBusy())) + { + RobotLog.ii("NFusion", "Arm Current Pos: %7d, Target Pos: %7d", + robot.motorArm.getCurrentPosition(), newArmPosition); + } + + robot.motorArm.setPower(0); + //sleep(50); + } + + public Direction moveRobot(double x1, double y1, double currOrient, + double x2, double y2, double newOrient, double speed) + { + double angle; + double turn_angle; + Direction direction = Direction.FORWARD; + + // angle will return Anti-clockwise value [-90, 90] + // Example A(-12,50), B(-60, 60). Anglex2) { + // Go backward to (x2,y2) + direction = Direction.BACKWARD; + } else if (x2>x1) { + // Go Forward to (x2, y2) + direction = Direction.FORWARD; + } else if (y1>y2) { + // Go Backward to (x2, y2) + direction = Direction.BACKWARD; + } else { + // Go Forward to (x2, y2) + direction = Direction.FORWARD; + } + + myEncoderDrive(direction, speed, distance, 1000); + myEncoderTurn(speed, newOrient - angle); + + /* + // Move 60+ degree Anti-CLOCKWISE + if (turn_angle < -180) { + turn_angle = turn_angle + 180; + myEncoderTurn(0.4, turn_angle); + myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle -180); + } else if(turn_angle > 180) { + turn_angle = turn_angle - 180; + myEncoderTurn(0.4, turn_angle); + myEncoderDrive(Direction.BACKWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle + 180); + } else { + myEncoderTurn(0.4, turn_angle); + // Move FWD + myEncoderDrive(Direction.FORWARD, 0.75, distance, 1000); + myEncoderTurn(0.4, newOrient - angle); + } + */ + return direction; + } + + // Run Carousel + public void spinCarousel(double timeout, long sleeptime) + { + runtime.reset(); + + robot.motorCarouselSpin.setPower(-0.8); + while (opModeIsActive() && + runtime.milliseconds() < timeout) + { + telemetry.addData("NFAuto", "Running Carousel for Duck for %f milisec", timeout); + telemetry.update(); + } + robot.motorCarouselSpin.setPower(0); + //sleep(sleeptime); + DuckCount++; + RobotLog.ii("NFAuto", "Number of Duck delivered: %d, timeout %f", DuckCount, timeout); + } + + public void myEncoderTurn(double speed, double degree) + { + double inch_cnt; + + if (Math.abs(degree) < 1) return; + + // Adjust to turn less than 180 + if (degree < -180) { + degree = degree + 360; + } + if (degree > 180) { + degree = degree - 360; + } + + inch_cnt = degree*(COUNTS_FULL_TURN/360); + RobotLog.ii("Input:myEncoderTurn", "Speed/Degree/inch_cnt, %f, %f %f", + speed, degree, inch_cnt); + + if (degree >= 0) { + myEncoderDrive(Direction.ANTI_CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); + } else { + myEncoderDrive(Direction.CLOCK_WISE_TURN, speed, Math.abs(inch_cnt), 3000); + } + } + + public void myEncoderDrive(Direction direction, double speed, double Inches, + double timeoutS) { //SensorsToUse sensors_2_use) + int newFrontLeftTarget = 0; + int newFrontRightTarget = 0; + int newBackLeftTarget = 0; + int newBackRightTarget = 0; + int remainingDistance; + int cnt = 0; + double new_speed = 0; + + + RobotLog.ii("Input", "Enter - myEncoderDrive - speed=%f, Inches=%f, timeout=%f", + speed, Inches, timeoutS); + telemetry.addData("Path1", "Enter - myEncoderDrive - speed=%f," + + " Inches=%f, timeout=%f", speed, Inches, timeoutS); + telemetry.update(); + + // Turn off ENCODER + // Reset Encoder beginning to see it gets better. + robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + RobotLog.ii("Path0", "Starting Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + + // Ensure that the op mode is still active + if (opModeIsActive() && !isStopRequested()) { + + // Determine new target position, and pass to motor controller + if (direction == Direction.BACKWARD) { + //Go forward + RobotLog.ii("NFusion", "Moving BACKWARD.... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.FORWARD) { + //Go backward + RobotLog.ii("NFusion", "Moving FORWARD..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.LEFT) { + //Move Right + RobotLog.ii("NFusion", "Moving LEFT..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.RIGHT) { + //Move Left + RobotLog.ii("NFusion", "Moving RIGHT..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.F_RIGHT_DIG) { + //Forward Left Diagonal + RobotLog.ii("NFusion", "Moving F_RIGHT_DIG ..... [%f inches]..", Inches); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.F_LEFT_DIG) { + //Move Forward Right Diagonal + RobotLog.ii("NFusion", "Moving F_LEFT_DIG..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else if (direction == Direction.B_RIGHT_DIG){ + //Move Backward Left Diagonal + RobotLog.ii("NFusion", "Moving B_RIGHT_DIG..... [%f inches]..", Inches); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.B_LEFT_DIG) { + //Backward Right Diagonal + RobotLog.ii("NFusion", "Moving B_LEFT_DIG ..... [%f inches]..", Inches); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.ANTI_CLOCK_WISE_TURN) { + // Turn Clock Wise + RobotLog.ii("NFusion", "Turn Anti Clockwise ..... [%f ms]..", timeoutS); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + } else if (direction == Direction.CLOCK_WISE_TURN) { + // Turn Clock Wise + RobotLog.ii("NFusion", "Turn Clockwise ..... [%f ms]..", timeoutS); + newFrontLeftTarget = robot.motorFrontLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newFrontRightTarget = robot.motorFrontRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + newBackLeftTarget = robot.motorBackLeft.getCurrentPosition() + (int) (-1 * Inches * COUNTS_PER_INCH); + newBackRightTarget = robot.motorBackRight.getCurrentPosition() + (int) (Inches * COUNTS_PER_INCH); + } else { + // Do Not move + speed = 0; + } + + // Set the Encoder to the target position. + robot.motorFrontLeft.setTargetPosition(newFrontLeftTarget); + robot.motorFrontRight.setTargetPosition(newFrontRightTarget); + robot.motorBackLeft.setTargetPosition(newBackLeftTarget); + robot.motorBackRight.setTargetPosition(newBackRightTarget); + + // Turn On RUN_TO_POSITION + robot.motorFrontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorFrontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorBackLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.motorBackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + // Set power for the motors. + robot.motorFrontLeft.setPower(Math.abs(speed)); + robot.motorFrontRight.setPower(Math.abs(speed)); + robot.motorBackLeft.setPower(Math.abs(speed)); + robot.motorBackRight.setPower(Math.abs(speed)); + + // reset the timeout time and start motion. + runtime.reset(); + + RobotLog.ii("NFusion", "Final Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", + newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); + + while (opModeIsActive() && + (runtime.milliseconds() < timeoutS) && + ((robot.motorFrontLeft.isBusy() || robot.motorFrontRight.isBusy()) && + (robot.motorBackLeft.isBusy() || robot.motorBackRight.isBusy()))) { + // Display it for the driver every 10 ms + if(runtime.milliseconds() > cnt * 30 ) { // Print every 30 ms + RobotLog.ii("NFusion", "Current Target FL: %7d, FR: %7d, BL: %7d, BR: %7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.addData("Path1", "Running to: FL: %7d FR: %7d BL: %7d BR: %7d", + newFrontLeftTarget, newFrontRightTarget, newBackLeftTarget, newBackRightTarget); + telemetry.addData("Path2", "Running at: FL: %7d FR: %7d BL: %7d BR: %7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + telemetry.update(); + cnt++; + } + // Reduce the Speed before stopping + remainingDistance = Math.abs(newFrontLeftTarget-robot.motorFrontLeft.getCurrentPosition()); + if (remainingDistance < 10) + { + remainingDistance = Math.abs(newFrontRightTarget-robot.motorFrontRight.getCurrentPosition()); + } + if ((remainingDistance < ENCODER_COUNT_BEFORE_STOP) && (remainingDistance >= 10)){ + new_speed = Math.abs(speed)*(remainingDistance/(float)ENCODER_COUNT_BEFORE_STOP); + robot.motorFrontLeft.setPower(new_speed); + robot.motorFrontRight.setPower(new_speed); + robot.motorBackLeft.setPower(new_speed); + robot.motorBackRight.setPower(new_speed); + if((cnt % 5) == 0) { // skip 5 cnt and print + RobotLog.ii("NFusion", "Remaining Dist: %7d, Speed %f, new Speed %f", + remainingDistance, speed, new_speed); + } + } + } + + //RobotLog.ii("NFusion", "Motor Encoder Status FL: %d, FR: %d, BL: %d, BR: %d", + // motorFrontLeft.isBusy(), + // motorFrontRight.isBusy(), + // motorBackLeft.isBusy(), + //motorBackRight.isBusy()); + + // Stop all motion; + robot.motorFrontLeft.setPower(0); + robot.motorFrontRight.setPower(0); + robot.motorBackLeft.setPower(0); + robot.motorBackRight.setPower(0); + + //sleep(500); // optional pause after each move + + // Turn off ENCODER + // Reset Encoder + //robot.motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //robot.motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + RobotLog.ii("Path0", "Exit - myEncoderDrive Last Position: Front L:%7d R:%7d, Back L:%7d R:%7d", + robot.motorFrontLeft.getCurrentPosition(), + robot.motorFrontRight.getCurrentPosition(), + robot.motorBackLeft.getCurrentPosition(), + robot.motorBackRight.getCurrentPosition()); + + //telemetry.addData("Status", "Movement Distance: %7d", + // distance_traveled); + //telemetry.update(); + + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Manual_Complete_Blue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Manual_Complete_Blue.java new file mode 100644 index 000000000000..14897d9bafb4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Manual_Complete_Blue.java @@ -0,0 +1,237 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.RobotLog; + +@TeleOp +public class Manual_Complete_Blue extends LinearOpMode +{ + /* Declare OpMode members. */ + //NFMyRobot robot = new NFMyRobot(); // Use NF my Robot h/w + + static final double INCREMENT = 0.125; // amount to slew servo each CYCLE_MS cycle + static final int CYCLE_MS = 50; // period of each cycle + static final double MAX_POS_LEFT = 1.0; // Maximum rotational left_position + static final double MIN_POS_LEFT = 0.75; + static final double MAX_POS_RIGHT = 0.0; // Minimum rotational left_position + static final double MIN_POS_RIGHT = 0.25; + + // Define class members + Servo left_servo; + Servo right_servo; + + double left_position = 0.75; // Start at halfway left_position + double right_position = 0.25; // Start at halfway right_position + + //double left_position = (((MAX_POS - MIN_POS)/4)*3); // Start at halfway left_position + //double right_position = (((MAX_POS - MIN_POS) / 4)); // Start at halfway right_position + float ltrigger; + //double init_position; + //float rtrigger; + boolean lbumper; + boolean click_a; + boolean rbumper; + boolean close = false; + boolean release = false; + double speed_val = 1.0; + //boolean slideOpen = false; + //boolean stopped = true; + + @Override + public void runOpMode() throws InterruptedException + { + // Declare our motors + // Make sure your ID's match your configuration + DcMotor motorFrontLeft = hardwareMap.dcMotor.get("motorFrontLeft"); + DcMotor motorBackLeft = hardwareMap.dcMotor.get("motorBackLeft"); + DcMotor motorFrontRight = hardwareMap.dcMotor.get("motorFrontRight"); + DcMotor motorBackRight = hardwareMap.dcMotor.get("motorBackRight"); + DcMotor motorArm = hardwareMap.dcMotor.get("motorArm"); + DcMotor motorCarousel = hardwareMap.dcMotor.get("motorCarousel"); + + // Reverse the right side motors + // Reverse left motors if you are using NeveRests + motorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); + motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE); + + // Connect to servo (Assume PushBot Left Hand) + // Change the text in quotes to match any servo name on your robot. + left_servo = hardwareMap.get(Servo.class, "left_hand"); + right_servo = hardwareMap.get(Servo.class, "right_hand"); + + //New Slider Servo + //slider = hardwareMap.get(Servo.class, "left_hand"); + //New Intake Motor + //DcMotor motorSpin = hardwareMap.dcMotor.get("motorSpin"); + + // Wait for the start button + telemetry.addData(">", "Press Start to scan Servo." ); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + //Old RobotLog.ii("Servo initial Left Position", "%5.2f", left_servo.getPosition()); + //Old RobotLog.ii("Servo initial Right Position", "%5.2f", right_servo.getPosition()); + + //init_position = slider.getPosition(); + + while (opModeIsActive()) { + + // Carousel Control + rbumper = gamepad2.right_bumper; + if (rbumper) { + motorCarousel.setPower(0.65); + } + else { + motorCarousel.setPower(0); + } + + click_a = gamepad1.a; + if(click_a) + { + //speed_val = (double)(((int)speed_val+1) % 3); + if (speed_val == 1.0) + { + speed_val = 2.5; + } + else + { + speed_val = 1.0; + } + } + + double y = gamepad1.left_stick_y; // Remember, this is reversed! + double x = -gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing + double rx = -gamepad1.right_stick_x; + + // Denominator is the largest motor power (absolute value) or 1 + // This ensures all the powers maintain the same ratio, but only when + // at least one is out of the range [-1, 1] + telemetry.addData("Values", "x %f, y %f, rx %f",x, y, rx); + //telemetry.update(); + RobotLog.ii("Values", "x %f, y %f, rx %f",x, y, rx); + + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx),speed_val); + double frontLeftPower = (y + x + rx) / denominator; + double backLeftPower = (y - x + rx) / denominator; + double frontRightPower = (y - x - rx) / denominator; + double backRightPower = (y + x - rx) / denominator; + + telemetry.addData("NF", "FL:%f, FR:%f, Deno:%f", frontLeftPower, frontRightPower, denominator); + RobotLog.ii("NFusion", "x %f, y %f, rx %f, deno %f", x, y, rx, denominator); + RobotLog.ii("NFusion", "FrontLeft %f, FrontRight %f, BackLeft %f BackRight %f", + frontLeftPower, frontRightPower, backLeftPower, backRightPower); + motorFrontLeft.setPower(frontLeftPower); + motorBackLeft.setPower(backLeftPower); + motorFrontRight.setPower(frontRightPower); + motorBackRight.setPower(backRightPower); + + double armPos = gamepad2.left_stick_y; + // armPos = armPos / 2; + motorArm.setPower(armPos); + + // Claw Movements - Close/Open. + ltrigger = this.gamepad2.left_trigger; + lbumper = this.gamepad2.left_bumper; + + if (ltrigger > 0) { + release = false; + close = true; + } + else { + close = false; + } + + if (lbumper) { + close = false; + release = true; + } + else { + release = false; + } + + left_position = left_servo.getPosition(); + right_position = right_servo.getPosition(); + + // slew the servo, according to the rampUp (direction) variable. + if (release) { + // Keep stepping up until we hit the max value. + left_position -= INCREMENT ; + if (left_position < MIN_POS_LEFT) { + left_position = MIN_POS_LEFT; + } + right_position += INCREMENT ; + if (right_position >= MIN_POS_RIGHT) { + right_position = MIN_POS_RIGHT; + } + } + + if (close) { + // Keep stepping down until we hit the min value. + left_position += INCREMENT ; + if (left_position >= MAX_POS_LEFT) { + left_position = MAX_POS_LEFT; + } + right_position -= INCREMENT ; + if (right_position <= MAX_POS_RIGHT) { + right_position = MAX_POS_RIGHT; + } + } + + // Display the current value + telemetry.addData("Servo Position:", "%5.2f", left_position); + telemetry.addData("Servo Position", "%5.2f", right_position); + telemetry.addData(">", "Press Stop to end test." ); + telemetry.update(); + + RobotLog.ii("Servo Left Position", "%5.2f", left_position); + RobotLog.ii("Servo Right Position", "%5.2f", right_position); + + // Set the servo to the new left_position and pause; + left_servo.setPosition(left_position); + right_servo.setPosition(right_position); + sleep(CYCLE_MS); + idle(); + } + RobotLog.ii("Servo Final Left Position", "%5.2f", left_position); + RobotLog.ii("Servo Final Right Position", "%5.2f", right_position); + } +} + +/* + // Intake Control + lbumper = gamepad2.left_bumper; + if (lbumper) { + motorSpin.setPower(0.75); + } + else { + motorSpin.setPower(0); + } + + // New Slider + rtrigger = gamepad2.right_trigger; + if (rtrigger>0) + { + if (stopped) { + if (slideOpen) { + // It was opened - close slide + slider.setPosition(0); // Closing Value to be adjusted. + slideOpen = false; + } else { + // it was closed - Slide opened + slider.setPosition(0.25); // Opening Value to be adjusted. + slideOpen = true; + } + stopped = false; + telemetry.addData("Servo Position", "%5.2f", slider.getPosition()); + } + } else { + stopped = true; + } +*/ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Manual_Complete_Red.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Manual_Complete_Red.java new file mode 100644 index 000000000000..afb8d579c880 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Manual_Complete_Red.java @@ -0,0 +1,195 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.RobotLog; + +@TeleOp +public class Manual_Complete_Red extends LinearOpMode +{ + /* Declare OpMode members. */ + //NFMyRobot robot = new NFMyRobot(); // Use NF my Robot h/w + + static final double INCREMENT = 0.125; // amount to slew servo each CYCLE_MS cycle + static final int CYCLE_MS = 50; // period of each cycle + static final double MAX_POS_LEFT = 1.0; // Maximum rotational left_position + static final double MIN_POS_LEFT = 0.75; + static final double MAX_POS_RIGHT = 0.0; // Minimum rotational left_position + static final double MIN_POS_RIGHT = 0.25; + + // Define class members + Servo left_servo; + Servo right_servo; + + double left_position = 0.75; // Start at halfway left_position + double right_position = 0.25; // Start at halfway right_position + + //double left_position = (((MAX_POS - MIN_POS)/4)*3); // Start at halfway left_position + //double right_position = (((MAX_POS - MIN_POS) / 4)); // Start at halfway right_position + float ltrigger; + boolean lbumper; + boolean rbumper; + boolean click_a; + boolean close = false; + boolean release = false; + double speed_val = 1.0; + + @Override + public void runOpMode() throws InterruptedException + { + + // Declare our motors + // Make sure your ID's match your configuration + DcMotor motorFrontLeft = hardwareMap.dcMotor.get("motorFrontLeft"); + DcMotor motorBackLeft = hardwareMap.dcMotor.get("motorBackLeft"); + DcMotor motorFrontRight = hardwareMap.dcMotor.get("motorFrontRight"); + DcMotor motorBackRight = hardwareMap.dcMotor.get("motorBackRight"); + DcMotor motorArm = hardwareMap.dcMotor.get("motorArm"); + DcMotor motorCarousel = hardwareMap.dcMotor.get("motorCarousel"); + + // Reverse the right side motors + // Reverse left motors if you are using NeveRests + motorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); + motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE); + + // Connect to servo (Assume PushBot Left Hand) + // Change the text in quotes to match any servo name on your robot. + left_servo = hardwareMap.get(Servo.class, "left_hand"); + right_servo = hardwareMap.get(Servo.class, "right_hand"); + + // Wait for the start button + telemetry.addData(">", "Press Start to scan Servo." ); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + RobotLog.ii("Servo initial Left Position", "%5.2f", left_servo.getPosition()); + RobotLog.ii("Servo initial Right Position", "%5.2f", right_servo.getPosition()); + + while (opModeIsActive()) { + + rbumper = gamepad2.right_bumper; + if (rbumper) { + motorCarousel.setPower(-0.65); + } + else { + motorCarousel.setPower(0); + } + + click_a = gamepad1.a; + if(click_a) + { + //speed_val = (double)(((int)speed_val+1) % 3); + if (speed_val == 1.0) + { + speed_val = 2.5; + } + else + { + speed_val = 1.0; + } + } + + double y = gamepad1.left_stick_y; // Remember, this is reversed! + double x = -gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing + double rx = -gamepad1.right_stick_x; + + // Denominator is the largest motor power (absolute value) or 1 + // This ensures all the powers maintain the same ratio, but only when + // at least one is out of the range [-1, 1] + telemetry.addData("Values", "x %f, y %f, rx %f",x, y, rx); + //telemetry.update(); + RobotLog.ii("Values", "x %f, y %f, rx %f",x, y, rx); + + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx),speed_val); + double frontLeftPower = (y + x + rx) / denominator; + double backLeftPower = (y - x + rx) / denominator; + double frontRightPower = (y - x - rx) / denominator; + double backRightPower = (y + x - rx) / denominator; + + telemetry.addData("NF", "FL:%f, FR:%f, Deno:%f", frontLeftPower, frontRightPower, denominator); + RobotLog.ii("NFusion", "x %f, y %f, rx %f, deno %f", x, y, rx, denominator); + RobotLog.ii("NFusion", "FrontLeft %f, FrontRight %f, BackLeft %f BackRight %f", + frontLeftPower, frontRightPower, backLeftPower, backRightPower); + motorFrontLeft.setPower(frontLeftPower); + motorBackLeft.setPower(backLeftPower); + motorFrontRight.setPower(frontRightPower); + motorBackRight.setPower(backRightPower); + + double armPos = gamepad2.left_stick_y; + // armPos = armPos / 2; + motorArm.setPower(armPos); + + // Claw Movements - Close/Open. + ltrigger = this.gamepad2.left_trigger; + lbumper = this.gamepad2.left_bumper; + + if (ltrigger > 0) { + release = false; + close = true; + } + else { + close = false; + } + + if (lbumper) { + close = false; + release = true; + } + else { + release = false; + } + + left_position = left_servo.getPosition(); + right_position = right_servo.getPosition(); + + // slew the servo, according to the rampUp (direction) variable. + if (release) { + // Keep stepping up until we hit the max value. + left_position -= INCREMENT ; + if (left_position < MIN_POS_LEFT) { + left_position = MIN_POS_LEFT; + } + right_position += INCREMENT ; + if (right_position >= MIN_POS_RIGHT) { + right_position = MIN_POS_RIGHT; + } + } + + if (close) { + // Keep stepping down until we hit the min value. + left_position += INCREMENT ; + if (left_position >= MAX_POS_LEFT) { + left_position = MAX_POS_LEFT; + } + right_position -= INCREMENT ; + if (right_position <= MAX_POS_RIGHT) { + right_position = MAX_POS_RIGHT; + } + } + + + // Display the current value + telemetry.addData("Servo Position", "%5.2f", left_position); + telemetry.addData("Servo Position", "%5.2f", right_position); + telemetry.addData(">", "Press Stop to end test." ); + telemetry.update(); + + RobotLog.ii("Servo Left Position", "%5.2f", left_position); + RobotLog.ii("Servo Right Position", "%5.2f", right_position); + + // Set the servo to the new left_position and pause; + left_servo.setPosition(left_position); + right_servo.setPosition(right_position); + sleep(CYCLE_MS); + idle(); + } + RobotLog.ii("Servo Final Left Position", "%5.2f", left_position); + RobotLog.ii("Servo Final Right Position", "%5.2f", right_position); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/NFMyRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/NFMyRobot.java index 619431939bb9..9f523e69f171 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/NFMyRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/NFMyRobot.java @@ -1,130 +1,131 @@ -/* Copyright (c) 2017 FIRST. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted (subject to the limitations in the disclaimer below) provided that - * the following conditions are met: - * - * Redistributions of source code must retain the above copyright notice, this list - * of conditions and the following disclaimer. - * - * Redistributions in binary form must reproduce the above copyright notice, this - * list of conditions and the following disclaimer in the documentation and/or - * other materials provided with the distribution. - * - * Neither the name of FIRST nor the names of its contributors may be used to endorse or - * promote products derived from this software without specific prior written permission. - * - * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS - * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.Servo; -import com.qualcomm.robotcore.util.ElapsedTime; - -/** - * This is NOT an opmode. - * - * This class can be used to define all the specific hardware for a single robot. - * In this case that robot is a Pushbot. - * See PushbotTeleopTank_Iterative and others classes starting with "Pushbot" for usage examples. - * - * This hardware class assumes the following device names have been configured on the robot: - * Note: All names are lower case and some have single spaces between words. - * - * Motor channel: Front Left drive motor: "motorFrontLeft" - * Motor channel: Front Right drive motor: "motorFrontRight" - * Motor channel: Back Left drive motor: "motorBackLeft" - * Motor channel: Back Right drive motor: "motorBackRight" - * Motor channel: Manipulator drive motor: "Arm" - * Servo channel: Servo to open left claw: "leftHand" - * Servo channel: Servo to open right claw: "rightHand" - * Motor channel: Carousel Spin: "motorCarousel" - */ - -public class NFMyRobot -{ - /* Public OpMode members. */ - public DcMotor motorFrontLeft = null; - public DcMotor motorFrontRight = null; - public DcMotor motorBackLeft = null; - public DcMotor motorBackRight = null; - public DcMotor motorCarouselSpin = null; - public DcMotor motorArm = null; - //public DcMotor robotArm = null; - public Servo servo_1 = null; - public Servo servo_2 = null; - public Servo servo_3 = null; - - public static final double MID_SERVO = 0.5 ; - public static final double ARM_UP_POWER = 0.45 ; - public static final double ARM_DOWN_POWER = -0.45 ; - - - /* local OpMode members. */ - HardwareMap hwMap = null; - private ElapsedTime period = new ElapsedTime(); - - /* Constructor */ - public NFMyRobot(){ - - } - - - /* Initialize standard Hardware interfaces */ - public void init(HardwareMap ahwMap) { - // Save reference to Hardware map - hwMap = ahwMap; - - // Define and Initialize Motors - motorFrontLeft = hwMap.dcMotor.get("motorFrontLeft"); - motorFrontRight = hwMap.dcMotor.get("motorFrontRight"); - motorBackLeft = hwMap.dcMotor.get("motorBackLeft"); - motorBackRight = hwMap.dcMotor.get("motorBackRight"); - //robotArm = hwMap.dcMotor.get("Arm"); - motorCarouselSpin = hwMap.dcMotor.get("motorCarousel"); - servo_1 = hwMap.servo.get("left_hand"); - servo_2 = hwMap.servo.get("right_hand"); - //servo_3 = hwMap.servo.get(Servo.class, "thumb"); - - - // Reverse the right side motors - motorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); - motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE); - - // Set all motors to zero power - motorFrontLeft.setPower(0); - motorFrontRight.setPower(0); - motorBackLeft.setPower(0); - motorBackRight.setPower(0); - //robotArm.setPower(0); - //motorCarouselSpin.setPower(0); - - // Set all motors to run without encoders. - // May want to use RUN_USING_ENCODERS if encoders are installed. - motorFrontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - motorFrontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - motorBackLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - motorBackRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - - // Define and initialize ALL installed servos. - //leftClaw = hwMap.servo.get("leftHand"); - //rightClaw = hwMap.servo.get("rightHand"); - //leftClaw.setPosition(MID_SERVO); - //rightClaw.setPosition(MID_SERVO); - } - } - +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; + +/** + * This is NOT an opmode. + * + * This class can be used to define all the specific hardware for a single robot. + * In this case that robot is a Pushbot. + * See PushbotTeleopTank_Iterative and others classes starting with "Pushbot" for usage examples. + * + * This hardware class assumes the following device names have been configured on the robot: + * Note: All names are lower case and some have single spaces between words. + * + * Motor channel: Front Left drive motor: "motorFrontLeft" + * Motor channel: Front Right drive motor: "motorFrontRight" + * Motor channel: Back Left drive motor: "motorBackLeft" + * Motor channel: Back Right drive motor: "motorBackRight" + * Motor channel: Manipulator drive motor: "Arm" + * Servo channel: Servo to open left claw: "leftHand" + * Servo channel: Servo to open right claw: "rightHand" + * Motor channel: Carousel Spin: "motorCarousel" + */ + +public class NFMyRobot +{ + /* Public OpMode members. */ + public DcMotor motorFrontLeft = null; + public DcMotor motorFrontRight = null; + public DcMotor motorBackLeft = null; + public DcMotor motorBackRight = null; + public DcMotor motorCarouselSpin = null; + public DcMotor motorArm = null; + //public DcMotor robotArm = null; + public Servo servo_1 = null; + public Servo servo_2 = null; + public Servo servo_3 = null; + + public static final double MID_SERVO = 0.5 ; + public static final double ARM_UP_POWER = 0.45 ; + public static final double ARM_DOWN_POWER = -0.45 ; + + + /* local OpMode members. */ + HardwareMap hwMap = null; + private ElapsedTime period = new ElapsedTime(); + + /* Constructor */ + public NFMyRobot(){ + + } + + + /* Initialize standard Hardware interfaces */ + public void init(HardwareMap ahwMap) { + // Save reference to Hardware map + hwMap = ahwMap; + + // Define and Initialize Motors + motorFrontLeft = hwMap.dcMotor.get("motorFrontLeft"); + motorFrontRight = hwMap.dcMotor.get("motorFrontRight"); + motorBackLeft = hwMap.dcMotor.get("motorBackLeft"); + motorBackRight = hwMap.dcMotor.get("motorBackRight"); + motorArm = hwMap.dcMotor.get("motorArm"); + motorCarouselSpin = hwMap.dcMotor.get("motorCarousel"); + servo_1 = hwMap.servo.get("left_hand"); + servo_2 = hwMap.servo.get("right_hand"); + //servo_3 = hwMap.servo.get(Servo.class, "thumb"); + + + // Reverse the right side motors + motorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); + motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE); + + // Set all motors to zero power + motorFrontLeft.setPower(0); + motorFrontRight.setPower(0); + motorBackLeft.setPower(0); + motorBackRight.setPower(0); + motorArm.setPower(0); + motorCarouselSpin.setPower(0); + + // Set all motors to run without encoders. + // May want to use RUN_USING_ENCODERS if encoders are installed. + motorFrontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + motorFrontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + motorBackLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + motorBackRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + motorArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + // Define and initialize ALL installed servos. + //leftClaw = hwMap.servo.get("leftHand"); + //rightClaw = hwMap.servo.get("rightHand"); + //leftClaw.setPosition(MID_SERVO); + //rightClaw.setPosition(MID_SERVO); + } + } + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md index 8b137891791f..d3f5a12faa99 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md @@ -1 +1 @@ - +