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..90da515 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", @@ -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 @@ -251,13 +255,11 @@ public class Autonomoustest extends LinearOpMode { public void executeAuto() { arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - gripper.setPosition(1); - sleep(1000); - driveForward(28); + driveForward(26); int blueleft = readColorLeft(); int blueright = readColorRight(); double backboard = 29; - if (blueleft > 75) + if (blueleft > 50 ) { //telemetry.addData("color sensor","left"); if(blueleft > blueright) @@ -271,44 +273,45 @@ public class Autonomoustest extends LinearOpMode { straightLeft(32); turnLeft(10); driveForward(18); - driveForward(-31); - straightRight(31.5); - raisearm(80); - wrist.setPosition(0); - raisearm(100); - driveForward(-1); - gripper.setPosition(0.25); + driveForward(-40); +// straightRight(31.5); +// raisearm(80); +// wrist.setPosition(0); +// raisearm(100); +// driveForward(-1); +// gripper.setPosition(0.25); terminateOpModeNow(); } - if (blueright > 175) + if (blueright > 50) { if(blueleft < blueright) telemetry.addData("color sensor","right"); straightRight(11); raisearm(80); arm.setPower(0); - driveForward(-13.5); + driveForward(-15.5); turnLeft(90); straightLeft(15); driveForward(8); - driveForward(-26); - straightRight(22.5); - raisearm(80); - wrist.setPosition(0); - raisearm(100); - driveForward(-5); - gripper.setPosition(.25); + 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(4); + driveForward(2.5); raisearm(80); arm.setPower(0); driveForward(-8); @@ -323,6 +326,9 @@ public class Autonomoustest extends LinearOpMode { wrist.setPosition(0); raisearm(100); gripper.setPosition(.25); + sleep(500); + driveForward(5); + raisearm(-270); telemetry.update(); sleep(250); 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);