/* * Copyright (c) 2023 Titan Robotics Club (http://www.titanrobotics.com) * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in all * copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE * SOFTWARE. */ package teamcode; import android.os.Environment; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot.*; import org.openftc.easyopencv.OpenCvCameraRotation; import ftclib.drivebase.FtcRobotDrive; import ftclib.drivebase.FtcSwerveDrive; import ftclib.motor.FtcMotorActuator.MotorType; import trclib.dataprocessor.TrcUtil; 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 Gobilda motor parameters. */ public static class Gobilda { // 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 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; // Vision 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 useLimelightVision = 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 useExternalOdometry = 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"; // Front Camera webCam1 = new FrontCamParams(); // Back Camera 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}; // Odometry Wheels odWheelScale = Math.PI * ODWHEEL_DIAMETER / ODWHEEL_CPR; // 0.00105687652708656383937269814237 in/count xOdWheelIndices = new int[] {FtcRobotDrive.INDEX_RIGHT_BACK}; xOdWheelXOffsets = new double[] {0.0}; xOdWheelYOffsets = new double[] {-168.0 * TrcUtil.INCHES_PER_MM}; 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}; // 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 blinkinName = "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}; // Odometry Wheels odWheelScale = Math.PI * ODWHEEL_DIAMETER / ODWHEEL_CPR; // 0.00105687652708656383937269814237 in/count xOdWheelIndices = new int[] {FtcRobotDrive.INDEX_RIGHT_BACK}; xOdWheelXOffsets = new double[] {0.0}; xOdWheelYOffsets = new double[] {-168.0 * TrcUtil.INCHES_PER_MM}; 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}; // 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 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"; // 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}; // Odometry Wheels odWheelScale = Math.PI * ODWHEEL_DIAMETER / ODWHEEL_CPR; // 0.00105687652708656383937269814237 in/count xOdWheelIndices = new int[] {FtcRobotDrive.INDEX_RIGHT_BACK}; xOdWheelXOffsets = new double[] {0.0}; xOdWheelYOffsets = new double[] {-168.0 * TrcUtil.INCHES_PER_MM}; 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}; // 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 blinkinName = "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