Skip to content

Commit

Permalink
Updated CommonLib.
Browse files Browse the repository at this point in the history 
Updated FtcLib.
Added color blob tuning support to FtcTest.
  • Loading branch information
@trc492
trc492 committed Sep 21, 2023
1 parent 574e179 commit 7e80bb4
Show file tree
Hide file tree
Showing 6 changed files with 297 additions and 62 deletions.
2 changes: 1 addition & 1 deletion TeamCode/src/main/java/TrcCommonLib
Submodule TrcCommonLib updated 2 files
+31 −8 trclib/TrcOpenCvColorBlobPipeline.java
+5 −3 trclib/TrcOpenCvPipeline.java
2 changes: 1 addition & 1 deletion TeamCode/src/main/java/TrcFtcLib
Submodule TrcFtcLib updated 4 files
+25 −4 ftclib/FtcEocvColorBlobProcessor.java
+28 −10 ftclib/FtcRawEocvColorBlobPipeline.java
+223 −0 ftclib/FtcRawEocvVision.java
+26 −14 ftclib/FtcVisionTensorFlow.java
143 changes: 138 additions & 5 deletions TeamCode/src/main/java/teamcode/FtcTest.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
/*
* Copyright (c) 2022 Titan Robotics Club (http://www.titanrobotics.com)
* Copyright (c) 2023 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
Expand All @@ -24,6 +24,7 @@

import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import java.util.Arrays;
import java.util.Locale;

import TrcCommonLib.command.CmdDriveMotorsTest;
Expand All @@ -34,6 +35,7 @@
import TrcCommonLib.trclib.TrcElapsedTimer;
import TrcCommonLib.trclib.TrcGameController;
import TrcCommonLib.trclib.TrcOpenCvColorBlobPipeline;
import TrcCommonLib.trclib.TrcOpenCvDetector;
import TrcCommonLib.trclib.TrcPidController;
import TrcCommonLib.trclib.TrcPose2D;
import TrcCommonLib.trclib.TrcRobot;
Expand Down Expand Up @@ -67,6 +69,7 @@ private enum Test
SENSORS_TEST,
SUBSYSTEMS_TEST,
VISION_TEST,
TUNE_COLORBLOB_VISION,
DRIVE_SPEED_TEST,
DRIVE_MOTORS_TEST,
X_TIMED_DRIVE,
Expand Down Expand Up @@ -122,12 +125,19 @@ public String toString()
private FtcChoiceMenu<Test> testMenu = null;

private TrcRobot.RobotCommand testCommand = null;
// Drive Speed Test.
private double maxDriveVelocity = 0.0;
private double maxDriveAcceleration = 0.0;
private double prevTime = 0.0;
private double prevVelocity = 0.0;

// Swerve Steering Calibration.
private boolean steerCalibrating = false;
// Color Blob Vision Turning.
private static final double[] COLOR_THRESHOLD_LOW_RANGES = {0.0, 0.0, 0.0};
private static final double[] COLOR_THRESHOLD_HIGH_RANGES = {255.0, 255.0, 255.0};
private double[] colorThresholds = null;
private int colorThresholdIndex = 0;
private double colorThresholdMultiplier = 1.0;
private boolean teleOpControlEnabled = true;

//
Expand Down Expand Up @@ -261,9 +271,7 @@ public void startMode(TrcRobot.RunMode prevMode, TrcRobot.RunMode nextMode)
case VISION_TEST:
if (robot.vision != null)
{
//
// Vision generally will impact performance, so we only enable it if it's needed.
//
if (robot.vision.aprilTagVision != null)
{
robot.globalTracer.traceInfo(funcName, "Enabling AprilTagVision.");
Expand All @@ -290,6 +298,18 @@ public void startMode(TrcRobot.RunMode prevMode, TrcRobot.RunMode nextMode)
}
break;

case TUNE_COLORBLOB_VISION:
if (robot.vision != null && robot.vision.rawColorBlobVision != null)
{
robot.globalTracer.traceInfo(funcName, "Enabling FtcRawEocvVision.");
robot.vision.setRawColorBlobVisionEnabled(true);
colorThresholds = robot.vision.getRawColorBlobThresholds();
colorThresholdIndex = 0;
colorThresholdMultiplier = 1.0;
updateColorThresholds();
}
break;

case PID_DRIVE:
case TUNE_X_PID:
case TUNE_Y_PID:
Expand Down Expand Up @@ -483,6 +503,7 @@ public void periodic(double elapsedTime, boolean slowPeriodicLoop)
break;

case VISION_TEST:
case TUNE_COLORBLOB_VISION:
doVisionTest();
break;

Expand Down Expand Up @@ -531,7 +552,8 @@ public void periodic(double elapsedTime, boolean slowPeriodicLoop)
@Override
public void driverButtonEvent(TrcGameController gamepad, int button, boolean pressed)
{
if (allowButtonControl() || testChoices.test == Test.VISION_TEST)
if (allowButtonControl() || testChoices.test == Test.VISION_TEST ||
testChoices.test == Test.TUNE_COLORBLOB_VISION)
{
boolean processed = false;
//
Expand Down Expand Up @@ -562,15 +584,63 @@ public void driverButtonEvent(TrcGameController gamepad, int button, boolean pre
}
processed = true;
}
else if (testChoices.test == Test.TUNE_COLORBLOB_VISION &&
robot.vision != null && robot.vision.rawColorBlobVision != null)
{
if (pressed)
{
// Commit color thresholds change.
robot.vision.setRawColorBlobThresholds(colorThresholds);
}
processed = true;
}
break;

case FtcGamepad.GAMEPAD_B:
if (testChoices.test == Test.TUNE_COLORBLOB_VISION &&
robot.vision != null && robot.vision.rawColorBlobVision != null)
{
if (pressed)
{
// Increment to next color threshold index.
colorThresholdIndex++;
if (colorThresholdIndex >= colorThresholds.length)
{
colorThresholdIndex = colorThresholds.length - 1;
}
}
processed = true;
}
break;

case FtcGamepad.GAMEPAD_X:
if (testChoices.test == Test.TUNE_COLORBLOB_VISION &&
robot.vision != null && robot.vision.rawColorBlobVision != null)
{
if (pressed)
{
// Decrement to previous color threshold index.
colorThresholdIndex--;
if (colorThresholdIndex < 0)
{
colorThresholdIndex = 0;
}
}
processed = true;
}
break;

case FtcGamepad.GAMEPAD_Y:
if (testChoices.test == Test.TUNE_COLORBLOB_VISION &&
robot.vision != null && robot.vision.rawColorBlobVision != null)
{
if (pressed)
{
// Set display to next intermediate Mat in the pipeline.
robot.vision.rawColorBlobVision.getPipeline().setNextVideoOutput();
}
processed = true;
}
break;

case FtcGamepad.GAMEPAD_DPAD_UP:
Expand All @@ -584,6 +654,19 @@ public void driverButtonEvent(TrcGameController gamepad, int button, boolean pre
teleOpControlEnabled = !pressed;
processed = true;
}
else if (testChoices.test == Test.TUNE_COLORBLOB_VISION &&
robot.vision != null && robot.vision.rawColorBlobVision != null)
{
if (pressed &&
colorThresholds[colorThresholdIndex] + colorThresholdMultiplier <=
COLOR_THRESHOLD_HIGH_RANGES[colorThresholdIndex/2])
{
// Increment color threshold value.
colorThresholds[colorThresholdIndex] += colorThresholdMultiplier;
updateColorThresholds();
}
processed = true;
}
break;

case FtcGamepad.GAMEPAD_DPAD_DOWN:
Expand All @@ -597,6 +680,19 @@ public void driverButtonEvent(TrcGameController gamepad, int button, boolean pre
teleOpControlEnabled = !pressed;
processed = true;
}
else if (testChoices.test == Test.TUNE_COLORBLOB_VISION &&
robot.vision != null && robot.vision.rawColorBlobVision != null)
{
if (pressed &&
colorThresholds[colorThresholdIndex] - colorThresholdMultiplier >=
COLOR_THRESHOLD_LOW_RANGES[colorThresholdIndex/2])
{
// Decrement color threshold value.
colorThresholds[colorThresholdIndex] -= colorThresholdMultiplier;
updateColorThresholds();
}
processed = true;
}
break;

case FtcGamepad.GAMEPAD_DPAD_LEFT:
Expand All @@ -610,6 +706,16 @@ public void driverButtonEvent(TrcGameController gamepad, int button, boolean pre
teleOpControlEnabled = !pressed;
processed = true;
}
else if (testChoices.test == Test.TUNE_COLORBLOB_VISION &&
robot.vision != null && robot.vision.rawColorBlobVision != null)
{
if (pressed && colorThresholdMultiplier * 10.0 <= 100.0)
{
// Increment the significant multiplier.
colorThresholdMultiplier *= 10.0;
}
processed = true;
}
break;

case FtcGamepad.GAMEPAD_DPAD_RIGHT:
Expand All @@ -623,6 +729,16 @@ public void driverButtonEvent(TrcGameController gamepad, int button, boolean pre
teleOpControlEnabled = !pressed;
processed = true;
}
else if (testChoices.test == Test.TUNE_COLORBLOB_VISION &&
robot.vision != null && robot.vision.rawColorBlobVision != null)
{
if (pressed && colorThresholdMultiplier / 10.0 >= 1.0)
{
// Decrement the significant multiplier.
colorThresholdMultiplier /= 10.0;
}
processed = true;
}
break;
}
//
Expand Down Expand Up @@ -689,6 +805,14 @@ public void operatorButtonEvent(TrcGameController gamepad, int button, boolean p
}
} //operatorButtonEvent

/**
* This method displays the current color thresholds on the dashboard.
*/
private void updateColorThresholds()
{
robot.dashboard.displayPrintf(10, "Thresholds: %s", Arrays.toString(colorThresholds));
} //updateColorThresholds

