SCDS/FtcTemplate
7
0
Fork 0
mirror of https://github.com/trc492/FtcTemplate.git synced 2025-07-01 13:01:24 -07:00
Code Issues Packages Projects Releases Wiki Activity

Sync'd latest changes from IntoTheDeep.

Browse Source 
Updated ftclib, trclib.
This commit is contained in:
Titan Robotics Club
2024-12-24 02:37:08 -08:00
parent 94b100eca4
commit 5a3bb9474b
10 changed files with 287 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
TeamCode/src/main/java/ftclib

Submodule TeamCode/src/main/java/ftclib updated: 2c529954d6...fbd66737d8

11
TeamCode/src/main/java/teamcode/FtcAuto.java
View File

@ -43,7 +43,7 @@ import trclib.timer.TrcTimer;
/**
* This class contains the Autonomous Mode program.
*/
@Autonomous(name="FtcAutonomous", group="Ftcxxxx")
@Autonomous(name="FtcAutonomous", group="FtcTeam")
public class FtcAuto extends FtcOpMode
{
private final String moduleName = getClass().getSimpleName();
@ -73,9 +73,9 @@ public class FtcAuto extends FtcOpMode
public static class AutoChoices
{
public double delay = 0.0;
public Alliance alliance = Alliance.RED_ALLIANCE;
public StartPos startPos = StartPos.LEFT;
public AutoStrategy strategy = AutoStrategy.DO_NOTHING;
public Alliance alliance = null;
public StartPos startPos = null;
public AutoStrategy strategy = null;
public double xTarget = 0.0;
public double yTarget = 0.0;
public double turnTarget = 0.0;
@ -192,7 +192,8 @@ public class FtcAuto extends FtcOpMode
/**
* This method is called periodically after robotInit() is called but before competition starts. For example,
* we can put vision code here to detect target before autonomous starts.
* we can put vision code here to detect target before autonomous starts, or code to move the subsystems to a
* particular configuration to be within the 18-inch starting restriction.
*/
@Override
public void initPeriodic()

49
TeamCode/src/main/java/teamcode/FtcTeleOp.java
View File

@ -29,6 +29,7 @@ import java.util.Locale;
import ftclib.drivebase.FtcSwerveDrive;
import ftclib.driverio.FtcGamepad;
import ftclib.robotcore.FtcOpMode;
import teamcode.subsystems.RumbleIndicator;
import trclib.drivebase.TrcDriveBase;
import trclib.pathdrive.TrcPose2D;
import trclib.robotcore.TrcDbgTrace;
@ -38,7 +39,7 @@ import trclib.timer.TrcTimer;
/**
* This class contains the TeleOp Mode program.
*/
@TeleOp(name="FtcTeleOp", group="Ftcxxxx")
@TeleOp(name="FtcTeleOp", group="FtcTeam")
public class FtcTeleOp extends FtcOpMode
{
private final String moduleName = getClass().getSimpleName();
@ -50,6 +51,7 @@ public class FtcTeleOp extends FtcOpMode
private double turnPowerScale;
private boolean driverAltFunc = false;
private boolean operatorAltFunc = false;
private boolean statusUpdateOn = false;
private boolean relocalizing = false;
private TrcPose2D robotFieldPose = null;
@ -85,11 +87,19 @@ public class FtcTeleOp extends FtcOpMode
driverGamepad = new FtcGamepad("DriverGamepad", gamepad1);
driverGamepad.setButtonEventHandler(this::driverButtonEvent);
driverGamepad.setLeftStickInverted(false, true);
driverGamepad.setRightStickInverted(false, true);
operatorGamepad = new FtcGamepad("OperatorGamepad", gamepad2);
operatorGamepad.setButtonEventHandler(this::operatorButtonEvent);
operatorGamepad.setLeftStickInverted(false, true);
operatorGamepad.setRightStickInverted(false, true);
if (RobotParams.Preferences.useRumble)
{
robot.driverRumble = new RumbleIndicator("DriverRumble", driverGamepad);
robot.operatorRumble = new RumbleIndicator("OperatorRumble", operatorGamepad);
}
drivePowerScale = RobotParams.Robot.DRIVE_NORMAL_SCALE;
turnPowerScale = RobotParams.Robot.TURN_NORMAL_SCALE;
setDriveOrientation(RobotParams.Robot.DRIVE_ORIENTATION);
@ -208,6 +218,19 @@ public class FtcTeleOp extends FtcOpMode
1, "RobotDrive: Power=(%.2f,y=%.2f,rot=%.2f),Mode:%s",
inputs[0], inputs[1], inputs[2], robot.robotDrive.driveBase.getDriveOrientation());
}
// Check for EndGame warning.
if (elapsedTime > RobotParams.Game.ENDGAME_DEADLINE)
{
if (robot.driverRumble != null)
{
robot.driverRumble.setRumblePattern(RumbleIndicator.ENDGAME_DEADLINE);
}
if (robot.operatorRumble != null)
{
robot.operatorRumble.setRumblePattern(RumbleIndicator.ENDGAME_DEADLINE);
}
}
}
//
// Other subsystems.
@ -217,7 +240,7 @@ public class FtcTeleOp extends FtcOpMode
// Analog control of subsystems.
}
// Display subsystem status.
if (RobotParams.Preferences.doStatusUpdate)
if (RobotParams.Preferences.doStatusUpdate || statusUpdateOn)
{
robot.updateStatus(2);
}
@ -294,7 +317,14 @@ public class FtcTeleOp extends FtcOpMode
case B:
case X:
break;
case Y:
if (driverAltFunc && pressed)
{
robot.globalTracer.traceInfo(moduleName, ">>>>> Cancel all.");
robot.cancelAll();
}
break;
case LeftBumper:
@ -303,6 +333,16 @@ public class FtcTeleOp extends FtcOpMode
break;
case RightBumper:
if (driverAltFunc)
{
if (!RobotParams.Preferences.doStatusUpdate)
{
// Toggle status update ON/OFF.
statusUpdateOn = !statusUpdateOn;
}
}
else
{
// Press and hold for slow drive.
if (pressed)
{
@ -316,6 +356,7 @@ public class FtcTeleOp extends FtcOpMode
drivePowerScale = RobotParams.Robot.DRIVE_NORMAL_SCALE;
turnPowerScale = RobotParams.Robot.TURN_NORMAL_SCALE;
}
}
break;
case DpadUp:
@ -401,9 +442,9 @@ public class FtcTeleOp extends FtcOpMode
if (pressed)
{
// Zero calibrate all subsystems (arm, elevator and turret).
robot.globalTracer.traceInfo(moduleName, ">>>>> ZeroCalibrate pressed.");
robot.globalTracer.traceInfo(moduleName, ">>>>> ZeroCalibrating.");
robot.cancelAll();
robot.zeroCalibrate(moduleName);
robot.zeroCalibrate(moduleName, null);
}
break;

