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

Sync'd latest changes.

Browse Source
This commit is contained in:
Titan Robotics Club
2024-12-25 02:27:00 -08:00
parent 5a3bb9474b
commit 90dfa2e493
5 changed files with 78 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -162,10 +162,10 @@ public class FtcAuto extends FtcOpMode
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 != 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.");
@ -183,7 +183,7 @@ public class FtcAuto extends FtcOpMode
// robot.globalTracer.traceInfo(moduleName, "Enabling BlueBlobVision.");
// robot.vision.setBlueBlobVisionEnabled(true);
// }
}
// }
} //robotInit
//
@ -268,9 +268,9 @@ public class FtcAuto extends FtcOpMode
robot.battery.setEnabled(false);
}
printPerformanceMetrics();
robot.globalTracer.traceInfo(
moduleName, "***** Stopping autonomous: " + TrcTimer.getCurrentTimeString() + " *****");
printPerformanceMetrics();
if (TrcDbgTrace.isTraceLogOpened())
{

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

@ -54,6 +54,7 @@ public class FtcTeleOp extends FtcOpMode
private boolean statusUpdateOn = false;
private boolean relocalizing = false;
private TrcPose2D robotFieldPose = null;
private Integer savedLimelightPipeline = null;
//
// Implements FtcOpMode abstract method.
@ -136,16 +137,16 @@ public class FtcTeleOp extends FtcOpMode
//
if (robot.vision != null)
{
if (robot.vision.limelightVision != null)
{
robot.globalTracer.traceInfo(moduleName, "Enabling Limelight AprilTagVision.");
robot.vision.setLimelightVisionEnabled(0, true);
}
else if (robot.vision.aprilTagVision != null)
if (robot.vision.aprilTagVision != null)
{
robot.globalTracer.traceInfo(moduleName, "Enabling WebCam AprilTagVision.");
robot.vision.setAprilTagVisionEnabled(true);
}
else if (robot.vision.limelightVision != null)
{
robot.globalTracer.traceInfo(moduleName, "Enabling Limelight AprilTagVision.");
robot.vision.setLimelightVisionEnabled(0, true);
}
}
} //startMode
@ -163,9 +164,9 @@ public class FtcTeleOp extends FtcOpMode
// 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() + " *****");
printPerformanceMetrics();
if (TrcDbgTrace.isTraceLogOpened())
{
@ -187,6 +188,7 @@ public class FtcTeleOp extends FtcOpMode
{
if (slowPeriodicLoop)
{
int lineNum = 1;
//
// DriveBase subsystem.
//
@ -214,9 +216,13 @@ public class FtcTeleOp extends FtcOpMode
{
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());
if (RobotParams.Preferences.doStatusUpdate || statusUpdateOn)
{
robot.dashboard.displayPrintf(
lineNum++, "RobotDrive: Power=(%.2f,y=%.2f,rot=%.2f),Mode:%s",
inputs[0], inputs[1], inputs[2], robot.robotDrive.driveBase.getDriveOrientation());
}
}
// Check for EndGame warning.
if (elapsedTime > RobotParams.Game.ENDGAME_DEADLINE)
@ -242,7 +248,7 @@ public class FtcTeleOp extends FtcOpMode
// Display subsystem status.
if (RobotParams.Preferences.doStatusUpdate || statusUpdateOn)
{
robot.updateStatus(2);
robot.updateStatus(lineNum);
}
}
} //periodic
@ -370,7 +376,7 @@ public class FtcTeleOp extends FtcOpMode
{
robot.globalTracer.traceInfo(moduleName, ">>>>> ZeroCalibrating.");
robot.cancelAll();
robot.zeroCalibrate();
robot.zeroCalibrate(moduleName, null);
if (robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive)
{
// Drive base is a Swerve Drive, align all steering wheels forward.
@ -381,27 +387,47 @@ public class FtcTeleOp extends FtcOpMode
break;
case Start:
if (robot.vision != null &&
(robot.vision.isLimelightVisionEnabled() || robot.vision.isAprilTagVisionEnabled()) &&
robot.robotDrive != null)
if (robot.vision != 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)
boolean hasAprilTagVision = robot.vision.isAprilTagVisionEnabled();
if (!hasAprilTagVision && robot.vision.limelightVision != null)
{
if (robotFieldPose != null)
hasAprilTagVision = true;
if (pressed)
{
// 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;
// Webcam AprilTag vision is not enable, enable Limelight AprilTag pipeline instead.
savedLimelightPipeline = robot.vision.limelightVision.getPipeline();
robot.vision.setLimelightVisionEnabled(0, true);
}
}
else
if (hasAprilTagVision)
{
robot.globalTracer.traceInfo(moduleName, ">>>>> Start re-localizing ...");
// 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;
if (savedLimelightPipeline != null)
{
// Done with AprilTag re-localization, restore previous Limelight pipeline.
robot.vision.limelightVision.setPipeline(savedLimelightPipeline);
savedLimelightPipeline = null;
}
}
}
else
{
robot.globalTracer.traceInfo(moduleName, ">>>>> Start re-localizing ...");
}
}
}
break;

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

