Files
FtcTemplate/TeamCode/src/main/java/teamcode/FtcTest.java
Titan Robotics Club bcc3343a1c Syncd latest fixes in PurePursuitDrive.
Updated trclib, ftclib.
2025-01-08 20:30:15 -08:00

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