diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/BlueBoardSide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/BlueBoardSide.java index 922151b..cdfca38 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/BlueBoardSide.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/BlueBoardSide.java @@ -110,6 +110,7 @@ else if (hasBeenPressed) { @Override public void runStart() { + openClaw(0.5); telemetry.addData("found: ", sensorRange.getDistance(DistanceUnit.INCH)); telemetry.update(); int scanTimes = 0; @@ -135,9 +136,9 @@ public void runStart() { telemetry.update(); closeClaw(); // close - sleep(500); + sleep(700); // lift arm above ground, holding 1 pixel - rotateArm(400, 1); + rotateArm(100, 1); sleep(1000); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/BlueBoardSideGuess.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/BlueBoardSideGuess.java index 1e975c4..de31f5d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/BlueBoardSideGuess.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/BlueBoardSideGuess.java @@ -92,6 +92,7 @@ else if (hasBeenPressed) { @Override public void runStart() { + openClaw(0.5); // guess that team prop is in middle closeClaw(); sleep(2000); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/BlueFarSideGuess.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/BlueFarSideGuess.java index a496009..628baae 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/BlueFarSideGuess.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/BlueFarSideGuess.java @@ -100,6 +100,7 @@ else if (hasBeenPressed) { @Override public void runStart() { + openClaw(0.5); // guess that team prop is in middle strafeRightAuto(3,3,500); // away from rigging diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/RedBoardSideGuess.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/RedBoardSideGuess.java index 1d48477..55a2e04 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/RedBoardSideGuess.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/RedBoardSideGuess.java @@ -92,6 +92,7 @@ else if (hasBeenPressed) { @Override public void runStart() { + openClaw(0.5); // guess that team prop is in middle closeClaw(); sleep(2000); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/RedFarSide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/RedFarSide.java index a9d726c..0afb81b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/RedFarSide.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/RedFarSide.java @@ -103,6 +103,7 @@ else if (hasBeenPressed) { @Override public void runStart() { + openClaw(0.5); telemetry.addData("found: ", sensorRange.getDistance(DistanceUnit.INCH)); telemetry.update(); @@ -129,9 +130,9 @@ public void runStart() { telemetry.update(); closeClaw(); // close - sleep(500); + sleep(700); // lift arm above ground, holding 1 pixel - rotateArm(400, 1); + rotateArm(100, 1); sleep(1000); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/RedFarSideGuess.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/RedFarSideGuess.java index 4310403..fff0530 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/RedFarSideGuess.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/RedFarSideGuess.java @@ -100,6 +100,7 @@ else if (hasBeenPressed) { @Override public void runStart() { + openClaw(0.5); // guess that team prop is in middle strafeLeftAuto(3,2,500); // away from rigging diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/teleop/FieldCentric.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/teleop/FieldCentric.java index da31229..1183785 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/teleop/FieldCentric.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/teleop/FieldCentric.java @@ -5,6 +5,7 @@ import static org.firstinspires.ftc.teamcode.operations.inputs.AprilTag.visionPortal; import static org.firstinspires.ftc.teamcode.operations.inputs.Target_inputs.cameraConnected; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.definingDriveMovements.EachMotorSet.driveStop; +import static org.firstinspires.ftc.teamcode.operations.outputs.motors.servos.claw.clawMovements.openClaw; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DistanceSensor; @@ -41,6 +42,7 @@ public void runInitLoop() { @Override public void runStart() { + openClaw(0.5); //claw.setPosition(claw.getPosition()+2000); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/teleop/RobotCentric.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/teleop/RobotCentric.java index 22d113a..666cb9d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/teleop/RobotCentric.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/teleop/RobotCentric.java @@ -11,6 +11,7 @@ import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.ConfigureMotors.fl; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.ConfigureMotors.fr; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.definingDriveMovements.EachMotorSet.driveStop; +import static org.firstinspires.ftc.teamcode.operations.outputs.motors.servos.claw.clawMovements.openClaw; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; @@ -53,6 +54,7 @@ public void runStart() { br.setMode(DcMotor.RunMode.RUN_USING_ENCODER); arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); shaft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + openClaw(0.5); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/inOut/driverControlled/FieldCentric.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/inOut/driverControlled/FieldCentric.java index 2dca019..dc9a320 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/inOut/driverControlled/FieldCentric.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/inOut/driverControlled/FieldCentric.java @@ -18,6 +18,7 @@ import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.ConfigureMotors.mapMotors; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Mecanum.botHeading; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Mecanum.dpadMovements; +import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Mecanum.extraSpeed; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Mecanum.fieldCentricMath; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Target_drive.backLeftPower; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Target_drive.backRightPower; @@ -25,6 +26,7 @@ import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Target_drive.frontRightPower; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.definingDriveMovements.EachMotorSet.drive; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.servos.claw.Target_claw.claw; +import static org.firstinspires.ftc.teamcode.operations.outputs.motors.servos.claw.clawMovements.openClaw; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; @@ -64,7 +66,7 @@ public static void runLoopFieldCentric(Telemetry telemetry, Gamepad gamepad1, Ga telemetry.addData("Distance in Inches: ",sensorRange.getDistance(DistanceUnit.INCH)); dpadMovements(gamepad1, speed); // sets waypoints to the d_pads's positions - + extraSpeed(gamepad1); imuReset(gamepad1.options); // resets imu case of accidents or incidences fieldCentricMath(); // does the required math for Mecanum drive as well as getting imu for field centric @@ -87,7 +89,7 @@ public static void runLoopFieldCentric(Telemetry telemetry, Gamepad gamepad1, Ga //clawUseWithGamepad(gamepad2); // using the claw (open/close) //armUseWithGamepad(gamepad2); // using the arm (rotation) //shaftUseWithGamepad(gamepad2); // using the shaft/lift (up/down) - drive(frontLeftPower,frontRightPower,backLeftPower,backRightPower); + drive(extraSpeed+frontLeftPower,extraSpeed+frontRightPower,extraSpeed+backLeftPower,extraSpeed+backRightPower); // sets each motor to the encoder counts given by the waypoints method } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/inOut/driverControlled/RobotCentric.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/inOut/driverControlled/RobotCentric.java index 8cd1c92..f5ccb58 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/inOut/driverControlled/RobotCentric.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/inOut/driverControlled/RobotCentric.java @@ -19,6 +19,7 @@ import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.ConfigureMotors.mapMotors; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Mecanum.botHeading; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Mecanum.dpadMovements; +import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Mecanum.extraSpeed; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Mecanum.robotCentricMath; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Target_drive.backLeftPower; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Target_drive.backRightPower; @@ -70,7 +71,9 @@ public static void runLoopRobotCentric(Telemetry telemetry, Gamepad gamepad1, Ga telemetry.addData("claw: ",claw.getPosition()); telemetry.addData("Distance in Inches: ",sensorRange.getDistance(DistanceUnit.INCH)); telemetry.addData("touch sensor", button.isPressed()); + dpadMovements(gamepad1, speed); // sets waypoints to the d_pads's positions + extraSpeed(gamepad1); imuReset(gamepad1.options); // resets imu case of accidents or incidences diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/motors/drive/Mecanum.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/motors/drive/Mecanum.java index de650cf..2f35b69 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/motors/drive/Mecanum.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/motors/drive/Mecanum.java @@ -9,6 +9,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; public class Mecanum { + public static double extraSpeed; public static double botHeading; static double rotX; @@ -51,6 +52,9 @@ public static void waypoints(double Y, double X, double RX) { rx = RX; } + public static void extraSpeed(Gamepad gamepad1) { + extraSpeed = gamepad1.right_trigger; + } public static void dpadMovements(Gamepad gamepad1, double speed) { // Mecanum using the dpad, if drivers want it, its there. // how this works is it sets the x and y values depending on what button is pressed. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/motors/servos/claw/clawMovements.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/motors/servos/claw/clawMovements.java index d76abb6..c591485 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/motors/servos/claw/clawMovements.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/motors/servos/claw/clawMovements.java @@ -5,7 +5,7 @@ public class clawMovements { public static void openClaw() { - claw.setPosition(1); // 0.5 + claw.setPosition(0.9); // 0.5 } public static void openClaw(double value) {