diff --git a/TeamCode/src/main/java/ftclib b/TeamCode/src/main/java/ftclib index 12962c5..56d85d3 160000 --- a/TeamCode/src/main/java/ftclib +++ b/TeamCode/src/main/java/ftclib @@ -1 +1 @@ -Subproject commit 12962c5ae8eefce4ffabf6b4bc104fe78e418882 +Subproject commit 56d85d3fe7d1dbbfc49789c762ae2ac411fd027f diff --git a/TeamCode/src/main/java/teamcode/FtcAuto.java b/TeamCode/src/main/java/teamcode/FtcAuto.java index 97cf078..99a66d9 100644 --- a/TeamCode/src/main/java/teamcode/FtcAuto.java +++ b/TeamCode/src/main/java/teamcode/FtcAuto.java @@ -184,8 +184,6 @@ public class FtcAuto extends FtcOpMode // robot.vision.setBlueBlobVisionEnabled(true); // } } - - robot.zeroCalibrate(); } //robotInit // diff --git a/TeamCode/src/main/java/teamcode/FtcTeleOp.java b/TeamCode/src/main/java/teamcode/FtcTeleOp.java index 160ba5c..5394299 100644 --- a/TeamCode/src/main/java/teamcode/FtcTeleOp.java +++ b/TeamCode/src/main/java/teamcode/FtcTeleOp.java @@ -68,8 +68,6 @@ public class FtcTeleOp extends FtcOpMode // Create and initialize robot object. // robot = new Robot(TrcRobot.getRunMode()); - drivePowerScale = RobotParams.Robot.DRIVE_NORMAL_SCALE; - turnPowerScale = RobotParams.Robot.TURN_NORMAL_SCALE; // // Open trace log. @@ -86,10 +84,14 @@ public class FtcTeleOp extends FtcOpMode // driverGamepad = new FtcGamepad("DriverGamepad", gamepad1); driverGamepad.setButtonEventHandler(this::driverButtonEvent); + driverGamepad.setLeftStickInverted(false, true); + operatorGamepad = new FtcGamepad("OperatorGamepad", gamepad2); operatorGamepad.setButtonEventHandler(this::operatorButtonEvent); - driverGamepad.setLeftStickInverted(false, true); operatorGamepad.setRightStickInverted(false, true); + + drivePowerScale = RobotParams.Robot.DRIVE_NORMAL_SCALE; + turnPowerScale = RobotParams.Robot.TURN_NORMAL_SCALE; setDriveOrientation(RobotParams.Robot.DRIVE_ORIENTATION); } //robotInit @@ -122,10 +124,18 @@ public class FtcTeleOp extends FtcOpMode // // Enable AprilTag vision for re-localization. // - if (robot.vision != null && robot.vision.aprilTagVision != null) + if (robot.vision != null) { - robot.globalTracer.traceInfo(moduleName, "Enabling AprilTagVision."); - robot.vision.setAprilTagVisionEnabled(true); + if (robot.vision.limelightVision != null) + { + robot.globalTracer.traceInfo(moduleName, "Enabling Limelight AprilTagVision."); + robot.vision.setLimelightVisionEnabled(0, true); + } + else if (robot.vision.aprilTagVision != null) + { + robot.globalTracer.traceInfo(moduleName, "Enabling WebCam AprilTagVision."); + robot.vision.setAprilTagVisionEnabled(true); + } } } //startMode @@ -204,6 +214,7 @@ public class FtcTeleOp extends FtcOpMode // if (RobotParams.Preferences.useSubsystems) { + // Analog control of subsystems. } // Display subsystem status. if (RobotParams.Preferences.doStatusUpdate) @@ -249,29 +260,24 @@ public class FtcTeleOp extends FtcOpMode switch (button) { case A: - // Toggle between field or robot oriented driving, only applicable for holonomic drive base. - if (driverAltFunc) + if (robot.robotDrive != null && pressed) { - if (pressed && robot.robotDrive != null) + if (driverAltFunc) { if (robot.robotDrive.driveBase.isGyroAssistEnabled()) { - // Disable GyroAssist drive. robot.globalTracer.traceInfo(moduleName, ">>>>> Disabling GyroAssist."); robot.robotDrive.driveBase.setGyroAssistEnabled(null); } else { - // Enable GyroAssist drive. robot.globalTracer.traceInfo(moduleName, ">>>>> Enabling GyroAssist."); robot.robotDrive.driveBase.setGyroAssistEnabled(robot.robotDrive.pidDrive.getTurnPidCtrl()); } } - } - else - { - if (pressed && robot.robotDrive != null && robot.robotDrive.driveBase.supportsHolonomicDrive()) + else if (robot.robotDrive.driveBase.supportsHolonomicDrive()) { + // Toggle between field or robot oriented driving, only applicable for holonomic drive base. if (robot.robotDrive.driveBase.getDriveOrientation() != TrcDriveBase.DriveOrientation.FIELD) { robot.globalTracer.traceInfo(moduleName, ">>>>> Enabling FIELD mode."); @@ -321,7 +327,7 @@ public class FtcTeleOp extends FtcOpMode case Back: if (pressed) { - robot.globalTracer.traceInfo(moduleName, ">>>>> ZeroCalibrate pressed."); + robot.globalTracer.traceInfo(moduleName, ">>>>> ZeroCalibrating."); robot.cancelAll(); robot.zeroCalibrate(); if (robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive) @@ -334,7 +340,9 @@ public class FtcTeleOp extends FtcOpMode break; case Start: - if (robot.vision != null && robot.vision.aprilTagVision != null && robot.robotDrive != null) + if (robot.vision != null && + (robot.vision.isLimelightVisionEnabled() || robot.vision.isAprilTagVisionEnabled()) && + robot.robotDrive != null) { // On press of the button, we will start looking for AprilTag for re-localization. // On release of the button, we will set the robot's field location if we found the AprilTag. diff --git a/TeamCode/src/main/java/teamcode/FtcTest.java b/TeamCode/src/main/java/teamcode/FtcTest.java index fd81d3c..a02bdc9 100644 --- a/TeamCode/src/main/java/teamcode/FtcTest.java +++ b/TeamCode/src/main/java/teamcode/FtcTest.java @@ -37,7 +37,7 @@ import ftclib.driverio.FtcMenu; import ftclib.driverio.FtcValueMenu; import ftclib.robotcore.FtcPidCoeffCache; import ftclib.vision.FtcLimelightVision; -import teamcode.vision.Vision; +import teamcode.subsystems.Vision; import trclib.command.CmdDriveMotorsTest; import trclib.command.CmdPidDrive; import trclib.command.CmdTimedDrive; @@ -126,6 +126,7 @@ public class FtcTest extends FtcTeleOp // Drive Speed Test. private double maxDriveVelocity = 0.0; private double maxDriveAcceleration = 0.0; + private double maxTurnVelocity = 0.0; private double prevTime = 0.0; private double prevVelocity = 0.0; // Swerve Steering Calibration. @@ -395,12 +396,19 @@ public class FtcTest extends FtcTeleOp maxDriveAcceleration = acceleration; } + if (velPose.angle > maxTurnVelocity) + { + maxTurnVelocity = velPose.angle; + } + prevTime = currTime; prevVelocity = velocity; robot.dashboard.displayPrintf(lineNum++, "Drive Vel: (%.1f/%.1f)", velocity, maxDriveVelocity); robot.dashboard.displayPrintf( lineNum++, "Drive Accel: (%.1f/%.1f)", acceleration, maxDriveAcceleration); + robot.dashboard.displayPrintf( + lineNum++, "Turn Vel: (%.1f/%.1f)", velPose.angle, maxTurnVelocity); } break; } diff --git a/TeamCode/src/main/java/teamcode/Robot.java b/TeamCode/src/main/java/teamcode/Robot.java index 2e70260..2da57d9 100644 --- a/TeamCode/src/main/java/teamcode/Robot.java +++ b/TeamCode/src/main/java/teamcode/Robot.java @@ -31,7 +31,7 @@ import ftclib.robotcore.FtcOpMode; import ftclib.sensor.FtcRobotBattery; import teamcode.subsystems.LEDIndicator; import teamcode.subsystems.RobotBase; -import teamcode.vision.Vision; +import teamcode.subsystems.Vision; import trclib.motor.TrcMotor; import trclib.motor.TrcServo; import trclib.pathdrive.TrcPose2D; @@ -62,6 +62,7 @@ public class Robot public LEDIndicator ledIndicator; public FtcRobotBattery battery; // Subsystems. + // Autotasks. /** * Constructor: Create an instance of the object. @@ -92,7 +93,7 @@ public class Robot } // 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.VisionOnly) + if (RobotParams.Preferences.robotType != RobotBase.RobotType.VisionOnly) { // Create and initialize sensors and indicators. if (robotInfo.indicatorName != null) @@ -109,6 +110,10 @@ public class Robot // if (RobotParams.Preferences.useSubsystems) { + // Create subsystems. + // Zero calibrate all subsystems only at init time. + zeroCalibrate(); + // Create autotasks. } } @@ -283,11 +288,7 @@ public class Robot { globalTracer.traceInfo(moduleName, "Cancel all operations."); - if (robotDrive != null) - { - // Cancel all auto-assist driving. - robotDrive.cancel(); - } + if (robotDrive != null) robotDrive.cancel(); } //cancelAll /** @@ -316,6 +317,88 @@ public class Robot { } //setRobotStartPosition + /** + * This method adjusts the given pose in the red alliance to be the specified alliance. + * + * @param x specifies x position in the red alliance in the specified unit. + * @param y specifies y position in the red alliance in the specified unit. + * @param heading specifies heading in the red alliance in degrees. + * @param alliance specifies the alliance to be converted to. + * @param isTileUnit specifies true if x and y are in tile unit, false if in inches. + * @return pose adjusted to be in the specified alliance in inches. + */ + public TrcPose2D adjustPoseByAlliance( + double x, double y, double heading, FtcAuto.Alliance alliance, boolean isTileUnit) + { + TrcPose2D newPose = new TrcPose2D(x, y, heading); + + if (alliance == FtcAuto.Alliance.BLUE_ALLIANCE) + { + // Translate blue alliance pose to red alliance pose. + if (RobotParams.Game.fieldIsMirrored) + { + // Mirrored field. + double angleDelta = (newPose.angle - 90.0)*2.0; + newPose.angle -= angleDelta; + newPose.y = -newPose.y; + } + else + { + // Symmetrical field. + newPose.x = -newPose.x; + newPose.y = -newPose.y; + newPose.angle = (newPose.angle + 180.0) % 360.0; + } + } + + if (isTileUnit) + { + newPose.x *= RobotParams.Field.FULL_TILE_INCHES; + newPose.y *= RobotParams.Field.FULL_TILE_INCHES; + } + + return newPose; + } //adjustPoseByAlliance + + /** + * This method adjusts the given pose in the red alliance to be the specified alliance. + * + * @param x specifies x position in the red alliance in tile unit. + * @param y specifies y position in the red alliance in tile unit. + * @param heading specifies heading in the red alliance in degrees. + * @param alliance specifies the alliance to be converted to. + * @return pose adjusted to be in the specified alliance in inches. + */ + public TrcPose2D adjustPoseByAlliance(double x, double y, double heading, FtcAuto.Alliance alliance) + { + return adjustPoseByAlliance(x, y, heading, alliance, true); + } //adjustPoseByAlliance + + /** + * This method adjusts the given pose in the red alliance to be the specified alliance. + * + * @param pose specifies pose in the red alliance in the specified unit. + * @param alliance specifies the alliance to be converted to. + * @param isTileUnit specifies true if pose is in tile units, false in inches. + * @return pose adjusted to be in the specified alliance in inches. + */ + public TrcPose2D adjustPoseByAlliance(TrcPose2D pose, FtcAuto.Alliance alliance, boolean isTileUnit) + { + return adjustPoseByAlliance(pose.x, pose.y, pose.angle, alliance, isTileUnit); + } //adjustPoseByAlliance + + /** + * This method adjusts the given pose in the red alliance to be the specified alliance. + * + * @param pose specifies pose in the blue alliance in tile unit. + * @param alliance specifies the alliance to be converted to. + * @return pose adjusted to be in the specified alliance in inches. + */ + public TrcPose2D adjustPoseByAlliance(TrcPose2D pose, FtcAuto.Alliance alliance) + { + return adjustPoseByAlliance(pose, alliance, true); + } //adjustPoseByAlliance + /** * This method sends the text string to the Driver Station to be spoken using text to speech. * diff --git a/TeamCode/src/main/java/teamcode/RobotParams.java b/TeamCode/src/main/java/teamcode/RobotParams.java index 09a7580..16e46cc 100644 --- a/TeamCode/src/main/java/teamcode/RobotParams.java +++ b/TeamCode/src/main/java/teamcode/RobotParams.java @@ -24,29 +24,104 @@ package teamcode; import android.os.Environment; -import com.qualcomm.hardware.rev.RevHubOrientationOnRobot.*; - -import org.openftc.easyopencv.OpenCvCameraRotation; - -import ftclib.drivebase.FtcRobotDrive; -import ftclib.drivebase.FtcSwerveDrive; -import ftclib.motor.FtcMotorActuator.MotorType; -import ftclib.sensor.FtcPinpointOdometry; -import ftclib.sensor.FtcSparkFunOtos; -import trclib.dataprocessor.TrcUtil; -import trclib.drivebase.TrcDriveBase; +import teamcode.subsystems.RobotBase; import trclib.drivebase.TrcDriveBase.DriveOrientation; import trclib.driverio.TrcGameController.DriveMode; import trclib.pathdrive.TrcPose2D; -import trclib.pathdrive.TrcPose3D; -import trclib.robotcore.TrcPidController.PidCoefficients; -import trclib.vision.TrcHomographyMapper; /** * This class contains robot and subsystem constants and parameters. */ public class RobotParams { + /** + * 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 final RobotBase.RobotType robotType = RobotBase.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; + // Driver feedback + // Status Update: Status Update may affect robot loop time, don't do it when in competition. + public static final boolean doStatusUpdate = !inCompetition; + public static final boolean showSubsystems = true; + public static final boolean useBlinkinLED = false; + public static final boolean useGobildaLED = false; + // Vision + public static final boolean useVision = false; + public static final boolean useWebCam = false; // false to use Android phone camera. + public static final boolean useBuiltinCamBack = false; // For Android Phone as Robot Controller. + public static final boolean tuneColorBlobVision = false; + public static final boolean useLimelightVision = false; + public static final boolean useCameraStreamProcessor = false; + public static final boolean useAprilTagVision = false; + public static final boolean useColorBlobVision = false; + public static final boolean doWatershed = false; + public static final boolean showVisionView = !inCompetition; + public static final boolean showVisionStat = false; + // Drive Base + public static final boolean useDriveBase = false; + public static final boolean usePinpointOdometry = false; + public static final boolean useSparkfunOTOS = false; + // Subsystems + public static final boolean useSubsystems = false; + } //class Preferences + + /** + * This class contains miscellaneous robot info. + */ + public static class Robot + { + 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 STEER_ZERO_CAL_FILE = TEAM_FOLDER_PATH + "/SteerZeroCalibration.txt"; + public static final double DASHBOARD_UPDATE_INTERVAL = 0.2; // in msec + public static final String ROBOT_CODEBASE = "Robot2025"; + // Robot Drive Parameters. + public static final DriveMode DRIVE_MODE = DriveMode.ArcadeMode; + public static final DriveOrientation DRIVE_ORIENTATION = DriveOrientation.ROBOT; + public static final double DRIVE_SLOW_SCALE = 0.3; + public static final double DRIVE_NORMAL_SCALE = 1.0; + public static final double TURN_SLOW_SCALE = 0.3; + public static final double TURN_NORMAL_SCALE = 0.6; + public static final double ROBOT_LENGTH = 18.0; + public static final double ROBOT_WIDTH = 18.0; + } //class Robot + + /** + * This class contains season specific game element information. + */ + public static class Game + { + public static final boolean fieldIsMirrored = false; + // AprilTag 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 + }; + // Robot start locations. + } //class Game + + /** + * 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 Gobilda motor parameters. */ @@ -70,510 +145,4 @@ public class RobotParams MOTOR_5203_435_ENC_PPR * MOTOR_5203_435_MAX_RPM / 60.0; // 2787.9135 pps } //class Gobilda - /** - * 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 Robot - { - 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 STEER_ZERO_CAL_FILE = TEAM_FOLDER_PATH + "/SteerZeroCalibration.txt"; - public static final double DASHBOARD_UPDATE_INTERVAL = 0.1; // in msec - public static final String ROBOT_CODEBASE = "Robot2025"; - public static final double ROBOT_LENGTH = 17.0; - public static final double ROBOT_WIDTH = 17.0; - // Robot Drive Parameters. - public static final DriveMode DRIVE_MODE = DriveMode.ArcadeMode; - public static final DriveOrientation DRIVE_ORIENTATION = DriveOrientation.ROBOT; - public static final double DRIVE_SLOW_SCALE = 0.3; - public static final double DRIVE_NORMAL_SCALE = 1.0; - public static final double TURN_SLOW_SCALE = 0.3; - public static final double TURN_NORMAL_SCALE = 0.6; - } //class Robot - - /** - * 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 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; - public static final boolean showSubsystems = true; - public static final boolean useBlinkinLED = false; - public static final boolean useGobildaLED = false; - // Vision - public static final boolean useVision = false; - public static final boolean useWebCam = false; // false to use Android phone camera. - public static final boolean useBuiltinCamBack = false; // For Android Phone as Robot Controller. - public static final boolean tuneColorBlobVision = false; - public static final boolean useLimelightVision = false; - public static final boolean useCameraStreamProcessor = false; - public static final boolean useAprilTagVision = false; - public static final boolean useColorBlobVision = false; - public static final boolean showVisionView = !inCompetition; - public static final boolean showVisionStat = false; - // Drive Base - public static final boolean useDriveBase = false; - public static final boolean usePinpointOdometry = false; - public static final boolean useSparkfunOTOS = false; - // Subsystems - public static final boolean useSubsystems = false; - } //class Preferences - - // - // Robot Parameters. - // - - /** - * This class contains the parameters of the front camera. - */ - public static class FrontCamParams extends FtcRobotDrive.VisionInfo - { - public FrontCamParams() - { - camName = "Webcam 1"; - camImageWidth = 640; - camImageHeight = 480; - camXOffset = 0.0; // Inches to the right from robot center - camYOffset = 2.0; // Inches forward from robot center - camZOffset = 9.75; // Inches up from the floor - camPitch = 15.0; // degrees down from horizontal - camYaw = 0.0; // degrees clockwise from robot front - camRoll = 0.0; - camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll); - camOrientation = OpenCvCameraRotation.UPRIGHT; - // 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 - Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Top Left - 11.4375, 44.75 - Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Top Right - -2.5625, 21.0 - Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Bottom Left - 2.5626, 21.0 - Robot.ROBOT_LENGTH/2.0 - camYOffset); // World Bottom Right - } //FrontCamParams - } //class FrontCamParams - - /** - * This class contains the parameters of the back camera. - */ - public static class BackCamParams extends FtcRobotDrive.VisionInfo - { - public BackCamParams() - { - camName = "Webcam 2"; - camImageWidth = 640; - camImageHeight = 480; - camXOffset = 0.0; // Inches to the right from robot center - camYOffset = 2.0; // Inches forward from robot center - camZOffset = 9.75; // Inches up from the floor - camPitch = 15.0; // degrees down from horizontal - camYaw = 0.0; // degrees clockwise from robot front - camRoll = 0.0; - camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll); - camOrientation = OpenCvCameraRotation.UPRIGHT; - // 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 - Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Top Left - 11.4375, 44.75 - Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Top Right - -2.5625, 21.0 - Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Bottom Left - 2.5626, 21.0 - Robot.ROBOT_LENGTH/2.0 - camYOffset); // World Bottom Right - } //BackCamParams - } //class BackCamParams - - /** - * This class contains the parameters of the Limelight vision processor. - */ - public static class LimelightParams extends FtcRobotDrive.VisionInfo - { - public LimelightParams() - { - camName = "Limelight3a"; - camImageWidth = 640; - camImageHeight = 480; - camXOffset = 0.0; // Inches to the right from robot center - camYOffset = 2.0; // Inches forward from robot center - camZOffset = 9.75; // Inches up from the floor - camPitch = 15.0; // degrees down from horizontal - camYaw = 0.0; // degrees clockwise from robot front - camRoll = 0.0; - camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll); - camOrientation = OpenCvCameraRotation.UPRIGHT; - // 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 - Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Top Left - 11.4375, 44.75 - Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Top Right - -2.5625, 21.0 - Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Bottom Left - 2.5626, 21.0 - Robot.ROBOT_LENGTH/2.0 - camYOffset); // World Bottom Right - } //LimelightParams - } //class LimelightParams - - public static class VisionOnlyParams extends FtcRobotDrive.RobotInfo - { - public VisionOnlyParams() - { - robotName = "VisionOnly"; - webCam1 = new FrontCamParams(); - webCam2 = new BackCamParams(); - limelight = new LimelightParams(); - } //VisionOnlyParams - } //class VisionOnlyParams - - /** - * This class contains the Differential Robot Parameters. - */ - public static class DifferentialParams 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 DifferentialParams() - { - robotName = "DifferentialRobot"; - // Robot Dimensions - robotLength = Robot.ROBOT_LENGTH; - robotWidth = Robot.ROBOT_WIDTH; - wheelBaseLength = (24.0 * 14)*TrcUtil.INCHES_PER_MM; - wheelBaseWidth = 16.0; - // IMU - imuName = "imu"; - hubLogoDirection = LogoFacingDirection.UP; - hubUsbDirection = UsbFacingDirection.FORWARD; - // Drive Motors - driveMotorType = MotorType.DcMotor; - driveMotorNames = new String[] {"lfDriveMotor", "rfDriveMotor"}; - driveMotorInverted = new boolean[] {true, false}; - odometryType = TrcDriveBase.OdometryType.MotorOdometry; - // Odometry Wheels - odWheelXScale = odWheelYScale = Math.PI * ODWHEEL_DIAMETER / ODWHEEL_CPR; - xOdWheelSensorNames = null; - xOdWheelIndices = new int[] {FtcRobotDrive.INDEX_RIGHT_BACK}; - xOdWheelXOffsets = new double[] {0.0}; - xOdWheelYOffsets = new double[] {-168.0 * TrcUtil.INCHES_PER_MM}; - yOdWheelSensorNames = null; - yOdWheelIndices = new int[] {FtcRobotDrive.INDEX_LEFT_FRONT, FtcRobotDrive.INDEX_RIGHT_FRONT}; - yOdWheelXOffsets = new double[] {-144.0 * TrcUtil.INCHES_PER_MM, -12.0 * TrcUtil.INCHES_PER_MM}; - yOdWheelYOffsets = new double[] {144.0 * TrcUtil.INCHES_PER_MM, -12.0 * TrcUtil.INCHES_PER_MM}; - // Absolute Odometry - if (odometryType == TrcDriveBase.OdometryType.AbsoluteOdometry) - { - if (RobotParams.Preferences.usePinpointOdometry) - { - FtcPinpointOdometry.Config ppOdoConfig = new FtcPinpointOdometry.Config() - .setPodOffsets(0.0, 0.0) - .setEncoderResolution(ODWHEEL_CPR / Math.PI * ODWHEEL_DIAMETER) - .setEncodersInverted(false, false); - absoluteOdometry = new FtcPinpointOdometry("pinpointOdo", ppOdoConfig); - } - else if (RobotParams.Preferences.useSparkfunOTOS) - { - FtcSparkFunOtos.Config otosConfig = new FtcSparkFunOtos.Config() - .setOffset(0.0, 0.0, 0.0) - .setScale(1.0, 1.0); - absoluteOdometry = new FtcSparkFunOtos("sparkfunOtos", otosConfig); - } - } - else - { - absoluteOdometry = null; - } - // Drive Motor Odometry - 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; - 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); - // Vision - webCam1 = new FrontCamParams(); - webCam2 = new BackCamParams(); - limelight = new LimelightParams(); - // Miscellaneous - indicatorName = "blinkin"; - } //DifferentialParams - } //class DifferentialParams - - /** - * 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"; - // Robot Dimensions - robotLength = Robot.ROBOT_LENGTH; - robotWidth = Robot.ROBOT_WIDTH; - wheelBaseLength = (24.0 * 14)*TrcUtil.INCHES_PER_MM; - wheelBaseWidth = 16.0; - // IMU - imuName = "imu"; - hubLogoDirection = LogoFacingDirection.UP; - hubUsbDirection = UsbFacingDirection.FORWARD; - // Drive Motors - driveMotorType = MotorType.DcMotor; - driveMotorNames = new String[] {"lfDriveMotor", "rfDriveMotor", "lbDriveMotor", "rbDriveMotor"}; - driveMotorInverted = new boolean[] {true, false, true, false}; - odometryType = TrcDriveBase.OdometryType.AbsoluteOdometry; - // Odometry Wheels - odWheelXScale = odWheelYScale = Math.PI * ODWHEEL_DIAMETER / ODWHEEL_CPR; - xOdWheelSensorNames = new String[] {"xOdWheelSensor"}; - xOdWheelIndices = new int[] {0}; - xOdWheelXOffsets = new double[] {0.0}; - xOdWheelYOffsets = new double[] {-168.0 * TrcUtil.INCHES_PER_MM}; - yOdWheelSensorNames = new String[] {"yLeftOdWheelSensor", "yRightOdWheelSensor"}; - yOdWheelIndices = new int[] {1, 2}; - yOdWheelXOffsets = new double[] {-144.0 * TrcUtil.INCHES_PER_MM, -12.0 * TrcUtil.INCHES_PER_MM}; - yOdWheelYOffsets = new double[] {144.0 * TrcUtil.INCHES_PER_MM, -12.0 * TrcUtil.INCHES_PER_MM}; - // Absolute Odometry - if (odometryType == TrcDriveBase.OdometryType.AbsoluteOdometry) - { - if (RobotParams.Preferences.usePinpointOdometry) - { - FtcPinpointOdometry.Config ppOdoConfig = new FtcPinpointOdometry.Config() - .setPodOffsets(0.0, 0.0) - .setEncoderResolution(ODWHEEL_CPR / Math.PI * ODWHEEL_DIAMETER) - .setEncodersInverted(false, false); - absoluteOdometry = new FtcPinpointOdometry("pinpointOdo", ppOdoConfig); - } - else if (RobotParams.Preferences.useSparkfunOTOS) - { - FtcSparkFunOtos.Config otosConfig = new FtcSparkFunOtos.Config() - .setOffset(0.0, 0.0, 0.0) - .setScale(1.0, 1.0); - absoluteOdometry = new FtcSparkFunOtos("sparkfunOtos", otosConfig); - } - } - else - { - absoluteOdometry = null; - } - // 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); - // Vision - webCam1 = new FrontCamParams(); - webCam2 = new BackCamParams(); - limelight = new LimelightParams(); - // Miscellaneous - indicatorName = "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"; - // Robot Dimensions - robotLength = Robot.ROBOT_LENGTH; - robotWidth = Robot.ROBOT_WIDTH; - wheelBaseLength = (24.0 * 14)*TrcUtil.INCHES_PER_MM; - wheelBaseWidth = 16.0; - // IMU - imuName = "imu"; - hubLogoDirection = LogoFacingDirection.UP; - hubUsbDirection = UsbFacingDirection.FORWARD; - // Drive Motors - driveMotorType = MotorType.DcMotor; - driveMotorNames = new String[] {"lfDriveMotor", "rfDriveMotor", "lbDriveMotor", "rbDriveMotor"}; - driveMotorInverted = new boolean[] {true, false, true, false}; - odometryType = TrcDriveBase.OdometryType.OdometryWheels; - // Odometry Wheels - odWheelXScale = odWheelYScale = Math.PI * ODWHEEL_DIAMETER / ODWHEEL_CPR; - xOdWheelSensorNames = null; - xOdWheelIndices = new int[] {FtcRobotDrive.INDEX_RIGHT_BACK}; - xOdWheelXOffsets = new double[] {0.0}; - xOdWheelYOffsets = new double[] {-168.0 * TrcUtil.INCHES_PER_MM}; - yOdWheelSensorNames = null; - yOdWheelIndices = new int[] {FtcRobotDrive.INDEX_LEFT_FRONT, FtcRobotDrive.INDEX_RIGHT_FRONT}; - yOdWheelXOffsets = new double[] {-144.0 * TrcUtil.INCHES_PER_MM, -12.0 * TrcUtil.INCHES_PER_MM}; - yOdWheelYOffsets = new double[] {144.0 * TrcUtil.INCHES_PER_MM, -12.0 * TrcUtil.INCHES_PER_MM}; - // Absolute Odometry - if (odometryType == TrcDriveBase.OdometryType.AbsoluteOdometry) - { - if (RobotParams.Preferences.usePinpointOdometry) - { - FtcPinpointOdometry.Config ppOdoConfig = new FtcPinpointOdometry.Config() - .setPodOffsets(0.0, 0.0) - .setEncoderResolution(ODWHEEL_CPR / Math.PI * ODWHEEL_DIAMETER) - .setEncodersInverted(false, false); - absoluteOdometry = new FtcPinpointOdometry("pinpointOdo", ppOdoConfig); - } - else if (RobotParams.Preferences.useSparkfunOTOS) - { - FtcSparkFunOtos.Config otosConfig = new FtcSparkFunOtos.Config() - .setOffset(0.0, 0.0, 0.0) - .setScale(1.0, 1.0); - absoluteOdometry = new FtcSparkFunOtos("sparkfunOtos", otosConfig); - } - } - else - { - absoluteOdometry = null; - } - // 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); - // Vision - webCam1 = new FrontCamParams(); - webCam2 = new BackCamParams(); - limelight = new LimelightParams(); - // Miscellaneous - indicatorName = "blinkin"; - // Steer Encoders - steerEncoderNames = new String[] {"lfSteerEncoder", "rfSteerEncoder", "lbSteerEncoder", "rbSteerEncoder"}; - steerEncoderInverted = new boolean[] {false, false, false, false}; - steerEncoderZeros = new double[] {0.474812, 0.467663, 0.541338, 0.545340}; - steerZerosFilePath = Robot.STEER_ZERO_CAL_FILE; - // 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 - - // - // Subsystems. - // - } //class RobotParams diff --git a/TeamCode/src/main/java/teamcode/autotasks/TaskAuto.java b/TeamCode/src/main/java/teamcode/autotasks/TaskAuto.java index 8538f5c..1a1510c 100644 --- a/TeamCode/src/main/java/teamcode/autotasks/TaskAuto.java +++ b/TeamCode/src/main/java/teamcode/autotasks/TaskAuto.java @@ -78,15 +78,6 @@ public class TaskAuto extends TrcAutoTask startAutoTask(State.START, new TaskParams(), completionEvent); } //autoAssist - /** - * This method cancels an in progress auto-assist operation if any. - */ - public void autoAssistCancel() - { - tracer.traceInfo(moduleName, "Canceling auto-assist."); - stopAutoTask(false); - } //autoAssistCancel - // // Implement TrcAutoTask abstract methods. // diff --git a/TeamCode/src/main/java/teamcode/subsystems/LEDIndicator.java b/TeamCode/src/main/java/teamcode/subsystems/LEDIndicator.java index 4839bfe..53f08c2 100644 --- a/TeamCode/src/main/java/teamcode/subsystems/LEDIndicator.java +++ b/TeamCode/src/main/java/teamcode/subsystems/LEDIndicator.java @@ -61,12 +61,12 @@ public class LEDIndicator // Highest priority. new TrcGobildaIndicatorLight.Pattern(RED_BLOB, TrcGobildaIndicatorLight.Color.Red), new TrcGobildaIndicatorLight.Pattern(BLUE_BLOB, TrcGobildaIndicatorLight.Color.Blue), - new TrcGobildaIndicatorLight.Pattern(NO_BLOB, TrcGobildaIndicatorLight.Color.Azure), + new TrcGobildaIndicatorLight.Pattern(NO_BLOB, TrcGobildaIndicatorLight.Color.Cyan), new TrcGobildaIndicatorLight.Pattern(APRIL_TAG, TrcGobildaIndicatorLight.Color.Green), new TrcGobildaIndicatorLight.Pattern(DRIVE_ORIENTATION_FIELD, TrcGobildaIndicatorLight.Color.Violet), new TrcGobildaIndicatorLight.Pattern(DRIVE_ORIENTATION_ROBOT, TrcGobildaIndicatorLight.Color.White), new TrcGobildaIndicatorLight.Pattern(DRIVE_ORIENTATION_INVERTED, TrcGobildaIndicatorLight.Color.Orange), - new TrcGobildaIndicatorLight.Pattern(OFF_PATTERN, TrcGobildaIndicatorLight.Color.Off) + new TrcGobildaIndicatorLight.Pattern(OFF_PATTERN, TrcGobildaIndicatorLight.Color.Black) // Lowest priority. }; indicator = new FtcGobildaIndicatorLight(indicatorName); diff --git a/TeamCode/src/main/java/teamcode/subsystems/RobotBase.java b/TeamCode/src/main/java/teamcode/subsystems/RobotBase.java index e93526b..3e47780 100644 --- a/TeamCode/src/main/java/teamcode/subsystems/RobotBase.java +++ b/TeamCode/src/main/java/teamcode/subsystems/RobotBase.java @@ -22,17 +22,305 @@ package teamcode.subsystems; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; + import ftclib.drivebase.FtcDifferentialDrive; import ftclib.drivebase.FtcMecanumDrive; import ftclib.drivebase.FtcRobotDrive; import ftclib.drivebase.FtcSwerveDrive; +import ftclib.motor.FtcMotorActuator; +import ftclib.sensor.FtcPinpointOdometry; +import ftclib.sensor.FtcSparkFunOtos; import teamcode.RobotParams; +import trclib.dataprocessor.TrcUtil; +import trclib.drivebase.TrcDriveBase; +import trclib.robotcore.TrcPidController; /** * This class creates the appropriate Robot Drive Base according to the specified robot type. */ public class RobotBase { + /** + * 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 + + public static class VisionOnlyParams extends FtcRobotDrive.RobotInfo + { + public VisionOnlyParams() + { + robotName = "VisionOnly"; + webCam1 = new Vision.FrontCamParams(); + webCam2 = new Vision.BackCamParams(); + limelight = new Vision.LimelightParams(); + } //VisionOnlyParams + } //class VisionOnlyParams + + /** + * This class contains the Differential Robot Parameters. + */ + public static class DifferentialParams extends FtcRobotDrive.RobotInfo + { + public DifferentialParams() + { + robotName = "DifferentialRobot"; + // Robot Dimensions (measured from CAD model if possible) + robotLength = RobotParams.Robot.ROBOT_LENGTH; + robotWidth = RobotParams.Robot.ROBOT_WIDTH; + wheelBaseLength = (24.0 * 14)*TrcUtil.INCHES_PER_MM; + wheelBaseWidth = 16.0; + // IMU + imuName = "imu"; + hubLogoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; + hubUsbDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; + // Drive Motors + driveMotorType = FtcMotorActuator.MotorType.DcMotor; + driveMotorNames = new String[] {"lfDriveMotor", "rfDriveMotor"}; + driveMotorInverted = new boolean[] {true, false}; + odometryType = TrcDriveBase.OdometryType.MotorOdometry; + // Drive Motor Odometry + yDrivePosScale = 0.02166184604662450653409090909091; // in/count + // Robot Drive Characteristics + robotMaxVelocity = 80.0; // inches/sec + robotMaxAcceleration = 350.0; // inches/sec2 + robotMaxTurnRate = 80.0; // degrees/sec + profiledMaxVelocity = robotMaxVelocity; + profiledMaxAcceleration = robotMaxAcceleration; + profiledMaxTurnRate = robotMaxTurnRate; + // DriveBase PID Parameters + drivePidTolerance = 1.0; + turnPidTolerance = 1.0; + yDrivePidCoeffs = new TrcPidController.PidCoefficients(0.06, 0.0, 0.002, 0.0, 0.0); + yDrivePidPowerLimit = 1.0; + yDriveMaxPidRampRate = null; + turnPidCoeffs = new TrcPidController.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 TrcPidController.PidCoefficients(0.0, 0.0, 0.0, 1.0/profiledMaxVelocity, 0.0); + } //DifferentialParams + } //class DifferentialParams + + /** + * 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"; + // Robot Dimensions + robotLength = RobotParams.Robot.ROBOT_LENGTH; + robotWidth = RobotParams.Robot.ROBOT_WIDTH; + wheelBaseLength = (24.0 * 14)*TrcUtil.INCHES_PER_MM; + wheelBaseWidth = 16.0; + // IMU + imuName = "imu"; + hubLogoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; + hubUsbDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; + // Drive Motors + driveMotorType = FtcMotorActuator.MotorType.DcMotor; + driveMotorNames = new String[] {"lfDriveMotor", "rfDriveMotor", "lbDriveMotor", "rbDriveMotor"}; + driveMotorInverted = new boolean[] {true, false, true, false}; + odometryType = TrcDriveBase.OdometryType.AbsoluteOdometry; + // Odometry Wheels + odWheelXScale = odWheelYScale = Math.PI * ODWHEEL_DIAMETER / ODWHEEL_CPR; + xOdWheelSensorNames = new String[] {"xOdWheelSensor"}; + xOdWheelIndices = new int[] {0}; + xOdWheelXOffsets = new double[] {0.0}; + xOdWheelYOffsets = new double[] {-168.0 * TrcUtil.INCHES_PER_MM}; + yOdWheelSensorNames = new String[] {"yLeftOdWheelSensor", "yRightOdWheelSensor"}; + yOdWheelIndices = new int[] {1, 2}; + yOdWheelXOffsets = new double[] {-144.0 * TrcUtil.INCHES_PER_MM, 144.0 * TrcUtil.INCHES_PER_MM}; + yOdWheelYOffsets = new double[] {-12.0 * TrcUtil.INCHES_PER_MM, -12.0 * TrcUtil.INCHES_PER_MM}; + // Absolute Odometry + if (odometryType == TrcDriveBase.OdometryType.AbsoluteOdometry) + { + if (RobotParams.Preferences.usePinpointOdometry) + { + FtcPinpointOdometry.Config ppOdoConfig = new FtcPinpointOdometry.Config() + .setPodOffsets(-144.0, -168.0) + .setEncoderResolution(ODWHEEL_CPR / Math.PI * ODWHEEL_DIAMETER) + .setEncodersInverted(false, false); + absoluteOdometry = new FtcPinpointOdometry("pinpointOdo", ppOdoConfig); + headingWrapRangeLow = -180.0; + headingWrapRangeHigh = 180.0; + } + else if (RobotParams.Preferences.useSparkfunOTOS) + { + FtcSparkFunOtos.Config otosConfig = new FtcSparkFunOtos.Config() + .setOffset(0.0, -12.0 * TrcUtil.INCHES_PER_MM, 0.0) + .setScale(1.0, 1.0); + absoluteOdometry = new FtcSparkFunOtos("sparkfunOtos", otosConfig); + } + } + else + { + absoluteOdometry = null; + } + // Drive Motor Odometry + xDrivePosScale = 1.0; // in/count + yDrivePosScale = 1.0; // in/count + // Robot Drive Characteristics + robotMaxVelocity = 80.0; // inches/sec + robotMaxAcceleration = 350.0; // inches/sec2 + robotMaxTurnRate = 80.0; // degrees/sec + profiledMaxVelocity = robotMaxVelocity; + profiledMaxAcceleration = robotMaxAcceleration; + profiledMaxTurnRate = robotMaxTurnRate; + // DriveBase PID Parameters + drivePidTolerance = 1.0; + turnPidTolerance = 1.0; + xDrivePidCoeffs = new TrcPidController.PidCoefficients(0.95, 0.0, 0.001, 0.0, 0.0); + xDrivePidPowerLimit = 1.0; + xDriveMaxPidRampRate = null; + yDrivePidCoeffs = new TrcPidController.PidCoefficients(0.06, 0.0, 0.002, 0.0, 0.0); + yDrivePidPowerLimit = 1.0; + yDriveMaxPidRampRate = null; + turnPidCoeffs = new TrcPidController.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 TrcPidController.PidCoefficients(0.0, 0.0, 0.0, 1.0/profiledMaxVelocity, 0.0); + // Vision + webCam1 = new Vision.FrontCamParams(); + webCam2 = new Vision.BackCamParams(); + limelight = new Vision.LimelightParams(); + // Miscellaneous + indicatorName = "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"; + // Robot Dimensions + robotLength = RobotParams.Robot.ROBOT_LENGTH; + robotWidth = RobotParams.Robot.ROBOT_WIDTH; + wheelBaseLength = (24.0 * 14)*TrcUtil.INCHES_PER_MM; + wheelBaseWidth = 16.0; + // IMU + imuName = "imu"; + hubLogoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; + hubUsbDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; + // Drive Motors + driveMotorType = FtcMotorActuator.MotorType.DcMotor; + driveMotorNames = new String[] {"lfDriveMotor", "rfDriveMotor", "lbDriveMotor", "rbDriveMotor"}; + driveMotorInverted = new boolean[] {true, false, true, false}; + odometryType = TrcDriveBase.OdometryType.OdometryWheels; + // Odometry Wheels + odWheelXScale = odWheelYScale = Math.PI * ODWHEEL_DIAMETER / ODWHEEL_CPR; + xOdWheelSensorNames = null; + xOdWheelIndices = new int[] {FtcRobotDrive.INDEX_RIGHT_BACK}; + xOdWheelXOffsets = new double[] {0.0}; + xOdWheelYOffsets = new double[] {-168.0 * TrcUtil.INCHES_PER_MM}; + yOdWheelSensorNames = null; + yOdWheelIndices = new int[] {FtcRobotDrive.INDEX_LEFT_FRONT, FtcRobotDrive.INDEX_RIGHT_FRONT}; + yOdWheelXOffsets = new double[] {-144.0 * TrcUtil.INCHES_PER_MM, 144.0 * TrcUtil.INCHES_PER_MM}; + yOdWheelYOffsets = new double[] {-12.0 * TrcUtil.INCHES_PER_MM, -12.0 * TrcUtil.INCHES_PER_MM}; + // Absolute Odometry + if (odometryType == TrcDriveBase.OdometryType.AbsoluteOdometry) + { + if (RobotParams.Preferences.usePinpointOdometry) + { + FtcPinpointOdometry.Config ppOdoConfig = new FtcPinpointOdometry.Config() + .setPodOffsets(-144.0, -168.0) + .setEncoderResolution(ODWHEEL_CPR / Math.PI * ODWHEEL_DIAMETER) + .setEncodersInverted(false, false); + absoluteOdometry = new FtcPinpointOdometry("pinpointOdo", ppOdoConfig); + headingWrapRangeLow = -180.0; + headingWrapRangeHigh = 180.0; + } + else if (RobotParams.Preferences.useSparkfunOTOS) + { + FtcSparkFunOtos.Config otosConfig = new FtcSparkFunOtos.Config() + .setOffset(0.0, -12.0 * TrcUtil.INCHES_PER_MM, 0.0) + .setScale(1.0, 1.0); + absoluteOdometry = new FtcSparkFunOtos("sparkfunOtos", otosConfig); + } + } + else + { + absoluteOdometry = null; + } + // Drive Motor Odometry + xDrivePosScale = 1.0; // in/count + yDrivePosScale = 1.0; // in/count + // Robot Drive Characteristics + robotMaxVelocity = 80.0; // inches/sec + robotMaxAcceleration = 350.0; // inches/sec2 + robotMaxTurnRate = 80.0; // degrees/sec + profiledMaxVelocity = robotMaxVelocity; + profiledMaxAcceleration = robotMaxAcceleration; + profiledMaxTurnRate = robotMaxTurnRate; + // DriveBase PID Parameters + drivePidTolerance = 1.0; + turnPidTolerance = 1.0; + xDrivePidCoeffs = yDrivePidCoeffs = new TrcPidController.PidCoefficients(0.95, 0.0, 0.001, 0.0, 0.0); + xDrivePidPowerLimit = yDrivePidPowerLimit = 1.0; + xDriveMaxPidRampRate = yDriveMaxPidRampRate = null; + turnPidCoeffs = new TrcPidController.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 TrcPidController.PidCoefficients(0.0, 0.0, 0.0, 1.0/profiledMaxVelocity, 0.0); + // Vision + webCam1 = new Vision.FrontCamParams(); + webCam2 = new Vision.BackCamParams(); + limelight = new Vision.LimelightParams(); + // Miscellaneous + indicatorName = "blinkin"; + // Steer Encoders + steerEncoderNames = new String[] {"lfSteerEncoder", "rfSteerEncoder", "lbSteerEncoder", "rbSteerEncoder"}; + steerEncoderInverted = new boolean[] {false, false, false, false}; + steerEncoderZeros = new double[] {0.474812, 0.467663, 0.541338, 0.545340}; + steerZerosFilePath = RobotParams.Robot.STEER_ZERO_CAL_FILE; + // Steer Motors + steerMotorType = FtcMotorActuator.MotorType.CRServo; + steerMotorNames = new String[] {"lfSteerServo", "rfSteerServo", "lbSteerServo", "rbSteerServo"}; + steerMotorInverted = new boolean[] {true, true, true, true}; + steerMotorPidCoeffs = new TrcPidController.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 + private final FtcRobotDrive.RobotInfo robotInfo; private final FtcRobotDrive robotDrive; @@ -44,24 +332,23 @@ public class RobotBase switch (RobotParams.Preferences.robotType) { case VisionOnly: - robotInfo = new RobotParams.VisionOnlyParams(); + robotInfo = new VisionOnlyParams(); robotDrive = null; break; case DifferentialRobot: - robotInfo = new RobotParams.DifferentialParams(); + robotInfo = new DifferentialParams(); robotDrive = RobotParams.Preferences.useDriveBase? new FtcDifferentialDrive(robotInfo): null; break; case MecanumRobot: - robotInfo = new RobotParams.MecanumParams(); + robotInfo = new MecanumParams(); robotDrive = RobotParams.Preferences.useDriveBase? new FtcMecanumDrive(robotInfo): null; break; case SwerveRobot: - robotInfo = new RobotParams.SwerveParams(); - robotDrive = RobotParams.Preferences.useDriveBase? - new FtcSwerveDrive((RobotParams.SwerveParams) robotInfo): null; + robotInfo = new SwerveParams(); + robotDrive = RobotParams.Preferences.useDriveBase? new FtcSwerveDrive((SwerveParams) robotInfo): null; break; default: diff --git a/TeamCode/src/main/java/teamcode/vision/Vision.java b/TeamCode/src/main/java/teamcode/subsystems/Vision.java similarity index 81% rename from TeamCode/src/main/java/teamcode/vision/Vision.java rename to TeamCode/src/main/java/teamcode/subsystems/Vision.java index f5d4672..6d79fe2 100644 --- a/TeamCode/src/main/java/teamcode/vision/Vision.java +++ b/TeamCode/src/main/java/teamcode/subsystems/Vision.java @@ -20,9 +20,7 @@ * SOFTWARE. */ -package teamcode.vision; - -import com.qualcomm.robotcore.hardware.LED; +package teamcode.subsystems; import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; @@ -33,9 +31,11 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; import org.opencv.imgproc.Imgproc; import org.openftc.easyopencv.OpenCvCamera; import org.openftc.easyopencv.OpenCvCameraFactory; +import org.openftc.easyopencv.OpenCvCameraRotation; import java.util.ArrayList; +import ftclib.drivebase.FtcRobotDrive; import ftclib.robotcore.FtcOpMode; import ftclib.vision.FtcCameraStreamProcessor; import ftclib.vision.FtcEocvColorBlobProcessor; @@ -47,9 +47,10 @@ import ftclib.vision.FtcVisionAprilTag; import ftclib.vision.FtcVisionEocvColorBlob; import teamcode.Robot; import teamcode.RobotParams; -import teamcode.subsystems.LEDIndicator; import trclib.pathdrive.TrcPose2D; +import trclib.pathdrive.TrcPose3D; import trclib.robotcore.TrcDbgTrace; +import trclib.vision.TrcHomographyMapper; import trclib.vision.TrcOpenCvColorBlobPipeline; import trclib.vision.TrcOpenCvDetector; import trclib.vision.TrcVisionTargetInfo; @@ -63,6 +64,90 @@ public class Vision { private final String moduleName = getClass().getSimpleName(); + /** + * This class contains the parameters of the front camera. + */ + public static class FrontCamParams extends FtcRobotDrive.VisionInfo + { + public FrontCamParams() + { + camName = "Webcam 1"; + camImageWidth = 640; + camImageHeight = 480; + camXOffset = 0.0; // Inches to the right from robot center + camYOffset = 2.0; // Inches forward from robot center + camZOffset = 9.75; // Inches up from the floor + camPitch = 15.0; // degrees down from horizontal + camYaw = 0.0; // degrees clockwise from robot front + camRoll = 0.0; + camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll); + camOrientation = OpenCvCameraRotation.UPRIGHT; + // 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 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Top Left + 11.4375, 44.75 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Top Right + -2.5625, 21.0 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Bottom Left + 2.5626, 21.0 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset); // World Bottom Right + } //FrontCamParams + } //class FrontCamParams + + /** + * This class contains the parameters of the back camera. + */ + public static class BackCamParams extends FtcRobotDrive.VisionInfo + { + public BackCamParams() + { + camName = "Webcam 2"; + camImageWidth = 640; + camImageHeight = 480; + camXOffset = 0.0; // Inches to the right from robot center + camYOffset = 2.0; // Inches forward from robot center + camZOffset = 9.75; // Inches up from the floor + camPitch = 15.0; // degrees down from horizontal + camYaw = 0.0; // degrees clockwise from robot front + camRoll = 0.0; + camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll); + camOrientation = OpenCvCameraRotation.UPRIGHT; + // 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 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Top Left + 11.4375, 44.75 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Top Right + -2.5625, 21.0 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Bottom Left + 2.5626, 21.0 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset); // World Bottom Right + } //BackCamParams + } //class BackCamParams + + /** + * This class contains the parameters of the Limelight vision processor. + */ + public static class LimelightParams extends FtcRobotDrive.VisionInfo + { + public LimelightParams() + { + camName = "Limelight3a"; + camImageWidth = 640; + camImageHeight = 480; + camXOffset = 0.0; // Inches to the right from robot center + camYOffset = 2.0; // Inches forward from robot center + camZOffset = 9.75; // Inches up from the floor + camPitch = 15.0; // degrees down from horizontal + camYaw = 0.0; // degrees clockwise from robot front + camRoll = 0.0; + camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll); + } //LimelightParams + } //class LimelightParams + public enum ColorBlobType { RedBlob, @@ -145,7 +230,8 @@ public class Vision tracer.traceInfo(moduleName, "Starting RawEocvColorBlobVision..."); rawColorBlobPipeline = new FtcRawEocvColorBlobPipeline( - "rawColorBlobPipeline", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, true); + "rawColorBlobPipeline", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, + true, false); // By default, display original Mat. rawColorBlobPipeline.setVideoOutput(0); rawColorBlobPipeline.setAnnotateEnabled(true); @@ -162,6 +248,7 @@ public class Vision { limelightVision = new FtcLimelightVision( robot.robotInfo.limelight.camName, robot.robotInfo.limelight.camPose, this::getTargetGroundOffset); + limelightVision.setPipeline(0); } // Creating Vision Processors for VisionPortal. ArrayList visionProcessorsList = new ArrayList<>(); @@ -192,13 +279,13 @@ public class Vision tracer.traceInfo(moduleName, "Starting ColorBlobVision..."); redBlobVision = new FtcVisionEocvColorBlob( - "RedBlob", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, true, + "RedBlob", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, true, false, robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true); redBlobProcessor = redBlobVision.getVisionProcessor(); visionProcessorsList.add(redBlobProcessor); blueBlobVision = new FtcVisionEocvColorBlob( - "BlueBlob", colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams, true, + "BlueBlob", colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams, true, false, robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true); blueBlobProcessor = blueBlobVision.getVisionProcessor(); visionProcessorsList.add(blueBlobProcessor); @@ -314,10 +401,12 @@ public class Vision int[] gainSetting = vision.getGainSetting(); int currGain = vision.getCurrentGain(); - robot.dashboard.displayPrintf( - lineNum, "Exp: %d (%d:%d), Gain: %d (%d:%d)", - currExposure, exposureSetting[0], exposureSetting[1], - currGain, gainSetting != null? gainSetting[0]: 0, gainSetting != null? gainSetting[1]: 0); + if (exposureSetting != null && gainSetting != null) + { + robot.dashboard.displayPrintf( + lineNum, "Exp: %d (%d:%d), Gain: %d (%d:%d)", + currExposure, exposureSetting[0], exposureSetting[1], currGain, gainSetting[0], gainSetting[1]); + } } //displayExposureSettings /** @@ -386,7 +475,9 @@ public class Vision if (lineNum != -1) { robot.dashboard.displayPrintf( - lineNum, "RawColorBlob: %s", colorBlobInfo != null? colorBlobInfo: "Not found."); + lineNum, "RawColorBlob: %s, heading=%.3f", + colorBlobInfo != null? colorBlobInfo: "Not found.", + robot.robotDrive != null? robot.robotDrive.driveBase.getHeading(): 0.0); } return colorBlobInfo; @@ -598,7 +689,17 @@ public class Vision { TrcPose2D robotPose = null; - if (aprilTagVision != null) + if (isLimelightVisionEnabled()) + { + TrcVisionTargetInfo aprilTagInfo = + getLimelightDetectedObject(FtcLimelightVision.ResultType.Fiducial, null, -1); + + if (aprilTagInfo != null) + { + robotPose = aprilTagInfo.detectedObj.robotPose; + } + } + else if (isAprilTagVisionEnabled()) { // Find any AprilTag in view. TrcVisionTargetInfo aprilTagInfo = getDetectedAprilTag(null, -1); @@ -676,20 +777,17 @@ public class Vision ColorBlobType colorBlobType, int lineNum) { TrcVisionTargetInfo colorBlobInfo = null; - String colorBlobName = null; switch (colorBlobType) { case RedBlob: colorBlobInfo = redBlobVision != null? redBlobVision.getBestDetectedTargetInfo( null, this::compareDistance, 0.0, 0.0): null; - colorBlobName = LEDIndicator.RED_BLOB; break; case BlueBlob: colorBlobInfo = blueBlobVision != null? blueBlobVision.getBestDetectedTargetInfo( null, this::compareDistance, 0.0, 0.0): null; - colorBlobName = LEDIndicator.BLUE_BLOB; break; case AnyColorBlob: @@ -717,7 +815,6 @@ public class Vision colorBlobList.sort(this::compareDistance); } colorBlobInfo = colorBlobList.get(0); - colorBlobName = colorBlobInfo.detectedObj.label; } break; } @@ -730,13 +827,21 @@ public class Vision if (colorBlobInfo != null && robot.ledIndicator != null) { - robot.ledIndicator.setDetectedPattern(colorBlobName); + robot.ledIndicator.setDetectedPattern(colorBlobInfo.detectedObj.label); } if (lineNum != -1) { - robot.dashboard.displayPrintf( - lineNum, "%s: %s", colorBlobName, colorBlobInfo != null? colorBlobInfo: "Not found."); + if (colorBlobInfo != null) + { + robot.dashboard.displayPrintf( + lineNum, "%s: %s (rotatedAngle=%.1f", + colorBlobInfo.detectedObj.label, colorBlobInfo, colorBlobInfo.detectedObj.rotatedRect.angle); + } + else + { + robot.dashboard.displayPrintf(lineNum, "No colorblob found."); + } } return colorBlobInfo; @@ -772,7 +877,7 @@ public class Vision TrcVisionTargetInfo a, TrcVisionTargetInfo b) { - return (int)((b.objPose.y - a.objPose.y)*100); + return (int)((a.objPose.y - b.objPose.y)*100); } //compareDistance } //class Vision diff --git a/TeamCode/src/main/java/trclib b/TeamCode/src/main/java/trclib index d9953b9..5a6a5b4 160000 --- a/TeamCode/src/main/java/trclib +++ b/TeamCode/src/main/java/trclib @@ -1 +1 @@ -Subproject commit d9953b914bd293f540aebc502d118c8959c79127 +Subproject commit 5a6a5b4e4ca63ca28dcbf20b5bdc9db4f892071b