SCDS/FtcRobotControllerSDK
7
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Complete refactoring of classes; added additional demo for 04/20/2024 event at SRJC

Browse Source
This commit is contained in:
Carlos
2024-04-20 10:45:09 -07:00
parent 832de95fc1
commit 9e8609e68d
8 changed files with 345 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsCameraTest.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsCameraDemo.java
View File

@ -11,8 +11,8 @@ import org.firstinspires.ftc.vision.VisionPortal;
import java.util.Locale;
@TeleOp(name = "DevApp - Camera Test (HDMI)", group = "SCDS Development")
public class CometBotsCameraTest extends LinearOpMode {
@TeleOp(name = "CometBots Camera Demo (HDMI)", group = "[14493] CometBots")
public class CometBotsCameraDemo extends LinearOpMode {
final boolean USING_WEBCAM = true;
final BuiltinCameraDirection INTERNAL_CAM_DIR = BuiltinCameraDirection.FRONT;
@ -34,7 +34,7 @@ public class CometBotsCameraTest extends LinearOpMode {
if (USING_WEBCAM)
{
portal = new VisionPortal.Builder()
.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
.setCamera(hardwareMap.get(WebcamName.class, "watch out"))
.setCameraResolution(new Size(RESOLUTION_WIDTH, RESOLUTION_HEIGHT))
.build();
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsIMUTest.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsIMUDemo.java
View File

@ -9,8 +9,8 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
@TeleOp(name = "DevApp - IMU Test", group = "SCDS Development")
public class CometBotsIMUTest extends LinearOpMode {
@TeleOp(name = "CometBots IMU Demo", group = "[14493] CometBots")
public class CometBotsIMUDemo extends LinearOpMode {
IMU imu;

166
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsLinearOpMode.java Normal file
View File

@ -0,0 +1,166 @@
/* Copyright (c) 2021 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.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.Gamepad;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.cometbots.MovementLibrary;
/*
* 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="CometBots Linear OpMode Demo", group = "[14493] CometBots")
public class CometBotsLinearOpMode extends LinearOpMode {
// Declare OpMode members for each of the 4 motors.
private ElapsedTime runtime = new ElapsedTime();
private DcMotor rhw = null;
private DcMotor rlw = null;
private DcMotor lhw = null;
private DcMotor llw = 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.
rhw = hardwareMap.get(DcMotor.class, "right hand wheel");
rlw = hardwareMap.get(DcMotor.class, "right leg wheel");
lhw = hardwareMap.get(DcMotor.class, "left hand wheel");
llw = hardwareMap.get(DcMotor.class, "left leg wheel");
/*
Put all motors in an array for easier handling
NOTE: Placement of motor in array is important especially when dealing with functions
that set direction
*/
DcMotor[] motors = {rhw, rlw, lhw, llw};
/*
Initialize all wheels forward using motors a
*/
MovementLibrary.setDirectionForward(motors);
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;
}
// This is test code:
//
// Uncomment the following code to test your motor directions.
// Each button should make the corresponding motor run FORWARD.
// 1) First get all the motors to take to correct positions on the robot
// by adjusting your Robot Configuration if necessary.
// 2) Then make sure they run in the correct direction by modifying the
// the setDirection() calls above.
// Once the correct motors move in the correct direction re-comment this code.
/*
leftFrontPower = gamepad1.x ? 1.0 : 0.0; // X gamepad
leftBackPower = gamepad1.a ? 1.0 : 0.0; // A gamepad
rightFrontPower = gamepad1.y ? 1.0 : 0.0; // Y gamepad
rightBackPower = gamepad1.b ? 1.0 : 0.0; // B gamepad
*/
// Send calculated power to wheels
lhw.setPower(leftFrontPower);
rhw.setPower(rightFrontPower);
llw.setPower(leftBackPower);
rlw.setPower(rightBackPower);
// Show the elapsed game time and wheel power.
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();
}
}}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsOpenCVTest.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsOpenCVDemo.java
View File

@ -1,6 +1,7 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
@ -8,8 +9,9 @@ import org.openftc.easyopencv.OpenCvCamera;
import org.openftc.easyopencv.OpenCvCameraFactory;
import org.openftc.easyopencv.OpenCvCameraRotation;
@Autonomous(name="DevApp - OpenCV Test", group="SCDS Development")
public class CometBotsOpenCVTest extends LinearOpMode {
@Disabled
@Autonomous(name="DevApp - OpenCV Demo", group="[14493] CometBots")
public class CometBotsOpenCVDemo extends LinearOpMode {
private OpenCvCamera controlHubCam; // Use OpenCvCamera class from FTC SDK
private static final int CAMERA_WIDTH = 1280; // width of wanted camera resolution
@ -27,7 +29,7 @@ public class CometBotsOpenCVTest extends LinearOpMode {
// Use OpenCvCameraFactory class from FTC SDK to create camera instance
controlHubCam = OpenCvCameraFactory.getInstance().createWebcam(
hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId);
hardwareMap.get(WebcamName.class, "watch out"), cameraMonitorViewId);
// controlHubCam.setPipeline(new YellowBlobDetectionPipeline());

48
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsProject.java
View File

@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode;
import static org.firstinspires.ftc.teamcode.cometbots.Constants.*;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.DcMotor;
@ -12,6 +13,7 @@ import com.qualcomm.robotcore.hardware.TouchSensor;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.cometbots.MovementLibrary;
@Disabled
@Autonomous(name = "Development Class", group = "[14493] CometBots")
public class CometBotsProject extends LinearOpMode {
@ -38,12 +40,32 @@ public class CometBotsProject extends LinearOpMode {
/*
Initialize all wheels forward using motors array
*/
MovementLibrary.setDirectionForward(motors);
// MovementLibrary.setDirectionForward(motors);
MovementLibrary.setLeftSideBackward(motors);
MovementLibrary.setRightSideForward(motors);
/*
Initialize wheel direction with base speed
Capture color readout from color sensor
*/
telemetry.addData("Color Sensor [Red]", colorSensor.red());
telemetry.addData("Color Sensor [Green]", colorSensor.green());
telemetry.addData("Color Sensor [Blue]", colorSensor.blue());
telemetry.addData("Color Sensor [Alpha]", colorSensor.alpha());
/*
Capture distance readout from distance sensor
*/
MovementLibrary.setSpeed(motors, CB_BASE_SPEED);
telemetry.addData("Distance Sensor [inches]", distanceSensor.getDistance(DistanceUnit.INCH));
/*
Capture state when touch sensor is pressed
*/
if (touchSensor.isPressed()) {
telemetry.speak("Touch sensor is pressed");
telemetry.addData("Touch sensor pressed?", touchSensor.isPressed());
}
telemetry.update();
/*
Wait for the driver to hit START in the driver hub
@ -52,10 +74,11 @@ public class CometBotsProject extends LinearOpMode {
while (opModeIsActive()) {
if (touchSensor.isPressed()) {
telemetry.speak("Touch sensor is pressed");
telemetry.addData("Touch sensor pressed?", touchSensor.isPressed());
}
/*
Initialize wheel direction with base speed
*/
MovementLibrary.setSpeed(motors, CB_BASE_SPEED);
/*
Distance traveled in inches
@ -63,14 +86,6 @@ public class CometBotsProject extends LinearOpMode {
double distanceInInches = distanceSensor.getDistance(DistanceUnit.INCH);
telemetry.addData("Distance traveled (inches)", "%.2f", distanceInInches);
/*
Capture color readout from color sensor
*/
telemetry.addData("Color Sensor [Red]", colorSensor.red());
telemetry.addData("Color Sensor [Green]", colorSensor.green());
telemetry.addData("Color Sensor [Blue]", colorSensor.blue());
telemetry.addData("Color Sensor [Alpha]", colorSensor.alpha());
/*
Speed up when going over an object that is blue-ish / green
*/
@ -80,9 +95,6 @@ public class CometBotsProject extends LinearOpMode {
} else {
MovementLibrary.setSpeed(motors, CB_BASE_SPEED);
}
telemetry.update();
}
}
}

111
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsSensorDemo.java Normal file
View File

@ -0,0 +1,111 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DistanceSensor;
import com.qualcomm.robotcore.hardware.TouchSensor;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.cometbots.MovementLibrary;
import org.openftc.easyopencv.OpenCvCamera;
import org.openftc.easyopencv.OpenCvCameraFactory;
import org.openftc.easyopencv.OpenCvCameraRotation;
@Autonomous(name = "CometBot Sensor Demo", group = "[14493] CometBots")
public class CometBotsSensorDemo extends LinearOpMode {
private OpenCvCamera controlHubCam; // Use OpenCvCamera class from FTC SDK
private static final int CAMERA_WIDTH = 1280; // width of wanted camera resolution
private static final int CAMERA_HEIGHT = 720; // height of wanted camera resolution
// Calculate the distance using the formula
public static final double objectWidthInRealWorldUnits = 3.75; // Replace with the actual width of the object in real-world units
public static final double focalLength = 728; // Replace with the focal length of the camera in pixels
private DcMotor rhw = null;
private DcMotor rlw = null;
private DcMotor lhw = null;
private DcMotor llw = null;
@Override
public void runOpMode() throws InterruptedException {
// 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.
rhw = hardwareMap.get(DcMotor.class, "right hand wheel");
rlw = hardwareMap.get(DcMotor.class, "right leg wheel");
lhw = hardwareMap.get(DcMotor.class, "left hand wheel");
llw = hardwareMap.get(DcMotor.class, "left leg wheel");
/*
Put all motors in an array for easier handling
NOTE: Placement of motor in array is important especially when dealing with functions
that set direction
*/
DcMotor[] motors = {rhw, rlw, lhw, llw};
// Create an instance of the camera
int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
"cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
// Use OpenCvCameraFactory class from FTC SDK to create camera instance
controlHubCam = OpenCvCameraFactory.getInstance().createWebcam(
hardwareMap.get(WebcamName.class, "watch out"), cameraMonitorViewId);
TouchSensor touchSensor = hardwareMap.get(TouchSensor.class, "touchsensor");
DistanceSensor distanceSensor = hardwareMap.get(DistanceSensor.class, "distancesensor");
ColorSensor colorSensor = hardwareMap.get(ColorSensor.class, "colorsensor");
controlHubCam.openCameraDevice();
controlHubCam.startStreaming(CAMERA_WIDTH, CAMERA_HEIGHT, OpenCvCameraRotation.UPRIGHT);
FtcDashboard.getInstance().startCameraStream(controlHubCam, 0);
/*
Wait for the driver to hit START in the driver hub
*/
waitForStart();
while (opModeIsActive()) {
/*
Capture color readout from color sensor
*/
telemetry.addData("Color Sensor [Red]", colorSensor.red());
telemetry.addData("Color Sensor [Green]", colorSensor.green());
telemetry.addData("Color Sensor [Blue]", colorSensor.blue());
telemetry.addData("Color Sensor [Alpha]", colorSensor.alpha());
/*
Capture distance readout from distance sensor
*/
telemetry.addData("Distance Sensor [inches]", distanceSensor.getDistance(DistanceUnit.INCH));
/*
Capture state when touch sensor is pressed
*/
telemetry.addData("Touch sensor pressed?", touchSensor.isPressed());
if (touchSensor.isPressed()) {
telemetry.speak("Touch sensor is pressed");
}
if (distanceSensor.getDistance(DistanceUnit.INCH) < 3) {
telemetry.speak("Warning, unknown object near robot");
MovementLibrary.setDirectionRight(motors);
MovementLibrary.setSpeed(motors,0.25);
sleep(1500);
MovementLibrary.setSpeed(motors,0);
}
/*
Update stats continually
*/
telemetry.update();
}
}
}

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptTensorFlowObjectDetectionEasy.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsTensorFlowDemo.java
View File

@ -29,7 +29,6 @@
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;
@ -48,8 +47,8 @@ import java.util.List;
* 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: TensorFlow Object Detection Easy", group = "Concept")
public class ConceptTensorFlowObjectDetectionEasy extends LinearOpMode {
@TeleOp(name = "CometBots TensorFlow Demo (HDMI)", group = "[14493] CometBots")
public class CometBotsTensorFlowDemo extends LinearOpMode {
private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera
@ -110,7 +109,7 @@ public class ConceptTensorFlowObjectDetectionEasy extends LinearOpMode {
// Create the vision portal the easy way.
if (USE_WEBCAM) {
visionPortal = VisionPortal.easyCreateWithDefaults(
hardwareMap.get(WebcamName.class, "Webcam 1"), tfod);
hardwareMap.get(WebcamName.class, "watch out"), tfod);
} else {
visionPortal = VisionPortal.easyCreateWithDefaults(
BuiltinCameraDirection.BACK, tfod);

32
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/MovementLibrary.java
View File

@ -1,37 +1,55 @@
package org.firstinspires.ftc.teamcode.cometbots;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
public class MovementLibrary {
public static void setDirectionForward(DcMotor[] motors) {
motors[0].setDirection(DcMotor.Direction.REVERSE);
motors[1].setDirection(DcMotor.Direction.FORWARD);
setLeftSideForward(motors);
setRightSideForward(motors);
}
public static void setLeftSideForward(DcMotor[] motors) {
motors[2].setDirection(DcMotor.Direction.FORWARD);
motors[3].setDirection(DcMotor.Direction.REVERSE);
}
public static void setDirectionBackward(DcMotor[] motors) {
motors[0].setDirection(DcMotor.Direction.FORWARD);
motors[1].setDirection(DcMotor.Direction.REVERSE);
public static void setRightSideForward(DcMotor[] motors) {
motors[0].setDirection(DcMotor.Direction.REVERSE);
motors[1].setDirection(DcMotor.Direction.FORWARD);
}
public static void setLeftSideBackward(DcMotor[] motors) {
motors[2].setDirection(DcMotor.Direction.REVERSE);
motors[3].setDirection(DcMotor.Direction.FORWARD);
}
public static void setRightSideBackward(DcMotor[] motors) {
motors[0].setDirection(DcMotor.Direction.FORWARD);
motors[1].setDirection(DcMotor.Direction.REVERSE);
}
public static void setDirectionBackward(DcMotor[] motors) {
setLeftSideBackward(motors);
setRightSideBackward(motors);
}
public static void setSpeed(DcMotor[] motors, double speedValue) {
for (DcMotor motor : motors) {
motor.setPower(speedValue);
}
}
public static void setDirectionRight(DcMotor[] motors) {
public static void setDirectionLeft(DcMotor[] motors) {
motors[0].setDirection(DcMotor.Direction.REVERSE);
motors[1].setDirection(DcMotor.Direction.REVERSE);
motors[2].setDirection(DcMotor.Direction.FORWARD);
motors[3].setDirection(DcMotor.Direction.FORWARD);
}
public static void setDirectionLeft(DcMotor[] motors) {
public static void setDirectionRight(DcMotor[] motors) {
motors[0].setDirection(DcMotor.Direction.FORWARD);
motors[1].setDirection(DcMotor.Direction.FORWARD);
motors[2].setDirection(DcMotor.Direction.REVERSE);