/**
* This method creates and displays the test menus and record the selected choices.
*/
Expand Down Expand Up @@ -734,6 +858,7 @@ private void doTestMenus()
testMenu.addChoice("Sensors test", Test.SENSORS_TEST, true);
testMenu.addChoice("Subsystems test", Test.SUBSYSTEMS_TEST, false);
testMenu.addChoice("Vision test", Test.VISION_TEST, false);
testMenu.addChoice("Tune ColorBlob vision", Test.TUNE_COLORBLOB_VISION, false);
testMenu.addChoice("Drive speed test", Test.DRIVE_SPEED_TEST, false);
testMenu.addChoice("Drive motors test", Test.DRIVE_MOTORS_TEST, false);
testMenu.addChoice("X Timed drive", Test.X_TIMED_DRIVE, false, driveTimeMenu);
Expand Down Expand Up @@ -951,6 +1076,14 @@ private void doVisionTest()
{
int lineNum = 9;

if (robot.vision.rawColorBlobVision != null)
{
TrcVisionTargetInfo<TrcOpenCvDetector.DetectedObject<?>> colorBlobInfo =
robot.vision.rawColorBlobVision.getBestDetectedTargetInfo(null, null, 0.0, 0.0);
robot.dashboard.displayPrintf(
lineNum++, "ColorBlob: %s", colorBlobInfo != null? colorBlobInfo: "Not found.");
}

if (robot.vision.aprilTagVision != null)
{
TrcVisionTargetInfo<FtcVisionAprilTag.DetectedObject> aprilTagInfo =
Expand Down
9 changes: 8 additions & 1 deletion TeamCode/src/main/java/teamcode/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,8 @@ public Robot(TrcRobot.RunMode runMode)
//
// Initialize vision subsystems.
//
if (RobotParams.Preferences.useAprilTagVision ||
if (RobotParams.Preferences.tuneColorBlobVision ||
RobotParams.Preferences.useAprilTagVision ||
RobotParams.Preferences.useColorBlobVision ||
RobotParams.Preferences.useTensorFlowVision)
{
Expand Down Expand Up @@ -223,6 +224,12 @@ public void stopMode(TrcRobot.RunMode runMode)
//
if (vision != null)
{
if (vision.rawColorBlobVision != null)
{
globalTracer.traceInfo(funcName, "Disabling RawColorBlobVision.");
vision.setRawColorBlobVisionEnabled(false);
}

if (vision.aprilTagVision != null)
{
globalTracer.traceInfo(funcName, "Disabling AprilTagVision.");
Expand Down
5 changes: 5 additions & 0 deletions TeamCode/src/main/java/teamcode/RobotParams.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,8 @@

import com.qualcomm.robotcore.hardware.DcMotor;

import org.openftc.easyopencv.OpenCvCameraRotation;

import TrcCommonLib.trclib.TrcDriveBase.DriveOrientation;
import TrcCommonLib.trclib.TrcHomographyMapper;
import TrcCommonLib.trclib.TrcPidController;
Expand All @@ -49,9 +51,11 @@ public static class Preferences
// Vision
public static boolean useWebCam = true;
public static boolean useBuiltinCamBack = false;
public static boolean tuneColorBlobVision = false;
public static boolean useAprilTagVision = false;
public static boolean useColorBlobVision = false;
public static boolean useTensorFlowVision = false;
public static boolean useTfodModelAsset = false;
public static boolean showVisionView = true;
// Robot
public static boolean noRobot = false;
Expand Down Expand Up @@ -115,6 +119,7 @@ public static class Preferences
//
public static final int CAM_IMAGE_WIDTH = 640;
public static final int CAM_IMAGE_HEIGHT = 480;
public static final OpenCvCameraRotation CAM_ORIENTATION = OpenCvCameraRotation.UPRIGHT;
// Camera location on robot.
public static final double CAM_FRONT_OFFSET = 2.000;//Camera offset from front of robot in inches
public static final double CAM_LEFT_OFFSET = 7.125;//Camera offset from left of robot in inches
Expand Down
Loading

0 comments on commit 7e80bb4

Please sign in to comment.