SCDS/FtcTemplate
7
0
Fork 0
mirror of https://github.com/trc492/FtcTemplate.git synced 2025-07-04 22:41:23 -07:00
Code Issues Packages Projects Releases Wiki Activity

Removed the fork relationship from FTC SDK so that people can fork this repo without problem if they already had an FTC SDK fork.

Browse Source
This commit is contained in:
Titan Robotics Club
2024-05-28 17:44:09 -07:00
parent 34a9ce290b
commit 653736be1c
145 changed files with 602787 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
TeamCode/src/main/java/TrcCommonLib Submodule

Submodule TeamCode/src/main/java/TrcCommonLib added at 25e3de57d1

1
TeamCode/src/main/java/TrcFtcLib Submodule

Submodule TeamCode/src/main/java/TrcFtcLib added at cb1d4d7d64

376
TeamCode/src/main/java/teamcode/FtcAuto.java Normal file
View File

@ -0,0 +1,376 @@
/*
* Copyright (c) 2022 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 com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import java.util.Locale;
import TrcCommonLib.command.CmdPidDrive;
import TrcCommonLib.command.CmdTimedDrive;
import TrcCommonLib.trclib.TrcDbgTrace;
import TrcCommonLib.trclib.TrcPose2D;
import TrcCommonLib.trclib.TrcRobot;
import TrcCommonLib.trclib.TrcTimer;
import TrcFtcLib.ftclib.FtcChoiceMenu;
import TrcFtcLib.ftclib.FtcMatchInfo;
import TrcFtcLib.ftclib.FtcMenu;
import TrcFtcLib.ftclib.FtcOpMode;
import TrcFtcLib.ftclib.FtcValueMenu;
/**
* This class contains the Autonomous Mode program.
*/
@Autonomous(name="FtcAutonomous", group="Ftcxxxx")
public class FtcAuto extends FtcOpMode
{
private static final String moduleName = FtcAuto.class.getSimpleName();
public enum Alliance
{
RED_ALLIANCE,
BLUE_ALLIANCE
} //enum Alliance
public enum StartPos
{
LEFT,
RIGHT
} //enum StartPos
public enum AutoStrategy
{
PID_DRIVE,
TIMED_DRIVE,
DO_NOTHING
} //enum AutoStrategy
/**
* This class stores the autonomous menu choices.
*/
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 double xTarget = 0.0;
public double yTarget = 0.0;
public double turnTarget = 0.0;
public double driveTime = 0.0;
public double drivePower = 0.0;
@Override
public String toString()
{
return String.format(
Locale.US,
"delay=%.0f " +
"alliance=\"%s\" " +
"startPos=\"%s\" " +
"strategy=\"%s\" " +
"xTarget=%.1f " +
"yTarget=%.1f " +
"turnTarget=%.0f " +
"driveTime=%.0f " +
"drivePower=%.1f",
delay, alliance, startPos, strategy, xTarget, yTarget, turnTarget, driveTime, drivePower);
} //toString
} //class AutoChoices
public static final AutoChoices autoChoices = new AutoChoices();
private Robot robot;
private TrcRobot.RobotCommand autoCommand;
//
// Implements 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()
{
//
// Create and initialize robot object.
//
robot = new Robot(TrcRobot.getRunMode());
//
// Open trace log.
//
if (RobotParams.Preferences.useTraceLog)
{
Robot.matchInfo = FtcMatchInfo.getMatchInfo();
String filePrefix = String.format(
Locale.US, "%s%02d_Auto", Robot.matchInfo.matchType, Robot.matchInfo.matchNumber);
TrcDbgTrace.openTraceLog(RobotParams.LOG_FOLDER_PATH, filePrefix);
}
//
// Create and run choice menus.
//
doAutoChoicesMenus();
//
// Create autonomous command according to chosen strategy.
//
switch (autoChoices.strategy)
{
case PID_DRIVE:
if (RobotParams.Preferences.robotType != RobotParams.RobotType.NoRobot)
{
autoCommand = new CmdPidDrive(
robot.robotDrive.driveBase, robot.robotDrive.pidDrive, autoChoices.delay,
autoChoices.drivePower, null,
new TrcPose2D(autoChoices.xTarget*12.0, autoChoices.yTarget*12.0, autoChoices.turnTarget));
}
break;
case TIMED_DRIVE:
if (RobotParams.Preferences.robotType != RobotParams.RobotType.NoRobot)
{
autoCommand = new CmdTimedDrive(
robot.robotDrive.driveBase, autoChoices.delay, autoChoices.driveTime,
0.0, autoChoices.drivePower, 0.0);
}
break;
case DO_NOTHING:
default:
autoCommand = null;
break;
}
if (robot.vision != null)
{
// Enabling vision early so we can detect target before match starts if necessary.
// Only enable the necessary vision for that purpose.
// if (robot.vision.aprilTagVision != null)
// {
// robot.globalTracer.traceInfo(moduleName, "Enabling AprilTagVision.");
// robot.vision.setAprilTagVisionEnabled(true);
// }
//
// if (robot.vision.redBlobVision != null)
// {
// robot.globalTracer.traceInfo(moduleName, "Enabling RedBlobVision.");
// robot.vision.setRedBlobVisionEnabled(true);
// }
//
// if (robot.vision.blueBlobVision != null)
// {
// robot.globalTracer.traceInfo(moduleName, "Enabling BlueBlobVision.");
// robot.vision.setBlueBlobVisionEnabled(true);
// }
//
// if (robot.vision.tensorFlowVision != null)
// {
// robot.globalTracer.traceInfo(moduleName, "Enabling TensorFlowVision.");
// robot.vision.setTensorFlowVisionEnabled(true);
// }
}
robot.zeroCalibrate();
} //robotInit
//
// Overrides TrcRobot.RobotMode methods.
//
/**
* 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.
*/
@Override
public void initPeriodic()
{
} //initPeriodic
/**
* This method is called when the competition mode is about to start. In FTC, this is called when the "Play"
* button on the Driver Station is pressed. Typically, you put code that will prepare the robot for start of
* competition here such as resetting the encoders/sensors and enabling some sensors to start sampling.
*
* @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)
{
if (TrcDbgTrace.isTraceLogOpened())
{
TrcDbgTrace.setTraceLogEnabled(true);
}
robot.globalTracer.traceInfo(
moduleName, "***** Starting autonomous: " + TrcTimer.getCurrentTimeString() + " *****");
if (Robot.matchInfo != null)
{
robot.globalTracer.logInfo(moduleName, "MatchInfo", Robot.matchInfo.toString());
}
robot.globalTracer.logInfo(moduleName, "AutoChoices", autoChoices.toString());
robot.dashboard.clearDisplay();
//
// Tell robot object opmode is about to start so it can do the necessary start initialization for the mode.
//
robot.startMode(nextMode);
if (robot.vision != null)
{
// We are done with detecting object with TensorFlow, shut it down.
if (robot.vision.tensorFlowVision != null)
{
robot.globalTracer.traceInfo(moduleName, "Disabling TensorFlowVision.");
robot.vision.setTensorFlowVisionEnabled(false);
}
}
if (robot.battery != null)
{
robot.battery.setEnabled(true);
}
} //startMode
/**
* This method is called when competition mode is about to end. Typically, you put code that will do clean
* up here such as disabling the sampling of some sensors.
*
* @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)
{
//
// Opmode is about to stop, cancel autonomous command in progress if any.
//
if (autoCommand != null)
{
autoCommand.cancel();
}
//
// Tell robot object opmode is about to stop so it can do the necessary cleanup for the mode.
//
robot.stopMode(prevMode);
if (robot.battery != null)
{
robot.battery.setEnabled(false);
}
printPerformanceMetrics();
robot.globalTracer.traceInfo(
moduleName, "***** Stopping autonomous: " + TrcTimer.getCurrentTimeString() + " *****");
if (TrcDbgTrace.isTraceLogOpened())
{
TrcDbgTrace.closeTraceLog();
}
} //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)
{
if (autoCommand != null)
{
//
// Run the autonomous command.
//
autoCommand.cmdPeriodic(elapsedTime);
}
} //periodic
/**
* This method creates the autonomous menus, displays them and stores the choices.
*/
private void doAutoChoicesMenus()
{
//
// Construct menus.
//
FtcValueMenu delayMenu = new FtcValueMenu("Delay time:", null, 0.0, 30.0, 1.0, 0.0, " %.0f sec");
FtcChoiceMenu<Alliance> allianceMenu = new FtcChoiceMenu<>("Alliance:", delayMenu);
FtcChoiceMenu<StartPos> startPosMenu = new FtcChoiceMenu<>("Start Position:", allianceMenu);
FtcChoiceMenu<AutoStrategy> strategyMenu = new FtcChoiceMenu<>("Auto Strategies:", startPosMenu);
FtcValueMenu xTargetMenu = new FtcValueMenu(
"xTarget:", strategyMenu, -12.0, 12.0, 0.5, 4.0, " %.1f ft");
FtcValueMenu yTargetMenu = new FtcValueMenu(
"yTarget:", xTargetMenu, -12.0, 12.0, 0.5, 4.0, " %.1f ft");
FtcValueMenu turnTargetMenu = new FtcValueMenu(
"turnTarget:", yTargetMenu, -180.0, 180.0, 5.0, 90.0, " %.0f deg");
FtcValueMenu driveTimeMenu = new FtcValueMenu(
"Drive time:", strategyMenu, 0.0, 30.0, 1.0, 5.0, " %.0f sec");
FtcValueMenu drivePowerMenu = new FtcValueMenu(
"Drive power:", strategyMenu, -1.0, 1.0, 0.1, 0.5, " %.1f");
// Link Value Menus to their children.
delayMenu.setChildMenu(allianceMenu);
xTargetMenu.setChildMenu(yTargetMenu);
yTargetMenu.setChildMenu(turnTargetMenu);
turnTargetMenu.setChildMenu(drivePowerMenu);
driveTimeMenu.setChildMenu(drivePowerMenu);
//
// Populate choice menus.
//
allianceMenu.addChoice("Red", Alliance.RED_ALLIANCE, true, startPosMenu);
allianceMenu.addChoice("Blue", Alliance.BLUE_ALLIANCE, false, startPosMenu);
startPosMenu.addChoice("Start Position Left", StartPos.LEFT, true, strategyMenu);
startPosMenu.addChoice("Start Position Right", StartPos.RIGHT, false, strategyMenu);
strategyMenu.addChoice("PID Drive", AutoStrategy.PID_DRIVE, false, xTargetMenu);
strategyMenu.addChoice("Timed Drive", AutoStrategy.TIMED_DRIVE, false, driveTimeMenu);
strategyMenu.addChoice("Do nothing", AutoStrategy.DO_NOTHING, true);
//
// Traverse menus.
//
FtcMenu.walkMenuTree(delayMenu);
//
// Fetch choices.
//
autoChoices.delay = delayMenu.getCurrentValue();
autoChoices.alliance = allianceMenu.getCurrentChoiceObject();
autoChoices.startPos = startPosMenu.getCurrentChoiceObject();
autoChoices.strategy = strategyMenu.getCurrentChoiceObject();
autoChoices.xTarget = xTargetMenu.getCurrentValue();
autoChoices.yTarget = yTargetMenu.getCurrentValue();
autoChoices.turnTarget = turnTargetMenu.getCurrentValue();
autoChoices.driveTime = driveTimeMenu.getCurrentValue();
autoChoices.drivePower = drivePowerMenu.getCurrentValue();
//
// Show choices.
//
robot.dashboard.displayPrintf(1, "Auto Choices: %s", autoChoices);
} //doAutoChoicesMenus
} //class FtcAuto

413
TeamCode/src/main/java/teamcode/FtcTeleOp.java Normal file
View File

