diff --git a/TeamCode/src/main/java/ftclib b/TeamCode/src/main/java/ftclib index da540d6..fc94d6f 160000 --- a/TeamCode/src/main/java/ftclib +++ b/TeamCode/src/main/java/ftclib @@ -1 +1 @@ -Subproject commit da540d600b12d1accb934cffee2b8267f0ddcfa1 +Subproject commit fc94d6fd47d4a17866bff8aff9dde7b45bee57b4 diff --git a/TeamCode/src/main/java/teamcode/FtcAuto.java b/TeamCode/src/main/java/teamcode/FtcAuto.java index 8614413..359e6a6 100644 --- a/TeamCode/src/main/java/teamcode/FtcAuto.java +++ b/TeamCode/src/main/java/teamcode/FtcAuto.java @@ -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 /** diff --git a/TeamCode/src/main/java/teamcode/FtcTeleOp.java b/TeamCode/src/main/java/teamcode/FtcTeleOp.java index 145109c..aa6921f 100644 --- a/TeamCode/src/main/java/teamcode/FtcTeleOp.java +++ b/TeamCode/src/main/java/teamcode/FtcTeleOp.java @@ -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: diff --git a/TeamCode/src/main/java/teamcode/FtcTest.java b/TeamCode/src/main/java/teamcode/FtcTest.java index d765fa0..e38bc34 100644 --- a/TeamCode/src/main/java/teamcode/FtcTest.java +++ b/TeamCode/src/main/java/teamcode/FtcTest.java @@ -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 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) diff --git a/TeamCode/src/main/java/teamcode/Robot.java b/TeamCode/src/main/java/teamcode/Robot.java index 39c8e91..2e6da86 100644 --- a/TeamCode/src/main/java/teamcode/Robot.java +++ b/TeamCode/src/main/java/teamcode/Robot.java @@ -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 /** diff --git a/TeamCode/src/main/java/teamcode/RobotParams.java b/TeamCode/src/main/java/teamcode/RobotParams.java index 13a0d34..6724c6b 100644 --- a/TeamCode/src/main/java/teamcode/RobotParams.java +++ b/TeamCode/src/main/java/teamcode/RobotParams.java @@ -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 diff --git a/TeamCode/src/main/java/teamcode/drivebases/MecanumDrive.java b/TeamCode/src/main/java/teamcode/drivebases/MecanumDrive.java deleted file mode 100644 index 850afaf..0000000 --- a/TeamCode/src/main/java/teamcode/drivebases/MecanumDrive.java +++ /dev/null @@ -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 diff --git a/TeamCode/src/main/java/teamcode/drivebases/RobotDrive.java b/TeamCode/src/main/java/teamcode/drivebases/RobotDrive.java deleted file mode 100644 index 75a6021..0000000 --- a/TeamCode/src/main/java/teamcode/drivebases/RobotDrive.java +++ /dev/null @@ -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 diff --git a/TeamCode/src/main/java/teamcode/drivebases/SwerveDrive.java b/TeamCode/src/main/java/teamcode/drivebases/SwerveDrive.java deleted file mode 100644 index 2dd9363..0000000 --- a/TeamCode/src/main/java/teamcode/drivebases/SwerveDrive.java +++ /dev/null @@ -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 diff --git a/TeamCode/src/main/java/teamcode/subsystems/RobotBase.java b/TeamCode/src/main/java/teamcode/subsystems/RobotBase.java new file mode 100644 index 0000000..236d8b4 --- /dev/null +++ b/TeamCode/src/main/java/teamcode/subsystems/RobotBase.java @@ -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 diff --git a/TeamCode/src/main/java/teamcode/vision/Vision.java b/TeamCode/src/main/java/teamcode/vision/Vision.java index 99c6586..128ea72 100644 --- a/TeamCode/src/main/java/teamcode/vision/Vision.java +++ b/TeamCode/src/main/java/teamcode/vision/Vision.java @@ -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 + diff --git a/TeamCode/src/main/java/trclib b/TeamCode/src/main/java/trclib index 9f717c6..33ff7c1 160000 --- a/TeamCode/src/main/java/trclib +++ b/TeamCode/src/main/java/trclib @@ -1 +1 @@ -Subproject commit 9f717c68f706581e831a538a54a5bef452465547 +Subproject commit 33ff7c177ac84e076c1be8ad7c56e3341a9f7d30