mirror of
https://github.com/trc492/FtcTemplate.git
synced 2025-07-04 06:21:23 -07:00
Sync'd latest changes.
Updated ftclib, trclib. Reorganized params.
This commit is contained in:
Titan Robotics Club
@ -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
|
||||
|
||||
Reference in New Issue
Block a user