@ -0,0 +1,413 @@
/*
* Copyright (c) 2022 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 com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import java.util.Locale;
import TrcCommonLib.trclib.TrcDbgTrace;
import TrcCommonLib.trclib.TrcDriveBase;
import TrcCommonLib.trclib.TrcGameController;
import TrcCommonLib.trclib.TrcPose2D;
import TrcCommonLib.trclib.TrcRobot;
import TrcCommonLib.trclib.TrcTimer;
import TrcFtcLib.ftclib.FtcGamepad;
import TrcFtcLib.ftclib.FtcOpMode;
import teamcode.drivebases.SwerveDrive;
/**
* This class contains the TeleOp Mode program.
*/
@TeleOp(name="FtcTeleOp", group="Ftcxxxx")
public class FtcTeleOp extends FtcOpMode
{
private static final String moduleName = FtcTeleOp.class.getSimpleName();
protected Robot robot;
protected FtcGamepad driverGamepad;
protected FtcGamepad operatorGamepad;
private double drivePowerScale = RobotParams.DRIVE_POWER_SCALE_NORMAL;
private double turnPowerScale = RobotParams.TURN_POWER_SCALE_NORMAL;
private boolean manualOverride = false;
private boolean relocalizing = false;
private TrcPose2D robotFieldPose = null;
//
// Implements 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()
{
//
// Create and initialize robot object.
//
robot = new Robot(TrcRobot.getRunMode());
//
// Open trace log.
//
if (RobotParams.Preferences.useTraceLog)
{
String filePrefix = Robot.matchInfo != null?
String.format(Locale.US, "%s%02d_TeleOp", Robot.matchInfo.matchType, Robot.matchInfo.matchNumber):
"Unknown_TeleOp";
TrcDbgTrace.openTraceLog(RobotParams.LOG_FOLDER_PATH, filePrefix);
}
//
// Create and initialize Gamepads.
//
driverGamepad = new FtcGamepad("DriverGamepad", gamepad1, this::driverButtonEvent);
operatorGamepad = new FtcGamepad("OperatorGamepad", gamepad2, this::operatorButtonEvent);
driverGamepad.setYInverted(true);
operatorGamepad.setYInverted(true);
setDriveOrientation(TrcDriveBase.DriveOrientation.ROBOT);
} //robotInit
//
// Overrides TrcRobot.RobotMode methods.
//
/**
* This method is called when the competition mode is about to start. In FTC, this is called when the "Play"
* button on the Driver Station is pressed. Typically, you put code that will prepare the robot for start of
* competition here such as resetting the encoders/sensors and enabling some sensors to start sampling.
*
* @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)
{
if (TrcDbgTrace.isTraceLogOpened())
{
TrcDbgTrace.setTraceLogEnabled(true);
}
robot.globalTracer.traceInfo(
moduleName, "***** Starting TeleOp: " + TrcTimer.getCurrentTimeString() + " *****");
robot.dashboard.clearDisplay();
//
// Tell robot object opmode is about to start so it can do the necessary start initialization for the mode.
//
robot.startMode(nextMode);
//
// Enable AprilTag vision for re-localization.
//
if (robot.vision != null && robot.vision.aprilTagVision != null)
{
robot.globalTracer.traceInfo(moduleName, "Enabling AprilTagVision.");
robot.vision.setAprilTagVisionEnabled(true);
}
} //startMode
/**
* This method is called when competition mode is about to end. Typically, you put code that will do clean
* up here such as disabling the sampling of some sensors.
*
* @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)
{
//
// Tell robot object opmode is about to stop so it can do the necessary cleanup for the mode.
//
robot.stopMode(prevMode);
printPerformanceMetrics();
robot.globalTracer.traceInfo(
moduleName, "***** Stopping TeleOp: " + TrcTimer.getCurrentTimeString() + " *****");
if (TrcDbgTrace.isTraceLogOpened())
{
TrcDbgTrace.closeTraceLog();
}
} //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)
{
if (slowPeriodicLoop)
{
//
// DriveBase subsystem.
//
if (robot.robotDrive != null)
{
double[] inputs = driverGamepad.getDriveInputs(
RobotParams.ROBOT_DRIVE_MODE, true, drivePowerScale, turnPowerScale);
if (robot.robotDrive.driveBase.supportsHolonomicDrive())
{
robot.robotDrive.driveBase.holonomicDrive(
null, inputs[0], inputs[1], inputs[2], robot.robotDrive.driveBase.getDriveGyroAngle());
}
else
{
robot.robotDrive.driveBase.arcadeDrive(inputs[1], inputs[2]);
}
robot.dashboard.displayPrintf(
1, "RobotDrive: Power=(%.2f,y=%.2f,rot=%.2f),Mode:%s",
inputs[0], inputs[1], inputs[2], robot.robotDrive.driveBase.getDriveOrientation());
// We are trying to re-localize the robot and vision hasn't seen AprilTag yet.
if (relocalizing && robotFieldPose == null)
{
robotFieldPose = robot.vision.getRobotFieldPose();
}
}
//
// Other subsystems.
//
if (RobotParams.Preferences.useSubsystems)
{
}
// Display subsystem status.
if (RobotParams.Preferences.doStatusUpdate)
{
robot.updateStatus();
}
}
} //periodic
/**
* This method sets the drive orientation mode and updates the LED to indicate so.
*
* @param orientation specifies the drive orientation (FIELD, ROBOT, INVERTED).
*/
public void setDriveOrientation(TrcDriveBase.DriveOrientation orientation)
{
if (robot.robotDrive != null)
{
robot.globalTracer.traceInfo(moduleName, "driveOrientation=" + orientation);
robot.robotDrive.driveBase.setDriveOrientation(
orientation, orientation == TrcDriveBase.DriveOrientation.FIELD);
if (robot.blinkin != null)
{
robot.blinkin.setDriveOrientation(orientation);
}
}
} //setDriveOrientation
//
// Implements TrcGameController.ButtonHandler interface.
//
/**
* This method is called when driver gamepad button event is detected.
*
* @param gamepad specifies the game controller object that generated the event.
* @param button specifies the button ID that generates the event.
* @param pressed specifies true if the button is pressed, false otherwise.
*/
public void driverButtonEvent(TrcGameController gamepad, int button, boolean pressed)
{
robot.dashboard.displayPrintf(8, "%s: %04x->%s", gamepad, button, pressed? "Pressed": "Released");
switch (button)
{
case FtcGamepad.GAMEPAD_A:
if (pressed)
{
robot.globalTracer.traceInfo(moduleName, ">>>>> CancelAll is pressed.");
if (robot.robotDrive != null)
{
// Cancel all auto-assist driving.
robot.robotDrive.cancel();
}
}
break;
case FtcGamepad.GAMEPAD_B:
break;
case FtcGamepad.GAMEPAD_X:
break;
case FtcGamepad.GAMEPAD_Y:
if (pressed && robot.robotDrive != null)
{
if (robot.robotDrive.driveBase.isGyroAssistEnabled())
{
// Disable GyroAssist drive.
robot.globalTracer.traceInfo(moduleName, ">>>>> Disabling GyroAssist.");
robot.robotDrive.driveBase.setGyroAssistEnabled(null);
}
else
{
// Enable GyroAssist drive.
robot.globalTracer.traceInfo(moduleName, ">>>>> Enabling GyroAssist.");
robot.robotDrive.driveBase.setGyroAssistEnabled(robot.robotDrive.pidDrive.getTurnPidCtrl());
}
}
break;
case FtcGamepad.GAMEPAD_LBUMPER:
// Toggle between field or robot oriented driving, only applicable for holonomic drive base.
if (pressed && robot.robotDrive != null && robot.robotDrive.driveBase.supportsHolonomicDrive())
{
if (robot.robotDrive.driveBase.getDriveOrientation() != TrcDriveBase.DriveOrientation.FIELD)
{
robot.globalTracer.traceInfo(moduleName, ">>>>> Enabling FIELD mode.");
setDriveOrientation(TrcDriveBase.DriveOrientation.FIELD);
}
else
{
robot.globalTracer.traceInfo(moduleName, ">>>>> Enabling ROBOT mode.");
setDriveOrientation(TrcDriveBase.DriveOrientation.ROBOT);
}
}
break;
case FtcGamepad.GAMEPAD_RBUMPER:
// Press and hold for slow drive.
if (pressed)
{
robot.globalTracer.traceInfo(moduleName, ">>>>> DrivePower slow.");
drivePowerScale = RobotParams.DRIVE_POWER_SCALE_SLOW;
turnPowerScale = RobotParams.TURN_POWER_SCALE_SLOW;
}
else
{
robot.globalTracer.traceInfo(moduleName, ">>>>> DrivePower normal.");
drivePowerScale = RobotParams.DRIVE_POWER_SCALE_NORMAL;
turnPowerScale = RobotParams.TURN_POWER_SCALE_NORMAL;
}
break;
case FtcGamepad.GAMEPAD_DPAD_UP:
break;
case FtcGamepad.GAMEPAD_DPAD_DOWN:
break;
case FtcGamepad.GAMEPAD_DPAD_LEFT:
break;
case FtcGamepad.GAMEPAD_DPAD_RIGHT:
break;
case FtcGamepad.GAMEPAD_START:
if (robot.vision != null && robot.vision.aprilTagVision != null && robot.robotDrive != null)
{
// On press of the button, we will start looking for AprilTag for re-localization.
// On release of the button, we will set the robot's field location if we found the AprilTag.
relocalizing = pressed;
if (!pressed)
{
if (robotFieldPose != null)
{
// Vision found an AprilTag, set the new robot field location.
robot.globalTracer.traceInfo(
moduleName, ">>>>> Finish re-localizing: pose=" + robotFieldPose);
robot.robotDrive.driveBase.setFieldPosition(robotFieldPose, false);
robotFieldPose = null;
}
}
else
{
robot.globalTracer.traceInfo(moduleName, ">>>>> Start re-localizing ...");
}
}
break;
case FtcGamepad.GAMEPAD_BACK:
if (pressed && robot.robotDrive != null && robot.robotDrive instanceof SwerveDrive)
{
// Drive base is a Swerve Drive, align all steering wheels forward.
robot.globalTracer.traceInfo(moduleName, ">>>>> Set SteerAngle to zero.");
((SwerveDrive) robot.robotDrive).setSteerAngle(0.0, false, false);
}
break;
}
} //driverButtonEvent
/**
* This method is called when operator gamepad button event is detected.
*
* @param gamepad specifies the game controller object that generated the event.
* @param button specifies the button ID that generates the event.
* @param pressed specifies true if the button is pressed, false otherwise.
*/
public void operatorButtonEvent(TrcGameController gamepad, int button, boolean pressed)
{
robot.dashboard.displayPrintf(8, "%s: %04x->%s", gamepad, button, pressed? "Pressed": "Released");
switch (button)
{
case FtcGamepad.GAMEPAD_A:
break;
case FtcGamepad.GAMEPAD_B:
break;
case FtcGamepad.GAMEPAD_X:
break;
case FtcGamepad.GAMEPAD_Y:
break;
case FtcGamepad.GAMEPAD_LBUMPER:
break;
case FtcGamepad.GAMEPAD_RBUMPER:
robot.globalTracer.traceInfo(moduleName, ">>>>> ManulOverride=" + pressed);
manualOverride = pressed;
break;
case FtcGamepad.GAMEPAD_DPAD_UP:
break;
case FtcGamepad.GAMEPAD_DPAD_DOWN:
break;
case FtcGamepad.GAMEPAD_DPAD_LEFT:
break;
case FtcGamepad.GAMEPAD_DPAD_RIGHT:
break;
case FtcGamepad.GAMEPAD_BACK:
if (pressed)
{
// Zero calibrate all subsystems (arm, elevator and turret).
robot.globalTracer.traceInfo(moduleName, ">>>>> ZeroCalibrate pressed.");
robot.zeroCalibrate(moduleName);
}
break;
}
} //operatorButtonEvent
} //class FtcTeleOp

1145
TeamCode/src/main/java/teamcode/FtcTest.java Normal file
View File

File diff suppressed because it is too large Load Diff

21
TeamCode/src/main/java/teamcode/LICENSE Normal file
View File

@ -0,0 +1,21 @@
MIT License
Copyright (c) 2022 Titan Robotics Club
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.

323
TeamCode/src/main/java/teamcode/Robot.java Normal file
View File

