mirror of
https://github.com/trc492/FtcTemplate.git
synced 2025-07-02 13:31:24 -07:00
Updated ftclib.
Sync'd with FRC changes.
This commit is contained in:
Submodule TeamCode/src/main/java/ftclib updated: a4b99cd416...542c65c802
@ -129,7 +129,7 @@ public class FtcAuto extends FtcOpMode
|
|||||||
Robot.matchInfo = FtcMatchInfo.getMatchInfo();
|
Robot.matchInfo = FtcMatchInfo.getMatchInfo();
|
||||||
String filePrefix = String.format(
|
String filePrefix = String.format(
|
||||||
Locale.US, "%s%02d_Auto", Robot.matchInfo.matchType, Robot.matchInfo.matchNumber);
|
Locale.US, "%s%02d_Auto", Robot.matchInfo.matchType, Robot.matchInfo.matchNumber);
|
||||||
TrcDbgTrace.openTraceLog(RobotParams.System.LOG_FOLDER_PATH, filePrefix);
|
TrcDbgTrace.openTraceLog(RobotParams.Robot.LOG_FOLDER_PATH, filePrefix);
|
||||||
}
|
}
|
||||||
//
|
//
|
||||||
// Create and run choice menus.
|
// Create and run choice menus.
|
||||||
|
@ -68,8 +68,8 @@ public class FtcTeleOp extends FtcOpMode
|
|||||||
// Create and initialize robot object.
|
// Create and initialize robot object.
|
||||||
//
|
//
|
||||||
robot = new Robot(TrcRobot.getRunMode());
|
robot = new Robot(TrcRobot.getRunMode());
|
||||||
drivePowerScale = robot.robotInfo.driveNormalScale;
|
drivePowerScale = RobotParams.Robot.DRIVE_NORMAL_SCALE;
|
||||||
turnPowerScale = robot.robotInfo.turnNormalScale;
|
turnPowerScale = RobotParams.Robot.TURN_NORMAL_SCALE;
|
||||||
|
|
||||||
//
|
//
|
||||||
// Open trace log.
|
// Open trace log.
|
||||||
@ -79,7 +79,7 @@ public class FtcTeleOp extends FtcOpMode
|
|||||||
String filePrefix = Robot.matchInfo != null?
|
String filePrefix = Robot.matchInfo != null?
|
||||||
String.format(Locale.US, "%s%02d_TeleOp", Robot.matchInfo.matchType, Robot.matchInfo.matchNumber):
|
String.format(Locale.US, "%s%02d_TeleOp", Robot.matchInfo.matchType, Robot.matchInfo.matchNumber):
|
||||||
"Unknown_TeleOp";
|
"Unknown_TeleOp";
|
||||||
TrcDbgTrace.openTraceLog(RobotParams.System.LOG_FOLDER_PATH, filePrefix);
|
TrcDbgTrace.openTraceLog(RobotParams.Robot.LOG_FOLDER_PATH, filePrefix);
|
||||||
}
|
}
|
||||||
//
|
//
|
||||||
// Create and initialize Gamepads.
|
// Create and initialize Gamepads.
|
||||||
@ -90,7 +90,7 @@ public class FtcTeleOp extends FtcOpMode
|
|||||||
operatorGamepad.setButtonEventHandler(this::operatorButtonEvent);
|
operatorGamepad.setButtonEventHandler(this::operatorButtonEvent);
|
||||||
driverGamepad.setLeftStickInverted(false, true);
|
driverGamepad.setLeftStickInverted(false, true);
|
||||||
operatorGamepad.setRightStickInverted(false, true);
|
operatorGamepad.setRightStickInverted(false, true);
|
||||||
setDriveOrientation(robot.robotInfo.driveOrientation);
|
setDriveOrientation(RobotParams.Robot.DRIVE_ORIENTATION);
|
||||||
} //robotInit
|
} //robotInit
|
||||||
|
|
||||||
//
|
//
|
||||||
@ -183,7 +183,7 @@ public class FtcTeleOp extends FtcOpMode
|
|||||||
else
|
else
|
||||||
{
|
{
|
||||||
double[] inputs = driverGamepad.getDriveInputs(
|
double[] inputs = driverGamepad.getDriveInputs(
|
||||||
robot.robotInfo.driveMode, true, drivePowerScale, turnPowerScale);
|
RobotParams.Robot.DRIVE_MODE, true, drivePowerScale, turnPowerScale);
|
||||||
|
|
||||||
if (robot.robotDrive.driveBase.supportsHolonomicDrive())
|
if (robot.robotDrive.driveBase.supportsHolonomicDrive())
|
||||||
{
|
{
|
||||||
@ -312,14 +312,14 @@ public class FtcTeleOp extends FtcOpMode
|
|||||||
if (pressed)
|
if (pressed)
|
||||||
{
|
{
|
||||||
robot.globalTracer.traceInfo(moduleName, ">>>>> DrivePower slow.");
|
robot.globalTracer.traceInfo(moduleName, ">>>>> DrivePower slow.");
|
||||||
drivePowerScale = robot.robotInfo.driveSlowScale;
|
drivePowerScale = RobotParams.Robot.DRIVE_SLOW_SCALE;
|
||||||
turnPowerScale = robot.robotInfo.turnSlowScale;
|
turnPowerScale = RobotParams.Robot.TURN_SLOW_SCALE;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
robot.globalTracer.traceInfo(moduleName, ">>>>> DrivePower normal.");
|
robot.globalTracer.traceInfo(moduleName, ">>>>> DrivePower normal.");
|
||||||
drivePowerScale = robot.robotInfo.driveNormalScale;
|
drivePowerScale = RobotParams.Robot.DRIVE_NORMAL_SCALE;
|
||||||
turnPowerScale = robot.robotInfo.turnNormalScale;
|
turnPowerScale = RobotParams.Robot.TURN_NORMAL_SCALE;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
@ -117,7 +117,7 @@ public class FtcTest extends FtcTeleOp
|
|||||||
|
|
||||||
} //class TestChoices
|
} //class TestChoices
|
||||||
|
|
||||||
private final FtcPidCoeffCache pidCoeffCache = new FtcPidCoeffCache(RobotParams.System.TEAM_FOLDER_PATH);
|
private final FtcPidCoeffCache pidCoeffCache = new FtcPidCoeffCache(RobotParams.Robot.TEAM_FOLDER_PATH);
|
||||||
private final TestChoices testChoices = new TestChoices();
|
private final TestChoices testChoices = new TestChoices();
|
||||||
private TrcElapsedTimer elapsedTimer = null;
|
private TrcElapsedTimer elapsedTimer = null;
|
||||||
private FtcChoiceMenu<Test> testMenu = null;
|
private FtcChoiceMenu<Test> testMenu = null;
|
||||||
|
@ -125,7 +125,7 @@ public class Robot
|
|||||||
@Override
|
@Override
|
||||||
public String toString()
|
public String toString()
|
||||||
{
|
{
|
||||||
return robotInfo != null? robotInfo.robotName: RobotParams.System.DEF_ROBOT_NAME;
|
return robotInfo != null? robotInfo.robotName: RobotParams.Robot.ROBOT_CODEBASE;
|
||||||
} //toString
|
} //toString
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -267,6 +267,9 @@ public class Robot
|
|||||||
//
|
//
|
||||||
// Display other subsystem status here.
|
// Display other subsystem status here.
|
||||||
//
|
//
|
||||||
|
if (RobotParams.Preferences.showSubsystems)
|
||||||
|
{
|
||||||
|
}
|
||||||
}
|
}
|
||||||
} //updateStatus
|
} //updateStatus
|
||||||
|
|
||||||
|
@ -34,6 +34,7 @@ import ftclib.motor.FtcMotorActuator.MotorType;
|
|||||||
import trclib.dataprocessor.TrcUtil;
|
import trclib.dataprocessor.TrcUtil;
|
||||||
import trclib.drivebase.TrcDriveBase.DriveOrientation;
|
import trclib.drivebase.TrcDriveBase.DriveOrientation;
|
||||||
import trclib.driverio.TrcGameController;
|
import trclib.driverio.TrcGameController;
|
||||||
|
import trclib.driverio.TrcGameController.DriveMode;
|
||||||
import trclib.pathdrive.TrcPose2D;
|
import trclib.pathdrive.TrcPose2D;
|
||||||
import trclib.robotcore.TrcPidController.PidCoefficients;
|
import trclib.robotcore.TrcPidController.PidCoefficients;
|
||||||
import trclib.vision.TrcHomographyMapper;
|
import trclib.vision.TrcHomographyMapper;
|
||||||
@ -86,13 +87,24 @@ public class RobotParams
|
|||||||
/**
|
/**
|
||||||
* This class contains miscellaneous robot info.
|
* This class contains miscellaneous robot info.
|
||||||
*/
|
*/
|
||||||
public static class System
|
public static class Robot
|
||||||
{
|
{
|
||||||
public static final String TEAM_FOLDER_PATH =
|
public static final String TEAM_FOLDER_PATH =
|
||||||
Environment.getExternalStorageDirectory().getPath() + "/FIRST/ftcTeam";
|
Environment.getExternalStorageDirectory().getPath() + "/FIRST/ftcTeam";
|
||||||
public static final String LOG_FOLDER_PATH = TEAM_FOLDER_PATH + "/tracelogs";
|
public static final String LOG_FOLDER_PATH = TEAM_FOLDER_PATH + "/tracelogs";
|
||||||
public static final String DEF_ROBOT_NAME = "Robot3543";
|
public static final String STEER_ZERO_CAL_FILE = TEAM_FOLDER_PATH + "/SteerZeroCalibration.txt";
|
||||||
} //class System
|
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
|
* When the season starts, the competition robot may not be ready for programmers. It's crucial to save time by
|
||||||
@ -149,36 +161,80 @@ public class RobotParams
|
|||||||
// Robot Parameters.
|
// 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 TrcPose2D(camXOffset, camYOffset, camYaw);
|
||||||
|
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 front 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 TrcPose2D(camXOffset, camYOffset, camYaw);
|
||||||
|
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
|
||||||
|
|
||||||
public static class VisionOnlyParams extends FtcRobotDrive.RobotInfo
|
public static class VisionOnlyParams extends FtcRobotDrive.RobotInfo
|
||||||
{
|
{
|
||||||
public VisionOnlyParams()
|
public VisionOnlyParams()
|
||||||
{
|
{
|
||||||
robotName = "VisionOnly";
|
robotName = "VisionOnly";
|
||||||
// Vision
|
// Front Camera
|
||||||
webCam1Name = "Webcam 1";
|
webCam1 = new FrontCamParams();
|
||||||
webCam2Name = "Webcam 2";
|
// Back Camera
|
||||||
camImageWidth = 640;
|
webCam2 = new BackCamParams();
|
||||||
camImageHeight = 480;
|
|
||||||
camOrientation = OpenCvCameraRotation.UPRIGHT;
|
|
||||||
// Camera location On Robot
|
|
||||||
camXOffset = 0.0; //Camera right offset from robot centroid
|
|
||||||
camYOffset = 2.0; //Camera forward offset from robot centroid
|
|
||||||
camZOffset = 9.75; //Camera height offset from floor
|
|
||||||
camTiltDown = 15.00; //Camera tilt down angle from horizontal in deg
|
|
||||||
camPose = new TrcPose2D(camXOffset, camYOffset, 0.0);
|
|
||||||
// Homography: cameraRect in pixels, worldRect in inches
|
|
||||||
cameraRect = new TrcHomographyMapper.Rectangle(
|
|
||||||
0.0, 120.0, // Camera Top Left
|
|
||||||
camImageWidth -1, 120.0, // Camera Top Right
|
|
||||||
0.0, camImageHeight - 1, // Camera Bottom Left
|
|
||||||
camImageWidth - 1, camImageHeight - 1); // Camera Bottom Right
|
|
||||||
worldRect = new TrcHomographyMapper.Rectangle(
|
|
||||||
-12.5626, 48.0 - robotLength/2.0 - camYOffset, // World Top Left
|
|
||||||
11.4375, 44.75 - robotLength/2.0 - camYOffset, // World Top Right
|
|
||||||
-2.5625, 21.0 - robotLength/2.0 - camYOffset, // World Bottom Left
|
|
||||||
2.5626, 21.0 - robotLength/2.0 - camYOffset); // World Bottom Right
|
|
||||||
} //VisionOnlyParams
|
} //VisionOnlyParams
|
||||||
|
|
||||||
} //class VisionOnlyParams
|
} //class VisionOnlyParams
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -193,19 +249,19 @@ public class RobotParams
|
|||||||
public DifferentialParams()
|
public DifferentialParams()
|
||||||
{
|
{
|
||||||
robotName = "DifferentialRobot";
|
robotName = "DifferentialRobot";
|
||||||
|
// Robot Dimensions
|
||||||
|
robotLength = Robot.ROBOT_LENGTH;
|
||||||
|
robotWidth = Robot.ROBOT_WIDTH;
|
||||||
|
wheelBaseLength = (24.0 * 14)*TrcUtil.INCHES_PER_MM;
|
||||||
|
wheelBaseWidth = 16.0;
|
||||||
// IMU
|
// IMU
|
||||||
imuName = "imu";
|
imuName = "imu";
|
||||||
hubLogoDirection = LogoFacingDirection.UP;
|
hubLogoDirection = LogoFacingDirection.UP;
|
||||||
hubUsbDirection = UsbFacingDirection.FORWARD;
|
hubUsbDirection = UsbFacingDirection.FORWARD;
|
||||||
// Robot Dimensions
|
|
||||||
robotLength = 17.0;
|
|
||||||
robotWidth = 17.0;
|
|
||||||
wheelBaseLength = (24.0 * 14)*TrcUtil.INCHES_PER_MM;
|
|
||||||
wheelBaseWidth = 16.0;
|
|
||||||
// Drive Motors
|
// Drive Motors
|
||||||
driveMotorType = MotorType.DcMotor;
|
driveMotorType = MotorType.DcMotor;
|
||||||
driveMotorNames = new String[] {"lfDriveMotor", "rfDriveMotor", "lbDriveMotor", "rbDriveMotor"};
|
driveMotorNames = new String[] {"lfDriveMotor", "rfDriveMotor"};
|
||||||
driveMotorInverted = new boolean[] {true, false, true, false};
|
driveMotorInverted = new boolean[] {true, false};
|
||||||
// Odometry Wheels
|
// Odometry Wheels
|
||||||
odWheelScale = Math.PI * ODWHEEL_DIAMETER / ODWHEEL_CPR; // 0.00105687652708656383937269814237 in/count
|
odWheelScale = Math.PI * ODWHEEL_DIAMETER / ODWHEEL_CPR; // 0.00105687652708656383937269814237 in/count
|
||||||
xOdWheelOffsetX = 0.0;
|
xOdWheelOffsetX = 0.0;
|
||||||
@ -215,7 +271,6 @@ public class RobotParams
|
|||||||
yRightOdWheelOffsetX = 144.0 * TrcUtil.INCHES_PER_MM;
|
yRightOdWheelOffsetX = 144.0 * TrcUtil.INCHES_PER_MM;
|
||||||
yRightOdWheelOffsetY = -12.0 * TrcUtil.INCHES_PER_MM;
|
yRightOdWheelOffsetY = -12.0 * TrcUtil.INCHES_PER_MM;
|
||||||
// Drive Motor Odometry
|
// Drive Motor Odometry
|
||||||
xDrivePosScale = 0.01924724265461924299065420560748; // in/count
|
|
||||||
yDrivePosScale = 0.02166184604662450653409090909091; // in/count
|
yDrivePosScale = 0.02166184604662450653409090909091; // in/count
|
||||||
// Robot Drive Characteristics
|
// Robot Drive Characteristics
|
||||||
robotMaxVelocity = 23.0; // inches/sec
|
robotMaxVelocity = 23.0; // inches/sec
|
||||||
@ -227,9 +282,6 @@ public class RobotParams
|
|||||||
// DriveBase PID Parameters
|
// DriveBase PID Parameters
|
||||||
drivePidTolerance = 1.0;
|
drivePidTolerance = 1.0;
|
||||||
turnPidTolerance = 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);
|
yDrivePidCoeffs = new PidCoefficients(0.06, 0.0, 0.002, 0.0, 0.0);
|
||||||
yDrivePidPowerLimit = 1.0;
|
yDrivePidPowerLimit = 1.0;
|
||||||
yDriveMaxPidRampRate = null;
|
yDriveMaxPidRampRate = null;
|
||||||
@ -241,39 +293,12 @@ public class RobotParams
|
|||||||
// PurePursuit Parameters
|
// PurePursuit Parameters
|
||||||
ppdFollowingDistance = 6.0;
|
ppdFollowingDistance = 6.0;
|
||||||
velPidCoeffs = new PidCoefficients(0.0, 0.0, 0.0, 1.0 / profiledMaxVelocity, 0.0);
|
velPidCoeffs = new PidCoefficients(0.0, 0.0, 0.0, 1.0 / profiledMaxVelocity, 0.0);
|
||||||
// Robot Drive
|
|
||||||
driveMode = TrcGameController.DriveMode.ArcadeMode;
|
|
||||||
driveOrientation = DriveOrientation.ROBOT;
|
|
||||||
driveSlowScale = 0.3;
|
|
||||||
driveNormalScale = 1.0;
|
|
||||||
turnSlowScale = 0.3;
|
|
||||||
turnNormalScale = 0.6;
|
|
||||||
// Vision
|
// Vision
|
||||||
webCam1Name = "Webcam 1";
|
webCam1 = new FrontCamParams();
|
||||||
webCam2Name = "Webcam 2";
|
webCam2 = new BackCamParams();
|
||||||
camImageWidth = 640;
|
// Miscellaneous
|
||||||
camImageHeight = 480;
|
|
||||||
camOrientation = OpenCvCameraRotation.UPRIGHT;
|
|
||||||
// Camera location On Robot
|
|
||||||
camXOffset = 0.0; //Camera right offset from robot centroid
|
|
||||||
camYOffset = 2.0; //Camera forward offset from robot centroid
|
|
||||||
camZOffset = 9.75; //Camera height offset from floor
|
|
||||||
camTiltDown = 15.00; //Camera tilt down angle from horizontal in deg
|
|
||||||
camPose = new TrcPose2D(camXOffset, camYOffset, 0.0);
|
|
||||||
// Homography: cameraRect in pixels, worldRect in inches
|
|
||||||
cameraRect = new TrcHomographyMapper.Rectangle(
|
|
||||||
0.0, 120.0, // Camera Top Left
|
|
||||||
camImageWidth -1, 120.0, // Camera Top Right
|
|
||||||
0.0, camImageHeight - 1, // Camera Bottom Left
|
|
||||||
camImageWidth - 1, camImageHeight - 1); // Camera Bottom Right
|
|
||||||
worldRect = new TrcHomographyMapper.Rectangle(
|
|
||||||
-12.5626, 48.0 - robotLength/2.0 - camYOffset, // World Top Left
|
|
||||||
11.4375, 44.75 - robotLength/2.0 - camYOffset, // World Top Right
|
|
||||||
-2.5625, 21.0 - robotLength/2.0 - camYOffset, // World Bottom Left
|
|
||||||
2.5626, 21.0 - robotLength/2.0 - camYOffset); // World Bottom Right
|
|
||||||
blinkinName = "blinkin";
|
blinkinName = "blinkin";
|
||||||
} //DifferentialParams
|
} //DifferentialParams
|
||||||
|
|
||||||
} //class DifferentialParams
|
} //class DifferentialParams
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -288,15 +313,15 @@ public class RobotParams
|
|||||||
public MecanumParams()
|
public MecanumParams()
|
||||||
{
|
{
|
||||||
robotName = "MecanumRobot";
|
robotName = "MecanumRobot";
|
||||||
|
// Robot Dimensions
|
||||||
|
robotLength = Robot.ROBOT_LENGTH;
|
||||||
|
robotWidth = Robot.ROBOT_WIDTH;
|
||||||
|
wheelBaseLength = (24.0 * 14)*TrcUtil.INCHES_PER_MM;
|
||||||
|
wheelBaseWidth = 16.0;
|
||||||
// IMU
|
// IMU
|
||||||
imuName = "imu";
|
imuName = "imu";
|
||||||
hubLogoDirection = LogoFacingDirection.UP;
|
hubLogoDirection = LogoFacingDirection.UP;
|
||||||
hubUsbDirection = UsbFacingDirection.FORWARD;
|
hubUsbDirection = UsbFacingDirection.FORWARD;
|
||||||
// Robot Dimensions
|
|
||||||
robotLength = 17.0;
|
|
||||||
robotWidth = 17.0;
|
|
||||||
wheelBaseLength = (24.0 * 14)*TrcUtil.INCHES_PER_MM;
|
|
||||||
wheelBaseWidth = 16.0;
|
|
||||||
// Drive Motors
|
// Drive Motors
|
||||||
driveMotorType = MotorType.DcMotor;
|
driveMotorType = MotorType.DcMotor;
|
||||||
driveMotorNames = new String[] {"lfDriveMotor", "rfDriveMotor", "lbDriveMotor", "rbDriveMotor"};
|
driveMotorNames = new String[] {"lfDriveMotor", "rfDriveMotor", "lbDriveMotor", "rbDriveMotor"};
|
||||||
@ -336,39 +361,12 @@ public class RobotParams
|
|||||||
// PurePursuit Parameters
|
// PurePursuit Parameters
|
||||||
ppdFollowingDistance = 6.0;
|
ppdFollowingDistance = 6.0;
|
||||||
velPidCoeffs = new PidCoefficients(0.0, 0.0, 0.0, 1.0 / profiledMaxVelocity, 0.0);
|
velPidCoeffs = new PidCoefficients(0.0, 0.0, 0.0, 1.0 / profiledMaxVelocity, 0.0);
|
||||||
// Robot Drive
|
|
||||||
driveMode = TrcGameController.DriveMode.ArcadeMode;
|
|
||||||
driveOrientation = DriveOrientation.ROBOT;
|
|
||||||
driveSlowScale = 0.3;
|
|
||||||
driveNormalScale = 1.0;
|
|
||||||
turnSlowScale = 0.3;
|
|
||||||
turnNormalScale = 0.6;
|
|
||||||
// Vision
|
// Vision
|
||||||
webCam1Name = "Webcam 1";
|
webCam1 = new FrontCamParams();
|
||||||
webCam2Name = "Webcam 2";
|
webCam2 = new BackCamParams();
|
||||||
camImageWidth = 640;
|
// Miscellaneous
|
||||||
camImageHeight = 480;
|
|
||||||
camOrientation = OpenCvCameraRotation.UPRIGHT;
|
|
||||||
// Camera location On Robot
|
|
||||||
camXOffset = 0.0; //Camera right offset from robot centroid
|
|
||||||
camYOffset = 2.0; //Camera forward offset from robot centroid
|
|
||||||
camZOffset = 9.75; //Camera height offset from floor
|
|
||||||
camTiltDown = 15.00; //Camera tilt down angle from horizontal in deg
|
|
||||||
camPose = new TrcPose2D(camXOffset, camYOffset, 0.0);
|
|
||||||
// Homography: cameraRect in pixels, worldRect in inches
|
|
||||||
cameraRect = new TrcHomographyMapper.Rectangle(
|
|
||||||
0.0, 120.0, // Camera Top Left
|
|
||||||
camImageWidth -1, 120.0, // Camera Top Right
|
|
||||||
0.0, camImageHeight - 1, // Camera Bottom Left
|
|
||||||
camImageWidth - 1, camImageHeight - 1); // Camera Bottom Right
|
|
||||||
worldRect = new TrcHomographyMapper.Rectangle(
|
|
||||||
-12.5626, 48.0 - robotLength/2.0 - camYOffset, // World Top Left
|
|
||||||
11.4375, 44.75 - robotLength/2.0 - camYOffset, // World Top Right
|
|
||||||
-2.5625, 21.0 - robotLength/2.0 - camYOffset, // World Bottom Left
|
|
||||||
2.5626, 21.0 - robotLength/2.0 - camYOffset); // World Bottom Right
|
|
||||||
blinkinName = "blinkin";
|
blinkinName = "blinkin";
|
||||||
} //MecanumParams
|
} //MecanumParams
|
||||||
|
|
||||||
} //class MecanumParams
|
} //class MecanumParams
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -383,15 +381,15 @@ public class RobotParams
|
|||||||
public SwerveParams()
|
public SwerveParams()
|
||||||
{
|
{
|
||||||
robotName = "SwerveRobot";
|
robotName = "SwerveRobot";
|
||||||
|
// Robot Dimensions
|
||||||
|
robotLength = Robot.ROBOT_LENGTH;
|
||||||
|
robotWidth = Robot.ROBOT_WIDTH;
|
||||||
|
wheelBaseLength = (24.0 * 14)*TrcUtil.INCHES_PER_MM;
|
||||||
|
wheelBaseWidth = 16.0;
|
||||||
// IMU
|
// IMU
|
||||||
imuName = "imu";
|
imuName = "imu";
|
||||||
hubLogoDirection = LogoFacingDirection.UP;
|
hubLogoDirection = LogoFacingDirection.UP;
|
||||||
hubUsbDirection = UsbFacingDirection.FORWARD;
|
hubUsbDirection = UsbFacingDirection.FORWARD;
|
||||||
// Robot Dimensions
|
|
||||||
robotLength = 17.0;
|
|
||||||
robotWidth = 17.0;
|
|
||||||
wheelBaseLength = (24.0 * 14)*TrcUtil.INCHES_PER_MM;
|
|
||||||
wheelBaseWidth = 16.0;
|
|
||||||
// Drive Motors
|
// Drive Motors
|
||||||
driveMotorType = MotorType.DcMotor;
|
driveMotorType = MotorType.DcMotor;
|
||||||
driveMotorNames = new String[] {"lfDriveMotor", "rfDriveMotor", "lbDriveMotor", "rbDriveMotor"};
|
driveMotorNames = new String[] {"lfDriveMotor", "rfDriveMotor", "lbDriveMotor", "rbDriveMotor"};
|
||||||
@ -428,42 +426,16 @@ public class RobotParams
|
|||||||
// PurePursuit Parameters
|
// PurePursuit Parameters
|
||||||
ppdFollowingDistance = 6.0;
|
ppdFollowingDistance = 6.0;
|
||||||
velPidCoeffs = new PidCoefficients(0.0, 0.0, 0.0, 1.0 / profiledMaxVelocity, 0.0);
|
velPidCoeffs = new PidCoefficients(0.0, 0.0, 0.0, 1.0 / profiledMaxVelocity, 0.0);
|
||||||
// Robot Drive
|
|
||||||
driveMode = TrcGameController.DriveMode.ArcadeMode;
|
|
||||||
driveOrientation = DriveOrientation.ROBOT;
|
|
||||||
driveSlowScale = 0.3;
|
|
||||||
driveNormalScale = 1.0;
|
|
||||||
turnSlowScale = 0.3;
|
|
||||||
turnNormalScale = 0.6;
|
|
||||||
// Vision
|
// Vision
|
||||||
webCam1Name = "Webcam 1";
|
webCam1 = new FrontCamParams();
|
||||||
webCam2Name = "Webcam 2";
|
webCam2 = new BackCamParams();
|
||||||
camImageWidth = 640;
|
// Miscellaneous
|
||||||
camImageHeight = 480;
|
|
||||||
camOrientation = OpenCvCameraRotation.UPRIGHT;
|
|
||||||
// Camera location On Robot
|
|
||||||
camXOffset = 0.0; //Camera right offset from robot centroid
|
|
||||||
camYOffset = 2.0; //Camera forward offset from robot centroid
|
|
||||||
camZOffset = 9.75; //Camera height offset from floor
|
|
||||||
camTiltDown = 15.00; //Camera tilt down angle from horizontal in deg
|
|
||||||
camPose = new TrcPose2D(camXOffset, camYOffset, 0.0);
|
|
||||||
// Homography: cameraRect in pixels, worldRect in inches
|
|
||||||
cameraRect = new TrcHomographyMapper.Rectangle(
|
|
||||||
0.0, 120.0, // Camera Top Left
|
|
||||||
camImageWidth -1, 120.0, // Camera Top Right
|
|
||||||
0.0, camImageHeight - 1, // Camera Bottom Left
|
|
||||||
camImageWidth - 1, camImageHeight - 1); // Camera Bottom Right
|
|
||||||
worldRect = new TrcHomographyMapper.Rectangle(
|
|
||||||
-12.5626, 48.0 - robotLength/2.0 - camYOffset, // World Top Left
|
|
||||||
11.4375, 44.75 - robotLength/2.0 - camYOffset, // World Top Right
|
|
||||||
-2.5625, 21.0 - robotLength/2.0 - camYOffset, // World Bottom Left
|
|
||||||
2.5626, 21.0 - robotLength/2.0 - camYOffset); // World Bottom Right
|
|
||||||
blinkinName = "blinkin";
|
blinkinName = "blinkin";
|
||||||
// Steer Encoders
|
// Steer Encoders
|
||||||
steerEncoderNames = new String[] {"lfSteerEncoder", "rfSteerEncoder", "lbSteerEncoder", "rbSteerEncoder"};
|
steerEncoderNames = new String[] {"lfSteerEncoder", "rfSteerEncoder", "lbSteerEncoder", "rbSteerEncoder"};
|
||||||
steerEncoderInverted = new boolean[] {};
|
steerEncoderInverted = new boolean[] {false, false, false, false};
|
||||||
steerEncoderZeros = new double[] {0.474812, 0.467663, 0.541338, 0.545340};
|
steerEncoderZeros = new double[] {0.474812, 0.467663, 0.541338, 0.545340};
|
||||||
steerZerosFilePath = System.TEAM_FOLDER_PATH + "/SteerCalibration.txt";
|
steerZerosFilePath = Robot.STEER_ZERO_CAL_FILE;
|
||||||
// Steer Motors
|
// Steer Motors
|
||||||
steerMotorType = MotorType.CRServo;
|
steerMotorType = MotorType.CRServo;
|
||||||
steerMotorNames = new String[] {"lfSteerServo", "rfSteerServo", "lbSteerServo", "rbSteerServo"};
|
steerMotorNames = new String[] {"lfSteerServo", "rfSteerServo", "lbSteerServo", "rbSteerServo"};
|
||||||
@ -473,7 +445,6 @@ public class RobotParams
|
|||||||
// Swerve Modules
|
// Swerve Modules
|
||||||
swerveModuleNames = new String[] {"lfWheel", "rfWheel", "lbWheel", "rbWheel"};
|
swerveModuleNames = new String[] {"lfWheel", "rfWheel", "lbWheel", "rbWheel"};
|
||||||
} //SwerveParams
|
} //SwerveParams
|
||||||
|
|
||||||
} //class SwerveParams
|
} //class SwerveParams
|
||||||
|
|
||||||
//
|
//
|
||||||
|
@ -106,7 +106,7 @@ public class Vision
|
|||||||
{
|
{
|
||||||
FtcOpMode opMode = FtcOpMode.getInstance();
|
FtcOpMode opMode = FtcOpMode.getInstance();
|
||||||
|
|
||||||
if (robot.robotInfo.webCam1Name == null &&
|
if (robot.robotInfo.webCam1.camName == null &&
|
||||||
(RobotParams.Preferences.useWebCam || RobotParams.Preferences.tuneColorBlobVision))
|
(RobotParams.Preferences.useWebCam || RobotParams.Preferences.tuneColorBlobVision))
|
||||||
{
|
{
|
||||||
throw new IllegalArgumentException("Must provide a valid WebCam 1 name.");
|
throw new IllegalArgumentException("Must provide a valid WebCam 1 name.");
|
||||||
@ -114,10 +114,10 @@ public class Vision
|
|||||||
|
|
||||||
this.tracer = new TrcDbgTrace();
|
this.tracer = new TrcDbgTrace();
|
||||||
this.robot = robot;
|
this.robot = robot;
|
||||||
this.webcam1 = robot.robotInfo.webCam1Name != null?
|
this.webcam1 = robot.robotInfo.webCam1.camName != null?
|
||||||
opMode.hardwareMap.get(WebcamName.class, robot.robotInfo.webCam1Name): null;
|
opMode.hardwareMap.get(WebcamName.class, robot.robotInfo.webCam1.camName): null;
|
||||||
this.webcam2 = robot.robotInfo.webCam2Name != null?
|
this.webcam2 = robot.robotInfo.webCam2.camName != null?
|
||||||
opMode.hardwareMap.get(WebcamName.class, robot.robotInfo.webCam2Name): null;
|
opMode.hardwareMap.get(WebcamName.class, robot.robotInfo.webCam2.camName): null;
|
||||||
if (RobotParams.Preferences.tuneColorBlobVision)
|
if (RobotParams.Preferences.tuneColorBlobVision)
|
||||||
{
|
{
|
||||||
OpenCvCamera openCvCamera;
|
OpenCvCamera openCvCamera;
|
||||||
@ -140,8 +140,9 @@ public class Vision
|
|||||||
rawColorBlobPipeline.setVideoOutput(0);
|
rawColorBlobPipeline.setVideoOutput(0);
|
||||||
rawColorBlobPipeline.setAnnotateEnabled(true);
|
rawColorBlobPipeline.setAnnotateEnabled(true);
|
||||||
rawColorBlobVision = new FtcRawEocvVision(
|
rawColorBlobVision = new FtcRawEocvVision(
|
||||||
"rawColorBlobVision", robot.robotInfo.camImageWidth, robot.robotInfo.camImageHeight, null, null,
|
"rawColorBlobVision", robot.robotInfo.webCam1.camImageWidth, robot.robotInfo.webCam1.camImageHeight,
|
||||||
openCvCamera, robot.robotInfo.camOrientation);
|
null, null,
|
||||||
|
openCvCamera, robot.robotInfo.webCam1.camOrientation);
|
||||||
rawColorBlobVision.setFpsMeterEnabled(RobotParams.Preferences.showVisionStat);
|
rawColorBlobVision.setFpsMeterEnabled(RobotParams.Preferences.showVisionStat);
|
||||||
setRawColorBlobVisionEnabled(false);
|
setRawColorBlobVisionEnabled(false);
|
||||||
}
|
}
|
||||||
@ -172,13 +173,13 @@ public class Vision
|
|||||||
|
|
||||||
redBlobVision = new FtcVisionEocvColorBlob(
|
redBlobVision = new FtcVisionEocvColorBlob(
|
||||||
"RedBlob", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, true,
|
"RedBlob", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, true,
|
||||||
robot.robotInfo.cameraRect, robot.robotInfo.worldRect, true);
|
robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true);
|
||||||
redBlobProcessor = redBlobVision.getVisionProcessor();
|
redBlobProcessor = redBlobVision.getVisionProcessor();
|
||||||
visionProcessorsList.add(redBlobProcessor);
|
visionProcessorsList.add(redBlobProcessor);
|
||||||
|
|
||||||
blueBlobVision = new FtcVisionEocvColorBlob(
|
blueBlobVision = new FtcVisionEocvColorBlob(
|
||||||
"BlueBlob", colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams, true,
|
"BlueBlob", colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams, true,
|
||||||
robot.robotInfo.cameraRect, robot.robotInfo.worldRect, true);
|
robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true);
|
||||||
blueBlobProcessor = blueBlobVision.getVisionProcessor();
|
blueBlobProcessor = blueBlobVision.getVisionProcessor();
|
||||||
visionProcessorsList.add(blueBlobProcessor);
|
visionProcessorsList.add(blueBlobProcessor);
|
||||||
}
|
}
|
||||||
@ -187,8 +188,8 @@ public class Vision
|
|||||||
{
|
{
|
||||||
tracer.traceInfo(moduleName, "Starting TensorFlowVision...");
|
tracer.traceInfo(moduleName, "Starting TensorFlowVision...");
|
||||||
tensorFlowVision = new FtcVisionTensorFlow(
|
tensorFlowVision = new FtcVisionTensorFlow(
|
||||||
null, true, TFOD_MODEL_ASSET, TFOD_TARGET_LABELS, robot.robotInfo.cameraRect,
|
null, true, TFOD_MODEL_ASSET, TFOD_TARGET_LABELS, robot.robotInfo.webCam1.cameraRect,
|
||||||
robot.robotInfo.worldRect);
|
robot.robotInfo.webCam1.worldRect);
|
||||||
tensorFlowProcessor = tensorFlowVision.getVisionProcessor();
|
tensorFlowProcessor = tensorFlowVision.getVisionProcessor();
|
||||||
tensorFlowProcessor.setMinResultConfidence(TFOD_MIN_CONFIDENCE);
|
tensorFlowProcessor.setMinResultConfidence(TFOD_MIN_CONFIDENCE);
|
||||||
visionProcessorsList.add(tensorFlowProcessor);
|
visionProcessorsList.add(tensorFlowProcessor);
|
||||||
@ -200,7 +201,7 @@ public class Vision
|
|||||||
{
|
{
|
||||||
// Use USB webcams.
|
// Use USB webcams.
|
||||||
vision = new FtcVision(
|
vision = new FtcVision(
|
||||||
webcam1, webcam2, robot.robotInfo.camImageWidth, robot.robotInfo.camImageHeight,
|
webcam1, webcam2, robot.robotInfo.webCam1.camImageWidth, robot.robotInfo.webCam1.camImageHeight,
|
||||||
RobotParams.Preferences.showVisionView, RobotParams.Preferences.showVisionStat, visionProcessors);
|
RobotParams.Preferences.showVisionView, RobotParams.Preferences.showVisionStat, visionProcessors);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
@ -209,7 +210,7 @@ public class Vision
|
|||||||
vision = new FtcVision(
|
vision = new FtcVision(
|
||||||
RobotParams.Preferences.useBuiltinCamBack?
|
RobotParams.Preferences.useBuiltinCamBack?
|
||||||
BuiltinCameraDirection.BACK: BuiltinCameraDirection.FRONT,
|
BuiltinCameraDirection.BACK: BuiltinCameraDirection.FRONT,
|
||||||
robot.robotInfo.camImageWidth, robot.robotInfo.camImageHeight,
|
robot.robotInfo.webCam1.camImageWidth, robot.robotInfo.webCam1.camImageHeight,
|
||||||
RobotParams.Preferences.showVisionView, RobotParams.Preferences.showVisionStat, visionProcessors);
|
RobotParams.Preferences.showVisionView, RobotParams.Preferences.showVisionStat, visionProcessors);
|
||||||
}
|
}
|
||||||
// Disable all vision until they are needed.
|
// Disable all vision until they are needed.
|
||||||
@ -437,7 +438,7 @@ public class Vision
|
|||||||
TrcPose2D aprilTagPose =
|
TrcPose2D aprilTagPose =
|
||||||
RobotParams.Game.APRILTAG_POSES[aprilTagInfo.detectedObj.aprilTagDetection.id - 1];
|
RobotParams.Game.APRILTAG_POSES[aprilTagInfo.detectedObj.aprilTagDetection.id - 1];
|
||||||
TrcPose2D cameraPose = aprilTagPose.subtractRelativePose(aprilTagInfo.objPose);
|
TrcPose2D cameraPose = aprilTagPose.subtractRelativePose(aprilTagInfo.objPose);
|
||||||
robotPose = cameraPose.subtractRelativePose(robot.robotInfo.camPose);
|
robotPose = cameraPose.subtractRelativePose(robot.robotInfo.webCam1.camPose);
|
||||||
tracer.traceInfo(
|
tracer.traceInfo(
|
||||||
moduleName,
|
moduleName,
|
||||||
"AprilTagId=" + aprilTagInfo.detectedObj.aprilTagDetection.id +
|
"AprilTagId=" + aprilTagInfo.detectedObj.aprilTagDetection.id +
|
||||||
|
Reference in New Issue
Block a user