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 7d621e5..33ab08c 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 @@ -2,7 +2,8 @@ import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.LogoFacingDirection.UP; import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.UsbFacingDirection.RIGHT; -import static org.firstinspires.ftc.teamcode.operations.inOut.driverControlled.RobotCentric.sensorRange; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.mapOtherThings; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.sensorRange; import static org.firstinspires.ftc.teamcode.operations.inputs.AprilTag.initAprilTag; import static org.firstinspires.ftc.teamcode.operations.inputs.Imu.imuGet; import static org.firstinspires.ftc.teamcode.operations.inputs.TouchSensorButton.button; @@ -79,11 +80,7 @@ public void runInit() { initAprilTag(hardwareMap, "Webcam 1", telemetry); Encoders.clear(); - sensorRange = hardwareMap.get(DistanceSensor.class, "left_eye"); - - arm = hardwareMap.get(DcMotorEx.class, "arm"); - shaft = hardwareMap.get(DcMotor.class, "shaft"); - claw = hardwareMap.get(Servo.class, "claw"); + mapOtherThings(hardwareMap); TouchSensorButton.mapDigital(hardwareMap); // button 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 de31f5d..218f168 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 @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.gamecode.autonomous; -import static org.firstinspires.ftc.teamcode.operations.inOut.driverControlled.RobotCentric.sensorRange; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.mapOtherThings; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.sensorRange; import static org.firstinspires.ftc.teamcode.operations.inputs.AprilTag.initAprilTag; import static org.firstinspires.ftc.teamcode.operations.inputs.TouchSensorButton.button; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.arm.Target_arm.arm; @@ -62,11 +63,7 @@ public void runInit() { initAprilTag(hardwareMap, "Webcam 1", telemetry); Encoders.clear(); - sensorRange = hardwareMap.get(DistanceSensor.class, "left_eye"); - - arm = hardwareMap.get(DcMotorEx.class, "arm"); - shaft = hardwareMap.get(DcMotor.class, "shaft"); - claw = hardwareMap.get(Servo.class, "claw"); + mapOtherThings(hardwareMap); TouchSensorButton.mapDigital(hardwareMap); // button } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/BlueFarSide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/BlueFarSide.java index 8d42fad..a058a9a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/BlueFarSide.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/BlueFarSide.java @@ -2,7 +2,8 @@ import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.LogoFacingDirection.UP; import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.UsbFacingDirection.RIGHT; -import static org.firstinspires.ftc.teamcode.operations.inOut.driverControlled.RobotCentric.sensorRange; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.mapOtherThings; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.sensorRange; import static org.firstinspires.ftc.teamcode.operations.inputs.AprilTag.initAprilTag; import static org.firstinspires.ftc.teamcode.operations.inputs.Imu.imuGet; import static org.firstinspires.ftc.teamcode.operations.inputs.TouchSensorButton.button; @@ -79,11 +80,7 @@ public void runInit() { initAprilTag(hardwareMap, "Webcam 1", telemetry); Encoders.clear(); - sensorRange = hardwareMap.get(DistanceSensor.class, "left_eye"); - - arm = hardwareMap.get(DcMotorEx.class, "arm"); - shaft = hardwareMap.get(DcMotor.class, "shaft"); - claw = hardwareMap.get(Servo.class, "claw"); + mapOtherThings(hardwareMap); TouchSensorButton.mapDigital(hardwareMap); // button @@ -136,7 +133,7 @@ public void runStart() { telemetry.update(); closeClaw(); // close - rotateArm(800, 1); + rotateArm(1000, 1); if (teamprop == 2) { // if the middle team prop is found then go to it and drop the pixel @@ -193,21 +190,22 @@ public void runStart() { forwardAuto(17, 1, 1000); strafeRightAuto(5, 1, 1000); turnLeftAuto(500 * 3 / 2, 1, 1000); - forwardAuto(2,1,1000); + forwardAuto(10,2,1000); openClaw(); - backwardAuto(2,1,1000); + backwardAuto(5,1,1000); turnRightAuto(500 * 3, 1, 1000); - backwardAuto(30,1,1000); + backwardAuto(30,5,1000); } } // park - forwardAuto(3, 1,1000); // - strafeRightAuto(5,1,1000); // - forwardAuto(15,1,1000); // - strafeRightAuto(17,1,1000); // - forwardAuto(38,4,1000); - strafeLeftAuto(150,10,1000); + forwardAuto(3, 3,500); // + strafeLeftAuto(5,3,500); + strafeRightAuto(8,3,500); // + forwardAuto(15,3,500); // + strafeRightAuto(13,3,500); // + forwardAuto(30,5,500); + strafeLeftAuto(123,10,1000); } 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 9b81796..3fc1753 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 @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.gamecode.autonomous; -import static org.firstinspires.ftc.teamcode.operations.inOut.driverControlled.RobotCentric.sensorRange; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.mapOtherThings; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.sensorRange; import static org.firstinspires.ftc.teamcode.operations.inputs.AprilTag.initAprilTag; import static org.firstinspires.ftc.teamcode.operations.inputs.TouchSensorButton.button; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.shaft.Target_shaft.shaft; @@ -64,11 +65,7 @@ public void runInit() { initAprilTag(hardwareMap, "Webcam 1", telemetry); Encoders.clear(); - sensorRange = hardwareMap.get(DistanceSensor.class, "left_eye"); - - arm = hardwareMap.get(DcMotorEx.class, "arm"); - shaft = hardwareMap.get(DcMotor.class, "shaft"); - claw = hardwareMap.get(Servo.class, "claw"); + mapOtherThings(hardwareMap); fl.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); fr.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); @@ -103,23 +100,24 @@ public void runStart() { openClaw(0.5); // guess that team prop is in middle - strafeRightAuto(3,3,500); // away from rigging - forwardAuto(25,3,500); - backwardAuto(25,3,500); + strafeRightAuto(3,3,100); // away from rigging + forwardAuto(25,3,100); + backwardAuto(25,3,100); // park - forwardAuto(3, 3,500); // - strafeRightAuto(8,3,500); // - forwardAuto(15,3,500); // - strafeRightAuto(17,3,500); // - forwardAuto(48,4,500); - strafeLeftAuto(123,5,500); + forwardAuto(3, 3,100); // + strafeLeftAuto(10,3,100); + strafeRightAuto(8,3,100); // + forwardAuto(15,3,100); // + strafeRightAuto(17,3,100); // + forwardAuto(48,4,100); + strafeLeftAuto(123,5,100); } @Override public void runLoop() { - telemetry.addData("target ", 500); + telemetry.addData("target ", 100); telemetry.addData("position FL ", fl.getCurrentPosition()); telemetry.addData("position FR ", fr.getCurrentPosition()); telemetry.addData("position BL ", bl.getCurrentPosition()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/RedBoardSide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/RedBoardSide.java index 24a1d5c..27e9aa3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/RedBoardSide.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/RedBoardSide.java @@ -2,7 +2,8 @@ import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.LogoFacingDirection.UP; import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.UsbFacingDirection.RIGHT; -import static org.firstinspires.ftc.teamcode.operations.inOut.driverControlled.RobotCentric.sensorRange; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.mapOtherThings; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.sensorRange; import static org.firstinspires.ftc.teamcode.operations.inputs.AprilTag.initAprilTag; import static org.firstinspires.ftc.teamcode.operations.inputs.Imu.imuGet; import static org.firstinspires.ftc.teamcode.operations.inputs.TouchSensorButton.button; @@ -79,11 +80,7 @@ public void runInit() { initAprilTag(hardwareMap, "Webcam 1", telemetry); Encoders.clear(); - sensorRange = hardwareMap.get(DistanceSensor.class, "left_eye"); - - arm = hardwareMap.get(DcMotorEx.class, "arm"); - shaft = hardwareMap.get(DcMotor.class, "shaft"); - claw = hardwareMap.get(Servo.class, "claw"); + mapOtherThings(hardwareMap); TouchSensorButton.mapDigital(hardwareMap); // button @@ -136,10 +133,7 @@ public void runStart() { telemetry.update(); closeClaw(); // close - sleep(700); - // lift arm above ground, holding 1 pixel - rotateArm(100, 1); - sleep(1000); + rotateArm(1000, 1); if (teamprop == 2) { // if the middle team prop is found then go to it and drop the pixel @@ -147,17 +141,17 @@ public void runStart() { telemetry.addData("teamprop", teamprop); telemetry.addData("distance", sensorRange.getDistance(DistanceUnit.INCH)); telemetry.update(); - forwardAuto(26, 5, 800); + forwardAuto(26, 2, 1000); openClaw(); - // park - backwardAuto(26, 5, 800); + backwardAuto(27,1,1000); + } else { scanTimes = 0; found = false; // if the middle team prop is not found then search for the team prop on zone 1 - forwardAuto(8,1,10000); - strafeRightAuto(5, 1, 800); + forwardAuto(14,1,1000); + strafeRightAuto(10, 1, 1000); /*turnLeftAuto(500 * 3, 2, 500); // 90 degrees openClaw();*/ @@ -179,37 +173,33 @@ public void runStart() { } if (teamprop == 3) { + sleep(10000); telemetry.addData("", "found!"); telemetry.addData("teamprop", teamprop); telemetry.addData("distance", sensorRange.getDistance(DistanceUnit.INCH)); telemetry.update(); - forwardAuto(23, 5, 800); - strafeRightAuto(10, 1, 800); + forwardAuto(16, 1, 1000); + strafeRightAuto(5, 1, 1000); openClaw(0.5); - // park - strafeLeftAuto(5,1,800); - backwardAuto(26, 5, 800); + backwardAuto(10,1,1000); + strafeLeftAuto(20,1,1000); + backwardAuto(30,1,1000); } else { // zone 1 - strafeLeftAuto(10, 1, 800); - forwardAuto(25, 5, 800); - strafeRightAuto(5, 1, 800); - turnLeftAuto(500 * 3, 1, 800); - forwardAuto(2,1,800); + strafeLeftAuto(10, 1, 1000); + forwardAuto(17, 1, 1000); + strafeRightAuto(5, 1, 1000); + turnLeftAuto(500 * 3 / 2, 1, 1000); + forwardAuto(10,2,1000); openClaw(); - // park - turnRightAuto(500 * 3,1,800); - backwardAuto(26, 5, 800); + backwardAuto(5,1,1000); + turnRightAuto(500 * 3, 1, 1000); + backwardAuto(30,5,1000); } } // park - forwardAuto(3, 3,500); // - strafeLeftAuto(8,3,500); // - forwardAuto(15,3,500); // - strafeLeftAuto(17,3,500); // - forwardAuto(48,4,500); - strafeRightAuto(123,5,500); + strafeRightAuto(30,5,1000); } 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 55a2e04..d067598 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 @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.gamecode.autonomous; -import static org.firstinspires.ftc.teamcode.operations.inOut.driverControlled.RobotCentric.sensorRange; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.mapOtherThings; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.sensorRange; import static org.firstinspires.ftc.teamcode.operations.inputs.AprilTag.initAprilTag; import static org.firstinspires.ftc.teamcode.operations.inputs.TouchSensorButton.button; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.arm.Target_arm.arm; @@ -62,11 +63,7 @@ public void runInit() { initAprilTag(hardwareMap, "Webcam 1", telemetry); Encoders.clear(); - sensorRange = hardwareMap.get(DistanceSensor.class, "left_eye"); - - arm = hardwareMap.get(DcMotorEx.class, "arm"); - shaft = hardwareMap.get(DcMotor.class, "shaft"); - claw = hardwareMap.get(Servo.class, "claw"); + mapOtherThings(hardwareMap); TouchSensorButton.mapDigital(hardwareMap); // button } 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 0afb81b..96d2b7e 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 @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.gamecode.autonomous; -import static org.firstinspires.ftc.teamcode.operations.inOut.driverControlled.RobotCentric.sensorRange; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.mapOtherThings; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.sensorRange; import static org.firstinspires.ftc.teamcode.operations.inputs.AprilTag.initAprilTag; import static org.firstinspires.ftc.teamcode.operations.inputs.TouchSensorButton.button; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.arm.Target_arm.arm; @@ -67,11 +68,7 @@ public void runInit() { initAprilTag(hardwareMap, "Webcam 1", telemetry); Encoders.clear(); - sensorRange = hardwareMap.get(DistanceSensor.class, "left_eye"); - - arm = hardwareMap.get(DcMotorEx.class, "arm"); - shaft = hardwareMap.get(DcMotor.class, "shaft"); - claw = hardwareMap.get(Servo.class, "claw"); + mapOtherThings(hardwareMap); fl.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); fr.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); 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 fff0530..4d80af8 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 @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.gamecode.autonomous; -import static org.firstinspires.ftc.teamcode.operations.inOut.driverControlled.RobotCentric.sensorRange; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.mapOtherThings; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.sensorRange; import static org.firstinspires.ftc.teamcode.operations.inputs.AprilTag.initAprilTag; import static org.firstinspires.ftc.teamcode.operations.inputs.TouchSensorButton.button; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.arm.Target_arm.arm; @@ -63,11 +64,7 @@ public void runInit() { initAprilTag(hardwareMap, "Webcam 1", telemetry); Encoders.clear(); - sensorRange = hardwareMap.get(DistanceSensor.class, "left_eye"); - - arm = hardwareMap.get(DcMotorEx.class, "arm"); - shaft = hardwareMap.get(DcMotor.class, "shaft"); - claw = hardwareMap.get(Servo.class, "claw"); + mapOtherThings(hardwareMap); fl.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); fr.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); 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 c40afaf..829676f 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,8 @@ import static org.firstinspires.ftc.teamcode.operations.inOut.driverControlled.RobotCentric.initRobotCentric; 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.driverStation.DriverStation.output; +import static org.firstinspires.ftc.teamcode.operations.outputs.driverStation.DriverStation.outputMake; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.arm.Target_arm.arm; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.shaft.Target_shaft.shaft; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.ConfigureMotors.bl; @@ -42,15 +44,18 @@ public void runOpMode() { @Override public void runInit() { + outputMake(telemetry,"running init"); initFieldCentric(hardwareMap,telemetry); } @Override public void runInitLoop() { + outputMake(telemetry,"running init loop"); } @Override public void runStart() { + outputMake(telemetry,"running start"); //claw.setPosition(claw.getPosition()+2000); fl.setMode(DcMotor.RunMode.RUN_USING_ENCODER); fr.setMode(DcMotor.RunMode.RUN_USING_ENCODER); @@ -64,13 +69,14 @@ public void runStart() { @Override public void runLoop() { - + output(telemetry, "running loop"); runLoopFieldCentric(telemetry,gamepad1,gamepad2,speed); } @Override public void runStop() { + outputMake(telemetry,"running stop"); if (cameraConnected) { visionPortal.close(); } 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 ec9d35e..94530ef 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 @@ -4,6 +4,8 @@ import static org.firstinspires.ftc.teamcode.operations.inOut.driverControlled.RobotCentric.runLoopRobotCentric; 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.driverStation.DriverStation.output; +import static org.firstinspires.ftc.teamcode.operations.outputs.driverStation.DriverStation.outputMake; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.arm.Target_arm.arm; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.shaft.Target_shaft.shaft; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.ConfigureMotors.bl; @@ -41,15 +43,18 @@ public void runOpMode() { @Override public void runInit() { + outputMake(telemetry,"running init"); initRobotCentric(hardwareMap,telemetry); } @Override public void runInitLoop() { + outputMake(telemetry,"running init loop"); } @Override public void runStart() { + outputMake(telemetry,"running start"); //claw.setPosition(claw.getPosition()+2000); fl.setMode(DcMotor.RunMode.RUN_USING_ENCODER); fr.setMode(DcMotor.RunMode.RUN_USING_ENCODER); @@ -63,12 +68,14 @@ public void runStart() { @Override public void runLoop() { + output(telemetry, "running loop"); runLoopRobotCentric(telemetry,gamepad1,gamepad2,speed); } @Override public void runStop() { + outputMake(telemetry,"running stop"); if (cameraConnected) { visionPortal.close(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/tests/ClawTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/tests/ClawTest.java index a6a9c97..b4117d7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/tests/ClawTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/tests/ClawTest.java @@ -1,6 +1,6 @@ package org.firstinspires.ftc.teamcode.gamecode.tests; -import static org.firstinspires.ftc.teamcode.operations.inOut.driverControlled.RobotCentric.sensorRange; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.mapOtherThings; import static org.firstinspires.ftc.teamcode.operations.inputs.AprilTag.initAprilTag; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.arm.Target_arm.arm; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.shaft.Target_shaft.shaft; @@ -41,11 +41,7 @@ public void runInit() { initAprilTag(hardwareMap, "Webcam 1", telemetry); Encoders.clear(); - sensorRange = hardwareMap.get(DistanceSensor.class, "left_eye"); - - arm = hardwareMap.get(DcMotorEx.class, "arm"); - shaft = hardwareMap.get(DcMotor.class, "shaft"); - claw = hardwareMap.get(Servo.class, "claw"); + mapOtherThings(hardwareMap); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/tests/EncoderTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/tests/EncoderTest.java index 9aad7dd..3a5f5a5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/tests/EncoderTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/tests/EncoderTest.java @@ -2,7 +2,7 @@ import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.LogoFacingDirection.LEFT; import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.UsbFacingDirection.UP; -import static org.firstinspires.ftc.teamcode.operations.inOut.driverControlled.RobotCentric.sensorRange; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.mapOtherThings; import static org.firstinspires.ftc.teamcode.operations.inputs.AprilTag.initAprilTag; import static org.firstinspires.ftc.teamcode.operations.inputs.AprilTag.visionPortal; import static org.firstinspires.ftc.teamcode.operations.inputs.Imu.imuGet; @@ -12,6 +12,7 @@ import static org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.shaft.Target_shaft.shaft; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.ConfigureMotors.bl; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.ConfigureMotors.br; +import static org.firstinspires.ftc.teamcode.operations.inputs.Odometer.*; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.ConfigureMotors.fl; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.ConfigureMotors.forwardMotors; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.ConfigureMotors.fr; @@ -63,11 +64,7 @@ public void runInit() { imuGet(hardwareMap, DeviceNames.DEFAULT_IMU.hardwareMapName(), LEFT.name(), UP.name()); Encoders.clear(); - sensorRange = hardwareMap.get(DistanceSensor.class, "left_eye"); - - arm = hardwareMap.get(DcMotorEx.class, "arm"); - shaft = hardwareMap.get(DcMotor.class, "shaft"); - claw = hardwareMap.get(Servo.class, "claw"); + mapOtherThings(hardwareMap); } @Override @@ -81,6 +78,7 @@ public void runStart() { // example if I have 5,000 ticks in 3 and 2/16 of and inch then I do 5,000 / 3 2/16 = 1600 ticks in one inch // for measuring angles I will have to work with a specific angel though, such as 90 degrees, 180, or 360 rotateArm(-200,-2); + } // 150 degrees diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/inOut/Configs.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/inOut/Configs.java new file mode 100644 index 0000000..b300ef9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/inOut/Configs.java @@ -0,0 +1,27 @@ +package org.firstinspires.ftc.teamcode.operations.inOut; + +import static org.firstinspires.ftc.teamcode.gamecode.teleop.RobotCentric.wrist; +import static org.firstinspires.ftc.teamcode.operations.inputs.Odometer.oLeft; +import static org.firstinspires.ftc.teamcode.operations.inputs.Odometer.oRight; +import static org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.arm.Target_arm.arm; +import static org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.shaft.Target_shaft.shaft; +import static org.firstinspires.ftc.teamcode.operations.outputs.motors.servos.claw.Target_claw.claw; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DistanceSensor; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; + +public class Configs { + public static DistanceSensor sensorRange; + public static void mapOtherThings(HardwareMap hardwareMap) { + sensorRange = hardwareMap.get(DistanceSensor.class, "left_eye"); + wrist = hardwareMap.get(Servo.class, "wrist"); + arm = hardwareMap.get(DcMotorEx.class, "arm"); + shaft = hardwareMap.get(DcMotor.class, "shaft"); + claw = hardwareMap.get(Servo.class, "claw"); + oLeft = hardwareMap.get(DcMotor.class, "oLeft"); + oRight = hardwareMap.get(DcMotor.class, "oRight"); + } +} 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 53cd21d..abb88e2 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 @@ -3,13 +3,17 @@ import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.LogoFacingDirection.UP; import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.UsbFacingDirection.RIGHT; import static org.firstinspires.ftc.teamcode.gamecode.teleop.RobotCentric.wrist; -import static org.firstinspires.ftc.teamcode.operations.inOut.driverControlled.RobotCentric.sensorRange; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.mapOtherThings; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.sensorRange; import static org.firstinspires.ftc.teamcode.operations.inOut.driverControlled.gamepad2.General.runLoopGamepad2; import static org.firstinspires.ftc.teamcode.operations.inputs.AprilTag.initAprilTag; import static org.firstinspires.ftc.teamcode.operations.inputs.Imu.imuGet; import static org.firstinspires.ftc.teamcode.operations.inputs.Imu.imuReset; +import static org.firstinspires.ftc.teamcode.operations.inputs.Odometer.oLeft; +import static org.firstinspires.ftc.teamcode.operations.inputs.Odometer.oRight; import static org.firstinspires.ftc.teamcode.operations.inputs.Target_inputs.imu; import static org.firstinspires.ftc.teamcode.operations.inputs.TouchSensorButton.button; +import static org.firstinspires.ftc.teamcode.operations.outputs.driverStation.DriverStation.outputMake; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.arm.Target_arm.arm; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.shaft.Target_shaft.shaft; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.ConfigureMotors.bl; @@ -42,6 +46,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.operations.inputs.DeviceNames; import org.firstinspires.ftc.teamcode.operations.inputs.TouchSensorButton; +import org.firstinspires.ftc.teamcode.operations.outputs.driverStation.TelemetryShow; import org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.arm.ConfigureArm; import org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.shaft.ConfigureShaft; import org.firstinspires.ftc.teamcode.operations.outputs.motors.servos.claw.ConfigureClaw; @@ -57,41 +62,22 @@ public static void initFieldCentric(HardwareMap hardwareMap, Telemetry telemetry forwardMotors(false,true,false,true); imuGet(hardwareMap, DeviceNames.DEFAULT_IMU.hardwareMapName(), RIGHT.name(), UP.name()); initAprilTag(hardwareMap, DeviceNames.DEFAULT_CAMERA.hardwareMapName(), telemetry); - sensorRange = hardwareMap.get(DistanceSensor.class, "left_eye"); - wrist = hardwareMap.get(Servo.class, "wrist"); - arm = hardwareMap.get(DcMotorEx.class, "arm"); - shaft = hardwareMap.get(DcMotor.class, "shaft"); - claw = hardwareMap.get(Servo.class, "claw"); + mapOtherThings(hardwareMap); TouchSensorButton.mapDigital(hardwareMap); // button arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); } public static void runLoopFieldCentric(Telemetry telemetry, Gamepad gamepad1, Gamepad gamepad2, double speed){ - 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 fieldCentricMath(); // does the required math for Mecanum drive as well as getting imu for field centric - telemetry.addData("bot heading: ", botHeading); runLoopGamepad2(gamepad2); - - - - telemetry.addData("position FL ", fl.getCurrentPosition()); - telemetry.addData("position FR ", fr.getCurrentPosition()); - telemetry.addData("position BL ", bl.getCurrentPosition()); - telemetry.addData("position BR ", br.getCurrentPosition()); - telemetry.addData("degrees ", imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); - telemetry.addData("arm", arm.getCurrentPosition()); - telemetry.addData("shaft", shaft.getCurrentPosition()); - telemetry.update(); + TelemetryShow.allLoopMessages(telemetry); //clawUseWithGamepad(gamepad2); // using the claw (open/close) 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 44a9a2a..42242a7 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 @@ -3,11 +3,15 @@ import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.LogoFacingDirection.UP; import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.UsbFacingDirection.RIGHT; import static org.firstinspires.ftc.teamcode.gamecode.teleop.RobotCentric.wrist; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.mapOtherThings; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.sensorRange; import static org.firstinspires.ftc.teamcode.operations.inOut.driverControlled.gamepad2.General.*; import static org.firstinspires.ftc.teamcode.operations.inOut.driverControlled.gamepad2.General.runLoopGamepad2; import static org.firstinspires.ftc.teamcode.operations.inputs.AprilTag.initAprilTag; import static org.firstinspires.ftc.teamcode.operations.inputs.Imu.imuGet; import static org.firstinspires.ftc.teamcode.operations.inputs.Imu.imuReset; +import static org.firstinspires.ftc.teamcode.operations.inputs.Odometer.oLeft; +import static org.firstinspires.ftc.teamcode.operations.inputs.Odometer.oRight; import static org.firstinspires.ftc.teamcode.operations.inputs.Target_inputs.imu; import static org.firstinspires.ftc.teamcode.operations.inputs.TouchSensorButton.button; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.arm.Target_arm.arm; @@ -42,12 +46,12 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.operations.inputs.DeviceNames; import org.firstinspires.ftc.teamcode.operations.inputs.TouchSensorButton; +import org.firstinspires.ftc.teamcode.operations.outputs.driverStation.TelemetryShow; import org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.arm.ConfigureArm; import org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.shaft.ConfigureShaft; import org.firstinspires.ftc.teamcode.operations.outputs.motors.servos.claw.ConfigureClaw; public class RobotCentric { - public static DistanceSensor sensorRange; public static void initRobotCentric(HardwareMap hardwareMap, Telemetry telemetry) { mapMotors(hardwareMap, "fl","fr","bl","br"); @@ -58,22 +62,14 @@ public static void initRobotCentric(HardwareMap hardwareMap, Telemetry telemetry forwardMotors(false,true,false,true); imuGet(hardwareMap, DeviceNames.DEFAULT_IMU.hardwareMapName(), RIGHT.name(), UP.name()); initAprilTag(hardwareMap, DeviceNames.DEFAULT_CAMERA.hardwareMapName(), telemetry); - sensorRange = hardwareMap.get(DistanceSensor.class, "left_eye"); - wrist = hardwareMap.get(Servo.class, "wrist"); - arm = hardwareMap.get(DcMotorEx.class, "arm"); - shaft = hardwareMap.get(DcMotor.class, "shaft"); - claw = hardwareMap.get(Servo.class, "claw"); + mapOtherThings(hardwareMap); TouchSensorButton.mapDigital(hardwareMap); // button arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); } public static void runLoopRobotCentric(Telemetry telemetry, Gamepad gamepad1, Gamepad gamepad2, double speed){ - 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); @@ -81,20 +77,8 @@ public static void runLoopRobotCentric(Telemetry telemetry, Gamepad gamepad1, Ga imuReset(gamepad1.options); // resets imu case of accidents or incidences robotCentricMath(); // does the required math for Mecanum drive (Robot Centric) - telemetry.addData("bot heading: ", botHeading); runLoopGamepad2(gamepad2); - - - telemetry.addData("claw wrist ", wrist.getPosition()); - telemetry.addData("position FL ", fl.getCurrentPosition()); - telemetry.addData("position FR ", fr.getCurrentPosition()); - telemetry.addData("position BL ", bl.getCurrentPosition()); - telemetry.addData("position BR ", br.getCurrentPosition()); - telemetry.addData("degrees ", imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); - telemetry.addData("arm", arm.getCurrentPosition()); - telemetry.addData("shaft", shaft.getCurrentPosition()); - telemetry.addData("arm Power", arm.getPower()); - telemetry.update(); + TelemetryShow.allLoopMessages(telemetry); //clawUseWithGamepad(gamepad2); // using the claw (open/close) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/inputs/Odometer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/inputs/Odometer.java new file mode 100644 index 0000000..ed58d0b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/inputs/Odometer.java @@ -0,0 +1,10 @@ +package org.firstinspires.ftc.teamcode.operations.inputs; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Encoders; + +public class Odometer { + public static DcMotor oLeft; + public static DcMotor oRight; +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/driverStation/DriverStation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/driverStation/DriverStation.java index 7cb1a2a..a8bfac5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/driverStation/DriverStation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/driverStation/DriverStation.java @@ -18,11 +18,14 @@ public static void output(@NonNull Telemetry display, Object output) { public static void output(@NonNull Telemetry display, String caption, Object output) { // @NonNull : declares that the object is not at null value // display to telemetry with a caption or with a default caption if desired + // if string caption can be converted into boolean then use the boolean captionIf = Boolean.parseBoolean(caption); if (captionIf) { display.addData(defultCaption.nameValue(), output); } - display.addData(caption, output); + else { + display.addData(caption, output); + } display.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/driverStation/TelemetryShow.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/driverStation/TelemetryShow.java new file mode 100644 index 0000000..06666e4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/driverStation/TelemetryShow.java @@ -0,0 +1,39 @@ +package org.firstinspires.ftc.teamcode.operations.outputs.driverStation; + +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.sensorRange; +import static org.firstinspires.ftc.teamcode.operations.inputs.Odometer.oLeft; +import static org.firstinspires.ftc.teamcode.operations.inputs.Odometer.oRight; +import static org.firstinspires.ftc.teamcode.operations.inputs.Target_inputs.imu; +import static org.firstinspires.ftc.teamcode.operations.inputs.TouchSensorButton.button; +import static org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.arm.Target_arm.arm; +import static org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.shaft.Target_shaft.shaft; +import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.ConfigureMotors.bl; +import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.ConfigureMotors.br; +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.Mecanum.botHeading; +import static org.firstinspires.ftc.teamcode.operations.outputs.motors.servos.claw.Target_claw.claw; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +public class TelemetryShow { + public static void allLoopMessages(org.firstinspires.ftc.robotcore.external.Telemetry telemetry) { + + telemetry.addData("claw: ",claw.getPosition()); + telemetry.addData("Distance in Inches: ",sensorRange.getDistance(DistanceUnit.INCH)); + telemetry.addData("touch sensor", button.isPressed()); + telemetry.addData("bot heading: ", botHeading); + telemetry.addData("position FL ", fl.getCurrentPosition()); + telemetry.addData("position FR ", fr.getCurrentPosition()); + telemetry.addData("position BL ", bl.getCurrentPosition()); + telemetry.addData("position BR ", br.getCurrentPosition()); + telemetry.addData("degrees ", imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); + telemetry.addData("arm", arm.getCurrentPosition()); + telemetry.addData("shaft", shaft.getCurrentPosition()); + telemetry.addData("oLeft", oLeft.getCurrentPosition()); + telemetry.addData("oRight", oRight.getCurrentPosition()); + telemetry.update(); + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/motors/armLift/arm/autoArmOp/PixelOnBoard.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/motors/armLift/arm/autoArmOp/PixelOnBoard.java index 7d3b9b1..46dfcc8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/motors/armLift/arm/autoArmOp/PixelOnBoard.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/motors/armLift/arm/autoArmOp/PixelOnBoard.java @@ -1,6 +1,6 @@ package org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.arm.autoArmOp; -import static org.firstinspires.ftc.teamcode.operations.inOut.driverControlled.RobotCentric.sensorRange; +import static org.firstinspires.ftc.teamcode.operations.inOut.Configs.sensorRange; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.armLift.arm.armMovements.rotateArm; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.definingDriveMovements.CentricMovements.fieldCentric.forward; import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.definingDriveMovements.EachMotorSet.drive;