@ -0,0 +1,323 @@
/*
* Copyright (c) 2022 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 TrcCommonLib.trclib.TrcDbgTrace;
import TrcCommonLib.trclib.TrcDigitalInput;
import TrcCommonLib.trclib.TrcMotor;
import TrcCommonLib.trclib.TrcPose2D;
import TrcCommonLib.trclib.TrcRobot;
import TrcCommonLib.trclib.TrcServo;
import TrcCommonLib.trclib.TrcTimer;
import TrcFtcLib.ftclib.FtcDashboard;
import TrcFtcLib.ftclib.FtcMatchInfo;
import TrcFtcLib.ftclib.FtcOpMode;
import TrcFtcLib.ftclib.FtcRobotBattery;
import teamcode.drivebases.MecanumDrive;
import teamcode.drivebases.RobotDrive;
import teamcode.drivebases.SwerveDrive;
import teamcode.subsystems.BlinkinLEDs;
import teamcode.vision.Vision;
/**
* This class creates the robot object that consists of sensors, indicators, drive base and all the subsystems.
*/
public class Robot
{
private static final String moduleName = Robot.class.getSimpleName();
private static final double STATUS_UPDATE_INTERVAL = 0.1; // 100 msec
//
// Global objects.
//
public final FtcOpMode opMode;
public final TrcDbgTrace globalTracer;
public final FtcDashboard dashboard;
public static FtcMatchInfo matchInfo = null;
private static TrcPose2D endOfAutoRobotPose = null;
private static double nextStatusUpdateTime = 0.0;
//
// Vision subsystems.
//
public Vision vision;
//
// Sensors and indicators.
//
public BlinkinLEDs blinkin;
public FtcRobotBattery battery;
//
// Subsystems.
//
public RobotDrive robotDrive;
/**
* Constructor: Create an instance of the object.
*
* @param runMode specifies robot running mode (Auto, TeleOp, Test), can be used to create and initialize mode
* specific sensors and subsystems if necessary.
*/
public Robot(TrcRobot.RunMode runMode)
{
//
// Initialize global objects.
//
opMode = FtcOpMode.getInstance();
globalTracer = TrcDbgTrace.getGlobalTracer();
dashboard = FtcDashboard.getInstance();
nextStatusUpdateTime = TrcTimer.getCurrentTime();
speak("Init starting");
//
// Initialize vision subsystems.
//
if (RobotParams.Preferences.tuneColorBlobVision ||
RobotParams.Preferences.useAprilTagVision ||
RobotParams.Preferences.useColorBlobVision ||
RobotParams.Preferences.useTensorFlowVision)
{
vision = new Vision(this);
}
//
// If robotType is NoRobot, the robot controller is disconnected from the robot for testing vision.
// In this case, we should not instantiate any robot hardware.
//
if (RobotParams.Preferences.robotType != RobotParams.RobotType.NoRobot)
{
//
// Create and initialize sensors and indicators.
//
if (RobotParams.Preferences.useBlinkin)
{
blinkin = new BlinkinLEDs(RobotParams.HWNAME_BLINKIN);
}
if (RobotParams.Preferences.useBatteryMonitor)
{
battery = new FtcRobotBattery();
}
//
// Create and initialize RobotDrive.
//
robotDrive =
RobotParams.Preferences.robotType == RobotParams.RobotType.SwerveRobot?
new SwerveDrive(): new MecanumDrive();
//
// Create and initialize other subsystems.
//
if (RobotParams.Preferences.useSubsystems)
{
}
}
speak("Init complete");
} //Robot
/**
* This method returns the instance name.
*
* @return instance name.
*/
@Override
public String toString()
{
return RobotParams.ROBOT_NAME;
} //toString
/**
* This method is call when the robot mode is about to start. It contains code to initialize robot hardware
* necessary for running the robot mode.
*
* @param runMode specifies the robot mode it is about to start, can be used to initialize mode specific hardware.
*/
public void startMode(TrcRobot.RunMode runMode)
{
if (robotDrive != null)
{
//
// Since the IMU gyro is giving us cardinal heading, we need to enable its cardinal to cartesian converter.
//
if (robotDrive.gyro != null)
{
robotDrive.gyro.setEnabled(true);
// The following are performance counters, could be disabled for competition if you want.
// But it might give you some insight if somehow autonomous wasn't performing as expected.
robotDrive.gyro.setElapsedTimerEnabled(true);
}
//
// Enable odometry for all opmodes. We may need odometry in TeleOp for auto-assist drive.
//
robotDrive.driveBase.setOdometryEnabled(true);
if (runMode == TrcRobot.RunMode.TELEOP_MODE)
{
if (endOfAutoRobotPose != null)
{
// We had a previous autonomous run that saved the robot position at the end, use it.
robotDrive.driveBase.setFieldPosition(endOfAutoRobotPose);
globalTracer.traceInfo(moduleName, "Restore saved RobotPose=" + endOfAutoRobotPose);
}
}
// Consume it so it's no longer valid for next run.
endOfAutoRobotPose = null;
}
TrcDigitalInput.setElapsedTimerEnabled(true);
TrcMotor.setElapsedTimerEnabled(true);
TrcServo.setElapsedTimerEnabled(true);
} //startMode
/**
* This method is call when the robot mode is about to end. It contains code to cleanup robot hardware before
* exiting the robot mode.
*
* @param runMode specifies the robot mode it is about to stop, can be used to cleanup mode specific hardware.
*/
public void stopMode(TrcRobot.RunMode runMode)
{
//
// Print all performance counters if there are any.
//
if (robotDrive != null && robotDrive.gyro != null)
{
robotDrive.gyro.printElapsedTime(globalTracer);
robotDrive.gyro.setElapsedTimerEnabled(false);
}
TrcDigitalInput.printElapsedTime(globalTracer);
TrcDigitalInput.setElapsedTimerEnabled(false);
TrcMotor.printElapsedTime(globalTracer);
TrcMotor.setElapsedTimerEnabled(false);
TrcServo.printElapsedTime(globalTracer);
TrcServo.setElapsedTimerEnabled(false);
//
// Disable vision.
//
if (vision != null)
{
if (vision.rawColorBlobVision != null)
{
globalTracer.traceInfo(moduleName, "Disabling RawColorBlobVision.");
vision.setRawColorBlobVisionEnabled(false);
}
if (vision.aprilTagVision != null)
{
globalTracer.traceInfo(moduleName, "Disabling AprilTagVision.");
vision.setAprilTagVisionEnabled(false);
}
if (vision.redBlobVision != null)
{
globalTracer.traceInfo(moduleName, "Disabling RedBlobVision.");
vision.setRedBlobVisionEnabled(false);
}
if (vision.blueBlobVision != null)
{
globalTracer.traceInfo(moduleName, "Disabling BlueBlobVision.");
vision.setBlueBlobVisionEnabled(false);
}
if (vision.tensorFlowVision != null)
{
globalTracer.traceInfo(moduleName, "Disabling TensorFlowVision.");
vision.setTensorFlowVisionEnabled(false);
}
vision.close();
}
if (robotDrive != null)
{
if (runMode == TrcRobot.RunMode.AUTO_MODE)
{
// Save current robot location at the end of autonomous so subsequent teleop run can restore it.
endOfAutoRobotPose = robotDrive.driveBase.getFieldPosition();
globalTracer.traceInfo(moduleName, "Saved robot pose=" + endOfAutoRobotPose);
}
//
// Disable odometry.
//
robotDrive.driveBase.setOdometryEnabled(false);
//
// Disable gyro task.
//
if (robotDrive.gyro != null)
{
robotDrive.gyro.setEnabled(false);
}
}
} //stopMode
/**
* This method update all subsystem status on the dashboard.
*/
public void updateStatus()
{
if (TrcTimer.getCurrentTime() > nextStatusUpdateTime)
{
int lineNum = 2;
nextStatusUpdateTime += STATUS_UPDATE_INTERVAL;
if (robotDrive != null)
{
dashboard.displayPrintf(lineNum++, "DriveBase: Pose=%s", robotDrive.driveBase.getFieldPosition());
}
//
// Display other subsystem status here.
//
}
} //updateStatus
/**
* This method zero calibrates all subsystems.
*
* @param owner specifies the owner ID to check if the caller has ownership of the motor.
*/
public void zeroCalibrate(String owner)
{
} //zeroCalibrate
/**
* This method zero calibrates all subsystems.
*/
public void zeroCalibrate()
{
zeroCalibrate(null);
} //zeroCalibrate
/**
* This method sets the robot's starting position according to the autonomous choices.
*
* @param autoChoices specifies all the auto choices.
*/
public void setRobotStartPosition(FtcAuto.AutoChoices autoChoices)
{
} //setRobotStartPosition
/**
* This method sends the text string to the Driver Station to be spoken using text to speech.
*
* @param sentence specifies the sentence to be spoken by the Driver Station.
*/
public void speak(String sentence)
{
opMode.telemetry.speak(sentence);
} //speak
} //class Robot

295
TeamCode/src/main/java/teamcode/RobotParams.java Normal file
View File

