diff --git a/teamcode/BlueBoardSide.java b/teamcode/BlueBoardSide.java deleted file mode 100644 index f2eb8aa..0000000 --- a/teamcode/BlueBoardSide.java +++ /dev/null @@ -1,61 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -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.outputs.motors.drive.ConfigureMotors.br; -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.ConfigureMotors.forwardMotors; -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.ConfigureMotors.mapMotors; -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.DistanceMovements.forward; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.hardware.DcMotor; - -import org.firstinspires.ftc.teamcode.operations.TargetOperations; - -@Autonomous(name="Blue, Board Side") -public class BlueBoardSide extends TargetOperations { - - DcMotor arm; - - @Override - public void runOpMode() throws InterruptedException { - // LinearOpMode that calls a different form of OpMode: - runInit(); - while(opModeInInit()){runInitLoop();} - waitForStart(); - runStart(); - if (isStopRequested()){runStop();} // stop OpMode if the button is pressed - while (opModeIsActive()) {runLoop();} - visionPortal.close(); // close view for camera - } - - @Override - public void runInit() { - arm = hardwareMap.dcMotor.get("arm"); - mapMotors(hardwareMap, "fl","fr","bl","br"); - forwardMotors(true,false,true,false); - // ^ set motor directions - initAprilTag(hardwareMap, "Webcam 1"); - - } - - @Override - public void runInitLoop(){} - - @Override - public void runStart() { - forward(300, 1); - - } - @Override - public void runStop() { - stopAll(); - } - @Override - public void runLoop() { - - telemetry.addData("position", br.getCurrentPosition()); - telemetry.update(); - } -} - diff --git a/teamcode/BlueFarSide.java b/teamcode/BlueFarSide.java deleted file mode 100644 index d372a7e..0000000 --- a/teamcode/BlueFarSide.java +++ /dev/null @@ -1,45 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; - -import org.firstinspires.ftc.teamcode.operations.TargetOperations; - -@Autonomous(name="Blue, Far Side") -public class BlueFarSide extends TargetOperations { - - @Override - public void runOpMode() { - - runInit(); - while(opModeInInit()){runInitLoop();} - waitForStart(); - runStart(); - if (isStopRequested()){runStop();} - while (opModeIsActive()) {runLoop();} - } - - @Override - public void runInit() { - - } - - @Override - public void runInitLoop() { - - } - - @Override - public void runStart() { - - } - - @Override - public void runLoop() { - - } - - @Override - public void runStop() { - - } -} \ No newline at end of file diff --git a/teamcode/GameCode.java b/teamcode/GameCode.java deleted file mode 100644 index 34cffc7..0000000 --- a/teamcode/GameCode.java +++ /dev/null @@ -1,81 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import static org.firstinspires.ftc.teamcode.operations.inOut.Gamepad2.runBarMotor; -import static org.firstinspires.ftc.teamcode.operations.inOut.Gamepad2.runPixelMotor; -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.outputs.motors.drive.ConfigureMotors.forwardMotors; -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.ConfigureMotors.mapMotors; -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.HardcoreMovements.drive; -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.HardcoreMovements.driveStop; -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Mecanum.backLeftPower; -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Mecanum.backRightPower; -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Mecanum.dpadMovements; -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Mecanum.fieldCentricMath; -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Mecanum.frontLeftPower; -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Mecanum.frontRightPower; - -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.teamcode.operations.TargetOperations; - -@TeleOp -public class GameCode extends TargetOperations { - - double speed = 0.5; // speed used when using hardcore mode - - @Override - public void runOpMode() { - runInit(); - while(opModeInInit()){runInitLoop();} - waitForStart(); - runStart(); - if (isStopRequested()){runStop();} - while (opModeIsActive()) {runLoop();} - } - - @Override - public void runInit() { - mapMotors(hardwareMap, "fr","fl","br","bl"); - forwardMotors(false,true,false,true); - // ConfigureMotorBar.mapMotor("bar"); - // this is commented because it might show an error sense this motor is not configured and can't be because the motor is not yet connected. - // ConfigureMotorPixel.mapMotor("pixel"); - imuGet(hardwareMap, "imu", "LEFT", "UP"); - initAprilTag(hardwareMap, "Webcam 1"); - } - - @Override - public void runInitLoop() { - - } - - @Override - public void runStart() { - } - - @Override - public void runLoop() { - - dpadMovements(gamepad1, speed); // sets waypoints to the d_pads's positions - // calling the joystickMovements method is not needed here because the program calls that method if there is no input found within the dpadMovements method - - 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 - - runPixelMotor(gamepad2.a); - runBarMotor(gamepad2.y); - // just guesses to how the code might look, this part of the robot has not been built yet. - - drive(frontLeftPower,backLeftPower,frontRightPower,backRightPower); - // sets each motor to the speed given by the waypoints method - - } - - @Override - public void runStop() { - driveStop(); // stops all robot movements an slams on the breaks - - } -} \ No newline at end of file diff --git a/teamcode/RedBoardSide.java b/teamcode/RedBoardSide.java deleted file mode 100644 index 31f56bc..0000000 --- a/teamcode/RedBoardSide.java +++ /dev/null @@ -1,45 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; - -import org.firstinspires.ftc.teamcode.operations.TargetOperations; - -@Autonomous(name="Red, Board Side") -public class RedBoardSide extends TargetOperations { - @Override - public void runOpMode() { - - runInit(); - while(opModeInInit()){runInitLoop();} - waitForStart(); - runStart(); - if (isStopRequested()){runStop();} - while (opModeIsActive()) {runLoop();} - } - - @Override - public void runInit() { - - } - - @Override - public void runInitLoop() { - - } - - @Override - public void runStart() { - - } - - @Override - public void runLoop() { - - } - - @Override - public void runStop() { - - } - -} \ No newline at end of file diff --git a/teamcode/RedFarSide.java b/teamcode/RedFarSide.java deleted file mode 100644 index 59aeb68..0000000 --- a/teamcode/RedFarSide.java +++ /dev/null @@ -1,46 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; - -import org.firstinspires.ftc.teamcode.operations.TargetOperations; - -@Autonomous(name="Red, Far Side") -public class RedFarSide extends TargetOperations { - - @Override - public void runOpMode() { - - runInit(); - while(opModeInInit()){runInitLoop();} - waitForStart(); - runStart(); - if (isStopRequested()){runStop();} - while (opModeIsActive()) {runLoop();} - } - - @Override - public void runInit() { - - } - - @Override - public void runInitLoop() { - - } - - @Override - public void runStart() { - - } - - @Override - public void runLoop() { - - } - - @Override - public void runStop() { - - } - -} \ No newline at end of file diff --git a/teamcode/operations/TargetOperations.java b/teamcode/operations/TargetOperations.java deleted file mode 100644 index aa5d7d4..0000000 --- a/teamcode/operations/TargetOperations.java +++ /dev/null @@ -1,14 +0,0 @@ -package org.firstinspires.ftc.teamcode.operations; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -public abstract class TargetOperations extends LinearOpMode implements TargetSetup { - - public static boolean pixelMotorConfig = false; - public static boolean barMotorConfig = false; - - public void stopAll() { - requestOpModeStop(); - } - -} \ No newline at end of file diff --git a/teamcode/operations/TargetSetup.java b/teamcode/operations/TargetSetup.java deleted file mode 100644 index 4ac5484..0000000 --- a/teamcode/operations/TargetSetup.java +++ /dev/null @@ -1,15 +0,0 @@ -// public is not needed, because Interface has public by default -package org.firstinspires.ftc.teamcode.operations; - - interface TargetSetup { - /* - this interface was made so that - both LinearOpMode and OpMode can be - ran within the same class - */ - void runInit(); - void runInitLoop(); - void runStart(); - void runLoop(); - void runStop(); -} diff --git a/teamcode/operations/inOut/Gamepad2.java b/teamcode/operations/inOut/Gamepad2.java deleted file mode 100644 index f5267af..0000000 --- a/teamcode/operations/inOut/Gamepad2.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.firstinspires.ftc.teamcode.operations.inOut; - -import static org.firstinspires.ftc.teamcode.operations.TargetOperations.barMotorConfig; -import static org.firstinspires.ftc.teamcode.operations.TargetOperations.pixelMotorConfig; -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.pixelMotor.ConfigureMotorPixel.pixelMotor; - -import org.firstinspires.ftc.teamcode.operations.outputs.motors.barMotor.BarMotorMovements; - -public class Gamepad2 { - - public static void runPixelMotor(boolean button) { - if (pixelMotorConfig) { - // only do the code if pixelMotor is configured - if (button) { - pixelMotor.setPower(1); - } - else { - pixelMotor.setPower(0); - } - } - } - - - public static void runBarMotor(boolean button) { - if (barMotorConfig) { - if (button) { - BarMotorMovements.rotate(50,1); - } - } - } -} \ No newline at end of file diff --git a/teamcode/operations/inputs/AprilTag.java b/teamcode/operations/inputs/AprilTag.java deleted file mode 100644 index cb6e982..0000000 --- a/teamcode/operations/inputs/AprilTag.java +++ /dev/null @@ -1,111 +0,0 @@ -package org.firstinspires.ftc.teamcode.operations.inputs; - -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.HardcoreMovements.*; - -import com.qualcomm.robotcore.hardware.Gamepad; -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.vision.VisionPortal; -import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; -import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; - -import java.util.ArrayList; -import java.util.List; - -public class AprilTag { - - public static VisionPortal visionPortal; - - public static double firstTagId, tagId; - public static AprilTagDetection firstTagFound; - public static double tagsFound; - public static String tagName; - - public static List tagCenter, tagPosition; - - public static AprilTagProcessor aprilTag; - - public static double alignTagControlled = 1; - - public static boolean tagFound = false; - - - public static void initAprilTag(HardwareMap mapHardware, String CameraName) { - - // Create the AprilTag processor - aprilTag = AprilTagProcessor.easyCreateWithDefaults(); - - visionPortal = VisionPortal.easyCreateWithDefaults( - mapHardware.get(WebcamName.class, CameraName), aprilTag); - } - - public static void setAprilTagVariables() { - // add AprilTag found information to set variables - - List currentDetections = aprilTag.getDetections(); - firstTagFound = currentDetections.get(0); - firstTagId = firstTagFound.id; - tagsFound = currentDetections.size(); - - // Step through the list of detections and display info for each one. - for (AprilTagDetection detection : currentDetections) { - if (detection.metadata != null) { - - tagName = detection.metadata.name; - tagPosition = new ArrayList(); - tagPosition.add(detection.ftcPose.x); - tagPosition.add(detection.ftcPose.y); - tagPosition.add(detection.ftcPose.z); - tagPosition.add(detection.ftcPose.pitch); - tagPosition.add(detection.ftcPose.roll); - tagPosition.add(detection.ftcPose.yaw); - tagPosition.add(detection.ftcPose.range); - tagPosition.add(detection.ftcPose.bearing); - tagPosition.add(detection.ftcPose.elevation); - - } - else { - - tagName = "unknown"; - } - - tagId = detection.id; - - tagCenter = new ArrayList(); - tagCenter.add(detection.center.x); - tagCenter.add(detection.center.y); - } - } - - public static void searchForTagByControlled(Gamepad gamepad1, double speed) { - - if (tagId != alignTagControlled) { - goDirection(gamepad1.left_bumper, gamepad1.right_bumper, speed); // if the desired AprilTag is not found then go the desired direction until the april tag is found - } - - else if (tagId == alignTagControlled) { - tagFound = true; - // express output - } - - } - - public static void setAprilTagToFind (Gamepad gamepad1) { - // by default set to 1; - - if (gamepad1.x) { - alignTagControlled = 1; - // go to april tag 1 - } - - else if (gamepad1.y) { - alignTagControlled = 2; - } - - else if (gamepad1.b) { - alignTagControlled = 3; - } - } - -} diff --git a/teamcode/operations/inputs/Imu.java b/teamcode/operations/inputs/Imu.java deleted file mode 100644 index 1da5225..0000000 --- a/teamcode/operations/inputs/Imu.java +++ /dev/null @@ -1,86 +0,0 @@ -package org.firstinspires.ftc.teamcode.operations.inputs; - -import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD; -import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.LogoFacingDirection.DOWN; -import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.LogoFacingDirection.FORWARD; -import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.LogoFacingDirection.LEFT; -import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.LogoFacingDirection.RIGHT; -import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.LogoFacingDirection.UP; - -import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; -import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.IMU; - - -public class Imu { - - public static IMU imu; - - public static void imuGet (HardwareMap mapHardware, String hardwareMapImuName, String logoFacing, String usbFacing) { - // get imu from hardware map and set the directions to the directions of the robot's brain - imu = mapHardware.get(IMU .class, hardwareMapImuName); - // Retrieve the IMU from the hardware map - IMU.Parameters parameters = null; - // Adjust the orientation parameters to match your robot - if (logoFacing == UP.name() && usbFacing == FORWARD.name()) { - parameters = new IMU.Parameters(new RevHubOrientationOnRobot( - UP, - RevHubOrientationOnRobot.UsbFacingDirection.FORWARD)); - } - - else if (logoFacing == LEFT.name() && usbFacing == UP.name()) { - parameters = new IMU.Parameters(new RevHubOrientationOnRobot( - RevHubOrientationOnRobot.LogoFacingDirection.LEFT, - RevHubOrientationOnRobot.UsbFacingDirection.UP)); - } - - else if (logoFacing == RIGHT.name() && usbFacing == UP.name()) { - parameters = new IMU.Parameters(new RevHubOrientationOnRobot( - RevHubOrientationOnRobot.LogoFacingDirection.RIGHT, - RevHubOrientationOnRobot.UsbFacingDirection.UP)); - } - - else if (logoFacing == FORWARD.name() && usbFacing == UP.name()) { - parameters = new IMU.Parameters(new RevHubOrientationOnRobot( - RevHubOrientationOnRobot.LogoFacingDirection.FORWARD, - RevHubOrientationOnRobot.UsbFacingDirection.UP)); - } - - else if (logoFacing == BACKWARD.name() && usbFacing == UP.name()) { - parameters = new IMU.Parameters(new RevHubOrientationOnRobot( - RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD, - RevHubOrientationOnRobot.UsbFacingDirection.UP)); - } - - else if (logoFacing == FORWARD.name() && usbFacing == RIGHT.name()) { - parameters = new IMU.Parameters(new RevHubOrientationOnRobot( - RevHubOrientationOnRobot.LogoFacingDirection.FORWARD, - RevHubOrientationOnRobot.UsbFacingDirection.RIGHT)); - } - - else if (logoFacing == UP.name() && usbFacing == BACKWARD.name()) { - parameters = new IMU.Parameters(new RevHubOrientationOnRobot( - UP, - RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD)); - } - - else if (logoFacing == DOWN.name() && usbFacing == FORWARD.name()) { - parameters = new IMU.Parameters(new RevHubOrientationOnRobot( - RevHubOrientationOnRobot.LogoFacingDirection.DOWN, - RevHubOrientationOnRobot.UsbFacingDirection.FORWARD)); - } - - else if (logoFacing == FORWARD.name() && usbFacing == LEFT.name()) { - parameters = new IMU.Parameters(new RevHubOrientationOnRobot( - RevHubOrientationOnRobot.LogoFacingDirection.FORWARD, - RevHubOrientationOnRobot.UsbFacingDirection.LEFT)); - } - imu.initialize(parameters); - } - - public static void imuReset(boolean button) { - if (button) { - imu.resetYaw(); - } - } -} diff --git a/teamcode/operations/outputs/DriverStation.java b/teamcode/operations/outputs/DriverStation.java deleted file mode 100644 index 345424f..0000000 --- a/teamcode/operations/outputs/DriverStation.java +++ /dev/null @@ -1,10 +0,0 @@ -package org.firstinspires.ftc.teamcode.operations.outputs; - -import org.firstinspires.ftc.robotcore.external.Telemetry; - -public class DriverStation { - public static void output(Telemetry display, Object output) { - // display to telemetry without a caption - display.addData("", output); - } -} diff --git a/teamcode/operations/outputs/motors/barMotor/BarMotorMovements.java b/teamcode/operations/outputs/motors/barMotor/BarMotorMovements.java deleted file mode 100644 index 709d381..0000000 --- a/teamcode/operations/outputs/motors/barMotor/BarMotorMovements.java +++ /dev/null @@ -1,20 +0,0 @@ -package org.firstinspires.ftc.teamcode.operations.outputs.motors.barMotor; - -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.barMotor.ConfigureMotorBar.barMotor; - -import com.qualcomm.robotcore.hardware.DcMotor; - -public class BarMotorMovements { - - public void motorEncoder () { - barMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - barMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - barMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - } - - public static void rotate(int rotations, int speed) { - barMotor.setTargetPosition(rotations); - barMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); - barMotor.setPower(speed); - } -} diff --git a/teamcode/operations/outputs/motors/barMotor/ConfigureMotorBar.java b/teamcode/operations/outputs/motors/barMotor/ConfigureMotorBar.java deleted file mode 100644 index f7defe3..0000000 --- a/teamcode/operations/outputs/motors/barMotor/ConfigureMotorBar.java +++ /dev/null @@ -1,16 +0,0 @@ -package org.firstinspires.ftc.teamcode.operations.outputs.motors.barMotor; - -import static org.firstinspires.ftc.teamcode.operations.TargetOperations.barMotorConfig; - -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.HardwareMap; - -public class ConfigureMotorBar { - - public static DcMotor barMotor; - public static void mapMotor (HardwareMap hardwareMap, String motorName) { - barMotor = hardwareMap.dcMotor.get(motorName); - barMotorConfig = true; - } - -} diff --git a/teamcode/operations/outputs/motors/drive/ConfigureMotors.java b/teamcode/operations/outputs/motors/drive/ConfigureMotors.java deleted file mode 100644 index b010bd9..0000000 --- a/teamcode/operations/outputs/motors/drive/ConfigureMotors.java +++ /dev/null @@ -1,66 +0,0 @@ -package org.firstinspires.ftc.teamcode.operations.outputs.motors.drive; - -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.HardwareMap; - -public class ConfigureMotors { - - public static DcMotorEx fr; - public static DcMotorEx fl; - public static DcMotorEx br; - public static DcMotorEx bl; - - public static double y; - public static double x; - public static double rx; - - // Get an instance of HardwareMap - - - public static void mapMotors (HardwareMap mapHardware, String frontLeftMotor, String frontRightMotor, String backLeftMotor, String backRightMotor){ - fl = (DcMotorEx) mapHardware.dcMotor.get(frontLeftMotor); - fr = (DcMotorEx) mapHardware.dcMotor.get(frontRightMotor); - bl = (DcMotorEx) mapHardware.dcMotor.get(backLeftMotor); - br = (DcMotorEx) mapHardware.dcMotor.get(backRightMotor); - - - } - - - public static void forwardMotors (boolean frontLeftMotor, boolean frontRightMotor, boolean backLeftMotor, boolean backRightMotor) { - - if (frontRightMotor) { - fr.setDirection(DcMotorSimple.Direction.FORWARD); - } - - else { - fr.setDirection(DcMotorSimple.Direction.REVERSE); - } - - if (frontLeftMotor) { - fl.setDirection(DcMotorSimple.Direction.FORWARD); - } - - else { - fl.setDirection(DcMotorSimple.Direction.REVERSE); - } - - if (backRightMotor) { - br.setDirection(DcMotorSimple.Direction.FORWARD); - } - - else { - br.setDirection(DcMotorSimple.Direction.REVERSE); - } - - if (backLeftMotor) { - bl.setDirection(DcMotorSimple.Direction.FORWARD); - } - - else { - bl.setDirection(DcMotorSimple.Direction.REVERSE); - } - } - -} diff --git a/teamcode/operations/outputs/motors/drive/DistanceMovements.java b/teamcode/operations/outputs/motors/drive/DistanceMovements.java deleted file mode 100644 index 29fc99c..0000000 --- a/teamcode/operations/outputs/motors/drive/DistanceMovements.java +++ /dev/null @@ -1,43 +0,0 @@ -package org.firstinspires.ftc.teamcode.operations.outputs.motors.drive; - -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.HardcoreMovements.*; - -public class DistanceMovements { - static double ticksPerMovements = 20; // using motors 20:1 - static double ticksPerInchForward = 5; - // ticksPer141Inches = ???; - static double ticksPerInchRight = 5; - static double ticksPerDegree = 5; - public static void forward(double inches, double speed) { - int tp = (int) (inches*(ticksPerInchForward*ticksPerMovements)); - Encoders.use(); - Encoders.clear(); - Encoders.target(tp/2, tp/2, tp, tp); - Encoders.go(); - drive(speed/2,speed/2,speed,speed); - - - } - - public static void right(double inches, double speed) { - int tp = (int) (inches*(ticksPerInchRight*ticksPerMovements)); - Encoders.use(); - Encoders.clear(); - Encoders.target(tp, tp, tp, tp); - Encoders.go(); - drive(speed,speed,speed,speed); - - - } - - public static void turnRight(double degrees, double speed) { - int tp = (int) (degrees*(ticksPerDegree*ticksPerMovements)); - Encoders.use(); - Encoders.clear(); - Encoders.target(tp, tp, tp, tp); - Encoders.go(); - drive(speed,speed,speed,speed); - - - } -} diff --git a/teamcode/operations/outputs/motors/drive/Encoders.java b/teamcode/operations/outputs/motors/drive/Encoders.java deleted file mode 100644 index 5714719..0000000 --- a/teamcode/operations/outputs/motors/drive/Encoders.java +++ /dev/null @@ -1,33 +0,0 @@ -package org.firstinspires.ftc.teamcode.operations.outputs.motors.drive; -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.ConfigureMotors.*; - -import com.qualcomm.robotcore.hardware.DcMotor; - -public class Encoders { - public static void target(int frontLeft, int frontRight, int backLeft, int backRight){ - fl.setTargetPosition(frontLeft); - fr.setTargetPosition(frontRight); - bl.setTargetPosition(backLeft); - br.setTargetPosition(backRight); - } - public static void go() { - fl.setMode(DcMotor.RunMode.RUN_TO_POSITION); - fr.setMode(DcMotor.RunMode.RUN_TO_POSITION); - bl.setMode(DcMotor.RunMode.RUN_TO_POSITION); - br.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } - - public static void clear() { - fl.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - fr.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - bl.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - br.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - } - - public static void use() { - fl.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - fr.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - bl.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - br.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - } -} diff --git a/teamcode/operations/outputs/motors/drive/HardcoreMovements.java b/teamcode/operations/outputs/motors/drive/HardcoreMovements.java deleted file mode 100644 index 97ca1b5..0000000 --- a/teamcode/operations/outputs/motors/drive/HardcoreMovements.java +++ /dev/null @@ -1,79 +0,0 @@ -package org.firstinspires.ftc.teamcode.operations.outputs.motors.drive; - -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.HardcoreMovements.fieldCentric.left; -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.HardcoreMovements.fieldCentric.right; -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Mecanum.backLeftPower; -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Mecanum.backRightPower; -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Mecanum.fieldCentricMath; -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Mecanum.frontLeftPower; -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Mecanum.frontRightPower; -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.Mecanum.waypoints; - -import com.qualcomm.robotcore.hardware.DcMotor; - -public class HardcoreMovements { - - public static void drive(double frontLeftSpeed, double frontRightSpeed, double backLeftSpeed, double backRightSpeed) { - fr.setPower(frontLeftSpeed); - fl.setPower(frontRightSpeed); - br.setPower(backLeftSpeed); - bl.setPower(backRightSpeed); - - } - - public static void driveStop() { - - drive(0,0,0,0); - fl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - fr.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - bl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - br.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - } - // ^ set all motors to 0, this stops all movements - - public static void goDirection(boolean left, boolean right, double speed) { - if (left) { - left(speed); - } - - else if (right) { - right(speed); - } - } - - public static class fieldCentric { - // these methods must be ran in the loop() of this project - public static void left(double speed) { - waypoints(0, -speed, 0); - fieldCentricMath(); - drive(frontLeftPower,backLeftPower,frontRightPower,backRightPower); - } - - public static void right(double speed) { - waypoints(0, speed, 0); - fieldCentricMath(); - drive(frontLeftPower,backLeftPower,frontRightPower,backRightPower); - } - public static void forward(double speed) { - waypoints(speed, 0, 0); - fieldCentricMath(); - drive(frontLeftPower,backLeftPower,frontRightPower,backRightPower); - } - - public static void backward(double speed) { - waypoints(-speed, 0, 0); - fieldCentricMath(); - drive(frontLeftPower,backLeftPower,frontRightPower,backRightPower); - } - - - } - -} - - - diff --git a/teamcode/operations/outputs/motors/drive/Mecanum.java b/teamcode/operations/outputs/motors/drive/Mecanum.java deleted file mode 100644 index fe9cbc9..0000000 --- a/teamcode/operations/outputs/motors/drive/Mecanum.java +++ /dev/null @@ -1,78 +0,0 @@ -package org.firstinspires.ftc.teamcode.operations.outputs.motors.drive; - -import static org.firstinspires.ftc.teamcode.operations.inputs.Imu.imu; -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.HardcoreMovements.*; - -import com.qualcomm.robotcore.hardware.Gamepad; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; - -public class Mecanum { - - static double botHeading; - static double rotX; - static double rotY; - static double denominator; - public static double frontLeftPower; - public static double frontRightPower; - public static double backLeftPower; - public static double backRightPower; - public static double x,y,rx; - public static String direction = "left"; - - public static void fieldCentricMath() { - botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); - - // Rotate the movement direction counter to the robot's rotation - rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading); - rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading); - - rotX = rotX * 1.1; // Counteract imperfect strafing - - // Denominator is the largest motor power (absolute value) or 1 - // This ensures all the powers maintain the same ratio, - // but only if at least one is out of the range [-1, 1] - denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1); - frontLeftPower = (rotY + rotX + rx) / denominator; - backLeftPower = (rotY - rotX + rx) / denominator; - frontRightPower = (rotY - rotX - rx) / denominator; - backRightPower = (rotY + rotX - rx) / denominator; - } - - public static void waypoints(double Y, double X, double RX) { - // adds waypoint values to variables for motor movements and speeds - y = Y; - x = X; - rx = RX; - } - - 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. - // it is replacing the joystick value with its own. - if (gamepad1.dpad_left) { - fieldCentric.left(speed); - } - - else if (gamepad1.dpad_right) { - fieldCentric.right(speed); - } - - else if (gamepad1.dpad_up) { - fieldCentric.forward(speed); - } - - else if (gamepad1.dpad_down) { - fieldCentric.backward(speed); - } - - else { - joyStickMovements(gamepad1); // this way if no input is read from the d_pad then it looks for input given by the joysticks - // if there is no input given by either then the robot is at rest. - } - } - - public static void joyStickMovements(Gamepad gamepad1) { - waypoints(-gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x); // Remember, Y stick value is reversed - } -} diff --git a/teamcode/operations/outputs/motors/pixelMotor/ConfigureMotorPixel.java b/teamcode/operations/outputs/motors/pixelMotor/ConfigureMotorPixel.java deleted file mode 100644 index 099c454..0000000 --- a/teamcode/operations/outputs/motors/pixelMotor/ConfigureMotorPixel.java +++ /dev/null @@ -1,17 +0,0 @@ -package org.firstinspires.ftc.teamcode.operations.outputs.motors.pixelMotor; - -import static org.firstinspires.ftc.teamcode.operations.TargetOperations.pixelMotorConfig; - -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.HardwareMap; - -public class ConfigureMotorPixel { - - public static DcMotor pixelMotor; - - public static void mapMotor (HardwareMap hardwareMap, String motorName) { - pixelMotor = hardwareMap.dcMotor.get(motorName); - pixelMotorConfig = true; - } - -} diff --git a/teamcode/operations/outputs/motors/pixelMotor/PixelMotorMovements.java b/teamcode/operations/outputs/motors/pixelMotor/PixelMotorMovements.java deleted file mode 100644 index d1704f7..0000000 --- a/teamcode/operations/outputs/motors/pixelMotor/PixelMotorMovements.java +++ /dev/null @@ -1,21 +0,0 @@ -package org.firstinspires.ftc.teamcode.operations.outputs.motors.pixelMotor; - -import static org.firstinspires.ftc.teamcode.operations.outputs.motors.pixelMotor.ConfigureMotorPixel.pixelMotor; - -import com.qualcomm.robotcore.hardware.DcMotor; - -public class PixelMotorMovements { - - public static void motorEncoder () { - pixelMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - pixelMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - pixelMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - } - - public static void rotate (int rotations, int speed) { - pixelMotor.setTargetPosition(rotations); - pixelMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); - pixelMotor.setPower(speed); - } - -} diff --git a/teamcode/operations/readme.md b/teamcode/operations/readme.md deleted file mode 100644 index 7ccec1b..0000000 --- a/teamcode/operations/readme.md +++ /dev/null @@ -1,55 +0,0 @@ -## HardwareMap -Any class that does not extend LinearOpMode or OpMode must include HardwareMap inside the method, -this way it can be set where the OpMode/LinearOpMode is extended. - -### here is an example: -
-public static void mapMotors (HardwareMap mapHardware, String frontLeftMotor, String frontRightMotor, String backLeftMotor, String backRightMotor){
-        fl = mapHardware.dcMotor.get(frontLeftMotor);
-        fr = mapHardware.dcMotor.get(frontRightMotor);
-        bl = mapHardware.dcMotor.get(backLeftMotor);
-        br = mapHardware.dcMotor.get(backRightMotor);
-    }
-
-This is how hardwareMap can be used, by renaming in within the methods and calling it from the code that extends a class that allows the hardwareMap to be used. - -### here is an example of the method being called: -
-package org.firstinspires.ftc.teamcode;
-
-import static org.firstinspires.ftc.teamcode.operations.outputs.motors.drive.configureMotors.mapMotors;
-
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-
-public class example extends OpMode {
-    @Override
-    public void init() {
-        mapMotors(hardwareMap, "fr","fl","br","bl");
-    }
-
-    @Override
-    public void loop() {
-
-    }
-}
-
-In green is the mapMotors being called out off the example class and being ran inside the init method. This method will send hardwareMap to the mapMotors method and map the motors "fr", "fl", "br", and "bl". - -## Telemetry -The same notation must be acquired for telemetry. If I wanted to call telemetry from a class that has no extensions that connect to the OpMode class than I need to call a value in my meathod that gets the telementry for me, using the Telementry module. - -### here is an example of the method: -
-public static void output(Telemetry display, Object output) {
-        // display to telemetry without a caption
-        display.addData("", output);
-        display.update();
-    }
-
-Again this works just like the HardwareMap module, but with telemetry. In order for telemetry to operate correctly outside of an extention of OpMode it must look for an input of telementry on the other side. -### here is an example of the output method being called: -
-output(telemetry, "Robot Stopped.");
-
-once the output method is called it sends a message to the driver station that the robot has been stopped. -##### There are also other keywords that might need to be called such as Gamepad if your using gamepad1 or gamepad2 outside of an extension of OpMode. \ No newline at end of file diff --git a/teamcode/readme.md b/teamcode/readme.md deleted file mode 100644 index aa4d138..0000000 --- a/teamcode/readme.md +++ /dev/null @@ -1 +0,0 @@ -# 2023-2024 Season team code