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

Updated ftclib.

Browse Source 
Updated trclib.
Refactored RobotParams to make it more structured.
Restructured CmdPidDrive to add a start method so that the caller can repeatedly call it to set a new path.
Created new RobotBase class that will create the appropriate robot DriveBase according to robotType.
Generalized and moved RobotDrive, MecanumDrive and SwerveDrive to ftclib.
This commit is contained in:
Titan Robotics Club
2024-08-20 12:20:12 -07:00
parent e0328f5a29
commit 5bfd77ccc2
12 changed files with 597 additions and 1041 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
TeamCode/src/main/java/ftclib

Submodule TeamCode/src/main/java/ftclib updated: da540d600b...fc94d6fd47

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

@ -22,6 +22,8 @@
package teamcode;
import androidx.annotation.NonNull;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import java.util.Locale;
@ -80,6 +82,7 @@ public class FtcAuto extends FtcOpMode
public double driveTime = 0.0;
public double drivePower = 0.0;
@NonNull
@Override
public String toString()
{
@ -126,7 +129,7 @@ public class FtcAuto extends FtcOpMode
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);
TrcDbgTrace.openTraceLog(RobotParams.System.LOG_FOLDER_PATH, filePrefix);
}
//
// Create and run choice menus.
@ -138,17 +141,14 @@ public class FtcAuto extends FtcOpMode
switch (autoChoices.strategy)
{
case PID_DRIVE:
if (RobotParams.Preferences.robotType != RobotParams.RobotType.NoRobot)
if (robot.robotDrive != null)
{
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));
autoCommand = new CmdPidDrive(robot.robotDrive.driveBase, robot.robotDrive.pidDrive);
}
break;
case TIMED_DRIVE:
if (RobotParams.Preferences.robotType != RobotParams.RobotType.NoRobot)
if (robot.robotDrive != null)
{
autoCommand = new CmdTimedDrive(
robot.robotDrive.driveBase, autoChoices.delay, autoChoices.driveTime,
@ -249,6 +249,13 @@ public class FtcAuto extends FtcOpMode
{
robot.battery.setEnabled(true);
}
if (autoChoices.strategy == AutoStrategy.PID_DRIVE && autoCommand != null)
{
((CmdPidDrive) autoCommand).start(
autoChoices.delay, autoChoices.drivePower, null,
new TrcPose2D(autoChoices.xTarget*12.0, autoChoices.yTarget*12.0, autoChoices.turnTarget));
}
} //startMode
/**

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

@ -26,9 +26,9 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import java.util.Locale;
import ftclib.drivebase.FtcSwerveDrive;
import ftclib.driverio.FtcGamepad;
import ftclib.robotcore.FtcOpMode;
import teamcode.drivebases.SwerveDrive;
import trclib.drivebase.TrcDriveBase;
import trclib.driverio.TrcGameController;
import trclib.pathdrive.TrcPose2D;
@ -47,9 +47,9 @@ public class FtcTeleOp extends FtcOpMode
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 double drivePowerScale;
private double turnPowerScale;
private boolean operatorAltFunc = false;
private boolean relocalizing = false;
private TrcPose2D robotFieldPose = null;
@ -68,6 +68,9 @@ public class FtcTeleOp extends FtcOpMode
// Create and initialize robot object.
//
robot = new Robot(TrcRobot.getRunMode());
drivePowerScale = robot.robotInfo.driveNormalScale;
turnPowerScale = robot.robotInfo.turnNormalScale;
//
// Open trace log.
//
@ -76,7 +79,7 @@ public class FtcTeleOp extends FtcOpMode
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);
TrcDbgTrace.openTraceLog(RobotParams.System.LOG_FOLDER_PATH, filePrefix);
}
//
// Create and initialize Gamepads.
@ -85,7 +88,7 @@ public class FtcTeleOp extends FtcOpMode
operatorGamepad = new FtcGamepad("OperatorGamepad", gamepad2, this::operatorButtonEvent);
driverGamepad.setYInverted(true);
operatorGamepad.setYInverted(true);
setDriveOrientation(TrcDriveBase.DriveOrientation.ROBOT);
setDriveOrientation(robot.robotInfo.driveOrientation);
} //robotInit
//
@ -178,7 +181,7 @@ public class FtcTeleOp extends FtcOpMode
else
{
double[] inputs = driverGamepad.getDriveInputs(
RobotParams.ROBOT_DRIVE_MODE, true, drivePowerScale, turnPowerScale);
robot.robotInfo.driveMode, true, drivePowerScale, turnPowerScale);
if (robot.robotDrive.driveBase.supportsHolonomicDrive())
{
@ -302,14 +305,14 @@ public class FtcTeleOp extends FtcOpMode
if (pressed)
{
robot.globalTracer.traceInfo(moduleName, ">>>>> DrivePower slow.");
drivePowerScale = RobotParams.DRIVE_POWER_SCALE_SLOW;
turnPowerScale = RobotParams.TURN_POWER_SCALE_SLOW;
drivePowerScale = robot.robotInfo.driveSlowScale;
turnPowerScale = robot.robotInfo.turnSlowScale;
}
else
{
robot.globalTracer.traceInfo(moduleName, ">>>>> DrivePower normal.");
drivePowerScale = RobotParams.DRIVE_POWER_SCALE_NORMAL;
turnPowerScale = RobotParams.TURN_POWER_SCALE_NORMAL;
drivePowerScale = robot.robotInfo.driveNormalScale;
turnPowerScale = robot.robotInfo.turnNormalScale;
}
break;
@ -350,11 +353,11 @@ public class FtcTeleOp extends FtcOpMode
break;
case FtcGamepad.GAMEPAD_BACK:
if (pressed && robot.robotDrive != null && robot.robotDrive instanceof SwerveDrive)
if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive)
{
// 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);
((FtcSwerveDrive) robot.robotDrive).setSteerAngle(0.0, false, false);
}
break;
}
@ -389,8 +392,8 @@ public class FtcTeleOp extends FtcOpMode
break;
case FtcGamepad.GAMEPAD_RBUMPER:
robot.globalTracer.traceInfo(moduleName, ">>>>> ManulOverride=" + pressed);
manualOverride = pressed;
robot.globalTracer.traceInfo(moduleName, ">>>>> OperatorAltFunc=" + pressed);
operatorAltFunc = pressed;
break;
case FtcGamepad.GAMEPAD_DPAD_UP:

202
TeamCode/src/main/java/teamcode/FtcTest.java
View File

@ -22,6 +22,8 @@
package teamcode;
import androidx.annotation.NonNull;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
@ -29,13 +31,13 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import java.util.Arrays;
import java.util.Locale;
import ftclib.drivebase.FtcRobotDrive;
import ftclib.drivebase.FtcSwerveDrive;
import ftclib.driverio.FtcChoiceMenu;
import ftclib.driverio.FtcGamepad;
import ftclib.driverio.FtcMenu;
import ftclib.driverio.FtcValueMenu;
import ftclib.robotcore.FtcPidCoeffCache;
import teamcode.drivebases.RobotDrive;
import teamcode.drivebases.SwerveDrive;
import trclib.command.CmdDriveMotorsTest;
import trclib.command.CmdPidDrive;
import trclib.command.CmdTimedDrive;
@ -89,11 +91,12 @@ public class FtcTest extends FtcTeleOp
double turnTarget = 0.0;
double driveTime = 0.0;
double drivePower = 0.0;
TrcPidController.PidCoefficients tunePidCoeff = null;
TrcPidController.PidCoefficients tunePidCoeffs = null;
double tuneDistance = 0.0;
double tuneHeading = 0.0;
double tuneDrivePower = 0.0;
@NonNull
@Override
public String toString()
{
@ -105,17 +108,17 @@ public class FtcTest extends FtcTeleOp
"turnTarget=%1f " +
"driveTime=%.1f " +
"drivePower=%.1f " +
"tunePidCoeff=%s " +
"tunePidCoeffs=%s " +
"tuneDistance=%.1f " +
"tuneHeading=%.1f " +
"tuneDrivePower=%.1f",
test, xTarget, yTarget, turnTarget, driveTime, drivePower, tunePidCoeff, tuneDistance, tuneHeading,
test, xTarget, yTarget, turnTarget, driveTime, drivePower, tunePidCoeffs, tuneDistance, tuneHeading,
tuneDrivePower);
} //toString
} //class TestChoices
private final FtcPidCoeffCache pidCoeffCache = new FtcPidCoeffCache(RobotParams.TEAM_FOLDER_PATH);
private final FtcPidCoeffCache pidCoeffCache = new FtcPidCoeffCache(RobotParams.System.TEAM_FOLDER_PATH);
private final TestChoices testChoices = new TestChoices();
private TrcElapsedTimer elapsedTimer = null;
private FtcChoiceMenu<Test> testMenu = null;
@ -197,41 +200,13 @@ public class FtcTest extends FtcTeleOp
break;
case PID_DRIVE:
if (robot.robotDrive != null)
{
// Distance targets are in feet, so convert them into inches.
testCommand = new CmdPidDrive(
robot.robotDrive.driveBase, robot.robotDrive.pidDrive, 0.0, testChoices.drivePower, null,
new TrcPose2D(testChoices.xTarget*12.0, testChoices.yTarget*12.0, testChoices.turnTarget));
}
break;
case TUNE_X_PID:
if (robot.robotDrive != null)
{
// Distance target is in feet, so convert it into inches.
testCommand = new CmdPidDrive(
robot.robotDrive.driveBase, robot.robotDrive.pidDrive, 0.0, testChoices.tuneDrivePower,
testChoices.tunePidCoeff, new TrcPose2D(testChoices.tuneDistance*12.0, 0.0, 0.0));
}
break;
case TUNE_Y_PID:
if (robot.robotDrive != null)
{
// Distance target is in feet, so convert it into inches.
testCommand = new CmdPidDrive(
robot.robotDrive.driveBase, robot.robotDrive.pidDrive, 0.0, testChoices.tuneDrivePower,
testChoices.tunePidCoeff, new TrcPose2D(0.0, testChoices.tuneDistance*12.0, 0.0));
}
break;
case TUNE_TURN_PID:
if (robot.robotDrive != null)
if (robot.robotDrive != null &&
(testChoices.test != Test.TUNE_X_PID || robot.robotDrive.driveBase.supportsHolonomicDrive()))
{
testCommand = new CmdPidDrive(
robot.robotDrive.driveBase, robot.robotDrive.pidDrive, 0.0, testChoices.tuneDrivePower,
testChoices.tunePidCoeff, new TrcPose2D(0.0, 0.0, testChoices.tuneHeading));
testCommand = new CmdPidDrive(robot.robotDrive.driveBase, robot.robotDrive.pidDrive);
}
break;
}
@ -306,11 +281,41 @@ public class FtcTest extends FtcTeleOp
break;
case PID_DRIVE:
case TUNE_X_PID:
case TUNE_Y_PID:
case TUNE_TURN_PID:
if (robot.robotDrive != null)
if (testCommand != null)
{
((CmdPidDrive) testCommand).start(
0.0, testChoices.drivePower, null,
new TrcPose2D(testChoices.xTarget*12.0, testChoices.yTarget*12.0, testChoices.turnTarget));
robot.robotDrive.pidDrive.setTraceLevel(TrcDbgTrace.MsgLevel.INFO, logEvents, debugPid, false);
}
break;
case TUNE_X_PID:
if (testCommand != null)
{
((CmdPidDrive) testCommand).start(
0.0, testChoices.drivePower, testChoices.tunePidCoeffs,
new TrcPose2D(testChoices.tuneDistance*12.0, 0.0, 0.0));
robot.robotDrive.pidDrive.setTraceLevel(TrcDbgTrace.MsgLevel.INFO, logEvents, debugPid, false);
}
break;
case TUNE_Y_PID:
if (testCommand != null)
{
((CmdPidDrive) testCommand).start(
0.0, testChoices.drivePower, testChoices.tunePidCoeffs,
new TrcPose2D(0.0, testChoices.tuneDistance*12.0, 0.0));
robot.robotDrive.pidDrive.setTraceLevel(TrcDbgTrace.MsgLevel.INFO, logEvents, debugPid, false);
}
break;
case TUNE_TURN_PID:
if (testCommand != null)
{
((CmdPidDrive) testCommand).start(
0.0, testChoices.drivePower, testChoices.tunePidCoeffs,
new TrcPose2D(0.0, 0.0, testChoices.tuneHeading));
robot.robotDrive.pidDrive.setTraceLevel(TrcDbgTrace.MsgLevel.INFO, logEvents, debugPid, false);
}
break;
@ -420,19 +425,19 @@ public class FtcTest extends FtcTeleOp
lineNum++, "RobotPose=%s", robot.robotDrive.driveBase.getFieldPosition());
robot.dashboard.displayPrintf(
lineNum++, "rawEnc=lf:%.0f,rf:%.0f,lb:%.0f,rb:%.0f",
robot.robotDrive.driveMotors[RobotDrive.INDEX_LEFT_FRONT].getPosition(),
robot.robotDrive.driveMotors[RobotDrive.INDEX_RIGHT_FRONT].getPosition(),
robot.robotDrive.driveMotors[RobotDrive.INDEX_LEFT_BACK].getPosition(),
robot.robotDrive.driveMotors[RobotDrive.INDEX_RIGHT_BACK].getPosition());
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_LEFT_FRONT].getPosition(),
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_RIGHT_FRONT].getPosition(),
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_LEFT_BACK].getPosition(),
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_RIGHT_BACK].getPosition());
}
break;
case TUNE_X_PID:
case TUNE_Y_PID:
case TUNE_TURN_PID:
if (robot.robotDrive != null && testChoices.tunePidCoeff != null)
if (robot.robotDrive != null && testChoices.tunePidCoeffs != null)
{
robot.dashboard.displayPrintf(7, "TunePid=%s", testChoices.tunePidCoeff);
robot.dashboard.displayPrintf(7, "TunePid=%s", testChoices.tunePidCoeffs);
}
//
// Intentionally falling through.
@ -458,10 +463,10 @@ public class FtcTest extends FtcTeleOp
robot.dashboard.displayPrintf(
lineNum++, "RobotPose=%s,rawEnc=lf:%.0f,rf:%.0f,lb:%.0f,rb:%.0f",
robot.robotDrive.driveBase.getFieldPosition(),
robot.robotDrive.driveMotors[RobotDrive.INDEX_LEFT_FRONT].getPosition(),
robot.robotDrive.driveMotors[RobotDrive.INDEX_RIGHT_FRONT].getPosition(),
robot.robotDrive.driveMotors[RobotDrive.INDEX_LEFT_BACK].getPosition(),
robot.robotDrive.driveMotors[RobotDrive.INDEX_RIGHT_BACK].getPosition());
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_LEFT_FRONT].getPosition(),
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_RIGHT_FRONT].getPosition(),
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_LEFT_BACK].getPosition(),
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_RIGHT_BACK].getPosition());
if (xPidCtrl != null)
{
xPidCtrl.displayPidInfo(lineNum);
@ -506,29 +511,34 @@ public class FtcTest extends FtcTeleOp
break;
case CALIBRATE_SWERVE_STEERING:
if (robot.robotDrive != null && (robot.robotDrive instanceof SwerveDrive) && steerCalibrating)
if (robot.robotDrive != null && (robot.robotDrive instanceof FtcSwerveDrive) && steerCalibrating)
{
SwerveDrive swerveDrive = (SwerveDrive) robot.robotDrive;
FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive;
swerveDrive.runSteeringCalibration();
if (swerveDrive.calibrationCount > 0)
if (swerveDrive.steerZeroCalibrationCount > 0)
{
robot.dashboard.displayPrintf(lineNum++, "Count = %d", swerveDrive.calibrationCount);
robot.dashboard.displayPrintf(
lineNum++, "Count = %d", swerveDrive.steerZeroCalibrationCount);
robot.dashboard.displayPrintf(
lineNum++, "Encoder: lf=%.3f/%f",
swerveDrive.steerEncoders[SwerveDrive.INDEX_LEFT_FRONT].getRawPosition(),
swerveDrive.zeroPositions[SwerveDrive.INDEX_LEFT_FRONT]/swerveDrive.calibrationCount);
swerveDrive.steerEncoders[FtcSwerveDrive.INDEX_LEFT_FRONT].getRawPosition(),
swerveDrive.steerZeros[FtcSwerveDrive.INDEX_LEFT_FRONT]
/ swerveDrive.steerZeroCalibrationCount);
robot.dashboard.displayPrintf(
lineNum++, "Encoder: rf=%.3f/%f",
swerveDrive.steerEncoders[SwerveDrive.INDEX_RIGHT_FRONT].getRawPosition(),
swerveDrive.zeroPositions[SwerveDrive.INDEX_RIGHT_FRONT]/swerveDrive.calibrationCount);
swerveDrive.steerEncoders[FtcSwerveDrive.INDEX_RIGHT_FRONT].getRawPosition(),
swerveDrive.steerZeros[FtcSwerveDrive.INDEX_RIGHT_FRONT]
/ swerveDrive.steerZeroCalibrationCount);
robot.dashboard.displayPrintf(
lineNum++, "Encoder: lb=%.3f/%f",
swerveDrive.steerEncoders[SwerveDrive.INDEX_LEFT_BACK].getRawPosition(),
swerveDrive.zeroPositions[SwerveDrive.INDEX_LEFT_BACK]/swerveDrive.calibrationCount);
swerveDrive.steerEncoders[FtcSwerveDrive.INDEX_LEFT_BACK].getRawPosition(),
swerveDrive.steerZeros[FtcSwerveDrive.INDEX_LEFT_BACK]
/ swerveDrive.steerZeroCalibrationCount);
robot.dashboard.displayPrintf(
lineNum++, "Encoder: rb=%.3f/%f",
swerveDrive.steerEncoders[SwerveDrive.INDEX_RIGHT_BACK].getRawPosition(),
swerveDrive.zeroPositions[SwerveDrive.INDEX_RIGHT_BACK]/swerveDrive.calibrationCount);
swerveDrive.steerEncoders[FtcSwerveDrive.INDEX_RIGHT_BACK].getRawPosition(),
swerveDrive.steerZeros[FtcSwerveDrive.INDEX_RIGHT_BACK]
/ swerveDrive.steerZeroCalibrationCount);
}
}
break;
@ -561,9 +571,9 @@ public class FtcTest extends FtcTeleOp
case FtcGamepad.GAMEPAD_A:
if (testChoices.test == Test.CALIBRATE_SWERVE_STEERING)
{
if (pressed && robot.robotDrive != null && robot.robotDrive instanceof SwerveDrive)
if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive)
{
SwerveDrive swerveDrive = (SwerveDrive) robot.robotDrive;
FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive;
steerCalibrating = !steerCalibrating;
if (steerCalibrating)
@ -674,9 +684,9 @@ public class FtcTest extends FtcTeleOp
{
// If we are moving swerve steering, make sure TeleOp doesn't interfere.
teleOpControlEnabled = !pressed;
if (pressed && robot.robotDrive != null && robot.robotDrive instanceof SwerveDrive)
if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive)
{
SwerveDrive swerveDrive = (SwerveDrive) robot.robotDrive;
FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive;
swerveDrive.setSteerAngle(0.0, false, true);
}
passToTeleOp = false;
@ -701,9 +711,9 @@ public class FtcTest extends FtcTeleOp
{
// If we are moving swerve steering, make sure TeleOp doesn't interfere.
teleOpControlEnabled = !pressed;
if (pressed && robot.robotDrive != null && robot.robotDrive instanceof SwerveDrive)
if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive)
{
SwerveDrive swerveDrive = (SwerveDrive) robot.robotDrive;
FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive;
swerveDrive.setSteerAngle(180.0, false, true);
}
passToTeleOp = false;
@ -728,9 +738,9 @@ public class FtcTest extends FtcTeleOp
{
// If we are moving swerve steering, make sure TeleOp doesn't interfere.
teleOpControlEnabled = !pressed;
if (pressed && robot.robotDrive != null && robot.robotDrive instanceof SwerveDrive)
if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive)
{
SwerveDrive swerveDrive = (SwerveDrive) robot.robotDrive;
FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive;
swerveDrive.setSteerAngle(270.0, false, true);
}
passToTeleOp = false;
@ -752,9 +762,9 @@ public class FtcTest extends FtcTeleOp
{
// If we are moving swerve steering, make sure TeleOp doesn't interfere.
teleOpControlEnabled = !pressed;
if (pressed && robot.robotDrive != null && robot.robotDrive instanceof SwerveDrive)
if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive)
{
SwerveDrive swerveDrive = (SwerveDrive) robot.robotDrive;
FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive;
swerveDrive.setSteerAngle(90.0, false, true);
}
passToTeleOp = false;
@ -799,26 +809,12 @@ public class FtcTest extends FtcTeleOp
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_DPAD_UP:
break;
case FtcGamepad.GAMEPAD_DPAD_DOWN:
break;
case FtcGamepad.GAMEPAD_DPAD_LEFT:
break;
case FtcGamepad.GAMEPAD_DPAD_RIGHT:
break;
}
@ -919,7 +915,7 @@ public class FtcTest extends FtcTeleOp
testChoices.turnTarget = turnTargetMenu.getCurrentValue();
testChoices.driveTime = driveTimeMenu.getCurrentValue();
testChoices.drivePower = drivePowerMenu.getCurrentValue();
testChoices.tunePidCoeff = new TrcPidController.PidCoefficients(
testChoices.tunePidCoeffs = new TrcPidController.PidCoefficients(
tuneKpMenu.getCurrentValue(), tuneKiMenu.getCurrentValue(),
tuneKdMenu.getCurrentValue(),tuneKfMenu.getCurrentValue());
testChoices.tuneDistance = tuneDistanceMenu.getCurrentValue();
@ -933,7 +929,7 @@ public class FtcTest extends FtcTeleOp
// Write the user input PID coefficients to a cache file so tune PID menu can read them as start value
// next time.
//
pidCoeffCache.writeCachedPidCoeff(tunePidCtrl, testChoices.tunePidCoeff);
pidCoeffCache.writeCachedPidCoeff(tunePidCtrl, testChoices.tunePidCoeffs);
}
//
// Show choices.
@ -1060,26 +1056,26 @@ public class FtcTest extends FtcTeleOp
{
robot.dashboard.displayPrintf(
lineNum++, "DriveEnc: lf=%.0f,rf=%.0f,lb=%.0f,rb=%.0f",
robot.robotDrive.driveMotors[RobotDrive.INDEX_LEFT_FRONT].getPosition(),
robot.robotDrive.driveMotors[RobotDrive.INDEX_RIGHT_FRONT].getPosition(),
robot.robotDrive.driveMotors[RobotDrive.INDEX_LEFT_BACK].getPosition(),
robot.robotDrive.driveMotors[RobotDrive.INDEX_RIGHT_BACK].getPosition());
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_LEFT_FRONT].getPosition(),
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_RIGHT_FRONT].getPosition(),
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_LEFT_BACK].getPosition(),
robot.robotDrive.driveMotors[FtcRobotDrive.INDEX_RIGHT_BACK].getPosition());
if (robot.robotDrive instanceof SwerveDrive)
if (robot.robotDrive instanceof FtcSwerveDrive)
{
SwerveDrive swerveDrive = (SwerveDrive) robot.robotDrive;
FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive;
robot.dashboard.displayPrintf(
lineNum++, "SteerEnc: lf=%.2f, rf=%.2f, lb=%.2f, rb=%.2f",
swerveDrive.steerEncoders[RobotDrive.INDEX_LEFT_FRONT].getScaledPosition(),
swerveDrive.steerEncoders[RobotDrive.INDEX_RIGHT_FRONT].getScaledPosition(),
swerveDrive.steerEncoders[RobotDrive.INDEX_LEFT_BACK].getScaledPosition(),
swerveDrive.steerEncoders[RobotDrive.INDEX_RIGHT_BACK].getScaledPosition());
swerveDrive.steerEncoders[FtcRobotDrive.INDEX_LEFT_FRONT].getScaledPosition(),
swerveDrive.steerEncoders[FtcRobotDrive.INDEX_RIGHT_FRONT].getScaledPosition(),
swerveDrive.steerEncoders[FtcRobotDrive.INDEX_LEFT_BACK].getScaledPosition(),
swerveDrive.steerEncoders[FtcRobotDrive.INDEX_RIGHT_BACK].getScaledPosition());
robot.dashboard.displayPrintf(
lineNum++, "SteerRaw: lf=%.2f, rf=%.2f, lb=%.2f, rb=%.2f",
swerveDrive.steerEncoders[RobotDrive.INDEX_LEFT_FRONT].getRawPosition(),
swerveDrive.steerEncoders[RobotDrive.INDEX_RIGHT_FRONT].getRawPosition(),
swerveDrive.steerEncoders[RobotDrive.INDEX_LEFT_BACK].getRawPosition(),
swerveDrive.steerEncoders[RobotDrive.INDEX_RIGHT_BACK].getRawPosition());
swerveDrive.steerEncoders[FtcRobotDrive.INDEX_LEFT_FRONT].getRawPosition(),
swerveDrive.steerEncoders[FtcRobotDrive.INDEX_RIGHT_FRONT].getRawPosition(),
swerveDrive.steerEncoders[FtcRobotDrive.INDEX_LEFT_BACK].getRawPosition(),
swerveDrive.steerEncoders[FtcRobotDrive.INDEX_RIGHT_BACK].getRawPosition());
}
if (robot.robotDrive.gyro != null)

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

@ -22,14 +22,15 @@
package teamcode;
import androidx.annotation.NonNull;
import ftclib.drivebase.FtcRobotDrive;
import ftclib.driverio.FtcDashboard;
import ftclib.driverio.FtcMatchInfo;
import ftclib.robotcore.FtcOpMode;
import ftclib.sensor.FtcRobotBattery;
import teamcode.drivebases.MecanumDrive;
import teamcode.drivebases.RobotDrive;
import teamcode.drivebases.SwerveDrive;
import teamcode.subsystems.BlinkinLEDs;
import teamcode.subsystems.RobotBase;
import teamcode.vision.Vision;
import trclib.motor.TrcMotor;
import trclib.motor.TrcServo;
@ -46,28 +47,22 @@ 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;
//
// Robot Drive.
public FtcRobotDrive.RobotInfo robotInfo;
public FtcRobotDrive robotDrive;
// 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.
@ -77,37 +72,33 @@ public class Robot
*/
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)
// Create and initialize Robot Base.
RobotBase robotBase = new RobotBase();
robotInfo = robotBase.getRobotInfo();
robotDrive = robotBase.getRobotDrive();
// Create and initialize vision subsystems.
if (RobotParams.Preferences.useVision &&
(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.
// If robotType is VisionOnly, 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)
if (RobotParams.Preferences.robotType != RobotParams.RobotType.VisionOnly)
{
//
// Create and initialize sensors and indicators.
//
if (RobotParams.Preferences.useBlinkin)
if (robotInfo.blinkinName != null)
{
blinkin = new BlinkinLEDs(RobotParams.HWNAME_BLINKIN);
blinkin = new BlinkinLEDs(robotInfo.blinkinName);
}
if (RobotParams.Preferences.useBatteryMonitor)
@ -115,12 +106,6 @@ public class Robot
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)
@ -136,10 +121,11 @@ public class Robot
*
* @return instance name.
*/
@NonNull
@Override
public String toString()
{
return RobotParams.ROBOT_NAME;
return robotInfo != null? robotInfo.robotName: RobotParams.System.DEF_ROBOT_NAME;
} //toString
/**

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

@ -24,15 +24,18 @@ package teamcode;
import android.os.Environment;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot.*;
import org.openftc.easyopencv.OpenCvCameraRotation;
import ftclib.driverio.FtcGamepad;
import ftclib.drivebase.FtcRobotDrive;
import ftclib.drivebase.FtcSwerveDrive;
import ftclib.driverio.FtcGamepad.DriveMode;
import ftclib.motor.FtcMotorActuator.MotorType;
import trclib.dataprocessor.TrcUtil;
import trclib.drivebase.TrcDriveBase.DriveOrientation;
import trclib.pathdrive.TrcPose2D;
import trclib.robotcore.TrcPidController;
import trclib.robotcore.TrcPidController.PidCoefficients;
import trclib.vision.TrcHomographyMapper;
/**
@ -40,262 +43,345 @@ import trclib.vision.TrcHomographyMapper;
*/
public class RobotParams
{
public enum RobotType
/**
* This class contains Gobilda motor parameters.
*/
public static class Gobilda
{
MecanumRobot,
SwerveRobot,
NoRobot
} //RobotType
// 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 MOTOR_5203_312_ENC_PPR = (((1.0 + 46.0/17.0)*(1.0 + 46.0/11.0))*28.0);
public static final double MOTOR_5203_312_MAX_RPM = 312.0;
public static final double MOTOR_5203_312_MAX_VEL_PPS =
MOTOR_5203_312_ENC_PPR * MOTOR_5203_312_MAX_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 MOTOR_5203_435_ENC_PPR = (((1.0 + 46.0/17.0)*(1.0 + 46.0/17.0))*28.0);
public static final double MOTOR_5203_435_MAX_RPM = 435.0;
public static final double MOTOR_5203_435_MAX_VEL_PPS =
MOTOR_5203_435_ENC_PPR * MOTOR_5203_435_MAX_RPM / 60.0; // 2787.9135 pps
} //class Gobilda
/**
* This class contains robot preferences. It controls enabling/disabling of various robot features.
* This class contains field dimension constants. Generally, these should not be changed.
*/
public static class Field
{
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;
} //class Field
/**
* This class contains season specific game element information.
*/
public static class Game
{
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
};
} //class Game
/**
* This class contains miscellaneous robot info.
*/
public static class System
{
public static final String TEAM_FOLDER_PATH =
Environment.getExternalStorageDirectory().getPath() + "/FIRST/ftcTeam";
public static final String LOG_FOLDER_PATH = TEAM_FOLDER_PATH + "/tracelogs";
public static final String DEF_ROBOT_NAME = "Robot3543";
} //class System
/**
* When the season starts, the competition robot may not be ready for programmers. It's crucial to save time by
* developing code on robots of previous seasons. By adding previous robots to the list of RobotType, one can
* easily switch the code to handle different robots.
*/
public enum RobotType
{
// This is useful for developing Vision code where all you need is a Control Hub and camera.
VisionOnly,
// Generic Differential Drive Base Robot
DifferentialRobot,
// Generic Mecanum Drive Base Robot
MecanumRobot,
// Generic Swerve Drive Base Robot
SwerveRobot
} //enum RobotType
/**
* This class contains robot preferences. It enables/disables various robot features. This is especially useful
* during robot development where some subsystems may not be available or ready yet. By disabling unavailable
* subsystems, one can test the rest of the robot without the fear of code crashing when some subsystems are not
* found.
*/
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;
public static final RobotType robotType = RobotType.MecanumRobot;
public static final boolean inCompetition = false;
public static final boolean useTraceLog = true;
public static final boolean useLoopPerformanceMonitor = true;
public static final boolean useBatteryMonitor = false;
// Status Update: Status Update may affect robot loop time, don't do it when in competition.
public static final 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;
public static final boolean useVision = false;
public static final boolean useWebCam = false;
public static final boolean useBuiltinCamBack = false; // For Android Phone as Robot Controller.
public static final boolean tuneColorBlobVision = false;
public static final boolean useAprilTagVision = false;
public static final boolean useColorBlobVision = false;
public static final boolean useTensorFlowVision = false;
public static final boolean showVisionView = !inCompetition;
public static final boolean showVisionStat = false;
// Drive Base
public static boolean useExternalOdometry = false;
public static boolean doSwervePhysicalAlignment = false;
public static boolean swerveDualServoSteering = true;
public static final boolean useDriveBase = false;
public static final boolean useExternalOdometry = false;
// Subsystems
public static boolean useSubsystems = false;
public static final 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.
// Robot Parameters.
//
public static class VisionOnlyParams extends FtcRobotDrive.RobotInfo
{
public VisionOnlyParams()
{
robotName = "VisionOnly";
// Vision
webCam1Name = "Webcam 1";
webCam2Name = "Webcam 2";
camImageWidth = 640;
camImageHeight = 480;
camOrientation = OpenCvCameraRotation.UPRIGHT;
// Camera location On Robot
camXOffset = 0.0; //Camera right offset from robot centroid
camYOffset = 2.0; //Camera forward offset from robot centroid
camZOffset = 9.75; //Camera height offset from floor
camTiltDown = 15.00; //Camera tilt down angle from horizontal in deg
camPose = new TrcPose2D(camXOffset, camYOffset, 0.0);
// Homography: cameraRect in pixels, worldRect in inches
cameraRect = new TrcHomographyMapper.Rectangle(
0.0, 120.0, // Camera Top Left
camImageWidth -1, 120.0, // Camera Top Right
0.0, camImageHeight - 1, // Camera Bottom Left
camImageWidth - 1, camImageHeight - 1); // Camera Bottom Right
worldRect = new TrcHomographyMapper.Rectangle(
-12.5626, 48.0 - robotLength/2.0 - camYOffset, // World Top Left
11.4375, 44.75 - robotLength/2.0 - camYOffset, // World Top Right
-2.5625, 21.0 - robotLength/2.0 - camYOffset, // World Bottom Left
2.5626, 21.0 - robotLength/2.0 - camYOffset); // World Bottom Right
} //VisionOnlyParams
} //class VisionOnlyParams
/**
* This class contains the Mecanum Robot Parameters.
*/
public static class MecanumParams extends FtcRobotDrive.RobotInfo
{
// Optii Odometry Wheel
private static final double ODWHEEL_DIAMETER = 35.0 * TrcUtil.INCHES_PER_MM;
private static final double ODWHEEL_CPR = 4096.0;
public MecanumParams()
{
robotName = "MecanumRobot";
// IMU
imuName = "imu";
hubLogoDirection = LogoFacingDirection.UP;
hubUsbDirection = UsbFacingDirection.FORWARD;
// Robot Dimensions
robotLength = 17.0;
robotWidth = 17.0;
wheelBaseLength = (24.0 * 14)*TrcUtil.INCHES_PER_MM;
wheelBaseWidth = 16.0;
// Drive Motors
driveMotorType = MotorType.DcMotor;
driveMotorNames = new String[] {"lfDriveMotor", "rfDriveMotor", "lbDriveMotor", "rbDriveMotor"};
driveMotorInverted = new boolean[] {true, false, true, false};
// Odometry Wheels
odWheelScale = Math.PI * ODWHEEL_DIAMETER / ODWHEEL_CPR; // 0.00105687652708656383937269814237 in/count
xOdWheelOffsetX = 0.0;
xOdWheelOffsetY = -168.0 * TrcUtil.INCHES_PER_MM;
yLeftOdWheelOffsetX = -144.0 * TrcUtil.INCHES_PER_MM;
yLeftOdWheelOffsetY = -12.0 * TrcUtil.INCHES_PER_MM;
yRightOdWheelOffsetX = 144.0 * TrcUtil.INCHES_PER_MM;
yRightOdWheelOffsetY = -12.0 * TrcUtil.INCHES_PER_MM;
// Drive Motor Odometry
xDrivePosScale = 0.01924724265461924299065420560748; // in/count
yDrivePosScale = 0.02166184604662450653409090909091; // in/count
// Robot Drive Characteristics
robotMaxVelocity = 23.0; // inches/sec
robotMaxAcceleration = 500.0; // inches/sec2
robotMaxTurnRate = 100.0; // degrees/sec
profiledMaxVelocity = robotMaxVelocity;
profiledMaxAcceleration = robotMaxAcceleration;
profiledMaxTurnRate = robotMaxTurnRate;
// DriveBase PID Parameters
drivePidTolerance = 1.0;
turnPidTolerance = 1.0;
xDrivePidCoeffs = new PidCoefficients(0.95, 0.0, 0.001, 0.0, 0.0);
xDrivePidPowerLimit = 1.0;
xDriveMaxPidRampRate = null;
yDrivePidCoeffs = new PidCoefficients(0.06, 0.0, 0.002, 0.0, 0.0);
yDrivePidPowerLimit = 1.0;
yDriveMaxPidRampRate = null;
turnPidCoeffs = new PidCoefficients(0.02, 0.0, 0.002, 0.0, 0.0);
turnPidPowerLimit = 0.5;
turnMaxPidRampRate = null;
// PID Stall Detection
pidStallDetectionEnabled = true;
// PurePursuit Parameters
ppdFollowingDistance = 6.0;
velPidCoeffs = new PidCoefficients(0.0, 0.0, 0.0, 1.0 / profiledMaxVelocity, 0.0);
// Robot Drive
driveMode = DriveMode.ARCADE_MODE;
driveOrientation = DriveOrientation.ROBOT;
driveSlowScale = 0.3;
driveNormalScale = 1.0;
turnSlowScale = 0.3;
turnNormalScale = 0.6;
// Vision
webCam1Name = "Webcam 1";
webCam2Name = "Webcam 2";
camImageWidth = 640;
camImageHeight = 480;
camOrientation = OpenCvCameraRotation.UPRIGHT;
// Camera location On Robot
camXOffset = 0.0; //Camera right offset from robot centroid
camYOffset = 2.0; //Camera forward offset from robot centroid
camZOffset = 9.75; //Camera height offset from floor
camTiltDown = 15.00; //Camera tilt down angle from horizontal in deg
camPose = new TrcPose2D(camXOffset, camYOffset, 0.0);
// Homography: cameraRect in pixels, worldRect in inches
cameraRect = new TrcHomographyMapper.Rectangle(
0.0, 120.0, // Camera Top Left
camImageWidth -1, 120.0, // Camera Top Right
0.0, camImageHeight - 1, // Camera Bottom Left
camImageWidth - 1, camImageHeight - 1); // Camera Bottom Right
worldRect = new TrcHomographyMapper.Rectangle(
-12.5626, 48.0 - robotLength/2.0 - camYOffset, // World Top Left
11.4375, 44.75 - robotLength/2.0 - camYOffset, // World Top Right
-2.5625, 21.0 - robotLength/2.0 - camYOffset, // World Bottom Left
2.5626, 21.0 - robotLength/2.0 - camYOffset); // World Bottom Right
blinkinName = "blinkin";
} //MecanumParams
} //class MecanumParams
/**
* This class contains the Swerve Drive Base Parameters.
*/
public static class SwerveParams extends FtcSwerveDrive.SwerveInfo
{
// Optii Odometry Wheel
private static final double ODWHEEL_DIAMETER = 35.0 * TrcUtil.INCHES_PER_MM;
private static final double ODWHEEL_CPR = 4096.0;
public SwerveParams()
{
robotName = "SwerveRobot";
// IMU
imuName = "imu";
hubLogoDirection = LogoFacingDirection.UP;
hubUsbDirection = UsbFacingDirection.FORWARD;
// Robot Dimensions
robotLength = 17.0;
robotWidth = 17.0;
wheelBaseLength = (24.0 * 14)*TrcUtil.INCHES_PER_MM;
wheelBaseWidth = 16.0;
// Drive Motors
driveMotorType = MotorType.DcMotor;
driveMotorNames = new String[] {"lfDriveMotor", "rfDriveMotor", "lbDriveMotor", "rbDriveMotor"};
driveMotorInverted = new boolean[] {true, false, true, false};
// Odometry Wheels
odWheelScale = Math.PI * ODWHEEL_DIAMETER / ODWHEEL_CPR; // 0.00105687652708656383937269814237 in/count
xOdWheelOffsetX = 0.0;
xOdWheelOffsetY = -168.0 * TrcUtil.INCHES_PER_MM;
yLeftOdWheelOffsetX = -144.0 * TrcUtil.INCHES_PER_MM;
yLeftOdWheelOffsetY = -12.0 * TrcUtil.INCHES_PER_MM;
yRightOdWheelOffsetX = 144.0 * TrcUtil.INCHES_PER_MM;
yRightOdWheelOffsetY = -12.0 * TrcUtil.INCHES_PER_MM;
// Drive Motor Odometry
xDrivePosScale = 0.01924724265461924299065420560748; // in/count
yDrivePosScale = 0.01924724265461924299065420560748; // in/count
// Robot Drive Characteristics
robotMaxVelocity = 23.0; // inches/sec
robotMaxAcceleration = 500.0; // inches/sec2
robotMaxTurnRate = 100.0; // degrees/sec
profiledMaxVelocity = robotMaxVelocity;
profiledMaxAcceleration = robotMaxAcceleration;
profiledMaxTurnRate = robotMaxTurnRate;
// DriveBase PID Parameters
drivePidTolerance = 1.0;
turnPidTolerance = 1.0;
xDrivePidCoeffs = yDrivePidCoeffs = new PidCoefficients(0.95, 0.0, 0.001, 0.0, 0.0);
xDrivePidPowerLimit = yDrivePidPowerLimit = 1.0;
xDriveMaxPidRampRate = yDriveMaxPidRampRate = null;
turnPidCoeffs = new PidCoefficients(0.02, 0.0, 0.002, 0.0, 0.0);
turnPidPowerLimit = 0.5;
turnMaxPidRampRate = null;
// PID Stall Detection
pidStallDetectionEnabled = true;
// PurePursuit Parameters
ppdFollowingDistance = 6.0;
velPidCoeffs = new PidCoefficients(0.0, 0.0, 0.0, 1.0 / profiledMaxVelocity, 0.0);
// Robot Drive
driveMode = DriveMode.ARCADE_MODE;
driveOrientation = DriveOrientation.ROBOT;
driveSlowScale = 0.3;
driveNormalScale = 1.0;
turnSlowScale = 0.3;
turnNormalScale = 0.6;
// Vision
webCam1Name = "Webcam 1";
webCam2Name = "Webcam 2";
camImageWidth = 640;
camImageHeight = 480;
camOrientation = OpenCvCameraRotation.UPRIGHT;
// Camera location On Robot
camXOffset = 0.0; //Camera right offset from robot centroid
camYOffset = 2.0; //Camera forward offset from robot centroid
camZOffset = 9.75; //Camera height offset from floor
camTiltDown = 15.00; //Camera tilt down angle from horizontal in deg
camPose = new TrcPose2D(camXOffset, camYOffset, 0.0);
// Homography: cameraRect in pixels, worldRect in inches
cameraRect = new TrcHomographyMapper.Rectangle(
0.0, 120.0, // Camera Top Left
camImageWidth -1, 120.0, // Camera Top Right
0.0, camImageHeight - 1, // Camera Bottom Left
camImageWidth - 1, camImageHeight - 1); // Camera Bottom Right
worldRect = new TrcHomographyMapper.Rectangle(
-12.5626, 48.0 - robotLength/2.0 - camYOffset, // World Top Left
11.4375, 44.75 - robotLength/2.0 - camYOffset, // World Top Right
-2.5625, 21.0 - robotLength/2.0 - camYOffset, // World Bottom Left
2.5626, 21.0 - robotLength/2.0 - camYOffset); // World Bottom Right
blinkinName = "blinkin";
// Steer Encoders
steerEncoderNames = new String[] {"lfSteerEncoder", "rfSteerEncoder", "lbSteerEncoder", "rbSteerEncoder"};
steerEncoderInverted = new boolean[] {};
steerEncoderZeros = new double[] {0.474812, 0.467663, 0.541338, 0.545340};
steerZerosFilePath = System.TEAM_FOLDER_PATH + "/SteerCalibration.txt";
// Steer Motors
steerMotorType = MotorType.CRServo;
steerMotorNames = new String[] {"lfSteerServo", "rfSteerServo", "lbSteerServo", "rbSteerServo"};
steerMotorInverted = new boolean[] {true, true, true, true};
steerMotorPidCoeffs = new PidCoefficients(0.0, 0.0, 0.0, 0.0, 0.0);
steerMotorPidTolerance = 1.0;
// Swerve Modules
swerveModuleNames = new String[] {"lfWheel", "rfWheel", "lbWheel", "rbWheel"};
} //SwerveParams
} //class SwerveParams
//
// 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 boolean LFSTEER_ENC_INVERTED = false;
public static final boolean RFSTEER_ENC_INVERTED = false;
public static final boolean LBSTEER_ENC_INVERTED = false;
public static final boolean RBSTEER_ENC_INVERTED = false;
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

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

@ -1,103 +0,0 @@
/*
* 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 teamcode.RobotParams;
import trclib.drivebase.TrcMecanumDriveBase;
import trclib.pathdrive.TrcPidDrive;
import trclib.pathdrive.TrcPurePursuitDrive;
import trclib.robotcore.TrcDbgTrace;
import trclib.sensor.TrcOdometryWheels;
/**
* 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
View File

@ -1,122 +0,0 @@
/*
* 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 ftclib.motor.FtcDcMotor;
import ftclib.sensor.FtcImu;
import teamcode.RobotParams;
import trclib.dataprocessor.TrcUtil;
import trclib.drivebase.TrcDriveBase;
import trclib.pathdrive.TrcPidDrive;
import trclib.pathdrive.TrcPurePursuitDrive;
/**
* 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

396
TeamCode/src/main/java/teamcode/drivebases/SwerveDrive.java
View File

@ -1,396 +0,0 @@
/*
* 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 ftclib.motor.FtcCRServo;
import ftclib.motor.FtcDcMotor;
import ftclib.sensor.FtcAnalogEncoder;
import teamcode.RobotParams;
import trclib.drivebase.TrcSwerveDriveBase;
import trclib.drivebase.TrcSwerveModule;
import trclib.motor.TrcMotor;
import trclib.pathdrive.TrcPidDrive;
import trclib.pathdrive.TrcPurePursuitDrive;
import trclib.robotcore.TrcDbgTrace;
import trclib.sensor.TrcOdometryWheels;
/**
* 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};
private final boolean[] steerEncoderInverted = {
RobotParams.LFSTEER_ENC_INVERTED, RobotParams.RFSTEER_ENC_INVERTED,
RobotParams.LBSTEER_ENC_INVERTED, RobotParams.RBSTEER_ENC_INVERTED};
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;
private String xModeOwner = null;
/**
* 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], new TrcMotor.ExternalSensors().setEncoder(encoders[i], steerEncoderInverted[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].setPositionPidParameters(
RobotParams.STEER_SERVO_KP, RobotParams.STEER_SERVO_KI,
RobotParams.STEER_SERVO_KD, RobotParams.STEER_SERVO_KF,
RobotParams.STEER_SERVO_IZONE, 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 set all the wheels into an X configuration so that nobody can bump us out of position. If owner
* is specifies, it will acquire execlusive ownership of the drivebase on behalf of the specified owner. On
* disable, it will release the ownership.
*
* @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 setXModeEnabled(String owner, boolean enabled)
{
if (enabled)
{
if (owner != null && !driveBase.hasOwnership(owner) && driveBase.acquireExclusiveAccess(owner))
{
xModeOwner = owner;
}
((TrcSwerveDriveBase) driveBase).setXMode(owner);
}
else if (xModeOwner != null)
{
driveBase.releaseExclusiveAccess(xModeOwner);
xModeOwner = null;
}
} //setXModeEnabled
/**
* 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

90
TeamCode/src/main/java/teamcode/subsystems/RobotBase.java Normal file
View File

@ -0,0 +1,90 @@
/*
* 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 ftclib.drivebase.FtcMecanumDrive;
import ftclib.drivebase.FtcRobotDrive;
import ftclib.drivebase.FtcSwerveDrive;
import teamcode.RobotParams;
/**
* This class creates the appropriate Robot Drive Base according to the specified robot type.
*/
public class RobotBase
{
private final FtcRobotDrive.RobotInfo robotInfo;
private final FtcRobotDrive robotDrive;
/**
* Constructor: Create an instance of the object.
*/
public RobotBase()
{
switch (RobotParams.Preferences.robotType)
{
case VisionOnly:
robotInfo = new RobotParams.VisionOnlyParams();
robotDrive = null;
break;
case MecanumRobot:
robotInfo = new RobotParams.MecanumParams();
robotDrive = RobotParams.Preferences.useDriveBase?
new FtcMecanumDrive(robotInfo, RobotParams.Preferences.useExternalOdometry): null;
break;
case SwerveRobot:
robotInfo = new RobotParams.SwerveParams();
robotDrive = RobotParams.Preferences.useDriveBase?
new FtcSwerveDrive(
(RobotParams.SwerveParams) robotInfo, RobotParams.Preferences.useExternalOdometry): null;
break;
default:
robotInfo = null;
robotDrive = null;
break;
}
} //RobotBase
/**
* This method returns the created RobotInfo object.
*
* @return created robot info.
*/
public FtcRobotDrive.RobotInfo getRobotInfo()
{
return robotInfo;
} //getRobotInfo
/**
* This method returns the created RobotBase object.
*
* @return created robot drive.
*/
public FtcRobotDrive getRobotDrive()
{
return robotDrive;
} //getRobotDrive
} //class RobotBase

33
TeamCode/src/main/java/teamcode/vision/Vision.java
View File

@ -106,11 +106,18 @@ public class Vision
{
FtcOpMode opMode = FtcOpMode.getInstance();
if (robot.robotInfo.webCam1Name == null &&
(RobotParams.Preferences.useWebCam || RobotParams.Preferences.tuneColorBlobVision))
{
throw new IllegalArgumentException("Must provide a valid WebCam 1 name.");
}
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;
this.webcam1 = robot.robotInfo.webCam1Name != null?
opMode.hardwareMap.get(WebcamName.class, robot.robotInfo.webCam1Name): null;
this.webcam2 = robot.robotInfo.webCam2Name != null?
opMode.hardwareMap.get(WebcamName.class, robot.robotInfo.webCam2Name): null;
if (RobotParams.Preferences.tuneColorBlobVision)
{
OpenCvCamera openCvCamera;
@ -133,8 +140,8 @@ public class Vision
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", robot.robotInfo.camImageWidth, robot.robotInfo.camImageHeight, null, null,
openCvCamera, robot.robotInfo.camOrientation);
rawColorBlobVision.setFpsMeterEnabled(RobotParams.Preferences.showVisionStat);
setRawColorBlobVisionEnabled(false);
}
@ -165,13 +172,13 @@ public class Vision
redBlobVision = new FtcVisionEocvColorBlob(
"RedBlob", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, true,
RobotParams.cameraRect, RobotParams.worldRect, true);
robot.robotInfo.cameraRect, robot.robotInfo.worldRect, true);
redBlobProcessor = redBlobVision.getVisionProcessor();
visionProcessorsList.add(redBlobProcessor);
blueBlobVision = new FtcVisionEocvColorBlob(
"BlueBlob", colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams, true,
RobotParams.cameraRect, RobotParams.worldRect, true);
robot.robotInfo.cameraRect, robot.robotInfo.worldRect, true);
blueBlobProcessor = blueBlobVision.getVisionProcessor();
visionProcessorsList.add(blueBlobProcessor);
}
@ -180,7 +187,8 @@ public class Vision
{
tracer.traceInfo(moduleName, "Starting TensorFlowVision...");
tensorFlowVision = new FtcVisionTensorFlow(
null, true, TFOD_MODEL_ASSET, TFOD_TARGET_LABELS, RobotParams.cameraRect, RobotParams.worldRect);
null, true, TFOD_MODEL_ASSET, TFOD_TARGET_LABELS, robot.robotInfo.cameraRect,
robot.robotInfo.worldRect);
tensorFlowProcessor = tensorFlowVision.getVisionProcessor();
tensorFlowProcessor.setMinResultConfidence(TFOD_MIN_CONFIDENCE);
visionProcessorsList.add(tensorFlowProcessor);
@ -192,7 +200,7 @@ public class Vision
{
// Use USB webcams.
vision = new FtcVision(
webcam1, webcam2, RobotParams.CAM_IMAGE_WIDTH, RobotParams.CAM_IMAGE_HEIGHT,
webcam1, webcam2, robot.robotInfo.camImageWidth, robot.robotInfo.camImageHeight,
RobotParams.Preferences.showVisionView, RobotParams.Preferences.showVisionStat, visionProcessors);
}
else
@ -201,7 +209,7 @@ public class Vision
vision = new FtcVision(
RobotParams.Preferences.useBuiltinCamBack?
BuiltinCameraDirection.BACK: BuiltinCameraDirection.FRONT,
RobotParams.CAM_IMAGE_WIDTH, RobotParams.CAM_IMAGE_HEIGHT,
robot.robotInfo.camImageWidth, robot.robotInfo.camImageHeight,
RobotParams.Preferences.showVisionView, RobotParams.Preferences.showVisionStat, visionProcessors);
}
// Disable all vision until they are needed.
@ -426,9 +434,10 @@ public class Vision
if (aprilTagInfo != null)
{
TrcPose2D aprilTagPose = RobotParams.APRILTAG_POSES[aprilTagInfo.detectedObj.aprilTagDetection.id - 1];
TrcPose2D aprilTagPose =
RobotParams.Game.APRILTAG_POSES[aprilTagInfo.detectedObj.aprilTagDetection.id - 1];
TrcPose2D cameraPose = aprilTagPose.subtractRelativePose(aprilTagInfo.objPose);
robotPose = cameraPose.subtractRelativePose(RobotParams.CAM_POSE);
robotPose = cameraPose.subtractRelativePose(robot.robotInfo.camPose);
tracer.traceInfo(
moduleName,
"AprilTagId=" + aprilTagInfo.detectedObj.aprilTagDetection.id +

2
TeamCode/src/main/java/trclib

Submodule TeamCode/src/main/java/trclib updated: 9f717c68f7...33ff7c177a