@ -0,0 +1,295 @@
/*
* 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 android.os.Environment;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import org.openftc.easyopencv.OpenCvCameraRotation;
import TrcCommonLib.trclib.TrcDriveBase.DriveOrientation;
import TrcCommonLib.trclib.TrcHomographyMapper;
import TrcCommonLib.trclib.TrcPidController;
import TrcCommonLib.trclib.TrcPose2D;
import TrcCommonLib.trclib.TrcUtil;
import TrcFtcLib.ftclib.FtcGamepad;
/**
* This class contains robot and subsystem constants and parameters.
*/
public class RobotParams
{
public static enum RobotType
{
MecanumRobot,
SwerveRobot,
NoRobot
} //RobotType
/**
* This class contains robot preferences. It controls enabling/disabling of various robot features.
*/
public static class Preferences
{
// Global config
public static RobotType robotType = RobotType.MecanumRobot;
public static boolean inCompetition = false;
// Miscellaneous
public static boolean useTraceLog = true;
public static boolean useLoopPerformanceMonitor = true;
public static boolean useBlinkin = false;
public static boolean useBatteryMonitor = false;
public static boolean doStatusUpdate = !inCompetition;
// Vision
public static boolean useWebCam = true;
public static boolean hasWebCam2 = false;
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 showVisionView = !inCompetition;
public static boolean showVisionStat = false;
// Drive Base
public static boolean useExternalOdometry = false;
public static boolean doSwervePhysicalAlignment = false;
public static boolean swerveDualServoSteering = true;
// Subsystems
public static boolean useSubsystems = false;
} //class Preferences
public static final String ROBOT_NAME = "Robotxxxx";
public static final RevHubOrientationOnRobot.LogoFacingDirection hubLogoDirection =
RevHubOrientationOnRobot.LogoFacingDirection.UP;
public static final RevHubOrientationOnRobot.UsbFacingDirection hubUsbDirection =
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
public static final String TEAM_FOLDER_PATH =
Environment.getExternalStorageDirectory().getPath() + "/FIRST/ftcxxxx";
public static final String LOG_FOLDER_PATH = TEAM_FOLDER_PATH + "/tracelogs";
public static final String STEERING_CALIBRATION_DATA_FILE = "SteerCalibration.txt";
//
// Hardware names.
//
// Miscellaneous.
public static final String HWNAME_IMU = "imu";
public static final String HWNAME_WEBCAM1 = "Webcam 1";
public static final String HWNAME_WEBCAM2 = "Webcam 2";
public static final String HWNAME_BLINKIN = "blinkin";
// Drive Base.
public static final String HWNAME_LFDRIVE_MOTOR = "lfDriveMotor";
public static final String HWNAME_RFDRIVE_MOTOR = "rfDriveMotor";
public static final String HWNAME_LBDRIVE_MOTOR = "lbDriveMotor";
public static final String HWNAME_RBDRIVE_MOTOR = "rbDriveMotor";
public static final String HWNAME_LFSTEER_SERVO = "lfSteerServo";
public static final String HWNAME_RFSTEER_SERVO = "rfSteerServo";
public static final String HWNAME_LBSTEER_SERVO = "lbSteerServo";
public static final String HWNAME_RBSTEER_SERVO = "rbSteerServo";
public static final String HWNAME_LFSTEER_ENCODER = "lfSteerEncoder";
public static final String HWNAME_RFSTEER_ENCODER = "rfSteerEncoder";
public static final String HWNAME_LBSTEER_ENCODER = "lbSteerEncoder";
public static final String HWNAME_RBSTEER_ENCODER = "rbSteerEncoder";
// Subsystems.
//
// Field dimensions.
//
public static final double FULL_FIELD_INCHES = 141.24;
public static final double HALF_FIELD_INCHES = FULL_FIELD_INCHES/2.0;
public static final double FULL_TILE_INCHES = FULL_FIELD_INCHES/6.0;
//
// Robot dimensions.
//
public static final double ROBOT_LENGTH = 17.0;
public static final double ROBOT_WIDTH = 17.0;
public static final double DRIVE_BASE_LENGTH = (24.0 * 14)*TrcUtil.INCHES_PER_MM;
public static final double DRIVE_BASE_WIDTH = 16.0;
//
// Game related locations.
//
public static final TrcPose2D[] APRILTAG_POSES = new TrcPose2D[] {
new TrcPose2D(0.0, 0.0, 0.0), // TagId 1
new TrcPose2D(0.0, 0.0, 0.0), // TagId 2
new TrcPose2D(0.0, 0.0, 0.0), // TagId 3
new TrcPose2D(0.0, 0.0, 0.0) // TagId 4
};
//
// Vision subsystem.
//
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_X_OFFSET = 0.0; //Camera right offset from robot centroid
public static final double CAM_Y_OFFSET = 2.0; //Camera forward offset from robot centroid
public static final double CAM_Z_OFFSET = 9.75; //Camera height offset from floor
public static final double CAM_TILT_DOWN = 15.00;//Camera tilt down angle from horizontal in deg
public static final TrcPose2D CAM_POSE = new TrcPose2D(CAM_X_OFFSET, CAM_Y_OFFSET, 0.0);
// // Camera: Micorosoft Lifecam HD 3000 v1/v2
// public static final double WEBCAM_FX = 678.154; // in pixels
// public static final double WEBCAM_FY = 678.170; // in pixels
// public static final double WEBCAM_CX = 318.135; // in pixels
// public static final double WEBCAM_CY = 228.374; // in pixels
// // Camera: Logitech C270
// public static final double WEBCAM_FX = 822.317; // in pixels
// public static final double WEBCAM_FY = 822.317; // in pixels
// public static final double WEBCAM_CX = 319.495; // in pixels
// public static final double WEBCAM_CY = 242.502; // in pixels
// // Camera: Logitech C310
// public static final double WEBCAM_FX = 821.993; // in pixels
// public static final double WEBCAM_FY = 821.993; // in pixels
// public static final double WEBCAM_CX = 330.489; // in pixels
// public static final double WEBCAM_CY = 248.997; // in pixels
// // Camera: Logitech C920
// public static final double WEBCAM_FX = 622.001; // in pixels
// public static final double WEBCAM_FY = 622.001; // in pixels
// public static final double WEBCAM_CX = 319.803; // in pixels
// public static final double WEBCAM_CY = 241.251; // in pixels
// Measurement unit: pixels
public static final double HOMOGRAPHY_CAMERA_TOPLEFT_X = 0.0;
public static final double HOMOGRAPHY_CAMERA_TOPLEFT_Y = 120.0;
public static final double HOMOGRAPHY_CAMERA_TOPRIGHT_X = CAM_IMAGE_WIDTH - 1;
public static final double HOMOGRAPHY_CAMERA_TOPRIGHT_Y = 120.0;
public static final double HOMOGRAPHY_CAMERA_BOTTOMLEFT_X = 0.0;
public static final double HOMOGRAPHY_CAMERA_BOTTOMLEFT_Y = CAM_IMAGE_HEIGHT - 1;
public static final double HOMOGRAPHY_CAMERA_BOTTOMRIGHT_X = CAM_IMAGE_WIDTH - 1;
public static final double HOMOGRAPHY_CAMERA_BOTTOMRIGHT_Y = CAM_IMAGE_HEIGHT - 1;
// Measurement unit: inches
public static final double HOMOGRAPHY_WORLD_TOPLEFT_X = -12.5625;
public static final double HOMOGRAPHY_WORLD_TOPLEFT_Y = 48.0 - ROBOT_LENGTH/2.0 - CAM_Y_OFFSET;
public static final double HOMOGRAPHY_WORLD_TOPRIGHT_X = 11.4375;
public static final double HOMOGRAPHY_WORLD_TOPRIGHT_Y = 44.75 - ROBOT_LENGTH/2.0 - CAM_Y_OFFSET;
public static final double HOMOGRAPHY_WORLD_BOTTOMLEFT_X = -2.5625;
public static final double HOMOGRAPHY_WORLD_BOTTOMLEFT_Y = 21.0 - ROBOT_LENGTH/2.0 - CAM_Y_OFFSET;
public static final double HOMOGRAPHY_WORLD_BOTTOMRIGHT_X = 2.5626;
public static final double HOMOGRAPHY_WORLD_BOTTOMRIGHT_Y = 21.0 - ROBOT_LENGTH/2.0 + CAM_Y_OFFSET;
public static final TrcHomographyMapper.Rectangle cameraRect = new TrcHomographyMapper.Rectangle(
RobotParams.HOMOGRAPHY_CAMERA_TOPLEFT_X, RobotParams.HOMOGRAPHY_CAMERA_TOPLEFT_Y,
RobotParams.HOMOGRAPHY_CAMERA_TOPRIGHT_X, RobotParams.HOMOGRAPHY_CAMERA_TOPRIGHT_Y,
RobotParams.HOMOGRAPHY_CAMERA_BOTTOMLEFT_X, RobotParams.HOMOGRAPHY_CAMERA_BOTTOMLEFT_Y,
RobotParams.HOMOGRAPHY_CAMERA_BOTTOMRIGHT_X, RobotParams.HOMOGRAPHY_CAMERA_BOTTOMRIGHT_Y);
public static final TrcHomographyMapper.Rectangle worldRect = new TrcHomographyMapper.Rectangle(
RobotParams.HOMOGRAPHY_WORLD_TOPLEFT_X, RobotParams.HOMOGRAPHY_WORLD_TOPLEFT_Y,
RobotParams.HOMOGRAPHY_WORLD_TOPRIGHT_X, RobotParams.HOMOGRAPHY_WORLD_TOPRIGHT_Y,
RobotParams.HOMOGRAPHY_WORLD_BOTTOMLEFT_X, RobotParams.HOMOGRAPHY_WORLD_BOTTOMLEFT_Y,
RobotParams.HOMOGRAPHY_WORLD_BOTTOMRIGHT_X, RobotParams.HOMOGRAPHY_WORLD_BOTTOMRIGHT_Y);
//
// Motor Odometries.
//
// https://www.gobilda.com/5203-series-yellow-jacket-planetary-gear-motor-19-2-1-ratio-24mm-length-8mm-rex-shaft-312-rpm-3-3-5v-encoder/
public static final double GOBILDA_5203_312_ENCODER_PPR = (((1.0 + 46.0/17.0)*(1.0 + 46.0/11.0))*28.0);
public static final double GOBILDA_5203_312_RPM = 312.0;
public static final double GOBILDA_5203_312_MAX_VELOCITY_PPS=
GOBILDA_5203_312_ENCODER_PPR*GOBILDA_5203_312_RPM/60.0; // 2795.9872 pps
//https://www.gobilda.com/5203-series-yellow-jacket-planetary-gear-motor-13-7-1-ratio-24mm-length-8mm-rex-shaft-435-rpm-3-3-5v-encoder/
public static final double GOBILDA_5203_435_ENCODER_PPR = (((1.0 + 46.0/17.0)*(1.0 + 46.0/17.0))*28.0);
public static final double GOBILDA_5203_435_RPM = 435.0;
public static final double GOBILDA_5203_435_MAX_VELOCITY_PPS=
GOBILDA_5203_435_ENCODER_PPR*GOBILDA_5203_435_RPM/60.0; // 2787.9135 pps
//
// DriveBase subsystem.
//
public static DriveOrientation DEF_DRIVE_ORIENTATION = DriveOrientation.FIELD;
public static final double LFSTEER_ZERO_POS = 0.474812;
public static final double RFSTEER_ZERO_POS = 0.467663;
public static final double LBSTEER_ZERO_POS = 0.541338;
public static final double RBSTEER_ZERO_POS = 0.545340;
public static final boolean LFDRIVE_INVERTED = true;
public static final boolean RFDRIVE_INVERTED = false;
public static final boolean LBDRIVE_INVERTED = true;
public static final boolean RBDRIVE_INVERTED = false;
public static final boolean LFSTEER_INVERTED = true;
public static final boolean RFSTEER_INVERTED = true;
public static final boolean LBSTEER_INVERTED = true;
public static final boolean RBSTEER_INVERTED = true;
public static final double STEER_SERVO_KP = 0.01;
public static final double STEER_SERVO_KI = 0.0;
public static final double STEER_SERVO_KD = 0.0;
public static final double STEER_SERVO_KF = 0.0;
public static final double STEER_SERVO_IZONE = 0.0;
public static final double STEER_SERVO_TOLERANCE = 0.5;
public static final boolean DRIVE_WHEEL_BRAKE_MODE_ON = true;
public static final double TURN_POWER_LIMIT = 0.5;
public static final double DRIVE_POWER_SCALE_SLOW = 0.5;
public static final double DRIVE_POWER_SCALE_NORMAL = 1.0;
public static final double TURN_POWER_SCALE_SLOW = 0.5;
public static final double TURN_POWER_SCALE_NORMAL = 1.0;
// Optii Odometry Wheel:
public static final double ODWHEEL_DIAMETER = 35.0 * TrcUtil.INCHES_PER_MM;
public static final double ODWHEEL_CPR = 4096.0;
public static final double ODWHEEL_INCHES_PER_COUNT = Math.PI*ODWHEEL_DIAMETER/ODWHEEL_CPR;
// Scale = 0.00105687652708656383937269814237 inches/count
public static final double YLEFT_ODWHEEL_X_OFFSET = -144.0 * TrcUtil.INCHES_PER_MM;
public static final double YLEFT_ODWHEEL_Y_OFFSET = -12.0 * TrcUtil.INCHES_PER_MM;
public static final double YRIGHT_ODWHEEL_X_OFFSET = 144.0 * TrcUtil.INCHES_PER_MM;
public static final double YRIGHT_ODWHEEL_Y_OFFSET = -12.0 * TrcUtil.INCHES_PER_MM;
public static final double X_ODWHEEL_X_OFFSET = 0.0;
public static final double X_ODWHEEL_Y_OFFSET = -168.0 * TrcUtil.INCHES_PER_MM;
public static final FtcGamepad.DriveMode ROBOT_DRIVE_MODE = FtcGamepad.DriveMode.ARCADE_MODE;
//
// Velocity controlled constants.
//
public static final double DRIVE_MOTOR_MAX_VELOCITY_PPS = GOBILDA_5203_312_MAX_VELOCITY_PPS;
public static final TrcPidController.PidCoefficients xPosPidCoeff =
new TrcPidController.PidCoefficients(0.095, 0.0, 0.001, 0.0);
public static final double XPOS_TOLERANCE = 1.0;
public static final double XPOS_INCHES_PER_COUNT = 0.01924724265461924299065420560748;
public static final Double X_RAMP_RATE = null;//10.0;
public static final TrcPidController.PidCoefficients yPosPidCoeff =
new TrcPidController.PidCoefficients(0.06, 0.0, 0.002, 0.0);
public static final double YPOS_TOLERANCE = 1.0;
public static final double YPOS_INCHES_PER_COUNT = 0.02166184604662450653409090909091;
public static final Double Y_RAMP_RATE = null;//10.0;
public static final TrcPidController.PidCoefficients turnPidCoeff =
new TrcPidController.PidCoefficients(0.02, 0.08, 0.003, 0.0, 30.0);
public static final double TURN_TOLERANCE = 1.0;
public static final Double TURN_RAMP_RATE = null;//10.0;
//
// Pure Pursuit parameters.
//
// No-Load max velocity (i.e. theoretical maximum)
// goBILDA 5203-312 motor, max shaft speed = 312 RPM
// motor-to-wheel gear ratio = 1:1
// max wheel speed = pi * wheel diameter * wheel gear ratio * motor RPM / 60.0
// = 3.1415926535897932384626433832795 * 4 in. * 1.0 * 312.0 / 60.0
// = 65.345127194667699360022982372214 in./sec.
public static final double ROBOT_MAX_VELOCITY = 23.0; // measured maximum from drive speed test.
public static final double ROBOT_MAX_ACCELERATION = 500.0; // measured maximum from drive speed test.
// KF should be set to the reciprocal of max tangential velocity (time to travel unit distance), units: sec./in.
public static final TrcPidController.PidCoefficients velPidCoeff =
new TrcPidController.PidCoefficients(0.0, 0.0, 0.0, 1.0/ROBOT_MAX_VELOCITY);
public static final double PPD_FOLLOWING_DISTANCE = 6.0;
public static final double PPD_POS_TOLERANCE = 1.0;
public static final double PPD_POS_ERR_RATE_THRESHOLD = 1.0;
public static final double PPD_TURN_TOLERANCE = 1.0;
public static final double PPD_TURN_ERR_RATE_THRESHOLD = 1.0;
} //class RobotParams

141
TeamCode/src/main/java/teamcode/autocommands/CmdAuto.java Normal file
View File

@ -0,0 +1,141 @@
/*
* 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.autocommands;
import TrcCommonLib.trclib.TrcEvent;
import TrcCommonLib.trclib.TrcRobot;
import TrcCommonLib.trclib.TrcStateMachine;
import TrcCommonLib.trclib.TrcTimer;
import teamcode.FtcAuto;
import teamcode.Robot;
/**
* This class implements an autonomous strategy.
*/
public class CmdAuto implements TrcRobot.RobotCommand
{
private static final String moduleName = CmdAuto.class.getSimpleName();
private enum State
{
START,
DONE
} //enum State
private final Robot robot;
private final FtcAuto.AutoChoices autoChoices;
private final TrcTimer timer;
private final TrcEvent event;
private final TrcStateMachine<State> sm;
/**
* Constructor: Create an instance of the object.
*
* @param robot specifies the robot object for providing access to various global objects.
* @param autoChoices specifies the autoChoices object.
*/
public CmdAuto(Robot robot, FtcAuto.AutoChoices autoChoices)
{
this.robot = robot;
this.autoChoices = autoChoices;
timer = new TrcTimer(moduleName);
event = new TrcEvent(moduleName);
sm = new TrcStateMachine<>(moduleName);
sm.start(State.START);
} //CmdAuto
//
// Implements the TrcRobot.RobotCommand interface.
//
/**
* This method checks if the current RobotCommand is running.
*
* @return true if the command is running, false otherwise.
*/
@Override
public boolean isActive()
{
return sm.isEnabled();
} //isActive
/**
* This method cancels the command if it is active.
*/
@Override
public void cancel()
{
timer.cancel();
sm.stop();
} //cancel
/**
* This method must be called periodically by the caller to drive the command sequence forward.
*
* @param elapsedTime specifies the elapsed time in seconds since the start of the robot mode.
* @return true if the command sequence is completed, false otherwise.
*/
@Override
public boolean cmdPeriodic(double elapsedTime)
{
State state = sm.checkReadyAndGetState();
if (state == null)
{
robot.dashboard.displayPrintf(8, "State: disabled or waiting (nextState=%s)...", sm.getNextState());
}
else
{
robot.dashboard.displayPrintf(8, "State: %s", state);
switch (state)
{
case START:
if (autoChoices.delay > 0.0)
{
timer.set(autoChoices.delay, event);
sm.waitForSingleEvent(event, State.DONE);
}
else
{
sm.setState(State.DONE);
}
break;
default:
case DONE:
// We are done.
cancel();
break;
}
robot.globalTracer.traceStateInfo(
sm.toString(), state, robot.robotDrive.driveBase, robot.robotDrive.pidDrive,
robot.robotDrive.purePursuitDrive, null);
}
return !sm.isEnabled();
} //cmdPeriodic
} //class CmdAuto

181
TeamCode/src/main/java/teamcode/autotasks/TaskAuto.java Normal file
View File

