diff --git a/FtcRobotController/teamcode/Teleop.java b/FtcRobotController/teamcode/Teleop.java new file mode 100644 index 000000000000..fa9f96a1f1bc --- /dev/null +++ b/FtcRobotController/teamcode/Teleop.java @@ -0,0 +1,89 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; + +import java.util.List; +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="limetestdefault", group="Linear OpMode") + +public class Teleop extends LinearOpMode { + + private Limelight3A limelight; + + @Override + public void runOpMode() throws InterruptedException + { + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + + telemetry.setMsTransmissionInterval(11); + limelight.setPollRateHz(100); + limelight.pipelineSwitch(0); + + + waitForStart(); + + /* + * Starts polling for data. + */ + limelight.start(); + + + telemetry.addData("ll3a status", limelight.isConnected()); + telemetry.update(); + + while (opModeIsActive()) { + LLResult result = limelight.getLatestResult(); + if (result != null) { + if (result.isValid()) { + Pose3D botpose = result.getBotpose(); + telemetry.addData("tx", result.getTx()); + telemetry.addData("ty", result.getTy()); + telemetry.addData("Botpose", botpose.toString()); + } + } + + } + } +} \ No newline at end of file diff --git a/FtcRobotController/teamcode/TestCar.java b/FtcRobotController/teamcode/TestCar.java new file mode 100644 index 000000000000..541ef2ed48c6 --- /dev/null +++ b/FtcRobotController/teamcode/TestCar.java @@ -0,0 +1,163 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="Test Car", group="Linear OpMode") +//@Disabled +public class TestCar extends LinearOpMode { + + // Declare OpMode members for each of the 4 motors. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor leftFrontDrive = null; + // private DcMotor leftBackDrive = null; + private DcMotor rightFrontDrive = null; + private DcMotor rightBackDrive = null; + + private DcMotor leftBackDrive = null; + + private DcMotor Arm1 = null; + ////private DcMotor Arm2 = null; + + @Override + public void runOpMode() { + + // Initialize the hardware variables. Note that the strings used here must correspond + // to the names assigned during the robot configuration step on the DS or RC devices. + leftFrontDrive = hardwareMap.get(DcMotor.class, "left_front_drive"); + leftBackDrive = hardwareMap.get(DcMotor.class, "left_back_drive"); + rightFrontDrive = hardwareMap.get(DcMotor.class, "right_front_drive"); + rightBackDrive = hardwareMap.get(DcMotor.class, "right_back_drive"); + Arm1 = hardwareMap.get(DcMotor.class, "ArmMotor1"); + ////Arm2 = hardwareMap.get(DcMotor.class, "ArmMotor2"); + + // ######################################################################################## + // !!! IMPORTANT Drive Information. Test your motor directions. !!!!! + // ######################################################################################## + // Most robots need the motors on one side to be reversed to drive forward. + // The motor reversals shown here are for a "direct drive" robot (the wheels turn the same direction as the motor shaft) + // If your robot has additional gear reductions or uses a right-angled drive, it's important to ensure + // that your motors are turning in the correct direction. So, start out with the reversals here, BUT + // when you first test your robot, push the left joystick forward and observe the direction the wheels turn. + // Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward + // Keep testing until ALL the wheels move the robot forward when you push the left joystick forward. + leftFrontDrive.setDirection(DcMotor.Direction.REVERSE); + leftBackDrive.setDirection(DcMotor.Direction.FORWARD); + rightFrontDrive.setDirection(DcMotor.Direction.REVERSE); + rightBackDrive.setDirection(DcMotor.Direction.FORWARD); + + + Arm1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + ////Arm2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + + // Wait for the game to start (driver presses START) + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + waitForStart(); + runtime.reset(); + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + double max; + + // POV Mode uses left joystick to go forward & strafe, and right joystick to rotate. + double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value + double lateral = gamepad1.left_stick_x; + double yaw = gamepad1.right_stick_x; + + // Combine the joystick requests for each axis-motion to determine each wheel's power. + // Set up a variable for each drive wheel to save the power level for telemetry. + double leftFrontPower = axial + lateral + yaw; + double rightFrontPower = axial - lateral - yaw; + double leftBackPower = axial - lateral + yaw; + double rightBackPower = axial + lateral - yaw; + + // Normalize the values so no wheel power exceeds 100% + // This ensures that the robot maintains the desired motion. + max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)); + max = Math.max(max, Math.abs(leftBackPower)); + max = Math.max(max, Math.abs(rightBackPower)); + + if (max > 1.0) { + leftFrontPower /= max; + rightFrontPower /= max; + leftBackPower /= max; + rightBackPower /= max; + } + + /* + to find the power: + find difference in distance between tarting point and where it wants to go + divide the current distance away over the total distance + set this as the power + */ + + if(gamepad2.dpad_up){ + Arm1.setPower(1); + } + else if (gamepad2.dpad_down){ + Arm1.setPower(1); + + }else{ + Arm1.setPower(0); + } + + // Send calculated power to wheels + leftFrontDrive.setPower(leftFrontPower); + rightFrontDrive.setPower(rightFrontPower); + leftBackDrive.setPower(leftBackPower); + rightBackDrive.setPower(rightBackPower); + + + //telemetry.addData("difference Test", differenceInArms); + //telemetry.addData("targetPosition Test", targetPosition); + // telemetry.addData("targetPositionArm2 Test", targetPositionArm2); + // telemetry.addData("zeroPositionArm1 Test", zeroPositionArm1); + // telemetry.addData("zeroPositionArm2 Test", zeroPositionArm2); + + // Show the elapsed game time and wheel power. +// telemetry.addData("Arm1 Test", Arm1.getCurrentPosition()); +// telemetry.addData("Arm2 Test", Arm2.getCurrentPosition()); + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); + telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); + telemetry.update(); + } + } +} diff --git a/FtcRobotController/teamcode/TestCar2.java b/FtcRobotController/teamcode/TestCar2.java new file mode 100644 index 000000000000..b3085cb16064 --- /dev/null +++ b/FtcRobotController/teamcode/TestCar2.java @@ -0,0 +1,222 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; + +import java.util.List; +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="Test Car2", group="Linear OpMode") +//@Disabled +public class TestCar2 extends LinearOpMode { + + // Declare OpMode members for each of the 4 motors. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor leftFrontDrive = null; + // private DcMotor leftBackDrive = null; + private DcMotor rightFrontDrive = null; + private DcMotor rightBackDrive = null; + + private DcMotor leftBackDrive = null; + private DcMotor intake = null; + private DcMotor kicker = null; + private Limelight3A limelight; + private Servo servo; + + @Override + public void runOpMode() { + + // Initialize the hardware variables. Note that the strings used here must correspond + // to the names assigned during the robot configuration step on the DS or RC devices. + leftFrontDrive = hardwareMap.get(DcMotor.class, "left_front_drive"); + leftBackDrive = hardwareMap.get(DcMotor.class, "left_back_drive"); + rightFrontDrive = hardwareMap.get(DcMotor.class, "right_front_drive"); + rightBackDrive = hardwareMap.get(DcMotor.class, "right_back_drive"); + intake = hardwareMap.get(DcMotor.class, "intake"); + kicker = hardwareMap.get(DcMotor.class, "kicker"); + ////Arm2 = hardwareMap.get(DcMotor.class, "ArmMotor2"); + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + servo = hardwareMap.get(Servo.class, "servo1"); + // ######################################################################################## + // !!! IMPORTANT Drive Information. Test your motor directions. !!!!! + // ######################################################################################## + // Most robots need the motors on one side to be reversed to drive forward. + // The motor reversals shown here are for a "direct drive" robot (the wheels turn the same direction as the motor shaft) + // If your robot has additional gear reductions or uses a right-angled drive, it's important to ensure + // that your motors are turning in the correct direction. So, start out with the reversals here, BUT + // when you first test your robot, push the left joystick forward and observe the direction the wheels turn. + // Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward + // Keep testing until ALL the wheels move the robot forward when you push the left joystick forward. + leftFrontDrive.setDirection(DcMotor.Direction.REVERSE); + leftBackDrive.setDirection(DcMotor.Direction.REVERSE); + rightFrontDrive.setDirection(DcMotor.Direction.REVERSE); + rightBackDrive.setDirection(DcMotor.Direction.FORWARD); + + // Optional: set direction + intake.setDirection(DcMotor.Direction.REVERSE); + kicker.setDirection(DcMotor.Direction.FORWARD); + + // Highly recommended for REV Core Hex motors + intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + kicker.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + //intake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + ////Arm2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + + // Wait for the game to start (driver presses START) + telemetry.addData("Status", "Initialized"); + telemetry.update(); + telemetry.setMsTransmissionInterval(11); + limelight.pipelineSwitch(0); + limelight.start(); + waitForStart(); + runtime.reset(); + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + double max; + + // POV Mode uses left joystick to go forward & strafe, and right joystick to rotate. + double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value + double lateral = gamepad1.left_stick_x; + double yaw = gamepad1.right_stick_x; + + // Combine the joystick requests for each axis-motion to determine each wheel's power. + // Set up a variable for each drive wheel to save the power level for telemetry. + double leftFrontPower = axial + lateral + yaw; + double rightFrontPower = axial - lateral - yaw; + double leftBackPower = axial - lateral + yaw; + double rightBackPower = axial + lateral - yaw; + + // Normalize the values so no wheel power exceeds 100% + // This ensures that the robot maintains the desired motion. + max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)); + max = Math.max(max, Math.abs(leftBackPower)); + max = Math.max(max, Math.abs(rightBackPower)); + + if (max > 1.0) { + leftFrontPower /= max; + rightFrontPower /= max; + leftBackPower /= max; + rightBackPower /= max; + } + + /* + to find the power: + find difference in distance between starting point and where it wants to go + divide the current distance away over the total distance + set this as the power + */ + + // testing the intake and kicker + if (gamepad1.dpad_up) { + intake.setPower(0.9); // forward hi +// } else if (gamepad1.dpad_down) { +// intake.setPower(1); // reverse + } else if (gamepad1.dpad_down){ + intake.setPower(0); + } else if (gamepad1.dpad_right){ + kicker.setPower(-1); + } else if (gamepad1.dpad_left){ + kicker.setPower(0); + } + + // for competition; using a (X on our gamepad) inst4 + if (gamepad1.aWasPressed()) { + kicker.setPower(-1); + intake.setPower(0.9); + } else if (gamepad1.aWasReleased()) { + kicker.setPower(0); + intake.setPower(0); + + if (gamepad1.y) { + servo.setPosition(1.0); + } + if (gamepad1.x) { + servo.setPosition(0.0); + } + telemetry.addData("Triangle(Y)", gamepad1.y); + telemetry.addData("Square (X)", gamepad1.x); + telemetry.addData("Servo Position", servo.getPosition()); + telemetry.update(); + } + LLResult result = limelight.getLatestResult(); + if (result != null) { + if (result.isValid()) { + Pose3D botpose = result.getBotpose(); + telemetry.addData("tx", result.getTx()); + telemetry.addData("ty", result.getTy()); + telemetry.addData("Botpose", botpose.toString()); +} +} // Send calculated power to wheels + leftFrontDrive.setPower(leftFrontPower); + rightFrontDrive.setPower(rightFrontPower); + leftBackDrive.setPower(leftBackPower); + rightBackDrive.setPower(rightBackPower); + + //telemetry.addData("difference Test", differenceInArms); + //telemetry.addData("targetPosition Test", targetPosition); + // telemetry.addData("targetPositionArm2 Test", targetPositionArm2); + // telemetry.addData("zeroPositionArm1 Test", zeroPositionArm1); + // telemetry.addData("zeroPositionArm2 Test", zeroPositionArm2); + + // Show the elapsed game time and wheel power. + telemetry.addData("leftFrontDrive", leftFrontPower); + telemetry.addData("rightFrontDrive", rightFrontPower); + telemetry.addData("leftBackDrive", leftBackPower); + telemetry.addData("rightBackDrive", rightBackPower); + //telemetry.addData("Status", "Run Time: " + runtime.toString()); + // telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); + //telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); + // telemetry.update(); + + if (isStopRequested()) { + return; + } + } + } +} + + + + diff --git a/FtcRobotController/teamcode/TestShooter.java b/FtcRobotController/teamcode/TestShooter.java new file mode 100644 index 000000000000..ef9b0ad39a97 --- /dev/null +++ b/FtcRobotController/teamcode/TestShooter.java @@ -0,0 +1,159 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="Test Shooter", group="Linear OpMode") +//@Disabled +public class TestShooter extends LinearOpMode { + + // Declare OpMode members for each of the 4 motors. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor leftFrontDrive = null; + // private DcMotor leftBackDrive = null; + private DcMotor rightFrontDrive = null; + private DcMotor rightBackDrive = null; + + private DcMotor leftBackDrive = null; + + private DcMotor LeftShooter = null; + // private DcMotor RightShooter = null; + + @Override + public void runOpMode() { + + // Initialize the hardware variables. Note that the strings used here must correspond + // to the names assigned during the robot configuration step on the DS or RC devices. + //leftFrontDrive = hardwareMap.get(DcMotor.class, "left_front_drive"); + //leftBackDrive = hardwareMap.get(DcMotor.class, "left_back_drive"); + //rightFrontDrive = hardwareMap.get(DcMotor.class, "right_front_drive"); + //rightBackDrive = hardwareMap.get(DcMotor.class, "right_back_drive"); + LeftShooter = hardwareMap.get(DcMotor.class, "LeftShooter"); + // RightShooter = hardwareMap.get(DcMotor.class, "RightShooter"); + + // ######################################################################################## + // !!! IMPORTANT Drive Information. Test your motor directions. !!!!! + // ######################################################################################## + // Most robots need the motors on one side to be reversed to drive forward. + // The motor reversals shown here are for a "direct drive" robot (the wheels turn the same direction as the motor shaft) + // If your robot has additional gear reductions or uses a right-angled drive, it's important to ensure + // that your motors are turning in the correct direction. So, start out with the reversals here, BUT + // when you first test your robot, push the left joystick forward and observe the direction the wheels turn. + // Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward + // Keep testing until ALL the wheels move the robot forward when you push the left joystick forward. + /*leftFrontDrive.setDirection(DcMotor.Direction.REVERSE); + leftBackDrive.setDirection(DcMotor.Direction.FORWARD); + rightFrontDrive.setDirection(DcMotor.Direction.REVERSE); + rightBackDrive.setDirection(DcMotor.Direction.FORWARD);*/ + + + LeftShooter.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + // RightShooter.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + LeftShooter.setDirection(DcMotor.Direction.REVERSE); + // RightShooter.setDirection(DcMotor.Direction.REVERSE); + + // Wait for the game to start (driver presses START) + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + waitForStart(); + runtime.reset(); + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + double max; + + // POV Mode uses left joystick to go forward & strafe, and right joystick to rotate. + double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value + double lateral = gamepad1.left_stick_x; + double yaw = gamepad1.right_stick_x; + + // Combine the joystick requests for each axis-motion to determine each wheel's power. + // Set up a variable for each drive wheel to save the power level for telemetry. + double leftFrontPower = axial + lateral + yaw; + double rightFrontPower = axial - lateral - yaw; + double leftBackPower = axial - lateral + yaw; + double rightBackPower = axial + lateral - yaw; + + // Normalize the values so no wheel power exceeds 100% + // This ensures that the robot maintains the desired motion. + max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)); + max = Math.max(max, Math.abs(leftBackPower)); + max = Math.max(max, Math.abs(rightBackPower)); + + if (max > 1.0) { + leftFrontPower /= max; + rightFrontPower /= max; + leftBackPower /= max; + rightBackPower /= max; + } + + /* + to find the power: + find difference in distance between tarting point and where it wants to go + divide the current distance away over the total distance + set this as the power + */ + + if(gamepad2.dpad_up){ + // RightShooter.setPower(1); + LeftShooter.setPower(.8); + } + else if (gamepad2.dpad_down){ + // RightShooter.setPower(0); + LeftShooter.setPower(0); + + } + + /*// Send calculated power to wheels + leftFrontDrive.setPower(leftFrontPower); + rightFrontDrive.setPower(rightFrontPower); + leftBackDrive.setPower(leftBackPower); + rightBackDrive.setPower(rightBackPower);*/ + + + //telemetry.addData("difference Test", differenceInArms); + //telemetry.addData("targetPosition Test", targetPosition); + // telemetry.addData("targetPositionArm2 Test", targetPositionArm2); + // telemetry.addData("zeroPositionArm1 Test", zeroPositionArm1); + // telemetry.addData("zeroPositionArm2 Test", zeroPositionArm2); + + // Show the elapsed game time and wheel power. + //telemetry.addData("Arm1 Test", LeftShooter.getCurrentPosition()); + // telemetry.addData("Arm2 Test", RightShooter.getCurrentPosition()); + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); + telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); + telemetry.update(); + } + } +} diff --git a/FtcRobotController/teamcode/Webcam.java b/FtcRobotController/teamcode/Webcam.java new file mode 100644 index 000000000000..faffcb3c1e61 --- /dev/null +++ b/FtcRobotController/teamcode/Webcam.java @@ -0,0 +1,217 @@ +/* Copyright (c) 2023 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import android.util.Size; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +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.List; + +/* + * This OpMode illustrates the basics of AprilTag recognition and pose estimation, + * including Java Builder structures for specifying Vision parameters. + * + * For an introduction to AprilTags, see the FTC-DOCS link below: + * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html + * + * In this sample, any visible tag ID will be detected and displayed, but only tags that are included in the default + * "TagLibrary" will have their position and orientation information displayed. This default TagLibrary contains + * the current Season's AprilTags and a small set of "test Tags" in the high number range. + * + * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera. + * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below. + * https://ftc-docs.firstinspires.org/apriltag-detection-values + * + * To experiment with using AprilTags to navigate, try out these two driving samples: + * RobotAutoDriveToAprilTagOmni and RobotAutoDriveToAprilTagTank + * + * There are many "default" VisionPortal and AprilTag configuration parameters that may be overridden if desired. + * These default parameters are shown as comments in the code below. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Concept: AprilTag", group = "Concept") +//@Disabled +public class Webcam extends LinearOpMode { + + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + /** + * The variable to store our instance of the AprilTag processor. + */ + private AprilTagProcessor aprilTag; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + @Override + public void runOpMode() { + + initAprilTag(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch START to start OpMode"); + telemetry.update(); + waitForStart(); + + if (opModeIsActive()) { + while (opModeIsActive()) { + + telemetryAprilTag(); + + // Push telemetry to the Driver Station. + telemetry.update(); + + // Save CPU resources; can resume streaming when needed. + if (gamepad1.dpad_down) { + visionPortal.stopStreaming(); + } else if (gamepad1.dpad_up) { + visionPortal.resumeStreaming(); + } + + // Share the CPU. + sleep(20); + } + } + + // Save more CPU resources when camera is no longer needed. + visionPortal.close(); + + } // end method runOpMode() + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + + // Create the AprilTag processor. + aprilTag = new AprilTagProcessor.Builder() + + // The following default settings are available to un-comment and edit as needed. + //.setDrawAxes(false) + //.setDrawCubeProjection(false) + //.setDrawTagOutline(true) + //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) + //.setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary()) + //.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) + + // == CAMERA CALIBRATION == + // If you do not manually specify calibration parameters, the SDK will attempt + // to load a predefined calibration for your camera. + //.setLensIntrinsics(578.272, 578.272, 402.145, 221.506) + // ... these parameters are fx, fy, cx, cy. + + .build(); + + // Adjust Image Decimation to trade-off detection-range for detection-rate. + // eg: Some typical detection data using a Logitech C920 WebCam + // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second + // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second + // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second (default) + // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second (default) + // Note: Decimation can be changed on-the-fly to adapt during a match. + //aprilTag.setDecimation(3); + + // Create the vision portal by using a builder. + VisionPortal.Builder builder = new VisionPortal.Builder(); + + // Set the camera (webcam vs. built-in RC phone camera). + if (USE_WEBCAM) { + builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")); + } else { + builder.setCamera(BuiltinCameraDirection.BACK); + } + + // Choose a camera resolution. Not all cameras support all resolutions. + //builder.setCameraResolution(new Size(640, 480)); + + // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. + //builder.enableLiveView(true); + + // Set the stream format; MJPEG uses less bandwidth than default YUY2. + //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); + + // Choose whether or not LiveView stops if no processors are enabled. + // If set "true", monitor shows solid orange screen if no processors enabled. + // If set "false", monitor shows camera view without annotations. + //builder.setAutoStopLiveView(false); + + // Set and enable the processor. + builder.addProcessor(aprilTag); + + // Build the Vision Portal, using the above settings. + visionPortal = builder.build(); + + // Disable or re-enable the aprilTag processor at any time. + //visionPortal.setProcessorEnabled(aprilTag, true); + + } // end method initAprilTag() + + + /** + * Add telemetry about AprilTag detections. + */ + private void telemetryAprilTag() { + + List currentDetections = aprilTag.getDetections(); + telemetry.addData("# AprilTags Detected", currentDetections.size()); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + } // end for() loop + + // Add "key" information to telemetry + telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); + telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); + telemetry.addLine("RBE = Range, Bearing & Elevation"); + + } // end method telemetryAprilTag() + +} // end class \ No newline at end of file diff --git a/FtcRobotController/teamcode/Webcam2.java b/FtcRobotController/teamcode/Webcam2.java new file mode 100644 index 000000000000..4312e500ae60 --- /dev/null +++ b/FtcRobotController/teamcode/Webcam2.java @@ -0,0 +1,216 @@ +/* Copyright (c) 2023 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +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.List; + +/* + * This OpMode illustrates the basics of AprilTag recognition and pose estimation, + * including Java Builder structures for specifying Vision parameters. + * + * For an introduction to AprilTags, see the FTC-DOCS link below: + * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html + * + * In this sample, any visible tag ID will be detected and displayed, but only tags that are included in the default + * "TagLibrary" will have their position and orientation information displayed. This default TagLibrary contains + * the current Season's AprilTags and a small set of "test Tags" in the high number range. + * + * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera. + * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below. + * https://ftc-docs.firstinspires.org/apriltag-detection-values + * + * To experiment with using AprilTags to navigate, try out these two driving samples: + * RobotAutoDriveToAprilTagOmni and RobotAutoDriveToAprilTagTank + * + * There are many "default" VisionPortal and AprilTag configuration parameters that may be overridden if desired. + * These default parameters are shown as comments in the code below. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Concept: AprilTag", group = "Concept") +//@Disabled +public class Webcam2 extends LinearOpMode { + + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + /** + * The variable to store our instance of the AprilTag processor. + */ + private AprilTagProcessor aprilTag; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + @Override + public void runOpMode() { + + initAprilTag(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch START to start OpMode"); + telemetry.update(); + waitForStart(); + + if (opModeIsActive()) { + while (opModeIsActive()) { + + telemetryAprilTag(); + + // Push telemetry to the Driver Station. + telemetry.update(); + + // Save CPU resources; can resume streaming when needed. + if (gamepad1.dpad_down) { + visionPortal.stopStreaming(); + } else if (gamepad1.dpad_up) { + visionPortal.resumeStreaming(); + } + + // Share the CPU. + sleep(20); + } + } + + // Save more CPU resources when camera is no longer needed. + visionPortal.close(); + + } // end method runOpMode() + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + + // Create the AprilTag processor. + aprilTag = new AprilTagProcessor.Builder() + + // The following default settings are available to un-comment and edit as needed. + //.setDrawAxes(false) + //.setDrawCubeProjection(false) + //.setDrawTagOutline(true) + //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) + //.setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary()) + //.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) + + // == CAMERA CALIBRATION == + // If you do not manually specify calibration parameters, the SDK will attempt + // to load a predefined calibration for your camera. + //.setLensIntrinsics(578.272, 578.272, 402.145, 221.506) + // ... these parameters are fx, fy, cx, cy. + + .build(); + + // Adjust Image Decimation to trade-off detection-range for detection-rate. + // eg: Some typical detection data using a Logitech C920 WebCam + // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second + // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second + // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second (default) + // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second (default) + // Note: Decimation can be changed on-the-fly to adapt during a match. + //aprilTag.setDecimation(3); + + // Create the vision portal by using a builder. + VisionPortal.Builder builder = new VisionPortal.Builder(); + + // Set the camera (webcam vs. built-in RC phone camera). + if (USE_WEBCAM) { + builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")); + } else { + builder.setCamera(BuiltinCameraDirection.BACK); + } + + // Choose a camera resolution. Not all cameras support all resolutions. + //builder.setCameraResolution(new Size(640, 480)); + + // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. + //builder.enableLiveView(true); + + // Set the stream format; MJPEG uses less bandwidth than default YUY2. + //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); + + // Choose whether or not LiveView stops if no processors are enabled. + // If set "true", monitor shows solid orange screen if no processors enabled. + // If set "false", monitor shows camera view without annotations. + //builder.setAutoStopLiveView(false); + + // Set and enable the processor. + builder.addProcessor(aprilTag); + + // Build the Vision Portal, using the above settings. + visionPortal = builder.build(); + + // Disable or re-enable the aprilTag processor at any time. + //visionPortal.setProcessorEnabled(aprilTag, true); + + } // end method initAprilTag() + + + /** + * Add telemetry about AprilTag detections. + */ + private void telemetryAprilTag() { + + List currentDetections = aprilTag.getDetections(); + telemetry.addData("# AprilTags Detected", currentDetections.size()); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + } // end for() loop + + // Add "key" information to telemetry + telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); + telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); + telemetry.addLine("RBE = Range, Bearing & Elevation"); + + } // end method telemetryAprilTag() + +} // end class \ No newline at end of file diff --git a/FtcRobotController/teamcode/limelightdefault.java b/FtcRobotController/teamcode/limelightdefault.java new file mode 100644 index 000000000000..4b0e00d6af32 --- /dev/null +++ b/FtcRobotController/teamcode/limelightdefault.java @@ -0,0 +1,71 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="limetestdefault", group="Linear OpMode") +//@Disabled +public class limelightdefault extends LinearOpMode { + + private Limelight3A limelight; + + @Override + public void runOpMode() throws InterruptedException + { + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + + telemetry.setMsTransmissionInterval(11); + + limelight.pipelineSwitch(0); + + /* + * Starts polling for data. + */ + limelight.start(); + + while (opModeIsActive()) { + LLResult result = limelight.getLatestResult(); + if (result != null) { + if (result.isValid()) { + Pose3D botpose = result.getBotpose(); + telemetry.addData("tx", result.getTx()); + telemetry.addData("ty", result.getTy()); + telemetry.addData("Botpose", botpose.toString()); + } + } + } + } +} diff --git a/FtcRobotController/teamcode/limelighttest.java b/FtcRobotController/teamcode/limelighttest.java new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/FtcRobotController/teamcode/limetest.java b/FtcRobotController/teamcode/limetest.java new file mode 100644 index 000000000000..8fd63b1c5d1e --- /dev/null +++ b/FtcRobotController/teamcode/limetest.java @@ -0,0 +1,168 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; + +import java.util.List; +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="limetest", group="Linear OpMode") +//@Disabled +public class limetest extends LinearOpMode { + + // Declare OpMode members for each of the 4 motors. + private ElapsedTime runtime = new ElapsedTime(); + + private DcMotor rotate = null; + // private DcMotor leftBackDrive = null; + + private Limelight3A limelight; + + + @Override + public void runOpMode() { + + // Initialize the hardware variables. Note that the strings used here must correspond + // to the names assigned during the robot configuration step on the DS or RC devices. + + rotate = hardwareMap.get(DcMotor.class, "rotate"); + ////Arm2 = hardwareMap.get(DcMotor.class, "ArmMotor2"); + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + // ######################################################################################## + // !!! IMPORTANT Drive Information. Test your motor directions. !!!!! + // ######################################################################################## + // Most robots need the motors on one side to be reversed to drive forward. + // The motor reversals shown here are for a "direct drive" robot (the wheels turn the same direction as the motor shaft) + // If your robot has additional gear reductions or uses a right-angled drive, it's important to ensure + // that your motors are turning in the correct direction. So, start out with the reversals here, BUT + // when you first test your robot, push the left joystick forward and observe the direction the wheels turn. + // Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward + // Keep testing until ALL the wheels move the robot forward when you push the left joystick forward. + + + + // Wait for the game to start (driver presses START) + telemetry.addData("Status", "Initialized"); + telemetry.update(); + telemetry.setMsTransmissionInterval(11); + limelight.pipelineSwitch(0); + limelight.start(); + waitForStart(); + runtime.reset(); + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + // testing the intake and kicker + if (gamepad1.dpad_up) { + rotate.setPower(0.8); // forward hi +// } else if (gamepad1.dpad_down) { +// intake.setPower(1); // reverse + } else if (gamepad1.dpad_down){ + rotate.setPower(-0.8); + } + else{ + rotate.setPower(0); + } +// else if (gamepad1.dpad_right){ +// kicker.setPower(-1); +// } else if (gamepad1.dpad_left){ +// kicker.setPower(0); +// } +// +// // for competition; using a (X on our gamepad) inst4 +// if (gamepad1.aWasPressed()) { +// kicker.setPower(-1); +// intake.setPower(0.9); +// } else if (gamepad1.aWasReleased()) { +// kicker.setPower(0); +// intake.setPower(0); +// +// if (gamepad1.y) { +// servo.setPosition(1.0); +// } +// if (gamepad1.x) { +// servo.setPosition(0.0); +// } +// telemetry.addData("Triangle(Y)", gamepad1.y); +// telemetry.addData("Square (X)", gamepad1.x); +// telemetry.addData("Servo Position", servo.getPosition()); +// telemetry.update(); + } + LLResult result = limelight.getLatestResult(); + if (result != null) { + if (result.isValid()) { + Pose3D botpose = result.getBotpose(); + telemetry.addData("power", 0.1); + telemetry.addData("tx", result.getTx()); + telemetry.addData("ty", result.getTy()); + telemetry.addData("Botpose", botpose.toString()); + } + } // Send calculated power to wheels + + + //telemetry.addData("difference Test", differenceInArms); + //telemetry.addData("targetPosition Test", targetPosition); + // telemetry.addData("targetPositionArm2 Test", targetPositionArm2); + // telemetry.addData("zeroPositionArm1 Test", zeroPositionArm1); + // telemetry.addData("zeroPositionArm2 Test", zeroPositionArm2); + + // Show the elapsed game time and wheel power. +// telemetry.addData("leftFrontDrive", leftFrontPower); +// telemetry.addData("rightFrontDrive", rightFrontPower); +// telemetry.addData("leftBackDrive", leftBackPower); +// telemetry.addData("rightBackDrive", rightBackPower); + //telemetry.addData("Status", "Run Time: " + runtime.toString()); + // telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); + //telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); + telemetry.update(); + + if (isStopRequested()) { + return; + } + } + } + + + + + diff --git a/FtcRobotController/teamcode/readme.md b/FtcRobotController/teamcode/readme.md new file mode 100644 index 000000000000..4d1da42de0c0 --- /dev/null +++ b/FtcRobotController/teamcode/readme.md @@ -0,0 +1,131 @@ +## TeamCode Module + +Welcome! + +This module, TeamCode, is the place where you will write/paste the code for your team's +robot controller App. This module is currently empty (a clean slate) but the +process for adding OpModes is straightforward. + +## Creating your own OpModes + +The easiest way to create your own OpMode is to copy a Sample OpMode and make it your own. + +Sample opmodes exist in the FtcRobotController module. +To locate these samples, find the FtcRobotController module in the "Project/Android" tab. + +Expand the following tree elements: + FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples + +### Naming of Samples + +To gain a better understanding of how the samples are organized, and how to interpret the +naming system, it will help to understand the conventions that were used during their creation. + +These conventions are described (in detail) in the sample_conventions.md file in this folder. + +To summarize: A range of different samples classes will reside in the java/external/samples. +The class names will follow a naming convention which indicates the purpose of each class. +The prefix of the name will be one of the following: + +Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure + of a particular style of OpMode. These are bare bones examples. + +Sensor: This is a Sample OpMode that shows how to use a specific sensor. + It is not intended to drive a functioning robot, it is simply showing the minimal code + required to read and display the sensor values. + +Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base. + It may be used to provide a common baseline driving OpMode, or + to demonstrate how a particular sensor or concept can be used to navigate. + +Concept: This is a sample OpMode that illustrates performing a specific function or concept. + These may be complex, but their operation should be explained clearly in the comments, + or the comments should reference an external doc, guide or tutorial. + Each OpMode should try to only demonstrate a single concept so they are easy to + locate based on their name. These OpModes may not produce a drivable robot. + +After the prefix, other conventions will apply: + +* Sensor class names are constructed as: Sensor - Company - Type +* Robot class names are constructed as: Robot - Mode - Action - OpModetype +* Concept class names are constructed as: Concept - Topic - OpModetype + +Once you are familiar with the range of samples available, you can choose one to be the +basis for your own robot. In all cases, the desired sample(s) needs to be copied into +your TeamCode module to be used. + +This is done inside Android Studio directly, using the following steps: + + 1) Locate the desired sample class in the Project/Android tree. + + 2) Right click on the sample class and select "Copy" + + 3) Expand the TeamCode/java folder + + 4) Right click on the org.firstinspires.ftc.teamcode folder and select "Paste" + + 5) You will be prompted for a class name for the copy. + Choose something meaningful based on the purpose of this class. + Start with a capital letter, and remember that there may be more similar classes later. + +Once your copy has been created, you should prepare it for use on your robot. +This is done by adjusting the OpMode's name, and enabling it to be displayed on the +Driver Station's OpMode list. + +Each OpMode sample class begins with several lines of code like the ones shown below: + +``` + @TeleOp(name="Template: Linear OpMode", group="Linear Opmode") + @Disabled +``` + +The name that will appear on the driver station's "opmode list" is defined by the code: + ``name="Template: Linear OpMode"`` +You can change what appears between the quotes to better describe your opmode. +The "group=" portion of the code can be used to help organize your list of OpModes. + +As shown, the current OpMode will NOT appear on the driver station's OpMode list because of the + ``@Disabled`` annotation which has been included. +This line can simply be deleted , or commented out, to make the OpMode visible. + + + +## ADVANCED Multi-Team App management: Cloning the TeamCode Module + +In some situations, you have multiple teams in your club and you want them to all share +a common code organization, with each being able to *see* the others code but each having +their own team module with their own code that they maintain themselves. + +In this situation, you might wish to clone the TeamCode module, once for each of these teams. +Each of the clones would then appear along side each other in the Android Studio module list, +together with the FtcRobotController module (and the original TeamCode module). + +Selective Team phones can then be programmed by selecting the desired Module from the pulldown list +prior to clicking to the green Run arrow. + +Warning: This is not for the inexperienced Software developer. +You will need to be comfortable with File manipulations and managing Android Studio Modules. +These changes are performed OUTSIDE of Android Studios, so close Android Studios before you do this. + +Also.. Make a full project backup before you start this :) + +To clone TeamCode, do the following: + +Note: Some names start with "Team" and others start with "team". This is intentional. + +1) Using your operating system file management tools, copy the whole "TeamCode" + folder to a sibling folder with a corresponding new name, eg: "Team0417". + +2) In the new Team0417 folder, delete the TeamCode.iml file. + +3) the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder + to a matching name with a lowercase 'team' eg: "team0417". + +4) In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that contains + package="org.firstinspires.ftc.teamcode" + to be + package="org.firstinspires.ftc.team0417" + +5) Add: include ':Team0417' to the "/settings.gradle" file. + +6) Open up Android Studios and clean out any old files by using the menu to "Build/Clean Project"" \ No newline at end of file diff --git a/FtcRobotController/teamcode/servoexamples.java b/FtcRobotController/teamcode/servoexamples.java new file mode 100644 index 000000000000..0e305710e62a --- /dev/null +++ b/FtcRobotController/teamcode/servoexamples.java @@ -0,0 +1,17 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +@TeleOp +public class servoExamples extends OpMode { + testBenchServo bench = new testBenchServo(); + @Override + public void init(){ + bench.init(); + } + @Override + public void loop(){ + + } +} diff --git a/FtcRobotController/teamcode/testBenchServo.java b/FtcRobotController/teamcode/testBenchServo.java new file mode 100644 index 000000000000..e38a7f634c28 --- /dev/null +++ b/FtcRobotController/teamcode/testBenchServo.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; +public class testBenchServo { + private Servo servoPos; + + public void init(HardwareMap hwMap){ + servoPos = hwMap.get(Servo.class, "Servo1"); + } + + public void setServoPos(double angle) { + servoPos.setPosition(angle); + } +} diff --git a/FtcRobotController/teamcode/testintake.java b/FtcRobotController/teamcode/testintake.java new file mode 100644 index 000000000000..c4d4dd5543e1 --- /dev/null +++ b/FtcRobotController/teamcode/testintake.java @@ -0,0 +1,178 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; + +import java.util.List; +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="testintake", group="Linear OpMode") +//@Disabled +public class testintake extends LinearOpMode { + + // Declare OpMode members for each of the 4 motors. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor leftFrontDrive = null; + // private DcMotor leftBackDrive = null; + private DcMotor rightFrontDrive = null; + private DcMotor rightBackDrive = null; + + private DcMotor leftBackDrive = null; + private DcMotor intake = null; + private DcMotor kicker = null; + private Limelight3A limelight; + private Servo servo; + + @Override + public void runOpMode() { + + // Initialize the hardware variables. Note that the strings used here must correspond + // to the names assigned during the robot configuration step on the DS or RC devices. + leftFrontDrive = hardwareMap.get(DcMotor.class, "left_front_drive"); + leftBackDrive = hardwareMap.get(DcMotor.class, "left_back_drive"); + rightFrontDrive = hardwareMap.get(DcMotor.class, "right_front_drive"); + rightBackDrive = hardwareMap.get(DcMotor.class, "right_back_drive"); + intake = hardwareMap.get(DcMotor.class, "intake"); + kicker = hardwareMap.get(DcMotor.class, "kicker"); + ////Arm2 = hardwareMap.get(DcMotor.class, "ArmMotor2"); + servo = hardwareMap.get(Servo.class, "servo1"); + // ######################################################################################## + // !!! IMPORTANT Drive Information. Test your motor directions. !!!!! + // ######################################################################################## + // Most robots need the motors on one side to be reversed to drive forward. + // The motor reversals shown here are for a "direct drive" robot (the wheels turn the same direction as the motor shaft) + // If your robot has additional gear reductions or uses a right-angled drive, it's important to ensure + // that your motors are turning in the correct direction. So, start out with the reversals here, BUT + // when you first test your robot, push the left joystick forward and observe the direction the wheels turn. + // Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward + // Keep testing until ALL the wheels move the robot forward when you push the left joystick forward. + leftFrontDrive.setDirection(DcMotor.Direction.REVERSE); + leftBackDrive.setDirection(DcMotor.Direction.REVERSE); + rightFrontDrive.setDirection(DcMotor.Direction.REVERSE); + rightBackDrive.setDirection(DcMotor.Direction.FORWARD); + + // Optional: set direction + intake.setDirection(DcMotor.Direction.FORWARD); + kicker.setDirection(DcMotor.Direction.FORWARD); + + // Highly recommended for REV Core Hex motors + intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + kicker.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + //intake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + ////Arm2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + + // Wait for the game to start (driver presses START) + telemetry.addData("Status", "Initialized"); + telemetry.update(); + telemetry.setMsTransmissionInterval(11); + waitForStart(); + runtime.reset(); + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + double max; + + // POV Mode uses left joystick to go forward & strafe, and right joystick to rotate. + double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value + double lateral = gamepad1.left_stick_x; + double yaw = gamepad1.right_stick_x; + + // Combine the joystick requests for each axis-motion to determine each wheel's power. + // Set up a variable for each drive wheel to save the power level for telemetry. + double leftFrontPower = axial + lateral + yaw; + double rightFrontPower = axial - lateral - yaw; + double leftBackPower = axial - lateral + yaw; + double rightBackPower = axial + lateral - yaw; + + // Normalize the values so no wheel power exceeds 100% + // This ensures that the robot maintains the desired motion. + max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)); + max = Math.max(max, Math.abs(leftBackPower)); + max = Math.max(max, Math.abs(rightBackPower)); + + if (max > 1.0) { + leftFrontPower /= max; + rightFrontPower /= max; + leftBackPower /= max; + rightBackPower /= max; + } + + /* + to find the power: + find difference in distance between starting point and where it wants to go + divide the current distance away over the total distance + set this as the power + */ + + // testing the intake and kicker + if (gamepad1.dpad_up) { + intake.setPower(1); // forward hi +// } else if (gamepad1.dpad_down) { +// intake.setPower(1); // reverse + } else if (gamepad1.dpad_down) { + intake.setPower(0); + } else if (gamepad1.dpad_right) { + kicker.setPower(-1); + } else if (gamepad1.dpad_left) { + kicker.setPower(0); + } + + // for competition; using a (X on our gamepad) inst4 + if (gamepad1.aWasPressed()) { + kicker.setPower(-1); + intake.setPower(0.9); + } else if (gamepad1.aWasReleased()) { + kicker.setPower(0); + intake.setPower(0); + + if (gamepad1.y) { + servo.setPosition(1.0); + } + if (gamepad1.x) { + servo.setPosition(0.0); + } + } + } + } +} diff --git a/FtcRobotController/teamcode/testtest.java b/FtcRobotController/teamcode/testtest.java new file mode 100644 index 000000000000..35ec60c66cb7 --- /dev/null +++ b/FtcRobotController/teamcode/testtest.java @@ -0,0 +1,56 @@ + package org.firstinspires.ftc.teamcode; + + import androidx.annotation.NonNull; + + import com.acmerobotics.dashboard.config.Config; + import com.acmerobotics.dashboard.telemetry.TelemetryPacket; + import com.acmerobotics.roadrunner.Action; + import com.acmerobotics.roadrunner.Pose2d; + import com.acmerobotics.roadrunner.SequentialAction; + import com.acmerobotics.roadrunner.Vector2d; + import com.acmerobotics.roadrunner.ftc.Actions; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +@Autonomous(name = "FirstAuto", group = "Autonomous") +public class testtest extends LinearOpMode { + + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor leftFrontDrive = null; + private DcMotor rightFrontDrive = null; + private DcMotor rightBackDrive = null; + private DcMotor leftBackDrive = null; + private DcMotor IntakeM = null; + + public class Shooter { + /*public class startIntake() implements Action { + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + IntakeM.setPower(1); + return false; + } + + } + public Action StartIntake() { + return new startIntake(); + }*/ + + } + + public void runOpMode() { + + waitForStart(); + Shooter shooter = new Shooter(); + + if (isStopRequested()) return; + //Actions.runBlocking(shooter.StartIntake()); + + + } + +} + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTest.java new file mode 100644 index 000000000000..f957e77241c2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTest.java @@ -0,0 +1,124 @@ +package org.firstinspires.ftc.teamcode; + +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierCurve; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.BezierPoint; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.Path; +import com.pedropathing.paths.PathChain; +import com.pedropathing.paths.PathConstraints; +import com.pedropathing.util.Timer; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; + +@Autonomous (name = "DecodeAuto", group = "Autonomous") +public class AutoTest extends OpMode { + private Follower follower; + private Timer pathTimer, actionTimer, opmodeTimer; + private int pathState; + + private final Pose loadPose = new Pose(10, 9, Math.toRadians(180)); // blue loading zone + private final Pose scorePose = new Pose(64, 80, Math.toRadians(140)); // where we shoot + private final Pose pickup1Pose = new Pose(18.24, 84.85, Math.toRadians(180)); // row of balls closest to goal + private final Pose pickup2Pose = new Pose(18.24, 60, Math.toRadians(180)); // middle row + private final Pose pickup3Pose = new Pose(18.24, 35, Math.toRadians(180)); // row closest to loading zone + + private Path start_path; + private PathChain load_path, pickup_path1, pickup_path2; + + private void createPaths() { + + load_path = follower.pathBuilder() + .addPath(new BezierCurve(scorePose, new Pose(72, 30), loadPose)) // moves to loading zone from scoring position + .setLinearHeadingInterpolation(scorePose.getHeading(), loadPose.getHeading()) + .addPath(new BezierCurve(follower.getPose(), new Pose(72, 30), scorePose)) // moves back to scoring position + .setLinearHeadingInterpolation(loadPose.getHeading(), scorePose.getHeading()) + .build(); + + pickup_path1 = follower.pathBuilder() + .addPath(new BezierLine(scorePose, pickup1Pose)) + .setLinearHeadingInterpolation(scorePose.getHeading(), pickup1Pose.getHeading()) + .addPath(new BezierLine(pickup1Pose, scorePose)) + .setLinearHeadingInterpolation(pickup1Pose.getHeading(), scorePose.getHeading()) + .build(); + + pickup_path2 = follower.pathBuilder() + .addPath(new BezierCurve(scorePose, new Pose(62.69, 58), pickup2Pose)) + .setLinearHeadingInterpolation(scorePose.getHeading(), pickup2Pose.getHeading()) + .addPath(new BezierCurve(pickup2Pose, new Pose(62.69, 58), scorePose)) + .setLinearHeadingInterpolation(pickup2Pose.getHeading(), scorePose.getHeading()) + .build(); + + pickup_path2 = follower.pathBuilder() + .addPath(new BezierLine(scorePose, pickup3Pose)) + .setLinearHeadingInterpolation(scorePose.getHeading(), pickup3Pose.getHeading()) + .addPath(new BezierLine(pickup3Pose, scorePose)) + .setLinearHeadingInterpolation(pickup3Pose.getHeading(), scorePose.getHeading()) + .build(); + } + + private boolean pickedup1, pickedup2 = false; + + public void PathUpdate() { // state machine + switch (pathState) { + case 0: + follower.followPath(load_path); + if (!pickedup1) { + changePath(1); + } + break; + case 1: + follower.followPath(pickup_path1); + pickedup1 = true; + changePath(2); + break; + case 2: + follower.followPath(pickup_path2); + pickedup2 = true; + break; + + } + } + + public void changePath(int change) { + pathState = change; + pathTimer.resetTimer(); + } + + // Called once when we hit INIT + public void init() { + pathTimer = new Timer(); + opmodeTimer = new Timer(); + + follower = Constants.createFollower(hardwareMap); + createPaths(); + follower.setStartingPose(scorePose); + } + + // Called once when we hit PLAY + public void start() { + opmodeTimer.resetTimer(); + changePath(0); + } + + // LOOPS as the op mode is running + public void loop() { + follower.update(); + PathUpdate(); + + // Feedback to Driver Hub for debugging + telemetry.addData("path state", pathState); + telemetry.addData("x", follower.getPose().getX()); + telemetry.addData("y", follower.getPose().getY()); + telemetry.addData("heading", follower.getPose().getHeading()); + telemetry.addData("time left", opmodeTimer.getElapsedTimeSeconds()); + telemetry.update(); + + if (opmodeTimer.getElapsedTimeSeconds() >= 30) { + return; // perhaps not necessary + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FinalRobotCode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FinalRobotCode.java new file mode 100644 index 000000000000..0404a12aa968 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FinalRobotCode.java @@ -0,0 +1,189 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +import java.util.List; + +@TeleOp(name = "Robot: Final", group = "Main") +public class FinalRobotCode extends LinearOpMode { + + /* Hardware Members */ + private DcMotor leftFrontDrive, rightFrontDrive, leftBackDrive, rightBackDrive; + private DcMotor rotate, intake, shooter; + private Servo kicker, pusher, hood; + private Limelight3A limelight; + + /* Limelight Tuning Constants */ + private static final double kP = 0.02; + private static final double MAX_ROTATE_POWER = 0.25; + private static final double TX_DEADBAND = 1.2; + private static final double GAIN = 0.2; // Low-pass filter gain + private double smoothedTx = 0; + private int targetTagID = 20; + + /* State tracking for toggle/button debouncing */ + private boolean lastLeftBumper = false; + private boolean lastRightBumper = false; + + @Override + public void runOpMode() { + // 1. Initialize Drivetrain + leftFrontDrive = hardwareMap.get(DcMotor.class, "left_front_drive"); + leftBackDrive = hardwareMap.get(DcMotor.class, "left_back_drive"); + rightFrontDrive = hardwareMap.get(DcMotor.class, "right_front_drive"); + rightBackDrive = hardwareMap.get(DcMotor.class, "right_back_drive"); + + leftFrontDrive.setDirection(DcMotor.Direction.FORWARD); + leftBackDrive.setDirection(DcMotor.Direction.REVERSE); + rightFrontDrive.setDirection(DcMotor.Direction.FORWARD); + rightBackDrive.setDirection(DcMotor.Direction.FORWARD); + + // 2. Initialize Mechanisms + rotate = hardwareMap.get(DcMotor.class, "rotate"); + intake = hardwareMap.get(DcMotor.class, "intake"); + shooter = hardwareMap.get(DcMotor.class, "shooter"); + hood = hardwareMap.get(Servo.class, "hood"); // TODO add the hood servo to the config + kicker = hardwareMap.get(Servo.class, "kicker"); + pusher = hardwareMap.get(Servo.class, "pusher"); + + rotate.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + // 3. Initialize Vision + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + limelight.pipelineSwitch(0); + limelight.start(); + + telemetry.addData("Status", "Initialized - Ready to Fly"); + telemetry.update(); + + waitForStart(); + + /* + * all button mappings: + * Left stick: forward and backward, strafe left and right + * right stick, rotate (change heading) left and right + * a: intake + * b: shooter + * x: kicker on intake + * y: pusher on shooter + * bumpers: limelight targettagID + * dpad up/down: override limelight rotation + * right trigger then bumpers: change hood angle */ + + while (opModeIsActive()) { + + // --- SECTION 1: MECANUM DRIVE --- + double axial = -gamepad1.left_stick_y; + double lateral = gamepad1.left_stick_x; + double yaw = gamepad1.right_stick_x; + + double lf = axial + lateral + yaw; + double rf = axial - lateral - yaw; + double lb = axial - lateral + yaw; + double rb = axial + lateral - yaw; + + // Normalize powers + double max = Math.max(Math.max(Math.abs(lf), Math.abs(rf)), Math.max(Math.abs(lb), Math.abs(rb))); + if (max > 1.0) { + lf /= max; rf /= max; lb /= max; rb /= max; + } + + leftFrontDrive.setPower(lf); + rightFrontDrive.setPower(rf); + leftBackDrive.setPower(lb); + rightBackDrive.setPower(rb); + + + // --- SECTION 2: LIMELIGHT TARGETING (ROTATE MOTOR) --- + LLResult result = limelight.getLatestResult(); + + // Cycle target ID with bumpers + if (gamepad1.left_bumper && !lastLeftBumper) targetTagID = Math.max(20, targetTagID - 1); + if (gamepad1.right_bumper && !lastRightBumper) targetTagID = Math.min(25, targetTagID + 1); + lastLeftBumper = gamepad1.left_bumper; + lastRightBumper = gamepad1.right_bumper; + + double rotatePower = 0.0; + boolean tagFound = false; + + if (result != null && result.isValid()) { + List fiducials = result.getFiducialResults(); + for (LLResultTypes.FiducialResult fr : fiducials) { + if (fr.getFiducialId() == targetTagID) { + tagFound = true; + // Apply Smoothing Filter + smoothedTx = (result.getTx() * GAIN) + (smoothedTx * (1.0 - GAIN)); + + if (Math.abs(smoothedTx) > TX_DEADBAND) { + rotatePower = smoothedTx * kP; + } + break; + } + } + } else { + smoothedTx = 0; // Reset filter if vision lost + } + + // Constraints and Manual Overrides for Rotation + rotatePower = Math.max(-MAX_ROTATE_POWER, Math.min(MAX_ROTATE_POWER, rotatePower)); + if (gamepad1.dpad_up) rotatePower = 0.3; // Manual override up + else if (gamepad1.dpad_down) rotatePower = -0.3; // Manual override down + + rotate.setPower(rotatePower); + + + + // --- SECTION 3: MECHANISMS (INTAKE, SHOOTER, SERVOS) --- + + // Intake (Motor) - Spin while A is pressed + if (gamepad1.a) intake.setPower(-0.8); + else intake.setPower(0); + + // Shooter (Motor) - Spin while B is pressed + if (gamepad1.b) shooter.setPower(1.0); + else shooter.setPower(0); + + // Kicker (Servo) - Continuous rotation while X is pressed + // (Assumes continuous rotation servo where 1.0 is full speed one way) + if (gamepad1.x) kicker.setPosition(1.0); + else kicker.setPosition(0.5); // 0.5 is usually "Stop" for CR Servos + + // Pusher (Servo) - Continuous rotation while Y is pressed + if (gamepad1.y) pusher.setPosition(1); + else pusher.setPosition(0.5); // reset by moving down: + + // Hood (Servo) - Moves up and down (ideally we want limelight to do this) + if ((gamepad1.right_trigger) > 0.0) { // right_trigger returns float + if (gamepad1.right_bumper) { + hood.setPosition(hood.getPosition() + 0.1); + } else if (gamepad1.left_bumper) { + hood.setPosition(hood.getPosition() - 0.1); + } + } // Marwan added this in anticipation of the new variable-hood shooter + + + /* + * if (gamepad.b) { + * + * } + * + * + * */ + + + // --- SECTION 4: TELEMETRY --- + telemetry.addData("Target ID", targetTagID); + telemetry.addData("Vision Locked", tagFound); + telemetry.addData("Rotate Power", "%.2f", rotatePower); + telemetry.addData("Drive", "LF:%.2f RF:%.2f", lf, rf); + telemetry.update(); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FinalRobotCodetest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FinalRobotCodetest.java new file mode 100644 index 000000000000..318e579df55c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FinalRobotCodetest.java @@ -0,0 +1,184 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.Range; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.Limelight3A; + +@TeleOp(name = "Robot: Smooth Tracking Fix", group = "Main") +public class FinalRobotCodetest extends LinearOpMode { + + /* --- HARDWARE --- */ + private DcMotor leftFrontDrive, rightFrontDrive, leftBackDrive, rightBackDrive; + private DcMotor rotate, intake, shooter; + private Servo kicker, pusher; + private Limelight3A limelight; + + /* --- CONSTANTS --- */ + private static final double TICKS_PER_REV = 288.0; + private static final double GEAR_RATIO = 1.6; + private static final double TICKS_PER_DEG = (TICKS_PER_REV * GEAR_RATIO) / 360.0; + private static final double TICKS_PER_INCH = 1800.0; + private static final double TRACK_WIDTH_TICKS = 15000.0; + + // --- TUNING (Reduced to stop oscillation) --- + private double kP = 0.006; // Very gentle correction speed + private double kF = 0.04; // Very low friction boost (was 0.15) + private double deadband = 1.5; // Ignore errors smaller than 1.5 degrees + private int targetTagID = 22; + + /* --- VARIABLES --- */ + private double robotX = 0, robotY = 0, robotHeading = 0; + private double tagFieldX = 0, tagFieldY = 0; + private boolean tagInitialized = false; + + // Toggle State Memory + private boolean lastLB = false, lastRB = false; + private boolean lastA = false, lastB = false, lastX = false; + private boolean intakeOn = false, shooterOn = false, kickerOn = false; + + @Override + public void runOpMode() { + // Init Hardware + leftFrontDrive = hardwareMap.get(DcMotor.class, "left_front_drive"); + leftBackDrive = hardwareMap.get(DcMotor.class, "left_back_drive"); + rightFrontDrive = hardwareMap.get(DcMotor.class, "right_front_drive"); + rightBackDrive = hardwareMap.get(DcMotor.class, "right_back_drive"); + + // IMPORTANT: Ensure these are plugged into the ports matching your config + // If these return 0, your ghost tracking will not work. + leftBackDrive.setDirection(DcMotor.Direction.REVERSE); + + rotate = hardwareMap.get(DcMotor.class, "rotate"); + rotate.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rotate.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rotate.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + intake = hardwareMap.get(DcMotor.class, "intake"); + shooter = hardwareMap.get(DcMotor.class, "shooter"); + kicker = hardwareMap.get(Servo.class, "kicker"); + pusher = hardwareMap.get(Servo.class, "pusher"); + + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + limelight.pipelineSwitch(0); + limelight.start(); + + waitForStart(); + + while (opModeIsActive()) { + // --- 1. ODOMETRY --- + // These encoders MUST count up/down when you turn the robot chassis + double lPos = leftFrontDrive.getCurrentPosition(); + double rPos = rightFrontDrive.getCurrentPosition(); + + robotHeading = (rPos - lPos) / TRACK_WIDTH_TICKS * 360.0; + double avgDist = ((lPos + rPos) / 2.0) / TICKS_PER_INCH; + robotX = avgDist * Math.cos(Math.toRadians(robotHeading)); + robotY = avgDist * Math.sin(Math.toRadians(robotHeading)); +//test + // --- 2. DRIVE --- + double axial = -gamepad1.left_stick_y; + double lateral = gamepad1.left_stick_x; + double yaw = gamepad1.right_stick_x; + leftFrontDrive.setPower(axial + lateral + yaw); + rightFrontDrive.setPower(axial - lateral - yaw); + leftBackDrive.setPower(axial - lateral + yaw); + rightBackDrive.setPower(axial + lateral - yaw); + + // --- 3. TRACKING LOGIC --- + LLResult result = limelight.getLatestResult(); + double currentTurretDeg = rotate.getCurrentPosition() / TICKS_PER_DEG; + + double rotatePower = 0; + double trackingError = 0; + boolean seeTagRightNow = false; + + // A. LIVE VISION + if (result != null && result.isValid()) { + for (LLResultTypes.FiducialResult fr : result.getFiducialResults()) { + if (fr.getFiducialId() == targetTagID) { + seeTagRightNow = true; + + double xCam = fr.getCameraPoseTargetSpace().getPosition().x; + double zCam = fr.getCameraPoseTargetSpace().getPosition().z; + double distCM = Math.sqrt(xCam*xCam + zCam*zCam) * 100.0; + + double tx = result.getTx(); + double absAngle = Math.toRadians(robotHeading + currentTurretDeg + tx); + double distInches = distCM / 2.54; + + tagFieldX = robotX + (distInches * Math.cos(absAngle)); + tagFieldY = robotY + (distInches * Math.sin(absAngle)); + tagInitialized = true; + + trackingError = tx; + break; + } + } + } + + // B. GHOST TRACKING + if (tagInitialized && !seeTagRightNow) { + double angleToTag = Math.toDegrees(Math.atan2(tagFieldY - robotY, tagFieldX - robotX)); + double targetTurretAngle = angleToTag - robotHeading; + + trackingError = targetTurretAngle - currentTurretDeg; + + // Normalize error + while (trackingError > 180) trackingError -= 360; + while (trackingError < -180) trackingError += 360; + } + + // --- 4. MOTOR POWER (Anti-Oscillation) --- + if (Math.abs(trackingError) > deadband) { + rotatePower = trackingError * kP; + + // Lower friction boost to prevent shaking + // Only apply if power is very low to prevent "kicking" + if (Math.abs(rotatePower) < 0.15) { + if (rotatePower > 0) rotatePower += kF; + else rotatePower -= kF; + } + } else { + rotatePower = 0; + } + + rotate.setPower(Range.clip(rotatePower, -0.35, 0.35)); + + // --- 5. CONTROLS --- + if (gamepad1.left_bumper && !lastLB) targetTagID--; + if (gamepad1.right_bumper && !lastRB) targetTagID++; + lastLB = gamepad1.left_bumper; lastRB = gamepad1.right_bumper; + + if (gamepad1.a && !lastA) intakeOn = !intakeOn; + lastA = gamepad1.a; + intake.setPower(intakeOn ? -0.8 : 0); + + if (gamepad1.b && !lastB) shooterOn = !shooterOn; + lastB = gamepad1.b; + shooter.setPower(shooterOn ? 1.0 : 0); + + if (gamepad1.x && !lastX) kickerOn = !kickerOn; + lastX = gamepad1.x; + kicker.setPosition(kickerOn ? 1.0 : 0.5); + pusher.setPosition(gamepad1.y ? 1.0 : 0.5); + + // --- 6. CRITICAL DEBUGGING --- + telemetry.addLine(seeTagRightNow ? "VISION: LOCKED" : "VISION: SEARCHING"); + telemetry.addData("Motor Power", rotatePower); + telemetry.addData("Error", "%.2f", trackingError); + + telemetry.addLine("--- ODOMETRY CHECK ---"); + // IF THESE ARE 0 WHEN YOU TURN, GHOST TRACKING WILL FAIL + telemetry.addData("Left Encoder", leftFrontDrive.getCurrentPosition()); + telemetry.addData("Right Encoder", rightFrontDrive.getCurrentPosition()); + telemetry.addData("Robot Heading", "%.1f°", robotHeading); + + telemetry.update(); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java new file mode 100644 index 000000000000..fa9f96a1f1bc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java @@ -0,0 +1,89 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; + +import java.util.List; +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="limetestdefault", group="Linear OpMode") + +public class Teleop extends LinearOpMode { + + private Limelight3A limelight; + + @Override + public void runOpMode() throws InterruptedException + { + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + + telemetry.setMsTransmissionInterval(11); + limelight.setPollRateHz(100); + limelight.pipelineSwitch(0); + + + waitForStart(); + + /* + * Starts polling for data. + */ + limelight.start(); + + + telemetry.addData("ll3a status", limelight.isConnected()); + telemetry.update(); + + while (opModeIsActive()) { + LLResult result = limelight.getLatestResult(); + if (result != null) { + if (result.isValid()) { + Pose3D botpose = result.getBotpose(); + telemetry.addData("tx", result.getTx()); + telemetry.addData("ty", result.getTy()); + telemetry.addData("Botpose", botpose.toString()); + } + } + + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestCar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestCar.java new file mode 100644 index 000000000000..541ef2ed48c6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestCar.java @@ -0,0 +1,163 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="Test Car", group="Linear OpMode") +//@Disabled +public class TestCar extends LinearOpMode { + + // Declare OpMode members for each of the 4 motors. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor leftFrontDrive = null; + // private DcMotor leftBackDrive = null; + private DcMotor rightFrontDrive = null; + private DcMotor rightBackDrive = null; + + private DcMotor leftBackDrive = null; + + private DcMotor Arm1 = null; + ////private DcMotor Arm2 = null; + + @Override + public void runOpMode() { + + // Initialize the hardware variables. Note that the strings used here must correspond + // to the names assigned during the robot configuration step on the DS or RC devices. + leftFrontDrive = hardwareMap.get(DcMotor.class, "left_front_drive"); + leftBackDrive = hardwareMap.get(DcMotor.class, "left_back_drive"); + rightFrontDrive = hardwareMap.get(DcMotor.class, "right_front_drive"); + rightBackDrive = hardwareMap.get(DcMotor.class, "right_back_drive"); + Arm1 = hardwareMap.get(DcMotor.class, "ArmMotor1"); + ////Arm2 = hardwareMap.get(DcMotor.class, "ArmMotor2"); + + // ######################################################################################## + // !!! IMPORTANT Drive Information. Test your motor directions. !!!!! + // ######################################################################################## + // Most robots need the motors on one side to be reversed to drive forward. + // The motor reversals shown here are for a "direct drive" robot (the wheels turn the same direction as the motor shaft) + // If your robot has additional gear reductions or uses a right-angled drive, it's important to ensure + // that your motors are turning in the correct direction. So, start out with the reversals here, BUT + // when you first test your robot, push the left joystick forward and observe the direction the wheels turn. + // Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward + // Keep testing until ALL the wheels move the robot forward when you push the left joystick forward. + leftFrontDrive.setDirection(DcMotor.Direction.REVERSE); + leftBackDrive.setDirection(DcMotor.Direction.FORWARD); + rightFrontDrive.setDirection(DcMotor.Direction.REVERSE); + rightBackDrive.setDirection(DcMotor.Direction.FORWARD); + + + Arm1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + ////Arm2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + + // Wait for the game to start (driver presses START) + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + waitForStart(); + runtime.reset(); + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + double max; + + // POV Mode uses left joystick to go forward & strafe, and right joystick to rotate. + double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value + double lateral = gamepad1.left_stick_x; + double yaw = gamepad1.right_stick_x; + + // Combine the joystick requests for each axis-motion to determine each wheel's power. + // Set up a variable for each drive wheel to save the power level for telemetry. + double leftFrontPower = axial + lateral + yaw; + double rightFrontPower = axial - lateral - yaw; + double leftBackPower = axial - lateral + yaw; + double rightBackPower = axial + lateral - yaw; + + // Normalize the values so no wheel power exceeds 100% + // This ensures that the robot maintains the desired motion. + max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)); + max = Math.max(max, Math.abs(leftBackPower)); + max = Math.max(max, Math.abs(rightBackPower)); + + if (max > 1.0) { + leftFrontPower /= max; + rightFrontPower /= max; + leftBackPower /= max; + rightBackPower /= max; + } + + /* + to find the power: + find difference in distance between tarting point and where it wants to go + divide the current distance away over the total distance + set this as the power + */ + + if(gamepad2.dpad_up){ + Arm1.setPower(1); + } + else if (gamepad2.dpad_down){ + Arm1.setPower(1); + + }else{ + Arm1.setPower(0); + } + + // Send calculated power to wheels + leftFrontDrive.setPower(leftFrontPower); + rightFrontDrive.setPower(rightFrontPower); + leftBackDrive.setPower(leftBackPower); + rightBackDrive.setPower(rightBackPower); + + + //telemetry.addData("difference Test", differenceInArms); + //telemetry.addData("targetPosition Test", targetPosition); + // telemetry.addData("targetPositionArm2 Test", targetPositionArm2); + // telemetry.addData("zeroPositionArm1 Test", zeroPositionArm1); + // telemetry.addData("zeroPositionArm2 Test", zeroPositionArm2); + + // Show the elapsed game time and wheel power. +// telemetry.addData("Arm1 Test", Arm1.getCurrentPosition()); +// telemetry.addData("Arm2 Test", Arm2.getCurrentPosition()); + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); + telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestCar2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestCar2.java new file mode 100644 index 000000000000..b3085cb16064 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestCar2.java @@ -0,0 +1,222 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; + +import java.util.List; +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="Test Car2", group="Linear OpMode") +//@Disabled +public class TestCar2 extends LinearOpMode { + + // Declare OpMode members for each of the 4 motors. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor leftFrontDrive = null; + // private DcMotor leftBackDrive = null; + private DcMotor rightFrontDrive = null; + private DcMotor rightBackDrive = null; + + private DcMotor leftBackDrive = null; + private DcMotor intake = null; + private DcMotor kicker = null; + private Limelight3A limelight; + private Servo servo; + + @Override + public void runOpMode() { + + // Initialize the hardware variables. Note that the strings used here must correspond + // to the names assigned during the robot configuration step on the DS or RC devices. + leftFrontDrive = hardwareMap.get(DcMotor.class, "left_front_drive"); + leftBackDrive = hardwareMap.get(DcMotor.class, "left_back_drive"); + rightFrontDrive = hardwareMap.get(DcMotor.class, "right_front_drive"); + rightBackDrive = hardwareMap.get(DcMotor.class, "right_back_drive"); + intake = hardwareMap.get(DcMotor.class, "intake"); + kicker = hardwareMap.get(DcMotor.class, "kicker"); + ////Arm2 = hardwareMap.get(DcMotor.class, "ArmMotor2"); + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + servo = hardwareMap.get(Servo.class, "servo1"); + // ######################################################################################## + // !!! IMPORTANT Drive Information. Test your motor directions. !!!!! + // ######################################################################################## + // Most robots need the motors on one side to be reversed to drive forward. + // The motor reversals shown here are for a "direct drive" robot (the wheels turn the same direction as the motor shaft) + // If your robot has additional gear reductions or uses a right-angled drive, it's important to ensure + // that your motors are turning in the correct direction. So, start out with the reversals here, BUT + // when you first test your robot, push the left joystick forward and observe the direction the wheels turn. + // Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward + // Keep testing until ALL the wheels move the robot forward when you push the left joystick forward. + leftFrontDrive.setDirection(DcMotor.Direction.REVERSE); + leftBackDrive.setDirection(DcMotor.Direction.REVERSE); + rightFrontDrive.setDirection(DcMotor.Direction.REVERSE); + rightBackDrive.setDirection(DcMotor.Direction.FORWARD); + + // Optional: set direction + intake.setDirection(DcMotor.Direction.REVERSE); + kicker.setDirection(DcMotor.Direction.FORWARD); + + // Highly recommended for REV Core Hex motors + intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + kicker.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + //intake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + ////Arm2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + + // Wait for the game to start (driver presses START) + telemetry.addData("Status", "Initialized"); + telemetry.update(); + telemetry.setMsTransmissionInterval(11); + limelight.pipelineSwitch(0); + limelight.start(); + waitForStart(); + runtime.reset(); + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + double max; + + // POV Mode uses left joystick to go forward & strafe, and right joystick to rotate. + double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value + double lateral = gamepad1.left_stick_x; + double yaw = gamepad1.right_stick_x; + + // Combine the joystick requests for each axis-motion to determine each wheel's power. + // Set up a variable for each drive wheel to save the power level for telemetry. + double leftFrontPower = axial + lateral + yaw; + double rightFrontPower = axial - lateral - yaw; + double leftBackPower = axial - lateral + yaw; + double rightBackPower = axial + lateral - yaw; + + // Normalize the values so no wheel power exceeds 100% + // This ensures that the robot maintains the desired motion. + max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)); + max = Math.max(max, Math.abs(leftBackPower)); + max = Math.max(max, Math.abs(rightBackPower)); + + if (max > 1.0) { + leftFrontPower /= max; + rightFrontPower /= max; + leftBackPower /= max; + rightBackPower /= max; + } + + /* + to find the power: + find difference in distance between starting point and where it wants to go + divide the current distance away over the total distance + set this as the power + */ + + // testing the intake and kicker + if (gamepad1.dpad_up) { + intake.setPower(0.9); // forward hi +// } else if (gamepad1.dpad_down) { +// intake.setPower(1); // reverse + } else if (gamepad1.dpad_down){ + intake.setPower(0); + } else if (gamepad1.dpad_right){ + kicker.setPower(-1); + } else if (gamepad1.dpad_left){ + kicker.setPower(0); + } + + // for competition; using a (X on our gamepad) inst4 + if (gamepad1.aWasPressed()) { + kicker.setPower(-1); + intake.setPower(0.9); + } else if (gamepad1.aWasReleased()) { + kicker.setPower(0); + intake.setPower(0); + + if (gamepad1.y) { + servo.setPosition(1.0); + } + if (gamepad1.x) { + servo.setPosition(0.0); + } + telemetry.addData("Triangle(Y)", gamepad1.y); + telemetry.addData("Square (X)", gamepad1.x); + telemetry.addData("Servo Position", servo.getPosition()); + telemetry.update(); + } + LLResult result = limelight.getLatestResult(); + if (result != null) { + if (result.isValid()) { + Pose3D botpose = result.getBotpose(); + telemetry.addData("tx", result.getTx()); + telemetry.addData("ty", result.getTy()); + telemetry.addData("Botpose", botpose.toString()); +} +} // Send calculated power to wheels + leftFrontDrive.setPower(leftFrontPower); + rightFrontDrive.setPower(rightFrontPower); + leftBackDrive.setPower(leftBackPower); + rightBackDrive.setPower(rightBackPower); + + //telemetry.addData("difference Test", differenceInArms); + //telemetry.addData("targetPosition Test", targetPosition); + // telemetry.addData("targetPositionArm2 Test", targetPositionArm2); + // telemetry.addData("zeroPositionArm1 Test", zeroPositionArm1); + // telemetry.addData("zeroPositionArm2 Test", zeroPositionArm2); + + // Show the elapsed game time and wheel power. + telemetry.addData("leftFrontDrive", leftFrontPower); + telemetry.addData("rightFrontDrive", rightFrontPower); + telemetry.addData("leftBackDrive", leftBackPower); + telemetry.addData("rightBackDrive", rightBackPower); + //telemetry.addData("Status", "Run Time: " + runtime.toString()); + // telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); + //telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); + // telemetry.update(); + + if (isStopRequested()) { + return; + } + } + } +} + + + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestShooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestShooter.java new file mode 100644 index 000000000000..5d62c7620245 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestShooter.java @@ -0,0 +1,114 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="Test Shooter", group="Linear OpMode") +//@Disabled +public class TestShooter extends LinearOpMode { + + // Declare OpMode members for each of the 4 motors. + private ElapsedTime runtime = new ElapsedTime(); + + private DcMotor shooter = null; + // private DcMotor RightShooter = null; + + @Override + public void runOpMode() { + + // Initialize the hardware variables. Note that the strings used here must correspond + // to the names assigned during the robot configuration step on the DS or RC devices. + //leftFrontDrive = hardwareMap.get(DcMotor.class, "left_front_drive"); + //leftBackDrive = hardwareMap.get(DcMotor.class, "left_back_drive"); + //rightFrontDrive = hardwareMap.get(DcMotor.class, "right_front_drive"); + //rightBackDrive = hardwareMap.get(DcMotor.class, "right_back_drive"); + shooter = hardwareMap.get(DcMotor.class, "shooter"); + // RightShooter = hardwareMap.get(DcMotor.class, "RightShooter"); + + // ######################################################################################## + // !!! IMPORTANT Drive Information. Test your motor directions. !!!!! + // ######################################################################################## + // Most robots need the motors on one side to be reversed to drive forward. + // The motor reversals shown here are for a "direct drive" robot (the wheels turn the same direction as the motor shaft) + // If your robot has additional gear reductions or uses a right-angled drive, it's important to ensure + // that your motors are turning in the correct direction. So, start out with the reversals here, BUT + // when you first test your robot, push the left joystick forward and observe the direction the wheels turn. + // Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward + // Keep testing until ALL the wheels move the robot forward when you push the left joystick forward. + /*leftFrontDrive.setDirection(DcMotor.Direction.REVERSE); + leftBackDrive.setDirection(DcMotor.Direction.FORWARD); + rightFrontDrive.setDirection(DcMotor.Direction.REVERSE); + rightBackDrive.setDirection(DcMotor.Direction.FORWARD);*/ + + + shooter.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + // RightShooter.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + shooter.setDirection(DcMotor.Direction.REVERSE); + // RightShooter.setDirection(DcMotor.Direction.REVERSE); + + // Wait for the game to start (driver presses START) + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + waitForStart(); + runtime.reset(); + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + double max; + + + // Combine the joystick requests for each axis-motion to determine each wheel's power. + + + /* + to find the power: + find difference in distance between tarting point and where it wants to go + divide the current distance away over the total distance + set this as the power + */ + + if(gamepad2.dpad_up){ + // RightShooter.setPower(1); + shooter.setPower(.8); + } + else if (gamepad2.dpad_down){ + // RightShooter.setPower(0); + shooter.setPower(0); + + } + + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Webcam.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Webcam.java new file mode 100644 index 000000000000..faffcb3c1e61 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Webcam.java @@ -0,0 +1,217 @@ +/* Copyright (c) 2023 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import android.util.Size; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +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.List; + +/* + * This OpMode illustrates the basics of AprilTag recognition and pose estimation, + * including Java Builder structures for specifying Vision parameters. + * + * For an introduction to AprilTags, see the FTC-DOCS link below: + * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html + * + * In this sample, any visible tag ID will be detected and displayed, but only tags that are included in the default + * "TagLibrary" will have their position and orientation information displayed. This default TagLibrary contains + * the current Season's AprilTags and a small set of "test Tags" in the high number range. + * + * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera. + * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below. + * https://ftc-docs.firstinspires.org/apriltag-detection-values + * + * To experiment with using AprilTags to navigate, try out these two driving samples: + * RobotAutoDriveToAprilTagOmni and RobotAutoDriveToAprilTagTank + * + * There are many "default" VisionPortal and AprilTag configuration parameters that may be overridden if desired. + * These default parameters are shown as comments in the code below. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Concept: AprilTag", group = "Concept") +//@Disabled +public class Webcam extends LinearOpMode { + + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + /** + * The variable to store our instance of the AprilTag processor. + */ + private AprilTagProcessor aprilTag; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + @Override + public void runOpMode() { + + initAprilTag(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch START to start OpMode"); + telemetry.update(); + waitForStart(); + + if (opModeIsActive()) { + while (opModeIsActive()) { + + telemetryAprilTag(); + + // Push telemetry to the Driver Station. + telemetry.update(); + + // Save CPU resources; can resume streaming when needed. + if (gamepad1.dpad_down) { + visionPortal.stopStreaming(); + } else if (gamepad1.dpad_up) { + visionPortal.resumeStreaming(); + } + + // Share the CPU. + sleep(20); + } + } + + // Save more CPU resources when camera is no longer needed. + visionPortal.close(); + + } // end method runOpMode() + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + + // Create the AprilTag processor. + aprilTag = new AprilTagProcessor.Builder() + + // The following default settings are available to un-comment and edit as needed. + //.setDrawAxes(false) + //.setDrawCubeProjection(false) + //.setDrawTagOutline(true) + //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) + //.setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary()) + //.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) + + // == CAMERA CALIBRATION == + // If you do not manually specify calibration parameters, the SDK will attempt + // to load a predefined calibration for your camera. + //.setLensIntrinsics(578.272, 578.272, 402.145, 221.506) + // ... these parameters are fx, fy, cx, cy. + + .build(); + + // Adjust Image Decimation to trade-off detection-range for detection-rate. + // eg: Some typical detection data using a Logitech C920 WebCam + // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second + // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second + // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second (default) + // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second (default) + // Note: Decimation can be changed on-the-fly to adapt during a match. + //aprilTag.setDecimation(3); + + // Create the vision portal by using a builder. + VisionPortal.Builder builder = new VisionPortal.Builder(); + + // Set the camera (webcam vs. built-in RC phone camera). + if (USE_WEBCAM) { + builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")); + } else { + builder.setCamera(BuiltinCameraDirection.BACK); + } + + // Choose a camera resolution. Not all cameras support all resolutions. + //builder.setCameraResolution(new Size(640, 480)); + + // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. + //builder.enableLiveView(true); + + // Set the stream format; MJPEG uses less bandwidth than default YUY2. + //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); + + // Choose whether or not LiveView stops if no processors are enabled. + // If set "true", monitor shows solid orange screen if no processors enabled. + // If set "false", monitor shows camera view without annotations. + //builder.setAutoStopLiveView(false); + + // Set and enable the processor. + builder.addProcessor(aprilTag); + + // Build the Vision Portal, using the above settings. + visionPortal = builder.build(); + + // Disable or re-enable the aprilTag processor at any time. + //visionPortal.setProcessorEnabled(aprilTag, true); + + } // end method initAprilTag() + + + /** + * Add telemetry about AprilTag detections. + */ + private void telemetryAprilTag() { + + List currentDetections = aprilTag.getDetections(); + telemetry.addData("# AprilTags Detected", currentDetections.size()); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + } // end for() loop + + // Add "key" information to telemetry + telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); + telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); + telemetry.addLine("RBE = Range, Bearing & Elevation"); + + } // end method telemetryAprilTag() + +} // end class \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Webcam2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Webcam2.java new file mode 100644 index 000000000000..4312e500ae60 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Webcam2.java @@ -0,0 +1,216 @@ +/* Copyright (c) 2023 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +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.List; + +/* + * This OpMode illustrates the basics of AprilTag recognition and pose estimation, + * including Java Builder structures for specifying Vision parameters. + * + * For an introduction to AprilTags, see the FTC-DOCS link below: + * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html + * + * In this sample, any visible tag ID will be detected and displayed, but only tags that are included in the default + * "TagLibrary" will have their position and orientation information displayed. This default TagLibrary contains + * the current Season's AprilTags and a small set of "test Tags" in the high number range. + * + * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera. + * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below. + * https://ftc-docs.firstinspires.org/apriltag-detection-values + * + * To experiment with using AprilTags to navigate, try out these two driving samples: + * RobotAutoDriveToAprilTagOmni and RobotAutoDriveToAprilTagTank + * + * There are many "default" VisionPortal and AprilTag configuration parameters that may be overridden if desired. + * These default parameters are shown as comments in the code below. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Concept: AprilTag", group = "Concept") +//@Disabled +public class Webcam2 extends LinearOpMode { + + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + /** + * The variable to store our instance of the AprilTag processor. + */ + private AprilTagProcessor aprilTag; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + @Override + public void runOpMode() { + + initAprilTag(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch START to start OpMode"); + telemetry.update(); + waitForStart(); + + if (opModeIsActive()) { + while (opModeIsActive()) { + + telemetryAprilTag(); + + // Push telemetry to the Driver Station. + telemetry.update(); + + // Save CPU resources; can resume streaming when needed. + if (gamepad1.dpad_down) { + visionPortal.stopStreaming(); + } else if (gamepad1.dpad_up) { + visionPortal.resumeStreaming(); + } + + // Share the CPU. + sleep(20); + } + } + + // Save more CPU resources when camera is no longer needed. + visionPortal.close(); + + } // end method runOpMode() + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + + // Create the AprilTag processor. + aprilTag = new AprilTagProcessor.Builder() + + // The following default settings are available to un-comment and edit as needed. + //.setDrawAxes(false) + //.setDrawCubeProjection(false) + //.setDrawTagOutline(true) + //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) + //.setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary()) + //.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) + + // == CAMERA CALIBRATION == + // If you do not manually specify calibration parameters, the SDK will attempt + // to load a predefined calibration for your camera. + //.setLensIntrinsics(578.272, 578.272, 402.145, 221.506) + // ... these parameters are fx, fy, cx, cy. + + .build(); + + // Adjust Image Decimation to trade-off detection-range for detection-rate. + // eg: Some typical detection data using a Logitech C920 WebCam + // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second + // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second + // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second (default) + // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second (default) + // Note: Decimation can be changed on-the-fly to adapt during a match. + //aprilTag.setDecimation(3); + + // Create the vision portal by using a builder. + VisionPortal.Builder builder = new VisionPortal.Builder(); + + // Set the camera (webcam vs. built-in RC phone camera). + if (USE_WEBCAM) { + builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")); + } else { + builder.setCamera(BuiltinCameraDirection.BACK); + } + + // Choose a camera resolution. Not all cameras support all resolutions. + //builder.setCameraResolution(new Size(640, 480)); + + // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. + //builder.enableLiveView(true); + + // Set the stream format; MJPEG uses less bandwidth than default YUY2. + //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); + + // Choose whether or not LiveView stops if no processors are enabled. + // If set "true", monitor shows solid orange screen if no processors enabled. + // If set "false", monitor shows camera view without annotations. + //builder.setAutoStopLiveView(false); + + // Set and enable the processor. + builder.addProcessor(aprilTag); + + // Build the Vision Portal, using the above settings. + visionPortal = builder.build(); + + // Disable or re-enable the aprilTag processor at any time. + //visionPortal.setProcessorEnabled(aprilTag, true); + + } // end method initAprilTag() + + + /** + * Add telemetry about AprilTag detections. + */ + private void telemetryAprilTag() { + + List currentDetections = aprilTag.getDetections(); + telemetry.addData("# AprilTags Detected", currentDetections.size()); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + } // end for() loop + + // Add "key" information to telemetry + telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); + telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); + telemetry.addLine("RBE = Range, Bearing & Elevation"); + + } // end method telemetryAprilTag() + +} // end class \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limelightdefault.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limelightdefault.java new file mode 100644 index 000000000000..78a0b994be6c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limelightdefault.java @@ -0,0 +1,72 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="limetestdefault", group="Linear OpMode") +@Disabled +public class limelightdefault extends LinearOpMode { + + private Limelight3A limelight; + + @Override + public void runOpMode() throws InterruptedException + { + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + + telemetry.setMsTransmissionInterval(11); + + limelight.pipelineSwitch(0); + + /* + * Starts polling for data. + */ + limelight.start(); + + while (opModeIsActive()) { + LLResult result = limelight.getLatestResult(); + if (result != null) { + if (result.isValid()) { + Pose3D botpose = result.getBotpose(); + telemetry.addData("tx", result.getTx()); + telemetry.addData("ty", result.getTy()); + telemetry.addData("Botpose", botpose.toString()); + } + } + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limelighttest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limelighttest.java new file mode 100644 index 000000000000..5b2ce6a542d1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limelighttest.java @@ -0,0 +1,96 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; + +import java.util.List; + +/* + * This OpMode illustrates how to use the Limelight3A Vision Sensor. + * + * @see Limelight + * + * Notes on configuration: + * + * The device presents itself, when plugged into a USB port on a Control Hub as an ethernet + * interface. A DHCP server running on the Limelight automatically assigns the Control Hub an + * ip address for the new ethernet interface. + * + * Since the Limelight is plugged into a USB port, it will be listed on the top level configuration + * activity along with the Control Hub Portal and other USB devices such as webcams. Typically + * serial numbers are displayed below the device's names. In the case of the Limelight device, the + * Control Hub's assigned ip address for that ethernet interface is used as the "serial number". + * + * Tapping the Limelight's name, transitions to a new screen where the user can rename the Limelight + * and specify the Limelight's ip address. Users should take care not to confuse the ip address of + * the Limelight itself, which can be configured through the Limelight settings page via a web browser, + * and the ip address the Limelight device assigned the Control Hub and which is displayed in small text + * below the name of the Limelight on the top level configuration screen. + */ +@TeleOp(name = "Sensor: Limelight3A tag 22", group = "Testing") +//@Disabled +public class limelighttest extends LinearOpMode { + + private Limelight3A limelight; + private DcMotor rotate = null; + + @Override + public void runOpMode() throws InterruptedException + { + rotate = hardwareMap.get(DcMotor.class, "rotate"); + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + + telemetry.setMsTransmissionInterval(11); + + limelight.pipelineSwitch(8); + + /* + * Starts polling for data. If you neglect to call start(), getLatestResult() will return null. + */ + limelight.start(); + + telemetry.addData(">", "Robot Ready. Press Play."); + telemetry.update(); + waitForStart(); + + while (opModeIsActive()) { + LLStatus status = limelight.getStatus(); + telemetry.addData("Name", "%s", + status.getName()); + telemetry.addData("LL", "Temp: %.1fC, CPU: %.1f%%, FPS: %d", + status.getTemp(), status.getCpu(),(int)status.getFps()); + telemetry.addData("Pipeline", "Index: %d, Type: %s", + status.getPipelineIndex(), status.getPipelineType()); + + LLResult result = limelight.getLatestResult(); + + if (result != null && result.isValid()) { + double tx = result.getTx(); // Horizontal offset in degrees + + // Simple Proportional Controller + double kP = 0.02; // Tune this value! + double turnPower = tx * kP; + + // Set turret motor power + telemetry.addData("rotate", "%d",turnPower); + //rotate.setPower(turnPower); + } else { + // No target found, stop or search + + rotate.setPower(0); + } + + + telemetry.update(); + } + limelight.stop(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limetest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limetest.java new file mode 100644 index 000000000000..2b0c912f7ccc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limetest.java @@ -0,0 +1,95 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.Limelight3A; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; + +@TeleOp(name = "Limelight rotating working", group = "Vision") +public class limetest extends LinearOpMode { + + private DcMotor rotate; + private Limelight3A limelight; + + /* ===== TUNING ===== */ + private static final double kP = 0.01; + private static final double MAX_POWER = 0.25; + private static final double TX_DEADBAND = 1.0; + + @Override + public void runOpMode() { + + rotate = hardwareMap.get(DcMotor.class, "rotate"); + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + + rotate.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + telemetry.setMsTransmissionInterval(20); + + telemetry.addLine("Limelight init complete"); + telemetry.update(); + + limelight.pipelineSwitch(0); // AprilTag pipeline + limelight.start(); + + waitForStart(); + + while (opModeIsActive()) { + + double rotatePower = 0.0; + + LLResult result = limelight.getLatestResult(); + + telemetry.addLine("===== LIMELIGHT DEBUG ====="); + + if (result == null) { + telemetry.addLine("Result: NULL (no data yet)"); + } else { + telemetry.addData("Target Valid", result.isValid()); + telemetry.addData("tx (horizontal error)", result.getTx()); + telemetry.addData("ty (vertical error)", result.getTy()); + telemetry.addData("ta (target area)", result.getTa()); + + Pose3D botpose = result.getBotpose(); + if (botpose != null) { + telemetry.addData("BotPose", botpose.toString()); + } + + /* ===== AUTO TRACK ARM ===== */ + if (result.isValid()) { + double tx = result.getTx(); + + if (Math.abs(tx) > TX_DEADBAND) { + rotatePower = tx * kP; + } else { + rotatePower = 0.0; + } + + rotatePower = Math.max( + -MAX_POWER, + Math.min(MAX_POWER, rotatePower) + ); + } + } + + /* ===== MANUAL OVERRIDE ===== */ + if (gamepad1.dpad_up) { + rotatePower = 0.2; + telemetry.addLine("MANUAL: DPAD UP"); + } else if (gamepad1.dpad_down) { + rotatePower = -0.2; + telemetry.addLine("MANUAL: DPAD DOWN"); + } + + rotate.setPower(rotatePower); + + telemetry.addLine("===== ARM DEBUG ====="); + telemetry.addData("Rotate Power", rotatePower); + + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limetestexp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limetestexp.java new file mode 100644 index 000000000000..9324aad8990d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limetestexp.java @@ -0,0 +1,102 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +import java.util.List; + +@TeleOp(name = "Limelight: final", group = "Vision") +public class limetestexp extends LinearOpMode { + + private DcMotor rotate; + private Limelight3A limelight; + + /* ===== TUNING ===== */ + private static final double kP = 0.01; + private static final double MAX_POWER = 0.25; + private static final double TX_DEADBAND = 1.2; // Slightly wider to stop "hunting" + + // Low-Pass Filter: 0.1 means 10% new data, 90% old data. + // Lower = smoother but slower. Higher = faster but twitchier. + private static final double GAIN = 0.2; + private double smoothedTx = 0; + + private int targetTagID = 20; + private boolean lastLeftBumper = false; + private boolean lastRightBumper = false; + + @Override + public void runOpMode() { + rotate = hardwareMap.get(DcMotor.class, "rotate"); + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + + rotate.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + limelight.pipelineSwitch(0); + limelight.start(); + + waitForStart(); + + while (opModeIsActive()) { + double rotatePower = 0.0; + LLResult result = limelight.getLatestResult(); + + // 1. ID Selection + if (gamepad1.left_bumper && !lastLeftBumper) targetTagID = Math.max(20, targetTagID - 1); + if (gamepad1.right_bumper && !lastRightBumper) targetTagID = Math.min(25, targetTagID + 1); + lastLeftBumper = gamepad1.left_bumper; + lastRightBumper = gamepad1.right_bumper; + + boolean correctTagVisible = false; + double distanceZ = 0; + + if (result != null && result.isValid()) { + // Check if our specific ID is in the frame + List fiducials = result.getFiducialResults(); + for (LLResultTypes.FiducialResult fr : fiducials) { + if (fr.getFiducialId() == targetTagID) { + correctTagVisible = true; + Pose3D pose = fr.getRobotPoseTargetSpace(); + if (pose != null) distanceZ = Math.abs(pose.getPosition().z); + break; + } + } + + if (correctTagVisible) { + // 2. APPLY LOW-PASS FILTER + // smoothedTx = (current_reading * GAIN) + (previous_smoothed * (1 - GAIN)) + double rawTx = result.getTx(); + smoothedTx = (rawTx * GAIN) + (smoothedTx * (1.0 - GAIN)); + + // 3. STABLE CALCULATION + if (Math.abs(smoothedTx) > TX_DEADBAND) { + rotatePower = smoothedTx * kP; + } + + rotatePower = Math.max(-MAX_POWER, Math.min(MAX_POWER, rotatePower)); + } + } else { + // If we lose the tag, reset the filter so it doesn't "jump" when it reappears + smoothedTx = 0; + } + + /* ===== MANUAL OVERRIDE ===== */ + if (gamepad1.dpad_up) rotatePower = 0.2; + else if (gamepad1.dpad_down) rotatePower = -0.2; + + rotate.setPower(rotatePower); + + // Telemetry for monitoring the "Smoothing" + telemetry.addData("Selected ID", targetTagID); + telemetry.addData("Tag Visible", correctTagVisible); + telemetry.addData("Raw Tx", result != null ? result.getTx() : 0); + telemetry.addData("Smoothed Tx", "%.2f", smoothedTx); + telemetry.addData("Distance", "%.2f", distanceZ," m"); + telemetry.update(); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java index fa30420ac7cd..c2d9b2ae5394 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -3,17 +3,50 @@ import com.pedropathing.follower.Follower; import com.pedropathing.follower.FollowerConstants; import com.pedropathing.ftc.FollowerBuilder; +import com.pedropathing.ftc.drivetrains.MecanumConstants; +import com.pedropathing.ftc.localization.constants.PinpointConstants; import com.pedropathing.paths.PathConstraints; +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + public class Constants { - public static FollowerConstants followerConstants = new FollowerConstants(); + public static FollowerConstants followerConstants = new FollowerConstants() + .mass(5) // TODO insert + .forwardZeroPowerAcceleration(10) // TODO insert + .lateralZeroPowerAcceleration(10); // TODO insert + + public static MecanumConstants driveConstants = new MecanumConstants() + .maxPower(.7) + .rightFrontMotorName("rf") + .rightRearMotorName("rb") + .leftRearMotorName("lb") + .leftFrontMotorName("lf") + .leftFrontMotorDirection(DcMotorSimple.Direction.FORWARD) + .leftRearMotorDirection(DcMotorSimple.Direction.REVERSE) + .rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD) + .rightRearMotorDirection(DcMotorSimple.Direction.FORWARD) + .xVelocity(20) // tuned with a 10-inch test: repeat at higher distance for accuracy + .yVelocity(20); // TODO: lateral velocity tuning public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1); + public static PinpointConstants localizerConstants = new PinpointConstants() + .forwardPodY(-6) + .strafePodX(-8) + .distanceUnit(DistanceUnit.INCH) + .hardwareMapName("pinpoint") //TODO: rename to match hardware names + .encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD) + .forwardEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD) + .strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD); + public static Follower createFollower(HardwareMap hardwareMap) { return new FollowerBuilder(followerConstants, hardwareMap) .pathConstraints(pathConstraints) + .pinpointLocalizer(localizerConstants) + .mecanumDrivetrain(driveConstants) .build(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/RunCustomPath.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/RunCustomPath.java new file mode 100644 index 000000000000..871d302b692a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/RunCustomPath.java @@ -0,0 +1,209 @@ +package org.firstinspires.ftc.teamcode.pedroPathing; + +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.Path; +import com.pedropathing.paths.PathChain; +import com.pedropathing.util.Timer; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +@Autonomous(name = "Example Auto", group = "Examples") +public class RunCustomPath extends OpMode { + + private Follower follower; + + private Timer pathTimer, opmodeTimer; + private int pathState; + + // ========================= + // POSES + // ========================= + + private final Pose startPose = new Pose(28.5, 128, Math.toRadians(180)); + private final Pose scorePose = new Pose(60, 85, Math.toRadians(135)); + private final Pose pickup1Pose = new Pose(37, 121, Math.toRadians(0)); + private final Pose pickup2Pose = new Pose(43, 130, Math.toRadians(0)); + private final Pose pickup3Pose = new Pose(49, 135, Math.toRadians(0)); + + // ========================= + // PATHS + // ========================= + + private Path scorePreload; + private PathChain grabPickup1, scorePickup1; + private PathChain grabPickup2, scorePickup2; + private PathChain grabPickup3, scorePickup3; + + public void buildPaths() { + + // Straight line preload score + scorePreload = new Path(new BezierLine(startPose, scorePose)); + scorePreload.setLinearHeadingInterpolation( + startPose.getHeading(), + scorePose.getHeading() + ); + + grabPickup1 = follower.pathBuilder() + .addPath(new BezierLine(scorePose, pickup1Pose)) + .setLinearHeadingInterpolation( + scorePose.getHeading(), + pickup1Pose.getHeading() + ) + .build(); + + scorePickup1 = follower.pathBuilder() + .addPath(new BezierLine(pickup1Pose, scorePose)) + .setLinearHeadingInterpolation( + pickup1Pose.getHeading(), + scorePose.getHeading() + ) + .build(); + + grabPickup2 = follower.pathBuilder() + .addPath(new BezierLine(scorePose, pickup2Pose)) + .setLinearHeadingInterpolation( + scorePose.getHeading(), + pickup2Pose.getHeading() + ) + .build(); + + scorePickup2 = follower.pathBuilder() + .addPath(new BezierLine(pickup2Pose, scorePose)) + .setLinearHeadingInterpolation( + pickup2Pose.getHeading(), + scorePose.getHeading() + ) + .build(); + + grabPickup3 = follower.pathBuilder() + .addPath(new BezierLine(scorePose, pickup3Pose)) + .setLinearHeadingInterpolation( + scorePose.getHeading(), + pickup3Pose.getHeading() + ) + .build(); + + scorePickup3 = follower.pathBuilder() + .addPath(new BezierLine(pickup3Pose, scorePose)) + .setLinearHeadingInterpolation( + pickup3Pose.getHeading(), + scorePose.getHeading() + ) + .build(); + } + + // ========================= + // FSM (STATE MACHINE) + // ========================= + + public void autonomousPathUpdate() { + + switch (pathState) { + + case 0: + follower.followPath(scorePreload); + setPathState(1); + break; + + case 1: + if (!follower.isBusy()) { + follower.followPath(grabPickup1, true); + setPathState(2); + } + break; + + case 2: + if (!follower.isBusy()) { + follower.followPath(scorePickup1, true); + setPathState(3); + } + break; + + case 3: + if (!follower.isBusy()) { + follower.followPath(grabPickup2, true); + setPathState(4); + } + break; + + case 4: + if (!follower.isBusy()) { + follower.followPath(scorePickup2, true); + setPathState(5); + } + break; + + case 5: + if (!follower.isBusy()) { + follower.followPath(grabPickup3, true); + setPathState(6); + } + break; + + case 6: + if (!follower.isBusy()) { + follower.followPath(scorePickup3, true); + setPathState(7); + } + break; + + case 7: + if (!follower.isBusy()) { + setPathState(-1); // Stop + } + break; + } + } + + public void setPathState(int state) { + pathState = state; + pathTimer.resetTimer(); + } + + // ========================= + // OPMODE METHODS + // ========================= + + @Override + public void init() { + + pathTimer = new Timer(); + opmodeTimer = new Timer(); + + follower = Constants.createFollower(hardwareMap); + + buildPaths(); + + follower.setStartingPose(startPose); + + telemetry.addLine("Initialized"); + telemetry.update(); + } + + @Override + public void init_loop() {} + + @Override + public void start() { + opmodeTimer.resetTimer(); + setPathState(0); + } + + @Override + public void loop() { + + follower.update(); + autonomousPathUpdate(); + + telemetry.addData("Path State", pathState); + telemetry.addData("X", follower.getPose().getX()); + telemetry.addData("Y", follower.getPose().getY()); + telemetry.addData("Heading", Math.toDegrees(follower.getPose().getHeading())); + telemetry.update(); + } + + @Override + public void stop() {} +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/RunCustomPath2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/RunCustomPath2.java new file mode 100644 index 000000000000..f6d758fd0e2d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/RunCustomPath2.java @@ -0,0 +1,136 @@ +package org.firstinspires.ftc.teamcode.pedroPathing; + +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.Path; +import com.pedropathing.paths.PathChain; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +@Autonomous(name = "Four Path Auto", group = "Auto") +public class RunCustomPath2 extends OpMode { + + private Follower follower; + private int pathState; + + // ========================= + // POSES + // ========================= + + private final Pose startPose = new Pose(28.5, 128, Math.toRadians(180)); + + private final Pose scorePreloadPose = new Pose(50.674285714285716, 115.17142857142859, Math.toRadians(135)); + + private final Pose grab1Pose = new Pose(70.46285714285715, 126.76000000000002, Math.toRadians(0)); + + private final Pose path3Pose = new Pose(48.42857142857143, 136.72, Math.toRadians(135)); // tangential + + private final Pose path4Pose = new Pose(28.5, 128, Math.toRadians(180)); // tangential + + // ========================= + // PATHS + // ========================= + + private Path scorePreload; + private PathChain grab1, path3, path4; + + public void buildPaths() { + + // Path 1: Start → ScorePreload + scorePreload = new Path(new BezierLine(startPose, scorePreloadPose)); + scorePreload.setLinearHeadingInterpolation(startPose.getHeading(), scorePreloadPose.getHeading()); + + // Path 2: ScorePreload → Grab1 + grab1 = follower.pathBuilder() + .addPath(new BezierLine(scorePreloadPose, grab1Pose)) + .setLinearHeadingInterpolation(scorePreloadPose.getHeading(), grab1Pose.getHeading()) + .build(); + + // Path 3: Grab1 → Path3 + path3 = follower.pathBuilder() + .addPath(new BezierLine(grab1Pose, path3Pose)) + .setLinearHeadingInterpolation(grab1Pose.getHeading(), path3Pose.getHeading()) + .build(); + + // Path 4: Path3 → Path4 + path4 = follower.pathBuilder() + .addPath(new BezierLine(path3Pose, path4Pose)) + .setLinearHeadingInterpolation(path3Pose.getHeading(), path4Pose.getHeading()) + .build(); + } + + // ========================= + // FSM + // ========================= + + public void autonomousPathUpdate() { + switch (pathState) { + case 0: + follower.followPath(scorePreload); + pathState = 1; + break; + + case 1: + if (!follower.isBusy()) { + follower.followPath(grab1); + pathState = 2; + } + break; + + case 2: + if (!follower.isBusy()) { + follower.followPath(path3); + pathState = 3; + } + break; + + case 3: + if (!follower.isBusy()) { + follower.followPath(path4); + pathState = 4; + } + break; + + case 4: + if (!follower.isBusy()) { + pathState = -1; // stop + } + break; + } + } + + // ========================= + // OPMODE METHODS + // ========================= + + @Override + public void init() { + follower = Constants.createFollower(hardwareMap); + buildPaths(); + follower.setStartingPose(startPose); + + telemetry.addLine("Initialized"); + telemetry.update(); + } + + @Override + public void start() { + pathState = 0; + } + + @Override + public void loop() { + follower.update(); + autonomousPathUpdate(); + + telemetry.addData("State", pathState); + telemetry.addData("X", follower.getPose().getX()); + telemetry.addData("Y", follower.getPose().getY()); + telemetry.addData("Heading (deg)", Math.toDegrees(follower.getPose().getHeading())); + telemetry.update(); + } + + @Override + public void stop() {} +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/RunCustomPath3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/RunCustomPath3.java new file mode 100644 index 000000000000..6a239e2cbe6d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/RunCustomPath3.java @@ -0,0 +1,133 @@ + +package org.firstinspires.ftc.teamcode.pedroPathing; +import android.app.slice.SliceMetrics; + +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.bylazar.configurables.annotations.Configurable; +import com.bylazar.telemetry.TelemetryManager; +import com.bylazar.telemetry.PanelsTelemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import com.pedropathing.geometry.BezierCurve; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.follower.Follower; +import com.pedropathing.paths.PathChain; +import com.pedropathing.geometry.Pose; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; + +import java.util.function.Consumer; + +@Autonomous(name = "Pedro Pathing Autonomous", group = "Autonomous") +@Configurable // Panels +public class RunCustomPath3 extends OpMode { + private TelemetryManager panelsTelemetry; // Panels Telemetry instance + public Follower follower; // Pedro Pathing follower instance + private int pathState, limelightAngle; // Current autonomous path state (state machine) + private Paths paths; // Paths defined in the Paths class + + private static DcMotor rotate, intake, shooter; + private static Servo kicker, pusher, hood; + + + + private Limelight3A limelight; + + public void defineMechanisms() { + rotate = hardwareMap.get(DcMotor.class, "rotate"); + intake = hardwareMap.get(DcMotor.class, "intake"); + shooter = hardwareMap.get(DcMotor.class, "shooter"); + hood = hardwareMap.get(Servo.class, "hood"); // TODO add the hood servo to the config + kicker = hardwareMap.get(Servo.class, "kicker"); + pusher = hardwareMap.get(Servo.class, "pusher"); + + limelightAngle = 23; + } + + @Override + public void init() { + panelsTelemetry = PanelsTelemetry.INSTANCE.getTelemetry(); + + follower = Constants.createFollower(hardwareMap); + follower.setStartingPose(new Pose(64, 80, Math.toRadians(140))); + defineMechanisms(); + changePath(0); + + paths = new Paths(follower); // Build paths + + panelsTelemetry.debug("Status", "Initialized"); + panelsTelemetry.update(telemetry); + } + + @Override + public void loop() { + follower.update(); // Update Pedro Pathing + autonomousPathUpdate(); // Update autonomous state machine + + // Log values to Panels and Driver Station + panelsTelemetry.debug("Path State", pathState); + panelsTelemetry.debug("X", follower.getPose().getX()); + panelsTelemetry.debug("Y", follower.getPose().getY()); + panelsTelemetry.debug("Heading", follower.getPose().getHeading()); + panelsTelemetry.update(telemetry); + + if (!follower.isBusy()) { + return; + } + } + + + public static class Paths { + public PathChain loadpath; + public PathChain Path2; + + public Paths(Follower follower) { + loadpath = follower.pathBuilder().addPath( + new BezierCurve( + new Pose(64.000, 80.000), + new Pose(35.246, 62.217), + new Pose(40.000, 90.000) + ) + ).setLinearHeadingInterpolation(Math.toRadians(140), Math.toRadians(180)) + .addParametricCallback( + 0.8, + () -> intake.setPower(1.0) + ) + .addParametricCallback( + 1.0, + () -> intake.setPower(0.0) + ) + .build(); + + Path2 = follower.pathBuilder().addPath( + new BezierCurve( + new Pose(40.000, 90.000), + new Pose(66.903, 114.186), + new Pose(64.000, 80.000) + ) + ).setTangentHeadingInterpolation() + .setReversed() + + .build(); + } + + + } + + public void changePath(int new_path) { + pathState = new_path; + } + + public void autonomousPathUpdate() { + switch (pathState) { + case 0: + follower.followPath(paths.loadpath, true); + changePath(1); + break; + case 1: + follower.followPath(paths.Path2); + break; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/servoExamples.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/servoExamples.java new file mode 100644 index 000000000000..f159a2f1c9ac --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/servoExamples.java @@ -0,0 +1,22 @@ +//package org.firstinspires.ftc.teamcode; +// +//import com.qualcomm.robotcore.eventloop.opmode.OpMode; +//import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +// +//@TeleOp +//public class servoExamples extends OpMode { +// testBenchServo bench = new testBenchServo(); +// @Override +// public void init(){ +// bench.init(hardwareMap); +// } +// @Override +// public void loop(){ +// if (gamepad2.a){ +// bench.setServoPos(0); +// } +// else { +// bench.setServoPos(1.0); +// } +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/targetTag.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/targetTag.java new file mode 100644 index 000000000000..4c3ad2bb0a69 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/targetTag.java @@ -0,0 +1,161 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; + +import java.util.List; + +/** + * Edited for stable aiming: + * - Uses getFiducialId() and per-tag getTargetXDegrees()/getTargetYDegrees() + * - Adds deadband, hysteresis, low-pass filtering, and derivative damping + * - Stops motor when no tag or small error + */ +@TeleOp(name = "Sensor: Limelight3A (Tag 22 Stable)", group = "Testing") +public class targetTag extends LinearOpMode { + + // ---------- Tuning constants (feel free to adjust) ---------- + private static final int TARGET_ID = 22; // tag to track + private static final double kP = 0.010; // proportional gain + private static final double kD = 0.0025;// derivative (damping) + private static final double DEAD_BAND_DEG = 1.0; // no motion inside ±1.0° + private static final double HYSTERESIS_EXTRA = 0.5; // extra to re-engage after stop + private static final double MAX_POWER = 0.20; // absolute safety cap + private static final double MIN_EFF_POWER = 0.06; // min to overcome stiction (only outside deadband) + private static final double LPF_ALPHA = 0.35; // 0..1 (lower = more smoothing) + + private Limelight3A limelight; + private DcMotor rotate; + + // ---------- Controller state ---------- + private double prevErrorDeg = 0.0; + private double filtErrorDeg = 0.0; + private boolean wasDriving = false; + + @Override + public void runOpMode() throws InterruptedException { + rotate = hardwareMap.get(DcMotor.class, "rotate"); + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + + // One-time motor setup to prevent drift + rotate.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + rotate.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rotate.setDirection(DcMotor.Direction.FORWARD); // flip if the sign feels backward + + telemetry.setMsTransmissionInterval(11); + + // Select your AprilTag/fiducial pipeline index + limelight.pipelineSwitch(0); // make sure pipeline 0 is an AprilTag pipeline in the LL UI + + // Start polling the Limelight (required or getLatestResult() returns null) + limelight.start(); + + telemetry.addData(">", "Robot Ready. Press Play."); + telemetry.update(); + + waitForStart(); + + while (opModeIsActive()) { + // Status info (temp, CPU, FPS, active pipeline) + LLStatus status = limelight.getStatus(); + telemetry.addData("LL Name", "%s", status.getName()); + telemetry.addData("LL", "Temp: %.1fC, CPU: %.1f%%, FPS: %d", + status.getTemp(), status.getCpu(), (int) status.getFps()); + telemetry.addData("Pipeline", "Index: %d, Type: %s", + status.getPipelineIndex(), status.getPipelineType()); + + LLResult result = limelight.getLatestResult(); + boolean drivingThisCycle = false; + + if (result != null && result.isValid()) { + // Find your specific tag + List tags = result.getFiducialResults(); + LLResultTypes.FiducialResult target = null; + for (LLResultTypes.FiducialResult t : tags) { + if (t.getFiducialId() == TARGET_ID) { // correct accessor + target = t; + break; + } + } + + if (target != null) { + // Per-tag horizontal error (degrees). Do NOT call tag.getTx(). + double errorDeg = target.getTargetXDegrees(); + + // Low-pass filter to reduce jitter + filtErrorDeg = LPF_ALPHA * errorDeg + (1.0 - LPF_ALPHA) * filtErrorDeg; + + // Deadband + hysteresis: require a bigger error to re-engage after stopping + double engageBand = DEAD_BAND_DEG + (wasDriving ? 0.0 : HYSTERESIS_EXTRA); + + if (Math.abs(filtErrorDeg) >= engageBand) { + // Derivative on measurement for damping + double dErr = (filtErrorDeg - prevErrorDeg); + + double power = kP * filtErrorDeg + kD * dErr; + + // Clip and enforce minimum effective power + double sign = Math.signum(power); + power = Math.min(Math.abs(power), MAX_POWER); + power = Math.max(power, MIN_EFF_POWER) * sign; + + rotate.setPower(power); + drivingThisCycle = true; + + telemetry.addData("Aim(Tag " + TARGET_ID + ")", + "err=%.2f°, filt=%.2f°, d=%.3f, pwr=%.2f", + errorDeg, filtErrorDeg, dErr, power); + } else { + rotate.setPower(0.0); + } + + prevErrorDeg = filtErrorDeg; + + // Optional: tag details + telemetry.addData("Tag", "id=%d, family=%s", target.getFiducialId(), target.getFamily()); + telemetry.addData("Angles", "x=%.2f°, y=%.2f°", target.getTargetXDegrees(), target.getTargetYDegrees()); + telemetry.addData("Area", "%.3f", target.getTargetArea()); + + } else { + telemetry.addLine("Target tag not visible"); + rotate.setPower(0.0); + // Decay filter slowly so we don't kick when the tag reappears + filtErrorDeg *= 0.9; + prevErrorDeg = filtErrorDeg; + } + + // Optional: frame-level selected target (may be a different object than your TARGET_ID) + telemetry.addData("Primary tx", "%.2f°", result.getTx()); + + // Optional: pose and latency readouts + Pose3D botpose = result.getBotpose(); + double captureLatency = result.getCaptureLatency(); + double targetingLatency = result.getTargetingLatency(); + double parseLatency = result.getParseLatency(); + telemetry.addData("Botpose", botpose.toString()); + telemetry.addData("Latency", "proc=%.1f ms (parse=%.1f ms)", + (captureLatency + targetingLatency), parseLatency); + + } else { + telemetry.addLine("Limelight: No data available"); + rotate.setPower(0.0); + // Decay filter slowly so we don't kick when data returns + filtErrorDeg *= 0.9; + prevErrorDeg = filtErrorDeg; + } + + telemetry.update(); + wasDriving = drivingThisCycle; + } + + limelight.stop(); + rotate.setPower(0.0); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testBenchServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testBenchServo.java new file mode 100644 index 000000000000..1a3ae6d82203 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testBenchServo.java @@ -0,0 +1,34 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.CRServo; + +@TeleOp(name = "CR Servo Test", group = "Linear Opmode") +public class testBenchServo extends OpMode { + + private CRServo crServo; + + @Override + public void init() { + + crServo = hardwareMap.get(CRServo.class, "crservo"); + + // Make sure it stops at init + crServo.setPower(0.0); + } + + @Override + public void loop() { + + if (gamepad1.y) { + crServo.setPower(1.0); // Forward + } + else if (gamepad1.a) { + crServo.setPower(-1.0); // Reverse + } + else if (gamepad1.square) { + crServo.setPower(0.0); // Stop + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testintake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testintake.java new file mode 100644 index 000000000000..f9c3ecb48167 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testintake.java @@ -0,0 +1,178 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; + +import java.util.List; +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="testintake", group="Linear OpMode") +//@Disabled +public class testintake extends LinearOpMode { + + // Declare OpMode members for each of the 4 motors. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor leftFrontDrive = null; + // private DcMotor leftBackDrive = null; + private DcMotor rightFrontDrive = null; + private DcMotor rightBackDrive = null; + + private DcMotor leftBackDrive = null; + private DcMotor intake = null; + private DcMotor kicker = null; + private Limelight3A limelight; + private Servo servo; + + @Override + public void runOpMode() { + + // Initialize the hardware variables. Note that the strings used here must correspond + // to the names assigned during the robot configuration step on the DS or RC devices. + leftFrontDrive = hardwareMap.get(DcMotor.class, "left_front_drive"); + leftBackDrive = hardwareMap.get(DcMotor.class, "left_back_drive"); + rightFrontDrive = hardwareMap.get(DcMotor.class, "right_front_drive"); + rightBackDrive = hardwareMap.get(DcMotor.class, "right_back_drive"); + intake = hardwareMap.get(DcMotor.class, "intake"); + kicker = hardwareMap.get(DcMotor.class, "kicker"); + ////Arm2 = hardwareMap.get(DcMotor.class, "ArmMotor2"); + servo = hardwareMap.get(Servo.class, "servo1"); + // ######################################################################################## + // !!! IMPORTANT Drive Information. Test your motor directions. !!!!! + // ######################################################################################## + // Most robots need the motors on one side to be reversed to drive forward. + // The motor reversals shown here are for a "direct drive" robot (the wheels turn the same direction as the motor shaft) + // If your robot has additional gear reductions or uses a right-angled drive, it's important to ensure + // that your motors are turning in the correct direction. So, start out with the reversals here, BUT + // when you first test your robot, push the left joystick forward and observe the direction the wheels turn. + // Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward + // Keep testing until ALL the wheels move the robot forward when you push the left joystick forward. + leftFrontDrive.setDirection(DcMotor.Direction.REVERSE); + leftBackDrive.setDirection(DcMotor.Direction.REVERSE); + rightFrontDrive.setDirection(DcMotor.Direction.REVERSE); + rightBackDrive.setDirection(DcMotor.Direction.FORWARD); + + // Optional: set direction + intake.setDirection(DcMotor.Direction.FORWARD); + kicker.setDirection(DcMotor.Direction.FORWARD); + + // Highly recommended for REV Core Hex motors + intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + kicker.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + //intake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + ////Arm2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + + // Wait for the game to start (driver presses START) + telemetry.addData("Status", "Initialized"); + telemetry.update(); + telemetry.setMsTransmissionInterval(11); + waitForStart(); + runtime.reset(); + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + double max; + + // POV Mode uses left joystick to go forward & strafe, and right joystick to rotate. + double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value + double lateral = gamepad1.left_stick_x; + double yaw = gamepad1.right_stick_x; + + // Combine the joystick requests for each axis-motion to determine each wheel's power. + // Set up a variable for each drive wheel to save the power level for telemetry. + double leftFrontPower = axial + lateral + yaw; + double rightFrontPower = axial - lateral - yaw; + double leftBackPower = axial - lateral + yaw; + double rightBackPower = axial + lateral - yaw; + + // Normalize the values so no wheel power exceeds 100% + // This ensures that the robot maintains the desired motion. + max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)); + max = Math.max(max, Math.abs(leftBackPower)); + max = Math.max(max, Math.abs(rightBackPower)); + + if (max > 1.0) { + leftFrontPower /= max; + rightFrontPower /= max; + leftBackPower /= max; + rightBackPower /= max; + } + + /* + to find the power: + find difference in distance between starting point and where it wants to go + divide the current distance away over the total distance + set this as the power + */ + + // testing the intake and kicker + if (gamepad1.dpad_up) { + intake.setPower(1); // forward hi +// } else if (gamepad1.dpad_down) { +// intake.setPower(1); // reverse + } else if (gamepad1.dpad_down) { + intake.setPower(0); + } else if (gamepad1.dpad_right) { + kicker.setPower(-1); + } else if (gamepad1.dpad_left) { + kicker.setPower(0); + } + + // for competition; using a (X on our gamepad) inst4 + if (gamepad1.aWasPressed()) { + kicker.setPower(-1); + intake.setPower(0.9); + } else if (gamepad1.aWasReleased()) { + kicker.setPower(0); + intake.setPower(0); + + if (gamepad1.y) { + // servo1.setPosition(1.0); + } + if (gamepad1.x) { + servo.setPosition(0.0); + } + } + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testtest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testtest.java new file mode 100644 index 000000000000..a8d5843f7e5d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testtest.java @@ -0,0 +1,56 @@ + package org.firstinspires.ftc.teamcode; + + import androidx.annotation.NonNull; + +// import com.acmerobotics.dashboard.config.Config; +// import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +// import com.acmerobotics.roadrunner.Action; +// import com.acmerobotics.roadrunner.Pose2d; +// import com.acmerobotics.roadrunner.SequentialAction; +// import com.acmerobotics.roadrunner.Vector2d; +// import com.acmerobotics.roadrunner.ftc.Actions; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +@Autonomous(name = "FirstAuto", group = "Autonomous") +public class testtest extends LinearOpMode { + + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor leftFrontDrive = null; + private DcMotor rightFrontDrive = null; + private DcMotor rightBackDrive = null; + private DcMotor leftBackDrive = null; + private DcMotor IntakeM = null; + + public class Shooter { + /*public class startIntake() implements Action { + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + IntakeM.setPower(1); + return false; + } + + } + public Action StartIntake() { + return new startIntake(); + }*/ + + } + + public void runOpMode() { + + waitForStart(); + Shooter shooter = new Shooter(); + + if (isStopRequested()) return; + //Actions.runBlocking(shooter.StartIntake()); + + + } + +} + + diff --git a/TeamCode/teamcode/TargetTag23.java b/TeamCode/teamcode/TargetTag23.java new file mode 100644 index 000000000000..9670430e5c01 --- /dev/null +++ b/TeamCode/teamcode/TargetTag23.java @@ -0,0 +1,161 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; + +import java.util.List; + +/** + * Edited for stable aiming: + * - Uses getFiducialId() and per-tag getTargetXDegrees()/getTargetYDegrees() + * - Adds deadband, hysteresis, low-pass filtering, and derivative damping + * - Stops motor when no tag or small error + */ +@TeleOp(name = "Sensor: Limelight3A (Tag 23 Stable)", group = "Testing") +public class TargetTag extends LinearOpMode { + + // ---------- Tuning constants (feel free to adjust) ---------- + private static final int TARGET_ID = 23; // tag to track + private static final double kP = 0.020; // proportional gain + private static final double kD = 0.0025;// derivative (damping) + private static final double DEAD_BAND_DEG = 1.0; // no motion inside ±1.0° + private static final double HYSTERESIS_EXTRA = 0.5; // extra to re-engage after stop + private static final double MAX_POWER = 0.20; // absolute safety cap + private static final double MIN_EFF_POWER = 0.06; // min to overcome stiction (only outside deadband) + private static final double LPF_ALPHA = 0.35; // 0..1 (lower = more smoothing) + + private Limelight3A limelight; + private DcMotor rotate; + + // ---------- Controller state ---------- + private double prevErrorDeg = 0.0; + private double filtErrorDeg = 0.0; + private boolean wasDriving = false; + + @Override + public void runOpMode() throws InterruptedException { + rotate = hardwareMap.get(DcMotor.class, "rotate"); + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + + // One-time motor setup to prevent drift + rotate.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + rotate.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rotate.setDirection(DcMotor.Direction.FORWARD); // flip if the sign feels backward + + telemetry.setMsTransmissionInterval(11); + + // Select your AprilTag/fiducial pipeline index + limelight.pipelineSwitch(0); // make sure pipeline 0 is an AprilTag pipeline in the LL UI + + // Start polling the Limelight (required or getLatestResult() returns null) + limelight.start(); + + telemetry.addData(">", "Robot Ready. Press Play."); + telemetry.update(); + + waitForStart(); + + while (opModeIsActive()) { + // Status info (temp, CPU, FPS, active pipeline) + LLStatus status = limelight.getStatus(); + telemetry.addData("LL Name", "%s", status.getName()); + telemetry.addData("LL", "Temp: %.1fC, CPU: %.1f%%, FPS: %d", + status.getTemp(), status.getCpu(), (int) status.getFps()); + telemetry.addData("Pipeline", "Index: %d, Type: %s", + status.getPipelineIndex(), status.getPipelineType()); + + LLResult result = limelight.getLatestResult(); + boolean drivingThisCycle = false; + + if (result != null && result.isValid()) { + // Find your specific tag + List tags = result.getFiducialResults(); + LLResultTypes.FiducialResult target = null; + for (LLResultTypes.FiducialResult t : tags) { + if (t.getFiducialId() == TARGET_ID) { // correct accessor + target = t; + break; + } + } + + if (target != null) { + // Per-tag horizontal error (degrees). Do NOT call tag.getTx(). + double errorDeg = target.getTargetXDegrees(); + + // Low-pass filter to reduce jitter + filtErrorDeg = LPF_ALPHA * errorDeg + (1.0 - LPF_ALPHA) * filtErrorDeg; + + // Deadband + hysteresis: require a bigger error to re-engage after stopping + double engageBand = DEAD_BAND_DEG + (wasDriving ? 0.0 : HYSTERESIS_EXTRA); + + if (Math.abs(filtErrorDeg) >= engageBand) { + // Derivative on measurement for damping + double dErr = (filtErrorDeg - prevErrorDeg); + + double power = kP * filtErrorDeg + kD * dErr; + + // Clip and enforce minimum effective power + double sign = Math.signum(power); + power = Math.min(Math.abs(power), MAX_POWER); + power = Math.max(power, MIN_EFF_POWER) * sign; + + rotate.setPower(power); + drivingThisCycle = true; + + telemetry.addData("Aim(Tag " + TARGET_ID + ")", + "err=%.2f°, filt=%.2f°, d=%.3f, pwr=%.2f", + errorDeg, filtErrorDeg, dErr, power); + } else { + rotate.setPower(0.0); + } + + prevErrorDeg = filtErrorDeg; + + // Optional: tag details + telemetry.addData("Tag", "id=%d, family=%s", target.getFiducialId(), target.getFamily()); + telemetry.addData("Angles", "x=%.2f°, y=%.2f°", target.getTargetXDegrees(), target.getTargetYDegrees()); + telemetry.addData("Area", "%.3f", target.getTargetArea()); + + } else { + telemetry.addLine("Target tag not visible"); + rotate.setPower(0.0); + // Decay filter slowly so we don't kick when the tag reappears + filtErrorDeg *= 0.9; + prevErrorDeg = filtErrorDeg; + } + + // Optional: frame-level selected target (may be a different object than your TARGET_ID) + telemetry.addData("Primary tx", "%.2f°", result.getTx()); + + // Optional: pose and latency readouts + Pose3D botpose = result.getBotpose(); + double captureLatency = result.getCaptureLatency(); + double targetingLatency = result.getTargetingLatency(); + double parseLatency = result.getParseLatency(); + telemetry.addData("Botpose", botpose.toString()); + telemetry.addData("Latency", "proc=%.1f ms (parse=%.1f ms)", + (captureLatency + targetingLatency), parseLatency); + + } else { + telemetry.addLine("Limelight: No data available"); + rotate.setPower(0.0); + // Decay filter slowly so we don't kick when data returns + filtErrorDeg *= 0.9; + prevErrorDeg = filtErrorDeg; + } + + telemetry.update(); + wasDriving = drivingThisCycle; + } + + limelight.stop(); + rotate.setPower(0.0); + } +} diff --git a/TeamCode/teamcode/Teleop.java b/TeamCode/teamcode/Teleop.java new file mode 100644 index 000000000000..fa9f96a1f1bc --- /dev/null +++ b/TeamCode/teamcode/Teleop.java @@ -0,0 +1,89 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; + +import java.util.List; +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="limetestdefault", group="Linear OpMode") + +public class Teleop extends LinearOpMode { + + private Limelight3A limelight; + + @Override + public void runOpMode() throws InterruptedException + { + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + + telemetry.setMsTransmissionInterval(11); + limelight.setPollRateHz(100); + limelight.pipelineSwitch(0); + + + waitForStart(); + + /* + * Starts polling for data. + */ + limelight.start(); + + + telemetry.addData("ll3a status", limelight.isConnected()); + telemetry.update(); + + while (opModeIsActive()) { + LLResult result = limelight.getLatestResult(); + if (result != null) { + if (result.isValid()) { + Pose3D botpose = result.getBotpose(); + telemetry.addData("tx", result.getTx()); + telemetry.addData("ty", result.getTy()); + telemetry.addData("Botpose", botpose.toString()); + } + } + + } + } +} \ No newline at end of file diff --git a/TeamCode/teamcode/TestCar.java b/TeamCode/teamcode/TestCar.java new file mode 100644 index 000000000000..541ef2ed48c6 --- /dev/null +++ b/TeamCode/teamcode/TestCar.java @@ -0,0 +1,163 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="Test Car", group="Linear OpMode") +//@Disabled +public class TestCar extends LinearOpMode { + + // Declare OpMode members for each of the 4 motors. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor leftFrontDrive = null; + // private DcMotor leftBackDrive = null; + private DcMotor rightFrontDrive = null; + private DcMotor rightBackDrive = null; + + private DcMotor leftBackDrive = null; + + private DcMotor Arm1 = null; + ////private DcMotor Arm2 = null; + + @Override + public void runOpMode() { + + // Initialize the hardware variables. Note that the strings used here must correspond + // to the names assigned during the robot configuration step on the DS or RC devices. + leftFrontDrive = hardwareMap.get(DcMotor.class, "left_front_drive"); + leftBackDrive = hardwareMap.get(DcMotor.class, "left_back_drive"); + rightFrontDrive = hardwareMap.get(DcMotor.class, "right_front_drive"); + rightBackDrive = hardwareMap.get(DcMotor.class, "right_back_drive"); + Arm1 = hardwareMap.get(DcMotor.class, "ArmMotor1"); + ////Arm2 = hardwareMap.get(DcMotor.class, "ArmMotor2"); + + // ######################################################################################## + // !!! IMPORTANT Drive Information. Test your motor directions. !!!!! + // ######################################################################################## + // Most robots need the motors on one side to be reversed to drive forward. + // The motor reversals shown here are for a "direct drive" robot (the wheels turn the same direction as the motor shaft) + // If your robot has additional gear reductions or uses a right-angled drive, it's important to ensure + // that your motors are turning in the correct direction. So, start out with the reversals here, BUT + // when you first test your robot, push the left joystick forward and observe the direction the wheels turn. + // Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward + // Keep testing until ALL the wheels move the robot forward when you push the left joystick forward. + leftFrontDrive.setDirection(DcMotor.Direction.REVERSE); + leftBackDrive.setDirection(DcMotor.Direction.FORWARD); + rightFrontDrive.setDirection(DcMotor.Direction.REVERSE); + rightBackDrive.setDirection(DcMotor.Direction.FORWARD); + + + Arm1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + ////Arm2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + + // Wait for the game to start (driver presses START) + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + waitForStart(); + runtime.reset(); + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + double max; + + // POV Mode uses left joystick to go forward & strafe, and right joystick to rotate. + double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value + double lateral = gamepad1.left_stick_x; + double yaw = gamepad1.right_stick_x; + + // Combine the joystick requests for each axis-motion to determine each wheel's power. + // Set up a variable for each drive wheel to save the power level for telemetry. + double leftFrontPower = axial + lateral + yaw; + double rightFrontPower = axial - lateral - yaw; + double leftBackPower = axial - lateral + yaw; + double rightBackPower = axial + lateral - yaw; + + // Normalize the values so no wheel power exceeds 100% + // This ensures that the robot maintains the desired motion. + max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)); + max = Math.max(max, Math.abs(leftBackPower)); + max = Math.max(max, Math.abs(rightBackPower)); + + if (max > 1.0) { + leftFrontPower /= max; + rightFrontPower /= max; + leftBackPower /= max; + rightBackPower /= max; + } + + /* + to find the power: + find difference in distance between tarting point and where it wants to go + divide the current distance away over the total distance + set this as the power + */ + + if(gamepad2.dpad_up){ + Arm1.setPower(1); + } + else if (gamepad2.dpad_down){ + Arm1.setPower(1); + + }else{ + Arm1.setPower(0); + } + + // Send calculated power to wheels + leftFrontDrive.setPower(leftFrontPower); + rightFrontDrive.setPower(rightFrontPower); + leftBackDrive.setPower(leftBackPower); + rightBackDrive.setPower(rightBackPower); + + + //telemetry.addData("difference Test", differenceInArms); + //telemetry.addData("targetPosition Test", targetPosition); + // telemetry.addData("targetPositionArm2 Test", targetPositionArm2); + // telemetry.addData("zeroPositionArm1 Test", zeroPositionArm1); + // telemetry.addData("zeroPositionArm2 Test", zeroPositionArm2); + + // Show the elapsed game time and wheel power. +// telemetry.addData("Arm1 Test", Arm1.getCurrentPosition()); +// telemetry.addData("Arm2 Test", Arm2.getCurrentPosition()); + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); + telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); + telemetry.update(); + } + } +} diff --git a/TeamCode/teamcode/TestCar2.java b/TeamCode/teamcode/TestCar2.java new file mode 100644 index 000000000000..b3085cb16064 --- /dev/null +++ b/TeamCode/teamcode/TestCar2.java @@ -0,0 +1,222 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; + +import java.util.List; +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="Test Car2", group="Linear OpMode") +//@Disabled +public class TestCar2 extends LinearOpMode { + + // Declare OpMode members for each of the 4 motors. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor leftFrontDrive = null; + // private DcMotor leftBackDrive = null; + private DcMotor rightFrontDrive = null; + private DcMotor rightBackDrive = null; + + private DcMotor leftBackDrive = null; + private DcMotor intake = null; + private DcMotor kicker = null; + private Limelight3A limelight; + private Servo servo; + + @Override + public void runOpMode() { + + // Initialize the hardware variables. Note that the strings used here must correspond + // to the names assigned during the robot configuration step on the DS or RC devices. + leftFrontDrive = hardwareMap.get(DcMotor.class, "left_front_drive"); + leftBackDrive = hardwareMap.get(DcMotor.class, "left_back_drive"); + rightFrontDrive = hardwareMap.get(DcMotor.class, "right_front_drive"); + rightBackDrive = hardwareMap.get(DcMotor.class, "right_back_drive"); + intake = hardwareMap.get(DcMotor.class, "intake"); + kicker = hardwareMap.get(DcMotor.class, "kicker"); + ////Arm2 = hardwareMap.get(DcMotor.class, "ArmMotor2"); + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + servo = hardwareMap.get(Servo.class, "servo1"); + // ######################################################################################## + // !!! IMPORTANT Drive Information. Test your motor directions. !!!!! + // ######################################################################################## + // Most robots need the motors on one side to be reversed to drive forward. + // The motor reversals shown here are for a "direct drive" robot (the wheels turn the same direction as the motor shaft) + // If your robot has additional gear reductions or uses a right-angled drive, it's important to ensure + // that your motors are turning in the correct direction. So, start out with the reversals here, BUT + // when you first test your robot, push the left joystick forward and observe the direction the wheels turn. + // Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward + // Keep testing until ALL the wheels move the robot forward when you push the left joystick forward. + leftFrontDrive.setDirection(DcMotor.Direction.REVERSE); + leftBackDrive.setDirection(DcMotor.Direction.REVERSE); + rightFrontDrive.setDirection(DcMotor.Direction.REVERSE); + rightBackDrive.setDirection(DcMotor.Direction.FORWARD); + + // Optional: set direction + intake.setDirection(DcMotor.Direction.REVERSE); + kicker.setDirection(DcMotor.Direction.FORWARD); + + // Highly recommended for REV Core Hex motors + intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + kicker.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + //intake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + ////Arm2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + + // Wait for the game to start (driver presses START) + telemetry.addData("Status", "Initialized"); + telemetry.update(); + telemetry.setMsTransmissionInterval(11); + limelight.pipelineSwitch(0); + limelight.start(); + waitForStart(); + runtime.reset(); + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + double max; + + // POV Mode uses left joystick to go forward & strafe, and right joystick to rotate. + double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value + double lateral = gamepad1.left_stick_x; + double yaw = gamepad1.right_stick_x; + + // Combine the joystick requests for each axis-motion to determine each wheel's power. + // Set up a variable for each drive wheel to save the power level for telemetry. + double leftFrontPower = axial + lateral + yaw; + double rightFrontPower = axial - lateral - yaw; + double leftBackPower = axial - lateral + yaw; + double rightBackPower = axial + lateral - yaw; + + // Normalize the values so no wheel power exceeds 100% + // This ensures that the robot maintains the desired motion. + max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)); + max = Math.max(max, Math.abs(leftBackPower)); + max = Math.max(max, Math.abs(rightBackPower)); + + if (max > 1.0) { + leftFrontPower /= max; + rightFrontPower /= max; + leftBackPower /= max; + rightBackPower /= max; + } + + /* + to find the power: + find difference in distance between starting point and where it wants to go + divide the current distance away over the total distance + set this as the power + */ + + // testing the intake and kicker + if (gamepad1.dpad_up) { + intake.setPower(0.9); // forward hi +// } else if (gamepad1.dpad_down) { +// intake.setPower(1); // reverse + } else if (gamepad1.dpad_down){ + intake.setPower(0); + } else if (gamepad1.dpad_right){ + kicker.setPower(-1); + } else if (gamepad1.dpad_left){ + kicker.setPower(0); + } + + // for competition; using a (X on our gamepad) inst4 + if (gamepad1.aWasPressed()) { + kicker.setPower(-1); + intake.setPower(0.9); + } else if (gamepad1.aWasReleased()) { + kicker.setPower(0); + intake.setPower(0); + + if (gamepad1.y) { + servo.setPosition(1.0); + } + if (gamepad1.x) { + servo.setPosition(0.0); + } + telemetry.addData("Triangle(Y)", gamepad1.y); + telemetry.addData("Square (X)", gamepad1.x); + telemetry.addData("Servo Position", servo.getPosition()); + telemetry.update(); + } + LLResult result = limelight.getLatestResult(); + if (result != null) { + if (result.isValid()) { + Pose3D botpose = result.getBotpose(); + telemetry.addData("tx", result.getTx()); + telemetry.addData("ty", result.getTy()); + telemetry.addData("Botpose", botpose.toString()); +} +} // Send calculated power to wheels + leftFrontDrive.setPower(leftFrontPower); + rightFrontDrive.setPower(rightFrontPower); + leftBackDrive.setPower(leftBackPower); + rightBackDrive.setPower(rightBackPower); + + //telemetry.addData("difference Test", differenceInArms); + //telemetry.addData("targetPosition Test", targetPosition); + // telemetry.addData("targetPositionArm2 Test", targetPositionArm2); + // telemetry.addData("zeroPositionArm1 Test", zeroPositionArm1); + // telemetry.addData("zeroPositionArm2 Test", zeroPositionArm2); + + // Show the elapsed game time and wheel power. + telemetry.addData("leftFrontDrive", leftFrontPower); + telemetry.addData("rightFrontDrive", rightFrontPower); + telemetry.addData("leftBackDrive", leftBackPower); + telemetry.addData("rightBackDrive", rightBackPower); + //telemetry.addData("Status", "Run Time: " + runtime.toString()); + // telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); + //telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); + // telemetry.update(); + + if (isStopRequested()) { + return; + } + } + } +} + + + + diff --git a/TeamCode/teamcode/TestShooter.java b/TeamCode/teamcode/TestShooter.java new file mode 100644 index 000000000000..ef9b0ad39a97 --- /dev/null +++ b/TeamCode/teamcode/TestShooter.java @@ -0,0 +1,159 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="Test Shooter", group="Linear OpMode") +//@Disabled +public class TestShooter extends LinearOpMode { + + // Declare OpMode members for each of the 4 motors. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor leftFrontDrive = null; + // private DcMotor leftBackDrive = null; + private DcMotor rightFrontDrive = null; + private DcMotor rightBackDrive = null; + + private DcMotor leftBackDrive = null; + + private DcMotor LeftShooter = null; + // private DcMotor RightShooter = null; + + @Override + public void runOpMode() { + + // Initialize the hardware variables. Note that the strings used here must correspond + // to the names assigned during the robot configuration step on the DS or RC devices. + //leftFrontDrive = hardwareMap.get(DcMotor.class, "left_front_drive"); + //leftBackDrive = hardwareMap.get(DcMotor.class, "left_back_drive"); + //rightFrontDrive = hardwareMap.get(DcMotor.class, "right_front_drive"); + //rightBackDrive = hardwareMap.get(DcMotor.class, "right_back_drive"); + LeftShooter = hardwareMap.get(DcMotor.class, "LeftShooter"); + // RightShooter = hardwareMap.get(DcMotor.class, "RightShooter"); + + // ######################################################################################## + // !!! IMPORTANT Drive Information. Test your motor directions. !!!!! + // ######################################################################################## + // Most robots need the motors on one side to be reversed to drive forward. + // The motor reversals shown here are for a "direct drive" robot (the wheels turn the same direction as the motor shaft) + // If your robot has additional gear reductions or uses a right-angled drive, it's important to ensure + // that your motors are turning in the correct direction. So, start out with the reversals here, BUT + // when you first test your robot, push the left joystick forward and observe the direction the wheels turn. + // Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward + // Keep testing until ALL the wheels move the robot forward when you push the left joystick forward. + /*leftFrontDrive.setDirection(DcMotor.Direction.REVERSE); + leftBackDrive.setDirection(DcMotor.Direction.FORWARD); + rightFrontDrive.setDirection(DcMotor.Direction.REVERSE); + rightBackDrive.setDirection(DcMotor.Direction.FORWARD);*/ + + + LeftShooter.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + // RightShooter.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + LeftShooter.setDirection(DcMotor.Direction.REVERSE); + // RightShooter.setDirection(DcMotor.Direction.REVERSE); + + // Wait for the game to start (driver presses START) + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + waitForStart(); + runtime.reset(); + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + double max; + + // POV Mode uses left joystick to go forward & strafe, and right joystick to rotate. + double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value + double lateral = gamepad1.left_stick_x; + double yaw = gamepad1.right_stick_x; + + // Combine the joystick requests for each axis-motion to determine each wheel's power. + // Set up a variable for each drive wheel to save the power level for telemetry. + double leftFrontPower = axial + lateral + yaw; + double rightFrontPower = axial - lateral - yaw; + double leftBackPower = axial - lateral + yaw; + double rightBackPower = axial + lateral - yaw; + + // Normalize the values so no wheel power exceeds 100% + // This ensures that the robot maintains the desired motion. + max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)); + max = Math.max(max, Math.abs(leftBackPower)); + max = Math.max(max, Math.abs(rightBackPower)); + + if (max > 1.0) { + leftFrontPower /= max; + rightFrontPower /= max; + leftBackPower /= max; + rightBackPower /= max; + } + + /* + to find the power: + find difference in distance between tarting point and where it wants to go + divide the current distance away over the total distance + set this as the power + */ + + if(gamepad2.dpad_up){ + // RightShooter.setPower(1); + LeftShooter.setPower(.8); + } + else if (gamepad2.dpad_down){ + // RightShooter.setPower(0); + LeftShooter.setPower(0); + + } + + /*// Send calculated power to wheels + leftFrontDrive.setPower(leftFrontPower); + rightFrontDrive.setPower(rightFrontPower); + leftBackDrive.setPower(leftBackPower); + rightBackDrive.setPower(rightBackPower);*/ + + + //telemetry.addData("difference Test", differenceInArms); + //telemetry.addData("targetPosition Test", targetPosition); + // telemetry.addData("targetPositionArm2 Test", targetPositionArm2); + // telemetry.addData("zeroPositionArm1 Test", zeroPositionArm1); + // telemetry.addData("zeroPositionArm2 Test", zeroPositionArm2); + + // Show the elapsed game time and wheel power. + //telemetry.addData("Arm1 Test", LeftShooter.getCurrentPosition()); + // telemetry.addData("Arm2 Test", RightShooter.getCurrentPosition()); + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); + telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); + telemetry.update(); + } + } +} diff --git a/TeamCode/teamcode/Webcam.java b/TeamCode/teamcode/Webcam.java new file mode 100644 index 000000000000..faffcb3c1e61 --- /dev/null +++ b/TeamCode/teamcode/Webcam.java @@ -0,0 +1,217 @@ +/* Copyright (c) 2023 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import android.util.Size; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +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.List; + +/* + * This OpMode illustrates the basics of AprilTag recognition and pose estimation, + * including Java Builder structures for specifying Vision parameters. + * + * For an introduction to AprilTags, see the FTC-DOCS link below: + * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html + * + * In this sample, any visible tag ID will be detected and displayed, but only tags that are included in the default + * "TagLibrary" will have their position and orientation information displayed. This default TagLibrary contains + * the current Season's AprilTags and a small set of "test Tags" in the high number range. + * + * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera. + * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below. + * https://ftc-docs.firstinspires.org/apriltag-detection-values + * + * To experiment with using AprilTags to navigate, try out these two driving samples: + * RobotAutoDriveToAprilTagOmni and RobotAutoDriveToAprilTagTank + * + * There are many "default" VisionPortal and AprilTag configuration parameters that may be overridden if desired. + * These default parameters are shown as comments in the code below. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Concept: AprilTag", group = "Concept") +//@Disabled +public class Webcam extends LinearOpMode { + + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + /** + * The variable to store our instance of the AprilTag processor. + */ + private AprilTagProcessor aprilTag; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + @Override + public void runOpMode() { + + initAprilTag(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch START to start OpMode"); + telemetry.update(); + waitForStart(); + + if (opModeIsActive()) { + while (opModeIsActive()) { + + telemetryAprilTag(); + + // Push telemetry to the Driver Station. + telemetry.update(); + + // Save CPU resources; can resume streaming when needed. + if (gamepad1.dpad_down) { + visionPortal.stopStreaming(); + } else if (gamepad1.dpad_up) { + visionPortal.resumeStreaming(); + } + + // Share the CPU. + sleep(20); + } + } + + // Save more CPU resources when camera is no longer needed. + visionPortal.close(); + + } // end method runOpMode() + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + + // Create the AprilTag processor. + aprilTag = new AprilTagProcessor.Builder() + + // The following default settings are available to un-comment and edit as needed. + //.setDrawAxes(false) + //.setDrawCubeProjection(false) + //.setDrawTagOutline(true) + //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) + //.setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary()) + //.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) + + // == CAMERA CALIBRATION == + // If you do not manually specify calibration parameters, the SDK will attempt + // to load a predefined calibration for your camera. + //.setLensIntrinsics(578.272, 578.272, 402.145, 221.506) + // ... these parameters are fx, fy, cx, cy. + + .build(); + + // Adjust Image Decimation to trade-off detection-range for detection-rate. + // eg: Some typical detection data using a Logitech C920 WebCam + // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second + // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second + // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second (default) + // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second (default) + // Note: Decimation can be changed on-the-fly to adapt during a match. + //aprilTag.setDecimation(3); + + // Create the vision portal by using a builder. + VisionPortal.Builder builder = new VisionPortal.Builder(); + + // Set the camera (webcam vs. built-in RC phone camera). + if (USE_WEBCAM) { + builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")); + } else { + builder.setCamera(BuiltinCameraDirection.BACK); + } + + // Choose a camera resolution. Not all cameras support all resolutions. + //builder.setCameraResolution(new Size(640, 480)); + + // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. + //builder.enableLiveView(true); + + // Set the stream format; MJPEG uses less bandwidth than default YUY2. + //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); + + // Choose whether or not LiveView stops if no processors are enabled. + // If set "true", monitor shows solid orange screen if no processors enabled. + // If set "false", monitor shows camera view without annotations. + //builder.setAutoStopLiveView(false); + + // Set and enable the processor. + builder.addProcessor(aprilTag); + + // Build the Vision Portal, using the above settings. + visionPortal = builder.build(); + + // Disable or re-enable the aprilTag processor at any time. + //visionPortal.setProcessorEnabled(aprilTag, true); + + } // end method initAprilTag() + + + /** + * Add telemetry about AprilTag detections. + */ + private void telemetryAprilTag() { + + List currentDetections = aprilTag.getDetections(); + telemetry.addData("# AprilTags Detected", currentDetections.size()); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + } // end for() loop + + // Add "key" information to telemetry + telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); + telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); + telemetry.addLine("RBE = Range, Bearing & Elevation"); + + } // end method telemetryAprilTag() + +} // end class \ No newline at end of file diff --git a/TeamCode/teamcode/Webcam2.java b/TeamCode/teamcode/Webcam2.java new file mode 100644 index 000000000000..4312e500ae60 --- /dev/null +++ b/TeamCode/teamcode/Webcam2.java @@ -0,0 +1,216 @@ +/* Copyright (c) 2023 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +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.List; + +/* + * This OpMode illustrates the basics of AprilTag recognition and pose estimation, + * including Java Builder structures for specifying Vision parameters. + * + * For an introduction to AprilTags, see the FTC-DOCS link below: + * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html + * + * In this sample, any visible tag ID will be detected and displayed, but only tags that are included in the default + * "TagLibrary" will have their position and orientation information displayed. This default TagLibrary contains + * the current Season's AprilTags and a small set of "test Tags" in the high number range. + * + * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera. + * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below. + * https://ftc-docs.firstinspires.org/apriltag-detection-values + * + * To experiment with using AprilTags to navigate, try out these two driving samples: + * RobotAutoDriveToAprilTagOmni and RobotAutoDriveToAprilTagTank + * + * There are many "default" VisionPortal and AprilTag configuration parameters that may be overridden if desired. + * These default parameters are shown as comments in the code below. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Concept: AprilTag", group = "Concept") +//@Disabled +public class Webcam2 extends LinearOpMode { + + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + /** + * The variable to store our instance of the AprilTag processor. + */ + private AprilTagProcessor aprilTag; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + @Override + public void runOpMode() { + + initAprilTag(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch START to start OpMode"); + telemetry.update(); + waitForStart(); + + if (opModeIsActive()) { + while (opModeIsActive()) { + + telemetryAprilTag(); + + // Push telemetry to the Driver Station. + telemetry.update(); + + // Save CPU resources; can resume streaming when needed. + if (gamepad1.dpad_down) { + visionPortal.stopStreaming(); + } else if (gamepad1.dpad_up) { + visionPortal.resumeStreaming(); + } + + // Share the CPU. + sleep(20); + } + } + + // Save more CPU resources when camera is no longer needed. + visionPortal.close(); + + } // end method runOpMode() + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + + // Create the AprilTag processor. + aprilTag = new AprilTagProcessor.Builder() + + // The following default settings are available to un-comment and edit as needed. + //.setDrawAxes(false) + //.setDrawCubeProjection(false) + //.setDrawTagOutline(true) + //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) + //.setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary()) + //.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) + + // == CAMERA CALIBRATION == + // If you do not manually specify calibration parameters, the SDK will attempt + // to load a predefined calibration for your camera. + //.setLensIntrinsics(578.272, 578.272, 402.145, 221.506) + // ... these parameters are fx, fy, cx, cy. + + .build(); + + // Adjust Image Decimation to trade-off detection-range for detection-rate. + // eg: Some typical detection data using a Logitech C920 WebCam + // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second + // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second + // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second (default) + // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second (default) + // Note: Decimation can be changed on-the-fly to adapt during a match. + //aprilTag.setDecimation(3); + + // Create the vision portal by using a builder. + VisionPortal.Builder builder = new VisionPortal.Builder(); + + // Set the camera (webcam vs. built-in RC phone camera). + if (USE_WEBCAM) { + builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")); + } else { + builder.setCamera(BuiltinCameraDirection.BACK); + } + + // Choose a camera resolution. Not all cameras support all resolutions. + //builder.setCameraResolution(new Size(640, 480)); + + // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. + //builder.enableLiveView(true); + + // Set the stream format; MJPEG uses less bandwidth than default YUY2. + //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); + + // Choose whether or not LiveView stops if no processors are enabled. + // If set "true", monitor shows solid orange screen if no processors enabled. + // If set "false", monitor shows camera view without annotations. + //builder.setAutoStopLiveView(false); + + // Set and enable the processor. + builder.addProcessor(aprilTag); + + // Build the Vision Portal, using the above settings. + visionPortal = builder.build(); + + // Disable or re-enable the aprilTag processor at any time. + //visionPortal.setProcessorEnabled(aprilTag, true); + + } // end method initAprilTag() + + + /** + * Add telemetry about AprilTag detections. + */ + private void telemetryAprilTag() { + + List currentDetections = aprilTag.getDetections(); + telemetry.addData("# AprilTags Detected", currentDetections.size()); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + } // end for() loop + + // Add "key" information to telemetry + telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); + telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); + telemetry.addLine("RBE = Range, Bearing & Elevation"); + + } // end method telemetryAprilTag() + +} // end class \ No newline at end of file diff --git a/TeamCode/teamcode/limelightdefault.java b/TeamCode/teamcode/limelightdefault.java new file mode 100644 index 000000000000..4b0e00d6af32 --- /dev/null +++ b/TeamCode/teamcode/limelightdefault.java @@ -0,0 +1,71 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="limetestdefault", group="Linear OpMode") +//@Disabled +public class limelightdefault extends LinearOpMode { + + private Limelight3A limelight; + + @Override + public void runOpMode() throws InterruptedException + { + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + + telemetry.setMsTransmissionInterval(11); + + limelight.pipelineSwitch(0); + + /* + * Starts polling for data. + */ + limelight.start(); + + while (opModeIsActive()) { + LLResult result = limelight.getLatestResult(); + if (result != null) { + if (result.isValid()) { + Pose3D botpose = result.getBotpose(); + telemetry.addData("tx", result.getTx()); + telemetry.addData("ty", result.getTy()); + telemetry.addData("Botpose", botpose.toString()); + } + } + } + } +} diff --git a/TeamCode/teamcode/limelighttest.java b/TeamCode/teamcode/limelighttest.java new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/TeamCode/teamcode/limetest.java b/TeamCode/teamcode/limetest.java new file mode 100644 index 000000000000..8fd63b1c5d1e --- /dev/null +++ b/TeamCode/teamcode/limetest.java @@ -0,0 +1,168 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; + +import java.util.List; +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="limetest", group="Linear OpMode") +//@Disabled +public class limetest extends LinearOpMode { + + // Declare OpMode members for each of the 4 motors. + private ElapsedTime runtime = new ElapsedTime(); + + private DcMotor rotate = null; + // private DcMotor leftBackDrive = null; + + private Limelight3A limelight; + + + @Override + public void runOpMode() { + + // Initialize the hardware variables. Note that the strings used here must correspond + // to the names assigned during the robot configuration step on the DS or RC devices. + + rotate = hardwareMap.get(DcMotor.class, "rotate"); + ////Arm2 = hardwareMap.get(DcMotor.class, "ArmMotor2"); + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + // ######################################################################################## + // !!! IMPORTANT Drive Information. Test your motor directions. !!!!! + // ######################################################################################## + // Most robots need the motors on one side to be reversed to drive forward. + // The motor reversals shown here are for a "direct drive" robot (the wheels turn the same direction as the motor shaft) + // If your robot has additional gear reductions or uses a right-angled drive, it's important to ensure + // that your motors are turning in the correct direction. So, start out with the reversals here, BUT + // when you first test your robot, push the left joystick forward and observe the direction the wheels turn. + // Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward + // Keep testing until ALL the wheels move the robot forward when you push the left joystick forward. + + + + // Wait for the game to start (driver presses START) + telemetry.addData("Status", "Initialized"); + telemetry.update(); + telemetry.setMsTransmissionInterval(11); + limelight.pipelineSwitch(0); + limelight.start(); + waitForStart(); + runtime.reset(); + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + // testing the intake and kicker + if (gamepad1.dpad_up) { + rotate.setPower(0.8); // forward hi +// } else if (gamepad1.dpad_down) { +// intake.setPower(1); // reverse + } else if (gamepad1.dpad_down){ + rotate.setPower(-0.8); + } + else{ + rotate.setPower(0); + } +// else if (gamepad1.dpad_right){ +// kicker.setPower(-1); +// } else if (gamepad1.dpad_left){ +// kicker.setPower(0); +// } +// +// // for competition; using a (X on our gamepad) inst4 +// if (gamepad1.aWasPressed()) { +// kicker.setPower(-1); +// intake.setPower(0.9); +// } else if (gamepad1.aWasReleased()) { +// kicker.setPower(0); +// intake.setPower(0); +// +// if (gamepad1.y) { +// servo.setPosition(1.0); +// } +// if (gamepad1.x) { +// servo.setPosition(0.0); +// } +// telemetry.addData("Triangle(Y)", gamepad1.y); +// telemetry.addData("Square (X)", gamepad1.x); +// telemetry.addData("Servo Position", servo.getPosition()); +// telemetry.update(); + } + LLResult result = limelight.getLatestResult(); + if (result != null) { + if (result.isValid()) { + Pose3D botpose = result.getBotpose(); + telemetry.addData("power", 0.1); + telemetry.addData("tx", result.getTx()); + telemetry.addData("ty", result.getTy()); + telemetry.addData("Botpose", botpose.toString()); + } + } // Send calculated power to wheels + + + //telemetry.addData("difference Test", differenceInArms); + //telemetry.addData("targetPosition Test", targetPosition); + // telemetry.addData("targetPositionArm2 Test", targetPositionArm2); + // telemetry.addData("zeroPositionArm1 Test", zeroPositionArm1); + // telemetry.addData("zeroPositionArm2 Test", zeroPositionArm2); + + // Show the elapsed game time and wheel power. +// telemetry.addData("leftFrontDrive", leftFrontPower); +// telemetry.addData("rightFrontDrive", rightFrontPower); +// telemetry.addData("leftBackDrive", leftBackPower); +// telemetry.addData("rightBackDrive", rightBackPower); + //telemetry.addData("Status", "Run Time: " + runtime.toString()); + // telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); + //telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); + telemetry.update(); + + if (isStopRequested()) { + return; + } + } + } + + + + + diff --git a/TeamCode/teamcode/readme.md b/TeamCode/teamcode/readme.md new file mode 100644 index 000000000000..4d1da42de0c0 --- /dev/null +++ b/TeamCode/teamcode/readme.md @@ -0,0 +1,131 @@ +## TeamCode Module + +Welcome! + +This module, TeamCode, is the place where you will write/paste the code for your team's +robot controller App. This module is currently empty (a clean slate) but the +process for adding OpModes is straightforward. + +## Creating your own OpModes + +The easiest way to create your own OpMode is to copy a Sample OpMode and make it your own. + +Sample opmodes exist in the FtcRobotController module. +To locate these samples, find the FtcRobotController module in the "Project/Android" tab. + +Expand the following tree elements: + FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples + +### Naming of Samples + +To gain a better understanding of how the samples are organized, and how to interpret the +naming system, it will help to understand the conventions that were used during their creation. + +These conventions are described (in detail) in the sample_conventions.md file in this folder. + +To summarize: A range of different samples classes will reside in the java/external/samples. +The class names will follow a naming convention which indicates the purpose of each class. +The prefix of the name will be one of the following: + +Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure + of a particular style of OpMode. These are bare bones examples. + +Sensor: This is a Sample OpMode that shows how to use a specific sensor. + It is not intended to drive a functioning robot, it is simply showing the minimal code + required to read and display the sensor values. + +Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base. + It may be used to provide a common baseline driving OpMode, or + to demonstrate how a particular sensor or concept can be used to navigate. + +Concept: This is a sample OpMode that illustrates performing a specific function or concept. + These may be complex, but their operation should be explained clearly in the comments, + or the comments should reference an external doc, guide or tutorial. + Each OpMode should try to only demonstrate a single concept so they are easy to + locate based on their name. These OpModes may not produce a drivable robot. + +After the prefix, other conventions will apply: + +* Sensor class names are constructed as: Sensor - Company - Type +* Robot class names are constructed as: Robot - Mode - Action - OpModetype +* Concept class names are constructed as: Concept - Topic - OpModetype + +Once you are familiar with the range of samples available, you can choose one to be the +basis for your own robot. In all cases, the desired sample(s) needs to be copied into +your TeamCode module to be used. + +This is done inside Android Studio directly, using the following steps: + + 1) Locate the desired sample class in the Project/Android tree. + + 2) Right click on the sample class and select "Copy" + + 3) Expand the TeamCode/java folder + + 4) Right click on the org.firstinspires.ftc.teamcode folder and select "Paste" + + 5) You will be prompted for a class name for the copy. + Choose something meaningful based on the purpose of this class. + Start with a capital letter, and remember that there may be more similar classes later. + +Once your copy has been created, you should prepare it for use on your robot. +This is done by adjusting the OpMode's name, and enabling it to be displayed on the +Driver Station's OpMode list. + +Each OpMode sample class begins with several lines of code like the ones shown below: + +``` + @TeleOp(name="Template: Linear OpMode", group="Linear Opmode") + @Disabled +``` + +The name that will appear on the driver station's "opmode list" is defined by the code: + ``name="Template: Linear OpMode"`` +You can change what appears between the quotes to better describe your opmode. +The "group=" portion of the code can be used to help organize your list of OpModes. + +As shown, the current OpMode will NOT appear on the driver station's OpMode list because of the + ``@Disabled`` annotation which has been included. +This line can simply be deleted , or commented out, to make the OpMode visible. + + + +## ADVANCED Multi-Team App management: Cloning the TeamCode Module + +In some situations, you have multiple teams in your club and you want them to all share +a common code organization, with each being able to *see* the others code but each having +their own team module with their own code that they maintain themselves. + +In this situation, you might wish to clone the TeamCode module, once for each of these teams. +Each of the clones would then appear along side each other in the Android Studio module list, +together with the FtcRobotController module (and the original TeamCode module). + +Selective Team phones can then be programmed by selecting the desired Module from the pulldown list +prior to clicking to the green Run arrow. + +Warning: This is not for the inexperienced Software developer. +You will need to be comfortable with File manipulations and managing Android Studio Modules. +These changes are performed OUTSIDE of Android Studios, so close Android Studios before you do this. + +Also.. Make a full project backup before you start this :) + +To clone TeamCode, do the following: + +Note: Some names start with "Team" and others start with "team". This is intentional. + +1) Using your operating system file management tools, copy the whole "TeamCode" + folder to a sibling folder with a corresponding new name, eg: "Team0417". + +2) In the new Team0417 folder, delete the TeamCode.iml file. + +3) the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder + to a matching name with a lowercase 'team' eg: "team0417". + +4) In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that contains + package="org.firstinspires.ftc.teamcode" + to be + package="org.firstinspires.ftc.team0417" + +5) Add: include ':Team0417' to the "/settings.gradle" file. + +6) Open up Android Studios and clean out any old files by using the menu to "Build/Clean Project"" \ No newline at end of file diff --git a/TeamCode/teamcode/servoexamples.java b/TeamCode/teamcode/servoexamples.java new file mode 100644 index 000000000000..0e305710e62a --- /dev/null +++ b/TeamCode/teamcode/servoexamples.java @@ -0,0 +1,17 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +@TeleOp +public class servoExamples extends OpMode { + testBenchServo bench = new testBenchServo(); + @Override + public void init(){ + bench.init(); + } + @Override + public void loop(){ + + } +} diff --git a/TeamCode/teamcode/testBenchServo.java b/TeamCode/teamcode/testBenchServo.java new file mode 100644 index 000000000000..e38a7f634c28 --- /dev/null +++ b/TeamCode/teamcode/testBenchServo.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; +public class testBenchServo { + private Servo servoPos; + + public void init(HardwareMap hwMap){ + servoPos = hwMap.get(Servo.class, "Servo1"); + } + + public void setServoPos(double angle) { + servoPos.setPosition(angle); + } +} diff --git a/TeamCode/teamcode/testintake.java b/TeamCode/teamcode/testintake.java new file mode 100644 index 000000000000..c4d4dd5543e1 --- /dev/null +++ b/TeamCode/teamcode/testintake.java @@ -0,0 +1,178 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; + +import java.util.List; +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="testintake", group="Linear OpMode") +//@Disabled +public class testintake extends LinearOpMode { + + // Declare OpMode members for each of the 4 motors. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor leftFrontDrive = null; + // private DcMotor leftBackDrive = null; + private DcMotor rightFrontDrive = null; + private DcMotor rightBackDrive = null; + + private DcMotor leftBackDrive = null; + private DcMotor intake = null; + private DcMotor kicker = null; + private Limelight3A limelight; + private Servo servo; + + @Override + public void runOpMode() { + + // Initialize the hardware variables. Note that the strings used here must correspond + // to the names assigned during the robot configuration step on the DS or RC devices. + leftFrontDrive = hardwareMap.get(DcMotor.class, "left_front_drive"); + leftBackDrive = hardwareMap.get(DcMotor.class, "left_back_drive"); + rightFrontDrive = hardwareMap.get(DcMotor.class, "right_front_drive"); + rightBackDrive = hardwareMap.get(DcMotor.class, "right_back_drive"); + intake = hardwareMap.get(DcMotor.class, "intake"); + kicker = hardwareMap.get(DcMotor.class, "kicker"); + ////Arm2 = hardwareMap.get(DcMotor.class, "ArmMotor2"); + servo = hardwareMap.get(Servo.class, "servo1"); + // ######################################################################################## + // !!! IMPORTANT Drive Information. Test your motor directions. !!!!! + // ######################################################################################## + // Most robots need the motors on one side to be reversed to drive forward. + // The motor reversals shown here are for a "direct drive" robot (the wheels turn the same direction as the motor shaft) + // If your robot has additional gear reductions or uses a right-angled drive, it's important to ensure + // that your motors are turning in the correct direction. So, start out with the reversals here, BUT + // when you first test your robot, push the left joystick forward and observe the direction the wheels turn. + // Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward + // Keep testing until ALL the wheels move the robot forward when you push the left joystick forward. + leftFrontDrive.setDirection(DcMotor.Direction.REVERSE); + leftBackDrive.setDirection(DcMotor.Direction.REVERSE); + rightFrontDrive.setDirection(DcMotor.Direction.REVERSE); + rightBackDrive.setDirection(DcMotor.Direction.FORWARD); + + // Optional: set direction + intake.setDirection(DcMotor.Direction.FORWARD); + kicker.setDirection(DcMotor.Direction.FORWARD); + + // Highly recommended for REV Core Hex motors + intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + kicker.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + //intake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + ////Arm2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + + // Wait for the game to start (driver presses START) + telemetry.addData("Status", "Initialized"); + telemetry.update(); + telemetry.setMsTransmissionInterval(11); + waitForStart(); + runtime.reset(); + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + double max; + + // POV Mode uses left joystick to go forward & strafe, and right joystick to rotate. + double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value + double lateral = gamepad1.left_stick_x; + double yaw = gamepad1.right_stick_x; + + // Combine the joystick requests for each axis-motion to determine each wheel's power. + // Set up a variable for each drive wheel to save the power level for telemetry. + double leftFrontPower = axial + lateral + yaw; + double rightFrontPower = axial - lateral - yaw; + double leftBackPower = axial - lateral + yaw; + double rightBackPower = axial + lateral - yaw; + + // Normalize the values so no wheel power exceeds 100% + // This ensures that the robot maintains the desired motion. + max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)); + max = Math.max(max, Math.abs(leftBackPower)); + max = Math.max(max, Math.abs(rightBackPower)); + + if (max > 1.0) { + leftFrontPower /= max; + rightFrontPower /= max; + leftBackPower /= max; + rightBackPower /= max; + } + + /* + to find the power: + find difference in distance between starting point and where it wants to go + divide the current distance away over the total distance + set this as the power + */ + + // testing the intake and kicker + if (gamepad1.dpad_up) { + intake.setPower(1); // forward hi +// } else if (gamepad1.dpad_down) { +// intake.setPower(1); // reverse + } else if (gamepad1.dpad_down) { + intake.setPower(0); + } else if (gamepad1.dpad_right) { + kicker.setPower(-1); + } else if (gamepad1.dpad_left) { + kicker.setPower(0); + } + + // for competition; using a (X on our gamepad) inst4 + if (gamepad1.aWasPressed()) { + kicker.setPower(-1); + intake.setPower(0.9); + } else if (gamepad1.aWasReleased()) { + kicker.setPower(0); + intake.setPower(0); + + if (gamepad1.y) { + servo.setPosition(1.0); + } + if (gamepad1.x) { + servo.setPosition(0.0); + } + } + } + } +} diff --git a/TeamCode/teamcode/testtest.java b/TeamCode/teamcode/testtest.java new file mode 100644 index 000000000000..35ec60c66cb7 --- /dev/null +++ b/TeamCode/teamcode/testtest.java @@ -0,0 +1,56 @@ + package org.firstinspires.ftc.teamcode; + + import androidx.annotation.NonNull; + + import com.acmerobotics.dashboard.config.Config; + import com.acmerobotics.dashboard.telemetry.TelemetryPacket; + import com.acmerobotics.roadrunner.Action; + import com.acmerobotics.roadrunner.Pose2d; + import com.acmerobotics.roadrunner.SequentialAction; + import com.acmerobotics.roadrunner.Vector2d; + import com.acmerobotics.roadrunner.ftc.Actions; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +@Autonomous(name = "FirstAuto", group = "Autonomous") +public class testtest extends LinearOpMode { + + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor leftFrontDrive = null; + private DcMotor rightFrontDrive = null; + private DcMotor rightBackDrive = null; + private DcMotor leftBackDrive = null; + private DcMotor IntakeM = null; + + public class Shooter { + /*public class startIntake() implements Action { + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + IntakeM.setPower(1); + return false; + } + + } + public Action StartIntake() { + return new startIntake(); + }*/ + + } + + public void runOpMode() { + + waitForStart(); + Shooter shooter = new Shooter(); + + if (isStopRequested()) return; + //Actions.runBlocking(shooter.StartIntake()); + + + } + +} + +