From eb81e1b4b62d8d2bb7b9fb74c0adaff3b50e1980 Mon Sep 17 00:00:00 2001 From: robotics2 Date: Sat, 14 Oct 2023 11:56:23 -0700 Subject: [PATCH] arm encoders made, tested, and implmented into main code --- .../ftc/teamcode/Autonomoustest.java | 238 +++++++++++------- 1 file changed, 146 insertions(+), 92 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java index dc87b1c..e7c5623 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java @@ -102,39 +102,10 @@ public class Autonomoustest extends LinearOpMode { static final double TICKS_TO_DEGREES = 0.07462686567; @Override - public void runOpMode() { + public void runOpMode() + { + hardwareinit(); - // Initialize the drive system variables - 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"); - colorRight = hardwareMap.get(ColorSensor.class, "color right"); - colorLeft = hardwareMap.get(ColorSensor.class, "color left"); - gripper = hardwareMap.get(Servo.class, "gripper"); - arm = hardwareMap.get(DcMotor.class, "arm raise"); - wrist = hardwareMap.get(Servo.class, "wrist"); - // 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.FORWARD); - - 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); // Send telemetry message to indicate successful Encoder reset /* telemetry.addData("Starting at", "%7d :%7d", leftDrive.getCurrentPosition(), @@ -146,70 +117,23 @@ public class Autonomoustest extends LinearOpMode { // Wait for the game to start (driver presses PLAY) waitForStart(); + { + //executeAuto(); + testGripper(); + + } // Step through each leg of the path, // Note: Reverse movement is obtained by setting a negative distance (not speed) - { - raisearm(100); - int blueleft = readColorLeft(); - int blueright = readColorRight(); - if (blueleft > 75) - { - //telemetry.addData("color sensor","left"); - if(blueleft > blueright) - telemetry.addData("color sensor","left"); - turnLeft(90); - straightLeft(2); - driveForward(11); - driveForward(-30); - terminateOpModeNow(); - - - - } - if (blueright > 75) - { - //telemetry.addData("color sensor", "right"); - if(blueleft < blueright) - telemetry.addData("color sensor","right"); - straightRight(11); - driveForward(-17); - turnRight(90); - straightLeft(8); - driveForward(-28.5); - raisearm(80); - straightRight(22); - driveForward(-11); - terminateOpModeNow(); - - - } - else - telemetry.addData("position","center"); - driveForward(7); - driveForward(-13); - turnRight(90); - driveForward(-35); - raisearm(80); - straightRight(22.5); - driveForward(-11); - - telemetry.update(); - sleep(250); - - } - - - //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. } - public void driveForward(double distance) { + +// + + public void driveForward(double distance) + { leftDrive.setDirection(DcMotor.Direction.REVERSE); rightDrive.setDirection(DcMotor.Direction.FORWARD); backrightDrive.setDirection(DcMotor.Direction.REVERSE); @@ -217,7 +141,8 @@ public class Autonomoustest extends LinearOpMode { encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); // S1: Forward 47 Inches with 5 Sec timeout } - public void straightLeft(double distance) { + public void straightLeft(double distance) + { leftDrive.setDirection(DcMotor.Direction.FORWARD); rightDrive.setDirection(DcMotor.Direction.FORWARD); backrightDrive.setDirection(DcMotor.Direction.FORWARD); @@ -225,7 +150,8 @@ public class Autonomoustest extends LinearOpMode { encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); } - public void straightRight(double distance) { + public void straightRight(double distance) + { leftDrive.setDirection(DcMotor.Direction.REVERSE); rightDrive.setDirection(DcMotor.Direction.REVERSE); backrightDrive.setDirection(DcMotor.Direction.REVERSE); @@ -233,7 +159,8 @@ public class Autonomoustest extends LinearOpMode { encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); } - public void turnLeft(double degrees) { + public void turnLeft(double degrees) + { leftDrive.setDirection(DcMotor.Direction.FORWARD); rightDrive.setDirection(DcMotor.Direction.FORWARD); backrightDrive.setDirection(DcMotor.Direction.REVERSE); @@ -276,6 +203,132 @@ public class Autonomoustest extends LinearOpMode { 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"); + colorRight = hardwareMap.get(ColorSensor.class, "color right"); + colorLeft = hardwareMap.get(ColorSensor.class, "color left"); + gripper = hardwareMap.get(Servo.class, "gripper"); + arm = hardwareMap.get(DcMotor.class, "arm raise"); + wrist = hardwareMap.get(Servo.class, "wrist"); + // 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); + sleep(3000); + gripper.setPosition(1); + sleep(3000); + } + public void executeAuto() + { + + raisearm(100); + wrist.setPosition(0); + driveForward(28); + int blueleft = readColorLeft(); + int blueright = readColorRight(); + if (blueleft > 75) + { + //telemetry.addData("color sensor","left"); + if(blueleft > blueright) + telemetry.addData("color sensor","left"); + turnLeft(90); + straightLeft(2); + driveForward(11); + driveForward(-30); + straightLeft(32); + turnLeft(10); + driveForward(18); + driveForward(-31); + double backboard = 34; + straightRight(backboard); + raisearm(80); + driveForward(-1); + gripper.setPosition(0); + terminateOpModeNow(); + + + + + } + if (blueright > 75) + { + //telemetry.addData("color sensor", "right"); + if(blueleft < blueright) + telemetry.addData("color sensor","right"); + straightRight(11.5); + driveForward(-17); + turnLeft(90); + straightLeft(15); + driveForward(8); + driveForward(-26); + double backboard = 22.5; + straightRight(backboard); + raisearm(80); + driveForward(-5); + gripper.setPosition(1); + terminateOpModeNow(); + + + } + else + telemetry.addData("position","center"); + driveForward(7); + driveForward(-7); + straightRight(11.5); + driveForward(-17); + turnLeft(90); + straightLeft(15); + driveForward(8); + driveForward(-26); + double backboard = 29; + straightRight(backboard); + raisearm(80); + driveForward(-5); + gripper.setPosition(1); + telemetry.update(); + sleep(250); + + + + + //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. + } @@ -400,6 +453,7 @@ public class Autonomoustest extends LinearOpMode { // Turn off RUN_TO_POSITION arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } } } \ No newline at end of file