@ -0,0 +1,181 @@
/*
* 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.autotasks;
import TrcCommonLib.trclib.TrcAutoTask;
import TrcCommonLib.trclib.TrcEvent;
import TrcCommonLib.trclib.TrcOwnershipMgr;
import TrcCommonLib.trclib.TrcRobot;
import TrcCommonLib.trclib.TrcTaskMgr;
import teamcode.Robot;
/**
* This class implements auto-assist task.
*/
public class TaskAuto extends TrcAutoTask<TaskAuto.State>
{
private static final String moduleName = TaskAuto.class.getSimpleName();
public enum State
{
START,
DONE
} //enum State
private static class TaskParams
{
TaskParams()
{
} //TaskParams
} //class TaskParams
private final String ownerName;
private final Robot robot;
private String currOwner = null;
/**
* Constructor: Create an instance of the object.
*
* @param ownerName specifies the owner name to take subsystem ownership, can be null if no ownership required.
* @param robot specifies the robot object that contains all the necessary subsystems.
*/
public TaskAuto(String ownerName, Robot robot)
{
super(moduleName, ownerName, TrcTaskMgr.TaskType.POST_PERIODIC_TASK);
this.ownerName = ownerName;
this.robot = robot;
} //TaskAuto
/**
* This method starts the auto-assist operation.
*
* @param completionEvent specifies the event to signal when done, can be null if none provided.
*/
public void autoAssist(TrcEvent completionEvent)
{
tracer.traceInfo(moduleName, "event=%s", completionEvent);
startAutoTask(State.START, new TaskParams(), completionEvent);
} //autoAssist
/**
* This method cancels an in progress auto-assist operation if any.
*/
public void autoAssistCancel()
{
tracer.traceInfo(moduleName, "Canceling auto-assist.");
stopAutoTask(false);
} //autoAssistCancel
//
// Implement TrcAutoTask abstract methods.
//
/**
* This method is called by the super class to acquire ownership of all subsystems involved in the auto-assist
* operation. This is typically done before starting an auto-assist operation.
*
* @return true if acquired all subsystems ownership, false otherwise. It releases all ownership if any acquire
* failed.
*/
@Override
protected boolean acquireSubsystemsOwnership()
{
boolean success = ownerName == null ||
(robot.robotDrive.driveBase.acquireExclusiveAccess(ownerName));
if (success)
{
currOwner = ownerName;
tracer.traceInfo(moduleName, "Successfully acquired subsystem ownerships.");
}
else
{
TrcOwnershipMgr ownershipMgr = TrcOwnershipMgr.getInstance();
tracer.traceWarn(
moduleName, "Failed to acquire subsystem ownership (currOwner=%s, robotDrive=%s).",
currOwner, ownershipMgr.getOwner(robot.robotDrive.driveBase));
releaseSubsystemsOwnership();
}
return success;
} //acquireSubsystemsOwnership
/**
* This method is called by the super class to release ownership of all subsystems involved in the auto-assist
* operation. This is typically done if the auto-assist operation is completed or canceled.
*/
@Override
protected void releaseSubsystemsOwnership()
{
if (ownerName != null)
{
TrcOwnershipMgr ownershipMgr = TrcOwnershipMgr.getInstance();
tracer.traceInfo(
moduleName, "Releasing subsystem ownership (currOwner=%s, robotDrive=%s).",
currOwner, ownershipMgr.getOwner(robot.robotDrive.driveBase));
robot.robotDrive.driveBase.releaseExclusiveAccess(currOwner);
currOwner = null;
}
} //releaseSubsystemsOwnership
/**
* This method is called by the super class to stop all the subsystems.
*/
@Override
protected void stopSubsystems()
{
tracer.traceInfo(moduleName, "Stopping subsystems.");
robot.robotDrive.cancel(currOwner);
} //stopSubsystems
/**
* This methods is called periodically to run the auto-assist task.
*
* @param params specifies the task parameters.
* @param state specifies the current state of the task.
* @param taskType specifies the type of task being run.
* @param runMode specifies the competition mode (e.g. Autonomous, TeleOp, Test).
* @param slowPeriodicLoop specifies true if it is running the slow periodic loop on the main robot thread,
* false if running the fast loop on the main robot thread.
*/
@Override
protected void runTaskState(
Object params, State state, TrcTaskMgr.TaskType taskType, TrcRobot.RunMode runMode, boolean slowPeriodicLoop)
{
TaskParams taskParams = (TaskParams) params;
switch (state)
{
case START:
break;
default:
case DONE:
// Stop task.
stopAutoTask(true);
break;
}
} //runTaskState
} //class TaskAuto

103
TeamCode/src/main/java/teamcode/drivebases/MecanumDrive.java Normal file
View File

@ -0,0 +1,103 @@
/*
* 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.drivebases;
import TrcCommonLib.trclib.TrcDbgTrace;
import TrcCommonLib.trclib.TrcMecanumDriveBase;
import TrcCommonLib.trclib.TrcOdometryWheels;
import TrcCommonLib.trclib.TrcPidDrive;
import TrcCommonLib.trclib.TrcPurePursuitDrive;
import teamcode.RobotParams;
/**
* This class creates the RobotDrive subsystem that consists of wheel motors and related objects for driving the
* robot.
*/
public class MecanumDrive extends RobotDrive
{
/**
* Constructor: Create an instance of the object.
*/
public MecanumDrive()
{
super();
driveMotors = createDriveMotors(driveMotorNames, driveMotorInverted);
driveBase = new TrcMecanumDriveBase(
driveMotors[INDEX_LEFT_FRONT], driveMotors[INDEX_LEFT_BACK],
driveMotors[INDEX_RIGHT_FRONT], driveMotors[INDEX_RIGHT_BACK], gyro);
if (RobotParams.Preferences.useExternalOdometry)
{
// Create the external odometry device that uses the right back encoder port as the X odometry and
// the left and right front encoder ports as the Y1 and Y2 odometry. Gyro will serve as the angle
// odometry.
TrcOdometryWheels driveBaseOdometry = new TrcOdometryWheels(
new TrcOdometryWheels.AxisSensor(
driveMotors[INDEX_RIGHT_BACK], RobotParams.X_ODWHEEL_Y_OFFSET, RobotParams.X_ODWHEEL_X_OFFSET),
new TrcOdometryWheels.AxisSensor[] {
new TrcOdometryWheels.AxisSensor(
driveMotors[INDEX_LEFT_FRONT], RobotParams.YLEFT_ODWHEEL_X_OFFSET,
RobotParams.YLEFT_ODWHEEL_Y_OFFSET),
new TrcOdometryWheels.AxisSensor(
driveMotors[INDEX_RIGHT_FRONT], RobotParams.YRIGHT_ODWHEEL_X_OFFSET,
RobotParams.YRIGHT_ODWHEEL_Y_OFFSET)},
gyro);
// Set the drive base to use the external odometry device overriding the built-in one.
driveBase.setDriveBaseOdometry(driveBaseOdometry);
driveBase.setOdometryScales(RobotParams.ODWHEEL_INCHES_PER_COUNT, RobotParams.ODWHEEL_INCHES_PER_COUNT);
}
else
{
driveBase.setOdometryScales(RobotParams.XPOS_INCHES_PER_COUNT, RobotParams.YPOS_INCHES_PER_COUNT);
}
//
// Create and initialize PID controllers.
//
pidDrive = new TrcPidDrive(
"pidDrive", driveBase,
RobotParams.xPosPidCoeff, RobotParams.XPOS_TOLERANCE, driveBase::getXPosition,
RobotParams.yPosPidCoeff, RobotParams.YPOS_TOLERANCE, driveBase::getYPosition,
RobotParams.turnPidCoeff, RobotParams.TURN_TOLERANCE, driveBase::getHeading);
pidDrive.setStallDetectionEnabled(true);
pidDrive.getXPidCtrl().setRampRate(RobotParams.X_RAMP_RATE);
pidDrive.getYPidCtrl().setRampRate(RobotParams.Y_RAMP_RATE);
pidDrive.getTurnPidCtrl().setRampRate(RobotParams.TURN_RAMP_RATE);
pidDrive.getTurnPidCtrl().setAbsoluteSetPoint(true);
// FTC robots generally have USB performance issues where the sampling rate of the gyro is not high enough.
// If the robot turns too fast, PID will cause oscillation. By limiting turn power, the robot turns slower.
pidDrive.getTurnPidCtrl().setOutputLimit(RobotParams.TURN_POWER_LIMIT);
// AbsoluteTargetMode eliminates cumulative errors on multi-segment runs because drive base is keeping track
// of the absolute target position.
pidDrive.setAbsoluteTargetModeEnabled(true);
pidDrive.setTraceLevel(TrcDbgTrace.MsgLevel.INFO, false, false, false);
purePursuitDrive = new TrcPurePursuitDrive(
"purePursuitDrive", driveBase,
RobotParams.PPD_FOLLOWING_DISTANCE, RobotParams.PPD_POS_TOLERANCE, RobotParams.PPD_TURN_TOLERANCE,
RobotParams.xPosPidCoeff, RobotParams.yPosPidCoeff, RobotParams.turnPidCoeff, RobotParams.velPidCoeff);
purePursuitDrive.setStallDetectionEnabled(true);
purePursuitDrive.setFastModeEnabled(true);
purePursuitDrive.setTraceLevel(TrcDbgTrace.MsgLevel.INFO, false, false, false);
} //MecanumDrive
} //class MecanumDrive

122
TeamCode/src/main/java/teamcode/drivebases/RobotDrive.java Normal file
View File

@ -0,0 +1,122 @@
/*
* 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.drivebases;
import TrcCommonLib.trclib.TrcDriveBase;
import TrcCommonLib.trclib.TrcPidDrive;
import TrcCommonLib.trclib.TrcPurePursuitDrive;
import TrcCommonLib.trclib.TrcUtil;
import TrcFtcLib.ftclib.FtcDcMotor;
import TrcFtcLib.ftclib.FtcImu;
import teamcode.RobotParams;
/**
* This class is intended to be extended by subclasses implementing different robot drive bases.
*/
public class RobotDrive
{
public static final int INDEX_LEFT_FRONT = 0;
public static final int INDEX_RIGHT_FRONT = 1;
public static final int INDEX_LEFT_BACK = 2;
public static final int INDEX_RIGHT_BACK = 3;
protected final String[] driveMotorNames = {
RobotParams.HWNAME_LFDRIVE_MOTOR, RobotParams.HWNAME_RFDRIVE_MOTOR,
RobotParams.HWNAME_LBDRIVE_MOTOR, RobotParams.HWNAME_RBDRIVE_MOTOR};
protected final boolean[] driveMotorInverted = {
RobotParams.LFDRIVE_INVERTED, RobotParams.RFDRIVE_INVERTED,
RobotParams.LBDRIVE_INVERTED, RobotParams.RBDRIVE_INVERTED};
//
// Sensors.
//
public final FtcImu gyro;
//
// Subclass needs to initialize the following variables.
//
// Drive motors.
public FtcDcMotor[] driveMotors;
// Drive Base.
public TrcDriveBase driveBase;
// Drive Controllers.
public TrcPidDrive pidDrive;
public TrcPurePursuitDrive purePursuitDrive;
/**
* Constructor: Create an instance of the object.
*/
public RobotDrive()
{
gyro = new FtcImu(RobotParams.HWNAME_IMU, RobotParams.hubLogoDirection, RobotParams.hubUsbDirection);
} //RobotDrive
/**
* This method cancels any PIDDrive operation still in progress.
*
* @param owner specifies the owner that requested the cancel.
*/
public void cancel(String owner)
{
if (pidDrive != null && pidDrive.isActive())
{
pidDrive.cancel(owner);
}
if (purePursuitDrive != null && purePursuitDrive.isActive())
{
purePursuitDrive.cancel(owner);
}
driveBase.stop(owner);
} //cancel
/**
* This method cancels any PIDDrive operation still in progress.
*/
public void cancel()
{
cancel(null);
} //cancel
/**
* This method creates and configures all drive motors.
*
* @param motorNames specifies an array of names for each drive motor.
* @param inverted specifies an array of boolean indicating if the drive motor needs to be inverted.
* @return an array of created drive motors.
*/
protected FtcDcMotor[] createDriveMotors(String[] motorNames, boolean[] inverted)
{
FtcDcMotor[] motors = new FtcDcMotor[motorNames.length];
for (int i = 0; i < motorNames.length; i++)
{
motors[i] = new FtcDcMotor(motorNames[i]);
motors[i].setBrakeModeEnabled(RobotParams.DRIVE_WHEEL_BRAKE_MODE_ON);
motors[i].setMotorInverted(inverted[i]);
motors[i].setVoltageCompensationEnabled(TrcUtil.BATTERY_NOMINAL_VOLTAGE);
}
return motors;
} //createDriveMotors
} //class RobotDrive

393
TeamCode/src/main/java/teamcode/drivebases/SwerveDrive.java Normal file
View File

