SCDS/FtcTemplate
7
0
Fork 0
mirror of https://github.com/trc492/FtcTemplate.git synced 2025-07-01 21:11:23 -07:00
Code Issues Packages Projects Releases Wiki Activity

Sync'd latest changes.

Browse Source 
Updated ftclib, trclib.
Reorganized params.
This commit is contained in:
Titan Robotics Club
2024-10-31 04:18:59 -07:00
parent 84f73042b6
commit ecfed76c1e
11 changed files with 636 additions and 587 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
TeamCode/src/main/java/ftclib

Submodule TeamCode/src/main/java/ftclib updated: 12962c5ae8...56d85d3fe7

2
TeamCode/src/main/java/teamcode/FtcAuto.java
View File

@ -184,8 +184,6 @@ public class FtcAuto extends FtcOpMode
// robot.vision.setBlueBlobVisionEnabled(true);
// }
}
robot.zeroCalibrate();
} //robotInit
//

42
TeamCode/src/main/java/teamcode/FtcTeleOp.java
View File

@ -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.

10
TeamCode/src/main/java/teamcode/FtcTest.java
View File

@ -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;
}

97
TeamCode/src/main/java/teamcode/Robot.java
View File

@ -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.
*

609
TeamCode/src/main/java/teamcode/RobotParams.java
View File

@ -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

9
TeamCode/src/main/java/teamcode/autotasks/TaskAuto.java
View File

@ -78,15 +78,6 @@ public class TaskAuto extends TrcAutoTask<TaskAuto.State>
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.
//

4
TeamCode/src/main/java/teamcode/subsystems/LEDIndicator.java
View File

@ -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);

299
TeamCode/src/main/java/teamcode/subsystems/RobotBase.java
View File

@ -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:

147
TeamCode/src/main/java/teamcode/vision/Vision.java → TeamCode/src/main/java/teamcode/subsystems/Vision.java
View File

@ -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<VisionProcessor> 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<FtcLimelightVision.DetectedObject> aprilTagInfo =
getLimelightDetectedObject(FtcLimelightVision.ResultType.Fiducial, null, -1);
if (aprilTagInfo != null)
{
robotPose = aprilTagInfo.detectedObj.robotPose;
}
}
else if (isAprilTagVisionEnabled())
{
// Find any AprilTag in view.
TrcVisionTargetInfo<FtcVisionAprilTag.DetectedObject> aprilTagInfo = getDetectedAprilTag(null, -1);
@ -676,20 +777,17 @@ public class Vision
ColorBlobType colorBlobType, int lineNum)
{
TrcVisionTargetInfo<TrcOpenCvColorBlobPipeline.DetectedObject> 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<TrcOpenCvColorBlobPipeline.DetectedObject> a,
TrcVisionTargetInfo<TrcOpenCvColorBlobPipeline.DetectedObject> b)
{
return (int)((b.objPose.y - a.objPose.y)*100);
return (int)((a.objPose.y - b.objPose.y)*100);
} //compareDistance
} //class Vision

2
TeamCode/src/main/java/trclib

Submodule TeamCode/src/main/java/trclib updated: d9953b914b...5a6a5b4e4c