diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java index 49f5650..fd91263 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java @@ -93,7 +93,7 @@ public class bluefront 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; @@ -214,6 +214,8 @@ public class bluefront 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 @@ -253,67 +255,68 @@ public class bluefront extends LinearOpMode { public void executeAuto() { arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - driveForward(28); + driveForward(26); int blueleft = readColorLeft(); int blueright = readColorRight(); double backboard = 29; - if (blueleft > 100) + if (blueleft > 75) { //telemetry.addData("color sensor","left"); if(blueleft > blueright) telemetry.addData("color sensor","left"); - straightLeft(8); - raisearm(30); + straightLeft(11); + raisearm(80); arm.setPower(0); - driveForward(-13.5); + driveForward(-15.5); turnRight(90); straightRight(15); driveForward(8); - driveForward(-26); - straightLeft(22.5); - raisearm(80); - wrist.setPosition(0); - raisearm(100); - driveForward(-5); - gripper.setPosition(.25); + driveForward(-38); +// straightLeft(22.5); +// raisearm(80); +// wrist.setPosition(0); +// raisearm(100); +// driveForward(-5); +// gripper.setPosition(.25); terminateOpModeNow(); } - if (blueright > 100) + if (blueright > 75) { if(blueleft < blueright) telemetry.addData("color sensor","right"); turnRight(90); straightLeft(2); - raisearm(30); + driveForward(6.5); + raisearm(80); arm.setPower(0); - driveForward(-15); + driveForward(-23); straightRight(32); turnRight(10); driveForward(18); - driveForward(-31); - straightLeft(34); - raisearm(80); - wrist.setPosition(0); - raisearm(100); - driveForward(-1); - gripper.setPosition(0.25); - terminateOpModeNow(); + driveForward(-40); +// straightLeft(34); +// raisearm(80); +// wrist.setPosition(0); +// raisearm(100); +// driveForward(-1); +// gripper.setPosition(0.25); +// terminateOpModeNow(); } else telemetry.addData("position","center"); - driveForward(11); - raisearm(30); + driveForward(2.5); + raisearm(80); arm.setPower(0); - driveForward(-6); + driveForward(-8); straightLeft(11.5); - driveForward(-17); + driveForward(-15); turnRight(90); straightRight(15); driveForward(8); @@ -322,8 +325,9 @@ public class bluefront extends LinearOpMode { raisearm(80); wrist.setPosition(0); raisearm(100); - driveForward(-5); gripper.setPosition(.25); + sleep(500); + driveForward(5); telemetry.update(); sleep(250);