@ -0,0 +1,393 @@
/*
* 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.drivebases;
import java.io.FileNotFoundException;
import java.io.FileOutputStream;
import java.io.FileReader;
import java.io.PrintStream;
import java.util.Arrays;
import java.util.Scanner;
import TrcCommonLib.trclib.TrcDbgTrace;
import TrcCommonLib.trclib.TrcOdometryWheels;
import TrcCommonLib.trclib.TrcPidDrive;
import TrcCommonLib.trclib.TrcPurePursuitDrive;
import TrcCommonLib.trclib.TrcSwerveDriveBase;
import TrcCommonLib.trclib.TrcSwerveModule;
import TrcFtcLib.ftclib.FtcAnalogEncoder;
import TrcFtcLib.ftclib.FtcCRServo;
import TrcFtcLib.ftclib.FtcDcMotor;
import teamcode.RobotParams;
/**
* This class creates the RobotDrive subsystem that consists of wheel motors and related objects for driving the
* robot.
*/
public class SwerveDrive extends RobotDrive
{
private static final String moduleName = SwerveDrive.class.getSimpleName();
private static final TrcDbgTrace globalTracer = TrcDbgTrace.getGlobalTracer();
private final String[] steerEncoderNames = {
RobotParams.HWNAME_LFSTEER_ENCODER, RobotParams.HWNAME_RFSTEER_ENCODER,
RobotParams.HWNAME_LBSTEER_ENCODER, RobotParams.HWNAME_RBSTEER_ENCODER};
public double[] zeroPositions = {
RobotParams.LFSTEER_ZERO_POS, RobotParams.RFSTEER_ZERO_POS,
RobotParams.LBSTEER_ZERO_POS, RobotParams.RBSTEER_ZERO_POS};
private final String[] steerServoNames = {
RobotParams.HWNAME_LFSTEER_SERVO, RobotParams.HWNAME_RFSTEER_SERVO,
RobotParams.HWNAME_LBSTEER_SERVO, RobotParams.HWNAME_RBSTEER_SERVO};
private final boolean[] steerServoInverted = {
RobotParams.LFSTEER_INVERTED, RobotParams.RFSTEER_INVERTED,
RobotParams.LBSTEER_INVERTED, RobotParams.RBSTEER_INVERTED};
private final String[] swerveModuleNames = {"lfSwerveModule", "rfSwerveModule", "lbSwerveModule", "rbSwerveModule"};
//
// Swerve steering motors and modules.
//
public final FtcAnalogEncoder[] steerEncoders;
public final FtcCRServo[] steerServos;
public final TrcSwerveModule[] swerveModules;
public int calibrationCount = 0;
/**
* Constructor: Create an instance of the object.
*/
public SwerveDrive()
{
super();
readSteeringCalibrationData();
driveMotors = createDriveMotors(driveMotorNames, driveMotorInverted);
steerEncoders = createSteerEncoders(
steerEncoderNames,
RobotParams.Preferences.doSwervePhysicalAlignment ? new double[] {0.0, 0.0, 0.0, 0.0} : zeroPositions);
steerServos = createSteerServos(steerServoNames, steerServoInverted, steerEncoders);
swerveModules = createSwerveModules(swerveModuleNames, driveMotors, steerServos);
driveBase = new TrcSwerveDriveBase(
swerveModules[INDEX_LEFT_FRONT], swerveModules[INDEX_LEFT_BACK],
swerveModules[INDEX_RIGHT_FRONT], swerveModules[INDEX_RIGHT_BACK],
gyro, RobotParams.DRIVE_BASE_WIDTH, RobotParams.DRIVE_BASE_LENGTH);
driveBase.setDriveOrientation(RobotParams.DEF_DRIVE_ORIENTATION, true);
if (RobotParams.Preferences.useExternalOdometry)
{
//
// Create the external odometry device that uses the right back encoder port as the X odometry and
// the left and right front encoder ports as the Y1 and Y2 odometry. Gyro will serve as the angle
// odometry.
//
TrcOdometryWheels driveBaseOdometry = new TrcOdometryWheels(
new TrcOdometryWheels.AxisSensor(
driveMotors[INDEX_RIGHT_BACK], RobotParams.X_ODWHEEL_Y_OFFSET, RobotParams.X_ODWHEEL_X_OFFSET),
new TrcOdometryWheels.AxisSensor[] {
new TrcOdometryWheels.AxisSensor(
driveMotors[INDEX_LEFT_FRONT], RobotParams.YLEFT_ODWHEEL_X_OFFSET,
RobotParams.YLEFT_ODWHEEL_Y_OFFSET),
new TrcOdometryWheels.AxisSensor(
driveMotors[INDEX_RIGHT_FRONT], RobotParams.YRIGHT_ODWHEEL_X_OFFSET,
RobotParams.YRIGHT_ODWHEEL_Y_OFFSET)},
gyro);
//
// Set the drive base to use the external odometry device overriding the built-in one.
//
driveBase.setDriveBaseOdometry(driveBaseOdometry);
driveBase.setOdometryScales(RobotParams.ODWHEEL_INCHES_PER_COUNT, RobotParams.ODWHEEL_INCHES_PER_COUNT);
}
else
{
driveBase.setOdometryScales(RobotParams.YPOS_INCHES_PER_COUNT, RobotParams.YPOS_INCHES_PER_COUNT);
}
//
// Create and initialize PID controllers.
//
pidDrive = new TrcPidDrive(
"pidDrive", driveBase,
RobotParams.xPosPidCoeff, RobotParams.XPOS_TOLERANCE, driveBase::getXPosition,
RobotParams.yPosPidCoeff, RobotParams.YPOS_TOLERANCE, driveBase::getYPosition,
RobotParams.turnPidCoeff, RobotParams.TURN_TOLERANCE, driveBase::getHeading);
pidDrive.getXPidCtrl().setRampRate(RobotParams.X_RAMP_RATE);
pidDrive.getYPidCtrl().setRampRate(RobotParams.Y_RAMP_RATE);
pidDrive.getTurnPidCtrl().setRampRate(RobotParams.TURN_RAMP_RATE);
pidDrive.getTurnPidCtrl().setAbsoluteSetPoint(true);
// FTC robots generally have USB performance issues where the sampling rate of the gyro is not high enough.
// If the robot turns too fast, PID will cause oscillation. By limiting turn power, the robot turns slower.
pidDrive.getTurnPidCtrl().setOutputLimit(RobotParams.TURN_POWER_LIMIT);
// AbsoluteTargetMode eliminates cumulative errors on multi-segment runs because drive base is keeping track
// of the absolute target position.
pidDrive.setAbsoluteTargetModeEnabled(true);
pidDrive.setTraceLevel(TrcDbgTrace.MsgLevel.INFO, false, false, false);
purePursuitDrive = new TrcPurePursuitDrive(
"purePursuitDrive", driveBase,
RobotParams.PPD_FOLLOWING_DISTANCE, RobotParams.PPD_POS_TOLERANCE, RobotParams.PPD_TURN_TOLERANCE,
RobotParams.xPosPidCoeff, RobotParams.yPosPidCoeff, RobotParams.turnPidCoeff, RobotParams.velPidCoeff);
purePursuitDrive.setFastModeEnabled(true);
purePursuitDrive.setTraceLevel(TrcDbgTrace.MsgLevel.INFO, false, false, false);
} //SwerveDrive
/**
* This method creates and configures all steer encoders.
*
* @param encoderNames specifies an array of names for each steer encoder.
* @param zeroOffsets specifies an array of zero offsets for each steer encoder.
* @return an array of created steer encoders.
*/
private FtcAnalogEncoder[] createSteerEncoders(String[] encoderNames, double[] zeroOffsets)
{
FtcAnalogEncoder[] encoders = new FtcAnalogEncoder[encoderNames.length];
for (int i = 0; i < steerEncoderNames.length; i++)
{
encoders[i] = new FtcAnalogEncoder(encoderNames[i]);
encoders[i].setScaleAndOffset(180.0, 0.0, zeroOffsets[i]);
// Enable Cartesian converter.
encoders[i].setEnabled(true);
}
return encoders;
} //createSteerEncoders
/**
* This method creates and configures all steer servos.
*
* @param servoNames specifies an array of names for each steer servo.
* @param inverted specifies an array of boolean indicating if the servo needs to be inverted.
* @param encoders specifies an array of encoders for each steer servo.
* @return an array of created steer servos.
*/
private FtcCRServo[] createSteerServos(String[] servoNames, boolean[] inverted, FtcAnalogEncoder[] encoders)
{
FtcCRServo[] servos = new FtcCRServo[servoNames.length];
for (int i = 0; i < servoNames.length; i++)
{
servos[i] = new FtcCRServo(servoNames[i], encoders[i]);
if (RobotParams.Preferences.swerveDualServoSteering)
{
FtcCRServo slaveServo = new FtcCRServo(servoNames[i] + ".slave", null);
slaveServo.follow(servos[i], false);
}
servos[i].setMotorInverted(inverted[i]);
servos[i].setSoftwarePidEnabled(true);
servos[i].setPositionPidCoefficients(
RobotParams.STEER_SERVO_KP, RobotParams.STEER_SERVO_KI,
RobotParams.STEER_SERVO_KD, RobotParams.STEER_SERVO_KF, RobotParams.STEER_SERVO_IZONE);
servos[i].setPositionPidTolerance(RobotParams.STEER_SERVO_TOLERANCE);
}
return servos;
} //createSteerServos
/**
* This method creates and configures all swerve modules.
*
* @param moduleNames specifies an array of names for each swerve module.needs to be inverted.
* @param driveMotors specifies an array of drive motors.
* @param steerServos specifies an array of steer servos.
* @return an array of created swerve modules.
*/
private TrcSwerveModule[] createSwerveModules(
String[] moduleNames, FtcDcMotor[] driveMotors, FtcCRServo[] steerServos)
{
TrcSwerveModule[] modules = new TrcSwerveModule[moduleNames.length];
for (int i = 0; i < moduleNames.length; i++)
{
modules[i] = new TrcSwerveModule(moduleNames[i], driveMotors[i], steerServos[i]);
}
return modules;
} //createSwerveModules
/**
* This method enables/disables performance monitoring of all steering servo motors.
*
* @param enabled specifies true to enable, false to disable.
*/
public void setSteerPerformanceMonitorEnabled(boolean enabled)
{
for (FtcCRServo servo: steerServos)
{
servo.setPerformanceMonitorEnabled(enabled);
}
} //setSteerPerformanceMonitorEnabled
/**
* This method prints the performance info to the trace log.
*/
public void printSteerPerformanceInfo()
{
for (FtcCRServo servo : steerServos)
{
servo.printPidControlTaskPerformance();
}
} //printSteerPerformanceInfo
/**
* This method sets the steering angle of all swerve modules.
*
* @param angle specifies the steer angle.
* @param optimize specifies true to optimize (only turns within +/- 90 degrees), false otherwse.
* @param hold specifies true to hold the angle, false otherwise.
*/
public void setSteerAngle(double angle, boolean optimize, boolean hold)
{
for (TrcSwerveModule module: swerveModules)
{
module.setSteerAngle(angle, optimize, hold);
}
} //setSteerAngle
/**
* This method checks if anti-defense mode is enabled.
*
* @return true if anti-defense mode is enabled, false if disabled.
*/
public boolean isAntiDefenseEnabled()
{
return ((TrcSwerveDriveBase) driveBase).isAntiDefenseEnabled();
} //isAntiDefenseEnabled
/**
* This method enables/disables the anti-defense mode where it puts all swerve wheels into an X-formation.
* By doing so, it is very difficult for others to push us around.
*
* @param owner specifies the ID string of the caller for checking ownership, can be null if caller is not
* ownership aware.
* @param enabled specifies true to enable anti-defense mode, false to disable.
*/
public void setAntiDefenseEnabled(String owner, boolean enabled)
{
if (owner == null || !enabled || driveBase.acquireExclusiveAccess(owner))
{
((TrcSwerveDriveBase) driveBase).setAntiDefenseEnabled(owner, enabled);
if (!enabled)
{
driveBase.releaseExclusiveAccess(owner);
}
}
} //setAntiDefenseEnabled
/**
* This method starts the steering calibration.
*/
public void startSteeringCalibration()
{
calibrationCount = 0;
Arrays.fill(zeroPositions, 0.0);
} //startSteeringCalibration
/**
* This method stops the steering calibration and saves the calibration data to a file.
*/
public void stopSteeringCalibration()
{
for (int i = 0; i < zeroPositions.length; i++)
{
zeroPositions[i] /= calibrationCount;
}
calibrationCount = 0;
saveSteeringCalibrationData();
} //stopSteeringCalibration
/**
* This method is called periodically to sample the steer encoders for averaging the zero position data.
*/
public void runSteeringCalibration()
{
for (int i = 0; i < zeroPositions.length; i++)
{
zeroPositions[i] += steerEncoders[i].getRawPosition();
}
calibrationCount++;
} //runSteeringCalibration
/**
* This method saves the calibration data to a file on the Robot Controller.
*/
public void saveSteeringCalibrationData()
{
try (PrintStream out = new PrintStream(new FileOutputStream(
RobotParams.TEAM_FOLDER_PATH + "/" + RobotParams.STEERING_CALIBRATION_DATA_FILE)))
{
for (int i = 0; i < steerServoNames.length; i++)
{
out.printf("%s: %f\n", steerServoNames[i], zeroPositions[i]);
}
out.close();
globalTracer.traceInfo(
moduleName, "SteeringCalibrationData%s=%s",
Arrays.toString(steerServoNames), Arrays.toString(zeroPositions));
}
catch (FileNotFoundException e)
{
e.printStackTrace();
}
} //saveSteeringCalibrationData
/**
* This method reads the steering calibration data from a file on the Robot Controller.
*
* @throws RuntimeException if file contains invalid data.
*/
public void readSteeringCalibrationData()
{
String line = null;
try (Scanner in = new Scanner(new FileReader(
RobotParams.TEAM_FOLDER_PATH + "/" + RobotParams.STEERING_CALIBRATION_DATA_FILE)))
{
for (int i = 0; i < steerServoNames.length; i++)
{
line = in.nextLine();
int colonPos = line.indexOf(':');
String name = colonPos == -1? null: line.substring(0, colonPos);
if (name == null || !name.equals(steerServoNames[i]))
{
throw new RuntimeException("Invalid servo name in line " + line);
}
zeroPositions[i] = Double.parseDouble(line.substring(colonPos + 1));
}
}
catch (FileNotFoundException e)
{
globalTracer.traceWarn(moduleName, "Steering calibration data file not found, using built-in defaults.");
}
catch (NumberFormatException e)
{
globalTracer.traceErr(moduleName, "Invalid zero position value in line %s", line);
}
catch (RuntimeException e)
{
globalTracer.traceErr(moduleName, "Invalid servo name in line %s", line);
}
globalTracer.traceInfo(
moduleName, "SteeringCalibrationData%s=%s", Arrays.toString(steerServoNames),
Arrays.toString(zeroPositions));
} //readSteeringCalibrationData
} //class SwerveDrive