@ -89,7 +89,7 @@ public class Robot
// Create and initialize vision subsystems.
if (RobotParams.Preferences.useVision &&
(RobotParams.Preferences.tuneColorBlobVision ||
RobotParams.Preferences.useAprilTagVision ||
RobotParams.Preferences.useWebcamAprilTagVision ||
RobotParams.Preferences.useColorBlobVision ||
RobotParams.Preferences.useLimelightVision))
{
@ -115,8 +115,13 @@ public class Robot
if (RobotParams.Preferences.useSubsystems)
{
// Create subsystems.
// Zero calibrate all subsystems only at init time.
zeroCalibrate();
// Zero calibrate all subsystems only in Auto or if TeleOp is run standalone without prior Auto.
// There is no reason to zero calibrate again if Auto was run right before TeleOp.
if (runMode == TrcRobot.RunMode.AUTO_MODE || FtcAuto.autoChoices.alliance == null)
{
zeroCalibrate(null, null);
}
// Create autotasks.
}
}
@ -312,14 +317,6 @@ public class Robot
{
} //zeroCalibrate
/**
* This method zero calibrates all subsystems.
*/
public void zeroCalibrate()
{
zeroCalibrate(null, null);
} //zeroCalibrate
/**
* This method sets the robot's starting position according to the autonomous choices.
*

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

@ -62,7 +62,7 @@ public class RobotParams
public static final boolean tuneColorBlobVision = false;
public static final boolean useLimelightVision = false;
public static final boolean useCameraStreamProcessor = false;
public static final boolean useAprilTagVision = false;
public static final boolean useWebcamAprilTagVision = false;
public static final boolean useColorBlobVision = false;
public static final boolean showVisionView = !inCompetition;
public static final boolean showVisionStat = false;

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

@ -273,7 +273,7 @@ public class Vision
// FtcDashboard.getInstance().startCameraStream(cameraStreamProcessor, 0);
}
if (RobotParams.Preferences.useAprilTagVision)
if (RobotParams.Preferences.useWebcamAprilTagVision)
{
tracer.traceInfo(moduleName, "Starting Webcam AprilTagVision...");
FtcVisionAprilTag.Parameters aprilTagParams = new FtcVisionAprilTag.Parameters()
@ -292,14 +292,14 @@ public class Vision
tracer.traceInfo(moduleName, "Starting Webcam ColorBlobVision...");
redBlobVision = new FtcVisionEocvColorBlob(
"RedBlob", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, true,
robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true);
LEDIndicator.RED_BLOB, colorConversion, redBlobColorThresholds, colorBlobFilterContourParams,
true, robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true);
redBlobProcessor = redBlobVision.getVisionProcessor();
visionProcessorsList.add(redBlobProcessor);
blueBlobVision = new FtcVisionEocvColorBlob(
"BlueBlob", colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams, true,
robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true);
LEDIndicator.BLUE_BLOB, colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams,
true, robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true);
blueBlobProcessor = blueBlobVision.getVisionProcessor();
visionProcessorsList.add(blueBlobProcessor);
}
@ -487,6 +487,11 @@ public class Vision
colorBlobInfo.detectedObj.label, colorBlobInfo.detectedObj.getRotatedRectVertices());
}
if (colorBlobInfo != null && robot.ledIndicator != null)
{
robot.ledIndicator.setDetectedPattern(colorBlobInfo.detectedObj.label);
}
if (lineNum != -1)
{
robot.dashboard.displayPrintf(