mirror of
https://github.com/trc492/FtcTemplate.git
synced 2025-07-01 13:01:24 -07:00
1134 lines
45 KiB
Java
1134 lines
45 KiB
Java
/*
|
|
* 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
|
|
* 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;
|
|
|
|
import androidx.annotation.NonNull;
|
|
|
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
|
|
|
import java.util.Arrays;
|
|
import java.util.Locale;
|
|
|
|
import ftclib.drivebase.FtcRobotDrive;
|
|
import ftclib.drivebase.FtcSwerveDrive;
|
|
import ftclib.driverio.FtcChoiceMenu;
|
|
import ftclib.driverio.FtcGamepad;
|
|
import ftclib.driverio.FtcMenu;
|
|
import ftclib.driverio.FtcValueMenu;
|
|
import ftclib.robotcore.FtcPidCoeffCache;
|
|
import ftclib.vision.FtcLimelightVision;
|
|
import teamcode.subsystems.Vision;
|
|
import trclib.command.CmdDriveMotorsTest;
|
|
import trclib.command.CmdPidDrive;
|
|
import trclib.command.CmdTimedDrive;
|
|
import trclib.dataprocessor.TrcUtil;
|
|
import trclib.pathdrive.TrcPose2D;
|
|
import trclib.robotcore.TrcDbgTrace;
|
|
import trclib.robotcore.TrcPidController;
|
|
import trclib.robotcore.TrcRobot;
|
|
import trclib.timer.TrcElapsedTimer;
|
|
import trclib.timer.TrcTimer;
|
|
|
|
/**
|
|
* This class contains the Test Mode program. It extends FtcTeleOp so that we can teleop control the robot for
|
|
* testing purposes. It provides numerous tests for diagnosing problems with the robot. It also provides tools
|
|
* for tuning and calibration.
|
|
*/
|
|
@TeleOp(name="FtcTest", group="FtcTeam")
|
|
public class FtcTest extends FtcTeleOp
|
|
{
|
|
private final String moduleName = getClass().getSimpleName();
|
|
private static final boolean logEvents = true;
|
|
private static final boolean debugPid = true;
|
|
|
|
private enum Test
|
|
{
|
|
SENSORS_TEST,
|
|
SUBSYSTEMS_TEST,
|
|
VISION_TEST,
|
|
TUNE_COLORBLOB_VISION,
|
|
DRIVE_SPEED_TEST,
|
|
DRIVE_MOTORS_TEST,
|
|
X_TIMED_DRIVE,
|
|
Y_TIMED_DRIVE,
|
|
PID_DRIVE,
|
|
TUNE_X_PID,
|
|
TUNE_Y_PID,
|
|
TUNE_TURN_PID,
|
|
PURE_PURSUIT_DRIVE,
|
|
CALIBRATE_SWERVE_STEERING
|
|
} //enum Test
|
|
|
|
/**
|
|
* This class stores the test menu choices.
|
|
*/
|
|
private static class TestChoices
|
|
{
|
|
Test test = Test.SENSORS_TEST;
|
|
double xTarget = 0.0;
|
|
double yTarget = 0.0;
|
|
double turnTarget = 0.0;
|
|
double driveTime = 0.0;
|
|
double drivePower = 0.0;
|
|
TrcPidController.PidCoefficients tunePidCoeffs = null;
|
|
double tuneDistance = 0.0;
|
|
double tuneHeading = 0.0;
|
|
double tuneDrivePower = 0.0;
|
|
|
|
@NonNull
|
|
@Override
|
|
public String toString()
|
|
{
|
|
return String.format(
|
|
Locale.US,
|
|
"test=\"%s\" " +
|
|
"xTarget=%.1f " +
|
|
"yTarget=%.1f " +
|
|
"turnTarget=%1f " +
|
|
"driveTime=%.1f " +
|
|
"drivePower=%.1f " +
|
|
"tunePidCoeffs=%s " +
|
|
"tuneDistance=%.1f " +
|
|
"tuneHeading=%.1f " +
|
|
"tuneDrivePower=%.1f",
|
|
test, xTarget, yTarget, turnTarget, driveTime, drivePower, tunePidCoeffs, tuneDistance, tuneHeading,
|
|
tuneDrivePower);
|
|
} //toString
|
|
|
|
} //class TestChoices
|
|
|
|
private final FtcPidCoeffCache pidCoeffCache = new FtcPidCoeffCache(RobotParams.Robot.TEAM_FOLDER_PATH);
|
|
private final TestChoices testChoices = new TestChoices();
|
|
private TrcElapsedTimer elapsedTimer = null;
|
|
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 maxTurnVelocity = 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;
|
|
private long exposure;
|
|
private boolean fpsMeterEnabled = false;
|
|
//
|
|
// Overrides FtcOpMode abstract method.
|
|
//
|
|
|
|
/**
|
|
* This method is called to initialize the robot. In FTC, this is called when the "Init" button on the Driver
|
|
* Station is pressed.
|
|
*/
|
|
@Override
|
|
public void robotInit()
|
|
{
|
|
//
|
|
// TeleOp initialization.
|
|
//
|
|
super.robotInit();
|
|
if (RobotParams.Preferences.useLoopPerformanceMonitor)
|
|
{
|
|
elapsedTimer = new TrcElapsedTimer("TestLoopMonitor", 2.0);
|
|
}
|
|
//
|
|
// Test menus.
|
|
//
|
|
doTestMenus();
|
|
//
|
|
// Create the robot command for the tests that need one.
|
|
//
|
|
switch (testChoices.test)
|
|
{
|
|
case DRIVE_MOTORS_TEST:
|
|
if (robot.robotDrive != null)
|
|
{
|
|
testCommand = new CmdDriveMotorsTest(robot.robotDrive.driveMotors, 5.0, 0.5);
|
|
}
|
|
break;
|
|
|
|
case X_TIMED_DRIVE:
|
|
if (robot.robotDrive != null)
|
|
{
|
|
testCommand = new CmdTimedDrive(
|
|
robot.robotDrive.driveBase, 0.0, testChoices.driveTime, testChoices.drivePower, 0.0, 0.0);
|
|
}
|
|
break;
|
|
|
|
case Y_TIMED_DRIVE:
|
|
if (robot.robotDrive != null)
|
|
{
|
|
testCommand = new CmdTimedDrive(
|
|
robot.robotDrive.driveBase, 0.0, testChoices.driveTime, 0.0, testChoices.drivePower, 0.0);
|
|
}
|
|
break;
|
|
|
|
case PID_DRIVE:
|
|
case TUNE_X_PID:
|
|
case TUNE_Y_PID:
|
|
case TUNE_TURN_PID:
|
|
if (robot.robotDrive != null &&
|
|
(testChoices.test != Test.TUNE_X_PID || robot.robotDrive.driveBase.supportsHolonomicDrive()))
|
|
{
|
|
testCommand = new CmdPidDrive(robot.robotDrive.driveBase, robot.robotDrive.pidDrive);
|
|
}
|
|
break;
|
|
}
|
|
} //robotInit
|
|
|
|
//
|
|
// Overrides TrcRobot.RobotMode methods.
|
|
//
|
|
|
|
/**
|
|
* This method is called before test mode is about to start so it can initialize appropriate subsystems for the
|
|
* test.
|
|
*
|
|
* @param prevMode specifies the previous RunMode it is coming from (always null for FTC).
|
|
* @param nextMode specifies the next RunMode it is going into.
|
|
*/
|
|
@Override
|
|
public void startMode(TrcRobot.RunMode prevMode, TrcRobot.RunMode nextMode)
|
|
{
|
|
super.startMode(prevMode, nextMode);
|
|
switch (testChoices.test)
|
|
{
|
|
case VISION_TEST:
|
|
if (robot.vision != null)
|
|
{
|
|
robot.vision.setCameraStreamEnabled(true);
|
|
if (robot.vision.vision != null)
|
|
{
|
|
exposure = robot.vision.vision.getCurrentExposure();
|
|
}
|
|
// 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 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.");
|
|
robot.vision.setColorBlobVisionEnabled(Vision.ColorBlobType.RedBlob, true);
|
|
}
|
|
|
|
if (robot.vision.blueBlobVision != null)
|
|
{
|
|
robot.globalTracer.traceInfo(moduleName, "Enabling BlueBlobVision.");
|
|
robot.vision.setColorBlobVisionEnabled(Vision.ColorBlobType.BlueBlob, true);
|
|
}
|
|
}
|
|
break;
|
|
|
|
case TUNE_COLORBLOB_VISION:
|
|
if (robot.vision != null && robot.vision.rawColorBlobVision != null)
|
|
{
|
|
robot.globalTracer.traceInfo(moduleName, "Enabling FtcRawEocvVision.");
|
|
robot.vision.setCameraStreamEnabled(true);
|
|
robot.vision.setRawColorBlobVisionEnabled(true);
|
|
colorThresholds = robot.vision.getRawColorBlobThresholds();
|
|
colorThresholdIndex = 0;
|
|
colorThresholdMultiplier = 1.0;
|
|
updateColorThresholds();
|
|
}
|
|
break;
|
|
|
|
case PID_DRIVE:
|
|
if (testCommand != null)
|
|
{
|
|
((CmdPidDrive) testCommand).start(
|
|
0.0, testChoices.drivePower, null,
|
|
new TrcPose2D(testChoices.xTarget*12.0, testChoices.yTarget*12.0, testChoices.turnTarget));
|
|
robot.robotDrive.pidDrive.setTraceLevel(TrcDbgTrace.MsgLevel.INFO, logEvents, debugPid, false);
|
|
}
|
|
break;
|
|
|
|
case TUNE_X_PID:
|
|
if (testCommand != null)
|
|
{
|
|
((CmdPidDrive) testCommand).start(
|
|
0.0, testChoices.drivePower, testChoices.tunePidCoeffs,
|
|
new TrcPose2D(testChoices.tuneDistance*12.0, 0.0, 0.0));
|
|
robot.robotDrive.pidDrive.setTraceLevel(TrcDbgTrace.MsgLevel.INFO, logEvents, debugPid, false);
|
|
}
|
|
break;
|
|
|
|
case TUNE_Y_PID:
|
|
if (testCommand != null)
|
|
{
|
|
((CmdPidDrive) testCommand).start(
|
|
0.0, testChoices.drivePower, testChoices.tunePidCoeffs,
|
|
new TrcPose2D(0.0, testChoices.tuneDistance*12.0, 0.0));
|
|
robot.robotDrive.pidDrive.setTraceLevel(TrcDbgTrace.MsgLevel.INFO, logEvents, debugPid, false);
|
|
}
|
|
break;
|
|
|
|
case TUNE_TURN_PID:
|
|
if (testCommand != null)
|
|
{
|
|
((CmdPidDrive) testCommand).start(
|
|
0.0, testChoices.drivePower, testChoices.tunePidCoeffs,
|
|
new TrcPose2D(0.0, 0.0, testChoices.tuneHeading));
|
|
robot.robotDrive.pidDrive.setTraceLevel(TrcDbgTrace.MsgLevel.INFO, logEvents, debugPid, false);
|
|
}
|
|
break;
|
|
|
|
case PURE_PURSUIT_DRIVE:
|
|
if (robot.robotDrive != null)
|
|
{
|
|
robot.robotDrive.purePursuitDrive.setTraceLevel(
|
|
TrcDbgTrace.MsgLevel.INFO, logEvents, debugPid, false);
|
|
//
|
|
// Doing a 48x48-inch square box with robot heading always pointing to the center of the box.
|
|
//
|
|
// Set the current position as the absolute field origin so the path can be an absolute path.
|
|
TrcPose2D startPose = new TrcPose2D(0.0, 0.0, 0.0);
|
|
robot.robotDrive.driveBase.setFieldPosition(startPose);
|
|
robot.robotDrive.purePursuitDrive.start(
|
|
false, robot.robotInfo.profiledMaxVelocity, robot.robotInfo.profiledMaxAcceleration,
|
|
robot.robotInfo.profiledMaxDeceleration, new TrcPose2D(0.0, 48.0, 90.0));
|
|
}
|
|
break;
|
|
}
|
|
} //startMode
|
|
|
|
/**
|
|
* This method is called before test mode is about to exit so it can do appropriate cleanup.
|
|
*
|
|
* @param prevMode specifies the previous RunMode it is coming from.
|
|
* @param nextMode specifies the next RunMode it is going into (always null for FTC).
|
|
*/
|
|
@Override
|
|
public void stopMode(TrcRobot.RunMode prevMode, TrcRobot.RunMode nextMode)
|
|
{
|
|
if (testCommand != null)
|
|
{
|
|
testCommand.cancel();
|
|
}
|
|
|
|
if (robot.robotDrive != null)
|
|
{
|
|
robot.robotDrive.cancel();
|
|
}
|
|
|
|
super.stopMode(prevMode, nextMode);
|
|
} //stopMode
|
|
|
|
/**
|
|
* This method is called periodically on the main robot thread. Typically, you put TeleOp control code here that
|
|
* doesn't require frequent update For example, TeleOp joystick code or status display code can be put here since
|
|
* human responses are considered slow.
|
|
*
|
|
* @param elapsedTime specifies the elapsed time since the mode started.
|
|
* @param slowPeriodicLoop specifies true if it is running the slow periodic loop on the main robot thread,
|
|
* false otherwise.
|
|
*/
|
|
@Override
|
|
public void periodic(double elapsedTime, boolean slowPeriodicLoop)
|
|
{
|
|
int lineNum = 9;
|
|
//
|
|
// Run the testCommand if any.
|
|
//
|
|
if (testCommand != null)
|
|
{
|
|
testCommand.cmdPeriodic(elapsedTime);
|
|
}
|
|
//
|
|
// Display test status.
|
|
//
|
|
switch (testChoices.test)
|
|
{
|
|
case DRIVE_SPEED_TEST:
|
|
if (robot.robotDrive != null)
|
|
{
|
|
double currTime = TrcTimer.getCurrentTime();
|
|
TrcPose2D velPose = robot.robotDrive.driveBase.getFieldVelocity();
|
|
double velocity = TrcUtil.magnitude(velPose.x, velPose.y);
|
|
double acceleration = 0.0;
|
|
|
|
if (prevTime != 0.0)
|
|
{
|
|
acceleration = (velocity - prevVelocity)/(currTime - prevTime);
|
|
}
|
|
|
|
if (velocity > maxDriveVelocity)
|
|
{
|
|
maxDriveVelocity = velocity;
|
|
}
|
|
|
|
if (acceleration > maxDriveAcceleration)
|
|
{
|
|
maxDriveAcceleration = acceleration;
|
|
}
|
|
|
|
if (velPose.angle > maxTurnVelocity)
|
|
{
|
|
maxTurnVelocity = velPose.angle;
|
|
}
|
|
|
|
prevTime = currTime;
|
|
prevVelocity = velocity;
|
|
|
|
robot.dashboard.displayPrintf(lineNum++, "Drive Vel: (%.1f/%.1f)", velocity, maxDriveVelocity);
|
|
robot.dashboard.displayPrintf(
|
|
lineNum++, "Drive Accel: (%.1f/%.1f)", acceleration, maxDriveAcceleration);
|
|
robot.dashboard.displayPrintf(
|
|
lineNum++, "Turn Vel: (%.1f/%.1f)", velPose.angle, maxTurnVelocity);
|
|
}
|
|
break;
|
|
}
|
|
|
|
if (elapsedTimer != null)
|
|
{
|
|
elapsedTimer.recordPeriodTime();
|
|
robot.dashboard.displayPrintf(
|
|
15, "Period: %.3f(%.3f/%.3f)",
|
|
elapsedTimer.getAverageElapsedTime(), elapsedTimer.getMinElapsedTime(),
|
|
elapsedTimer.getMaxElapsedTime());
|
|
}
|
|
|
|
if (slowPeriodicLoop)
|
|
{
|
|
if (allowTeleOp())
|
|
{
|
|
//
|
|
// Allow TeleOp to run so we can control the robot in subsystem test or drive speed test modes.
|
|
//
|
|
super.periodic(elapsedTime, true);
|
|
}
|
|
|
|
switch (testChoices.test)
|
|
{
|
|
case SENSORS_TEST:
|
|
case SUBSYSTEMS_TEST:
|
|
doSensorsTest(lineNum);
|
|
break;
|
|
|
|
case VISION_TEST:
|
|
case TUNE_COLORBLOB_VISION:
|
|
doVisionTest(lineNum);
|
|
break;
|
|
|
|
case X_TIMED_DRIVE:
|
|
case Y_TIMED_DRIVE:
|
|
if (robot.robotDrive != null)
|
|
{
|
|
robot.dashboard.displayPrintf(lineNum++, "Timed Drive: %.0f sec", testChoices.driveTime);
|
|
robot.dashboard.displayPrintf(
|
|
lineNum++, "RobotPose=%s", robot.robotDrive.driveBase.getFieldPosition());
|
|
robot.dashboard.displayPrintf(
|
|
lineNum++, "rawEnc=lf:%.0f,rf:%.0f,lb:%.0f,rb:%.0f",
|
|
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_LEFT_FRONT].getPosition(),
|
|
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_RIGHT_FRONT].getPosition(),
|
|
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_LEFT_BACK].getPosition(),
|
|
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_RIGHT_BACK].getPosition());
|
|
}
|
|
break;
|
|
|
|
case TUNE_X_PID:
|
|
case TUNE_Y_PID:
|
|
case TUNE_TURN_PID:
|
|
if (robot.robotDrive != null && testChoices.tunePidCoeffs != null)
|
|
{
|
|
robot.dashboard.displayPrintf(7, "TunePid=%s", testChoices.tunePidCoeffs);
|
|
}
|
|
//
|
|
// Intentionally falling through.
|
|
//
|
|
case PURE_PURSUIT_DRIVE:
|
|
case PID_DRIVE:
|
|
if (robot.robotDrive != null)
|
|
{
|
|
TrcPidController xPidCtrl, yPidCtrl, turnPidCtrl;
|
|
if (testChoices.test == Test.PURE_PURSUIT_DRIVE)
|
|
{
|
|
xPidCtrl = robot.robotDrive.purePursuitDrive.getXPosPidCtrl();
|
|
yPidCtrl = robot.robotDrive.purePursuitDrive.getYPosPidCtrl();
|
|
turnPidCtrl = robot.robotDrive.purePursuitDrive.getTurnPidCtrl();
|
|
}
|
|
else
|
|
{
|
|
xPidCtrl = robot.robotDrive.pidDrive.getXPidCtrl();
|
|
yPidCtrl = robot.robotDrive.pidDrive.getYPidCtrl();
|
|
turnPidCtrl = robot.robotDrive.pidDrive.getTurnPidCtrl();
|
|
}
|
|
|
|
robot.dashboard.displayPrintf(
|
|
lineNum++, "RobotPose=%s,rawEnc=lf:%.0f,rf:%.0f,lb:%.0f,rb:%.0f",
|
|
robot.robotDrive.driveBase.getFieldPosition(),
|
|
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_LEFT_FRONT].getPosition(),
|
|
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_RIGHT_FRONT].getPosition(),
|
|
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_LEFT_BACK].getPosition(),
|
|
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_RIGHT_BACK].getPosition());
|
|
if (xPidCtrl != null)
|
|
{
|
|
xPidCtrl.displayPidInfo(lineNum);
|
|
lineNum += 2;
|
|
}
|
|
yPidCtrl.displayPidInfo(lineNum);
|
|
lineNum += 2;
|
|
turnPidCtrl.displayPidInfo(lineNum);
|
|
}
|
|
break;
|
|
|
|
case CALIBRATE_SWERVE_STEERING:
|
|
if (robot.robotDrive != null && (robot.robotDrive instanceof FtcSwerveDrive) && steerCalibrating)
|
|
{
|
|
FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive;
|
|
swerveDrive.runSteeringCalibration();
|
|
swerveDrive.displaySteerZeroCalibration(lineNum);
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
} //periodic
|
|
|
|
//
|
|
// Overrides TrcGameController.ButtonHandler in TeleOp.
|
|
//
|
|
|
|
/**
|
|
* This method is called when driver gamepad button event is detected.
|
|
*
|
|
* @param button specifies the button that generates the event.
|
|
* @param pressed specifies true if the button is pressed, false otherwise.
|
|
*/
|
|
@Override
|
|
public void driverButtonEvent(FtcGamepad.ButtonType button, boolean pressed)
|
|
{
|
|
boolean passToTeleOp = true;
|
|
//
|
|
// In addition to or instead of the gamepad controls handled by FtcTeleOp, we can add to or override the
|
|
// FtcTeleOp gamepad actions.
|
|
//
|
|
robot.dashboard.displayPrintf(8, "Driver: %s=%s", button, pressed? "Pressed": "Released");
|
|
switch (button)
|
|
{
|
|
case A:
|
|
if (testChoices.test == Test.CALIBRATE_SWERVE_STEERING)
|
|
{
|
|
if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive)
|
|
{
|
|
FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive;
|
|
|
|
steerCalibrating = !steerCalibrating;
|
|
if (steerCalibrating)
|
|
{
|
|
// Start steer calibration.
|
|
swerveDrive.startSteeringCalibration();
|
|
}
|
|
else
|
|
{
|
|
// Stop steer calibration.
|
|
swerveDrive.stopSteeringCalibration();
|
|
}
|
|
}
|
|
passToTeleOp = false;
|
|
}
|
|
break;
|
|
|
|
case 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;
|
|
}
|
|
}
|
|
passToTeleOp = false;
|
|
}
|
|
else if (testChoices.test == Test.VISION_TEST)
|
|
{
|
|
if (pressed)
|
|
{
|
|
fpsMeterEnabled = !fpsMeterEnabled;
|
|
robot.vision.setFpsMeterEnabled(fpsMeterEnabled);
|
|
robot.globalTracer.traceInfo(moduleName, "fpsMeterEnabled = %s", fpsMeterEnabled);
|
|
}
|
|
passToTeleOp = false;
|
|
}
|
|
break;
|
|
|
|
case 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;
|
|
}
|
|
}
|
|
passToTeleOp = false;
|
|
}
|
|
break;
|
|
|
|
case 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();
|
|
}
|
|
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:
|
|
if (testChoices.test == Test.VISION_TEST && robot.vision != null && robot.vision.vision != null)
|
|
{
|
|
if (pressed)
|
|
{
|
|
exposure -= 100;
|
|
robot.vision.vision.setManualExposure(exposure, null);
|
|
}
|
|
passToTeleOp = false;
|
|
}
|
|
break;
|
|
|
|
case RightBumper:
|
|
if (testChoices.test == Test.VISION_TEST && robot.vision != null && robot.vision.vision != null)
|
|
{
|
|
if (pressed)
|
|
{
|
|
exposure += 100;
|
|
robot.vision.vision.setManualExposure(exposure, null);
|
|
}
|
|
passToTeleOp = false;
|
|
}
|
|
break;
|
|
|
|
case DpadUp:
|
|
if (testChoices.test == Test.SUBSYSTEMS_TEST)
|
|
{
|
|
// If we are moving swerve steering, make sure TeleOp doesn't interfere.
|
|
teleOpControlEnabled = !pressed;
|
|
if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive)
|
|
{
|
|
FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive;
|
|
swerveDrive.setSteerAngle(0.0, false, true);
|
|
}
|
|
passToTeleOp = false;
|
|
}
|
|
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();
|
|
}
|
|
passToTeleOp = false;
|
|
}
|
|
break;
|
|
|
|
case DpadDown:
|
|
if (testChoices.test == Test.SUBSYSTEMS_TEST)
|
|
{
|
|
// If we are moving swerve steering, make sure TeleOp doesn't interfere.
|
|
teleOpControlEnabled = !pressed;
|
|
if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive)
|
|
{
|
|
FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive;
|
|
swerveDrive.setSteerAngle(180.0, false, true);
|
|
}
|
|
passToTeleOp = false;
|
|
}
|
|
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();
|
|
}
|
|
passToTeleOp = false;
|
|
}
|
|
break;
|
|
|
|
case DpadLeft:
|
|
if (testChoices.test == Test.SUBSYSTEMS_TEST)
|
|
{
|
|
// If we are moving swerve steering, make sure TeleOp doesn't interfere.
|
|
teleOpControlEnabled = !pressed;
|
|
if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive)
|
|
{
|
|
FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive;
|
|
swerveDrive.setSteerAngle(270.0, false, true);
|
|
}
|
|
passToTeleOp = false;
|
|
}
|
|
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;
|
|
}
|
|
passToTeleOp = false;
|
|
}
|
|
break;
|
|
|
|
case DpadRight:
|
|
if (testChoices.test == Test.SUBSYSTEMS_TEST)
|
|
{
|
|
// If we are moving swerve steering, make sure TeleOp doesn't interfere.
|
|
teleOpControlEnabled = !pressed;
|
|
if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive)
|
|
{
|
|
FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive;
|
|
swerveDrive.setSteerAngle(90.0, false, true);
|
|
}
|
|
passToTeleOp = false;
|
|
}
|
|
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;
|
|
}
|
|
passToTeleOp = false;
|
|
}
|
|
break;
|
|
|
|
case Back:
|
|
case Start:
|
|
break;
|
|
}
|
|
//
|
|
// If the control was not processed by this method, pass it back to TeleOp.
|
|
//
|
|
if (passToTeleOp)
|
|
{
|
|
super.driverButtonEvent(button, pressed);
|
|
}
|
|
} //driverButtonEvent
|
|
|
|
/**
|
|
* This method is called when operator gamepad button event is detected.
|
|
*
|
|
* @param button specifies the button that generates the event.
|
|
* @param pressed specifies true if the button is pressed, false otherwise.
|
|
*/
|
|
@Override
|
|
public void operatorButtonEvent(FtcGamepad.ButtonType button, boolean pressed)
|
|
{
|
|
boolean passToTeleOp = true;
|
|
//
|
|
// In addition to or instead of the gamepad controls handled by FtcTeleOp, we can add to or override the
|
|
// FtcTeleOp gamepad actions.
|
|
//
|
|
robot.dashboard.displayPrintf(8, "Operator: %s=%s", button, pressed? "Pressed": "Released");
|
|
switch (button)
|
|
{
|
|
case A:
|
|
case B:
|
|
case X:
|
|
case Y:
|
|
case LeftBumper:
|
|
case RightBumper:
|
|
case DpadUp:
|
|
case DpadDown:
|
|
case DpadLeft:
|
|
case DpadRight:
|
|
case Back:
|
|
case Start:
|
|
break;
|
|
}
|
|
//
|
|
// If the control was not processed by this method, pass it back to TeleOp.
|
|
//
|
|
if (passToTeleOp)
|
|
{
|
|
super.operatorButtonEvent(button, pressed);
|
|
}
|
|
} //operatorButtonEvent
|
|
|
|
/**
|
|
* This method displays the current color thresholds on the dashboard.
|
|
*/
|
|
private void updateColorThresholds()
|
|
{
|
|
robot.dashboard.displayPrintf(7, "Thresholds: %s", Arrays.toString(colorThresholds));
|
|
} //updateColorThresholds
|
|
|
|
/**
|
|
* This method creates and displays the test menus and record the selected choices.
|
|
*/
|
|
private void doTestMenus()
|
|
{
|
|
//
|
|
// Create menus.
|
|
//
|
|
testMenu = new FtcChoiceMenu<>("Tests:", null);
|
|
FtcValueMenu xTargetMenu = new FtcValueMenu(
|
|
"xTarget:", testMenu, -10.0, 10.0, 0.5, 0.0, " %.1f ft");
|
|
FtcValueMenu yTargetMenu = new FtcValueMenu(
|
|
"yTarget:", testMenu, -10.0, 10.0, 0.5, 0.0, " %.1f ft");
|
|
FtcValueMenu turnTargetMenu = new FtcValueMenu(
|
|
"turnTarget:", testMenu, -180.0, 180.0, 5.0, 0.0, " %.0f deg");
|
|
FtcValueMenu driveTimeMenu = new FtcValueMenu(
|
|
"Drive time:", testMenu, 1.0, 10.0, 1.0, 4.0, " %.0f sec");
|
|
FtcValueMenu drivePowerMenu = new FtcValueMenu(
|
|
"Drive power:", testMenu, -1.0, 1.0, 0.1, 0.5, " %.1f");
|
|
//
|
|
// PID Tuning menus.
|
|
//
|
|
FtcValueMenu tuneKpMenu = new FtcValueMenu(
|
|
"Kp:", testMenu, 0.0, 1.0, 0.001, this::getTuneKp, " %f");
|
|
FtcValueMenu tuneKiMenu = new FtcValueMenu(
|
|
"Ki:", tuneKpMenu, 0.0, 1.0, 0.001, this::getTuneKi, " %f");
|
|
FtcValueMenu tuneKdMenu = new FtcValueMenu(
|
|
"Kd:", tuneKiMenu, 0.0, 1.0, 0.001, this::getTuneKd, " %f");
|
|
FtcValueMenu tuneKfMenu = new FtcValueMenu(
|
|
"Kf:", tuneKdMenu, 0.0, 1.0, 0.001, this::getTuneKf, " %f");
|
|
FtcValueMenu tuneDistanceMenu = new FtcValueMenu(
|
|
"PID Tune distance:", tuneKfMenu, -10.0, 10.0, 0.5, 0.0,
|
|
" %.1f ft");
|
|
FtcValueMenu tuneHeadingMenu = new FtcValueMenu(
|
|
"PID Tune heading:", tuneDistanceMenu, -180.0, 180.0, 5.0, 0.0,
|
|
" %.0f deg");
|
|
FtcValueMenu tuneDrivePowerMenu = new FtcValueMenu(
|
|
"PID Tune drive power:", tuneHeadingMenu, -1.0, 1.0, 0.1, 1.0,
|
|
" %.1f");
|
|
//
|
|
// Populate menus.
|
|
//
|
|
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);
|
|
testMenu.addChoice("Y Timed drive", Test.Y_TIMED_DRIVE, false, driveTimeMenu);
|
|
testMenu.addChoice("PID drive", Test.PID_DRIVE, false, xTargetMenu);
|
|
testMenu.addChoice("Tune X PID", Test.TUNE_X_PID, false, tuneKpMenu);
|
|
testMenu.addChoice("Tune Y PID", Test.TUNE_Y_PID, false, tuneKpMenu);
|
|
testMenu.addChoice("Tune Turn PID", Test.TUNE_TURN_PID, false, tuneKpMenu);
|
|
testMenu.addChoice("Pure Pursuit Drive", Test.PURE_PURSUIT_DRIVE, false);
|
|
testMenu.addChoice("Calibrate Swerve Steering", Test.CALIBRATE_SWERVE_STEERING, false);
|
|
|
|
xTargetMenu.setChildMenu(yTargetMenu);
|
|
yTargetMenu.setChildMenu(turnTargetMenu);
|
|
turnTargetMenu.setChildMenu(drivePowerMenu);
|
|
driveTimeMenu.setChildMenu(drivePowerMenu);
|
|
tuneKpMenu.setChildMenu(tuneKiMenu);
|
|
tuneKiMenu.setChildMenu(tuneKdMenu);
|
|
tuneKdMenu.setChildMenu(tuneKfMenu);
|
|
tuneKfMenu.setChildMenu(tuneDistanceMenu);
|
|
tuneDistanceMenu.setChildMenu(tuneHeadingMenu);
|
|
tuneHeadingMenu.setChildMenu(tuneDrivePowerMenu);
|
|
//
|
|
// Traverse menus.
|
|
//
|
|
FtcMenu.walkMenuTree(testMenu);
|
|
//
|
|
// Fetch choices.
|
|
//
|
|
testChoices.test = testMenu.getCurrentChoiceObject();
|
|
testChoices.xTarget = xTargetMenu.getCurrentValue();
|
|
testChoices.yTarget = yTargetMenu.getCurrentValue();
|
|
testChoices.turnTarget = turnTargetMenu.getCurrentValue();
|
|
testChoices.driveTime = driveTimeMenu.getCurrentValue();
|
|
testChoices.drivePower = drivePowerMenu.getCurrentValue();
|
|
testChoices.tunePidCoeffs = new TrcPidController.PidCoefficients(
|
|
tuneKpMenu.getCurrentValue(), tuneKiMenu.getCurrentValue(),
|
|
tuneKdMenu.getCurrentValue(),tuneKfMenu.getCurrentValue());
|
|
testChoices.tuneDistance = tuneDistanceMenu.getCurrentValue();
|
|
testChoices.tuneHeading = tuneHeadingMenu.getCurrentValue();
|
|
testChoices.tuneDrivePower = tuneDrivePowerMenu.getCurrentValue();
|
|
|
|
TrcPidController tunePidCtrl = getTunePidController(testChoices.test);
|
|
if (tunePidCtrl != null)
|
|
{
|
|
//
|
|
// Write the user input PID coefficients to a cache file so tune PID menu can read them as start value
|
|
// next time.
|
|
//
|
|
pidCoeffCache.writeCachedPidCoeff(tunePidCtrl, testChoices.tunePidCoeffs);
|
|
}
|
|
//
|
|
// Show choices.
|
|
//
|
|
robot.dashboard.displayPrintf(1, "Test Choices: %s", testChoices);
|
|
} //doTestMenus
|
|
|
|
/**
|
|
* This method returns the PID controller for the tune test.
|
|
*
|
|
* @param test specifies the selected test.
|
|
* @return tune PID controller.
|
|
*/
|
|
private TrcPidController getTunePidController(Test test)
|
|
{
|
|
TrcPidController pidCtrl;
|
|
|
|
switch (test)
|
|
{
|
|
case TUNE_X_PID:
|
|
pidCtrl = robot.robotDrive.pidDrive.getXPidCtrl();
|
|
break;
|
|
|
|
case TUNE_Y_PID:
|
|
pidCtrl = robot.robotDrive.pidDrive.getYPidCtrl();
|
|
break;
|
|
|
|
case TUNE_TURN_PID:
|
|
pidCtrl = robot.robotDrive.pidDrive.getTurnPidCtrl();
|
|
break;
|
|
|
|
default:
|
|
pidCtrl = null;
|
|
}
|
|
|
|
return pidCtrl;
|
|
} //getTunePidController
|
|
|
|
/**
|
|
* This method is called by the tuneKpMenu to get the start value to be displayed as the current value of the menu.
|
|
*
|
|
* @return start Kp value of the PID controller being tuned.
|
|
*/
|
|
private double getTuneKp()
|
|
{
|
|
double value = 0.0;
|
|
TrcPidController tunePidCtrl = getTunePidController(testMenu.getCurrentChoiceObject());
|
|
|
|
if (tunePidCtrl != null)
|
|
{
|
|
value = pidCoeffCache.getCachedPidCoeff(tunePidCtrl).kP;
|
|
}
|
|
|
|
return value;
|
|
} //getTuneKp
|
|
|
|
/**
|
|
* This method is called by the tuneKiMenu to get the start value to be displayed as the current value of the menu.
|
|
*
|
|
* @return start Ki value of the PID controller being tuned.
|
|
*/
|
|
private double getTuneKi()
|
|
{
|
|
double value = 0.0;
|
|
TrcPidController tunePidCtrl = getTunePidController(testMenu.getCurrentChoiceObject());
|
|
|
|
if (tunePidCtrl != null)
|
|
{
|
|
value = pidCoeffCache.getCachedPidCoeff(tunePidCtrl).kI;
|
|
}
|
|
|
|
return value;
|
|
} //getTuneKi
|
|
|
|
/**
|
|
* This method is called by the tuneKdMenu to get the start value to be displayed as the current value of the menu.
|
|
*
|
|
* @return start Kd value of the PID controller being tuned.
|
|
*/
|
|
private double getTuneKd()
|
|
{
|
|
double value = 0.0;
|
|
TrcPidController tunePidCtrl = getTunePidController(testMenu.getCurrentChoiceObject());
|
|
|
|
if (tunePidCtrl != null)
|
|
{
|
|
value = pidCoeffCache.getCachedPidCoeff(tunePidCtrl).kD;
|
|
}
|
|
|
|
return value;
|
|
} //getTuneKd
|
|
|
|
/**
|
|
* This method is called by the tuneKfMenu to get the start value to be displayed as the current value of the menu.
|
|
*
|
|
* @return start Kf value of the PID controller being tuned.
|
|
*/
|
|
double getTuneKf()
|
|
{
|
|
double value = 0.0;
|
|
TrcPidController tunePidCtrl = getTunePidController(testMenu.getCurrentChoiceObject());
|
|
|
|
if (tunePidCtrl != null)
|
|
{
|
|
value = pidCoeffCache.getCachedPidCoeff(tunePidCtrl).kF;
|
|
}
|
|
|
|
return value;
|
|
} //getTuneKF
|
|
|
|
/**
|
|
* 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(int lineNum)
|
|
{
|
|
//
|
|
// Read all sensors and display on the dashboard.
|
|
// Drive the robot around to sample different locations of the field.
|
|
//
|
|
if (robot.robotDrive != null)
|
|
{
|
|
robot.dashboard.displayPrintf(
|
|
lineNum++, "DriveEnc: lf=%.0f,rf=%.0f,lb=%.0f,rb=%.0f",
|
|
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_LEFT_FRONT].getPosition(),
|
|
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_RIGHT_FRONT].getPosition(),
|
|
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_LEFT_BACK].getPosition(),
|
|
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_RIGHT_BACK].getPosition());
|
|
|
|
if (robot.robotDrive instanceof FtcSwerveDrive)
|
|
{
|
|
FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive;
|
|
robot.dashboard.displayPrintf(
|
|
lineNum++, "SteerEnc: lf=%.2f, rf=%.2f, lb=%.2f, rb=%.2f",
|
|
swerveDrive.steerEncoders[FtcRobotDrive.INDEX_LEFT_FRONT].getScaledPosition(),
|
|
swerveDrive.steerEncoders[FtcRobotDrive.INDEX_RIGHT_FRONT].getScaledPosition(),
|
|
swerveDrive.steerEncoders[FtcRobotDrive.INDEX_LEFT_BACK].getScaledPosition(),
|
|
swerveDrive.steerEncoders[FtcRobotDrive.INDEX_RIGHT_BACK].getScaledPosition());
|
|
robot.dashboard.displayPrintf(
|
|
lineNum++, "SteerRaw: lf=%.2f, rf=%.2f, lb=%.2f, rb=%.2f",
|
|
swerveDrive.steerEncoders[FtcRobotDrive.INDEX_LEFT_FRONT].getRawPosition(),
|
|
swerveDrive.steerEncoders[FtcRobotDrive.INDEX_RIGHT_FRONT].getRawPosition(),
|
|
swerveDrive.steerEncoders[FtcRobotDrive.INDEX_LEFT_BACK].getRawPosition(),
|
|
swerveDrive.steerEncoders[FtcRobotDrive.INDEX_RIGHT_BACK].getRawPosition());
|
|
}
|
|
|
|
if (robot.robotDrive.gyro != null)
|
|
{
|
|
robot.dashboard.displayPrintf(
|
|
lineNum++, "Gyro(x,y,z): Heading=(%.1f,%.1f,%.1f), Rate=(%.3f,%.3f,%.3f)",
|
|
robot.robotDrive.gyro.getXHeading().value, robot.robotDrive.gyro.getYHeading().value,
|
|
robot.robotDrive.gyro.getZHeading().value, robot.robotDrive.gyro.getXRotationRate().value,
|
|
robot.robotDrive.gyro.getYRotationRate().value, robot.robotDrive.gyro.getZRotationRate().value);
|
|
}
|
|
}
|
|
} //doSensorsTest
|
|
|
|
/**
|
|
* 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(int lineNum)
|
|
{
|
|
if (robot.vision != null)
|
|
{
|
|
if (robot.vision.limelightVision != null)
|
|
{
|
|
robot.vision.getLimelightDetectedObject(
|
|
robot.vision.limelightVision.getPipeline() == 0?
|
|
FtcLimelightVision.ResultType.Fiducial: FtcLimelightVision.ResultType.Python,
|
|
null, lineNum++);
|
|
}
|
|
|
|
if (robot.vision.rawColorBlobVision != null)
|
|
{
|
|
robot.vision.getDetectedRawColorBlob(lineNum++);
|
|
}
|
|
|
|
if (robot.vision.aprilTagVision != null)
|
|
{
|
|
robot.vision.getDetectedAprilTag(null, lineNum++);
|
|
}
|
|
|
|
if (robot.vision.redBlobVision != null)
|
|
{
|
|
robot.vision.getDetectedColorBlob(Vision.ColorBlobType.RedBlob, 0.0, lineNum++);
|
|
}
|
|
|
|
if (robot.vision.blueBlobVision != null)
|
|
{
|
|
robot.vision.getDetectedColorBlob(Vision.ColorBlobType.BlueBlob, 0.0, lineNum++);
|
|
}
|
|
|
|
if (robot.vision.vision != null)
|
|
{
|
|
// displayExposureSettings is only available for VisionPortal.
|
|
robot.vision.displayExposureSettings(lineNum++);
|
|
}
|
|
}
|
|
} //doVisionTest
|
|
|
|
/**
|
|
* This method is called to determine if Test mode is allowed to do teleop control of the robot.
|
|
*
|
|
* @return true to allow and false otherwise.
|
|
*/
|
|
private boolean allowTeleOp()
|
|
{
|
|
return teleOpControlEnabled &&
|
|
(testChoices.test == Test.SUBSYSTEMS_TEST || testChoices.test == Test.DRIVE_SPEED_TEST);
|
|
} //allowTeleOp
|
|
|
|
} //class FtcTest
|