108
TeamCode/src/main/java/teamcode/subsystems/BlinkinLEDs.java Normal file
View File

@ -0,0 +1,108 @@
/*
* 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.subsystems;
import TrcCommonLib.trclib.TrcDriveBase;
import TrcCommonLib.trclib.TrcRevBlinkin;
import TrcFtcLib.ftclib.FtcRevBlinkin;
/**
* This class encapsulates the REV Blinkin LED controller to provide a priority indicator showing the status of the
* robot.
*/
public class BlinkinLEDs extends FtcRevBlinkin
{
// LED pattern names.
public static final String APRIL_TAG = "AprilTag";
public static final String RED_BLOB = "RedBlob";
public static final String BLUE_BLOB = "BlueBlob";
public static final String TENSOR_FLOW = "TensorFlow";
public static final String DRIVE_ORIENTATION_FIELD = "FieldMode";
public static final String DRIVE_ORIENTATION_ROBOT = "RobotMode";
public static final String DRIVE_ORIENTATION_INVERTED = "InvertedMode";
public static final String OFF_PATTERN = "Off";
/**
* Constructor: Create an instance of the object.
*
* @param instanceName specifies the instance name. This is also the REV Blinkin's hardware name.
*/
public BlinkinLEDs(String instanceName)
{
super(instanceName);
// LED Patterns are sorted in decreasing priority order.
final TrcRevBlinkin.Pattern[] ledPatternPriorities = {
// Highest priority.
new TrcRevBlinkin.Pattern(APRIL_TAG, RevLedPattern.SolidAqua),
new TrcRevBlinkin.Pattern(RED_BLOB, RevLedPattern.SolidRed),
new TrcRevBlinkin.Pattern(BLUE_BLOB, RevLedPattern.SolidBlue),
new TrcRevBlinkin.Pattern(TENSOR_FLOW, RevLedPattern.SolidYellow),
new TrcRevBlinkin.Pattern(DRIVE_ORIENTATION_FIELD, RevLedPattern.SolidViolet),
new TrcRevBlinkin.Pattern(DRIVE_ORIENTATION_ROBOT, RevLedPattern.SolidWhite),
new TrcRevBlinkin.Pattern(DRIVE_ORIENTATION_INVERTED, RevLedPattern.SolidGray),
new TrcRevBlinkin.Pattern(OFF_PATTERN, RevLedPattern.SolidBlack)
// Lowest priority.
};
setPatternPriorities(ledPatternPriorities);
} //BlinkinLEDs
/**
* This method sets the pattern ON for a period of time and turns off automatically afterwards.
*
* @param patternName specifies the name of the LED pattern to turn on.
*/
public void setDetectedPattern(String patternName)
{
setPatternState(patternName, true, 0.5);
} //setDetectedPattern
/**
* This method sets the LED to indicate the drive orientation mode of the robot.
*
* @param orientation specifies the drive orientation mode.
*/
public void setDriveOrientation(TrcDriveBase.DriveOrientation orientation)
{
switch (orientation)
{
case INVERTED:
setPatternState(DRIVE_ORIENTATION_INVERTED, true);
setPatternState(DRIVE_ORIENTATION_ROBOT, false);
setPatternState(DRIVE_ORIENTATION_FIELD, false);
break;
case ROBOT:
setPatternState(DRIVE_ORIENTATION_INVERTED, false);
setPatternState(DRIVE_ORIENTATION_ROBOT, true);
setPatternState(DRIVE_ORIENTATION_FIELD, false);
break;
case FIELD:
setPatternState(DRIVE_ORIENTATION_INVERTED, false);
setPatternState(DRIVE_ORIENTATION_ROBOT, false);
setPatternState(DRIVE_ORIENTATION_FIELD, true);
break;
}
} //setDriveOrientation
} //class BlinkinLEDs

642
TeamCode/src/main/java/teamcode/vision/Vision.java Normal file
View File

