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 1ab5777..8973fd5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java @@ -93,7 +93,7 @@ public class Autonomoustest extends LinearOpMode { 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.2; + static final double DRIVE_SPEED = 0.3; static final double TURN_SPEED = 0.4; static final double LONG_TIMEOUT = 1000; @@ -105,6 +105,8 @@ public class Autonomoustest extends LinearOpMode { public void runOpMode() { hardwareinit(); + gripper.setPosition(1); + sleep(1000); // Send telemetry message to indicate successful Encoder reset /* telemetry.addData("Starting at", "%7d :%7d", @@ -204,7 +206,7 @@ public class Autonomoustest extends LinearOpMode { } 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"); @@ -214,6 +216,8 @@ public class Autonomoustest extends LinearOpMode { gripper = hardwareMap.get(Servo.class, "gripper"); arm = hardwareMap.get(DcMotor.class, "arm raise"); wrist = hardwareMap.get(Servo.class, "wrist"); + wrist.setPosition(1); + 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 @@ -249,91 +253,93 @@ public class Autonomoustest extends LinearOpMode { } public void executeAuto() + { + arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + driveForward(26); + int blueleft = readColorLeft(); + int blueright = readColorRight(); + double backboard = 29; + if (blueleft > 50 ) { - arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - gripper.setPosition(1); - sleep(1000); - driveForward(28); - int blueleft = readColorLeft(); - int blueright = readColorRight(); - double backboard = 29; - if (blueleft > 75) - { - //telemetry.addData("color sensor","left"); - if(blueleft > blueright) - telemetry.addData("color sensor","left"); - turnLeft(90); - straightLeft(2); - driveForward(6.5); - raisearm(80); - arm.setPower(0); - driveForward(-23); - straightLeft(32); - turnLeft(10); - driveForward(18); - driveForward(-31); - straightRight(31.5); - raisearm(80); - wrist.setPosition(0); - raisearm(100); - driveForward(-1); - gripper.setPosition(0.25); - terminateOpModeNow(); - - - - - } - if (blueright > 175) - { - if(blueleft < blueright) - telemetry.addData("color sensor","right"); - straightRight(11); - raisearm(80); - arm.setPower(0); - driveForward(-13.5); - turnLeft(90); - straightLeft(15); - driveForward(8); - driveForward(-26); - straightRight(22.5); - raisearm(80); - wrist.setPosition(0); - raisearm(100); - driveForward(-5); - gripper.setPosition(.25); - terminateOpModeNow(); - - - } - else - telemetry.addData("position","center"); - driveForward(4); - raisearm(80); + //telemetry.addData("color sensor","left"); + if(blueleft > blueright) + telemetry.addData("color sensor","left"); + turnLeft(90); + straightLeft(2); + driveForward(6.5); + raisearm(80); arm.setPower(0); - driveForward(-8); - straightRight(11.5); - driveForward(-15); + driveForward(-23); + straightLeft(32); + turnLeft(10); + driveForward(18); + driveForward(-40); +// straightRight(31.5); +// raisearm(80); +// wrist.setPosition(0); +// raisearm(100); +// driveForward(-1); +// gripper.setPosition(0.25); + terminateOpModeNow(); + + + + + } + if (blueright > 50) + { + if(blueleft < blueright) + telemetry.addData("color sensor","right"); + straightRight(11); + raisearm(80); + arm.setPower(0); + driveForward(-15.5); turnLeft(90); straightLeft(15); driveForward(8); - driveForward(-26); - straightRight(29); - raisearm(80); - wrist.setPosition(0); - raisearm(100); - gripper.setPosition(.25); - telemetry.update(); - sleep(250); + driveForward(-38); +// straightRight(14.5); +// raisearm(80); +// wrist.setPosition(0); +// raisearm(100); +// gripper.setPosition(.25); +// driveForward(5); +// raisearm(-200); + terminateOpModeNow(); + + + } + else + telemetry.addData("position","center"); + driveForward(2.5); + raisearm(80); + arm.setPower(0); + driveForward(-8); + straightRight(11.5); + driveForward(-15); + turnLeft(90); + straightLeft(15); + driveForward(8); + driveForward(-26); + straightRight(29); + raisearm(80); + wrist.setPosition(0); + raisearm(100); + gripper.setPosition(.25); + sleep(500); + driveForward(5); + raisearm(-270); + telemetry.update(); + sleep(250); - //Values were created from robot with wheel issues 9/28/23 + //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. + telemetry.addData("Path", "Complete"); + telemetry.update(); + sleep(1000); // pause to display final telemetry message. }