59
TeamCode/src/main/java/teamcode/FtcTest.java
View File

@ -54,7 +54,7 @@ import trclib.timer.TrcTimer;
* testing purposes. It provides numerous tests for diagnosing problems with the robot. It also provides tools
* for tuning and calibration.
*/
@TeleOp(name="FtcTest", group="Ftcxxxx")
@TeleOp(name="FtcTest", group="FtcTeam")
public class FtcTest extends FtcTeleOp
{
private final String moduleName = getClass().getSimpleName();
@ -232,10 +232,16 @@ public class FtcTest extends FtcTeleOp
// Vision generally will impact performance, so we only enable it if it's needed.
if (robot.vision.aprilTagVision != null)
{
robot.globalTracer.traceInfo(moduleName, "Enabling AprilTagVision.");
robot.globalTracer.traceInfo(moduleName, "Enabling AprilTagVision for Webcam.");
robot.vision.setAprilTagVisionEnabled(true);
}
if (robot.vision.limelightVision != null)
{
robot.globalTracer.traceInfo(moduleName, "Enabling AprilTagVision for Limelight.");
robot.vision.setLimelightVisionEnabled(0, true);
}
if (robot.vision.redBlobVision != null)
{
robot.globalTracer.traceInfo(moduleName, "Enabling RedBlobVision.");
@ -247,12 +253,6 @@ public class FtcTest extends FtcTeleOp
robot.globalTracer.traceInfo(moduleName, "Enabling BlueBlobVision.");
robot.vision.setColorBlobVisionEnabled(Vision.ColorBlobType.BlueBlob, true);
}
if (robot.vision.limelightVision != null)
{
robot.globalTracer.traceInfo(moduleName, "Enabling LimelightVision.");
robot.vision.setLimelightVisionEnabled(0, true);
}
}
break;
@ -436,12 +436,12 @@ public class FtcTest extends FtcTeleOp
{
case SENSORS_TEST:
case SUBSYSTEMS_TEST:
doSensorsTest();
doSensorsTest(lineNum);
break;
case VISION_TEST:
case TUNE_COLORBLOB_VISION:
doVisionTest();
doVisionTest(lineNum);
break;
case X_TIMED_DRIVE:
@ -617,6 +617,17 @@ public class FtcTest extends FtcTeleOp
}
passToTeleOp = false;
}
else if (testChoices.test == Test.VISION_TEST &&
robot.vision != null && robot.vision.isLimelightVisionEnabled())
{
if (pressed)
{
int pipelineIndex = (robot.vision.limelightVision.getPipeline() + 1) %
Vision.LimelightParams.NUM_PIPELINES;
robot.vision.limelightVision.setPipeline(pipelineIndex);
robot.globalTracer.traceInfo(moduleName, "Switch Limelight pipeline to " + pipelineIndex);
}
}
break;
case LeftBumper:
@ -1015,10 +1026,11 @@ public class FtcTest extends FtcTeleOp
* This method reads all sensors and prints out their values. This is a very useful diagnostic tool to check
* if all sensors are working properly. For encoders, since test sensor mode is also teleop mode, you can
* operate the gamepads to turn the motors and check the corresponding encoder counts.
*
* @param lineNum specifies the starting line number on the dashboard to display sensor states.
*/
private void doSensorsTest()
private void doSensorsTest(int lineNum)
{
int lineNum = 9;
//
// Read all sensors and display on the dashboard.
// Drive the robot around to sample different locations of the field.
@ -1062,17 +1074,19 @@ public class FtcTest extends FtcTeleOp
/**
* This method calls vision code to detect target objects and display their info.
*
* @param lineNum specifies the starting line number on the dashboard to display vision info.
*/
private void doVisionTest()
private void doVisionTest(int lineNum)
{
if (robot.vision != null)
{
int lineNum = 9;
if (robot.vision.vision != null)
if (robot.vision.limelightVision != null)
{
// displayExposureSettings is only available for VisionPortal.
robot.vision.displayExposureSettings(lineNum++);
robot.vision.getLimelightDetectedObject(
robot.vision.limelightVision.getPipeline() == 0?
FtcLimelightVision.ResultType.Fiducial: FtcLimelightVision.ResultType.Python,
null, lineNum++);
}
if (robot.vision.rawColorBlobVision != null)
@ -1087,17 +1101,18 @@ public class FtcTest extends FtcTeleOp
if (robot.vision.redBlobVision != null)
{
robot.vision.getDetectedColorBlob(Vision.ColorBlobType.RedBlob, lineNum++);
robot.vision.getDetectedColorBlob(Vision.ColorBlobType.RedBlob, 0.0, lineNum++);
}
if (robot.vision.blueBlobVision != null)
{
robot.vision.getDetectedColorBlob(Vision.ColorBlobType.BlueBlob, lineNum++);
robot.vision.getDetectedColorBlob(Vision.ColorBlobType.BlueBlob, 0.0, lineNum++);
}
if (robot.vision.limelightVision != null)
if (robot.vision.vision != null)
{
robot.vision.getLimelightDetectedObject(FtcLimelightVision.ResultType.Fiducial, null, lineNum++);
// displayExposureSettings is only available for VisionPortal.
robot.vision.displayExposureSettings(lineNum++);
}
}
} //doVisionTest

27
TeamCode/src/main/java/teamcode/Robot.java
View File

@ -31,11 +31,13 @@ import ftclib.robotcore.FtcOpMode;
import ftclib.sensor.FtcRobotBattery;
import teamcode.subsystems.LEDIndicator;
import teamcode.subsystems.RobotBase;
import teamcode.subsystems.RumbleIndicator;
import teamcode.subsystems.Vision;
import trclib.motor.TrcMotor;
import trclib.motor.TrcServo;
import trclib.pathdrive.TrcPose2D;
import trclib.robotcore.TrcDbgTrace;
import trclib.robotcore.TrcEvent;
import trclib.robotcore.TrcRobot;
import trclib.sensor.TrcDigitalInput;
import trclib.timer.TrcTimer;
@ -60,6 +62,8 @@ public class Robot
public Vision vision;
// Sensors and indicators.
public LEDIndicator ledIndicator;
public RumbleIndicator driverRumble;
public RumbleIndicator operatorRumble;
public FtcRobotBattery battery;
// Subsystems.
// Autotasks.
@ -201,15 +205,21 @@ public class Robot
if (vision != null)
{
vision.setCameraStreamEnabled(false);
if (vision.rawColorBlobVision != null)
if (vision.isRawColorBlobVisionEnabled())
{
globalTracer.traceInfo(moduleName, "Disabling RawColorBlobVision.");
vision.setRawColorBlobVisionEnabled(false);
}
if (vision.aprilTagVision != null)
if (vision.isLimelightVisionEnabled())
{
globalTracer.traceInfo(moduleName, "Disabling AprilTagVision.");
globalTracer.traceInfo(moduleName, "Disabling LimelightVision.");
vision.setLimelightVisionEnabled(0, false);
}
if (vision.isAprilTagVisionEnabled())
{
globalTracer.traceInfo(moduleName, "Disabling Webcam AprilTagVision.");
vision.setAprilTagVisionEnabled(false);
}
@ -296,8 +306,9 @@ public class Robot
* This method zero calibrates all subsystems.
*
* @param owner specifies the owner ID to check if the caller has ownership of the motor.
* @param event specifies the event to signal when the zero calibration is done.
*/
public void zeroCalibrate(String owner)
public void zeroCalibrate(String owner, TrcEvent event)
{
} //zeroCalibrate
@ -306,7 +317,7 @@ public class Robot
*/
public void zeroCalibrate()
{
zeroCalibrate(null);
zeroCalibrate(null, null);
} //zeroCalibrate
/**
@ -372,7 +383,7 @@ public class Robot
*/
public TrcPose2D adjustPoseByAlliance(double x, double y, double heading, FtcAuto.Alliance alliance)
{
return adjustPoseByAlliance(x, y, heading, alliance, true);
return adjustPoseByAlliance(x, y, heading, alliance, false);
} //adjustPoseByAlliance
/**
@ -391,13 +402,13 @@ public class Robot
/**
* This method adjusts the given pose in the red alliance to be the specified alliance.
*
* @param pose specifies pose in the blue alliance in tile unit.
* @param pose specifies pose in the red alliance in tile unit.
* @param alliance specifies the alliance to be converted to.
* @return pose adjusted to be in the specified alliance in inches.
*/
public TrcPose2D adjustPoseByAlliance(TrcPose2D pose, FtcAuto.Alliance alliance)
{
return adjustPoseByAlliance(pose, alliance, true);
return adjustPoseByAlliance(pose, alliance, false);
} //adjustPoseByAlliance
/**

9
TeamCode/src/main/java/teamcode/RobotParams.java
View File

@ -30,7 +30,7 @@ import trclib.driverio.TrcGameController.DriveMode;
import trclib.pathdrive.TrcPose2D;
/**
* This class contains robot and subsystem constants and parameters.
* This class contains robot constants and parameters.
*/
public class RobotParams
{
@ -54,6 +54,7 @@ public class RobotParams
public static final boolean showSubsystems = true;
public static final boolean useBlinkinLED = false;
public static final boolean useGobildaLED = false;
public static final boolean useRumble = false;
// Vision
public static final boolean useVision = false;
public static final boolean useWebCam = false; // false to use Android phone camera.
@ -63,7 +64,6 @@ public class RobotParams
public static final boolean useCameraStreamProcessor = false;
public static final boolean useAprilTagVision = false;
public static final boolean useColorBlobVision = false;
public static final boolean doWatershed = false;
public static final boolean showVisionView = !inCompetition;
public static final boolean showVisionStat = false;
// Drive Base
@ -110,6 +110,11 @@ public class RobotParams
new TrcPose2D(0.0, 0.0, 0.0) // TagId 4
};
// Robot start locations.
// Game elapsed times.
public static final double AUTO_PERIOD = 30.0; // 30 seconds auto period
public static final double TELEOP_PERIOD = 120.0; // 2 minutes teleop period
public static final double PARKING_TIME = 10.0;
public static final double ENDGAME_DEADLINE = TELEOP_PERIOD - PARKING_TIME;
} //class Game
/**

2
TeamCode/src/main/java/teamcode/autocommands/CmdAuto.java
View File

@ -112,6 +112,8 @@ public class CmdAuto implements TrcRobot.RobotCommand
switch (state)
{
case START:
// Set robot location according to auto choices.
robot.setRobotStartPosition(autoChoices);
if (autoChoices.delay > 0.0)
{
robot.globalTracer.traceInfo(moduleName, "***** Do delay " + autoChoices.delay + "s.");

68
TeamCode/src/main/java/teamcode/subsystems/RumbleIndicator.java Normal file
View File

@ -0,0 +1,68 @@
/*
* Copyright (c) 2024 Titan Robotics Club (http://www.titanrobotics.com)
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package teamcode.subsystems;
import ftclib.driverio.FtcGamepad;
import ftclib.driverio.FtcGamepadRumble;
import trclib.driverio.TrcPriorityIndicator;
/**
* This class encapsulates the gamepad rumble to provide a priority indicator showing the status of the robot.
*/
public class RumbleIndicator
{
// Rumble pattern names.
public static final String ENDGAME_DEADLINE = "EndGameDeadline";
private final TrcPriorityIndicator<?> indicator;
/**
* Constructor: Create an instance of the object.
*
* @param indicatorName specifies the indicator hardware name.
* @param gamepad specifies the gamepad to use for the rumble.
*/
public RumbleIndicator(String indicatorName, FtcGamepad gamepad)
{
final FtcGamepadRumble.Pattern[] rumblePatternPriorities = {
// Highest priority.
new FtcGamepadRumble.Pattern(ENDGAME_DEADLINE, 3)
// Lowest priority.
};
indicator = new FtcGamepadRumble(indicatorName, gamepad.gamepad);
((FtcGamepadRumble) indicator).setPatternPriorities(rumblePatternPriorities);
} //RumbleIndicator
/**
* This method sets the rumble pattern ON for a period of time and turns off automatically afterwards.
*
* @param patternName specifies the name of the rumble pattern to turn on.
*/
public void setRumblePattern(String patternName)
{
if (indicator != null)
{
indicator.setPatternState(patternName, true, 1.0);
}
} //setRumblePattern
} //class RumbleIndicator

127
TeamCode/src/main/java/teamcode/subsystems/Vision.java
View File

@ -47,6 +47,7 @@ import ftclib.vision.FtcVisionAprilTag;
import ftclib.vision.FtcVisionEocvColorBlob;
import teamcode.Robot;
import teamcode.RobotParams;
import trclib.dataprocessor.TrcUtil;
import trclib.pathdrive.TrcPose2D;
import trclib.pathdrive.TrcPose3D;
import trclib.robotcore.TrcDbgTrace;
@ -74,25 +75,25 @@ public class Vision
camName = "Webcam 1";
camImageWidth = 640;
camImageHeight = 480;
camXOffset = 0.0; // Inches to the right from robot center
camYOffset = 2.0; // Inches forward from robot center
camZOffset = 9.75; // Inches up from the floor
camPitch = 15.0; // degrees down from horizontal
camYaw = 0.0; // degrees clockwise from robot front
camXOffset = -4.25; // Inches to the right from robot center
camYOffset = 5.5; // Inches forward from robot center
camZOffset = 10.608; // Inches up from the floor
camYaw = -2.0; // degrees clockwise from robot forward
camPitch = -32.346629699; // degrees up from horizontal
camRoll = 0.0;
camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll);
camOrientation = OpenCvCameraRotation.UPRIGHT;
// Homography: cameraRect in pixels, worldRect in inches
cameraRect = new TrcHomographyMapper.Rectangle(
0.0, 120.0, // Camera Top Left
camImageWidth -1, 120.0, // Camera Top Right
0.0, camImageHeight - 1, // Camera Bottom Left
camImageWidth - 1, camImageHeight - 1); // Camera Bottom Right
14.0, 28.0, // Camera Top Left
612.0, 33.0, // Camera Top Right
56.0, 448.0, // Camera Bottom Left
581.0, 430.5); // Camera Bottom Right
worldRect = new TrcHomographyMapper.Rectangle(
-12.5626, 48.0 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Top Left
11.4375, 44.75 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Top Right
-2.5625, 21.0 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Bottom Left
2.5626, 21.0 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset); // World Bottom Right
-19.0, 37.5, // World Top Left
24.0, 37.5, // World Top Right
-4.75, 9.0, // World Bottom Left
6.25, 9.0); // World Bottom Right
} //FrontCamParams
} //class FrontCamParams
@ -109,8 +110,8 @@ public class Vision
camXOffset = 0.0; // Inches to the right from robot center
camYOffset = 2.0; // Inches forward from robot center
camZOffset = 9.75; // Inches up from the floor
camPitch = 15.0; // degrees down from horizontal
camYaw = 0.0; // degrees clockwise from robot front
camPitch = 15.0; // degrees down from horizontal
camRoll = 0.0;
camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll);
camOrientation = OpenCvCameraRotation.UPRIGHT;
@ -133,16 +134,20 @@ public class Vision
*/
public static class LimelightParams extends FtcRobotDrive.VisionInfo
{
public static final int NUM_PIPELINES = 4;
public LimelightParams()
{
camName = "Limelight3a";
camImageWidth = 640;
camImageHeight = 480;
camXOffset = 0.0; // Inches to the right from robot center
camYOffset = 2.0; // Inches forward from robot center
camZOffset = 9.75; // Inches up from the floor
camPitch = 15.0; // degrees down from horizontal
camYaw = 0.0; // degrees clockwise from robot front
camHFov = 80.0; // in degrees
camVFov = 56.0; // in degrees
camXOffset = 135.47*TrcUtil.INCHES_PER_MM; // Inches to the right from robot center
camYOffset = 2.073; // Inches forward from robot center
camZOffset = 10.758; // Inches up from the floor
camYaw = -3.438; // degrees clockwise from robot front
camPitch = 0.0; // degrees down from horizontal
camRoll = 0.0;
camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll);
} //LimelightParams
@ -160,8 +165,8 @@ public class Vision
//
// YCrCb Color Space.
private static final int colorConversion = Imgproc.COLOR_RGB2YCrCb;
private static final double[] redBlobColorThresholds = {20.0, 120.0, 170.0, 220.0, 80.0, 120.0};
private static final double[] blueBlobColorThresholds = {20.0, 60.0, 100.0, 150.0, 150.0, 180.0};
private static final double[] redBlobColorThresholds = {10.0, 180.0, 170.0, 240.0, 80.0, 120.0};
private static final double[] blueBlobColorThresholds = {0.0, 180.0, 80.0, 150.0, 150.0, 200.0};
private static final TrcOpenCvColorBlobPipeline.FilterContourParams colorBlobFilterContourParams =
new TrcOpenCvColorBlobPipeline.FilterContourParams()
.setMinArea(500.0)
@ -171,6 +176,15 @@ public class Vision
.setSolidityRange(0.0, 100.0)
.setVerticesRange(0.0, 1000.0)
.setAspectRatioRange(0.5, 2.5);
private static final TrcOpenCvColorBlobPipeline.FilterContourParams tuneFilterContourParams =
new TrcOpenCvColorBlobPipeline.FilterContourParams()
.setMinArea(10.0)
.setMinPerimeter(12.0)
.setWidthRange(0.0, 1000.0)
.setHeightRange(0.0, 1000.0)
.setSolidityRange(0.0, 100.0)
.setVerticesRange(0.0, 1000.0)
.setAspectRatioRange(0.5, 2.5);
private final TrcDbgTrace tracer;
private final Robot robot;
@ -178,7 +192,7 @@ public class Vision
private FtcRawEocvColorBlobPipeline rawColorBlobPipeline;
public FtcRawEocvVision rawColorBlobVision;
public FtcLimelightVision limelightVision;
public FtcCameraStreamProcessor cameraStreamProcessor;
private FtcCameraStreamProcessor cameraStreamProcessor;
public FtcVisionAprilTag aprilTagVision;
private AprilTagProcessor aprilTagProcessor;
public FtcVisionEocvColorBlob redBlobVision;
@ -204,9 +218,9 @@ public class Vision
this.tracer = new TrcDbgTrace();
this.robot = robot;
this.webcam1 = robot.robotInfo.webCam1 != null?
webcam1 = robot.robotInfo.webCam1 != null?
opMode.hardwareMap.get(WebcamName.class, robot.robotInfo.webCam1.camName): null;
this.webcam2 = robot.robotInfo.webCam2 != null?
webcam2 = robot.robotInfo.webCam2 != null?
opMode.hardwareMap.get(WebcamName.class, robot.robotInfo.webCam2.camName): null;
if (RobotParams.Preferences.tuneColorBlobVision && webcam1 != null)
{
@ -230,8 +244,7 @@ public class Vision
tracer.traceInfo(moduleName, "Starting RawEocvColorBlobVision...");
rawColorBlobPipeline = new FtcRawEocvColorBlobPipeline(
"rawColorBlobPipeline", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams,
true, false);
"rawColorBlobPipeline", colorConversion, redBlobColorThresholds, tuneFilterContourParams, true);
// By default, display original Mat.
rawColorBlobPipeline.setVideoOutput(0);
rawColorBlobPipeline.setAnnotateEnabled(true);
@ -262,7 +275,7 @@ public class Vision
if (RobotParams.Preferences.useAprilTagVision)
{
tracer.traceInfo(moduleName, "Starting AprilTagVision...");
tracer.traceInfo(moduleName, "Starting Webcam AprilTagVision...");
FtcVisionAprilTag.Parameters aprilTagParams = new FtcVisionAprilTag.Parameters()
.setDrawTagIdEnabled(true)
.setDrawTagOutlineEnabled(true)
@ -276,16 +289,16 @@ public class Vision
if (RobotParams.Preferences.useColorBlobVision && robot.robotInfo.webCam1 != null)
{
tracer.traceInfo(moduleName, "Starting ColorBlobVision...");
tracer.traceInfo(moduleName, "Starting Webcam ColorBlobVision...");
redBlobVision = new FtcVisionEocvColorBlob(
"RedBlob", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, true, false,
"RedBlob", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, true,
robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true);
redBlobProcessor = redBlobVision.getVisionProcessor();
visionProcessorsList.add(redBlobProcessor);
blueBlobVision = new FtcVisionEocvColorBlob(
"BlueBlob", colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams, true, false,
"BlueBlob", colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams, true,
robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true);
blueBlobProcessor = blueBlobVision.getVisionProcessor();
visionProcessorsList.add(blueBlobProcessor);
@ -300,7 +313,8 @@ public class Vision
// Use USB webcams.
vision = new FtcVision(
webcam1, webcam2, robot.robotInfo.webCam1.camImageWidth, robot.robotInfo.webCam1.camImageHeight,
RobotParams.Preferences.showVisionView, RobotParams.Preferences.showVisionStat, visionProcessors);
RobotParams.Preferences.showVisionView, RobotParams.Preferences.showVisionStat,
visionProcessors);
}
else
{
@ -309,7 +323,8 @@ public class Vision
RobotParams.Preferences.useBuiltinCamBack?
BuiltinCameraDirection.BACK: BuiltinCameraDirection.FRONT,
robot.robotInfo.webCam1.camImageWidth, robot.robotInfo.webCam1.camImageHeight,
RobotParams.Preferences.showVisionView, RobotParams.Preferences.showVisionStat, visionProcessors);
RobotParams.Preferences.showVisionView, RobotParams.Preferences.showVisionStat,
visionProcessors);
}
// Disable all vision until they are needed.
@ -498,6 +513,8 @@ public class Vision
limelightVision.setPipeline(pipelineIndex);
}
limelightVision.setVisionEnabled(enabled);
tracer.traceInfo(moduleName, "Pipeline %d is %s: running=%s",
pipelineIndex, enabled? "enabled": "disabled", limelightVision.limelight.isRunning());
}
} //setLimelightVisionEnabled
@ -523,14 +540,33 @@ public class Vision
FtcLimelightVision.ResultType resultType, String label, int lineNum)
{
TrcVisionTargetInfo<FtcLimelightVision.DetectedObject> limelightInfo = null;
String objectName = null;
if (limelightVision != null)
{
limelightInfo = limelightVision.getBestDetectedTargetInfo(resultType, label, null);
String objectName = null;
int pipelineIndex = -1;
Double robotHeading = robot.robotDrive != null? robot.robotDrive.driveBase.getHeading(): null;
limelightInfo = limelightVision.getBestDetectedTargetInfo(resultType, label, robotHeading, null);
if (limelightInfo != null)
{
objectName = limelightInfo.detectedObj.label;
pipelineIndex = limelightVision.getPipeline();
switch (pipelineIndex)
{
case 0:
objectName = LEDIndicator.APRIL_TAG;
break;
case 1:
objectName = LEDIndicator.RED_BLOB;
break;
case 2:
objectName = LEDIndicator.BLUE_BLOB;
break;
default:
break;
}
}
@ -542,7 +578,9 @@ public class Vision
if (lineNum != -1)
{
robot.dashboard.displayPrintf(
lineNum, "%s: %s", objectName, limelightInfo != null? limelightInfo: "Not found.");
lineNum, "%s(%d): %s",
objectName, pipelineIndex, limelightInfo != null? limelightInfo: "Not found.");
}
}
return limelightInfo;
@ -770,11 +808,12 @@ public class Vision
* This method calls ColorBlob vision to detect the specified color blob object.
*
* @param colorBlobType specifies the color blob type to be detected.
* @param groundOffset specifies the ground offset of the detected sample.
* @param lineNum specifies the dashboard line number to display the detected object info, -1 to disable printing.
* @return detected color blob object info.
*/
public TrcVisionTargetInfo<TrcOpenCvColorBlobPipeline.DetectedObject> getDetectedColorBlob(
ColorBlobType colorBlobType, int lineNum)
ColorBlobType colorBlobType, double groundOffset, int lineNum)
{
TrcVisionTargetInfo<TrcOpenCvColorBlobPipeline.DetectedObject> colorBlobInfo = null;
@ -782,12 +821,12 @@ public class Vision
{
case RedBlob:
colorBlobInfo = redBlobVision != null? redBlobVision.getBestDetectedTargetInfo(
null, this::compareDistance, 0.0, 0.0): null;
null, this::compareDistance, groundOffset, robot.robotInfo.webCam1.camZOffset): null;
break;
case BlueBlob:
colorBlobInfo = blueBlobVision != null? blueBlobVision.getBestDetectedTargetInfo(
null, this::compareDistance, 0.0, 0.0): null;
null, this::compareDistance, groundOffset, robot.robotInfo.webCam1.camZOffset): null;
break;
case AnyColorBlob:
@ -795,14 +834,14 @@ public class Vision
new ArrayList<>();
colorBlobInfo = redBlobVision != null ? redBlobVision.getBestDetectedTargetInfo(
null, this::compareDistance, 0.0, 0.0) : null;
null, this::compareDistance, groundOffset, robot.robotInfo.webCam1.camZOffset) : null;
if (colorBlobInfo != null)
{
colorBlobList.add(colorBlobInfo);
}
colorBlobInfo = blueBlobVision != null ? blueBlobVision.getBestDetectedTargetInfo(
null, this::compareDistance, 0.0, 0.0) : null;
null, this::compareDistance, groundOffset, robot.robotInfo.webCam1.camZOffset) : null;
if (colorBlobInfo != null)
{
colorBlobList.add(colorBlobInfo);
@ -835,7 +874,7 @@ public class Vision
if (colorBlobInfo != null)
{
robot.dashboard.displayPrintf(
lineNum, "%s: %s (rotatedAngle=%.1f",
lineNum, "%s: %s (rotatedAngle=%.1f)",
colorBlobInfo.detectedObj.label, colorBlobInfo, colorBlobInfo.detectedObj.rotatedRect.angle);
}
else
@ -861,6 +900,10 @@ public class Vision
{
offset = 5.75;
}
else if ( resultType == FtcLimelightVision.ResultType.Python)
{
offset = 10.0;
}
return offset;
} //getTargetGroundOffset

2
TeamCode/src/main/java/trclib

Submodule TeamCode/src/main/java/trclib updated: 077f1325b7...75535f0b03