@ -0,0 +1,642 @@
/*
* 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.vision;
import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.vision.VisionProcessor;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
import org.firstinspires.ftc.vision.tfod.TfodProcessor;
import org.opencv.imgproc.Imgproc;
import org.openftc.easyopencv.OpenCvCamera;
import org.openftc.easyopencv.OpenCvCameraFactory;
import java.util.ArrayList;
import TrcCommonLib.trclib.TrcDbgTrace;
import TrcCommonLib.trclib.TrcOpenCvColorBlobPipeline;
import TrcCommonLib.trclib.TrcOpenCvDetector;
import TrcCommonLib.trclib.TrcPose2D;
import TrcCommonLib.trclib.TrcVisionTargetInfo;
import TrcFtcLib.ftclib.FtcEocvColorBlobProcessor;
import TrcFtcLib.ftclib.FtcOpMode;
import TrcFtcLib.ftclib.FtcRawEocvColorBlobPipeline;
import TrcFtcLib.ftclib.FtcRawEocvVision;
import TrcFtcLib.ftclib.FtcVision;
import TrcFtcLib.ftclib.FtcVisionAprilTag;
import TrcFtcLib.ftclib.FtcVisionEocvColorBlob;
import TrcFtcLib.ftclib.FtcVisionTensorFlow;
import teamcode.Robot;
import teamcode.RobotParams;
import teamcode.subsystems.BlinkinLEDs;
/**
* This class implements AprilTag/TensorFlow/Eocv Vision for the game season. It creates and initializes all the
* vision target info as well as providing info for the robot, camera and the field. It also provides methods to get
* the location of the robot and detected targets.
*/
public class Vision
{
private static final String moduleName = Vision.class.getSimpleName();
// Warning: EOCV converts camera stream to RGBA whereas Desktop OpenCV converts it to BGRA. Therefore, the correct
// color conversion must be RGBA (or RGB) to whatever color space you want to convert.
//
// YCrCb Color Space.
private static final int colorConversion = Imgproc.COLOR_RGB2YCrCb;
private static final double[] redBlobColorThresholds = {20.0, 120.0, 180.0, 240.0, 90.0, 120.0};
private static final double[] blueBlobColorThresholds = {20.0, 250.0, 40.0, 250.0, 160.0, 240.0};
private static final TrcOpenCvColorBlobPipeline.FilterContourParams colorBlobFilterContourParams =
new TrcOpenCvColorBlobPipeline.FilterContourParams()
.setMinArea(5000.0)
.setMinPerimeter(200.0)
.setWidthRange(50.0, 1000.0)
.setHeightRange(80.0, 1000.0)
.setSolidityRange(0.0, 100.0)
.setVerticesRange(0.0, 1000.0)
.setAspectRatioRange(0.3, 1.0);
private static final String TFOD_MODEL_ASSET = "MyObject.tflite";
private static final float TFOD_MIN_CONFIDENCE = 0.75f;
public static final String TFOD_OBJECT_LABEL = "MyObject";
public static final String[] TFOD_TARGET_LABELS = {TFOD_OBJECT_LABEL};
private final TrcDbgTrace tracer;
private final Robot robot;
private final WebcamName webcam1, webcam2;
private FtcRawEocvColorBlobPipeline rawColorBlobPipeline;
public FtcRawEocvVision rawColorBlobVision;
public FtcVisionAprilTag aprilTagVision;
private AprilTagProcessor aprilTagProcessor;
public FtcVisionEocvColorBlob redBlobVision;
private FtcEocvColorBlobProcessor redBlobProcessor;
public FtcVisionEocvColorBlob blueBlobVision;
private FtcEocvColorBlobProcessor blueBlobProcessor;
public FtcVisionTensorFlow tensorFlowVision;
private TfodProcessor tensorFlowProcessor;
public FtcVision vision;
/**
* Constructor: Create an instance of the object.
*
* @param robot specifies the robot object.
*/
public Vision(Robot robot)
{
FtcOpMode opMode = FtcOpMode.getInstance();
this.tracer = new TrcDbgTrace();
this.robot = robot;
this.webcam1 = opMode.hardwareMap.get(WebcamName.class, RobotParams.HWNAME_WEBCAM1);
this.webcam2 = RobotParams.Preferences.hasWebCam2?
opMode.hardwareMap.get(WebcamName.class, RobotParams.HWNAME_WEBCAM2): null;
if (RobotParams.Preferences.tuneColorBlobVision)
{
OpenCvCamera openCvCamera;
if (RobotParams.Preferences.showVisionView)
{
int cameraViewId = opMode.hardwareMap.appContext.getResources().getIdentifier(
"cameraMonitorViewId", "id", opMode.hardwareMap.appContext.getPackageName());
openCvCamera = OpenCvCameraFactory.getInstance().createWebcam(webcam1, cameraViewId);
}
else
{
openCvCamera = OpenCvCameraFactory.getInstance().createWebcam(webcam1);
}
tracer.traceInfo(moduleName, "Starting RawEocvColorBlobVision...");
rawColorBlobPipeline = new FtcRawEocvColorBlobPipeline(
"rawColorBlobPipeline", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, true);
// By default, display original Mat.
rawColorBlobPipeline.setVideoOutput(0);
rawColorBlobPipeline.setAnnotateEnabled(true);
rawColorBlobVision = new FtcRawEocvVision(
"rawColorBlobVision", RobotParams.CAM_IMAGE_WIDTH, RobotParams.CAM_IMAGE_HEIGHT, null, null,
openCvCamera, RobotParams.CAM_ORIENTATION);
rawColorBlobVision.setFpsMeterEnabled(RobotParams.Preferences.showVisionStat);
setRawColorBlobVisionEnabled(false);
}
else
{
// Creating Vision Processors for VisionPortal.
ArrayList<VisionProcessor> visionProcessorsList = new ArrayList<>();
if (RobotParams.Preferences.useAprilTagVision)
{
tracer.traceInfo(moduleName, "Starting AprilTagVision...");
FtcVisionAprilTag.Parameters aprilTagParams = new FtcVisionAprilTag.Parameters()
.setDrawTagIdEnabled(true)
.setDrawTagOutlineEnabled(true)
.setDrawAxesEnabled(false)
.setDrawCubeProjectionEnabled(false)
// .setLensIntrinsics(
// RobotParams.WEBCAM_FX, RobotParams.WEBCAM_FY, RobotParams.WEBCAM_CX, RobotParams.WEBCAM_CY)
.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES);
aprilTagVision = new FtcVisionAprilTag(aprilTagParams, AprilTagProcessor.TagFamily.TAG_36h11);
aprilTagProcessor = aprilTagVision.getVisionProcessor();
visionProcessorsList.add(aprilTagProcessor);
}
if (RobotParams.Preferences.useColorBlobVision)
{
tracer.traceInfo(moduleName, "Starting ColorBlobVision...");
redBlobVision = new FtcVisionEocvColorBlob(
"RedBlob", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, true,
RobotParams.cameraRect, RobotParams.worldRect, true);
redBlobProcessor = redBlobVision.getVisionProcessor();
visionProcessorsList.add(redBlobProcessor);
blueBlobVision = new FtcVisionEocvColorBlob(
"BlueBlob", colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams, true,
RobotParams.cameraRect, RobotParams.worldRect, true);
blueBlobProcessor = blueBlobVision.getVisionProcessor();
visionProcessorsList.add(blueBlobProcessor);
}
if (RobotParams.Preferences.useTensorFlowVision)
{
tracer.traceInfo(moduleName, "Starting TensorFlowVision...");
tensorFlowVision = new FtcVisionTensorFlow(
null, true, TFOD_MODEL_ASSET, TFOD_TARGET_LABELS, RobotParams.cameraRect, RobotParams.worldRect);
tensorFlowProcessor = tensorFlowVision.getVisionProcessor();
tensorFlowProcessor.setMinResultConfidence(TFOD_MIN_CONFIDENCE);
visionProcessorsList.add(tensorFlowProcessor);
}
VisionProcessor[] visionProcessors = new VisionProcessor[visionProcessorsList.size()];
visionProcessorsList.toArray(visionProcessors);
if (RobotParams.Preferences.useWebCam)
{
// Use USB webcams.
vision = new FtcVision(
webcam1, webcam2, RobotParams.CAM_IMAGE_WIDTH, RobotParams.CAM_IMAGE_HEIGHT,
RobotParams.Preferences.showVisionView, RobotParams.Preferences.showVisionStat, visionProcessors);
}
else
{
// Use phone camera.
vision = new FtcVision(
RobotParams.Preferences.useBuiltinCamBack?
BuiltinCameraDirection.BACK: BuiltinCameraDirection.FRONT,
RobotParams.CAM_IMAGE_WIDTH, RobotParams.CAM_IMAGE_HEIGHT,
RobotParams.Preferences.showVisionView, RobotParams.Preferences.showVisionStat, visionProcessors);
}
// Disable all vision until they are needed.
for (VisionProcessor processor: visionProcessors)
{
vision.setProcessorEnabled(processor, false);
}
}
} //Vision
/**
* This method closes the vision portal and is normally called at the end of an opmode.
*/
public void close()
{
if (vision != null)
{
vision.getVisionPortal().close();
}
} //close
/**
* This method enables/disables FPS meter on the viewport.
*
* @param enabled specifies true to enable FPS meter, false to disable.
*/
public void setFpsMeterEnabled(boolean enabled)
{
if (rawColorBlobVision != null)
{
rawColorBlobVision.setFpsMeterEnabled(enabled);
}
else if (vision != null)
{
vision.setFpsMeterEnabled(enabled);
}
} //setFpsMeterEnabled
/**
* This method returns the front webcam.
*
* @return front webcam.
*/
public WebcamName getFrontWebcam()
{
return webcam1;
} //getFrontWebcam
/**
* This method returns the rear webcam.
*
* @return rear webcam.
*/
public WebcamName getRearWebcam()
{
return webcam2;
} //getRearWebcam
/**
* This method returns the active camera if we have two webcams.
*
* @return active camera.
*/
public WebcamName getActiveWebcam()
{
return vision.getActiveWebcam();
} //getActiveWebcam
/**
* This method sets the active webcam.
*
* @param webcam specifies the webcam to be set as active.
*/
public void setActiveWebcam(WebcamName webcam)
{
vision.setActiveWebcam(webcam);
} //setActiveWebcam
/**
* This method displays the exposure settings on the dashboard. This helps tuning camera exposure.
*
* @param lineNum specifies the dashboard line number to display the info.
*/
public void displayExposureSettings(int lineNum)
{
long[] exposureSetting = vision.getExposureSetting();
long currExposure = vision.getCurrentExposure();
int[] gainSetting = vision.getGainSetting();
int currGain = vision.getCurrentGain();
robot.dashboard.displayPrintf(
lineNum, "Exp: %d (%d:%d), Gain: %d (%d:%d)",
currExposure, exposureSetting[0], exposureSetting[1],
currGain, gainSetting != null? gainSetting[0]: 0, gainSetting != null? gainSetting[1]: 0);
} //displayExposureSettings
/**
* This method returns the color threshold values of rawColorBlobVision.
*
* @return array of color threshold values.
*/
public double[] getRawColorBlobThresholds()
{
return rawColorBlobPipeline != null? rawColorBlobPipeline.getColorThresholds(): null;
} //getRawColorBlobThresholds
/**
* This method sets the color threshold values of rawColorBlobVision.
*
* @param colorThresholds specifies an array of color threshold values.
*/
public void setRawColorBlobThresholds(double... colorThresholds)
{
if (rawColorBlobPipeline != null)
{
rawColorBlobPipeline.setColorThresholds(colorThresholds);
}
} //setRawColorBlobThresholds
/**
* This method enables/disables raw ColorBlob vision.
*
* @param enabled specifies true to enable, false to disable.
*/
public void setRawColorBlobVisionEnabled(boolean enabled)
{
if (rawColorBlobVision != null)
{
rawColorBlobVision.setPipeline(enabled? rawColorBlobPipeline: null);
}
} //setRawColorBlobVisionEnabled
/**
* This method checks if raw ColorBlob vision is enabled.
*
* @return true if enabled, false if disabled.
*/
public boolean isRawColorBlobVisionEnabled()
{
return rawColorBlobVision != null && rawColorBlobVision.getPipeline() != null;
} //isRawColorBlobVisionEnabled
/**
* This method calls RawColorBlob vision to detect the color blob for color threshold tuning.
*
* @param lineNum specifies the dashboard line number to display the detected object info, -1 to disable printing.
* @return detected raw color blob object info.
*/
public TrcVisionTargetInfo<TrcOpenCvDetector.DetectedObject<?>> getDetectedRawColorBlob(int lineNum)
{
TrcVisionTargetInfo<TrcOpenCvDetector.DetectedObject<?>> colorBlobInfo =
rawColorBlobVision != null? rawColorBlobVision.getBestDetectedTargetInfo(null, null, 0.0, 0.0): null;
if (lineNum != -1)
{
robot.dashboard.displayPrintf(
lineNum, "RawColorBlob: %s", colorBlobInfo != null? colorBlobInfo: "Not found.");
}
return colorBlobInfo;
} //getDetectedRawColorBlob
/**
* This method enables/disables AprilTag vision.
*
* @param enabled specifies true to enable, false to disable.
*/
public void setAprilTagVisionEnabled(boolean enabled)
{
if (aprilTagProcessor != null)
{
vision.setProcessorEnabled(aprilTagProcessor, enabled);
}
} //setAprilTagVisionEnabled
/**
* This method checks if AprilTag vision is enabled.
*
* @return true if enabled, false if disabled.
*/
public boolean isAprilTagVisionEnabled()
{
return aprilTagProcessor != null && vision.isVisionProcessorEnabled(aprilTagProcessor);
} //isAprilTagVisionEnabled
/**
* This method calls AprilTag vision to detect the AprilTag object.
*
* @param id specifies the AprilTag ID to look for, null if match to any ID.
* @param lineNum specifies the dashboard line number to display the detected object info, -1 to disable printing.
* @return detected AprilTag object info.
*/
public TrcVisionTargetInfo<FtcVisionAprilTag.DetectedObject> getDetectedAprilTag(Integer id, int lineNum)
{
TrcVisionTargetInfo<FtcVisionAprilTag.DetectedObject> aprilTagInfo =
aprilTagVision.getBestDetectedTargetInfo(id, null);
if (aprilTagInfo != null && robot.blinkin != null)
{
robot.blinkin.setDetectedPattern(BlinkinLEDs.APRIL_TAG);
}
if (lineNum != -1)
{
robot.dashboard.displayPrintf(
lineNum, "%s: %s", BlinkinLEDs.APRIL_TAG, aprilTagInfo != null? aprilTagInfo : "Not found.");
}
return aprilTagInfo;
} //getDetectedAprilTag
/**
* This method calculates the robot's absolute field location with the detected AprilTagInfo.
*
* @param aprilTagInfo specifies the detected AprilTag info.
* @return robot field location.
*/
public TrcPose2D getRobotFieldPose(TrcVisionTargetInfo<FtcVisionAprilTag.DetectedObject> aprilTagInfo)
{
TrcPose2D robotPose = null;
if (aprilTagInfo != null)
{
TrcPose2D aprilTagPose = RobotParams.APRILTAG_POSES[aprilTagInfo.detectedObj.aprilTagDetection.id - 1];
TrcPose2D cameraPose = aprilTagPose.subtractRelativePose(aprilTagInfo.objPose);
robotPose = cameraPose.subtractRelativePose(RobotParams.CAM_POSE);
tracer.traceInfo(
moduleName,
"AprilTagId=" + aprilTagInfo.detectedObj.aprilTagDetection.id +
", aprilTagFieldPose=" + aprilTagPose +
", aprilTagPoseFromCamera=" + aprilTagInfo.objPose +
", cameraPose=" + cameraPose +
", robotPose=%s" + robotPose);
}
return robotPose;
} //getRobotFieldPose
/**
* This method uses vision to find an AprilTag and uses the AprilTag's absolute field location and its relative
* position from the camera to calculate the robot's absolute field location.
*
* @return robot field location.
*/
public TrcPose2D getRobotFieldPose()
{
TrcPose2D robotPose = null;
if (aprilTagVision != null)
{
// Find any AprilTag in view.
TrcVisionTargetInfo<FtcVisionAprilTag.DetectedObject> aprilTagInfo = getDetectedAprilTag(null, -1);
if (aprilTagInfo != null)
{
robotPose = getRobotFieldPose(aprilTagInfo);
}
}
return robotPose;
} //getRobotFieldPose
/**
* This method enables/disables RedBlob vision.
*
* @param enabled specifies true to enable, false to disable.
*/
public void setRedBlobVisionEnabled(boolean enabled)
{
if (redBlobProcessor != null)
{
vision.setProcessorEnabled(redBlobProcessor, enabled);
}
} //setRedBlobVisionEnabled
/**
* This method checks if RedBlob vision is enabled.
*
* @return true if enabled, false if disabled.
*/
public boolean isRedBlobVisionEnabled()
{
return redBlobProcessor != null && vision.isVisionProcessorEnabled(redBlobProcessor);
} //isRedBlobVisionEnabled
/**
* This method calls ColorBlob vision to detect the Red Blob object.
*
* @param lineNum specifies the dashboard line number to display the detected object info, -1 to disable printing.
* @return detected Red Blob object info.
*/
public TrcVisionTargetInfo<TrcOpenCvColorBlobPipeline.DetectedObject> getDetectedRedBlob(int lineNum)
{
TrcVisionTargetInfo<TrcOpenCvColorBlobPipeline.DetectedObject> colorBlobInfo =
redBlobVision.getBestDetectedTargetInfo(null, null, 0.0, 0.0);
if (colorBlobInfo != null && robot.blinkin != null)
{
robot.blinkin.setDetectedPattern(BlinkinLEDs.RED_BLOB);
}
if (lineNum != -1)
{
robot.dashboard.displayPrintf(
lineNum, "%s: %s", BlinkinLEDs.RED_BLOB, colorBlobInfo != null? colorBlobInfo: "Not found.");
}
return colorBlobInfo;
} //getDetectedRedBlob
/**
* This method enables/disables BlueBlob vision.
*
* @param enabled specifies true to enable, false to disable.
*/
public void setBlueBlobVisionEnabled(boolean enabled)
{
if (blueBlobProcessor != null)
{
vision.setProcessorEnabled(blueBlobProcessor, enabled);
}
} //setBlueBlobVisionEnabled
/**
* This method checks if BlueBlob vision is enabled.
*
* @return true if enabled, false if disabled.
*/
public boolean isBlueBlobVisionEnabled()
{
return blueBlobProcessor != null && vision.isVisionProcessorEnabled(blueBlobProcessor);
} //isBlueBlobVisionEnabled
/**
* This method calls ColorBlob vision to detect the Blue Blob object.
*
* @param lineNum specifies the dashboard line number to display the detected object info, -1 to disable printing.
* @return detected Blue Blob object info.
*/
public TrcVisionTargetInfo<TrcOpenCvColorBlobPipeline.DetectedObject> getDetectedBlueBlob(int lineNum)
{
TrcVisionTargetInfo<TrcOpenCvColorBlobPipeline.DetectedObject> colorBlobInfo =
blueBlobVision.getBestDetectedTargetInfo(null, null, 0.0, 0.0);
if (colorBlobInfo != null && robot.blinkin != null)
{
robot.blinkin.setDetectedPattern(BlinkinLEDs.BLUE_BLOB);
}
if (lineNum != -1)
{
robot.dashboard.displayPrintf(
lineNum, "%s: %s", BlinkinLEDs.BLUE_BLOB, colorBlobInfo != null? colorBlobInfo: "Not found.");
}
return colorBlobInfo;
} //getDetectedBlueBlob
/**
* This method enables/disables TensorFlow vision.
*
* @param enabled specifies true to enable, false to disable.
*/
public void setTensorFlowVisionEnabled(boolean enabled)
{
if (tensorFlowProcessor != null)
{
vision.setProcessorEnabled(tensorFlowProcessor, enabled);
}
} //setTensorFlowVisionEnabled
/**
* This method checks if TensorFlow vision is enabled.
*
* @return true if enabled, false if disabled.
*/
public boolean isTensorFlowVisionEnabled()
{
return tensorFlowProcessor != null && vision.isVisionProcessorEnabled(tensorFlowProcessor);
} //isTensorFlowVisionEnabled
/**
* This method calls TensorFlow vision to detect the Pixel objects.
*
* @param lineNum specifies the dashboard line number to display the detected object info, -1 to disable printing.
* @return detected Pixel object info.
*/
public TrcVisionTargetInfo<FtcVisionTensorFlow.DetectedObject> getDetectedTensorFlowPixel(int lineNum)
{
TrcVisionTargetInfo<FtcVisionTensorFlow.DetectedObject> tensorFlowInfo =
tensorFlowVision.getBestDetectedTargetInfo(TFOD_OBJECT_LABEL, null, this::compareConfidence, 0.0, 0.0);
if (tensorFlowInfo != null && robot.blinkin != null)
{
robot.blinkin.setDetectedPattern(BlinkinLEDs.TENSOR_FLOW);
}
if (lineNum != -1)
{
robot.dashboard.displayPrintf(
lineNum, "%s: %s", BlinkinLEDs.TENSOR_FLOW, tensorFlowInfo != null? tensorFlowInfo: "Not found.");
}
return tensorFlowInfo;
} //getDetectedTensorFlowPixel
/**
* This method is called by the Arrays.sort to sort the target object by increasing distance.
*
* @param a specifies the first target
* @param b specifies the second target.
* @return negative value if a has closer distance than b, 0 if a and b have equal distances, positive value
* if a has higher distance than b.
*/
private int compareDistance(
TrcVisionTargetInfo<TrcOpenCvColorBlobPipeline.DetectedObject> a,
TrcVisionTargetInfo<TrcOpenCvColorBlobPipeline.DetectedObject> b)
{
return (int)((b.objPose.y - a.objPose.y)*100);
} //compareDistance
/**
* This method is called by the Arrays.sort to sort the target object by decreasing confidence.
*
* @param a specifies the first target
* @param b specifies the second target.
* @return negative value if a has higher confidence than b, 0 if a and b have equal confidence, positive value
* if a has lower confidence than b.
*/
private int compareConfidence(
TrcVisionTargetInfo<FtcVisionTensorFlow.DetectedObject> a,
TrcVisionTargetInfo<FtcVisionTensorFlow.DetectedObject> b)
{
return (int)((b.detectedObj.confidence - a.detectedObj.confidence)*100);
} //compareConfidence
} //class Vision