From 913544f4fd1c06806405bca7ced4c3d32ddbadbe Mon Sep 17 00:00:00 2001 From: robotics3 Date: Thu, 25 Jan 2024 17:15:58 -0800 Subject: [PATCH] Add red back stage code (work in progress) --- HARDWARE.md | 33 +- .../ftc/teamcode/RedBackStageTest.java | 479 ++++++++++++++++++ 2 files changed, 495 insertions(+), 17 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedBackStageTest.java diff --git a/HARDWARE.md b/HARDWARE.md index dd27e58..4996c4c 100644 --- a/HARDWARE.md +++ b/HARDWARE.md @@ -4,28 +4,27 @@ ![Bird's eye view of robot.](/Robot.png "Bird's eye view of robot") -Configuration Name: **CometBoTsChassis2023** +Configuration Name: **cometBoTsChassis2023** There are two robots: 14493-DS, and FTC-992M. Below are the following configurations for our robots -| physical port | hub | robot part | robot part location | robot software config name | -|---------------|-----------|------------------------------|-------------------------------|----------------------------| -| motor0 | control | UltraPlanetary HD hex motor | right front leg frame | Drive front rt | -| motor1 | control | UltraPlanetary HD hex motor | right back leg frame | Drive back rt | -| motor2 | control | UltraPlanetary HD hex motor | left front leg frame | Drive front lt | -| motor3 | control | UltraPlanetary HD hex motor | left back leg frame | Drive back lt | -| I2C B0 | control | Color sensor V3 | Left outside leg frame | color left | - | I2C B1 | control | Color sensor V3 | Right outside leg frame | color right | - | I2C B0 | expansion | 2m distance sensor | Middle Back outside leg frame | distance | -| motor0 | expansion | UltraPlanetary HD hex motor | left back arm frame | arm raise | -| motor1 | expansion | Core Hex Motor | right back arm frame | hang | -| motor3 | expansion | UltraPlanetary HD hex motor* | arm frame back right | axle encoder | -| Servo 0 | expansion | Servo | on arm | wrist | -| Servo 1 | expansion | Servo | on arm | gripper | - -** * **: The device plugged into motor3 is actually a Digital Device encoder but behaves like a UltraPlanetary HD hex motor encoder. +| physical port | hub | robot part | robot part location | robot software config name | +|---------------|-----------|----------------------------|-------------------------------|----------------------------| +| motor0 | control | UltraPlanetary HD hex motor | right front leg frame | Drive front rt | +| motor1 | control | UltraPlanetary HD hex motor | right back leg frame | Drive back rt | +| motor2 | control | UltraPlanetary HD hex motor | left front leg frame | Drive front lt | +| motor3 | control | UltraPlanetary HD hex motor | left back leg frame | Drive back lt | +| I2C B0 | control | Color sensor V3 | Left outside leg frame | color left | + | I2C B1 | control | Color sensor V3 | Right outside leg frame | color right | + | I2C B0 | expansion | 2m distance sensor | Middle Back outside leg frame | distance | +| motor0 | expansion | UltraPlanetary HD hex motor | left back arm frame | arm raise | +| motor1 | expansion | Core Hex Motor | right back arm frame | hang | +| motor3 | expansion | Digital device | arm frame back right | axle encoder | +| Servo 0 | expansion | Servo | on arm | wrist | +| Servo 1 | expansion | Servo | on arm | gripper | + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedBackStageTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedBackStageTest.java new file mode 100644 index 0000000..01c9f6b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedBackStageTest.java @@ -0,0 +1,479 @@ +/* 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 android.annotation.SuppressLint; + +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.DistanceSensor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +/** + * This file illustrates the concept of driving a path based on encoder counts. + * The code is structured as a LinearOpMode + * + * The code REQUIRES that you DO have encoders on the wheels, + * otherwise you would use: RobotAutoDriveByTime; + * + * This code ALSO requires that the drive Motors have been configured such that a positive + + * power command moves them forward, and causes the encoders to count UP. + * + * The desired path in this example is: + * - Drive forward for 48 inches + * - Spin right for 12 Inches + * - Drive Backward 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 method 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 Studio 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="red (backstage test subject 716,980)", group="Robot") +//@Disabled +public class RedBackStageTest extends LinearOpMode { + + /* Declare OpMode members. */ + private DcMotor leftDrive = null; + private DcMotor rightDrive = null; + private DcMotor backrightDrive = null; + private DcMotor backleftDrive = null; + private DistanceSensor distanceRight = null; + private DistanceSensor distanceLeft = null; + private Servo wrist = null; + private Servo gripper = null; + private DcMotor arm = null; + private DistanceSensor distance = null; + + + private ElapsedTime runtime = new ElapsedTime(); + + // Calculate the COUNTS_PER_INCH for your specific drive train. + // Go to your motor vendor website to determine your motor's COUNTS_PER_MOTOR_REV + // For external drive gearing, set DRIVE_GEAR_REDUCTION as needed. + // For example, use a value of 2.0 for a 12-tooth spur gear driving a 24-tooth spur gear. + // This is gearing DOWN for less speed and more torque. + // For gearing UP, use a gear ratio less than 1.0. Note this will affect the direction of wheel rotation. + static final double COUNTS_PER_MOTOR_REV = 537.6; // eg: TETRIX Motor Encoder + static final double DRIVE_GEAR_REDUCTION = 1.0; // No External Gearing. + static final double WHEEL_DIAMETER_INCHES = 3.77953; // For figuring circumference + static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / + (WHEEL_DIAMETER_INCHES * Math.PI); + static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.7 * Math.PI); + static final double DRIVE_SPEED = 0.35; + static final double TURN_SPEED = 0.4; + + static final double LONG_TIMEOUT = 1000; + static final double DEGREE_TOO_DISTANCE = 0.21944444444; + static final double ARM_SPEED = .1; + static final double TICKS_TO_DEGREES = 0.07462686567; + + @Override + public void runOpMode() + { + hardwareinit(); + + // Send telemetry message to indicate successful Encoder reset + /* telemetry.addData("Starting at", "%7d :%7d", + leftDrive.getCurrentPosition(), + rightDrive.getCurrentPosition(), + backleftDrive.getCurrentPosition(), + backrightDrive.getCurrentPosition());*/ + + telemetry.update(); + + // Wait for the game to start (driver presses PLAY) + waitForStart(); + { + executeAuto(); + + } + + // Step through each leg of the path, + // Note: Reverse movement is obtained by setting a negative distance (not speed) + + + } + + +// + + public void driveForward(double distance) + { + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + backrightDrive.setDirection(DcMotor.Direction.REVERSE); + backleftDrive.setDirection(DcMotor.Direction.REVERSE); + encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); // S1: Forward 47 Inches with 5 Sec timeout + } + + public void straightLeft(double distance) + { + leftDrive.setDirection(DcMotor.Direction.FORWARD); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + backrightDrive.setDirection(DcMotor.Direction.FORWARD); + backleftDrive.setDirection(DcMotor.Direction.REVERSE); + encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); + } + + public void straightRight(double distance) + { + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.REVERSE); + backrightDrive.setDirection(DcMotor.Direction.REVERSE); + backleftDrive.setDirection(DcMotor.Direction.FORWARD); + encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); + } + + public void turnLeft(double degrees) + { + leftDrive.setDirection(DcMotor.Direction.FORWARD); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + backrightDrive.setDirection(DcMotor.Direction.REVERSE); + backleftDrive.setDirection(DcMotor.Direction.FORWARD); + double turning_distance = degrees * DEGREE_TOO_DISTANCE; + encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT); + } + + public void turnRight(double degrees) { + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.REVERSE); + backrightDrive.setDirection(DcMotor.Direction.FORWARD); + backleftDrive.setDirection(DcMotor.Direction.REVERSE); + double turning_distance = degrees * DEGREE_TOO_DISTANCE; + encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT); + } + public void raisearm(int degrees) { + armEncoder(ARM_SPEED, degrees*TICKS_TO_DEGREES, LONG_TIMEOUT); + + } + public void hardwareinit() + { + leftDrive = hardwareMap.get(DcMotor.class, "Drive front lt"); + rightDrive = hardwareMap.get(DcMotor.class, "Drive front rt"); + backleftDrive = hardwareMap.get(DcMotor.class, "Drive back lt"); + backrightDrive = hardwareMap.get(DcMotor.class, "Drive back rt"); + distanceRight = hardwareMap.get(DistanceSensor.class, "color right"); + distanceLeft = hardwareMap.get(DistanceSensor.class, "color left"); + gripper = hardwareMap.get(Servo.class, "gripper"); + arm = hardwareMap.get(DcMotor.class, "arm raise"); + wrist = hardwareMap.get(Servo.class, "wrist"); + distance = hardwareMap.get(DistanceSensor.class, "distance"); + sleep(1000); + // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. + // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive. + // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + backrightDrive.setDirection(DcMotor.Direction.REVERSE); + backleftDrive.setDirection(DcMotor.Direction.REVERSE); + arm.setDirection(DcMotor.Direction.REVERSE); + + leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + backleftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + backrightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + + leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backrightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + public void testWrist() + { + wrist.setPosition(0); + sleep(3000); + wrist.setPosition(1); + sleep(3000); + } + public void testGripper() + { + gripper.setPosition(0.5); + + } + @SuppressLint("SuspiciousIndentation") + public void executeAuto() + { + + arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + driveForward(26); + sleep(500); + + int distanceleft = (int)distanceLeft.getDistance(DistanceUnit.INCH); + int distanceright = (int)distanceRight.getDistance(DistanceUnit.INCH); + telemetry.addData("color left sensor",distanceleft); + telemetry.addData("color right sensor",distanceright); + telemetry.update(); + if (distanceleft < 7) + { + telemetry.addData("position","left"); + telemetry.update(); + turnLeft(90); + straightLeft(2); + driveForward(5.5); + raisearm(80); + arm.setPower(0); + driveForward(-21); + straightLeft(34); + driveForward(-10); + straightRight(35); + driveForward(-5); + raisearm(80); + wrist.setPosition(0); + raisearm(100); + gripper.setPosition(1); + sleep(500); + driveForward(4.5); + raisearm(-50); + wrist.setPosition(1); + raisearm(-70); + + + driveForward(1.5); + /* need to review */ + straightLeft(30); + driveForward(-15); + + terminateOpModeNow(); + + + } + if (distanceright < 7) + { + telemetry.addData("postion", "right"); + telemetry.update(); + straightRight(12); + raisearm(80); + arm.setPower(0); + driveForward(-15.5); + turnLeft(90); + straightLeft(15); + driveForward(-20.5); + straightRight(19); + driveForward(-1.5); + raisearm(80); + wrist.setPosition(0); + raisearm(100); + gripper.setPosition(1); + driveForward(8.5); + + raisearm(-50); + wrist.setPosition(1); + raisearm(-70); + + /* need to review */ + straightLeft(29); + + driveForward(-10); + + + terminateOpModeNow(); + + + } +else + telemetry.addData("postion","center"); + telemetry.update(); + driveForward(3.5); + raisearm(80); + arm.setPower(0); + driveForward(-8); + straightRight(11.5); + driveForward(-15); + turnLeft(90); + straightLeft(15); + driveForward(-18); + straightRight(29); + turnRight(10); + raisearm(80); + wrist.setPosition(0); + raisearm(100); + + gripper.setPosition(1); + driveForward(5); + + /* added from bluebackstage */ + raisearm(-50); + wrist.setPosition(1); + raisearm(-70); + straightLeft(29); + driveForward(-10); + + terminateOpModeNow(); + + + + //Values were created from robot with wheel issues 9/28/23 + + telemetry.addData("Path", "Complete"); + telemetry.update(); +// sleep(1000); // pause to display final telemetry message. + } + + + + + + /* + * Method to perform a relative move, based on encoder counts. + * Encoders are not reset as the move is based on the current position. + * Move will stop if any of three conditions occur: + * 1) Move gets to the desired position + * 2) Move runs out of time + * 3) Driver stops the opmode running. + + */ + + public void encoderDrive(double speed, + double leftInches, double rightInches, + double timeoutS) { + int newLeftTarget; + int newRightTarget; + int newBackLeftTarget; + int newbackRightTarget; + + + if (opModeIsActive()) { + + // Determine new target position, and pass to motor controller + newLeftTarget = leftDrive.getCurrentPosition() + (int) (leftInches * COUNTS_PER_INCH); + newRightTarget = rightDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH); + newBackLeftTarget = backleftDrive.getCurrentPosition() + (int) (leftInches * COUNTS_PER_INCH); + newbackRightTarget = backrightDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH); + leftDrive.setTargetPosition(newLeftTarget); + rightDrive.setTargetPosition(newRightTarget); + backrightDrive.setTargetPosition(newbackRightTarget); + backleftDrive.setTargetPosition(newBackLeftTarget); + + // Turn On RUN_TO_POSITION + leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + backrightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + backleftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + // reset the timeout time and start motion. + runtime.reset(); + leftDrive.setPower(Math.abs(speed)); + rightDrive.setPower(Math.abs(speed)); + backrightDrive.setPower(Math.abs(speed)); + backleftDrive.setPower(Math.abs(speed)); + + // keep looping while we are still active, and there is time left, and both motors are running. + // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits + // its target position, the motion will stop. This is "safer" in the event that the robot will + // always end the motion as soon as possible. + // However, if you require that BOTH motors have finished their moves before the robot continues + // onto the next step, use (isBusy() || isBusy()) in the loop test. + while (opModeIsActive() && + (runtime.seconds() < timeoutS) && + (leftDrive.isBusy() && rightDrive.isBusy() && backleftDrive.isBusy() && backrightDrive.isBusy() && backrightDrive.isBusy())) { + + // Display it for the driver. + telemetry.addData("Running to", " %7d :%7d", newLeftTarget, newRightTarget); + telemetry.addData("Currently at", " at %7d :%7d", + leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition(), backrightDrive.getCurrentPosition(), backleftDrive.getCurrentPosition()); + telemetry.update(); + } + + + leftDrive.setPower(0); + rightDrive.setPower(0); + backrightDrive.setPower(0); + backleftDrive.setPower(0); + + + // Turn off RUN_TO_POSITION + leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backrightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + sleep(250); // optional pause after each move. + } + } + + public void armEncoder(double speed, + double Inches, double timeoutS) { + int newarmTarget; + + + if (opModeIsActive()) { + + // Determine new target position, and pass to motor controller + newarmTarget = arm.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH); + arm.setTargetPosition(newarmTarget); + + // Turn On RUN_TO_POSITION + arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + // reset the timeout time and start motion. + runtime.reset(); + arm.setPower(Math.abs(speed)); + + // keep looping while we are still active, and there is time left, and both motors are running. + // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits + // its target position, the motion will stop. This is "safer" in the event that the robot will + // always end the motion as soon as possible. + // However, if you require that BOTH motors have finished their moves before the robot continues + // onto the next step, use (isBusy() || isBusy()) in the loop test. + while (opModeIsActive() && + (runtime.seconds() < timeoutS) && + (arm.isBusy())) { + + // Display it for the driver. + telemetry.addData("Running to", " %7d", newarmTarget); + telemetry.addData("Currently at", " at %7d", + arm.getCurrentPosition()); + telemetry.update(); + } + + + arm.setPower(0); + + + // Turn off RUN_TO_POSITION + arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + } + } +} \ No newline at end of file