From 4bb3a5ad61b3a9558299fa68c232d17330eaf4e8 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Sun, 24 Mar 2024 23:30:39 -0400 Subject: [PATCH 01/94] first commit --- .github/CONTRIBUTING.md | 68 + .github/PULL_REQUEST_TEMPLATE.md | 1 + .gitignore | 81 + FtcRobotController/build.gradle | 26 + .../src/main/AndroidManifest.xml | 79 + .../samples/BasicOmniOpMode_Linear.java | 167 ++ .../samples/BasicOpMode_Iterative.java | 140 ++ .../external/samples/BasicOpMode_Linear.java | 115 ++ .../external/samples/ConceptAprilTag.java | 217 +++ .../external/samples/ConceptAprilTagEasy.java | 163 ++ .../ConceptAprilTagOptimizeExposure.java | 246 +++ .../ConceptAprilTagSwitchableCameras.java | 196 +++ .../external/samples/ConceptDoubleVision.java | 202 +++ .../ConceptExploringIMUOrientation.java | 184 ++ .../samples/ConceptExternalHardwareClass.java | 142 ++ .../samples/ConceptGamepadRumble.java | 200 +++ .../samples/ConceptGamepadTouchpad.java | 77 + .../samples/ConceptMotorBulkRead.java | 227 +++ .../external/samples/ConceptNullOp.java | 89 + .../samples/ConceptRampMotorSpeed.java | 114 ++ .../external/samples/ConceptRevSPARKMini.java | 111 ++ .../external/samples/ConceptScanServo.java | 115 ++ .../external/samples/ConceptSoundsASJava.java | 133 ++ .../samples/ConceptSoundsOnBotJava.java | 120 ++ .../samples/ConceptSoundsSKYSTONE.java | 122 ++ .../external/samples/ConceptTelemetry.java | 177 ++ .../ConceptTensorFlowObjectDetection.java | 199 +++ .../ConceptTensorFlowObjectDetectionEasy.java | 142 ++ ...rFlowObjectDetectionSwitchableCameras.java | 186 +++ .../RobotAutoDriveByEncoder_Linear.java | 187 +++ .../samples/RobotAutoDriveByGyro_Linear.java | 429 +++++ .../samples/RobotAutoDriveByTime_Linear.java | 128 ++ .../samples/RobotAutoDriveToAprilTagOmni.java | 321 ++++ .../samples/RobotAutoDriveToAprilTagTank.java | 298 ++++ .../samples/RobotAutoDriveToLine_Linear.java | 142 ++ .../external/samples/RobotHardware.java | 167 ++ .../samples/RobotTeleopPOV_Linear.java | 159 ++ .../samples/RobotTeleopTank_Iterative.java | 160 ++ .../samples/SampleRevBlinkinLedDriver.java | 163 ++ .../external/samples/SensorBNO055IMU.java | 186 +++ .../samples/SensorBNO055IMUCalibration.java | 230 +++ .../external/samples/SensorColor.java | 221 +++ .../external/samples/SensorDigitalTouch.java | 78 + .../external/samples/SensorHuskyLens.java | 149 ++ .../samples/SensorIMUNonOrthogonal.java | 181 ++ .../external/samples/SensorIMUOrthogonal.java | 143 ++ .../external/samples/SensorKLNavxMicro.java | 128 ++ .../external/samples/SensorMRColor.java | 138 ++ .../external/samples/SensorMRGyro.java | 160 ++ .../samples/SensorMROpticalDistance.java | 70 + .../external/samples/SensorMRRangeSensor.java | 70 + .../external/samples/SensorREV2mDistance.java | 87 + .../external/samples/SensorTouch.java | 78 + .../samples/UtilityCameraFrameCapture.java | 127 ++ .../external/samples/readme.md | 45 + .../external/samples/sample_conventions.md | 108 ++ .../internal/FtcOpModeRegister.java | 70 + .../internal/FtcRobotControllerActivity.java | 845 ++++++++++ .../internal/PermissionValidatorWrapper.java | 91 + .../src/main/res/drawable-xhdpi/icon_menu.png | Bin 0 -> 975 bytes .../drawable-xhdpi/icon_robotcontroller.png | Bin 0 -> 4777 bytes .../res/layout/activity_ftc_controller.xml | 184 ++ .../main/res/menu/ftc_robot_controller.xml | 78 + FtcRobotController/src/main/res/raw/gold.wav | Bin 0 -> 104016 bytes .../src/main/res/raw/silver.wav | Bin 0 -> 99068 bytes .../src/main/res/values/dimens.xml | 40 + .../src/main/res/values/strings.xml | 72 + .../src/main/res/values/styles.xml | 23 + .../src/main/res/xml/app_settings.xml | 93 ++ .../src/main/res/xml/device_filter.xml | 49 + LICENSE | 29 + README.md | 1481 +++++++++++++++++ TeamCode/build.gradle | 33 + TeamCode/lib/OpModeAnnotationProcessor.jar | Bin 0 -> 6032 bytes TeamCode/src/main/AndroidManifest.xml | 11 + .../ftc/teamcode/pedroPathing/OVERVIEW.md | 117 ++ .../ftc/teamcode/pedroPathing/README.md | 29 + .../ftc/teamcode/pedroPathing/TUNING.md | 91 + .../examples/TeleOpEnhancements.java | 69 + .../follower/DriveVectorScaler.java | 152 ++ .../pedroPathing/follower/Follower.java | 879 ++++++++++ .../localization/PoseUpdater.java | 313 ++++ .../localization/RoadRunnerEncoder.java | 125 ++ .../localization/ThreeWheelLocalizer.java | 117 ++ .../pathGeneration/BezierCurve.java | 317 ++++ .../BezierCurveCoefficients.java | 66 + .../pathGeneration/BezierLine.java | 208 +++ .../pathGeneration/BezierPoint.java | 208 +++ .../pathGeneration/MathFunctions.java | 284 ++++ .../pedroPathing/pathGeneration/Path.java | 477 ++++++ .../pathGeneration/PathBuilder.java | 217 +++ .../pathGeneration/PathCallback.java | 78 + .../pathGeneration/PathChain.java | 94 ++ .../pedroPathing/pathGeneration/Point.java | 175 ++ .../pedroPathing/pathGeneration/Vector.java | 141 ++ .../teamcode/pedroPathing/tuning/Circle.java | 74 + .../tuning/CurvedBackAndForth.java | 85 + .../tuning/FollowerConstants.java | 181 ++ .../tuning/ForwardVelocityTuner.java | 147 ++ .../ForwardZeroPowerAccelerationTuner.java | 153 ++ .../LateralZeroPowerAccelerationTuner.java | 138 ++ .../tuning/StrafeVelocityTuner.java | 145 ++ .../tuning/StraightBackAndForth.java | 84 + .../util/CustomPIDFCoefficients.java | 67 + .../util/FeedForwardConstant.java | 21 + .../teamcode/pedroPathing/util/NanoTimer.java | 46 + .../pedroPathing/util/PIDFController.java | 223 +++ .../pedroPathing/util/SingleRunAction.java | 60 + .../ftc/teamcode/pedroPathing/util/Timer.java | 46 + .../org/firstinspires/ftc/teamcode/readme.md | 131 ++ TeamCode/src/main/res/raw/readme.md | 1 + TeamCode/src/main/res/values/strings.xml | 4 + .../main/res/xml/teamwebcamcalibrations.xml | 161 ++ build.common.gradle | 125 ++ build.dependencies.gradle | 25 + build.gradle | 29 + doc/legal/AudioBlocksSounds.txt | 21 + ...A - LEGO Open Source License Agreement.txt | 15 + doc/legal/LEGO Open Source License.pdf | Bin 0 -> 34100 bytes doc/media/PullRequest.PNG | Bin 0 -> 19655 bytes gradle.properties | 10 + gradle/wrapper/gradle-wrapper.jar | Bin 0 -> 58695 bytes gradle/wrapper/gradle-wrapper.properties | 5 + gradlew | 164 ++ gradlew.bat | 90 + libs/README.txt | 1 + libs/ftc.debug.keystore | Bin 0 -> 2146 bytes settings.gradle | 2 + 128 files changed, 17902 insertions(+) create mode 100644 .github/CONTRIBUTING.md create mode 100644 .github/PULL_REQUEST_TEMPLATE.md create mode 100644 .gitignore create mode 100644 FtcRobotController/build.gradle create mode 100644 FtcRobotController/src/main/AndroidManifest.xml create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Iterative.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Linear.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagOptimizeExposure.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptDoubleVision.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExploringIMUOrientation.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExternalHardwareClass.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadRumble.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadTouchpad.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRampMotorSpeed.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevSPARKMini.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptScanServo.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsASJava.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsOnBotJava.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsSKYSTONE.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTelemetry.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetection.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionEasy.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionSwitchableCameras.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByEncoder_Linear.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByGyro_Linear.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByTime_Linear.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToLine_Linear.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotHardware.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopPOV_Linear.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopTank_Iterative.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SampleRevBlinkinLedDriver.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMU.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMUCalibration.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorColor.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHuskyLens.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUNonOrthogonal.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUOrthogonal.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorKLNavxMicro.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRColor.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRGyro.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMROpticalDistance.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRRangeSensor.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREV2mDistance.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorTouch.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityCameraFrameCapture.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/readme.md create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/sample_conventions.md create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcOpModeRegister.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/PermissionValidatorWrapper.java create mode 100644 FtcRobotController/src/main/res/drawable-xhdpi/icon_menu.png create mode 100644 FtcRobotController/src/main/res/drawable-xhdpi/icon_robotcontroller.png create mode 100644 FtcRobotController/src/main/res/layout/activity_ftc_controller.xml create mode 100644 FtcRobotController/src/main/res/menu/ftc_robot_controller.xml create mode 100644 FtcRobotController/src/main/res/raw/gold.wav create mode 100644 FtcRobotController/src/main/res/raw/silver.wav create mode 100644 FtcRobotController/src/main/res/values/dimens.xml create mode 100644 FtcRobotController/src/main/res/values/strings.xml create mode 100644 FtcRobotController/src/main/res/values/styles.xml create mode 100644 FtcRobotController/src/main/res/xml/app_settings.xml create mode 100644 FtcRobotController/src/main/res/xml/device_filter.xml create mode 100644 LICENSE create mode 100644 README.md create mode 100644 TeamCode/build.gradle create mode 100644 TeamCode/lib/OpModeAnnotationProcessor.jar create mode 100644 TeamCode/src/main/AndroidManifest.xml create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurveCoefficients.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierLine.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierPoint.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathCallback.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathChain.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Vector.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/Circle.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/CurvedBackAndForth.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StraightBackAndForth.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomPIDFCoefficients.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FeedForwardConstant.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/NanoTimer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/PIDFController.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/SingleRunAction.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Timer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md create mode 100644 TeamCode/src/main/res/raw/readme.md create mode 100644 TeamCode/src/main/res/values/strings.xml create mode 100644 TeamCode/src/main/res/xml/teamwebcamcalibrations.xml create mode 100644 build.common.gradle create mode 100644 build.dependencies.gradle create mode 100644 build.gradle create mode 100644 doc/legal/AudioBlocksSounds.txt create mode 100644 doc/legal/Exhibit A - LEGO Open Source License Agreement.txt create mode 100644 doc/legal/LEGO Open Source License.pdf create mode 100644 doc/media/PullRequest.PNG create mode 100644 gradle.properties create mode 100644 gradle/wrapper/gradle-wrapper.jar create mode 100644 gradle/wrapper/gradle-wrapper.properties create mode 100644 gradlew create mode 100644 gradlew.bat create mode 100644 libs/README.txt create mode 100644 libs/ftc.debug.keystore create mode 100644 settings.gradle diff --git a/.github/CONTRIBUTING.md b/.github/CONTRIBUTING.md new file mode 100644 index 0000000..4d37ce7 --- /dev/null +++ b/.github/CONTRIBUTING.md @@ -0,0 +1,68 @@ +# Contributing to the FTC SDK + +The following is a set of guidelines for contributing the FIRST FTC SDK. The FTC Technology Team welcomes suggestions for improvements to core software, ideas for new features, requests for built-in support of new sensors, and well written bug reports. + +## How can I contribute? + +### Pull requests + +__STOP!__ If you are new to git, do not understand the mechanics of forks, branches, and pulls, if what you just read is confusing, __do not__ push this button. Most likely it won't do what you think it will. + +![Pull Button](../doc/media/PullRequest.PNG) + +If you are looking at this button then you've pushed some changes to your team's fork of ftctechnh/ftc_app. Congratulations! You are almost certainly finished. + +The vast majority of pull requests seen on the ftctechnh/ftc_app repository are not intended to be merged into the official SDK. Team software is just that, your team's. It's specific to the tasks you are trying to accomplish, the testing you are doing, and goals your team has. You don't want that pushed into the official SDK. + +If what you've read so far makes little sense, there are some very good git learning resources online. +[Git Book](https://git-scm.com/book/en/v2) +[Interactive Git Tutorial](https://try.github.io) + +### Guidlines for experienced GIT users. + +If you are absolutely certain that you want to push the big green button above, read on. Otherwise back _slowly away from keyboard_. + +The real intent for advanced users is often to issue a pull request from the [branch](https://www.atlassian.com/git/tutorials/using-branches/git-branch) on a local fork back to master on either the same local fork or a child of the team fork and not on the parent ftctechnh/ftc_app. See [Creating a Pull Request](https://help.github.com/articles/creating-a-pull-request-from-a-fork/). + +If that is indeed the intent, then you can merge your [topic branch](https://git-scm.com/book/en/v2/Git-Branching-Branching-Workflows#Topic-Branches) into master locally by hand before pushing it up to github, or if you want a pull request for pulls between branches on the same repository because, say, you want team members to look at your software before merging into master, you can select the base fork from the dropdown on the "Open a pull request" page and select your team repo instead of ftctechnh's. + +Alternatively, if you have a team repository forked from ftctechnh/ftc_app, and then team members individually fork from your team repository, then pull requests from the individual team member's forks will have the main team repository automatically selected as the base fork for the pull. And you won't inadvertently request to pull your team software into ftctechnh's repository. + +The latter would be the "best" way to manage software among a large team. But as with all things git there are many options. + +Pull requests that do not fall into the category above are evaluated by the FTC Technology Team on a case-by-case basis. Please note however that the deployment model of the SDK does not support direct pulls into ftctechnh/ftc_app. + +### Report bugs + +This section guides you through filing a bug report. The better the report the more likely it is to be root caused and fixed. Please refrain from feature requests or software enhancements when opening new issues. See Suggesting Enhancements below. + +#### Before submitting a bug report + +- Check the [forums](http://ftcforum.firstinspires.org/forum.php) to see if someone else has run into the problem and whether there is an official solution that doesn't require a new SDK. + +- Perform a search of current [issues](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues) to see if the problem has already been reported. If so, add a comment to the existing issue instead of creating a new one. + +#### How Do I Submit A (Good) Bug Report? + +Bugs are tracked as GitHub issues. Create an issue on ftctechnh/ftc_app and provide the following information. +Explain the problem and include additional details to help maintainers reproduce the problem: + +- Use a clear and descriptive title for the issue to identify the problem. + +- Describe the exact steps which reproduce the problem in as many details as possible. + +- Provide specific examples to demonstrate the steps. + +- Describe the behavior you observed after following the steps and point out what exactly is the problem with that behavior. Explain which behavior you expected to see instead and why. If applicable, include screenshots which show you following the described steps and clearly demonstrate the problem. + +- If you're reporting that the RobotController crashed, include the logfile with a stack trace of the crash. [Example of good bug report with stack trace](https://github.com/ftctechnh/ftc_app/issues/224) + +- If the problem wasn't triggered by a specific action, describe what you were doing before the problem happened and share more information using the guidelines below. + +### Suggesting Enhancements + +FIRST volunteers are awesome. You all have great ideas and we want to hear them. + +Enhancements should be broadly applicable to a large majority of teams, should not force teams to change their workflow, and should provide real value to the mission of FIRST as it relates to engaging youth in engineering activities. + +The best way to get momentum behind new features is to post a description of your idea in the discussions section of this repository. Build community support for it. The FTC Technology Team monitors the discussions. We'll hear you and if there's a large enough call for the feature it's very likely to get put on the list for a future release. diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md new file mode 100644 index 0000000..665369b --- /dev/null +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -0,0 +1 @@ +Before issuing a pull request, please see the contributing page. diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..b85aa2f --- /dev/null +++ b/.gitignore @@ -0,0 +1,81 @@ +# Built application files +*.apk +*.aar +*.ap_ +*.aab + +!libs/*.aar + +# Files for the ART/Dalvik VM +*.dex + +# Java/JDK files +*.class +*.hprof + +# Generated files +bin/ +gen/ +out/ +# Uncomment the following line in case you need and you don't have the release build type files in your app +# release/ + +# Gradle files +.gradle/ +build/ + +# Local configuration file (sdk path, etc) +local.properties + +# Proguard folder generated by Eclipse +proguard/ + +# Log Files +*.log + +# Android Studio Navigation editor temp files +.navigation/ + +# Android Studio captures folder +captures/ + +# IntelliJ +*.iml +.idea/ + +# For Mac users +.DS_Store + +# Keystore files +# Uncomment the following lines if you do not want to check your keystore files in. +#*.jks +#*.keystore + +# External native build folder generated in Android Studio 2.2 and later +.externalNativeBuild +.cxx/ + +# Google Services (e.g. APIs or Firebase) +# google-services.json + +# Freeline +freeline.py +freeline/ +freeline_project_description.json + +# fastlane +fastlane/report.xml +fastlane/Preview.html +fastlane/screenshots +fastlane/test_output +fastlane/readme.md + +# Version control +vcs.xml + +# lint +lint/intermediates/ +lint/generated/ +lint/outputs/ +lint/tmp/ +# lint/reports/ \ No newline at end of file diff --git a/FtcRobotController/build.gradle b/FtcRobotController/build.gradle new file mode 100644 index 0000000..e25651f --- /dev/null +++ b/FtcRobotController/build.gradle @@ -0,0 +1,26 @@ +import java.text.SimpleDateFormat + +// +// build.gradle in FtcRobotController +// +apply plugin: 'com.android.library' + +android { + + defaultConfig { + minSdkVersion 24 + //noinspection ExpiredTargetSdkVersion + targetSdkVersion 28 + buildConfigField "String", "APP_BUILD_TIME", '"' + (new SimpleDateFormat("yyyy-MM-dd'T'HH:mm:ss.SSSZ", Locale.ROOT).format(new Date())) + '"' + } + + compileSdkVersion 29 + + compileOptions { + sourceCompatibility JavaVersion.VERSION_1_8 + targetCompatibility JavaVersion.VERSION_1_8 + } + namespace = 'com.qualcomm.ftcrobotcontroller' +} + +apply from: '../build.dependencies.gradle' diff --git a/FtcRobotController/src/main/AndroidManifest.xml b/FtcRobotController/src/main/AndroidManifest.xml new file mode 100644 index 0000000..24ef7b2 --- /dev/null +++ b/FtcRobotController/src/main/AndroidManifest.xml @@ -0,0 +1,79 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java new file mode 100644 index 0000000..940b527 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java @@ -0,0 +1,167 @@ +/* Copyright (c) 2021 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="Basic: Omni Linear OpMode", group="Linear OpMode") +@Disabled +public class BasicOmniOpMode_Linear extends LinearOpMode { + + // Declare OpMode members for each of the 4 motors. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor leftFrontDrive = null; + private DcMotor leftBackDrive = null; + private DcMotor rightFrontDrive = null; + private DcMotor rightBackDrive = null; + + @Override + public void runOpMode() { + + // Initialize the hardware variables. Note that the strings used here must correspond + // to the names assigned during the robot configuration step on the DS or RC devices. + leftFrontDrive = hardwareMap.get(DcMotor.class, "left_front_drive"); + leftBackDrive = hardwareMap.get(DcMotor.class, "left_back_drive"); + rightFrontDrive = hardwareMap.get(DcMotor.class, "right_front_drive"); + rightBackDrive = hardwareMap.get(DcMotor.class, "right_back_drive"); + + // ######################################################################################## + // !!! IMPORTANT Drive Information. Test your motor directions. !!!!! + // ######################################################################################## + // Most robots need the motors on one side to be reversed to drive forward. + // The motor reversals shown here are for a "direct drive" robot (the wheels turn the same direction as the motor shaft) + // If your robot has additional gear reductions or uses a right-angled drive, it's important to ensure + // that your motors are turning in the correct direction. So, start out with the reversals here, BUT + // when you first test your robot, push the left joystick forward and observe the direction the wheels turn. + // Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward + // Keep testing until ALL the wheels move the robot forward when you push the left joystick forward. + leftFrontDrive.setDirection(DcMotor.Direction.REVERSE); + leftBackDrive.setDirection(DcMotor.Direction.REVERSE); + rightFrontDrive.setDirection(DcMotor.Direction.FORWARD); + rightBackDrive.setDirection(DcMotor.Direction.FORWARD); + + // Wait for the game to start (driver presses PLAY) + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + waitForStart(); + runtime.reset(); + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + double max; + + // POV Mode uses left joystick to go forward & strafe, and right joystick to rotate. + double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value + double lateral = gamepad1.left_stick_x; + double yaw = gamepad1.right_stick_x; + + // Combine the joystick requests for each axis-motion to determine each wheel's power. + // Set up a variable for each drive wheel to save the power level for telemetry. + double leftFrontPower = axial + lateral + yaw; + double rightFrontPower = axial - lateral - yaw; + double leftBackPower = axial - lateral + yaw; + double rightBackPower = axial + lateral - yaw; + + // Normalize the values so no wheel power exceeds 100% + // This ensures that the robot maintains the desired motion. + max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)); + max = Math.max(max, Math.abs(leftBackPower)); + max = Math.max(max, Math.abs(rightBackPower)); + + if (max > 1.0) { + leftFrontPower /= max; + rightFrontPower /= max; + leftBackPower /= max; + rightBackPower /= max; + } + + // This is test code: + // + // Uncomment the following code to test your motor directions. + // Each button should make the corresponding motor run FORWARD. + // 1) First get all the motors to take to correct positions on the robot + // by adjusting your Robot Configuration if necessary. + // 2) Then make sure they run in the correct direction by modifying the + // the setDirection() calls above. + // Once the correct motors move in the correct direction re-comment this code. + + /* + leftFrontPower = gamepad1.x ? 1.0 : 0.0; // X gamepad + leftBackPower = gamepad1.a ? 1.0 : 0.0; // A gamepad + rightFrontPower = gamepad1.y ? 1.0 : 0.0; // Y gamepad + rightBackPower = gamepad1.b ? 1.0 : 0.0; // B gamepad + */ + + // Send calculated power to wheels + leftFrontDrive.setPower(leftFrontPower); + rightFrontDrive.setPower(rightFrontPower); + leftBackDrive.setPower(leftBackPower); + rightBackDrive.setPower(rightBackPower); + + // Show the elapsed game time and wheel power. + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); + telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); + telemetry.update(); + } + }} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Iterative.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Iterative.java new file mode 100644 index 0000000..7c10408 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Iterative.java @@ -0,0 +1,140 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + +/* + * This file contains an example of an iterative (Non-Linear) "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode + * class is instantiated on the Robot Controller and executed. + * + * This particular OpMode just executes a basic Tank Drive Teleop for a two wheeled robot + * It includes all the skeletal structure that all iterative OpModes contain. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="Basic: Iterative OpMode", group="Iterative OpMode") +@Disabled +public class BasicOpMode_Iterative extends OpMode +{ + // Declare OpMode members. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor leftDrive = null; + private DcMotor rightDrive = null; + + /* + * Code to run ONCE when the driver hits INIT + */ + @Override + public void init() { + telemetry.addData("Status", "Initialized"); + + // Initialize the hardware variables. Note that the strings used here as parameters + // to 'get' must correspond to the names assigned during the robot configuration + // step (using the FTC Robot Controller app on the phone). + leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); + rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); + + // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. + // Pushing the left stick forward MUST make robot go forward. So adjust these two lines based on your first test drive. + // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + + // Tell the driver that initialization is complete. + telemetry.addData("Status", "Initialized"); + } + + /* + * Code to run REPEATEDLY after the driver hits INIT, but before they hit PLAY + */ + @Override + public void init_loop() { + } + + /* + * Code to run ONCE when the driver hits PLAY + */ + @Override + public void start() { + runtime.reset(); + } + + /* + * Code to run REPEATEDLY after the driver hits PLAY but before they hit STOP + */ + @Override + public void loop() { + // Setup a variable for each drive wheel to save power level for telemetry + double leftPower; + double rightPower; + + // Choose to drive using either Tank Mode, or POV Mode + // Comment out the method that's not used. The default below is POV. + + // POV Mode uses left stick to go forward, and right stick to turn. + // - This uses basic math to combine motions and is easier to drive straight. + double drive = -gamepad1.left_stick_y; + double turn = gamepad1.right_stick_x; + leftPower = Range.clip(drive + turn, -1.0, 1.0) ; + rightPower = Range.clip(drive - turn, -1.0, 1.0) ; + + // Tank Mode uses one stick to control each wheel. + // - This requires no math, but it is hard to drive forward slowly and keep straight. + // leftPower = -gamepad1.left_stick_y ; + // rightPower = -gamepad1.right_stick_y ; + + // Send calculated power to wheels + leftDrive.setPower(leftPower); + rightDrive.setPower(rightPower); + + // Show the elapsed game time and wheel power. + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower); + } + + /* + * Code to run ONCE after the driver hits STOP + */ + @Override + public void stop() { + } + +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Linear.java new file mode 100644 index 0000000..9fe0bb6 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Linear.java @@ -0,0 +1,115 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + + +/* + * This file contains an minimal example of a Linear "OpMode". An OpMode is a 'program' that runs in either + * the autonomous or the teleop period of an FTC match. The names of OpModes appear on the menu + * of the FTC Driver Station. When a selection is made from the menu, the corresponding OpMode + * class is instantiated on the Robot Controller and executed. + * + * This particular OpMode just executes a basic Tank Drive Teleop for a two wheeled robot + * It includes all the skeletal structure that all linear OpModes contain. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="Basic: Linear OpMode", group="Linear OpMode") +@Disabled +public class BasicOpMode_Linear extends LinearOpMode { + + // Declare OpMode members. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor leftDrive = null; + private DcMotor rightDrive = null; + + @Override + public void runOpMode() { + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + // Initialize the hardware variables. Note that the strings used here as parameters + // to 'get' must correspond to the names assigned during the robot configuration + // step (using the FTC Robot Controller app on the phone). + leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); + rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); + + // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. + // Pushing the left stick forward MUST make robot go forward. So adjust these two lines based on your first test drive. + // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + + // Wait for the game to start (driver presses PLAY) + waitForStart(); + runtime.reset(); + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + // Setup a variable for each drive wheel to save power level for telemetry + double leftPower; + double rightPower; + + // Choose to drive using either Tank Mode, or POV Mode + // Comment out the method that's not used. The default below is POV. + + // POV Mode uses left stick to go forward, and right stick to turn. + // - This uses basic math to combine motions and is easier to drive straight. + double drive = -gamepad1.left_stick_y; + double turn = gamepad1.right_stick_x; + leftPower = Range.clip(drive + turn, -1.0, 1.0) ; + rightPower = Range.clip(drive - turn, -1.0, 1.0) ; + + // Tank Mode uses one stick to control each wheel. + // - This requires no math, but it is hard to drive forward slowly and keep straight. + // leftPower = -gamepad1.left_stick_y ; + // rightPower = -gamepad1.right_stick_y ; + + // Send calculated power to wheels + leftDrive.setPower(leftPower); + rightDrive.setPower(rightPower); + + // Show the elapsed game time and wheel power. + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower); + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java new file mode 100644 index 0000000..b6aa4e5 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java @@ -0,0 +1,217 @@ +/* Copyright (c) 2023 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import android.util.Size; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; + +/* + * This OpMode illustrates the basics of AprilTag recognition and pose estimation, + * including Java Builder structures for specifying Vision parameters. + * + * For an introduction to AprilTags, see the FTC-DOCS link below: + * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html + * + * In this sample, any visible tag ID will be detected and displayed, but only tags that are included in the default + * "TagLibrary" will have their position and orientation information displayed. This default TagLibrary contains + * the current Season's AprilTags and a small set of "test Tags" in the high number range. + * + * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera. + * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below. + * https://ftc-docs.firstinspires.org/apriltag-detection-values + * + * To experiment with using AprilTags to navigate, try out these two driving samples: + * RobotAutoDriveToAprilTagOmni and RobotAutoDriveToAprilTagTank + * + * There are many "default" VisionPortal and AprilTag configuration parameters that may be overridden if desired. + * These default parameters are shown as comments in the code below. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Concept: AprilTag", group = "Concept") +@Disabled +public class ConceptAprilTag extends LinearOpMode { + + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + /** + * The variable to store our instance of the AprilTag processor. + */ + private AprilTagProcessor aprilTag; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + @Override + public void runOpMode() { + + initAprilTag(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.update(); + waitForStart(); + + if (opModeIsActive()) { + while (opModeIsActive()) { + + telemetryAprilTag(); + + // Push telemetry to the Driver Station. + telemetry.update(); + + // Save CPU resources; can resume streaming when needed. + if (gamepad1.dpad_down) { + visionPortal.stopStreaming(); + } else if (gamepad1.dpad_up) { + visionPortal.resumeStreaming(); + } + + // Share the CPU. + sleep(20); + } + } + + // Save more CPU resources when camera is no longer needed. + visionPortal.close(); + + } // end method runOpMode() + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + + // Create the AprilTag processor. + aprilTag = new AprilTagProcessor.Builder() + + // The following default settings are available to un-comment and edit as needed. + //.setDrawAxes(false) + //.setDrawCubeProjection(false) + //.setDrawTagOutline(true) + //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) + //.setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary()) + //.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) + + // == CAMERA CALIBRATION == + // If you do not manually specify calibration parameters, the SDK will attempt + // to load a predefined calibration for your camera. + //.setLensIntrinsics(578.272, 578.272, 402.145, 221.506) + // ... these parameters are fx, fy, cx, cy. + + .build(); + + // Adjust Image Decimation to trade-off detection-range for detection-rate. + // eg: Some typical detection data using a Logitech C920 WebCam + // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second + // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second + // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second (default) + // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second (default) + // Note: Decimation can be changed on-the-fly to adapt during a match. + //aprilTag.setDecimation(3); + + // Create the vision portal by using a builder. + VisionPortal.Builder builder = new VisionPortal.Builder(); + + // Set the camera (webcam vs. built-in RC phone camera). + if (USE_WEBCAM) { + builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")); + } else { + builder.setCamera(BuiltinCameraDirection.BACK); + } + + // Choose a camera resolution. Not all cameras support all resolutions. + //builder.setCameraResolution(new Size(640, 480)); + + // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. + //builder.enableLiveView(true); + + // Set the stream format; MJPEG uses less bandwidth than default YUY2. + //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); + + // Choose whether or not LiveView stops if no processors are enabled. + // If set "true", monitor shows solid orange screen if no processors enabled. + // If set "false", monitor shows camera view without annotations. + //builder.setAutoStopLiveView(false); + + // Set and enable the processor. + builder.addProcessor(aprilTag); + + // Build the Vision Portal, using the above settings. + visionPortal = builder.build(); + + // Disable or re-enable the aprilTag processor at any time. + //visionPortal.setProcessorEnabled(aprilTag, true); + + } // end method initAprilTag() + + + /** + * Add telemetry about AprilTag detections. + */ + private void telemetryAprilTag() { + + List currentDetections = aprilTag.getDetections(); + telemetry.addData("# AprilTags Detected", currentDetections.size()); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + } // end for() loop + + // Add "key" information to telemetry + telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); + telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); + telemetry.addLine("RBE = Range, Bearing & Elevation"); + + } // end method telemetryAprilTag() + +} // end class diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java new file mode 100644 index 0000000..d305d55 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java @@ -0,0 +1,163 @@ +/* Copyright (c) 2023 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; + +/* + * This OpMode illustrates the basics of AprilTag recognition and pose estimation, using + * the easy way. + * + * For an introduction to AprilTags, see the FTC-DOCS link below: + * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html + * + * In this sample, any visible tag ID will be detected and displayed, but only tags that are included in the default + * "TagLibrary" will have their position and orientation information displayed. This default TagLibrary contains + * the current Season's AprilTags and a small set of "test Tags" in the high number range. + * + * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera. + * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below. + * https://ftc-docs.firstinspires.org/apriltag-detection-values + * + * To experiment with using AprilTags to navigate, try out these two driving samples: + * RobotAutoDriveToAprilTagOmni and RobotAutoDriveToAprilTagTank + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Concept: AprilTag Easy", group = "Concept") +@Disabled +public class ConceptAprilTagEasy extends LinearOpMode { + + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + /** + * The variable to store our instance of the AprilTag processor. + */ + private AprilTagProcessor aprilTag; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + @Override + public void runOpMode() { + + initAprilTag(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.update(); + waitForStart(); + + if (opModeIsActive()) { + while (opModeIsActive()) { + + telemetryAprilTag(); + + // Push telemetry to the Driver Station. + telemetry.update(); + + // Save CPU resources; can resume streaming when needed. + if (gamepad1.dpad_down) { + visionPortal.stopStreaming(); + } else if (gamepad1.dpad_up) { + visionPortal.resumeStreaming(); + } + + // Share the CPU. + sleep(20); + } + } + + // Save more CPU resources when camera is no longer needed. + visionPortal.close(); + + } // end method runOpMode() + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + + // Create the AprilTag processor the easy way. + aprilTag = AprilTagProcessor.easyCreateWithDefaults(); + + // Create the vision portal the easy way. + if (USE_WEBCAM) { + visionPortal = VisionPortal.easyCreateWithDefaults( + hardwareMap.get(WebcamName.class, "Webcam 1"), aprilTag); + } else { + visionPortal = VisionPortal.easyCreateWithDefaults( + BuiltinCameraDirection.BACK, aprilTag); + } + + } // end method initAprilTag() + + /** + * Add telemetry about AprilTag detections. + */ + private void telemetryAprilTag() { + + List currentDetections = aprilTag.getDetections(); + telemetry.addData("# AprilTags Detected", currentDetections.size()); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + } // end for() loop + + // Add "key" information to telemetry + telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); + telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); + telemetry.addLine("RBE = Range, Bearing & Elevation"); + + } // end method telemetryAprilTag() + +} // end class diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagOptimizeExposure.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagOptimizeExposure.java new file mode 100644 index 0000000..3792992 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagOptimizeExposure.java @@ -0,0 +1,246 @@ +/* Copyright (c) 2023 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.Range; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.ExposureControl; +import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.GainControl; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; +import java.util.concurrent.TimeUnit; + +/* + * This OpMode determines the best Exposure for minimizing image motion-blur on a Webcam + * Note that it is not possible to control the exposure for a Phone Camera, so if you are using a Phone for the Robot Controller + * this OpMode/Feature only applies to an externally connected Webcam + * + * The goal is to determine the smallest (shortest) Exposure value that still provides reliable Tag Detection. + * Starting with the minimum Exposure and maximum Gain, the exposure is slowly increased until the Tag is + * detected reliably from the likely operational distance. + * + * + * The best way to run this optimization is to view the camera preview screen while changing the exposure and gains. + * + * To do this, you need to view the RobotController screen directly (not from Driver Station) + * This can be done directly from a RC phone screen (if you are using an external Webcam), but for a Control Hub you must either plug an + * HDMI monitor into the Control Hub HDMI port, or use an external viewer program like ScrCpy (https://scrcpy.org/) + * + * Use Android Studio to Copy this Class, and Paste it into the TeamCode/src/main/java/org/firstinspires/ftc/teamcode folder. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ + +@TeleOp(name="Optimize AprilTag Exposure", group = "Concept") +@Disabled +public class ConceptAprilTagOptimizeExposure extends LinearOpMode +{ + private VisionPortal visionPortal = null; // Used to manage the video source. + private AprilTagProcessor aprilTag; // Used for managing the AprilTag detection process. + private int myExposure ; + private int minExposure ; + private int maxExposure ; + private int myGain ; + private int minGain ; + private int maxGain ; + + boolean thisExpUp = false; + boolean thisExpDn = false; + boolean thisGainUp = false; + boolean thisGainDn = false; + + boolean lastExpUp = false; + boolean lastExpDn = false; + boolean lastGainUp = false; + boolean lastGainDn = false; + @Override public void runOpMode() + { + // Initialize the Apriltag Detection process + initAprilTag(); + + // Establish Min and Max Gains and Exposure. Then set a low exposure with high gain + getCameraSetting(); + myExposure = Math.min(5, minExposure); + myGain = maxGain; + setManualExposure(myExposure, myGain); + + // Wait for the match to begin. + telemetry.addData("Camera preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.update(); + waitForStart(); + + while (opModeIsActive()) + { + telemetry.addLine("Find lowest Exposure that gives reliable detection."); + telemetry.addLine("Use Left bump/trig to adjust Exposure."); + telemetry.addLine("Use Right bump/trig to adjust Gain.\n"); + + // Display how many Tags Detected + List currentDetections = aprilTag.getDetections(); + int numTags = currentDetections.size(); + if (numTags > 0 ) + telemetry.addData("Tag", "####### %d Detected ######", currentDetections.size()); + else + telemetry.addData("Tag", "----------- none - ----------"); + + telemetry.addData("Exposure","%d (%d - %d)", myExposure, minExposure, maxExposure); + telemetry.addData("Gain","%d (%d - %d)", myGain, minGain, maxGain); + telemetry.update(); + + // check to see if we need to change exposure or gain. + thisExpUp = gamepad1.left_bumper; + thisExpDn = gamepad1.left_trigger > 0.25; + thisGainUp = gamepad1.right_bumper; + thisGainDn = gamepad1.right_trigger > 0.25; + + // look for clicks to change exposure + if (thisExpUp && !lastExpUp) { + myExposure = Range.clip(myExposure + 1, minExposure, maxExposure); + setManualExposure(myExposure, myGain); + } else if (thisExpDn && !lastExpDn) { + myExposure = Range.clip(myExposure - 1, minExposure, maxExposure); + setManualExposure(myExposure, myGain); + } + + // look for clicks to change the gain + if (thisGainUp && !lastGainUp) { + myGain = Range.clip(myGain + 1, minGain, maxGain ); + setManualExposure(myExposure, myGain); + } else if (thisGainDn && !lastGainDn) { + myGain = Range.clip(myGain - 1, minGain, maxGain ); + setManualExposure(myExposure, myGain); + } + + lastExpUp = thisExpUp; + lastExpDn = thisExpDn; + lastGainUp = thisGainUp; + lastGainDn = thisGainDn; + + sleep(20); + } + } + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + // Create the AprilTag processor by using a builder. + aprilTag = new AprilTagProcessor.Builder().build(); + + // Create the WEBCAM vision portal by using a builder. + visionPortal = new VisionPortal.Builder() + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .addProcessor(aprilTag) + .build(); + } + + /* + Manually set the camera gain and exposure. + Can only be called AFTER calling initAprilTag(); + Returns true if controls are set. + */ + private boolean setManualExposure(int exposureMS, int gain) { + // Ensure Vision Portal has been setup. + if (visionPortal == null) { + return false; + } + + // Wait for the camera to be open + if (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING) { + telemetry.addData("Camera", "Waiting"); + telemetry.update(); + while (!isStopRequested() && (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING)) { + sleep(20); + } + telemetry.addData("Camera", "Ready"); + telemetry.update(); + } + + // Set camera controls unless we are stopping. + if (!isStopRequested()) + { + // Set exposure. Make sure we are in Manual Mode for these values to take effect. + ExposureControl exposureControl = visionPortal.getCameraControl(ExposureControl.class); + if (exposureControl.getMode() != ExposureControl.Mode.Manual) { + exposureControl.setMode(ExposureControl.Mode.Manual); + sleep(50); + } + exposureControl.setExposure((long)exposureMS, TimeUnit.MILLISECONDS); + sleep(20); + + // Set Gain. + GainControl gainControl = visionPortal.getCameraControl(GainControl.class); + gainControl.setGain(gain); + sleep(20); + return (true); + } else { + return (false); + } + } + + /* + Read this camera's minimum and maximum Exposure and Gain settings. + Can only be called AFTER calling initAprilTag(); + */ + private void getCameraSetting() { + // Ensure Vision Portal has been setup. + if (visionPortal == null) { + return; + } + + // Wait for the camera to be open + if (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING) { + telemetry.addData("Camera", "Waiting"); + telemetry.update(); + while (!isStopRequested() && (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING)) { + sleep(20); + } + telemetry.addData("Camera", "Ready"); + telemetry.update(); + } + + // Get camera control values unless we are stopping. + if (!isStopRequested()) { + ExposureControl exposureControl = visionPortal.getCameraControl(ExposureControl.class); + minExposure = (int)exposureControl.getMinExposure(TimeUnit.MILLISECONDS) + 1; + maxExposure = (int)exposureControl.getMaxExposure(TimeUnit.MILLISECONDS); + + GainControl gainControl = visionPortal.getCameraControl(GainControl.class); + minGain = gainControl.getMinGain(); + maxGain = gainControl.getMaxGain(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java new file mode 100644 index 0000000..9285224 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java @@ -0,0 +1,196 @@ +/* Copyright (c) 2023 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.ClassFactory; +import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraName; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.VisionPortal.CameraState; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; + +/* + * This OpMode illustrates the basics of AprilTag recognition and pose estimation, using + * two webcams. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Concept: AprilTag Switchable Cameras", group = "Concept") +@Disabled +public class ConceptAprilTagSwitchableCameras extends LinearOpMode { + + /* + * Variables used for switching cameras. + */ + private WebcamName webcam1, webcam2; + private boolean oldLeftBumper; + private boolean oldRightBumper; + + /** + * The variable to store our instance of the AprilTag processor. + */ + private AprilTagProcessor aprilTag; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + @Override + public void runOpMode() { + + initAprilTag(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.update(); + waitForStart(); + + if (opModeIsActive()) { + while (opModeIsActive()) { + + telemetryCameraSwitching(); + telemetryAprilTag(); + + // Push telemetry to the Driver Station. + telemetry.update(); + + // Save CPU resources; can resume streaming when needed. + if (gamepad1.dpad_down) { + visionPortal.stopStreaming(); + } else if (gamepad1.dpad_up) { + visionPortal.resumeStreaming(); + } + + doCameraSwitching(); + + // Share the CPU. + sleep(20); + } + } + + // Save more CPU resources when camera is no longer needed. + visionPortal.close(); + + } // end runOpMode() + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + + // Create the AprilTag processor by using a builder. + aprilTag = new AprilTagProcessor.Builder().build(); + + webcam1 = hardwareMap.get(WebcamName.class, "Webcam 1"); + webcam2 = hardwareMap.get(WebcamName.class, "Webcam 2"); + CameraName switchableCamera = ClassFactory.getInstance() + .getCameraManager().nameForSwitchableCamera(webcam1, webcam2); + + // Create the vision portal by using a builder. + visionPortal = new VisionPortal.Builder() + .setCamera(switchableCamera) + .addProcessor(aprilTag) + .build(); + + } // end method initAprilTag() + + /** + * Add telemetry about camera switching. + */ + private void telemetryCameraSwitching() { + + if (visionPortal.getActiveCamera().equals(webcam1)) { + telemetry.addData("activeCamera", "Webcam 1"); + telemetry.addData("Press RightBumper", "to switch to Webcam 2"); + } else { + telemetry.addData("activeCamera", "Webcam 2"); + telemetry.addData("Press LeftBumper", "to switch to Webcam 1"); + } + + } // end method telemetryCameraSwitching() + + /** + * Add telemetry about AprilTag detections. + */ + private void telemetryAprilTag() { + + List currentDetections = aprilTag.getDetections(); + telemetry.addData("# AprilTags Detected", currentDetections.size()); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + } // end for() loop + + // Add "key" information to telemetry + telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); + telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); + telemetry.addLine("RBE = Range, Bearing & Elevation"); + + } // end method telemetryAprilTag() + + /** + * Set the active camera according to input from the gamepad. + */ + private void doCameraSwitching() { + if (visionPortal.getCameraState() == CameraState.STREAMING) { + // If the left bumper is pressed, use Webcam 1. + // If the right bumper is pressed, use Webcam 2. + boolean newLeftBumper = gamepad1.left_bumper; + boolean newRightBumper = gamepad1.right_bumper; + if (newLeftBumper && !oldLeftBumper) { + visionPortal.setActiveCamera(webcam1); + } else if (newRightBumper && !oldRightBumper) { + visionPortal.setActiveCamera(webcam2); + } + oldLeftBumper = newLeftBumper; + oldRightBumper = newRightBumper; + } + + } // end method doCameraSwitching() + +} // end class diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptDoubleVision.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptDoubleVision.java new file mode 100644 index 0000000..1d50049 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptDoubleVision.java @@ -0,0 +1,202 @@ +/* Copyright (c) 2023 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.tfod.Recognition; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import org.firstinspires.ftc.vision.tfod.TfodProcessor; + +import java.util.List; + +/* + * This OpMode illustrates the basics of using both AprilTag recognition and TensorFlow + * Object Detection. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Concept: Double Vision", group = "Concept") +@Disabled +public class ConceptDoubleVision extends LinearOpMode { + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + /** + * The variable to store our instance of the AprilTag processor. + */ + private AprilTagProcessor aprilTag; + + /** + * The variable to store our instance of the TensorFlow Object Detection processor. + */ + private TfodProcessor tfod; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal myVisionPortal; + + @Override + public void runOpMode() { + initDoubleVision(); + + // This OpMode loops continuously, allowing the user to switch between + // AprilTag and TensorFlow Object Detection (TFOD) image processors. + while (!isStopRequested()) { + + if (opModeInInit()) { + telemetry.addData("DS preview on/off","3 dots, Camera Stream"); + telemetry.addLine(); + telemetry.addLine("----------------------------------------"); + } + + if (myVisionPortal.getProcessorEnabled(aprilTag)) { + // User instructions: Dpad left or Dpad right. + telemetry.addLine("Dpad Left to disable AprilTag"); + telemetry.addLine(); + telemetryAprilTag(); + } else { + telemetry.addLine("Dpad Right to enable AprilTag"); + } + telemetry.addLine(); + telemetry.addLine("----------------------------------------"); + if (myVisionPortal.getProcessorEnabled(tfod)) { + telemetry.addLine("Dpad Down to disable TFOD"); + telemetry.addLine(); + telemetryTfod(); + } else { + telemetry.addLine("Dpad Up to enable TFOD"); + } + + // Push telemetry to the Driver Station. + telemetry.update(); + + if (gamepad1.dpad_left) { + myVisionPortal.setProcessorEnabled(aprilTag, false); + } else if (gamepad1.dpad_right) { + myVisionPortal.setProcessorEnabled(aprilTag, true); + } + if (gamepad1.dpad_down) { + myVisionPortal.setProcessorEnabled(tfod, false); + } else if (gamepad1.dpad_up) { + myVisionPortal.setProcessorEnabled(tfod, true); + } + + sleep(20); + + } // end while loop + + } // end method runOpMode() + + + /** + * Initialize AprilTag and TFOD. + */ + private void initDoubleVision() { + // ----------------------------------------------------------------------------------------- + // AprilTag Configuration + // ----------------------------------------------------------------------------------------- + + aprilTag = new AprilTagProcessor.Builder() + .build(); + + // ----------------------------------------------------------------------------------------- + // TFOD Configuration + // ----------------------------------------------------------------------------------------- + + tfod = new TfodProcessor.Builder() + .build(); + + // ----------------------------------------------------------------------------------------- + // Camera Configuration + // ----------------------------------------------------------------------------------------- + + if (USE_WEBCAM) { + myVisionPortal = new VisionPortal.Builder() + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .addProcessors(tfod, aprilTag) + .build(); + } else { + myVisionPortal = new VisionPortal.Builder() + .setCamera(BuiltinCameraDirection.BACK) + .addProcessors(tfod, aprilTag) + .build(); + } + } // end initDoubleVision() + + /** + * Add telemetry about AprilTag detections. + */ + private void telemetryAprilTag() { + List currentDetections = aprilTag.getDetections(); + telemetry.addData("# AprilTags Detected", currentDetections.size()); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + } // end for() loop + + } // end method telemetryAprilTag() + + /** + * Add telemetry about TensorFlow Object Detection (TFOD) recognitions. + */ + private void telemetryTfod() { + List currentRecognitions = tfod.getRecognitions(); + telemetry.addData("# Objects Detected", currentRecognitions.size()); + + // Step through the list of recognitions and display info for each one. + for (Recognition recognition : currentRecognitions) { + double x = (recognition.getLeft() + recognition.getRight()) / 2 ; + double y = (recognition.getTop() + recognition.getBottom()) / 2 ; + + telemetry.addData(""," "); + telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100); + telemetry.addData("- Position", "%.0f / %.0f", x, y); + telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight()); + } // end for() loop + + } // end method telemetryTfod() + +} // end class diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExploringIMUOrientation.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExploringIMUOrientation.java new file mode 100644 index 0000000..751d54f --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExploringIMUOrientation.java @@ -0,0 +1,184 @@ +/* +Copyright (c) 2022 REV Robotics, FIRST + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of REV Robotics nor the names of its contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.IMU; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +/* + * This OpMode demonstrates the impact of setting the IMU orientation correctly or incorrectly. This + * code assumes there is an IMU configured with the name "imu". + * + * Note: This OpMode is more of a tool than a code sample. The User Interface portion of this code + * goes beyond simply showing how to interface to the IMU.
+ * For a minimal example of interfacing to an IMU, please see the SensorIMUOrthogonal or SensorIMUNonOrthogonal sample OpModes. + * + * This OpMode enables you to re-specify the Hub Mounting orientation dynamically by using gamepad controls. + * While doing so, the sample will display how Pitch, Roll and Yaw angles change as the hub is moved. + * + * The gamepad controls let you change the two parameters that specify how the Control/Expansion Hub is mounted.
+ * The first parameter specifies which direction the printed logo on the Hub is pointing.
+ * The second parameter specifies which direction the USB connector on the Hub is pointing.
+ * All directions are relative to the robot, and left/right is as viewed from behind the robot. + * + * How will you know if you have chosen the correct Orientation? With the correct orientation + * parameters selected, pitch/roll/yaw should act as follows: + * + * Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X)
+ * Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y)
+ * Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z)
+ * + * The Yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller) + * + * The rotational velocities should follow the change in corresponding axes. + */ + +@TeleOp(name="Concept: IMU Orientation", group="Concept") +@Disabled +public class ConceptExploringIMUOrientation extends LinearOpMode { + static RevHubOrientationOnRobot.LogoFacingDirection[] logoFacingDirections + = RevHubOrientationOnRobot.LogoFacingDirection.values(); + static RevHubOrientationOnRobot.UsbFacingDirection[] usbFacingDirections + = RevHubOrientationOnRobot.UsbFacingDirection.values(); + static int LAST_DIRECTION = logoFacingDirections.length - 1; + static float TRIGGER_THRESHOLD = 0.2f; + + IMU imu; + int logoFacingDirectionPosition; + int usbFacingDirectionPosition; + boolean orientationIsValid = true; + + @Override public void runOpMode() throws InterruptedException { + imu = hardwareMap.get(IMU.class, "imu"); + logoFacingDirectionPosition = 0; // Up + usbFacingDirectionPosition = 2; // Forward + + updateOrientation(); + + boolean justChangedLogoDirection = false; + boolean justChangedUsbDirection = false; + + // Loop until stop requested + while (!isStopRequested()) { + + // Check to see if Yaw reset is requested (Y button) + if (gamepad1.y) { + telemetry.addData("Yaw", "Resetting\n"); + imu.resetYaw(); + } else { + telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset.\n"); + } + + // Check to see if new Logo Direction is requested + if (gamepad1.left_bumper || gamepad1.right_bumper) { + if (!justChangedLogoDirection) { + justChangedLogoDirection = true; + if (gamepad1.left_bumper) { + logoFacingDirectionPosition--; + if (logoFacingDirectionPosition < 0) { + logoFacingDirectionPosition = LAST_DIRECTION; + } + } else { + logoFacingDirectionPosition++; + if (logoFacingDirectionPosition > LAST_DIRECTION) { + logoFacingDirectionPosition = 0; + } + } + updateOrientation(); + } + } else { + justChangedLogoDirection = false; + } + + // Check to see if new USB Direction is requested + if (gamepad1.left_trigger > TRIGGER_THRESHOLD || gamepad1.right_trigger > TRIGGER_THRESHOLD) { + if (!justChangedUsbDirection) { + justChangedUsbDirection = true; + if (gamepad1.left_trigger > TRIGGER_THRESHOLD) { + usbFacingDirectionPosition--; + if (usbFacingDirectionPosition < 0) { + usbFacingDirectionPosition = LAST_DIRECTION; + } + } else { + usbFacingDirectionPosition++; + if (usbFacingDirectionPosition > LAST_DIRECTION) { + usbFacingDirectionPosition = 0; + } + } + updateOrientation(); + } + } else { + justChangedUsbDirection = false; + } + + // Display User instructions and IMU data + telemetry.addData("logo Direction (set with bumpers)", logoFacingDirections[logoFacingDirectionPosition]); + telemetry.addData("usb Direction (set with triggers)", usbFacingDirections[usbFacingDirectionPosition] + "\n"); + + if (orientationIsValid) { + YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles(); + AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES); + + telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES)); + telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES)); + telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES)); + telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate); + telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate); + telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate); + } else { + telemetry.addData("Error", "Selected orientation on robot is invalid"); + } + + telemetry.update(); + } + } + + // apply any requested orientation changes. + void updateOrientation() { + RevHubOrientationOnRobot.LogoFacingDirection logo = logoFacingDirections[logoFacingDirectionPosition]; + RevHubOrientationOnRobot.UsbFacingDirection usb = usbFacingDirections[usbFacingDirectionPosition]; + try { + RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logo, usb); + imu.initialize(new IMU.Parameters(orientationOnRobot)); + orientationIsValid = true; + } catch (IllegalArgumentException e) { + orientationIsValid = false; + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExternalHardwareClass.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExternalHardwareClass.java new file mode 100644 index 0000000..2fe4154 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExternalHardwareClass.java @@ -0,0 +1,142 @@ +/* Copyright (c) 2022 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.Range; + +/* + * This OpMode illustrates how to use an external "hardware" class to modularize all the robot's sensors and actuators. + * This approach is very efficient because the same hardware class can be used by all of your teleop and autonomous OpModes + * without requiring many copy & paste operations. Once you have defined and tested the hardware class with one OpMode, + * it is instantly available to other OpModes. + * + * The real benefit of this approach is that as you tweak your robot hardware, you only need to make changes in ONE place (the Hardware Class). + * So, to be effective you should put as much or your hardware setup and access code as possible in the hardware class. + * Essentially anything you do with hardware in BOTH Teleop and Auto should likely go in the hardware class. + * + * The Hardware Class is created in a separate file, and then an "instance" of this class is created in each OpMode. + * In order for the class to do typical OpMode things (like send telemetry data) it must be passed a reference to the + * OpMode object when it's created, so it can access all core OpMode functions. This is illustrated below. + * + * In this concept sample, the hardware class file is called RobotHardware.java and it must accompany this sample OpMode. + * So, if you copy ConceptExternalHardwareClass.java into TeamCode (using Android Studio or OnBotJava) then RobotHardware.java + * must also be copied to the same location (maintaining its name). + * + * For comparison purposes, this sample and its accompanying hardware class duplicates the functionality of the + * RobotTelopPOV_Linear OpMode. It assumes three motors (left_drive, right_drive and arm) and two servos (left_hand and right_hand) + * + * View the RobotHardware.java class file for more details + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * In OnBot Java, add a new OpMode, select this sample, and select TeleOp. + * Also add another new file named RobotHardware.java, select the sample with that name, and select Not an OpMode. + */ + +@TeleOp(name="Concept: Robot Hardware Class", group="Robot") +@Disabled +public class ConceptExternalHardwareClass extends LinearOpMode { + + // Create a RobotHardware object to be used to access robot hardware. + // Prefix any hardware functions with "robot." to access this class. + RobotHardware robot = new RobotHardware(this); + + @Override + public void runOpMode() { + double drive = 0; + double turn = 0; + double arm = 0; + double handOffset = 0; + + // initialize all the hardware, using the hardware class. See how clean and simple this is? + robot.init(); + + // Send telemetry message to signify robot waiting; + // Wait for the game to start (driver presses PLAY) + waitForStart(); + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + // Run wheels in POV mode (note: The joystick goes negative when pushed forward, so negate it) + // In this mode the Left stick moves the robot fwd and back, the Right stick turns left and right. + // This way it's also easy to just drive straight, or just turn. + drive = -gamepad1.left_stick_y; + turn = gamepad1.right_stick_x; + + // Combine drive and turn for blended motion. Use RobotHardware class + robot.driveRobot(drive, turn); + + // Use gamepad left & right Bumpers to open and close the claw + // Use the SERVO constants defined in RobotHardware class. + // Each time around the loop, the servos will move by a small amount. + // Limit the total offset to half of the full travel range + if (gamepad1.right_bumper) + handOffset += robot.HAND_SPEED; + else if (gamepad1.left_bumper) + handOffset -= robot.HAND_SPEED; + handOffset = Range.clip(handOffset, -0.5, 0.5); + + // Move both servos to new position. Use RobotHardware class + robot.setHandPositions(handOffset); + + // Use gamepad buttons to move arm up (Y) and down (A) + // Use the MOTOR constants defined in RobotHardware class. + if (gamepad1.y) + arm = robot.ARM_UP_POWER; + else if (gamepad1.a) + arm = robot.ARM_DOWN_POWER; + else + arm = 0; + + robot.setArmPower(arm); + + // Send telemetry messages to explain controls and show robot status + telemetry.addData("Drive", "Left Stick"); + telemetry.addData("Turn", "Right Stick"); + telemetry.addData("Arm Up/Down", "Y & A Buttons"); + telemetry.addData("Hand Open/Closed", "Left and Right Bumpers"); + telemetry.addData("-", "-------"); + + telemetry.addData("Drive Power", "%.2f", drive); + telemetry.addData("Turn Power", "%.2f", turn); + telemetry.addData("Arm Power", "%.2f", arm); + telemetry.addData("Hand Position", "Offset = %.2f", handOffset); + telemetry.update(); + + // Pace this loop so hands move at a reasonable speed. + sleep(50); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadRumble.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadRumble.java new file mode 100644 index 0000000..cf846e1 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadRumble.java @@ -0,0 +1,200 @@ +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.util.ElapsedTime; + +/* + * This OpMode illustrates using the rumble feature of many gamepads. + * + * Note: Some gamepads "rumble" better than others. + * The Xbox & PS4 have a left (rumble1) and right (rumble2) rumble motor. + * These two gamepads have two distinct rumble modes: Large on the left, and small on the right + * The Etpark gamepad may only respond to rumble1, and may only run at full power. + * The Logitech F310 gamepad does not have *any* rumble ability. + * + * Moral: You should use this sample to experiment with your specific gamepads to explore their rumble features. + * + * The rumble motors are accessed through the standard gamepad1 and gamepad2 objects. + * Several new methods were added to the Gamepad class in FTC SDK Rev 7 + * The key methods are as follows: + * + * .rumble(double rumble1, double rumble2, int durationMs) + * This method sets the rumble power of both motors for a specific time duration. + * Both rumble arguments are motor-power levels in the 0.0 to 1.0 range. + * durationMs is the desired length of the rumble action in milliseconds. + * This method returns immediately. + * Note: + * Use a durationMs of Gamepad.RUMBLE_DURATION_CONTINUOUS to provide a continuous rumble + * Use a power of 0, or duration of 0 to stop a rumble. + * + * .rumbleBlips(int count) allows an easy way to signal the driver with a series of rumble blips. + * Just specify how many blips you want. + * This method returns immediately. + * + * .runRumbleEffect(customRumbleEffect) allows you to run a custom rumble sequence that you have + * built using the Gamepad.RumbleEffect.Builder() + * A "custom effect" is a sequence of steps, where each step can rumble any of the + * rumble motors for a specific period at a specific power level. + * The Custom Effect will play as the (un-blocked) OpMode continues to run + * + * .isRumbling() returns true if there is a rumble effect in progress. + * Use this call to prevent stepping on a current rumble. + * + * .stopRumble() Stop any ongoing rumble or custom rumble effect. + * + * .rumble(int durationMs) Full power rumble for fixed duration. + * + * Note: Whenever a new Rumble command is issued, any currently executing rumble action will + * be truncated, and the new action started immediately. Take these precautions: + * 1) Do Not SPAM the rumble motors by issuing rapid fire commands + * 2) Multiple sources for rumble commands must coordinate to avoid tromping on each other. + * + * This can be achieved several possible ways: + * 1) Only having one source for rumble actions + * 2) Issuing rumble commands on transitions, rather than states. + * e.g. The moment a touch sensor is pressed, rather than the entire time it is being pressed. + * 3) Scheduling rumble commands based on timed events. e.g. 10 seconds prior to endgame + * 4) Rumble on non-overlapping mechanical actions. e.g. arm fully-extended or fully-retracted. + * 5) Use isRumbling() to hold off on a new rumble if one is already in progress. + * + * The examples shown here are representstive of how to invoke a gamepad rumble. + * It is assumed that these will be modified to suit the specific robot and team strategy needs. + * + * ######## Read the telemetry display on the Driver Station Screen for instructions. ###### + * + * Ex 1) This example shows a) how to create a custom rumble effect, and then b) how to trigger it based + * on game time. One use for this might be to alert the driver that half-time or End-game is approaching. + * + * Ex 2) This example shows tying the rumble power to a changing sensor value. + * In this case it is the Gamepad trigger, but it could be any sensor output scaled to the 0-1 range. + * Since it takes over the rumble motors, it is only performed when the Left Bumper is pressed. + * Note that this approach MUST include a way to turn OFF the rumble when the button is released. + * + * Ex 3) This example shows a simple way to trigger a 3-blip sequence. In this case it is + * triggered by the gamepad A (Cross) button, but it could be any sensor, like a touch or light sensor. + * Note that this code ensures that it only rumbles once when the input goes true. + * + * Ex 4) This example shows how to trigger a single rumble when an input value gets over a certain value. + * In this case it is reading the Right Trigger, but it could be any variable sensor, like a + * range sensor, or position sensor. The code needs to ensure that it is only triggered once, so + * it waits till the sensor drops back below the threshold before it can trigger again. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ + +@Disabled +@TeleOp(name="Concept: Gamepad Rumble", group ="Concept") +public class ConceptGamepadRumble extends LinearOpMode +{ + boolean lastA = false; // Use to track the prior button state. + boolean lastLB = false; // Use to track the prior button state. + boolean highLevel = false; // used to prevent multiple level-based rumbles. + boolean secondHalf = false; // Use to prevent multiple half-time warning rumbles. + + Gamepad.RumbleEffect customRumbleEffect; // Use to build a custom rumble sequence. + ElapsedTime runtime = new ElapsedTime(); // Use to determine when end game is starting. + + final double HALF_TIME = 60.0; // Wait this many seconds before rumble-alert for half-time. + final double TRIGGER_THRESHOLD = 0.75; // Squeeze more than 3/4 to get rumble. + + @Override + public void runOpMode() + { + // Example 1. a) start by creating a three-pulse rumble sequence: right, LEFT, LEFT + customRumbleEffect = new Gamepad.RumbleEffect.Builder() + .addStep(0.0, 1.0, 500) // Rumble right motor 100% for 500 mSec + .addStep(0.0, 0.0, 300) // Pause for 300 mSec + .addStep(1.0, 0.0, 250) // Rumble left motor 100% for 250 mSec + .addStep(0.0, 0.0, 250) // Pause for 250 mSec + .addStep(1.0, 0.0, 250) // Rumble left motor 100% for 250 mSec + .build(); + + telemetry.addData(">", "Press Start"); + telemetry.update(); + + waitForStart(); + runtime.reset(); // Start game timer. + + // Loop while monitoring buttons for rumble triggers + while (opModeIsActive()) + { + // Read and save the current gamepad button states. + boolean currentA = gamepad1.a ; + boolean currentLB = gamepad1.left_bumper ; + + // Display the current Rumble status. Just for interest. + telemetry.addData(">", "Are we RUMBLING? %s\n", gamepad1.isRumbling() ? "YES" : "no" ); + + // ---------------------------------------------------------------------------------------- + // Example 1. b) Watch the runtime timer, and run the custom rumble when we hit half-time. + // Make sure we only signal once by setting "secondHalf" flag to prevent further rumbles. + // ---------------------------------------------------------------------------------------- + if ((runtime.seconds() > HALF_TIME) && !secondHalf) { + gamepad1.runRumbleEffect(customRumbleEffect); + secondHalf =true; + } + + // Display the time remaining while we are still counting down. + if (!secondHalf) { + telemetry.addData(">", "Halftime Alert Countdown: %3.0f Sec \n", (HALF_TIME - runtime.seconds()) ); + } + + + // ---------------------------------------------------------------------------------------- + // Example 2. If Left Bumper is being pressed, power the rumble motors based on the two trigger depressions. + // This is useful to see how the rumble feels at various power levels. + // ---------------------------------------------------------------------------------------- + if (currentLB) { + // Left Bumper is being pressed, so send left and right "trigger" values to left and right rumble motors. + gamepad1.rumble(gamepad1.left_trigger, gamepad1.right_trigger, Gamepad.RUMBLE_DURATION_CONTINUOUS); + + // Show what is being sent to rumbles + telemetry.addData(">", "Squeeze triggers to control rumbles"); + telemetry.addData("> : Rumble", "Left: %.0f%% Right: %.0f%%", gamepad1.left_trigger * 100, gamepad1.right_trigger * 100); + } else { + // Make sure rumble is turned off when Left Bumper is released (only one time each press) + if (lastLB) { + gamepad1.stopRumble(); + } + + // Prompt for manual rumble action + telemetry.addData(">", "Hold Left-Bumper to test Manual Rumble"); + telemetry.addData(">", "Press A (Cross) for three blips"); + telemetry.addData(">", "Squeeze right trigger slowly for 1 blip"); + } + lastLB = currentLB; // remember the current button state for next time around the loop + + + // ---------------------------------------------------------------------------------------- + // Example 3. Blip 3 times at the moment that A (Cross) is pressed. (look for pressed transition) + // BUT !!! Skip it altogether if the Gamepad is already rumbling. + // ---------------------------------------------------------------------------------------- + if (currentA && !lastA) { + if (!gamepad1.isRumbling()) // Check for possible overlap of rumbles. + gamepad1.rumbleBlips(3); + } + lastA = currentA; // remember the current button state for next time around the loop + + + // ---------------------------------------------------------------------------------------- + // Example 4. Rumble once when gamepad right trigger goes above the THRESHOLD. + // ---------------------------------------------------------------------------------------- + if (gamepad1.right_trigger > TRIGGER_THRESHOLD) { + if (!highLevel) { + gamepad1.rumble(0.9, 0, 200); // 200 mSec burst on left motor. + highLevel = true; // Hold off any more triggers + } + } else { + highLevel = false; // We can trigger again now. + } + + // Send the telemetry data to the Driver Station, and then pause to pace the program. + telemetry.update(); + sleep(10); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadTouchpad.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadTouchpad.java new file mode 100644 index 0000000..84d8cec --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadTouchpad.java @@ -0,0 +1,77 @@ +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.Telemetry; + +/* + * This OpMode illustrates using the touchpad feature found on some gamepads. + * + * The Sony PS4 gamepad can detect two distinct touches on the central touchpad. + * Other gamepads with different touchpads may provide mixed results. + * + * The touchpads are accessed through the standard gamepad1 and gamepad2 objects. + * Several new members were added to the Gamepad class in FTC SDK Rev 7 + * + * .touchpad_finger_1 returns true if at least one finger is detected. + * .touchpad_finger_1_x finger 1 X coordinate. Valid if touchpad_finger_1 is true + * .touchpad_finger_1_y finger 1 Y coordinate. Valid if touchpad_finger_1 is true + * + * .touchpad_finger_2 returns true if a second finger is detected + * .touchpad_finger_2_x finger 2 X coordinate. Valid if touchpad_finger_2 is true + * .touchpad_finger_2_y finger 2 Y coordinate. Valid if touchpad_finger_2 is true + * + * Finger touches are reported with an X and Y coordinate in following coordinate system. + * + * 1) X is the Horizontal axis, and Y is the vertical axis + * 2) The 0,0 origin is at the center of the touchpad. + * 3) 1.0, 1.0 is at the top right corner of the touchpad. + * 4) -1.0,-1.0 is at the bottom left corner of the touchpad. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ + +@Disabled +@TeleOp(name="Concept: Gamepad Touchpad", group ="Concept") +public class ConceptGamepadTouchpad extends LinearOpMode +{ + @Override + public void runOpMode() + { + telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); + + telemetry.addData(">", "Press Start"); + telemetry.update(); + + waitForStart(); + + while (opModeIsActive()) + { + boolean finger = false; + + // Display finger 1 x & y position if finger detected + if(gamepad1.touchpad_finger_1) + { + finger = true; + telemetry.addLine(String.format("Finger 1: x=%5.2f y=%5.2f\n", gamepad1.touchpad_finger_1_x, gamepad1.touchpad_finger_1_y)); + } + + // Display finger 2 x & y position if finger detected + if(gamepad1.touchpad_finger_2) + { + finger = true; + telemetry.addLine(String.format("Finger 2: x=%5.2f y=%5.2f\n", gamepad1.touchpad_finger_2_x, gamepad1.touchpad_finger_2_y)); + } + + if(!finger) + { + telemetry.addLine("No fingers"); + } + + telemetry.update(); + sleep(10); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java new file mode 100644 index 0000000..3bade8b --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java @@ -0,0 +1,227 @@ +/* Copyright (c) 2019 Phil Malone. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.util.ElapsedTime; + +import java.util.List; + +/* + * This OpMode illustrates how to use the Expansion Hub's Bulk-Read feature to speed up control cycle times. + * In this example there are 4 motors that need their encoder positions, and velocities read. + * The sample is written to work with one or two expansion hubs, with no assumption as to where the motors are located. + * + * Three scenarios are tested: + * Cache Mode = OFF This is the normal default, where no cache is used, and every read produces a discrete transaction with + * an expansion hub, which is the slowest approach, but guarentees that the value is as fresh (recent) as possible.. + * + * Cache Mode = AUTO This mode will attempt to minimize the number of discrete read commands, by performing bulk-reads + * and then returning values that have been cached. The cache is updated automatically whenever any specific encoder is re-read. + * This mode will always return new data, but it may perform more bulk-reads than absolutely required. + * Extra reads will be performed if multiple encoder/velocity reads are performed on the same encoder in one control cycle. + * This mode is a good compromise between the OFF and MANUAL modes. + * Note: If there are significant user-program delays between encoder reads, the cached value may not be fresh (recent). + * You can issue a clearBulkCache() call at any time force a fresh bulk-read on the next encoder read. + * + * Cache Mode = MANUAL This mode requires the user's code to determine the best time to clear the cached bulk-read data. + * Well organized code will reset the cache once at the beginning of the control cycle, and then immediately read and store all the encoder values. + * This approach will produce the shortest cycle times, but it does require the user to manually clear the cache. + * Since NO automatic Bulk-Reads are performed, neglecting to clear the bulk cache will result in the same values being returned + * each time an encoder read is performed. + * + * ------------------------------------- + * + * General tip to speed up your control cycles: + * + * No matter what method you use to read encoders and other inputs, you should try to + * avoid reading the same encoder input multiple times around a control loop. + * Under normal conditions, this will slow down the control loop. + * The preferred method is to read all the required inputs ONCE at the beginning of the loop, + * and save the values in variable that can be used by other parts of the control code. + * + * eg: if you are sending encoder positions to your telemetry display, putting a getCurrentPosition() + * call in the telemetry statement will force the code to go and get another copy which will take time. + * It's much better read the position into a variable once, and use that variable for control AND display. + * Reading saved variables takes no time at all. + * + * Once you put all your sensor reads at the beginning of the control cycle, it's very easy to use + * the bulk-read AUTO mode to streamline your cycle timing. + */ +@TeleOp (name = "Motor Bulk Reads", group = "Tests") +@Disabled +public class ConceptMotorBulkRead extends LinearOpMode { + + final int TEST_CYCLES = 500; // Number of control cycles to run to determine cycle times. + + private DcMotorEx m1, m2, m3, m4; // Motor Objects + private long e1, e2, e3, e4; // Encoder Values + private double v1, v2, v3, v4; // Velocities + + // Cycle Times + double t1 = 0; + double t2 = 0; + double t3 = 0; + + @Override + public void runOpMode() { + + int cycles; + + // Important Step 1: Make sure you use DcMotorEx when you instantiate your motors. + m1 = hardwareMap.get(DcMotorEx.class, "m1"); // Configure the robot to use these 4 motor names, + m2 = hardwareMap.get(DcMotorEx.class, "m2"); // or change these strings to match your existing Robot Configuration. + m3 = hardwareMap.get(DcMotorEx.class, "m3"); + m4 = hardwareMap.get(DcMotorEx.class, "m4"); + + // Important Step 2: Get access to a list of Expansion Hub Modules to enable changing caching methods. + List allHubs = hardwareMap.getAll(LynxModule.class); + + ElapsedTime timer = new ElapsedTime(); + + telemetry.addData(">", "Press play to start tests"); + telemetry.addData(">", "Test results will update for each access method."); + telemetry.update(); + waitForStart(); + + // -------------------------------------------------------------------------------------- + // Run control loop using legacy encoder reads + // In this mode, a single read is done for each encoder position, and a bulk read is done for each velocity read. + // This is the worst case scenario. + // This is the same as using LynxModule.BulkCachingMode.OFF + // -------------------------------------------------------------------------------------- + + displayCycleTimes("Test 1 of 3 (Wait for completion)"); + + timer.reset(); + cycles = 0; + while (opModeIsActive() && (cycles++ < TEST_CYCLES)) { + e1 = m1.getCurrentPosition(); + e2 = m2.getCurrentPosition(); + e3 = m3.getCurrentPosition(); + e4 = m4.getCurrentPosition(); + + v1 = m1.getVelocity(); + v2 = m2.getVelocity(); + v3 = m3.getVelocity(); + v4 = m4.getVelocity(); + + // Put Control loop action code here. + + } + // calculate the average cycle time. + t1 = timer.milliseconds() / cycles; + displayCycleTimes("Test 2 of 3 (Wait for completion)"); + + // -------------------------------------------------------------------------------------- + // Run test cycles using AUTO cache mode + // In this mode, only one bulk read is done per cycle, UNLESS you read a specific encoder/velocity item AGAIN in that cycle. + // -------------------------------------------------------------------------------------- + + // Important Step 3: Option A. Set all Expansion hubs to use the AUTO Bulk Caching mode + for (LynxModule module : allHubs) { + module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); + } + + timer.reset(); + cycles = 0; + while (opModeIsActive() && (cycles++ < TEST_CYCLES)) { + e1 = m1.getCurrentPosition(); // Uses 1 bulk-read for all 4 encoder/velocity reads, + e2 = m2.getCurrentPosition(); // but don't do any `get` operations more than once per cycle. + e3 = m3.getCurrentPosition(); + e4 = m4.getCurrentPosition(); + + v1 = m1.getVelocity(); + v2 = m2.getVelocity(); + v3 = m3.getVelocity(); + v4 = m4.getVelocity(); + + // Put Control loop action code here. + + } + // calculate the average cycle time. + t2 = timer.milliseconds() / cycles; + displayCycleTimes("Test 3 of 3 (Wait for completion)"); + + // -------------------------------------------------------------------------------------- + // Run test cycles using MANUAL cache mode + // In this mode, only one block read is done each control cycle. + // This is the MOST efficient method, but it does require that the cache is cleared manually each control cycle. + // -------------------------------------------------------------------------------------- + + // Important Step 3: Option B. Set all Expansion hubs to use the MANUAL Bulk Caching mode + for (LynxModule module : allHubs) { + module.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); + } + + timer.reset(); + cycles = 0; + while (opModeIsActive() && (cycles++ < TEST_CYCLES)) { + + // Important Step 4: If you are using MANUAL mode, you must clear the BulkCache once per control cycle + for (LynxModule module : allHubs) { + module.clearBulkCache(); + } + + e1 = m1.getCurrentPosition(); // Uses 1 bulk-read to obtain ALL the motor data + e2 = m2.getCurrentPosition(); // There is no penalty for doing more `get` operations in this cycle, + e3 = m3.getCurrentPosition(); // but they will return the same data. + e4 = m4.getCurrentPosition(); + + v1 = m1.getVelocity(); + v2 = m2.getVelocity(); + v3 = m3.getVelocity(); + v4 = m4.getVelocity(); + + // Put Control loop action code here. + + } + // calculate the average cycle time. + t3 = timer.milliseconds() / cycles; + displayCycleTimes("Complete"); + + // wait until op-mode is stopped by user, before clearing display. + while (opModeIsActive()) ; + } + + // Display three comparison times. + void displayCycleTimes(String status) { + telemetry.addData("Testing", status); + telemetry.addData("Cache = OFF", "%5.1f mS/cycle", t1); + telemetry.addData("Cache = AUTO", "%5.1f mS/cycle", t2); + telemetry.addData("Cache = MANUAL", "%5.1f mS/cycle", t3); + telemetry.update(); + } +} + diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java new file mode 100644 index 0000000..7ea4683 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java @@ -0,0 +1,89 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.ElapsedTime; + +/* + * Demonstrates an empty iterative OpMode + */ +@TeleOp(name = "Concept: NullOp", group = "Concept") +@Disabled +public class ConceptNullOp extends OpMode { + + private ElapsedTime runtime = new ElapsedTime(); + + /** + * This method will be called once, when the INIT button is pressed. + */ + @Override + public void init() { + telemetry.addData("Status", "Initialized"); + } + + /** + * This method will be called repeatedly during the period between when + * the init button is pressed and when the play button is pressed (or the + * OpMode is stopped). + */ + @Override + public void init_loop() { + } + + /** + * This method will be called once, when the play button is pressed. + */ + @Override + public void start() { + runtime.reset(); + } + + /** + * This method will be called repeatedly during the period between when + * the play button is pressed and when the OpMode is stopped. + */ + @Override + public void loop() { + telemetry.addData("Status", "Run Time: " + runtime.toString()); + } + + /** + * This method will be called once, when this OpMode is stopped. + *

+ * Your ability to control hardware from this method will be limited. + */ + @Override + public void stop() { + + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRampMotorSpeed.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRampMotorSpeed.java new file mode 100644 index 0000000..6e0be37 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRampMotorSpeed.java @@ -0,0 +1,114 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; + +/* + * This OpMode ramps a single motor speed up and down repeatedly until Stop is pressed. + * The code is structured as a LinearOpMode + * + * This code assumes a DC motor configured with the name "left_drive" as is found on a Robot. + * + * INCREMENT sets how much to increase/decrease the power each cycle + * CYCLE_MS sets the update period. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ +@TeleOp(name = "Concept: Ramp Motor Speed", group = "Concept") +@Disabled +public class ConceptRampMotorSpeed extends LinearOpMode { + + static final double INCREMENT = 0.01; // amount to ramp motor each CYCLE_MS cycle + static final int CYCLE_MS = 50; // period of each cycle + static final double MAX_FWD = 1.0; // Maximum FWD power applied to motor + static final double MAX_REV = -1.0; // Maximum REV power applied to motor + + // Define class members + DcMotor motor; + double power = 0; + boolean rampUp = true; + + + @Override + public void runOpMode() { + + // Connect to motor (Assume standard left wheel) + // Change the text in quotes to match any motor name on your robot. + motor = hardwareMap.get(DcMotor.class, "left_drive"); + + // Wait for the start button + telemetry.addData(">", "Press Start to run Motors." ); + telemetry.update(); + waitForStart(); + + // Ramp motor speeds till stop pressed. + while(opModeIsActive()) { + + // Ramp the motors, according to the rampUp variable. + if (rampUp) { + // Keep stepping up until we hit the max value. + power += INCREMENT ; + if (power >= MAX_FWD ) { + power = MAX_FWD; + rampUp = !rampUp; // Switch ramp direction + } + } + else { + // Keep stepping down until we hit the min value. + power -= INCREMENT ; + if (power <= MAX_REV ) { + power = MAX_REV; + rampUp = !rampUp; // Switch ramp direction + } + } + + // Display the current value + telemetry.addData("Motor Power", "%5.2f", power); + telemetry.addData(">", "Press Stop to end test." ); + telemetry.update(); + + // Set the motor to the new power and pause; + motor.setPower(power); + sleep(CYCLE_MS); + idle(); + } + + // Turn off motor and signal done; + motor.setPower(0); + telemetry.addData(">", "Done"); + telemetry.update(); + + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevSPARKMini.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevSPARKMini.java new file mode 100644 index 0000000..e710662 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevSPARKMini.java @@ -0,0 +1,111 @@ +/* Copyright (c) 2018 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + + +/* + * This OpMode executes a basic Tank Drive Teleop for a two wheeled robot using two REV SPARKminis. + * To use this example, connect two REV SPARKminis into servo ports on the Expansion Hub. On the + * robot configuration, use the drop down list under 'Servos' to select 'REV SPARKmini Controller' + * and name them 'left_drive' and 'right_drive'. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="REV SPARKmini Simple Drive Example", group="Concept") +@Disabled +public class ConceptRevSPARKMini extends LinearOpMode { + + // Declare OpMode members. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotorSimple leftDrive = null; + private DcMotorSimple rightDrive = null; + + @Override + public void runOpMode() { + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + // Initialize the hardware variables. Note that the strings used here as parameters + // to 'get' must correspond to the names assigned during the robot configuration + // step (using the FTC Robot Controller app on the phone). + leftDrive = hardwareMap.get(DcMotorSimple.class, "left_drive"); + rightDrive = hardwareMap.get(DcMotorSimple.class, "right_drive"); + + // Most robots need the motor on one side to be reversed to drive forward + // Reverse the motor that runs backward when connected directly to the battery + leftDrive.setDirection(DcMotorSimple.Direction.FORWARD); + rightDrive.setDirection(DcMotorSimple.Direction.REVERSE); + + // Wait for the game to start (driver presses PLAY) + waitForStart(); + runtime.reset(); + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + // Setup a variable for each drive wheel to save power level for telemetry + double leftPower; + double rightPower; + + // Choose to drive using either Tank Mode, or POV Mode + // Comment out the method that's not used. The default below is POV. + + // POV Mode uses left stick to go forward, and right stick to turn. + // - This uses basic math to combine motions and is easier to drive straight. + double drive = -gamepad1.left_stick_y; + double turn = gamepad1.right_stick_x; + leftPower = Range.clip(drive + turn, -1.0, 1.0) ; + rightPower = Range.clip(drive - turn, -1.0, 1.0) ; + + // Tank Mode uses one stick to control each wheel. + // - This requires no math, but it is hard to drive forward slowly and keep straight. + // leftPower = -gamepad1.left_stick_y ; + // rightPower = -gamepad1.right_stick_y ; + + // Send calculated power to wheels + leftDrive.setPower(leftPower); + rightDrive.setPower(rightPower); + + // Show the elapsed game time and wheel power. + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower); + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptScanServo.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptScanServo.java new file mode 100644 index 0000000..2b8ad33 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptScanServo.java @@ -0,0 +1,115 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; + +/* + * This OpMode scans a single servo back and forward until Stop is pressed. + * The code is structured as a LinearOpMode + * INCREMENT sets how much to increase/decrease the servo position each cycle + * CYCLE_MS sets the update period. + * + * This code assumes a Servo configured with the name "left_hand" as is found on a Robot. + * + * NOTE: When any servo position is set, ALL attached servos are activated, so ensure that any other + * connected servos are able to move freely before running this test. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ +@TeleOp(name = "Concept: Scan Servo", group = "Concept") +@Disabled +public class ConceptScanServo extends LinearOpMode { + + static final double INCREMENT = 0.01; // amount to slew servo each CYCLE_MS cycle + static final int CYCLE_MS = 50; // period of each cycle + static final double MAX_POS = 1.0; // Maximum rotational position + static final double MIN_POS = 0.0; // Minimum rotational position + + // Define class members + Servo servo; + double position = (MAX_POS - MIN_POS) / 2; // Start at halfway position + boolean rampUp = true; + + + @Override + public void runOpMode() { + + // Connect to servo (Assume Robot Left Hand) + // Change the text in quotes to match any servo name on your robot. + servo = hardwareMap.get(Servo.class, "left_hand"); + + // Wait for the start button + telemetry.addData(">", "Press Start to scan Servo." ); + telemetry.update(); + waitForStart(); + + + // Scan servo till stop pressed. + while(opModeIsActive()){ + + // slew the servo, according to the rampUp (direction) variable. + if (rampUp) { + // Keep stepping up until we hit the max value. + position += INCREMENT ; + if (position >= MAX_POS ) { + position = MAX_POS; + rampUp = !rampUp; // Switch ramp direction + } + } + else { + // Keep stepping down until we hit the min value. + position -= INCREMENT ; + if (position <= MIN_POS ) { + position = MIN_POS; + rampUp = !rampUp; // Switch ramp direction + } + } + + // Display the current value + telemetry.addData("Servo Position", "%5.2f", position); + telemetry.addData(">", "Press Stop to end test." ); + telemetry.update(); + + // Set the servo to the new position and pause; + servo.setPosition(position); + sleep(CYCLE_MS); + idle(); + } + + // Signal done; + telemetry.addData(">", "Done"); + telemetry.update(); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsASJava.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsASJava.java new file mode 100644 index 0000000..f0a855f --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsASJava.java @@ -0,0 +1,133 @@ +/* Copyright (c) 2018 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.ftccommon.SoundPlayer; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +/* + * This OpMode demonstrates how to play simple sounds on both the RC and DS phones. + * It illustrates how to build sounds into your application as a resource. + * This technique is best suited for use with Android Studio since it assumes you will be creating a new application + * + * If you are using OnBotJava, please see the ConceptSoundsOnBotJava sample + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * Operation: + * + * Gamepad X & B buttons are used to trigger sounds in this example, but any event can be used. + * Note: Time should be allowed for sounds to complete before playing other sounds. + * + * For sound files to be used as a compiled-in resource, they need to be located in a folder called "raw" under your "res" (resources) folder. + * You can create your own "raw" folder from scratch, or you can copy the one from the FtcRobotController module. + * + * Android Studio coders will ultimately need a folder in your path as follows: + * /TeamCode/src/main/res/raw + * + * Copy any .wav files you want to play into this folder. + * Make sure that your files ONLY use lower-case characters, and have no spaces or special characters other than underscore. + * + * The name you give your .wav files will become the resource ID for these sounds. + * eg: gold.wav becomes R.raw.gold + * + * If you wish to use the sounds provided for this sample, they are located in: + * /FtcRobotController/src/main/res/raw + * You can copy and paste the entire 'raw' folder using Android Studio. + * + */ + +@TeleOp(name="Concept: Sound Resources", group="Concept") +@Disabled +public class ConceptSoundsASJava extends LinearOpMode { + + // Declare OpMode members. + private boolean goldFound; // Sound file present flags + private boolean silverFound; + + private boolean isX = false; // Gamepad button state variables + private boolean isB = false; + + private boolean wasX = false; // Gamepad button history variables + private boolean WasB = false; + + @Override + public void runOpMode() { + + // Determine Resource IDs for sounds built into the RC application. + int silverSoundID = hardwareMap.appContext.getResources().getIdentifier("silver", "raw", hardwareMap.appContext.getPackageName()); + int goldSoundID = hardwareMap.appContext.getResources().getIdentifier("gold", "raw", hardwareMap.appContext.getPackageName()); + + // Determine if sound resources are found. + // Note: Preloading is NOT required, but it's a good way to verify all your sounds are available before you run. + if (goldSoundID != 0) + goldFound = SoundPlayer.getInstance().preload(hardwareMap.appContext, goldSoundID); + + if (silverSoundID != 0) + silverFound = SoundPlayer.getInstance().preload(hardwareMap.appContext, silverSoundID); + + // Display sound status + telemetry.addData("gold resource", goldFound ? "Found" : "NOT found\n Add gold.wav to /src/main/res/raw" ); + telemetry.addData("silver resource", silverFound ? "Found" : "Not found\n Add silver.wav to /src/main/res/raw" ); + + // Wait for the game to start (driver presses PLAY) + telemetry.addData(">", "Press Start to continue"); + telemetry.update(); + waitForStart(); + + telemetry.addData(">", "Press X, B to play sounds."); + telemetry.update(); + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + // say Silver each time gamepad X is pressed (This sound is a resource) + if (silverFound && (isX = gamepad1.x) && !wasX) { + SoundPlayer.getInstance().startPlaying(hardwareMap.appContext, silverSoundID); + telemetry.addData("Playing", "Resource Silver"); + telemetry.update(); + } + + // say Gold each time gamepad B is pressed (This sound is a resource) + if (goldFound && (isB = gamepad1.b) && !WasB) { + SoundPlayer.getInstance().startPlaying(hardwareMap.appContext, goldSoundID); + telemetry.addData("Playing", "Resource Gold"); + telemetry.update(); + } + + // Save last button states + wasX = isX; + WasB = isB; + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsOnBotJava.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsOnBotJava.java new file mode 100644 index 0000000..fbb7416 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsOnBotJava.java @@ -0,0 +1,120 @@ +/* Copyright (c) 2018 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.ftccommon.SoundPlayer; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import java.io.File; + +/* + * This OpMode demonstrates how to play simple sounds on both the RC and DS phones. + * It illustrates how to play sound files that have been copied to the RC Phone + * This technique is best suited for use with OnBotJava since it does not require the app to be modified. + * + * Operation: + * + * Gamepad X & B buttons are used to trigger sounds in this example, but any event can be used. + * Note: Time should be allowed for sounds to complete before playing other sounds. + * + * To play a new sound, you will need to copy the .wav files to the phone, and then provide the full path to them as part of your OpMode. + * This is done in this sample for the two sound files. silver.wav and gold.wav + * + * You can put the files in a variety of soundPaths, but we recommend you put them in the /FIRST/blocks/sounds folder. + * Your OpModes will have guaranteed access to this folder, and you can transfer files into this folder using the BLOCKS web page. + * -- There is a link called "sounds" on the right hand side of the color bar on the BLOCKS page that can be used to send sound files to this folder by default. + * Or you can use Windows File Manager, or ADB to transfer the sound files + * + * To get full use of THIS sample, you will need to copy two sound file called silver.wav and gold.wav to /FIRST/blocks/sounds on the RC phone. + * They can be located here: + * https://github.com/ftctechnh/ftc_app/tree/master/FtcRobotController/src/main/res/raw/gold.wav + * https://github.com/ftctechnh/ftc_app/tree/master/FtcRobotController/src/main/res/raw/silver.wav + */ + +@TeleOp(name="Concept: Sound Files", group="Concept") +@Disabled +public class ConceptSoundsOnBotJava extends LinearOpMode { + + // Point to sound files on the phone's drive + private String soundPath = "/FIRST/blocks/sounds"; + private File goldFile = new File("/sdcard" + soundPath + "/gold.wav"); + private File silverFile = new File("/sdcard" + soundPath + "/silver.wav"); + + // Declare OpMode members. + private boolean isX = false; // Gamepad button state variables + private boolean isB = false; + + private boolean wasX = false; // Gamepad button history variables + private boolean WasB = false; + + @Override + public void runOpMode() { + + // Make sure that the sound files exist on the phone + boolean goldFound = goldFile.exists(); + boolean silverFound = silverFile.exists(); + + // Display sound status + telemetry.addData("gold sound", goldFound ? "Found" : "NOT Found \nCopy gold.wav to " + soundPath ); + telemetry.addData("silver sound", silverFound ? "Found" : "NOT Found \nCopy silver.wav to " + soundPath ); + + // Wait for the game to start (driver presses PLAY) + telemetry.addData(">", "Press Start to continue"); + telemetry.update(); + waitForStart(); + + telemetry.addData(">", "Press X or B to play sounds."); + telemetry.update(); + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + // say Silver each time gamepad X is pressed (This sound is a resource) + if (silverFound && (isX = gamepad1.x) && !wasX) { + SoundPlayer.getInstance().startPlaying(hardwareMap.appContext, silverFile); + telemetry.addData("Playing", "Silver File"); + telemetry.update(); + } + + // say Gold each time gamepad B is pressed (This sound is a resource) + if (goldFound && (isB = gamepad1.b) && !WasB) { + SoundPlayer.getInstance().startPlaying(hardwareMap.appContext, goldFile); + telemetry.addData("Playing", "Gold File"); + telemetry.update(); + } + + // Save last button states + wasX = isX; + WasB = isB; + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsSKYSTONE.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsSKYSTONE.java new file mode 100644 index 0000000..983e434 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsSKYSTONE.java @@ -0,0 +1,122 @@ +/* Copyright (c) 2018 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import android.content.Context; +import com.qualcomm.ftccommon.SoundPlayer; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +/* + * This OpMode demonstrates how to play one of the several SKYSTONE/Star Wars sounds loaded into the SDK. + * It does this by creating a simple "chooser" controlled by the gamepad Up Down buttons. + * This code also prevents sounds from stacking up by setting a "playing" flag, which is cleared when the sound finishes playing. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * Operation: + * Use the DPAD to change the selected sound, and the Right Bumper to play it. + */ + +@TeleOp(name="SKYSTONE Sounds", group="Concept") +@Disabled +public class ConceptSoundsSKYSTONE extends LinearOpMode { + + // List of available sound resources + String sounds[] = {"ss_alarm", "ss_bb8_down", "ss_bb8_up", "ss_darth_vader", "ss_fly_by", + "ss_mf_fail", "ss_laser", "ss_laser_burst", "ss_light_saber", "ss_light_saber_long", "ss_light_saber_short", + "ss_light_speed", "ss_mine", "ss_power_up", "ss_r2d2_up", "ss_roger_roger", "ss_siren", "ss_wookie" }; + boolean soundPlaying = false; + + @Override + public void runOpMode() { + + // Variables for choosing from the available sounds + int soundIndex = 0; + int soundID = -1; + boolean was_dpad_up = false; + boolean was_dpad_down = false; + + Context myApp = hardwareMap.appContext; + + // create a sound parameter that holds the desired player parameters. + SoundPlayer.PlaySoundParams params = new SoundPlayer.PlaySoundParams(); + params.loopControl = 0; + params.waitForNonLoopingSoundsToFinish = true; + + // In this sample, we will skip waiting for the user to press play, and start displaying sound choices right away + while (!isStopRequested()) { + + // Look for DPAD presses to change the selection + if (gamepad1.dpad_down && !was_dpad_down) { + // Go to next sound (with list wrap) and display it + soundIndex = (soundIndex + 1) % sounds.length; + } + + if (gamepad1.dpad_up && !was_dpad_up) { + // Go to previous sound (with list wrap) and display it + soundIndex = (soundIndex + sounds.length - 1) % sounds.length; + } + + // Look for trigger to see if we should play sound + // Only start a new sound if we are currently not playing one. + if (gamepad1.right_bumper && !soundPlaying) { + + // Determine Resource IDs for the sounds you want to play, and make sure it's valid. + if ((soundID = myApp.getResources().getIdentifier(sounds[soundIndex], "raw", myApp.getPackageName())) != 0){ + + // Signal that the sound is now playing. + soundPlaying = true; + + // Start playing, and also Create a callback that will clear the playing flag when the sound is complete. + SoundPlayer.getInstance().startPlaying(myApp, soundID, params, null, + new Runnable() { + public void run() { + soundPlaying = false; + }} ); + } + } + + // Remember the last state of the dpad to detect changes. + was_dpad_up = gamepad1.dpad_up; + was_dpad_down = gamepad1.dpad_down; + + // Display the current sound choice, and the playing status. + telemetry.addData("", "Use DPAD up/down to choose sound."); + telemetry.addData("", "Press Right Bumper to play sound."); + telemetry.addData("", ""); + telemetry.addData("Sound >", sounds[soundIndex]); + telemetry.addData("Status >", soundPlaying ? "Playing" : "Stopped"); + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTelemetry.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTelemetry.java new file mode 100644 index 0000000..f2c6097 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTelemetry.java @@ -0,0 +1,177 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.VoltageSensor; +import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.Func; +import org.firstinspires.ftc.robotcore.external.Telemetry; + +/* + * This OpMode illustrates various ways in which telemetry can be + * transmitted from the robot controller to the driver station. The sample illustrates + * numeric and text data, formatted output, and optimized evaluation of expensive-to-acquire + * information. The telemetry log is illustrated by scrolling a poem + * to the driver station. + * + * Also see the Telemetry javadocs. + */ +@TeleOp(name = "Concept: Telemetry", group = "Concept") +@Disabled +public class ConceptTelemetry extends LinearOpMode { + /** Keeps track of the line of the poem which is to be emitted next */ + int poemLine = 0; + + /** Keeps track of how long it's been since we last emitted a line of poetry */ + ElapsedTime poemElapsed = new ElapsedTime(); + + static final String[] poem = new String[] { + + "Mary had a little lamb,", + "His fleece was white as snow,", + "And everywhere that Mary went,", + "The lamb was sure to go.", + "", + "He followed her to school one day,", + "Which was against the rule,", + "It made the children laugh and play", + "To see a lamb at school.", + "", + "And so the teacher turned it out,", + "But still it lingered near,", + "And waited patiently about,", + "Till Mary did appear.", + "", + "\"Why does the lamb love Mary so?\"", + "The eager children cry.", + "\"Why, Mary loves the lamb, you know,\"", + "The teacher did reply.", + "", + "" + }; + + @Override public void runOpMode() { + + /* we keep track of how long it's been since the OpMode was started, just + * to have some interesting data to show */ + ElapsedTime opmodeRunTime = new ElapsedTime(); + + // We show the log in oldest-to-newest order, as that's better for poetry + telemetry.log().setDisplayOrder(Telemetry.Log.DisplayOrder.OLDEST_FIRST); + // We can control the number of lines shown in the log + telemetry.log().setCapacity(6); + // The interval between lines of poetry, in seconds + double sPoemInterval = 0.6; + + /* + * Wait until we've been given the ok to go. For something to do, we emit the + * elapsed time as we sit here and wait. If we didn't want to do anything while + * we waited, we would just call waitForStart(). + */ + while (!isStarted()) { + telemetry.addData("time", "%.1f seconds", opmodeRunTime.seconds()); + telemetry.update(); + idle(); + } + + // Ok, we've been given the ok to go + + /* + * As an illustration, the first line on our telemetry display will display the battery voltage. + * The idea here is that it's expensive to compute the voltage (at least for purposes of illustration) + * so you don't want to do it unless the data is _actually_ going to make it to the + * driver station (recall that telemetry transmission is throttled to reduce bandwidth use. + * Note that getBatteryVoltage() below returns 'Infinity' if there's no voltage sensor attached. + * + * @see Telemetry#getMsTransmissionInterval() + */ + telemetry.addData("voltage", "%.1f volts", new Func() { + @Override public Double value() { + return getBatteryVoltage(); + } + }); + + // Reset to keep some timing stats for the post-'start' part of the OpMode + opmodeRunTime.reset(); + int loopCount = 1; + + // Go go gadget robot! + while (opModeIsActive()) { + + // Emit poetry if it's been a while + if (poemElapsed.seconds() > sPoemInterval) { + emitPoemLine(); + } + + // As an illustration, show some loop timing information + telemetry.addData("loop count", loopCount); + telemetry.addData("ms/loop", "%.3f ms", opmodeRunTime.milliseconds() / loopCount); + + // Show joystick information as some other illustrative data + telemetry.addLine("left joystick | ") + .addData("x", gamepad1.left_stick_x) + .addData("y", gamepad1.left_stick_y); + telemetry.addLine("right joystick | ") + .addData("x", gamepad1.right_stick_x) + .addData("y", gamepad1.right_stick_y); + + /* + * Transmit the telemetry to the driver station, subject to throttling. + * See the documentation for Telemetry.getMsTransmissionInterval() for more information. + */ + telemetry.update(); + + // Update loop info + loopCount++; + } + } + + // emits a line of poetry to the telemetry log + void emitPoemLine() { + telemetry.log().add(poem[poemLine]); + poemLine = (poemLine+1) % poem.length; + poemElapsed.reset(); + } + + // Computes the current battery voltage + double getBatteryVoltage() { + double result = Double.POSITIVE_INFINITY; + for (VoltageSensor sensor : hardwareMap.voltageSensor) { + double voltage = sensor.getVoltage(); + if (voltage > 0) { + result = Math.min(result, voltage); + } + } + return result; + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetection.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetection.java new file mode 100644 index 0000000..f8e3688 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetection.java @@ -0,0 +1,199 @@ +/* Copyright (c) 2019 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import android.util.Size; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.tfod.Recognition; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.tfod.TfodProcessor; + +import java.util.List; + +/* + * This OpMode illustrates the basics of TensorFlow Object Detection, + * including Java Builder structures for specifying Vision parameters. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Concept: TensorFlow Object Detection", group = "Concept") +@Disabled +public class ConceptTensorFlowObjectDetection extends LinearOpMode { + + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + // TFOD_MODEL_ASSET points to a model file stored in the project Asset location, + // this is only used for Android Studio when using models in Assets. + private static final String TFOD_MODEL_ASSET = "MyModelStoredAsAsset.tflite"; + // TFOD_MODEL_FILE points to a model file stored onboard the Robot Controller's storage, + // this is used when uploading models directly to the RC using the model upload interface. + private static final String TFOD_MODEL_FILE = "/sdcard/FIRST/tflitemodels/myCustomModel.tflite"; + // Define the labels recognized in the model for TFOD (must be in training order!) + private static final String[] LABELS = { + "Pixel", + }; + + /** + * The variable to store our instance of the TensorFlow Object Detection processor. + */ + private TfodProcessor tfod; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + @Override + public void runOpMode() { + + initTfod(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.update(); + waitForStart(); + + if (opModeIsActive()) { + while (opModeIsActive()) { + + telemetryTfod(); + + // Push telemetry to the Driver Station. + telemetry.update(); + + // Save CPU resources; can resume streaming when needed. + if (gamepad1.dpad_down) { + visionPortal.stopStreaming(); + } else if (gamepad1.dpad_up) { + visionPortal.resumeStreaming(); + } + + // Share the CPU. + sleep(20); + } + } + + // Save more CPU resources when camera is no longer needed. + visionPortal.close(); + + } // end runOpMode() + + /** + * Initialize the TensorFlow Object Detection processor. + */ + private void initTfod() { + + // Create the TensorFlow processor by using a builder. + tfod = new TfodProcessor.Builder() + + // With the following lines commented out, the default TfodProcessor Builder + // will load the default model for the season. To define a custom model to load, + // choose one of the following: + // Use setModelAssetName() if the custom TF Model is built in as an asset (AS only). + // Use setModelFileName() if you have downloaded a custom team model to the Robot Controller. + //.setModelAssetName(TFOD_MODEL_ASSET) + //.setModelFileName(TFOD_MODEL_FILE) + + // The following default settings are available to un-comment and edit as needed to + // set parameters for custom models. + //.setModelLabels(LABELS) + //.setIsModelTensorFlow2(true) + //.setIsModelQuantized(true) + //.setModelInputSize(300) + //.setModelAspectRatio(16.0 / 9.0) + + .build(); + + // Create the vision portal by using a builder. + VisionPortal.Builder builder = new VisionPortal.Builder(); + + // Set the camera (webcam vs. built-in RC phone camera). + if (USE_WEBCAM) { + builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")); + } else { + builder.setCamera(BuiltinCameraDirection.BACK); + } + + // Choose a camera resolution. Not all cameras support all resolutions. + //builder.setCameraResolution(new Size(640, 480)); + + // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. + //builder.enableLiveView(true); + + // Set the stream format; MJPEG uses less bandwidth than default YUY2. + //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); + + // Choose whether or not LiveView stops if no processors are enabled. + // If set "true", monitor shows solid orange screen if no processors enabled. + // If set "false", monitor shows camera view without annotations. + //builder.setAutoStopLiveView(false); + + // Set and enable the processor. + builder.addProcessor(tfod); + + // Build the Vision Portal, using the above settings. + visionPortal = builder.build(); + + // Set confidence threshold for TFOD recognitions, at any time. + //tfod.setMinResultConfidence(0.75f); + + // Disable or re-enable the TFOD processor at any time. + //visionPortal.setProcessorEnabled(tfod, true); + + } // end method initTfod() + + /** + * Add telemetry about TensorFlow Object Detection (TFOD) recognitions. + */ + private void telemetryTfod() { + + List currentRecognitions = tfod.getRecognitions(); + telemetry.addData("# Objects Detected", currentRecognitions.size()); + + // Step through the list of recognitions and display info for each one. + for (Recognition recognition : currentRecognitions) { + double x = (recognition.getLeft() + recognition.getRight()) / 2 ; + double y = (recognition.getTop() + recognition.getBottom()) / 2 ; + + telemetry.addData(""," "); + telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100); + telemetry.addData("- Position", "%.0f / %.0f", x, y); + telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight()); + } // end for() loop + + } // end method telemetryTfod() + +} // end class diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionEasy.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionEasy.java new file mode 100644 index 0000000..5c1b712 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionEasy.java @@ -0,0 +1,142 @@ +/* Copyright (c) 2019 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.tfod.Recognition; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.tfod.TfodProcessor; + +import java.util.List; + +/* + * This OpMode illustrates the basics of TensorFlow Object Detection, using + * the easiest way. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Concept: TensorFlow Object Detection Easy", group = "Concept") +@Disabled +public class ConceptTensorFlowObjectDetectionEasy extends LinearOpMode { + + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + /** + * The variable to store our instance of the TensorFlow Object Detection processor. + */ + private TfodProcessor tfod; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + @Override + public void runOpMode() { + + initTfod(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.update(); + waitForStart(); + + if (opModeIsActive()) { + while (opModeIsActive()) { + + telemetryTfod(); + + // Push telemetry to the Driver Station. + telemetry.update(); + + // Save CPU resources; can resume streaming when needed. + if (gamepad1.dpad_down) { + visionPortal.stopStreaming(); + } else if (gamepad1.dpad_up) { + visionPortal.resumeStreaming(); + } + + // Share the CPU. + sleep(20); + } + } + + // Save more CPU resources when camera is no longer needed. + visionPortal.close(); + + } // end runOpMode() + + /** + * Initialize the TensorFlow Object Detection processor. + */ + private void initTfod() { + + // Create the TensorFlow processor the easy way. + tfod = TfodProcessor.easyCreateWithDefaults(); + + // Create the vision portal the easy way. + if (USE_WEBCAM) { + visionPortal = VisionPortal.easyCreateWithDefaults( + hardwareMap.get(WebcamName.class, "Webcam 1"), tfod); + } else { + visionPortal = VisionPortal.easyCreateWithDefaults( + BuiltinCameraDirection.BACK, tfod); + } + + } // end method initTfod() + + /** + * Add telemetry about TensorFlow Object Detection (TFOD) recognitions. + */ + private void telemetryTfod() { + + List currentRecognitions = tfod.getRecognitions(); + telemetry.addData("# Objects Detected", currentRecognitions.size()); + + // Step through the list of recognitions and display info for each one. + for (Recognition recognition : currentRecognitions) { + double x = (recognition.getLeft() + recognition.getRight()) / 2 ; + double y = (recognition.getTop() + recognition.getBottom()) / 2 ; + + telemetry.addData(""," "); + telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100); + telemetry.addData("- Position", "%.0f / %.0f", x, y); + telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight()); + } // end for() loop + + } // end method telemetryTfod() + +} // end class diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionSwitchableCameras.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionSwitchableCameras.java new file mode 100644 index 0000000..1c0ed77 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionSwitchableCameras.java @@ -0,0 +1,186 @@ +/* Copyright (c) 2020 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.ClassFactory; +import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraName; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.tfod.Recognition; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.VisionPortal.CameraState; +import org.firstinspires.ftc.vision.tfod.TfodProcessor; + +import java.util.List; + +/* + * This OpMode illustrates the basics of TensorFlow Object Detection, using + * two webcams. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Concept: TensorFlow Object Detection Switchable Cameras", group = "Concept") +@Disabled +public class ConceptTensorFlowObjectDetectionSwitchableCameras extends LinearOpMode { + + /** + * Variables used for switching cameras. + */ + private WebcamName webcam1, webcam2; + private boolean oldLeftBumper; + private boolean oldRightBumper; + + /** + * The variable to store our instance of the TensorFlow Object Detection processor. + */ + private TfodProcessor tfod; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + @Override + public void runOpMode() { + + initTfod(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.update(); + waitForStart(); + + if (opModeIsActive()) { + while (opModeIsActive()) { + + telemetryCameraSwitching(); + telemetryTfod(); + + // Push telemetry to the Driver Station. + telemetry.update(); + + // Save CPU resources; can resume streaming when needed. + if (gamepad1.dpad_down) { + visionPortal.stopStreaming(); + } else if (gamepad1.dpad_up) { + visionPortal.resumeStreaming(); + } + + doCameraSwitching(); + + // Share the CPU. + sleep(20); + } + } + + // Save more CPU resources when camera is no longer needed. + visionPortal.close(); + + } // end runOpMode() + + /** + * Initialize the TensorFlow Object Detection processor. + */ + private void initTfod() { + + // Create the TensorFlow processor by using a builder. + tfod = new TfodProcessor.Builder().build(); + + webcam1 = hardwareMap.get(WebcamName.class, "Webcam 1"); + webcam2 = hardwareMap.get(WebcamName.class, "Webcam 2"); + CameraName switchableCamera = ClassFactory.getInstance() + .getCameraManager().nameForSwitchableCamera(webcam1, webcam2); + + // Create the vision portal by using a builder. + visionPortal = new VisionPortal.Builder() + .setCamera(switchableCamera) + .addProcessor(tfod) + .build(); + + } // end method initTfod() + + /** + * Add telemetry about camera switching. + */ + private void telemetryCameraSwitching() { + if (visionPortal.getActiveCamera().equals(webcam1)) { + telemetry.addData("activeCamera", "Webcam 1"); + telemetry.addData("Press RightBumper", "to switch to Webcam 2"); + } else { + telemetry.addData("activeCamera", "Webcam 2"); + telemetry.addData("Press LeftBumper", "to switch to Webcam 1"); + } + } // end method telemetryCameraSwitching() + + /** + * Add telemetry about TensorFlow Object Detection (TFOD) recognitions. + */ + private void telemetryTfod() { + + List currentRecognitions = tfod.getRecognitions(); + telemetry.addData("# Objects Detected", currentRecognitions.size()); + + // Step through the list of recognitions and display info for each one. + for (Recognition recognition : currentRecognitions) { + double x = (recognition.getLeft() + recognition.getRight()) / 2 ; + double y = (recognition.getTop() + recognition.getBottom()) / 2 ; + + telemetry.addData(""," "); + telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100); + telemetry.addData("- Position", "%.0f / %.0f", x, y); + telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight()); + } // end for() loop + + } // end method telemetryTfod() + + /** + * Set the active camera according to input from the gamepad. + */ + private void doCameraSwitching() { + if (visionPortal.getCameraState() == CameraState.STREAMING) { + // If the left bumper is pressed, use Webcam 1. + // If the right bumper is pressed, use Webcam 2. + boolean newLeftBumper = gamepad1.left_bumper; + boolean newRightBumper = gamepad1.right_bumper; + if (newLeftBumper && !oldLeftBumper) { + visionPortal.setActiveCamera(webcam1); + } else if (newRightBumper && !oldRightBumper) { + visionPortal.setActiveCamera(webcam2); + } + oldLeftBumper = newLeftBumper; + oldRightBumper = newRightBumper; + } + } // end method doCameraSwitching() + +} // end class diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByEncoder_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByEncoder_Linear.java new file mode 100644 index 0000000..be2e218 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByEncoder_Linear.java @@ -0,0 +1,187 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +/* + * This OpMode illustrates the concept of driving a path based on encoder counts. + * The code is structured as a LinearOpMode + * + * The code REQUIRES that you DO have encoders on the wheels, + * otherwise you would use: RobotAutoDriveByTime; + * + * This code ALSO requires that the drive Motors have been configured such that a positive + * power command moves them forward, and causes the encoders to count UP. + * + * The desired path in this example is: + * - Drive forward for 48 inches + * - Spin right for 12 Inches + * - Drive Backward for 24 inches + * - Stop and close the claw. + * + * The code is written using a method called: encoderDrive(speed, leftInches, rightInches, timeoutS) + * that performs the actual movement. + * This method assumes that each movement is relative to the last stopping place. + * There are other ways to perform encoder based moves, but this method is probably the simplest. + * This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@Autonomous(name="Robot: Auto Drive By Encoder", group="Robot") +@Disabled +public class RobotAutoDriveByEncoder_Linear extends LinearOpMode { + + /* Declare OpMode members. */ + private DcMotor leftDrive = null; + private DcMotor rightDrive = null; + + private ElapsedTime runtime = new ElapsedTime(); + + // Calculate the COUNTS_PER_INCH for your specific drive train. + // Go to your motor vendor website to determine your motor's COUNTS_PER_MOTOR_REV + // For external drive gearing, set DRIVE_GEAR_REDUCTION as needed. + // For example, use a value of 2.0 for a 12-tooth spur gear driving a 24-tooth spur gear. + // This is gearing DOWN for less speed and more torque. + // For gearing UP, use a gear ratio less than 1.0. Note this will affect the direction of wheel rotation. + static final double COUNTS_PER_MOTOR_REV = 1440 ; // eg: TETRIX Motor Encoder + static final double DRIVE_GEAR_REDUCTION = 1.0 ; // No External Gearing. + static final double WHEEL_DIAMETER_INCHES = 4.0 ; // For figuring circumference + static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / + (WHEEL_DIAMETER_INCHES * 3.1415); + static final double DRIVE_SPEED = 0.6; + static final double TURN_SPEED = 0.5; + + @Override + public void runOpMode() { + + // Initialize the drive system variables. + leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); + rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); + + // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. + // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive. + // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + + leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + // Send telemetry message to indicate successful Encoder reset + telemetry.addData("Starting at", "%7d :%7d", + leftDrive.getCurrentPosition(), + rightDrive.getCurrentPosition()); + telemetry.update(); + + // Wait for the game to start (driver presses PLAY) + waitForStart(); + + // Step through each leg of the path, + // Note: Reverse movement is obtained by setting a negative distance (not speed) + encoderDrive(DRIVE_SPEED, 48, 48, 5.0); // S1: Forward 47 Inches with 5 Sec timeout + encoderDrive(TURN_SPEED, 12, -12, 4.0); // S2: Turn Right 12 Inches with 4 Sec timeout + encoderDrive(DRIVE_SPEED, -24, -24, 4.0); // S3: Reverse 24 Inches with 4 Sec timeout + + telemetry.addData("Path", "Complete"); + telemetry.update(); + sleep(1000); // pause to display final telemetry message. + } + + /* + * Method to perform a relative move, based on encoder counts. + * Encoders are not reset as the move is based on the current position. + * Move will stop if any of three conditions occur: + * 1) Move gets to the desired position + * 2) Move runs out of time + * 3) Driver stops the OpMode running. + */ + public void encoderDrive(double speed, + double leftInches, double rightInches, + double timeoutS) { + int newLeftTarget; + int newRightTarget; + + // Ensure that the OpMode is still active + if (opModeIsActive()) { + + // Determine new target position, and pass to motor controller + newLeftTarget = leftDrive.getCurrentPosition() + (int)(leftInches * COUNTS_PER_INCH); + newRightTarget = rightDrive.getCurrentPosition() + (int)(rightInches * COUNTS_PER_INCH); + leftDrive.setTargetPosition(newLeftTarget); + rightDrive.setTargetPosition(newRightTarget); + + // Turn On RUN_TO_POSITION + leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + // reset the timeout time and start motion. + runtime.reset(); + leftDrive.setPower(Math.abs(speed)); + rightDrive.setPower(Math.abs(speed)); + + // keep looping while we are still active, and there is time left, and both motors are running. + // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits + // its target position, the motion will stop. This is "safer" in the event that the robot will + // always end the motion as soon as possible. + // However, if you require that BOTH motors have finished their moves before the robot continues + // onto the next step, use (isBusy() || isBusy()) in the loop test. + while (opModeIsActive() && + (runtime.seconds() < timeoutS) && + (leftDrive.isBusy() && rightDrive.isBusy())) { + + // Display it for the driver. + telemetry.addData("Running to", " %7d :%7d", newLeftTarget, newRightTarget); + telemetry.addData("Currently at", " at %7d :%7d", + leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition()); + telemetry.update(); + } + + // Stop all motion; + leftDrive.setPower(0); + rightDrive.setPower(0); + + // Turn off RUN_TO_POSITION + leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + sleep(250); // optional pause after each move. + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByGyro_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByGyro_Linear.java new file mode 100644 index 0000000..7885fe4 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByGyro_Linear.java @@ -0,0 +1,429 @@ +/* Copyright (c) 2022 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +/* + * This OpMode illustrates the concept of driving an autonomous path based on Gyro (IMU) heading and encoder counts. + * The code is structured as a LinearOpMode + * + * The path to be followed by the robot is built from a series of drive, turn or pause steps. + * Each step on the path is defined by a single function call, and these can be strung together in any order. + * + * The code REQUIRES that you have encoders on the drive motors, otherwise you should use: RobotAutoDriveByTime; + * + * This code uses the Universal IMU interface so it will work with either the BNO055, or BHI260 IMU. + * To run as written, the Control/Expansion hub should be mounted horizontally on a flat part of the robot chassis. + * The REV Logo should be facing UP, and the USB port should be facing forward. + * If this is not the configuration of your REV Control Hub, then the code should be modified to reflect the correct orientation. + * + * This sample requires that the drive Motors have been configured with names : left_drive and right_drive. + * It also requires that a positive power command moves both motors forward, and causes the encoders to count UP. + * So please verify that both of your motors move the robot forward on the first move. If not, make the required correction. + * See the beginning of runOpMode() to set the FORWARD/REVERSE option for each motor. + * + * This code uses RUN_TO_POSITION mode for driving straight, and RUN_USING_ENCODER mode for turning and holding. + * Note: This code implements the requirement of calling setTargetPosition() at least once before switching to RUN_TO_POSITION mode. + * + * Notes: + * + * All angles are referenced to the coordinate-frame that is set whenever resetHeading() is called. + * In this sample, the heading is reset when the Start button is touched on the Driver station. + * Note: It would be possible to reset the heading after each move, but this would accumulate steering errors. + * + * The angle of movement/rotation is assumed to be a standardized rotation around the robot Z axis, + * which means that a Positive rotation is Counter Clockwise, looking down on the field. + * This is consistent with the FTC field coordinate conventions set out in the document: + * https://ftc-docs.firstinspires.org/field-coordinate-system + * + * Control Approach. + * + * To reach, or maintain a required heading, this code implements a basic Proportional Controller where: + * + * Steering power = Heading Error * Proportional Gain. + * + * "Heading Error" is calculated by taking the difference between the desired heading and the actual heading, + * and then "normalizing" it by converting it to a value in the +/- 180 degree range. + * + * "Proportional Gain" is a constant that YOU choose to set the "strength" of the steering response. + * + * Use Android Studio to Copy this Class, and Paste it into your "TeamCode" folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@Autonomous(name="Robot: Auto Drive By Gyro", group="Robot") +@Disabled +public class RobotAutoDriveByGyro_Linear extends LinearOpMode { + + /* Declare OpMode members. */ + private DcMotor leftDrive = null; + private DcMotor rightDrive = null; + private IMU imu = null; // Control/Expansion Hub IMU + + private double headingError = 0; + + // These variable are declared here (as class members) so they can be updated in various methods, + // but still be displayed by sendTelemetry() + private double targetHeading = 0; + private double driveSpeed = 0; + private double turnSpeed = 0; + private double leftSpeed = 0; + private double rightSpeed = 0; + private int leftTarget = 0; + private int rightTarget = 0; + + // Calculate the COUNTS_PER_INCH for your specific drive train. + // Go to your motor vendor website to determine your motor's COUNTS_PER_MOTOR_REV + // For external drive gearing, set DRIVE_GEAR_REDUCTION as needed. + // For example, use a value of 2.0 for a 12-tooth spur gear driving a 24-tooth spur gear. + // This is gearing DOWN for less speed and more torque. + // For gearing UP, use a gear ratio less than 1.0. Note this will affect the direction of wheel rotation. + static final double COUNTS_PER_MOTOR_REV = 537.7 ; // eg: GoBILDA 312 RPM Yellow Jacket + static final double DRIVE_GEAR_REDUCTION = 1.0 ; // No External Gearing. + static final double WHEEL_DIAMETER_INCHES = 4.0 ; // For figuring circumference + static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / + (WHEEL_DIAMETER_INCHES * 3.1415); + + // These constants define the desired driving/control characteristics + // They can/should be tweaked to suit the specific robot drive train. + static final double DRIVE_SPEED = 0.4; // Max driving speed for better distance accuracy. + static final double TURN_SPEED = 0.2; // Max Turn speed to limit turn rate + static final double HEADING_THRESHOLD = 1.0 ; // How close must the heading get to the target before moving to next step. + // Requiring more accuracy (a smaller number) will often make the turn take longer to get into the final position. + // Define the Proportional control coefficient (or GAIN) for "heading control". + // We define one value when Turning (larger errors), and the other is used when Driving straight (smaller errors). + // Increase these numbers if the heading does not corrects strongly enough (eg: a heavy robot or using tracks) + // Decrease these numbers if the heading does not settle on the correct value (eg: very agile robot with omni wheels) + static final double P_TURN_GAIN = 0.02; // Larger is more responsive, but also less stable + static final double P_DRIVE_GAIN = 0.03; // Larger is more responsive, but also less stable + + + @Override + public void runOpMode() { + + // Initialize the drive system variables. + leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); + rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); + + // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. + // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive. + // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + + /* The next two lines define Hub orientation. + * The Default Orientation (shown) is when a hub is mounted horizontally with the printed logo pointing UP and the USB port pointing FORWARD. + * + * To Do: EDIT these two lines to match YOUR mounting configuration. + */ + RevHubOrientationOnRobot.LogoFacingDirection logoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; + RevHubOrientationOnRobot.UsbFacingDirection usbDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; + RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection); + + // Now initialize the IMU with this mounting orientation + // This sample expects the IMU to be in a REV Hub and named "imu". + imu = hardwareMap.get(IMU.class, "imu"); + imu.initialize(new IMU.Parameters(orientationOnRobot)); + + // Ensure the robot is stationary. Reset the encoders and set the motors to BRAKE mode + leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + leftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + // Wait for the game to start (Display Gyro value while waiting) + while (opModeInInit()) { + telemetry.addData(">", "Robot Heading = %4.0f", getHeading()); + telemetry.update(); + } + + // Set the encoders for closed loop speed control, and reset the heading. + leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + imu.resetYaw(); + + // Step through each leg of the path, + // Notes: Reverse movement is obtained by setting a negative distance (not speed) + // holdHeading() is used after turns to let the heading stabilize + // Add a sleep(2000) after any step to keep the telemetry data visible for review + + driveStraight(DRIVE_SPEED, 24.0, 0.0); // Drive Forward 24" + turnToHeading( TURN_SPEED, -45.0); // Turn CW to -45 Degrees + holdHeading( TURN_SPEED, -45.0, 0.5); // Hold -45 Deg heading for a 1/2 second + + driveStraight(DRIVE_SPEED, 17.0, -45.0); // Drive Forward 17" at -45 degrees (12"x and 12"y) + turnToHeading( TURN_SPEED, 45.0); // Turn CCW to 45 Degrees + holdHeading( TURN_SPEED, 45.0, 0.5); // Hold 45 Deg heading for a 1/2 second + + driveStraight(DRIVE_SPEED, 17.0, 45.0); // Drive Forward 17" at 45 degrees (-12"x and 12"y) + turnToHeading( TURN_SPEED, 0.0); // Turn CW to 0 Degrees + holdHeading( TURN_SPEED, 0.0, 1.0); // Hold 0 Deg heading for 1 second + + driveStraight(DRIVE_SPEED,-48.0, 0.0); // Drive in Reverse 48" (should return to approx. staring position) + + telemetry.addData("Path", "Complete"); + telemetry.update(); + sleep(1000); // Pause to display last telemetry message. + } + + /* + * ==================================================================================================== + * Driving "Helper" functions are below this line. + * These provide the high and low level methods that handle driving straight and turning. + * ==================================================================================================== + */ + + // ********** HIGH Level driving functions. ******************** + + /** + * Drive in a straight line, on a fixed compass heading (angle), based on encoder counts. + * Move will stop if either of these conditions occur: + * 1) Move gets to the desired position + * 2) Driver stops the OpMode running. + * + * @param maxDriveSpeed MAX Speed for forward/rev motion (range 0 to +1.0) . + * @param distance Distance (in inches) to move from current position. Negative distance means move backward. + * @param heading Absolute Heading Angle (in Degrees) relative to last gyro reset. + * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward. + * If a relative angle is required, add/subtract from the current robotHeading. + */ + public void driveStraight(double maxDriveSpeed, + double distance, + double heading) { + + // Ensure that the OpMode is still active + if (opModeIsActive()) { + + // Determine new target position, and pass to motor controller + int moveCounts = (int)(distance * COUNTS_PER_INCH); + leftTarget = leftDrive.getCurrentPosition() + moveCounts; + rightTarget = rightDrive.getCurrentPosition() + moveCounts; + + // Set Target FIRST, then turn on RUN_TO_POSITION + leftDrive.setTargetPosition(leftTarget); + rightDrive.setTargetPosition(rightTarget); + + leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + // Set the required driving speed (must be positive for RUN_TO_POSITION) + // Start driving straight, and then enter the control loop + maxDriveSpeed = Math.abs(maxDriveSpeed); + moveRobot(maxDriveSpeed, 0); + + // keep looping while we are still active, and BOTH motors are running. + while (opModeIsActive() && + (leftDrive.isBusy() && rightDrive.isBusy())) { + + // Determine required steering to keep on heading + turnSpeed = getSteeringCorrection(heading, P_DRIVE_GAIN); + + // if driving in reverse, the motor correction also needs to be reversed + if (distance < 0) + turnSpeed *= -1.0; + + // Apply the turning correction to the current driving speed. + moveRobot(driveSpeed, turnSpeed); + + // Display drive status for the driver. + sendTelemetry(true); + } + + // Stop all motion & Turn off RUN_TO_POSITION + moveRobot(0, 0); + leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + } + + /** + * Spin on the central axis to point in a new direction. + *

+ * Move will stop if either of these conditions occur: + *

+ * 1) Move gets to the heading (angle) + *

+ * 2) Driver stops the OpMode running. + * + * @param maxTurnSpeed Desired MAX speed of turn. (range 0 to +1.0) + * @param heading Absolute Heading Angle (in Degrees) relative to last gyro reset. + * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward. + * If a relative angle is required, add/subtract from current heading. + */ + public void turnToHeading(double maxTurnSpeed, double heading) { + + // Run getSteeringCorrection() once to pre-calculate the current error + getSteeringCorrection(heading, P_DRIVE_GAIN); + + // keep looping while we are still active, and not on heading. + while (opModeIsActive() && (Math.abs(headingError) > HEADING_THRESHOLD)) { + + // Determine required steering to keep on heading + turnSpeed = getSteeringCorrection(heading, P_TURN_GAIN); + + // Clip the speed to the maximum permitted value. + turnSpeed = Range.clip(turnSpeed, -maxTurnSpeed, maxTurnSpeed); + + // Pivot in place by applying the turning correction + moveRobot(0, turnSpeed); + + // Display drive status for the driver. + sendTelemetry(false); + } + + // Stop all motion; + moveRobot(0, 0); + } + + /** + * Obtain & hold a heading for a finite amount of time + *

+ * Move will stop once the requested time has elapsed + *

+ * This function is useful for giving the robot a moment to stabilize it's heading between movements. + * + * @param maxTurnSpeed Maximum differential turn speed (range 0 to +1.0) + * @param heading Absolute Heading Angle (in Degrees) relative to last gyro reset. + * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward. + * If a relative angle is required, add/subtract from current heading. + * @param holdTime Length of time (in seconds) to hold the specified heading. + */ + public void holdHeading(double maxTurnSpeed, double heading, double holdTime) { + + ElapsedTime holdTimer = new ElapsedTime(); + holdTimer.reset(); + + // keep looping while we have time remaining. + while (opModeIsActive() && (holdTimer.time() < holdTime)) { + // Determine required steering to keep on heading + turnSpeed = getSteeringCorrection(heading, P_TURN_GAIN); + + // Clip the speed to the maximum permitted value. + turnSpeed = Range.clip(turnSpeed, -maxTurnSpeed, maxTurnSpeed); + + // Pivot in place by applying the turning correction + moveRobot(0, turnSpeed); + + // Display drive status for the driver. + sendTelemetry(false); + } + + // Stop all motion; + moveRobot(0, 0); + } + + // ********** LOW Level driving functions. ******************** + + /** + * Use a Proportional Controller to determine how much steering correction is required. + * + * @param desiredHeading The desired absolute heading (relative to last heading reset) + * @param proportionalGain Gain factor applied to heading error to obtain turning power. + * @return Turning power needed to get to required heading. + */ + public double getSteeringCorrection(double desiredHeading, double proportionalGain) { + targetHeading = desiredHeading; // Save for telemetry + + // Determine the heading current error + headingError = targetHeading - getHeading(); + + // Normalize the error to be within +/- 180 degrees + while (headingError > 180) headingError -= 360; + while (headingError <= -180) headingError += 360; + + // Multiply the error by the gain to determine the required steering correction/ Limit the result to +/- 1.0 + return Range.clip(headingError * proportionalGain, -1, 1); + } + + /** + * Take separate drive (fwd/rev) and turn (right/left) requests, + * combines them, and applies the appropriate speed commands to the left and right wheel motors. + * @param drive forward motor speed + * @param turn clockwise turning motor speed. + */ + public void moveRobot(double drive, double turn) { + driveSpeed = drive; // save this value as a class member so it can be used by telemetry. + turnSpeed = turn; // save this value as a class member so it can be used by telemetry. + + leftSpeed = drive - turn; + rightSpeed = drive + turn; + + // Scale speeds down if either one exceeds +/- 1.0; + double max = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed)); + if (max > 1.0) + { + leftSpeed /= max; + rightSpeed /= max; + } + + leftDrive.setPower(leftSpeed); + rightDrive.setPower(rightSpeed); + } + + /** + * Display the various control parameters while driving + * + * @param straight Set to true if we are driving straight, and the encoder positions should be included in the telemetry. + */ + private void sendTelemetry(boolean straight) { + + if (straight) { + telemetry.addData("Motion", "Drive Straight"); + telemetry.addData("Target Pos L:R", "%7d:%7d", leftTarget, rightTarget); + telemetry.addData("Actual Pos L:R", "%7d:%7d", leftDrive.getCurrentPosition(), + rightDrive.getCurrentPosition()); + } else { + telemetry.addData("Motion", "Turning"); + } + + telemetry.addData("Heading- Target : Current", "%5.2f : %5.0f", targetHeading, getHeading()); + telemetry.addData("Error : Steer Pwr", "%5.1f : %5.1f", headingError, turnSpeed); + telemetry.addData("Wheel Speeds L : R", "%5.2f : %5.2f", leftSpeed, rightSpeed); + telemetry.update(); + } + + /** + * read the Robot heading directly from the IMU (in degrees) + */ + public double getHeading() { + YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles(); + return orientation.getYaw(AngleUnit.DEGREES); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByTime_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByTime_Linear.java new file mode 100644 index 0000000..b449258 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByTime_Linear.java @@ -0,0 +1,128 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +/* + * This OpMode illustrates the concept of driving a path based on time. + * The code is structured as a LinearOpMode + * + * The code assumes that you do NOT have encoders on the wheels, + * otherwise you would use: RobotAutoDriveByEncoder; + * + * The desired path in this example is: + * - Drive forward for 3 seconds + * - Spin right for 1.3 seconds + * - Drive Backward for 1 Second + * + * The code is written in a simple form with no optimizations. + * However, there are several ways that this type of sequence could be streamlined, + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@Autonomous(name="Robot: Auto Drive By Time", group="Robot") +@Disabled +public class RobotAutoDriveByTime_Linear extends LinearOpMode { + + /* Declare OpMode members. */ + private DcMotor leftDrive = null; + private DcMotor rightDrive = null; + + private ElapsedTime runtime = new ElapsedTime(); + + + static final double FORWARD_SPEED = 0.6; + static final double TURN_SPEED = 0.5; + + @Override + public void runOpMode() { + + // Initialize the drive system variables. + leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); + rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); + + // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. + // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive. + // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + + // Send telemetry message to signify robot waiting; + telemetry.addData("Status", "Ready to run"); // + telemetry.update(); + + // Wait for the game to start (driver presses PLAY) + waitForStart(); + + // Step through each leg of the path, ensuring that the Auto mode has not been stopped along the way + + // Step 1: Drive forward for 3 seconds + leftDrive.setPower(FORWARD_SPEED); + rightDrive.setPower(FORWARD_SPEED); + runtime.reset(); + while (opModeIsActive() && (runtime.seconds() < 3.0)) { + telemetry.addData("Path", "Leg 1: %4.1f S Elapsed", runtime.seconds()); + telemetry.update(); + } + + // Step 2: Spin right for 1.3 seconds + leftDrive.setPower(TURN_SPEED); + rightDrive.setPower(-TURN_SPEED); + runtime.reset(); + while (opModeIsActive() && (runtime.seconds() < 1.3)) { + telemetry.addData("Path", "Leg 2: %4.1f S Elapsed", runtime.seconds()); + telemetry.update(); + } + + // Step 3: Drive Backward for 1 Second + leftDrive.setPower(-FORWARD_SPEED); + rightDrive.setPower(-FORWARD_SPEED); + runtime.reset(); + while (opModeIsActive() && (runtime.seconds() < 1.0)) { + telemetry.addData("Path", "Leg 3: %4.1f S Elapsed", runtime.seconds()); + telemetry.update(); + } + + // Step 4: Stop + leftDrive.setPower(0); + rightDrive.setPower(0); + + telemetry.addData("Path", "Complete"); + telemetry.update(); + sleep(1000); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java new file mode 100644 index 0000000..21def8a --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java @@ -0,0 +1,321 @@ +/* Copyright (c) 2023 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.Range; +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.ExposureControl; +import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.GainControl; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; +import java.util.concurrent.TimeUnit; + +/* + * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag. + * The code assumes a Holonomic (Mecanum or X Drive) Robot. + * + * For an introduction to AprilTags, see the ftc-docs link below: + * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html + * + * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera. + * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below. + * https://ftc-docs.firstinspires.org/apriltag-detection-values + * + * The drive goal is to rotate to keep the Tag centered in the camera, while strafing to be directly in front of the tag, and + * driving towards the tag to achieve the desired distance. + * To reduce any motion blur (which will interrupt the detection process) the Camera exposure is reduced to a very low value (5mS) + * You can determine the best Exposure and Gain values by using the ConceptAprilTagOptimizeExposure OpMode in this Samples folder. + * + * The code assumes a Robot Configuration with motors named: leftfront_drive and rightfront_drive, leftback_drive and rightback_drive. + * The motor directions must be set so a positive power goes forward on all wheels. + * This sample assumes that the current game AprilTag Library (usually for the current season) is being loaded by default, + * so you should choose to approach a valid tag ID (usually starting at 0) + * + * Under manual control, the left stick will move forward/back & left/right. The right stick will rotate the robot. + * Manually drive the robot until it displays Target data on the Driver Station. + * + * Press and hold the *Left Bumper* to enable the automatic "Drive to target" mode. + * Release the Left Bumper to return to manual driving mode. + * + * Under "Drive To Target" mode, the robot has three goals: + * 1) Turn the robot to always keep the Tag centered on the camera frame. (Use the Target Bearing to turn the robot.) + * 2) Strafe the robot towards the centerline of the Tag, so it approaches directly in front of the tag. (Use the Target Yaw to strafe the robot) + * 3) Drive towards the Tag to get to the desired distance. (Use Tag Range to drive the robot forward/backward) + * + * Use DESIRED_DISTANCE to set how close you want the robot to get to the target. + * Speed and Turn sensitivity can be adjusted using the SPEED_GAIN, STRAFE_GAIN and TURN_GAIN constants. + * + * Use Android Studio to Copy this Class, and Paste it into the TeamCode/src/main/java/org/firstinspires/ftc/teamcode folder. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + * + */ + +@TeleOp(name="Omni Drive To AprilTag", group = "Concept") +@Disabled +public class RobotAutoDriveToAprilTagOmni extends LinearOpMode +{ + // Adjust these numbers to suit your robot. + final double DESIRED_DISTANCE = 12.0; // this is how close the camera should get to the target (inches) + + // Set the GAIN constants to control the relationship between the measured position error, and how much power is + // applied to the drive motors to correct the error. + // Drive = Error * Gain Make these values smaller for smoother control, or larger for a more aggressive response. + final double SPEED_GAIN = 0.02 ; // Forward Speed Control "Gain". eg: Ramp up to 50% power at a 25 inch error. (0.50 / 25.0) + final double STRAFE_GAIN = 0.015 ; // Strafe Speed Control "Gain". eg: Ramp up to 25% power at a 25 degree Yaw error. (0.25 / 25.0) + final double TURN_GAIN = 0.01 ; // Turn Control "Gain". eg: Ramp up to 25% power at a 25 degree error. (0.25 / 25.0) + + final double MAX_AUTO_SPEED = 0.5; // Clip the approach speed to this max value (adjust for your robot) + final double MAX_AUTO_STRAFE= 0.5; // Clip the approach speed to this max value (adjust for your robot) + final double MAX_AUTO_TURN = 0.3; // Clip the turn speed to this max value (adjust for your robot) + + private DcMotor leftFrontDrive = null; // Used to control the left front drive wheel + private DcMotor rightFrontDrive = null; // Used to control the right front drive wheel + private DcMotor leftBackDrive = null; // Used to control the left back drive wheel + private DcMotor rightBackDrive = null; // Used to control the right back drive wheel + + private static final boolean USE_WEBCAM = true; // Set true to use a webcam, or false for a phone camera + private static final int DESIRED_TAG_ID = -1; // Choose the tag you want to approach or set to -1 for ANY tag. + private VisionPortal visionPortal; // Used to manage the video source. + private AprilTagProcessor aprilTag; // Used for managing the AprilTag detection process. + private AprilTagDetection desiredTag = null; // Used to hold the data for a detected AprilTag + + @Override public void runOpMode() + { + boolean targetFound = false; // Set to true when an AprilTag target is detected + double drive = 0; // Desired forward power/speed (-1 to +1) + double strafe = 0; // Desired strafe power/speed (-1 to +1) + double turn = 0; // Desired turning power/speed (-1 to +1) + + // Initialize the Apriltag Detection process + initAprilTag(); + + // Initialize the hardware variables. Note that the strings used here as parameters + // to 'get' must match the names assigned during the robot configuration. + // step (using the FTC Robot Controller app on the phone). + leftFrontDrive = hardwareMap.get(DcMotor.class, "leftfront_drive"); + rightFrontDrive = hardwareMap.get(DcMotor.class, "rightfront_drive"); + leftBackDrive = hardwareMap.get(DcMotor.class, "leftback_drive"); + rightBackDrive = hardwareMap.get(DcMotor.class, "rightback_drive"); + + // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. + // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive. + // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips + leftFrontDrive.setDirection(DcMotor.Direction.REVERSE); + leftBackDrive.setDirection(DcMotor.Direction.REVERSE); + rightFrontDrive.setDirection(DcMotor.Direction.FORWARD); + rightBackDrive.setDirection(DcMotor.Direction.FORWARD); + + if (USE_WEBCAM) + setManualExposure(6, 250); // Use low exposure time to reduce motion blur + + // Wait for driver to press start + telemetry.addData("Camera preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.update(); + waitForStart(); + + while (opModeIsActive()) + { + targetFound = false; + desiredTag = null; + + // Step through the list of detected tags and look for a matching tag + List currentDetections = aprilTag.getDetections(); + for (AprilTagDetection detection : currentDetections) { + // Look to see if we have size info on this tag. + if (detection.metadata != null) { + // Check to see if we want to track towards this tag. + if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) { + // Yes, we want to use this tag. + targetFound = true; + desiredTag = detection; + break; // don't look any further. + } else { + // This tag is in the library, but we do not want to track it right now. + telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id); + } + } else { + // This tag is NOT in the library, so we don't have enough information to track to it. + telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", detection.id); + } + } + + // Tell the driver what we see, and what to do. + if (targetFound) { + telemetry.addData("\n>","HOLD Left-Bumper to Drive to Target\n"); + telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name); + telemetry.addData("Range", "%5.1f inches", desiredTag.ftcPose.range); + telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing); + telemetry.addData("Yaw","%3.0f degrees", desiredTag.ftcPose.yaw); + } else { + telemetry.addData("\n>","Drive using joysticks to find valid target\n"); + } + + // If Left Bumper is being pressed, AND we have found the desired target, Drive to target Automatically . + if (gamepad1.left_bumper && targetFound) { + + // Determine heading, range and Yaw (tag image rotation) error so we can use them to control the robot automatically. + double rangeError = (desiredTag.ftcPose.range - DESIRED_DISTANCE); + double headingError = desiredTag.ftcPose.bearing; + double yawError = desiredTag.ftcPose.yaw; + + // Use the speed and turn "gains" to calculate how we want the robot to move. + drive = Range.clip(rangeError * SPEED_GAIN, -MAX_AUTO_SPEED, MAX_AUTO_SPEED); + turn = Range.clip(headingError * TURN_GAIN, -MAX_AUTO_TURN, MAX_AUTO_TURN) ; + strafe = Range.clip(-yawError * STRAFE_GAIN, -MAX_AUTO_STRAFE, MAX_AUTO_STRAFE); + + telemetry.addData("Auto","Drive %5.2f, Strafe %5.2f, Turn %5.2f ", drive, strafe, turn); + } else { + + // drive using manual POV Joystick mode. Slow things down to make the robot more controlable. + drive = -gamepad1.left_stick_y / 2.0; // Reduce drive rate to 50%. + strafe = -gamepad1.left_stick_x / 2.0; // Reduce strafe rate to 50%. + turn = -gamepad1.right_stick_x / 3.0; // Reduce turn rate to 33%. + telemetry.addData("Manual","Drive %5.2f, Strafe %5.2f, Turn %5.2f ", drive, strafe, turn); + } + telemetry.update(); + + // Apply desired axes motions to the drivetrain. + moveRobot(drive, strafe, turn); + sleep(10); + } + } + + /** + * Move robot according to desired axes motions + *

+ * Positive X is forward + *

+ * Positive Y is strafe left + *

+ * Positive Yaw is counter-clockwise + */ + public void moveRobot(double x, double y, double yaw) { + // Calculate wheel powers. + double leftFrontPower = x -y -yaw; + double rightFrontPower = x +y +yaw; + double leftBackPower = x +y -yaw; + double rightBackPower = x -y +yaw; + + // Normalize wheel powers to be less than 1.0 + double max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)); + max = Math.max(max, Math.abs(leftBackPower)); + max = Math.max(max, Math.abs(rightBackPower)); + + if (max > 1.0) { + leftFrontPower /= max; + rightFrontPower /= max; + leftBackPower /= max; + rightBackPower /= max; + } + + // Send powers to the wheels. + leftFrontDrive.setPower(leftFrontPower); + rightFrontDrive.setPower(rightFrontPower); + leftBackDrive.setPower(leftBackPower); + rightBackDrive.setPower(rightBackPower); + } + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + // Create the AprilTag processor by using a builder. + aprilTag = new AprilTagProcessor.Builder().build(); + + // Adjust Image Decimation to trade-off detection-range for detection-rate. + // eg: Some typical detection data using a Logitech C920 WebCam + // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second + // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second + // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second + // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second + // Note: Decimation can be changed on-the-fly to adapt during a match. + aprilTag.setDecimation(2); + + // Create the vision portal by using a builder. + if (USE_WEBCAM) { + visionPortal = new VisionPortal.Builder() + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .addProcessor(aprilTag) + .build(); + } else { + visionPortal = new VisionPortal.Builder() + .setCamera(BuiltinCameraDirection.BACK) + .addProcessor(aprilTag) + .build(); + } + } + + /* + Manually set the camera gain and exposure. + This can only be called AFTER calling initAprilTag(), and only works for Webcams; + */ + private void setManualExposure(int exposureMS, int gain) { + // Wait for the camera to be open, then use the controls + + if (visionPortal == null) { + return; + } + + // Make sure camera is streaming before we try to set the exposure controls + if (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING) { + telemetry.addData("Camera", "Waiting"); + telemetry.update(); + while (!isStopRequested() && (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING)) { + sleep(20); + } + telemetry.addData("Camera", "Ready"); + telemetry.update(); + } + + // Set camera controls unless we are stopping. + if (!isStopRequested()) + { + ExposureControl exposureControl = visionPortal.getCameraControl(ExposureControl.class); + if (exposureControl.getMode() != ExposureControl.Mode.Manual) { + exposureControl.setMode(ExposureControl.Mode.Manual); + sleep(50); + } + exposureControl.setExposure((long)exposureMS, TimeUnit.MILLISECONDS); + sleep(20); + GainControl gainControl = visionPortal.getCameraControl(GainControl.class); + gainControl.setGain(gain); + sleep(20); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java new file mode 100644 index 0000000..58bbaa6 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java @@ -0,0 +1,298 @@ +/* Copyright (c) 2023 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.Range; +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.ExposureControl; +import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.GainControl; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; +import java.util.concurrent.TimeUnit; + +/* + * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag. + * The code assumes a basic two-wheel (Tank) Robot Drivetrain + * + * For an introduction to AprilTags, see the ftc-docs link below: + * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html + * + * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera. + * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below. + * https://ftc-docs.firstinspires.org/apriltag-detection-values + * + * The driving goal is to rotate to keep the tag centered in the camera, while driving towards the tag to achieve the desired distance. + * To reduce any motion blur (which will interrupt the detection process) the Camera exposure is reduced to a very low value (5mS) + * You can determine the best exposure and gain values by using the ConceptAprilTagOptimizeExposure OpMode in this Samples folder. + * + * The code assumes a Robot Configuration with motors named left_drive and right_drive. + * The motor directions must be set so a positive power goes forward on both wheels; + * This sample assumes that the default AprilTag Library (usually for the current season) is being loaded by default + * so you should choose to approach a valid tag ID (usually starting at 0) + * + * Under manual control, the left stick will move forward/back, and the right stick will rotate the robot. + * This is called POV Joystick mode, different than Tank Drive (where each joystick controls a wheel). + * + * Manually drive the robot until it displays Target data on the Driver Station. + * Press and hold the *Left Bumper* to enable the automatic "Drive to target" mode. + * Release the Left Bumper to return to manual driving mode. + * + * Under "Drive To Target" mode, the robot has two goals: + * 1) Turn the robot to always keep the Tag centered on the camera frame. (Use the Target Bearing to turn the robot.) + * 2) Drive towards the Tag to get to the desired distance. (Use Tag Range to drive the robot forward/backward) + * + * Use DESIRED_DISTANCE to set how close you want the robot to get to the target. + * Speed and Turn sensitivity can be adjusted using the SPEED_GAIN and TURN_GAIN constants. + * + * Use Android Studio to Copy this Class, and Paste it into the TeamCode/src/main/java/org/firstinspires/ftc/teamcode folder. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + * + */ + +@TeleOp(name="Tank Drive To AprilTag", group = "Concept") +@Disabled +public class RobotAutoDriveToAprilTagTank extends LinearOpMode +{ + // Adjust these numbers to suit your robot. + final double DESIRED_DISTANCE = 12.0; // this is how close the camera should get to the target (inches) + + // Set the GAIN constants to control the relationship between the measured position error, and how much power is + // applied to the drive motors to correct the error. + // Drive = Error * Gain Make these values smaller for smoother control, or larger for a more aggressive response. + final double SPEED_GAIN = 0.02 ; // Speed Control "Gain". eg: Ramp up to 50% power at a 25 inch error. (0.50 / 25.0) + final double TURN_GAIN = 0.01 ; // Turn Control "Gain". eg: Ramp up to 25% power at a 25 degree error. (0.25 / 25.0) + + final double MAX_AUTO_SPEED = 0.5; // Clip the approach speed to this max value (adjust for your robot) + final double MAX_AUTO_TURN = 0.25; // Clip the turn speed to this max value (adjust for your robot) + + private DcMotor leftDrive = null; // Used to control the left drive wheel + private DcMotor rightDrive = null; // Used to control the right drive wheel + + private static final boolean USE_WEBCAM = true; // Set true to use a webcam, or false for a phone camera + private static final int DESIRED_TAG_ID = -1; // Choose the tag you want to approach or set to -1 for ANY tag. + private VisionPortal visionPortal; // Used to manage the video source. + private AprilTagProcessor aprilTag; // Used for managing the AprilTag detection process. + private AprilTagDetection desiredTag = null; // Used to hold the data for a detected AprilTag + + @Override public void runOpMode() + { + boolean targetFound = false; // Set to true when an AprilTag target is detected + double drive = 0; // Desired forward power/speed (-1 to +1) +ve is forward + double turn = 0; // Desired turning power/speed (-1 to +1) +ve is CounterClockwise + + // Initialize the Apriltag Detection process + initAprilTag(); + + // Initialize the hardware variables. Note that the strings used here as parameters + // to 'get' must match the names assigned during the robot configuration. + // step (using the FTC Robot Controller app on the phone). + leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); + rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); + + // To drive forward, most robots need the motor on one side to be reversed because the axles point in opposite directions. + // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive. + // Note: The settings here assume direct drive on left and right wheels. Single Gear Reduction or 90 Deg drives may require direction flips + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + + if (USE_WEBCAM) + setManualExposure(6, 250); // Use low exposure time to reduce motion blur + + // Wait for the driver to press Start + telemetry.addData("Camera preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.update(); + waitForStart(); + + while (opModeIsActive()) + { + targetFound = false; + desiredTag = null; + + // Step through the list of detected tags and look for a matching tag + List currentDetections = aprilTag.getDetections(); + for (AprilTagDetection detection : currentDetections) { + // Look to see if we have size info on this tag. + if (detection.metadata != null) { + // Check to see if we want to track towards this tag. + if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) { + // Yes, we want to use this tag. + targetFound = true; + desiredTag = detection; + break; // don't look any further. + } else { + // This tag is in the library, but we do not want to track it right now. + telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id); + } + } else { + // This tag is NOT in the library, so we don't have enough information to track to it. + telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", detection.id); + } + } + + // Tell the driver what we see, and what to do. + if (targetFound) { + telemetry.addData("\n>","HOLD Left-Bumper to Drive to Target\n"); + telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name); + telemetry.addData("Range", "%5.1f inches", desiredTag.ftcPose.range); + telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing); + } else { + telemetry.addData("\n>","Drive using joysticks to find valid target\n"); + } + + // If Left Bumper is being pressed, AND we have found the desired target, Drive to target Automatically . + if (gamepad1.left_bumper && targetFound) { + + // Determine heading and range error so we can use them to control the robot automatically. + double rangeError = (desiredTag.ftcPose.range - DESIRED_DISTANCE); + double headingError = desiredTag.ftcPose.bearing; + + // Use the speed and turn "gains" to calculate how we want the robot to move. Clip it to the maximum + drive = Range.clip(rangeError * SPEED_GAIN, -MAX_AUTO_SPEED, MAX_AUTO_SPEED); + turn = Range.clip(headingError * TURN_GAIN, -MAX_AUTO_TURN, MAX_AUTO_TURN) ; + + telemetry.addData("Auto","Drive %5.2f, Turn %5.2f", drive, turn); + } else { + + // drive using manual POV Joystick mode. + drive = -gamepad1.left_stick_y / 2.0; // Reduce drive rate to 50%. + turn = -gamepad1.right_stick_x / 4.0; // Reduce turn rate to 25%. + telemetry.addData("Manual","Drive %5.2f, Turn %5.2f", drive, turn); + } + telemetry.update(); + + // Apply desired axes motions to the drivetrain. + moveRobot(drive, turn); + sleep(10); + } + } + + /** + * Move robot according to desired axes motions + *

+ * Positive X is forward + *

+ * Positive Yaw is counter-clockwise + */ + public void moveRobot(double x, double yaw) { + // Calculate left and right wheel powers. + double leftPower = x - yaw; + double rightPower = x + yaw; + + // Normalize wheel powers to be less than 1.0 + double max = Math.max(Math.abs(leftPower), Math.abs(rightPower)); + if (max >1.0) { + leftPower /= max; + rightPower /= max; + } + + // Send powers to the wheels. + leftDrive.setPower(leftPower); + rightDrive.setPower(rightPower); + } + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + // Create the AprilTag processor by using a builder. + aprilTag = new AprilTagProcessor.Builder().build(); + + // Adjust Image Decimation to trade-off detection-range for detection-rate. + // eg: Some typical detection data using a Logitech C920 WebCam + // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second + // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second + // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second + // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second + // Note: Decimation can be changed on-the-fly to adapt during a match. + aprilTag.setDecimation(2); + + // Create the vision portal by using a builder. + if (USE_WEBCAM) { + visionPortal = new VisionPortal.Builder() + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .addProcessor(aprilTag) + .build(); + } else { + visionPortal = new VisionPortal.Builder() + .setCamera(BuiltinCameraDirection.BACK) + .addProcessor(aprilTag) + .build(); + } + } + + /* + Manually set the camera gain and exposure. + This can only be called AFTER calling initAprilTag(), and only works for Webcams; + */ + private void setManualExposure(int exposureMS, int gain) { + // Wait for the camera to be open, then use the controls + + if (visionPortal == null) { + return; + } + + // Make sure camera is streaming before we try to set the exposure controls + if (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING) { + telemetry.addData("Camera", "Waiting"); + telemetry.update(); + while (!isStopRequested() && (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING)) { + sleep(20); + } + telemetry.addData("Camera", "Ready"); + telemetry.update(); + } + + // Set camera controls unless we are stopping. + if (!isStopRequested()) + { + ExposureControl exposureControl = visionPortal.getCameraControl(ExposureControl.class); + if (exposureControl.getMode() != ExposureControl.Mode.Manual) { + exposureControl.setMode(ExposureControl.Mode.Manual); + sleep(50); + } + exposureControl.setExposure((long)exposureMS, TimeUnit.MILLISECONDS); + sleep(20); + GainControl gainControl = visionPortal.getCameraControl(GainControl.class); + gainControl.setGain(gain); + sleep(20); + telemetry.addData("Camera", "Ready"); + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToLine_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToLine_Linear.java new file mode 100644 index 0000000..e5076c0 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToLine_Linear.java @@ -0,0 +1,142 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; +import com.qualcomm.robotcore.hardware.SwitchableLight; + +/* + * This OpMode illustrates the concept of driving up to a line and then stopping. + * The code is structured as a LinearOpMode + * + * The Sensor used here can be a REV Color Sensor V2 or V3. Make sure the white LED is turned on. + * The sensor can be plugged into any I2C port, and must be named "sensor_color" in the active configuration. + * + * Depending on the height of your color sensor, you may want to set the sensor "gain". + * The higher the gain, the greater the reflected light reading will be. + * Use the SensorColor sample in this folder to determine the minimum gain value that provides an + * "Alpha" reading of 1.0 when you are on top of the white line. In this sample, we use a gain of 15 + * which works well with a Rev V2 color sensor + * + * Setting the correct WHITE_THRESHOLD value is key to stopping correctly. + * This should be set halfway between the bare-tile, and white-line "Alpha" values. + * The reflected light value can be read on the screen once the OpMode has been INIT, but before it is STARTED. + * Move the sensor on and off the white line and note the min and max readings. + * Edit this code to make WHITE_THRESHOLD halfway between the min and max. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@Autonomous(name="Robot: Auto Drive To Line", group="Robot") +@Disabled +public class RobotAutoDriveToLine_Linear extends LinearOpMode { + + /* Declare OpMode members. */ + private DcMotor leftDrive = null; + private DcMotor rightDrive = null; + + /** The variable to store a reference to our color sensor hardware object */ + NormalizedColorSensor colorSensor; + + static final double WHITE_THRESHOLD = 0.5; // spans between 0.0 - 1.0 from dark to light + static final double APPROACH_SPEED = 0.25; + + @Override + public void runOpMode() { + + // Initialize the drive system variables. + leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); + rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); + + // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. + // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive. + // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + + // If there are encoders connected, switch to RUN_USING_ENCODER mode for greater accuracy + // leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + // rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + // Get a reference to our sensor object. It's recommended to use NormalizedColorSensor over + // ColorSensor, because NormalizedColorSensor consistently gives values between 0 and 1, while + // the values you get from ColorSensor are dependent on the specific sensor you're using. + colorSensor = hardwareMap.get(NormalizedColorSensor.class, "sensor_color"); + + // If necessary, turn ON the white LED (if there is no LED switch on the sensor) + if (colorSensor instanceof SwitchableLight) { + ((SwitchableLight)colorSensor).enableLight(true); + } + + // Some sensors allow you to set your light sensor gain for optimal sensitivity... + // See the SensorColor sample in this folder for how to determine the optimal gain. + // A gain of 15 causes a Rev Color Sensor V2 to produce an Alpha value of 1.0 at about 1.5" above the floor. + colorSensor.setGain(15); + + // Wait for driver to press PLAY) + // Abort this loop is started or stopped. + while (opModeInInit()) { + + // Send telemetry message to signify robot waiting; + telemetry.addData("Status", "Ready to drive to white line."); // + + // Display the light level while we are waiting to start + getBrightness(); + } + + // Start the robot moving forward, and then begin looking for a white line. + leftDrive.setPower(APPROACH_SPEED); + rightDrive.setPower(APPROACH_SPEED); + + // run until the white line is seen OR the driver presses STOP; + while (opModeIsActive() && (getBrightness() < WHITE_THRESHOLD)) { + sleep(5); + } + + // Stop all motors + leftDrive.setPower(0); + rightDrive.setPower(0); + } + + // to obtain reflected light, read the normalized values from the color sensor. Return the Alpha channel. + double getBrightness() { + NormalizedRGBA colors = colorSensor.getNormalizedColors(); + telemetry.addData("Light Level (0 to 1)", "%4.2f", colors.alpha); + telemetry.update(); + + return colors.alpha; + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotHardware.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotHardware.java new file mode 100644 index 0000000..b1c8de6 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotHardware.java @@ -0,0 +1,167 @@ +/* Copyright (c) 2022 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.Range; + +/* + * This file works in conjunction with the External Hardware Class sample called: ConceptExternalHardwareClass.java + * Please read the explanations in that Sample about how to use this class definition. + * + * This file defines a Java Class that performs all the setup and configuration for a sample robot's hardware (motors and sensors). + * It assumes three motors (left_drive, right_drive and arm) and two servos (left_hand and right_hand) + * + * This one file/class can be used by ALL of your OpModes without having to cut & paste the code each time. + * + * Where possible, the actual hardware objects are "abstracted" (or hidden) so the OpMode code just makes calls into the class, + * rather than accessing the internal hardware directly. This is why the objects are declared "private". + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with *exactly the same name*. + * + * Or... In OnBot Java, add a new file named RobotHardware.java, select this sample, and select Not an OpMode. + * Also add a new OpMode, select the sample ConceptExternalHardwareClass.java, and select TeleOp. + * + */ + +public class RobotHardware { + + /* Declare OpMode members. */ + private LinearOpMode myOpMode = null; // gain access to methods in the calling OpMode. + + // Define Motor and Servo objects (Make them private so they can't be accessed externally) + private DcMotor leftDrive = null; + private DcMotor rightDrive = null; + private DcMotor armMotor = null; + private Servo leftHand = null; + private Servo rightHand = null; + + // Define Drive constants. Make them public so they CAN be used by the calling OpMode + public static final double MID_SERVO = 0.5 ; + public static final double HAND_SPEED = 0.02 ; // sets rate to move servo + public static final double ARM_UP_POWER = 0.45 ; + public static final double ARM_DOWN_POWER = -0.45 ; + + // Define a constructor that allows the OpMode to pass a reference to itself. + public RobotHardware (LinearOpMode opmode) { + myOpMode = opmode; + } + + /** + * Initialize all the robot's hardware. + * This method must be called ONCE when the OpMode is initialized. + *

+ * All of the hardware devices are accessed via the hardware map, and initialized. + */ + public void init() { + // Define and Initialize Motors (note: need to use reference to actual OpMode). + leftDrive = myOpMode.hardwareMap.get(DcMotor.class, "left_drive"); + rightDrive = myOpMode.hardwareMap.get(DcMotor.class, "right_drive"); + armMotor = myOpMode.hardwareMap.get(DcMotor.class, "arm"); + + // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. + // Pushing the left stick forward MUST make robot go forward. So adjust these two lines based on your first test drive. + // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + + // If there are encoders connected, switch to RUN_USING_ENCODER mode for greater accuracy + // leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + // rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + // Define and initialize ALL installed servos. + leftHand = myOpMode.hardwareMap.get(Servo.class, "left_hand"); + rightHand = myOpMode.hardwareMap.get(Servo.class, "right_hand"); + leftHand.setPosition(MID_SERVO); + rightHand.setPosition(MID_SERVO); + + myOpMode.telemetry.addData(">", "Hardware Initialized"); + myOpMode.telemetry.update(); + } + + /** + * Calculates the left/right motor powers required to achieve the requested + * robot motions: Drive (Axial motion) and Turn (Yaw motion). + * Then sends these power levels to the motors. + * + * @param Drive Fwd/Rev driving power (-1.0 to 1.0) +ve is forward + * @param Turn Right/Left turning power (-1.0 to 1.0) +ve is CW + */ + public void driveRobot(double Drive, double Turn) { + // Combine drive and turn for blended motion. + double left = Drive + Turn; + double right = Drive - Turn; + + // Scale the values so neither exceed +/- 1.0 + double max = Math.max(Math.abs(left), Math.abs(right)); + if (max > 1.0) + { + left /= max; + right /= max; + } + + // Use existing function to drive both wheels. + setDrivePower(left, right); + } + + /** + * Pass the requested wheel motor powers to the appropriate hardware drive motors. + * + * @param leftWheel Fwd/Rev driving power (-1.0 to 1.0) +ve is forward + * @param rightWheel Fwd/Rev driving power (-1.0 to 1.0) +ve is forward + */ + public void setDrivePower(double leftWheel, double rightWheel) { + // Output the values to the motor drives. + leftDrive.setPower(leftWheel); + rightDrive.setPower(rightWheel); + } + + /** + * Pass the requested arm power to the appropriate hardware drive motor + * + * @param power driving power (-1.0 to 1.0) + */ + public void setArmPower(double power) { + armMotor.setPower(power); + } + + /** + * Send the two hand-servos to opposing (mirrored) positions, based on the passed offset. + * + * @param offset + */ + public void setHandPositions(double offset) { + offset = Range.clip(offset, -0.5, 0.5); + leftHand.setPosition(MID_SERVO + offset); + rightHand.setPosition(MID_SERVO - offset); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopPOV_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopPOV_Linear.java new file mode 100644 index 0000000..454d5a7 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopPOV_Linear.java @@ -0,0 +1,159 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.Range; + +/* + * This OpMode executes a POV Game style Teleop for a direct drive robot + * The code is structured as a LinearOpMode + * + * In this mode the left stick moves the robot FWD and back, the Right stick turns left and right. + * It raises and lowers the arm using the Gamepad Y and A buttons respectively. + * It also opens and closes the claws slowly using the left and right Bumper buttons. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="Robot: Teleop POV", group="Robot") +@Disabled +public class RobotTeleopPOV_Linear extends LinearOpMode { + + /* Declare OpMode members. */ + public DcMotor leftDrive = null; + public DcMotor rightDrive = null; + public DcMotor leftArm = null; + public Servo leftClaw = null; + public Servo rightClaw = null; + + double clawOffset = 0; + + public static final double MID_SERVO = 0.5 ; + public static final double CLAW_SPEED = 0.02 ; // sets rate to move servo + public static final double ARM_UP_POWER = 0.45 ; + public static final double ARM_DOWN_POWER = -0.45 ; + + @Override + public void runOpMode() { + double left; + double right; + double drive; + double turn; + double max; + + // Define and Initialize Motors + leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); + rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); + leftArm = hardwareMap.get(DcMotor.class, "left_arm"); + + // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. + // Pushing the left stick forward MUST make robot go forward. So adjust these two lines based on your first test drive. + // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + + // If there are encoders connected, switch to RUN_USING_ENCODER mode for greater accuracy + // leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + // rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + // Define and initialize ALL installed servos. + leftClaw = hardwareMap.get(Servo.class, "left_hand"); + rightClaw = hardwareMap.get(Servo.class, "right_hand"); + leftClaw.setPosition(MID_SERVO); + rightClaw.setPosition(MID_SERVO); + + // Send telemetry message to signify robot waiting; + telemetry.addData(">", "Robot Ready. Press Play."); // + telemetry.update(); + + // Wait for the game to start (driver presses PLAY) + waitForStart(); + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + // Run wheels in POV mode (note: The joystick goes negative when pushed forward, so negate it) + // In this mode the Left stick moves the robot fwd and back, the Right stick turns left and right. + // This way it's also easy to just drive straight, or just turn. + drive = -gamepad1.left_stick_y; + turn = gamepad1.right_stick_x; + + // Combine drive and turn for blended motion. + left = drive + turn; + right = drive - turn; + + // Normalize the values so neither exceed +/- 1.0 + max = Math.max(Math.abs(left), Math.abs(right)); + if (max > 1.0) + { + left /= max; + right /= max; + } + + // Output the safe vales to the motor drives. + leftDrive.setPower(left); + rightDrive.setPower(right); + + // Use gamepad left & right Bumpers to open and close the claw + if (gamepad1.right_bumper) + clawOffset += CLAW_SPEED; + else if (gamepad1.left_bumper) + clawOffset -= CLAW_SPEED; + + // Move both servos to new position. Assume servos are mirror image of each other. + clawOffset = Range.clip(clawOffset, -0.5, 0.5); + leftClaw.setPosition(MID_SERVO + clawOffset); + rightClaw.setPosition(MID_SERVO - clawOffset); + + // Use gamepad buttons to move arm up (Y) and down (A) + if (gamepad1.y) + leftArm.setPower(ARM_UP_POWER); + else if (gamepad1.a) + leftArm.setPower(ARM_DOWN_POWER); + else + leftArm.setPower(0.0); + + // Send telemetry message to signify robot running; + telemetry.addData("claw", "Offset = %.2f", clawOffset); + telemetry.addData("left", "%.2f", left); + telemetry.addData("right", "%.2f", right); + telemetry.update(); + + // Pace this loop so jaw action is reasonable speed. + sleep(50); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopTank_Iterative.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopTank_Iterative.java new file mode 100644 index 0000000..b9f4fcf --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopTank_Iterative.java @@ -0,0 +1,160 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.Range; + +/* + * This OpMode executes a Tank Drive control TeleOp a direct drive robot + * The code is structured as an Iterative OpMode + * + * In this mode, the left and right joysticks control the left and right motors respectively. + * Pushing a joystick forward will make the attached motor drive forward. + * It raises and lowers the claw using the Gamepad Y and A buttons respectively. + * It also opens and closes the claws slowly using the left and right Bumper buttons. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="Robot: Teleop Tank", group="Robot") +@Disabled +public class RobotTeleopTank_Iterative extends OpMode{ + + /* Declare OpMode members. */ + public DcMotor leftDrive = null; + public DcMotor rightDrive = null; + public DcMotor leftArm = null; + public Servo leftClaw = null; + public Servo rightClaw = null; + + double clawOffset = 0; + + public static final double MID_SERVO = 0.5 ; + public static final double CLAW_SPEED = 0.02 ; // sets rate to move servo + public static final double ARM_UP_POWER = 0.50 ; // Run arm motor up at 50% power + public static final double ARM_DOWN_POWER = -0.25 ; // Run arm motor down at -25% power + + /* + * Code to run ONCE when the driver hits INIT + */ + @Override + public void init() { + // Define and Initialize Motors + leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); + rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); + leftArm = hardwareMap.get(DcMotor.class, "left_arm"); + + // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. + // Pushing the left and right sticks forward MUST make robot go forward. So adjust these two lines based on your first test drive. + // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + + // If there are encoders connected, switch to RUN_USING_ENCODER mode for greater accuracy + // leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + // rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + // Define and initialize ALL installed servos. + leftClaw = hardwareMap.get(Servo.class, "left_hand"); + rightClaw = hardwareMap.get(Servo.class, "right_hand"); + leftClaw.setPosition(MID_SERVO); + rightClaw.setPosition(MID_SERVO); + + // Send telemetry message to signify robot waiting; + telemetry.addData(">", "Robot Ready. Press Play."); // + } + + /* + * Code to run REPEATEDLY after the driver hits INIT, but before they hit PLAY + */ + @Override + public void init_loop() { + } + + /* + * Code to run ONCE when the driver hits PLAY + */ + @Override + public void start() { + } + + /* + * Code to run REPEATEDLY after the driver hits PLAY but before they hit STOP + */ + @Override + public void loop() { + double left; + double right; + + // Run wheels in tank mode (note: The joystick goes negative when pushed forward, so negate it) + left = -gamepad1.left_stick_y; + right = -gamepad1.right_stick_y; + + leftDrive.setPower(left); + rightDrive.setPower(right); + + // Use gamepad left & right Bumpers to open and close the claw + if (gamepad1.right_bumper) + clawOffset += CLAW_SPEED; + else if (gamepad1.left_bumper) + clawOffset -= CLAW_SPEED; + + // Move both servos to new position. Assume servos are mirror image of each other. + clawOffset = Range.clip(clawOffset, -0.5, 0.5); + leftClaw.setPosition(MID_SERVO + clawOffset); + rightClaw.setPosition(MID_SERVO - clawOffset); + + // Use gamepad buttons to move the arm up (Y) and down (A) + if (gamepad1.y) + leftArm.setPower(ARM_UP_POWER); + else if (gamepad1.a) + leftArm.setPower(ARM_DOWN_POWER); + else + leftArm.setPower(0.0); + + // Send telemetry message to signify robot running; + telemetry.addData("claw", "Offset = %.2f", clawOffset); + telemetry.addData("left", "%.2f", left); + telemetry.addData("right", "%.2f", right); + } + + /* + * Code to run ONCE after the driver hits STOP + */ + @Override + public void stop() { + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SampleRevBlinkinLedDriver.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SampleRevBlinkinLedDriver.java new file mode 100644 index 0000000..bcf5b80 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SampleRevBlinkinLedDriver.java @@ -0,0 +1,163 @@ +/* + * Copyright (c) 2018 Craig MacFarlane + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted + * (subject to the limitations in the disclaimer below) provided that the following conditions are + * met: + * + * Redistributions of source code must retain the above copyright notice, this list of conditions + * and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this list of conditions + * and the following disclaimer in the documentation and/or other materials provided with the + * distribution. + * + * Neither the name of Craig MacFarlane nor the names of its contributors may be used to + * endorse or promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS LICENSE. THIS + * SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, + * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF + * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.internal.system.Deadline; + +import java.util.concurrent.TimeUnit; + +/* + * This OpMode demonstrates use of the REV Robotics Blinkin LED Driver. + * AUTO mode cycles through all of the patterns. + * MANUAL mode allows the user to manually change patterns using the + * left and right bumpers of a gamepad. + * + * Configure the driver on a servo port, and name it "blinkin". + * + * Displays the first pattern upon init. + */ +@TeleOp(name="BlinkinExample") +@Disabled +public class SampleRevBlinkinLedDriver extends OpMode { + + /* + * Change the pattern every 10 seconds in AUTO mode. + */ + private final static int LED_PERIOD = 10; + + /* + * Rate limit gamepad button presses to every 500ms. + */ + private final static int GAMEPAD_LOCKOUT = 500; + + RevBlinkinLedDriver blinkinLedDriver; + RevBlinkinLedDriver.BlinkinPattern pattern; + + Telemetry.Item patternName; + Telemetry.Item display; + DisplayKind displayKind; + Deadline ledCycleDeadline; + Deadline gamepadRateLimit; + + protected enum DisplayKind { + MANUAL, + AUTO + } + + @Override + public void init() + { + displayKind = DisplayKind.AUTO; + + blinkinLedDriver = hardwareMap.get(RevBlinkinLedDriver.class, "blinkin"); + pattern = RevBlinkinLedDriver.BlinkinPattern.RAINBOW_RAINBOW_PALETTE; + blinkinLedDriver.setPattern(pattern); + + display = telemetry.addData("Display Kind: ", displayKind.toString()); + patternName = telemetry.addData("Pattern: ", pattern.toString()); + + ledCycleDeadline = new Deadline(LED_PERIOD, TimeUnit.SECONDS); + gamepadRateLimit = new Deadline(GAMEPAD_LOCKOUT, TimeUnit.MILLISECONDS); + } + + @Override + public void loop() + { + handleGamepad(); + + if (displayKind == DisplayKind.AUTO) { + doAutoDisplay(); + } else { + /* + * MANUAL mode: Nothing to do, setting the pattern as a result of a gamepad event. + */ + } + } + + /* + * handleGamepad + * + * Responds to a gamepad button press. Demonstrates rate limiting for + * button presses. If loop() is called every 10ms and and you don't rate + * limit, then any given button press may register as multiple button presses, + * which in this application is problematic. + * + * A: Manual mode, Right bumper displays the next pattern, left bumper displays the previous pattern. + * B: Auto mode, pattern cycles, changing every LED_PERIOD seconds. + */ + protected void handleGamepad() + { + if (!gamepadRateLimit.hasExpired()) { + return; + } + + if (gamepad1.a) { + setDisplayKind(DisplayKind.MANUAL); + gamepadRateLimit.reset(); + } else if (gamepad1.b) { + setDisplayKind(DisplayKind.AUTO); + gamepadRateLimit.reset(); + } else if ((displayKind == DisplayKind.MANUAL) && (gamepad1.left_bumper)) { + pattern = pattern.previous(); + displayPattern(); + gamepadRateLimit.reset(); + } else if ((displayKind == DisplayKind.MANUAL) && (gamepad1.right_bumper)) { + pattern = pattern.next(); + displayPattern(); + gamepadRateLimit.reset(); + } + } + + protected void setDisplayKind(DisplayKind displayKind) + { + this.displayKind = displayKind; + display.setValue(displayKind.toString()); + } + + protected void doAutoDisplay() + { + if (ledCycleDeadline.hasExpired()) { + pattern = pattern.next(); + displayPattern(); + ledCycleDeadline.reset(); + } + } + + protected void displayPattern() + { + blinkinLedDriver.setPattern(pattern); + patternName.setValue(pattern.toString()); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMU.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMU.java new file mode 100644 index 0000000..405da1e --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMU.java @@ -0,0 +1,186 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.Func; +import org.firstinspires.ftc.robotcore.external.navigation.Acceleration; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.Velocity; + +import java.util.Locale; + +/* + * This OpMode gives a short demo on how to use the BNO055 Inertial Motion Unit (IMU) from AdaFruit. + * + * Note: this is a Legacy example that will not work with newer Control/Expansion Hubs that use a different IMU + * Please use the new SensorIMUOrthogonal or SensorIMUNonOrthogonal samples for a more universal IMU interface. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * @see Adafruit IMU + */ +@TeleOp(name = "Sensor: BNO055 IMU", group = "Sensor") +@Disabled // Comment this out to add to the OpMode list +public class SensorBNO055IMU extends LinearOpMode + { + //---------------------------------------------------------------------------------------------- + // State + //---------------------------------------------------------------------------------------------- + + // The IMU sensor object + BNO055IMU imu; + + // State used for updating telemetry + Orientation angles; + Acceleration gravity; + + //---------------------------------------------------------------------------------------------- + // Main logic + //---------------------------------------------------------------------------------------------- + + @Override public void runOpMode() { + + // Set up the parameters with which we will use our IMU. Note that integration + // algorithm here just reports accelerations to the logcat log; it doesn't actually + // provide positional information. + BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); + parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; + parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC; + parameters.calibrationDataFile = "BNO055IMUCalibration.json"; // see the calibration sample OpMode + parameters.loggingEnabled = true; + parameters.loggingTag = "IMU"; + parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator(); + + // Retrieve and initialize the IMU. We expect the IMU to be attached to an I2C port + // on a Core Device Interface Module, configured to be a sensor of type "AdaFruit IMU", + // and named "imu". + imu = hardwareMap.get(BNO055IMU.class, "imu"); + imu.initialize(parameters); + + // Set up our telemetry dashboard + composeTelemetry(); + + // Wait until we're told to go + waitForStart(); + + // Start the logging of measured acceleration + imu.startAccelerationIntegration(new Position(), new Velocity(), 1000); + + // Loop and update the dashboard + while (opModeIsActive()) { + telemetry.update(); + } + } + + //---------------------------------------------------------------------------------------------- + // Telemetry Configuration + //---------------------------------------------------------------------------------------------- + + void composeTelemetry() { + + // At the beginning of each telemetry update, grab a bunch of data + // from the IMU that we will then display in separate lines. + telemetry.addAction(new Runnable() { @Override public void run() + { + // Acquiring the angles is relatively expensive; we don't want + // to do that in each of the three items that need that info, as that's + // three times the necessary expense. + angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); + gravity = imu.getGravity(); + } + }); + + telemetry.addLine() + .addData("status", new Func() { + @Override public String value() { + return imu.getSystemStatus().toShortString(); + } + }) + .addData("calib", new Func() { + @Override public String value() { + return imu.getCalibrationStatus().toString(); + } + }); + + telemetry.addLine() + .addData("heading", new Func() { + @Override public String value() { + return formatAngle(angles.angleUnit, angles.firstAngle); + } + }) + .addData("roll", new Func() { + @Override public String value() { + return formatAngle(angles.angleUnit, angles.secondAngle); + } + }) + .addData("pitch", new Func() { + @Override public String value() { + return formatAngle(angles.angleUnit, angles.thirdAngle); + } + }); + + telemetry.addLine() + .addData("grvty", new Func() { + @Override public String value() { + return gravity.toString(); + } + }) + .addData("mag", new Func() { + @Override public String value() { + return String.format(Locale.getDefault(), "%.3f", + Math.sqrt(gravity.xAccel*gravity.xAccel + + gravity.yAccel*gravity.yAccel + + gravity.zAccel*gravity.zAccel)); + } + }); + } + + //---------------------------------------------------------------------------------------------- + // Formatting + //---------------------------------------------------------------------------------------------- + + String formatAngle(AngleUnit angleUnit, double angle) { + return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle)); + } + + String formatDegrees(double degrees){ + return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees)); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMUCalibration.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMUCalibration.java new file mode 100644 index 0000000..93f1789 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMUCalibration.java @@ -0,0 +1,230 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.ReadWriteFile; +import org.firstinspires.ftc.robotcore.external.Func; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.internal.system.AppUtil; + +import java.io.File; +import java.util.Locale; + +/* + * This OpMode calibrates a BNO055 IMU per + * "Section 3.11 Calibration" of the BNO055 specification. + * + * Note: this is a Legacy example that will not work with newer Control/Expansion Hubs that use a different IMU + * Please use the new SensorIMUOrthogonal or SensorIMUNonOrthogonal samples for a more universal IMU interface. + * + * Manual calibration of the IMU is definitely NOT necessary: except for the magnetometer (which is not used by the + * default "IMU" SensorMode), the BNO055 is internally self-calibrating and thus can be very successfully used without + * manual intervention. That said, performing a one-time calibration, saving the results persistently, then loading them + * again at each run can help reduce the time that automatic calibration requires. + * + * This summary of the calibration process from Intel is informative: + * http://iotdk.intel.com/docs/master/upm/classupm_1_1_b_n_o055.html + * + * "This device requires calibration in order to operate accurately. [...] Calibration data is + * lost on a power cycle. See one of the examples for a description of how to calibrate the device, + * but in essence: + * + * There is a calibration status register available [...] that returns the calibration status + * of the accelerometer (ACC), magnetometer (MAG), gyroscope (GYR), and overall system (SYS). + * Each of these values range from 0 (uncalibrated) to 3 (fully calibrated). Calibration [ideally] + * involves certain motions to get all 4 values at 3. The motions are as follows (though see the + * datasheet for more information): + * + * 1. GYR: Simply let the sensor sit flat for a few seconds. + * 2. ACC: Move the sensor in various positions. Start flat, then rotate slowly by 45 + * degrees, hold for a few seconds, then continue rotating another 45 degrees and + * hold, etc. 6 or more movements of this type may be required. You can move through + * any axis you desire, but make sure that the device is lying at least once + * perpendicular to the x, y, and z axis. + * 3. MAG: Move slowly in a figure 8 pattern in the air, until the calibration values reaches 3. + * 4. SYS: This will usually reach 3 when the other items have also reached 3. If not, continue + * slowly moving the device though various axes until it does." + * + * To calibrate the IMU, run this sample OpMode with a gamepad attached to the driver station. + * Once the IMU has reached sufficient calibration as reported on telemetry, press the 'A' + * button on the gamepad to write the calibration to a file. That file can then be indicated + * later when running an OpMode which uses the IMU. + * + * Note: if your intended uses of the IMU do not include use of all its sensors (for example, + * you might not use the magnetometer), then it makes little sense for you to wait for full + * calibration of the sensors you are not using before saving the calibration data. Indeed, + * it appears that in a SensorMode that doesn't use the magnetometer (for example), the + * magnetometer cannot actually be calibrated. + * + * References: + * The AdafruitBNO055IMU Javadoc + * The BNO055IMU.Parameters.calibrationDataFile Javadoc + * The BNO055 product page: https://www.bosch-sensortec.com/bst/products/all_products/bno055 + * The BNO055 datasheet: https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf + */ +@TeleOp(name = "Sensor: BNO055 IMU Calibration", group = "Sensor") +@Disabled // Uncomment this to add to the OpMode list +public class SensorBNO055IMUCalibration extends LinearOpMode + { + //---------------------------------------------------------------------------------------------- + // State + //---------------------------------------------------------------------------------------------- + + // Our sensors, motors, and other devices go here, along with other long term state + BNO055IMU imu; + + // State used for updating telemetry + Orientation angles; + + //---------------------------------------------------------------------------------------------- + // Main logic + //---------------------------------------------------------------------------------------------- + + @Override public void runOpMode() { + + telemetry.log().setCapacity(12); + telemetry.log().add(""); + telemetry.log().add("Please refer to the calibration instructions"); + telemetry.log().add("contained in the Adafruit IMU calibration"); + telemetry.log().add("sample OpMode."); + telemetry.log().add(""); + telemetry.log().add("When sufficient calibration has been reached,"); + telemetry.log().add("press the 'A' button to write the current"); + telemetry.log().add("calibration data to a file."); + telemetry.log().add(""); + + // We are expecting the IMU to be attached to an I2C port on a Core Device Interface Module and named "imu". + BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); + parameters.loggingEnabled = true; + parameters.loggingTag = "IMU"; + imu = hardwareMap.get(BNO055IMU.class, "imu"); + imu.initialize(parameters); + + composeTelemetry(); + telemetry.log().add("Waiting for start..."); + + // Wait until we're told to go + while (!isStarted()) { + telemetry.update(); + idle(); + } + + telemetry.log().add("...started..."); + + while (opModeIsActive()) { + + if (gamepad1.a) { + + // Get the calibration data + BNO055IMU.CalibrationData calibrationData = imu.readCalibrationData(); + + // Save the calibration data to a file. You can choose whatever file + // name you wish here, but you'll want to indicate the same file name + // when you initialize the IMU in an OpMode in which it is used. If you + // have more than one IMU on your robot, you'll of course want to use + // different configuration file names for each. + String filename = "AdafruitIMUCalibration.json"; + File file = AppUtil.getInstance().getSettingsFile(filename); + ReadWriteFile.writeFile(file, calibrationData.serialize()); + telemetry.log().add("saved to '%s'", filename); + + // Wait for the button to be released + while (gamepad1.a) { + telemetry.update(); + idle(); + } + } + + telemetry.update(); + } + } + + void composeTelemetry() { + + // At the beginning of each telemetry update, grab a bunch of data + // from the IMU that we will then display in separate lines. + telemetry.addAction(new Runnable() { @Override public void run() + { + // Acquiring the angles is relatively expensive; we don't want + // to do that in each of the three items that need that info, as that's + // three times the necessary expense. + angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); + } + }); + + telemetry.addLine() + .addData("status", new Func() { + @Override public String value() { + return imu.getSystemStatus().toShortString(); + } + }) + .addData("calib", new Func() { + @Override public String value() { + return imu.getCalibrationStatus().toString(); + } + }); + + telemetry.addLine() + .addData("heading", new Func() { + @Override public String value() { + return formatAngle(angles.angleUnit, angles.firstAngle); + } + }) + .addData("roll", new Func() { + @Override public String value() { + return formatAngle(angles.angleUnit, angles.secondAngle); + } + }) + .addData("pitch", new Func() { + @Override public String value() { + return formatAngle(angles.angleUnit, angles.thirdAngle); + } + }); + } + + //---------------------------------------------------------------------------------------------- + // Formatting + //---------------------------------------------------------------------------------------------- + + String formatAngle(AngleUnit angleUnit, double angle) { + return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle)); + } + + String formatDegrees(double degrees){ + return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees)); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorColor.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorColor.java new file mode 100644 index 0000000..7546c9d --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorColor.java @@ -0,0 +1,221 @@ +/* Copyright (c) 2017-2020 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import android.app.Activity; +import android.graphics.Color; +import android.view.View; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DistanceSensor; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; +import com.qualcomm.robotcore.hardware.SwitchableLight; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +/* + * This OpMode shows how to use a color sensor in a generic + * way, regardless of which particular make or model of color sensor is used. The OpMode + * assumes that the color sensor is configured with a name of "sensor_color". + * + * There will be some variation in the values measured depending on the specific sensor you are using. + * + * You can increase the gain (a multiplier to make the sensor report higher values) by holding down + * the A button on the gamepad, and decrease the gain by holding down the B button on the gamepad. + * + * If the color sensor has a light which is controllable from software, you can use the X button on + * the gamepad to toggle the light on and off. The REV sensors don't support this, but instead have + * a physical switch on them to turn the light on and off, beginning with REV Color Sensor V2. + * + * If the color sensor also supports short-range distance measurements (usually via an infrared + * proximity sensor), the reported distance will be written to telemetry. As of September 2020, + * the only color sensors that support this are the ones from REV Robotics. These infrared proximity + * sensor measurements are only useful at very small distances, and are sensitive to ambient light + * and surface reflectivity. You should use a different sensor if you need precise distance measurements. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ +@TeleOp(name = "Sensor: Color", group = "Sensor") +@Disabled +public class SensorColor extends LinearOpMode { + + /** The colorSensor field will contain a reference to our color sensor hardware object */ + NormalizedColorSensor colorSensor; + + /** The relativeLayout field is used to aid in providing interesting visual feedback + * in this sample application; you probably *don't* need this when you use a color sensor on your + * robot. Note that you won't see anything change on the Driver Station, only on the Robot Controller. */ + View relativeLayout; + + /* + * The runOpMode() method is the root of this OpMode, as it is in all LinearOpModes. + * Our implementation here, though is a bit unusual: we've decided to put all the actual work + * in the runSample() method rather than directly in runOpMode() itself. The reason we do that is + * that in this sample we're changing the background color of the robot controller screen as the + * OpMode runs, and we want to be able to *guarantee* that we restore it to something reasonable + * and palatable when the OpMode ends. The simplest way to do that is to use a try...finally + * block around the main, core logic, and an easy way to make that all clear was to separate + * the former from the latter in separate methods. + */ + @Override public void runOpMode() { + + // Get a reference to the RelativeLayout so we can later change the background + // color of the Robot Controller app to match the hue detected by the RGB sensor. + int relativeLayoutId = hardwareMap.appContext.getResources().getIdentifier("RelativeLayout", "id", hardwareMap.appContext.getPackageName()); + relativeLayout = ((Activity) hardwareMap.appContext).findViewById(relativeLayoutId); + + try { + runSample(); // actually execute the sample + } finally { + // On the way out, *guarantee* that the background is reasonable. It doesn't actually start off + // as pure white, but it's too much work to dig out what actually was used, and this is good + // enough to at least make the screen reasonable again. + // Set the panel back to the default color + relativeLayout.post(new Runnable() { + public void run() { + relativeLayout.setBackgroundColor(Color.WHITE); + } + }); + } + } + + protected void runSample() { + // You can give the sensor a gain value, will be multiplied by the sensor's raw value before the + // normalized color values are calculated. Color sensors (especially the REV Color Sensor V3) + // can give very low values (depending on the lighting conditions), which only use a small part + // of the 0-1 range that is available for the red, green, and blue values. In brighter conditions, + // you should use a smaller gain than in dark conditions. If your gain is too high, all of the + // colors will report at or near 1, and you won't be able to determine what color you are + // actually looking at. For this reason, it's better to err on the side of a lower gain + // (but always greater than or equal to 1). + float gain = 2; + + // Once per loop, we will update this hsvValues array. The first element (0) will contain the + // hue, the second element (1) will contain the saturation, and the third element (2) will + // contain the value. See http://web.archive.org/web/20190311170843/https://infohost.nmt.edu/tcc/help/pubs/colortheory/web/hsv.html + // for an explanation of HSV color. + final float[] hsvValues = new float[3]; + + // xButtonPreviouslyPressed and xButtonCurrentlyPressed keep track of the previous and current + // state of the X button on the gamepad + boolean xButtonPreviouslyPressed = false; + boolean xButtonCurrentlyPressed = false; + + // Get a reference to our sensor object. It's recommended to use NormalizedColorSensor over + // ColorSensor, because NormalizedColorSensor consistently gives values between 0 and 1, while + // the values you get from ColorSensor are dependent on the specific sensor you're using. + colorSensor = hardwareMap.get(NormalizedColorSensor.class, "sensor_color"); + + // If possible, turn the light on in the beginning (it might already be on anyway, + // we just make sure it is if we can). + if (colorSensor instanceof SwitchableLight) { + ((SwitchableLight)colorSensor).enableLight(true); + } + + // Wait for the start button to be pressed. + waitForStart(); + + // Loop until we are asked to stop + while (opModeIsActive()) { + // Explain basic gain information via telemetry + telemetry.addLine("Hold the A button on gamepad 1 to increase gain, or B to decrease it.\n"); + telemetry.addLine("Higher gain values mean that the sensor will report larger numbers for Red, Green, and Blue, and Value\n"); + + // Update the gain value if either of the A or B gamepad buttons is being held + if (gamepad1.a) { + // Only increase the gain by a small amount, since this loop will occur multiple times per second. + gain += 0.005; + } else if (gamepad1.b && gain > 1) { // A gain of less than 1 will make the values smaller, which is not helpful. + gain -= 0.005; + } + + // Show the gain value via telemetry + telemetry.addData("Gain", gain); + + // Tell the sensor our desired gain value (normally you would do this during initialization, + // not during the loop) + colorSensor.setGain(gain); + + // Check the status of the X button on the gamepad + xButtonCurrentlyPressed = gamepad1.x; + + // If the button state is different than what it was, then act + if (xButtonCurrentlyPressed != xButtonPreviouslyPressed) { + // If the button is (now) down, then toggle the light + if (xButtonCurrentlyPressed) { + if (colorSensor instanceof SwitchableLight) { + SwitchableLight light = (SwitchableLight)colorSensor; + light.enableLight(!light.isLightOn()); + } + } + } + xButtonPreviouslyPressed = xButtonCurrentlyPressed; + + // Get the normalized colors from the sensor + NormalizedRGBA colors = colorSensor.getNormalizedColors(); + + /* Use telemetry to display feedback on the driver station. We show the red, green, and blue + * normalized values from the sensor (in the range of 0 to 1), as well as the equivalent + * HSV (hue, saturation and value) values. See http://web.archive.org/web/20190311170843/https://infohost.nmt.edu/tcc/help/pubs/colortheory/web/hsv.html + * for an explanation of HSV color. */ + + // Update the hsvValues array by passing it to Color.colorToHSV() + Color.colorToHSV(colors.toColor(), hsvValues); + + telemetry.addLine() + .addData("Red", "%.3f", colors.red) + .addData("Green", "%.3f", colors.green) + .addData("Blue", "%.3f", colors.blue); + telemetry.addLine() + .addData("Hue", "%.3f", hsvValues[0]) + .addData("Saturation", "%.3f", hsvValues[1]) + .addData("Value", "%.3f", hsvValues[2]); + telemetry.addData("Alpha", "%.3f", colors.alpha); + + /* If this color sensor also has a distance sensor, display the measured distance. + * Note that the reported distance is only useful at very close range, and is impacted by + * ambient light and surface reflectivity. */ + if (colorSensor instanceof DistanceSensor) { + telemetry.addData("Distance (cm)", "%.3f", ((DistanceSensor) colorSensor).getDistance(DistanceUnit.CM)); + } + + telemetry.update(); + + // Change the Robot Controller's background color to match the color detected by the color sensor. + relativeLayout.post(new Runnable() { + public void run() { + relativeLayout.setBackgroundColor(Color.HSVToColor(hsvValues)); + } + }); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java new file mode 100644 index 0000000..44c3ca9 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java @@ -0,0 +1,78 @@ +/* Copyright (c) 2024 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DigitalChannel; + +/* + * This OpMode demonstrates how to use a digital channel. + * + * The OpMode assumes that the digital channel is configured with a name of "digitalTouch". + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Sensor: digital channel", group = "Sensor") +@Disabled +public class SensorDigitalTouch extends LinearOpMode { + DigitalChannel digitalTouch; // Digital channel Object + + @Override + public void runOpMode() { + + // get a reference to our touchSensor object. + digitalTouch = hardwareMap.get(DigitalChannel.class, "digitalTouch"); + + digitalTouch.setMode(DigitalChannel.Mode.INPUT); + telemetry.addData("DigitalTouchSensorExample", "Press start to continue..."); + telemetry.update(); + + // wait for the start button to be pressed. + waitForStart(); + + // while the OpMode is active, loop and read the digital channel. + // Note we use opModeIsActive() as our loop condition because it is an interruptible method. + while (opModeIsActive()) { + + // button is pressed if value returned is LOW or false. + // send the info back to driver station using telemetry function. + if (digitalTouch.getState() == false) { + telemetry.addData("Button", "PRESSED"); + } else { + telemetry.addData("Button", "NOT PRESSED"); + } + + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHuskyLens.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHuskyLens.java new file mode 100644 index 0000000..1ec1f55 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHuskyLens.java @@ -0,0 +1,149 @@ +/* +Copyright (c) 2023 FIRST + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of FIRST nor the names of its contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.dfrobot.HuskyLens; +import com.qualcomm.hardware.rev.Rev2mDistanceSensor; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DistanceSensor; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.internal.system.Deadline; + +import java.util.concurrent.TimeUnit; + +/* + * This OpMode illustrates how to use the DFRobot HuskyLens. + * + * The HuskyLens is a Vision Sensor with a built-in object detection model. It can + * detect a number of predefined objects and AprilTags in the 36h11 family, can + * recognize colors, and can be trained to detect custom objects. See this website for + * documentation: https://wiki.dfrobot.com/HUSKYLENS_V1.0_SKU_SEN0305_SEN0336 + * + * This sample illustrates how to detect AprilTags, but can be used to detect other types + * of objects by changing the algorithm. It assumes that the HuskyLens is configured with + * a name of "huskylens". + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ +@TeleOp(name = "Sensor: HuskyLens", group = "Sensor") +@Disabled +public class SensorHuskyLens extends LinearOpMode { + + private final int READ_PERIOD = 1; + + private HuskyLens huskyLens; + + @Override + public void runOpMode() + { + huskyLens = hardwareMap.get(HuskyLens.class, "huskylens"); + + /* + * This sample rate limits the reads solely to allow a user time to observe + * what is happening on the Driver Station telemetry. Typical applications + * would not likely rate limit. + */ + Deadline rateLimit = new Deadline(READ_PERIOD, TimeUnit.SECONDS); + + /* + * Immediately expire so that the first time through we'll do the read. + */ + rateLimit.expire(); + + /* + * Basic check to see if the device is alive and communicating. This is not + * technically necessary here as the HuskyLens class does this in its + * doInitialization() method which is called when the device is pulled out of + * the hardware map. However, sometimes it's unclear why a device reports as + * failing on initialization. In the case of this device, it's because the + * call to knock() failed. + */ + if (!huskyLens.knock()) { + telemetry.addData(">>", "Problem communicating with " + huskyLens.getDeviceName()); + } else { + telemetry.addData(">>", "Press start to continue"); + } + + /* + * The device uses the concept of an algorithm to determine what types of + * objects it will look for and/or what mode it is in. The algorithm may be + * selected using the scroll wheel on the device, or via software as shown in + * the call to selectAlgorithm(). + * + * The SDK itself does not assume that the user wants a particular algorithm on + * startup, and hence does not set an algorithm. + * + * Users, should, in general, explicitly choose the algorithm they want to use + * within the OpMode by calling selectAlgorithm() and passing it one of the values + * found in the enumeration HuskyLens.Algorithm. + */ + huskyLens.selectAlgorithm(HuskyLens.Algorithm.TAG_RECOGNITION); + + telemetry.update(); + waitForStart(); + + /* + * Looking for AprilTags per the call to selectAlgorithm() above. A handy grid + * for testing may be found at https://wiki.dfrobot.com/HUSKYLENS_V1.0_SKU_SEN0305_SEN0336#target_20. + * + * Note again that the device only recognizes the 36h11 family of tags out of the box. + */ + while(opModeIsActive()) { + if (!rateLimit.hasExpired()) { + continue; + } + rateLimit.reset(); + + /* + * All algorithms, except for LINE_TRACKING, return a list of Blocks where a + * Block represents the outline of a recognized object along with its ID number. + * ID numbers allow you to identify what the device saw. See the HuskyLens documentation + * referenced in the header comment above for more information on IDs and how to + * assign them to objects. + * + * Returns an empty array if no objects are seen. + */ + HuskyLens.Block[] blocks = huskyLens.blocks(); + telemetry.addData("Block count", blocks.length); + for (int i = 0; i < blocks.length; i++) { + telemetry.addData("Block", blocks[i].toString()); + } + + telemetry.update(); + } + } +} \ No newline at end of file diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUNonOrthogonal.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUNonOrthogonal.java new file mode 100644 index 0000000..eb1c7ca --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUNonOrthogonal.java @@ -0,0 +1,181 @@ +/* Copyright (c) 2022 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.IMU; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.xyzOrientation; + +/* + * This OpMode shows how to use the new universal IMU interface. This + * interface may be used with the BNO055 IMU or the BHI260 IMU. It assumes that an IMU is configured + * on the robot with the name "imu". + * + * The sample will display the current Yaw, Pitch and Roll of the robot.
+ * With the correct orientation parameters selected, pitch/roll/yaw should act as follows: + * Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X) + * Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y) + * Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z) + * + * The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller) + * + * This specific sample DOES NOT assume that the Hub is mounted on one of the three orthogonal + * planes (X/Y, X/Z or Y/Z) OR that the Hub has only been rotated in a range of 90 degree increments. + * + * Note: if your Hub is mounted Orthogonally (on a orthogonal surface, angled at some multiple of + * 90 Degrees) then you should use the simpler SensorImuOrthogonal sample in this folder. + * + * But... If your Hub is mounted Non-Orthogonally, you must specify one or more rotational angles + * that transform a "Default" Hub orientation into your desired orientation. That is what is + * illustrated here. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + * + * Finally, edit this OpMode to use at least one angle around an axis to orient your Hub. + */ +@TeleOp(name = "Sensor: IMU Non-Orthogonal", group = "Sensor") +@Disabled // Comment this out to add to the OpMode list +public class SensorIMUNonOrthogonal extends LinearOpMode +{ + // The IMU sensor object + IMU imu; + + //---------------------------------------------------------------------------------------------- + // Main logic + //---------------------------------------------------------------------------------------------- + + @Override public void runOpMode() throws InterruptedException { + + // Retrieve and initialize the IMU. + // This sample expects the IMU to be in a REV Hub and named "imu". + imu = hardwareMap.get(IMU.class, "imu"); + + /* Define how the hub is mounted to the robot to get the correct Yaw, Pitch and Roll values. + * + * You can apply up to three axis rotations to orient your Hub according to how it's mounted on the robot. + * + * The starting point for these rotations is the "Default" Hub orientation, which is: + * 1) Hub laying flat on a horizontal surface, with the Printed Logo facing UP + * 2) Rotated such that the USB ports are facing forward on the robot. + * + * The order that the rotations are performed matters, so this sample shows doing them in the order X, Y, then Z. + * For specifying non-orthogonal hub mounting orientations, we must temporarily use axes + * defined relative to the Hub itself, instead of the usual Robot Coordinate System axes + * used for the results the IMU gives us. In the starting orientation, the Hub axes are + * aligned with the Robot Coordinate System: + * + * X Axis: Starting at Center of Hub, pointing out towards I2C connectors + * Y Axis: Starting at Center of Hub, pointing out towards USB connectors + * Z Axis: Starting at Center of Hub, pointing Up through LOGO + * + * Positive rotation is defined by right-hand rule with thumb pointing in +ve direction on axis. + * + * Some examples. + * + * ---------------------------------------------------------------------------------------------------------------------------------- + * Example A) Assume that the hub is mounted on a sloped plate at the back of the robot, with the USB ports coming out the top of the hub. + * The plate is tilted UP 60 degrees from horizontal. + * + * To get the "Default" hub into this configuration you would just need a single rotation. + * 1) Rotate the Hub +60 degrees around the X axis to tilt up the front edge. + * 2) No rotation around the Y or Z axes. + * + * So the X,Y,Z rotations would be 60,0,0 + * + * ---------------------------------------------------------------------------------------------------------------------------------- + * Example B) Assume that the hub is laying flat on the chassis, but it has been twisted 30 degrees towards the right front wheel to make + * the USB cable accessible. + * + * To get the "Default" hub into this configuration you would just need a single rotation, but around a different axis. + * 1) No rotation around the X or Y axes. + * 1) Rotate the Hub -30 degrees (Clockwise) around the Z axis, since a positive angle would be Counter Clockwise. + * + * So the X,Y,Z rotations would be 0,0,-30 + * + * ---------------------------------------------------------------------------------------------------------------------------------- + * Example C) Assume that the hub is mounted on a vertical plate on the right side of the robot, with the Logo facing out, and the + * Hub rotated so that the USB ports are facing down 30 degrees towards the back wheels of the robot. + * + * To get the "Default" hub into this configuration will require several rotations. + * 1) Rotate the hub +90 degrees around the X axis to get it standing upright with the logo pointing backwards on the robot + * 2) Next, rotate the hub +90 around the Y axis to get it facing to the right. + * 3) Finally rotate the hub +120 degrees around the Z axis to take the USB ports from vertical to sloping down 30 degrees and + * facing towards the back of the robot. + * + * So the X,Y,Z rotations would be 90,90,120 + */ + + // The next three lines define the desired axis rotations. + // To Do: EDIT these values to match YOUR mounting configuration. + double xRotation = 0; // enter the desired X rotation angle here. + double yRotation = 0; // enter the desired Y rotation angle here. + double zRotation = 0; // enter the desired Z rotation angle here. + + Orientation hubRotation = xyzOrientation(xRotation, yRotation, zRotation); + + // Now initialize the IMU with this mounting orientation + RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(hubRotation); + imu.initialize(new IMU.Parameters(orientationOnRobot)); + + // Loop and update the dashboard + while (!isStopRequested()) { + telemetry.addData("Hub orientation", "X=%.1f, Y=%.1f, Z=%.1f \n", xRotation, yRotation, zRotation); + + // Check to see if heading reset is requested + if (gamepad1.y) { + telemetry.addData("Yaw", "Resetting\n"); + imu.resetYaw(); + } else { + telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n"); + } + + // Retrieve Rotational Angles and Velocities + YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles(); + AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES); + + telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES)); + telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES)); + telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES)); + telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate); + telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate); + telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate); + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUOrthogonal.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUOrthogonal.java new file mode 100644 index 0000000..d92ce3e --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUOrthogonal.java @@ -0,0 +1,143 @@ +/* Copyright (c) 2022 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.IMU; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +/* + * This OpMode shows how to use the new universal IMU interface. This + * interface may be used with the BNO055 IMU or the BHI260 IMU. It assumes that an IMU is configured + * on the robot with the name "imu". + * + * The sample will display the current Yaw, Pitch and Roll of the robot.
+ * With the correct orientation parameters selected, pitch/roll/yaw should act as follows: + * Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X)
+ * Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y)
+ * Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z)
+ * + * The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller) + * + * This specific sample assumes that the Hub is mounted on one of the three orthogonal planes + * (X/Y, X/Z or Y/Z) and that the Hub has only been rotated in a range of 90 degree increments. + * + * Note: if your Hub is mounted on a surface angled at some non-90 Degree multiple (like 30) look at + * the alternative SensorImuNonOrthogonal sample in this folder. + * + * This "Orthogonal" requirement means that: + * + * 1) The Logo printed on the top of the Hub can ONLY be pointing in one of six directions: + * FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT. + * + * 2) The USB ports can only be pointing in one of the same six directions:
+ * FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT. + * + * So, To fully define how your Hub is mounted to the robot, you must simply specify:
+ * logoFacingDirection
+ * usbFacingDirection + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + * + * Finally, choose the two correct parameters to define how your Hub is mounted and edit this OpMode + * to use those parameters. + */ +@TeleOp(name = "Sensor: IMU Orthogonal", group = "Sensor") +@Disabled // Comment this out to add to the OpMode list +public class SensorIMUOrthogonal extends LinearOpMode +{ + // The IMU sensor object + IMU imu; + + //---------------------------------------------------------------------------------------------- + // Main logic + //---------------------------------------------------------------------------------------------- + + @Override public void runOpMode() throws InterruptedException { + + // Retrieve and initialize the IMU. + // This sample expects the IMU to be in a REV Hub and named "imu". + imu = hardwareMap.get(IMU.class, "imu"); + + /* Define how the hub is mounted on the robot to get the correct Yaw, Pitch and Roll values. + * + * Two input parameters are required to fully specify the Orientation. + * The first parameter specifies the direction the printed logo on the Hub is pointing. + * The second parameter specifies the direction the USB connector on the Hub is pointing. + * All directions are relative to the robot, and left/right is as-viewed from behind the robot. + */ + + /* The next two lines define Hub orientation. + * The Default Orientation (shown) is when a hub is mounted horizontally with the printed logo pointing UP and the USB port pointing FORWARD. + * + * To Do: EDIT these two lines to match YOUR mounting configuration. + */ + RevHubOrientationOnRobot.LogoFacingDirection logoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; + RevHubOrientationOnRobot.UsbFacingDirection usbDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; + + RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection); + + // Now initialize the IMU with this mounting orientation + // Note: if you choose two conflicting directions, this initialization will cause a code exception. + imu.initialize(new IMU.Parameters(orientationOnRobot)); + + // Loop and update the dashboard + while (!isStopRequested()) { + + telemetry.addData("Hub orientation", "Logo=%s USB=%s\n ", logoDirection, usbDirection); + + // Check to see if heading reset is requested + if (gamepad1.y) { + telemetry.addData("Yaw", "Resetting\n"); + imu.resetYaw(); + } else { + telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n"); + } + + // Retrieve Rotational Angles and Velocities + YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles(); + AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES); + + telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES)); + telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES)); + telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES)); + telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate); + telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate); + telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate); + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorKLNavxMicro.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorKLNavxMicro.java new file mode 100644 index 0000000..ccc945f --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorKLNavxMicro.java @@ -0,0 +1,128 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.kauailabs.NavxMicroNavigationSensor; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Gyroscope; +import com.qualcomm.robotcore.hardware.IntegratingGyroscope; +import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; + +/* + * This OpMode shows how to use Kauai Labs navX Micro Robotics Navigation + * Sensor. It assumes that the sensor is configured with a name of "navx". + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ +@TeleOp(name = "Sensor: KL navX Micro", group = "Sensor") +@Disabled +public class SensorKLNavxMicro extends LinearOpMode { + + /** In this sample, for illustration purposes we use two interfaces on the one gyro object. + * That's likely atypical: you'll probably use one or the other in any given situation, + * depending on what you're trying to do. {@link IntegratingGyroscope} (and it's base interface, + * {@link Gyroscope}) are common interfaces supported by possibly several different gyro + * implementations. {@link NavxMicroNavigationSensor}, by contrast, provides functionality that + * is unique to the navX Micro sensor. + */ + IntegratingGyroscope gyro; + NavxMicroNavigationSensor navxMicro; + + // A timer helps provide feedback while calibration is taking place + ElapsedTime timer = new ElapsedTime(); + + @Override public void runOpMode() throws InterruptedException { + // Get a reference to a Modern Robotics GyroSensor object. We use several interfaces + // on this object to illustrate which interfaces support which functionality. + navxMicro = hardwareMap.get(NavxMicroNavigationSensor.class, "navx"); + gyro = (IntegratingGyroscope)navxMicro; + // If you're only interested int the IntegratingGyroscope interface, the following will suffice. + // gyro = hardwareMap.get(IntegratingGyroscope.class, "navx"); + + // The gyro automatically starts calibrating. This takes a few seconds. + telemetry.log().add("Gyro Calibrating. Do Not Move!"); + + // Wait until the gyro calibration is complete + timer.reset(); + while (navxMicro.isCalibrating()) { + telemetry.addData("calibrating", "%s", Math.round(timer.seconds())%2==0 ? "|.." : "..|"); + telemetry.update(); + Thread.sleep(50); + } + telemetry.log().clear(); telemetry.log().add("Gyro Calibrated. Press Start."); + telemetry.clear(); telemetry.update(); + + // Wait for the start button to be pressed + waitForStart(); + telemetry.log().clear(); + + while (opModeIsActive()) { + + // Read dimensionalized data from the gyro. This gyro can report angular velocities + // about all three axes. Additionally, it internally integrates the Z axis to + // be able to report an absolute angular Z orientation. + AngularVelocity rates = gyro.getAngularVelocity(AngleUnit.DEGREES); + Orientation angles = gyro.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); + + telemetry.addLine() + .addData("dx", formatRate(rates.xRotationRate)) + .addData("dy", formatRate(rates.yRotationRate)) + .addData("dz", "%s deg/s", formatRate(rates.zRotationRate)); + + telemetry.addLine() + .addData("heading", formatAngle(angles.angleUnit, angles.firstAngle)) + .addData("roll", formatAngle(angles.angleUnit, angles.secondAngle)) + .addData("pitch", "%s deg", formatAngle(angles.angleUnit, angles.thirdAngle)); + telemetry.update(); + + idle(); // Always call idle() at the bottom of your while(opModeIsActive()) loop + } + } + + String formatRate(float rate) { + return String.format("%.3f", rate); + } + + String formatAngle(AngleUnit angleUnit, double angle) { + return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle)); + } + + String formatDegrees(double degrees){ + return String.format("%.1f", AngleUnit.DEGREES.normalize(degrees)); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRColor.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRColor.java new file mode 100644 index 0000000..32b37e0 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRColor.java @@ -0,0 +1,138 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import android.app.Activity; +import android.graphics.Color; +import android.view.View; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.ColorSensor; + +/* + * + * This OpMode that shows how to use + * a Modern Robotics Color Sensor. + * + * The OpMode assumes that the color sensor + * is configured with a name of "sensor_color". + * + * You can use the X button on gamepad1 to toggle the LED on and off. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ +@TeleOp(name = "Sensor: MR Color", group = "Sensor") +@Disabled +public class SensorMRColor extends LinearOpMode { + + ColorSensor colorSensor; // Hardware Device Object + + + @Override + public void runOpMode() { + + // hsvValues is an array that will hold the hue, saturation, and value information. + float hsvValues[] = {0F,0F,0F}; + + // values is a reference to the hsvValues array. + final float values[] = hsvValues; + + // get a reference to the RelativeLayout so we can change the background + // color of the Robot Controller app to match the hue detected by the RGB sensor. + int relativeLayoutId = hardwareMap.appContext.getResources().getIdentifier("RelativeLayout", "id", hardwareMap.appContext.getPackageName()); + final View relativeLayout = ((Activity) hardwareMap.appContext).findViewById(relativeLayoutId); + + // bPrevState and bCurrState represent the previous and current state of the button. + boolean bPrevState = false; + boolean bCurrState = false; + + // bLedOn represents the state of the LED. + boolean bLedOn = true; + + // get a reference to our ColorSensor object. + colorSensor = hardwareMap.get(ColorSensor.class, "sensor_color"); + + // Set the LED in the beginning + colorSensor.enableLed(bLedOn); + + // wait for the start button to be pressed. + waitForStart(); + + // while the OpMode is active, loop and read the RGB data. + // Note we use opModeIsActive() as our loop condition because it is an interruptible method. + while (opModeIsActive()) { + + // check the status of the x button on either gamepad. + bCurrState = gamepad1.x; + + // check for button state transitions. + if (bCurrState && (bCurrState != bPrevState)) { + + // button is transitioning to a pressed state. So Toggle LED + bLedOn = !bLedOn; + colorSensor.enableLed(bLedOn); + } + + // update previous state variable. + bPrevState = bCurrState; + + // convert the RGB values to HSV values. + Color.RGBToHSV(colorSensor.red() * 8, colorSensor.green() * 8, colorSensor.blue() * 8, hsvValues); + + // send the info back to driver station using telemetry function. + telemetry.addData("LED", bLedOn ? "On" : "Off"); + telemetry.addData("Clear", colorSensor.alpha()); + telemetry.addData("Red ", colorSensor.red()); + telemetry.addData("Green", colorSensor.green()); + telemetry.addData("Blue ", colorSensor.blue()); + telemetry.addData("Hue", hsvValues[0]); + + // change the background color to match the color detected by the RGB sensor. + // pass a reference to the hue, saturation, and value array as an argument + // to the HSVToColor method. + relativeLayout.post(new Runnable() { + public void run() { + relativeLayout.setBackgroundColor(Color.HSVToColor(0xff, values)); + } + }); + + telemetry.update(); + } + + // Set the panel back to the default color + relativeLayout.post(new Runnable() { + public void run() { + relativeLayout.setBackgroundColor(Color.WHITE); + } + }); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRGyro.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRGyro.java new file mode 100644 index 0000000..91c0062 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRGyro.java @@ -0,0 +1,160 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.IntegratingGyroscope; +import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; + +/* + * This OpMode shows how to use the Modern Robotics Gyro. + * + * The OpMode assumes that the gyro sensor is attached to a Device Interface Module + * I2C channel and is configured with a name of "gyro". + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list +*/ +@TeleOp(name = "Sensor: MR Gyro", group = "Sensor") +@Disabled +public class SensorMRGyro extends LinearOpMode { + + /* In this sample, for illustration purposes we use two interfaces on the one gyro object. + * That's likely atypical: you'll probably use one or the other in any given situation, + * depending on what you're trying to do. {@link IntegratingGyroscope} (and it's base interface, + * {@link Gyroscope}) are common interfaces supported by possibly several different gyro + * implementations. {@link ModernRoboticsI2cGyro}, by contrast, provides functionality that + * is unique to the Modern Robotics gyro sensor. + */ + IntegratingGyroscope gyro; + ModernRoboticsI2cGyro modernRoboticsI2cGyro; + + // A timer helps provide feedback while calibration is taking place + ElapsedTime timer = new ElapsedTime(); + + @Override + public void runOpMode() { + + boolean lastResetState = false; + boolean curResetState = false; + + // Get a reference to a Modern Robotics gyro object. We use several interfaces + // on this object to illustrate which interfaces support which functionality. + modernRoboticsI2cGyro = hardwareMap.get(ModernRoboticsI2cGyro.class, "gyro"); + gyro = (IntegratingGyroscope)modernRoboticsI2cGyro; + // If you're only interested int the IntegratingGyroscope interface, the following will suffice. + // gyro = hardwareMap.get(IntegratingGyroscope.class, "gyro"); + // A similar approach will work for the Gyroscope interface, if that's all you need. + + // Start calibrating the gyro. This takes a few seconds and is worth performing + // during the initialization phase at the start of each OpMode. + telemetry.log().add("Gyro Calibrating. Do Not Move!"); + modernRoboticsI2cGyro.calibrate(); + + // Wait until the gyro calibration is complete + timer.reset(); + while (!isStopRequested() && modernRoboticsI2cGyro.isCalibrating()) { + telemetry.addData("calibrating", "%s", Math.round(timer.seconds())%2==0 ? "|.." : "..|"); + telemetry.update(); + sleep(50); + } + + telemetry.log().clear(); telemetry.log().add("Gyro Calibrated. Press Start."); + telemetry.clear(); telemetry.update(); + + // Wait for the start button to be pressed + waitForStart(); + telemetry.log().clear(); + telemetry.log().add("Press A & B to reset heading"); + + // Loop until we're asked to stop + while (opModeIsActive()) { + + // If the A and B buttons are pressed just now, reset Z heading. + curResetState = (gamepad1.a && gamepad1.b); + if (curResetState && !lastResetState) { + modernRoboticsI2cGyro.resetZAxisIntegrator(); + } + lastResetState = curResetState; + + // The raw() methods report the angular rate of change about each of the + // three axes directly as reported by the underlying sensor IC. + int rawX = modernRoboticsI2cGyro.rawX(); + int rawY = modernRoboticsI2cGyro.rawY(); + int rawZ = modernRoboticsI2cGyro.rawZ(); + int heading = modernRoboticsI2cGyro.getHeading(); + int integratedZ = modernRoboticsI2cGyro.getIntegratedZValue(); + + // Read dimensionalized data from the gyro. This gyro can report angular velocities + // about all three axes. Additionally, it internally integrates the Z axis to + // be able to report an absolute angular Z orientation. + AngularVelocity rates = gyro.getAngularVelocity(AngleUnit.DEGREES); + float zAngle = gyro.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES).firstAngle; + + // Read administrative information from the gyro + int zAxisOffset = modernRoboticsI2cGyro.getZAxisOffset(); + int zAxisScalingCoefficient = modernRoboticsI2cGyro.getZAxisScalingCoefficient(); + + telemetry.addLine() + .addData("dx", formatRate(rates.xRotationRate)) + .addData("dy", formatRate(rates.yRotationRate)) + .addData("dz", "%s deg/s", formatRate(rates.zRotationRate)); + telemetry.addData("angle", "%s deg", formatFloat(zAngle)); + telemetry.addData("heading", "%3d deg", heading); + telemetry.addData("integrated Z", "%3d", integratedZ); + telemetry.addLine() + .addData("rawX", formatRaw(rawX)) + .addData("rawY", formatRaw(rawY)) + .addData("rawZ", formatRaw(rawZ)); + telemetry.addLine().addData("z offset", zAxisOffset).addData("z coeff", zAxisScalingCoefficient); + telemetry.update(); + } + } + + String formatRaw(int rawValue) { + return String.format("%d", rawValue); + } + + String formatRate(float rate) { + return String.format("%.3f", rate); + } + + String formatFloat(float rate) { + return String.format("%.3f", rate); + } + +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMROpticalDistance.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMROpticalDistance.java new file mode 100644 index 0000000..6d2dfa3 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMROpticalDistance.java @@ -0,0 +1,70 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.OpticalDistanceSensor; + +/* + * This OpMode shows how to use a Modern Robotics Optical Distance Sensor + * It assumes that the ODS sensor is configured with a name of "sensor_ods". + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ +@TeleOp(name = "Sensor: MR ODS", group = "Sensor") +@Disabled +public class SensorMROpticalDistance extends LinearOpMode { + + OpticalDistanceSensor odsSensor; // Hardware Device Object + + @Override + public void runOpMode() { + + // get a reference to our Light Sensor object. + odsSensor = hardwareMap.get(OpticalDistanceSensor.class, "sensor_ods"); + + // wait for the start button to be pressed. + waitForStart(); + + // while the OpMode is active, loop and read the light levels. + // Note we use opModeIsActive() as our loop condition because it is an interruptible method. + while (opModeIsActive()) { + + // send the info back to driver station using telemetry function. + telemetry.addData("Raw", odsSensor.getRawLightDetected()); + telemetry.addData("Normal", odsSensor.getLightDetected()); + + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRRangeSensor.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRRangeSensor.java new file mode 100644 index 0000000..2039ef9 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRRangeSensor.java @@ -0,0 +1,70 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cRangeSensor; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +/* + * This OpMode illustrates how to use the Modern Robotics Range Sensor. + * + * The OpMode assumes that the range sensor is configured with a name of "sensor_range". + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * @see MR Range Sensor + */ +@TeleOp(name = "Sensor: MR range sensor", group = "Sensor") +@Disabled // comment out or remove this line to enable this OpMode +public class SensorMRRangeSensor extends LinearOpMode { + + ModernRoboticsI2cRangeSensor rangeSensor; + + @Override public void runOpMode() { + + // get a reference to our compass + rangeSensor = hardwareMap.get(ModernRoboticsI2cRangeSensor.class, "sensor_range"); + + // wait for the start button to be pressed + waitForStart(); + + while (opModeIsActive()) { + telemetry.addData("raw ultrasonic", rangeSensor.rawUltrasonic()); + telemetry.addData("raw optical", rangeSensor.rawOptical()); + telemetry.addData("cm optical", "%.2f cm", rangeSensor.cmOptical()); + telemetry.addData("cm", "%.2f cm", rangeSensor.getDistance(DistanceUnit.CM)); + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREV2mDistance.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREV2mDistance.java new file mode 100644 index 0000000..13883c3 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREV2mDistance.java @@ -0,0 +1,87 @@ +/* +Copyright (c) 2018 FIRST + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of FIRST nor the names of its contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.rev.Rev2mDistanceSensor; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DistanceSensor; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +/* + * This OpMode illustrates how to use the REV Robotics 2M Distance Sensor. + * + * The OpMode assumes that the sensor is configured with a name of "sensor_distance". + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * See the sensor's product page: https://www.revrobotics.com/rev-31-1505/ + */ +@TeleOp(name = "Sensor: REV2mDistance", group = "Sensor") +@Disabled +public class SensorREV2mDistance extends LinearOpMode { + + private DistanceSensor sensorDistance; + + @Override + public void runOpMode() { + // you can use this as a regular DistanceSensor. + sensorDistance = hardwareMap.get(DistanceSensor.class, "sensor_distance"); + + // you can also cast this to a Rev2mDistanceSensor if you want to use added + // methods associated with the Rev2mDistanceSensor class. + Rev2mDistanceSensor sensorTimeOfFlight = (Rev2mDistanceSensor) sensorDistance; + + telemetry.addData(">>", "Press start to continue"); + telemetry.update(); + + waitForStart(); + while(opModeIsActive()) { + // generic DistanceSensor methods. + telemetry.addData("deviceName", sensorDistance.getDeviceName() ); + telemetry.addData("range", String.format("%.01f mm", sensorDistance.getDistance(DistanceUnit.MM))); + telemetry.addData("range", String.format("%.01f cm", sensorDistance.getDistance(DistanceUnit.CM))); + telemetry.addData("range", String.format("%.01f m", sensorDistance.getDistance(DistanceUnit.METER))); + telemetry.addData("range", String.format("%.01f in", sensorDistance.getDistance(DistanceUnit.INCH))); + + // Rev2mDistanceSensor specific methods. + telemetry.addData("ID", String.format("%x", sensorTimeOfFlight.getModelID())); + telemetry.addData("did time out", Boolean.toString(sensorTimeOfFlight.didTimeoutOccur())); + + telemetry.update(); + } + } + +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorTouch.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorTouch.java new file mode 100644 index 0000000..3d79447 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorTouch.java @@ -0,0 +1,78 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.TouchSensor; + +/* + * This OpMode demonstrates how to use a REV Robotics Touch Sensor, REV Robotics Magnetic Limit Switch, or other device + * that implements the TouchSensor interface. Any touch sensor that connects its output to ground when pressed + * (known as "active low") can be configured as a "REV Touch Sensor". This includes REV's Magnetic Limit Switch. + * + * The OpMode assumes that the touch sensor is configured with a name of "sensor_touch". + * + * A REV Robotics Touch Sensor must be configured on digital port number 1, 3, 5, or 7. + * A Magnetic Limit Switch can be configured on any digital port. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Sensor: REV touch sensor", group = "Sensor") +@Disabled +public class SensorTouch extends LinearOpMode { + TouchSensor touchSensor; // Touch sensor Object + + @Override + public void runOpMode() { + + // get a reference to our touchSensor object. + touchSensor = hardwareMap.get(TouchSensor.class, "sensor_touch"); + + // wait for the start button to be pressed. + waitForStart(); + + // while the OpMode is active, loop and read whether the sensor is being pressed. + // Note we use opModeIsActive() as our loop condition because it is an interruptible method. + while (opModeIsActive()) { + + // send the info back to driver station using telemetry function. + if (touchSensor.isPressed()) { + telemetry.addData("Touch Sensor", "Is Pressed"); + } else { + telemetry.addData("Touch Sensor", "Is Not Pressed"); + } + + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityCameraFrameCapture.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityCameraFrameCapture.java new file mode 100644 index 0000000..69420cc --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityCameraFrameCapture.java @@ -0,0 +1,127 @@ +/* + * Copyright (c) 2023 FIRST + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to + * endorse or promote products derived from this software without specific prior + * written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR + * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import android.util.Size; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; + +import java.util.Locale; + +/* + * This OpMode helps calibrate a webcam or RC phone camera, useful for AprilTag pose estimation + * with the FTC VisionPortal. It captures a camera frame (image) and stores it on the Robot Controller + * (Control Hub or RC phone), with each press of the gamepad button X (or Square). + * Full calibration instructions are here: + * + * https://ftc-docs.firstinspires.org/camera-calibration + * + * In Android Studio, copy this class into your "teamcode" folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + * + * In OnBot Java, use "Add File" to add this OpMode from the list of Samples. + */ + +@TeleOp(name = "Utility: Camera Frame Capture", group = "Utility") +@Disabled +public class UtilityCameraFrameCapture extends LinearOpMode +{ + /* + * EDIT THESE PARAMETERS AS NEEDED + */ + final boolean USING_WEBCAM = false; + final BuiltinCameraDirection INTERNAL_CAM_DIR = BuiltinCameraDirection.BACK; + final int RESOLUTION_WIDTH = 640; + final int RESOLUTION_HEIGHT = 480; + + // Internal state + boolean lastX; + int frameCount; + long capReqTime; + + @Override + public void runOpMode() + { + VisionPortal portal; + + if (USING_WEBCAM) + { + portal = new VisionPortal.Builder() + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .setCameraResolution(new Size(RESOLUTION_WIDTH, RESOLUTION_HEIGHT)) + .build(); + } + else + { + portal = new VisionPortal.Builder() + .setCamera(INTERNAL_CAM_DIR) + .setCameraResolution(new Size(RESOLUTION_WIDTH, RESOLUTION_HEIGHT)) + .build(); + } + + while (!isStopRequested()) + { + boolean x = gamepad1.x; + + if (x && !lastX) + { + portal.saveNextFrameRaw(String.format(Locale.US, "CameraFrameCapture-%06d", frameCount++)); + capReqTime = System.currentTimeMillis(); + } + + lastX = x; + + telemetry.addLine("######## Camera Capture Utility ########"); + telemetry.addLine(String.format(Locale.US, " > Resolution: %dx%d", RESOLUTION_WIDTH, RESOLUTION_HEIGHT)); + telemetry.addLine(" > Press X (or Square) to capture a frame"); + telemetry.addData(" > Camera Status", portal.getCameraState()); + + if (capReqTime != 0) + { + telemetry.addLine("\nCaptured Frame!"); + } + + if (capReqTime != 0 && System.currentTimeMillis() - capReqTime > 1000) + { + capReqTime = 0; + } + + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/readme.md b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/readme.md new file mode 100644 index 0000000..326978d --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/readme.md @@ -0,0 +1,45 @@ + +## Caution +No Team-specific code should be placed or modified in this ``.../samples`` folder. + +Samples should be Copied from here, and then Pasted into the team's +[/TeamCode/src/main/java/org/firstinspires/ftc/teamcode](../../../../../../../../../../TeamCode/src/main/java/org/firstinspires/ftc/teamcode) + folder, using the Android Studio cut and paste commands. This automatically changes all file and +class names to be consistent. From there, the sample can be modified to suit the team's needs. + +For more detailed instructions see the /teamcode readme. + +### Naming of Samples + +To gain a better understanding of how the samples are organized, and how to interpret the +naming system, it will help to understand the conventions that were used during their creation. + +These conventions are described (in detail) in the sample_conventions.md file in this folder. + +To summarize: A range of different samples classes will reside in the java/external/samples. +The class names will follow a naming convention which indicates the purpose of each class. +The prefix of the name will be one of the following: + +Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure + of a particular style of OpMode. These are bare bones examples. + +Sensor: This is a Sample OpMode that shows how to use a specific sensor. + It is not intended to drive a functioning robot, it is simply showing the minimal code + required to read and display the sensor values. + +Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base. + It may be used to provide a common baseline driving OpMode, or + to demonstrate how a particular sensor or concept can be used to navigate. + +Concept: This is a sample OpMode that illustrates performing a specific function or concept. + These may be complex, but their operation should be explained clearly in the comments, + or the comments should reference an external doc, guide or tutorial. + Each OpMode should try to only demonstrate a single concept so they are easy to + locate based on their name. These OpModes may not produce a drivable robot. + +After the prefix, other conventions will apply: + +* Sensor class names are constructed as: Sensor - Company - Type +* Robot class names are constructed as: Robot - Mode - Action - OpModetype +* Concept class names are constructed as: Concept - Topic - OpModetype + diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/sample_conventions.md b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/sample_conventions.md new file mode 100644 index 0000000..45968ef --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/sample_conventions.md @@ -0,0 +1,108 @@ +## Sample Class/Opmode conventions +#### V 1.1.0 8/9/2017 + +This document defines the FTC Sample OpMode and Class conventions. + +### OpMode Name + +To gain a better understanding of how the samples are organized, and how to interpret the +naming system, it will help to understand the conventions that were used during their creation. + +To summarize: A range of different samples classes will reside in the java/external/samples. +The class names will follow a naming convention which indicates the purpose of each class. +The prefix of the name will be one of the following: + +Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure + of a particular style of OpMode. These are bare bones examples. + +Sensor: This is a Sample OpMode that shows how to use a specific sensor. + It is not intended to drive a functioning robot, it is simply showing the minimal code + required to read and display the sensor values. + +Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base. + It may be used to provide a common baseline driving OpMode, or + to demonstrate how a particular sensor or concept can be used to navigate. + +Concept: This is a sample OpMode that illustrates performing a specific function or concept. + These may be complex, but their operation should be explained clearly in the comments, + or the comments should reference an external doc, guide or tutorial. + Each OpMode should try to only demonstrate a single concept so they are easy to + locate based on their name. These OpModes may not produce a drivable robot. + +After the prefix, other conventions will apply: + +* Sensor class names should constructed as: Sensor - Company - Type +* Robot class names should be constructed as: Robot - Mode - Action - OpModetype +* Concept class names should be constructed as: Concept - Topic - OpModetype + +### Sample OpMode Content/Style + +Code is formatted as per the Google Style Guide: + +https://google.github.io/styleguide/javaguide.html + +With “Sensor” and “Hardware” samples, the code should demonstrate the essential function, +and not be embellished with too much additional “clever” code. If a sensor has special +addressing needs, or has a variety of modes or outputs, these should be demonstrated as +simply as possible. + +Special programming methods, or robot control techniques should be reserved for “Concept” Samples, +and where possible, Samples should strive to only demonstrate a single concept, +eg: State machine coding, or a User Menu system, and not combine them into a single “all inclusive” +sample. This will prevent an “all inclusive” Sample being deleted just because one part of it +becomes obsolete. + +### Device Configuration Names + +The following device names are used in the external samples + +** Motors: +left_drive +right_drive +left_arm + +** Servos: +left_hand +right_hand +arm +claw + +** Sensors: +sensor_color +sensor_ir +sensor_light +sensor_ods +sensor_range +sensor_touch +sensor_color_distance +sensor_digital +digin +digout + +** Localization: +compass +gyro +imu +navx + +### Device Object Names + +Device Object names should use the same words as the device’s configuration name, but they +should be re-structured to be a suitable Java variable name. This should keep the same word order, +but adopt the style of beginning with a lower case letter, and then each subsequent word +starting with an upper case letter. + +Eg: from the examples above: tool, leftMotor, rightClawServo, rearLightSensor. + +Note: Sometimes it’s helpful to put the device type first, followed by the variant. +eg: motorLeft and motorRight, but this should only be done if the same word order +is used on the device configuration name. + +### OpMode code Comments + +Sample comments should read like normal code comments, that is, as an explanation of what the +sample code is doing. They should NOT be directives to the user, +like: “insert your joystick code here” as these comments typically aren’t +detailed enough to be useful. They also often get left in the code and become garbage. + +Instead, an example of the joystick code should be shown with a comment describing what it is doing. diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcOpModeRegister.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcOpModeRegister.java new file mode 100644 index 0000000..8c6ea59 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcOpModeRegister.java @@ -0,0 +1,70 @@ +/* Copyright (c) 2014, 2015 Qualcomm Technologies Inc + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Qualcomm Technologies Inc nor the names of its contributors +may be used to endorse or promote products derived from this software without +specific prior written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ + +package org.firstinspires.ftc.robotcontroller.internal; + +import com.qualcomm.robotcore.eventloop.opmode.OpModeManager; +import com.qualcomm.robotcore.eventloop.opmode.OpModeRegister; + +import org.firstinspires.ftc.robotcontroller.external.samples.ConceptNullOp; + +/** + * {@link FtcOpModeRegister} is responsible for registering OpModes for use in an FTC game. + * @see #register(OpModeManager) + */ +public class FtcOpModeRegister implements OpModeRegister { + + /** + * {@link #register(OpModeManager)} is called by the SDK game in order to register + * OpMode classes or instances that will participate in an FTC game. + * + * There are two mechanisms by which an OpMode may be registered. + * + * 1) The preferred method is by means of class annotations in the OpMode itself. + * See, for example the class annotations in {@link ConceptNullOp}. + * + * 2) The other, retired, method is to modify this {@link #register(OpModeManager)} + * method to include explicit calls to OpModeManager.register(). + * This method of modifying this file directly is discouraged, as it + * makes updates to the SDK harder to integrate into your code. + * + * @param manager the object which contains methods for carrying out OpMode registrations + * + * @see com.qualcomm.robotcore.eventloop.opmode.TeleOp + * @see com.qualcomm.robotcore.eventloop.opmode.Autonomous + */ + public void register(OpModeManager manager) { + + /** + * Any manual OpMode class registrations should go here. + */ + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java new file mode 100644 index 0000000..3f1f77c --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java @@ -0,0 +1,845 @@ +/* Copyright (c) 2014, 2015 Qualcomm Technologies Inc + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Qualcomm Technologies Inc nor the names of its contributors +may be used to endorse or promote products derived from this software without +specific prior written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ + +package org.firstinspires.ftc.robotcontroller.internal; + +import android.app.ActionBar; +import android.app.Activity; +import android.app.ActivityManager; +import android.content.ComponentName; +import android.content.Context; +import android.content.Intent; +import android.content.ServiceConnection; +import android.content.SharedPreferences; +import android.content.res.Configuration; +import android.hardware.usb.UsbDevice; +import android.hardware.usb.UsbManager; +import android.net.wifi.WifiManager; +import android.os.Bundle; +import android.os.IBinder; +import android.preference.PreferenceManager; +import androidx.annotation.NonNull; +import androidx.annotation.Nullable; +import androidx.annotation.StringRes; +import android.view.Menu; +import android.view.MenuItem; +import android.view.MotionEvent; +import android.view.View; +import android.view.WindowManager; +import android.webkit.WebView; +import android.widget.ImageButton; +import android.widget.LinearLayout; +import android.widget.LinearLayout.LayoutParams; +import android.widget.PopupMenu; +import android.widget.TextView; + +import com.google.blocks.ftcrobotcontroller.ProgrammingWebHandlers; +import com.google.blocks.ftcrobotcontroller.runtime.BlocksOpMode; +import com.qualcomm.ftccommon.ClassManagerFactory; +import com.qualcomm.ftccommon.FtcAboutActivity; +import com.qualcomm.ftccommon.FtcEventLoop; +import com.qualcomm.ftccommon.FtcEventLoopIdle; +import com.qualcomm.ftccommon.FtcRobotControllerService; +import com.qualcomm.ftccommon.FtcRobotControllerService.FtcRobotControllerBinder; +import com.qualcomm.ftccommon.FtcRobotControllerSettingsActivity; +import com.qualcomm.ftccommon.LaunchActivityConstantsList; +import com.qualcomm.ftccommon.LaunchActivityConstantsList.RequestCode; +import com.qualcomm.ftccommon.Restarter; +import com.qualcomm.ftccommon.UpdateUI; +import com.qualcomm.ftccommon.configuration.EditParameters; +import com.qualcomm.ftccommon.configuration.FtcLoadFileActivity; +import com.qualcomm.ftccommon.configuration.RobotConfigFile; +import com.qualcomm.ftccommon.configuration.RobotConfigFileManager; +import com.qualcomm.ftcrobotcontroller.BuildConfig; +import com.qualcomm.ftcrobotcontroller.R; +import com.qualcomm.hardware.HardwareFactory; +import com.qualcomm.robotcore.eventloop.EventLoopManager; +import com.qualcomm.robotcore.eventloop.opmode.FtcRobotControllerServiceState; +import com.qualcomm.robotcore.eventloop.opmode.OpModeRegister; +import com.qualcomm.robotcore.hardware.configuration.LynxConstants; +import com.qualcomm.robotcore.hardware.configuration.Utility; +import com.qualcomm.robotcore.robot.Robot; +import com.qualcomm.robotcore.robot.RobotState; +import com.qualcomm.robotcore.util.ClockWarningSource; +import com.qualcomm.robotcore.util.Device; +import com.qualcomm.robotcore.util.Dimmer; +import com.qualcomm.robotcore.util.ImmersiveMode; +import com.qualcomm.robotcore.util.RobotLog; +import com.qualcomm.robotcore.util.WebServer; +import com.qualcomm.robotcore.wifi.NetworkConnection; +import com.qualcomm.robotcore.wifi.NetworkConnectionFactory; +import com.qualcomm.robotcore.wifi.NetworkType; + +import org.firstinspires.ftc.ftccommon.external.SoundPlayingRobotMonitor; +import org.firstinspires.ftc.ftccommon.internal.AnnotatedHooksClassFilter; +import org.firstinspires.ftc.ftccommon.internal.FtcRobotControllerWatchdogService; +import org.firstinspires.ftc.ftccommon.internal.ProgramAndManageActivity; +import org.firstinspires.ftc.onbotjava.ExternalLibraries; +import org.firstinspires.ftc.onbotjava.OnBotJavaHelperImpl; +import org.firstinspires.ftc.onbotjava.OnBotJavaProgrammingMode; +import org.firstinspires.ftc.robotcore.external.navigation.MotionDetection; +import org.firstinspires.ftc.robotcore.internal.hardware.android.AndroidBoard; +import org.firstinspires.ftc.robotcore.internal.network.DeviceNameManagerFactory; +import org.firstinspires.ftc.robotcore.internal.network.PreferenceRemoterRC; +import org.firstinspires.ftc.robotcore.internal.network.StartResult; +import org.firstinspires.ftc.robotcore.internal.network.WifiDirectChannelChanger; +import org.firstinspires.ftc.robotcore.internal.network.WifiMuteEvent; +import org.firstinspires.ftc.robotcore.internal.network.WifiMuteStateMachine; +import org.firstinspires.ftc.robotcore.internal.opmode.ClassManager; +import org.firstinspires.ftc.robotcore.internal.opmode.OnBotJavaHelper; +import org.firstinspires.ftc.robotcore.internal.system.AppAliveNotifier; +import org.firstinspires.ftc.robotcore.internal.system.AppUtil; +import org.firstinspires.ftc.robotcore.internal.system.Assert; +import org.firstinspires.ftc.robotcore.internal.system.PreferencesHelper; +import org.firstinspires.ftc.robotcore.internal.system.ServiceController; +import org.firstinspires.ftc.robotcore.internal.ui.ThemedActivity; +import org.firstinspires.ftc.robotcore.internal.ui.UILocation; +import org.firstinspires.ftc.robotcore.internal.webserver.RobotControllerWebInfo; +import org.firstinspires.ftc.robotserver.internal.programmingmode.ProgrammingModeManager; +import org.firstinspires.inspection.RcInspectionActivity; +import org.threeten.bp.YearMonth; +import org.xmlpull.v1.XmlPullParserException; + +import java.io.FileNotFoundException; +import java.util.List; +import java.util.Queue; +import java.util.concurrent.ConcurrentLinkedQueue; + +@SuppressWarnings("WeakerAccess") +public class FtcRobotControllerActivity extends Activity + { + public static final String TAG = "RCActivity"; + public String getTag() { return TAG; } + + private static final int REQUEST_CONFIG_WIFI_CHANNEL = 1; + private static final int NUM_GAMEPADS = 2; + + protected WifiManager.WifiLock wifiLock; + protected RobotConfigFileManager cfgFileMgr; + + private OnBotJavaHelper onBotJavaHelper; + + protected ProgrammingModeManager programmingModeManager; + + protected UpdateUI.Callback callback; + protected Context context; + protected Utility utility; + protected StartResult prefRemoterStartResult = new StartResult(); + protected StartResult deviceNameStartResult = new StartResult(); + protected PreferencesHelper preferencesHelper; + protected final SharedPreferencesListener sharedPreferencesListener = new SharedPreferencesListener(); + + protected ImageButton buttonMenu; + protected TextView textDeviceName; + protected TextView textNetworkConnectionStatus; + protected TextView textRobotStatus; + protected TextView[] textGamepad = new TextView[NUM_GAMEPADS]; + protected TextView textOpMode; + protected TextView textErrorMessage; + protected ImmersiveMode immersion; + + protected UpdateUI updateUI; + protected Dimmer dimmer; + protected LinearLayout entireScreenLayout; + + protected FtcRobotControllerService controllerService; + protected NetworkType networkType; + + protected FtcEventLoop eventLoop; + protected Queue receivedUsbAttachmentNotifications; + + protected WifiMuteStateMachine wifiMuteStateMachine; + protected MotionDetection motionDetection; + + private static boolean permissionsValidated = false; + + private WifiDirectChannelChanger wifiDirectChannelChanger; + + protected class RobotRestarter implements Restarter { + + public void requestRestart() { + requestRobotRestart(); + } + + } + + protected boolean serviceShouldUnbind = false; + protected ServiceConnection connection = new ServiceConnection() { + @Override + public void onServiceConnected(ComponentName name, IBinder service) { + FtcRobotControllerBinder binder = (FtcRobotControllerBinder) service; + onServiceBind(binder.getService()); + } + + @Override + public void onServiceDisconnected(ComponentName name) { + RobotLog.vv(FtcRobotControllerService.TAG, "%s.controllerService=null", TAG); + controllerService = null; + } + }; + + @Override + protected void onNewIntent(Intent intent) { + super.onNewIntent(intent); + + if (UsbManager.ACTION_USB_DEVICE_ATTACHED.equals(intent.getAction())) { + UsbDevice usbDevice = intent.getParcelableExtra(UsbManager.EXTRA_DEVICE); + RobotLog.vv(TAG, "ACTION_USB_DEVICE_ATTACHED: %s", usbDevice.getDeviceName()); + + if (usbDevice != null) { // paranoia + // We might get attachment notifications before the event loop is set up, so + // we hold on to them and pass them along only when we're good and ready. + if (receivedUsbAttachmentNotifications != null) { // *total* paranoia + receivedUsbAttachmentNotifications.add(usbDevice); + passReceivedUsbAttachmentsToEventLoop(); + } + } + } + } + + protected void passReceivedUsbAttachmentsToEventLoop() { + if (this.eventLoop != null) { + for (;;) { + UsbDevice usbDevice = receivedUsbAttachmentNotifications.poll(); + if (usbDevice == null) + break; + this.eventLoop.onUsbDeviceAttached(usbDevice); + } + } + else { + // Paranoia: we don't want the pending list to grow without bound when we don't + // (yet) have an event loop + while (receivedUsbAttachmentNotifications.size() > 100) { + receivedUsbAttachmentNotifications.poll(); + } + } + } + + /** + * There are cases where a permission may be revoked and the system restart will restart the + * FtcRobotControllerActivity, instead of the launch activity. Detect when that happens, and throw + * the device back to the permission validator activity. + */ + protected boolean enforcePermissionValidator() { + if (!permissionsValidated) { + RobotLog.vv(TAG, "Redirecting to permission validator"); + Intent permissionValidatorIntent = new Intent(AppUtil.getDefContext(), PermissionValidatorWrapper.class); + startActivity(permissionValidatorIntent); + finish(); + return true; + } else { + RobotLog.vv(TAG, "Permissions validated already"); + return false; + } + } + + public static void setPermissionsValidated() { + permissionsValidated = true; + } + + @Override + protected void onCreate(Bundle savedInstanceState) { + super.onCreate(savedInstanceState); + + if (enforcePermissionValidator()) { + return; + } + + RobotLog.onApplicationStart(); // robustify against onCreate() following onDestroy() but using the same app instance, which apparently does happen + RobotLog.vv(TAG, "onCreate()"); + ThemedActivity.appAppThemeToActivity(getTag(), this); // do this way instead of inherit to help AppInventor + + // Oddly, sometimes after a crash & restart the root activity will be something unexpected, like from the before crash? We don't yet understand + RobotLog.vv(TAG, "rootActivity is of class %s", AppUtil.getInstance().getRootActivity().getClass().getSimpleName()); + RobotLog.vv(TAG, "launchActivity is of class %s", FtcRobotControllerWatchdogService.launchActivity()); + Assert.assertTrue(FtcRobotControllerWatchdogService.isLaunchActivity(AppUtil.getInstance().getRootActivity())); + Assert.assertTrue(AppUtil.getInstance().isRobotController()); + + // Quick check: should we pretend we're not here, and so allow the Lynx to operate as + // a stand-alone USB-connected module? + if (LynxConstants.isRevControlHub()) { + // Double-sure check that we can talk to the DB over the serial TTY + AndroidBoard.getInstance().getAndroidBoardIsPresentPin().setState(true); + } + + context = this; + utility = new Utility(this); + + DeviceNameManagerFactory.getInstance().start(deviceNameStartResult); + + PreferenceRemoterRC.getInstance().start(prefRemoterStartResult); + + receivedUsbAttachmentNotifications = new ConcurrentLinkedQueue(); + eventLoop = null; + + setContentView(R.layout.activity_ftc_controller); + + preferencesHelper = new PreferencesHelper(TAG, context); + preferencesHelper.writeBooleanPrefIfDifferent(context.getString(R.string.pref_rc_connected), true); + preferencesHelper.getSharedPreferences().registerOnSharedPreferenceChangeListener(sharedPreferencesListener); + + // Check if this RC app is from a later FTC season than what was installed previously + int ftcSeasonYearOfPreviouslyInstalledRc = preferencesHelper.readInt(getString(R.string.pref_ftc_season_year_of_current_rc), 0); + int ftcSeasonYearOfCurrentlyInstalledRc = AppUtil.getInstance().getFtcSeasonYear(AppUtil.getInstance().getLocalSdkBuildMonth()).getValue(); + if (ftcSeasonYearOfCurrentlyInstalledRc > ftcSeasonYearOfPreviouslyInstalledRc) { + preferencesHelper.writeIntPrefIfDifferent(getString(R.string.pref_ftc_season_year_of_current_rc), ftcSeasonYearOfCurrentlyInstalledRc); + // Since it's a new FTC season, we should reset certain settings back to their default values. + preferencesHelper.writeBooleanPrefIfDifferent(getString(R.string.pref_warn_about_2_4_ghz_band), true); + preferencesHelper.writeBooleanPrefIfDifferent(getString(R.string.pref_warn_about_obsolete_software), true); + preferencesHelper.writeBooleanPrefIfDifferent(getString(R.string.pref_warn_about_mismatched_app_versions), true); + preferencesHelper.writeBooleanPrefIfDifferent(getString(R.string.pref_warn_about_incorrect_clocks), true); + } + + entireScreenLayout = (LinearLayout) findViewById(R.id.entire_screen); + buttonMenu = (ImageButton) findViewById(R.id.menu_buttons); + buttonMenu.setOnClickListener(new View.OnClickListener() { + @Override + public void onClick(View v) { + PopupMenu popupMenu = new PopupMenu(FtcRobotControllerActivity.this, v); + popupMenu.setOnMenuItemClickListener(new PopupMenu.OnMenuItemClickListener() { + @Override + public boolean onMenuItemClick(MenuItem item) { + return onOptionsItemSelected(item); // Delegate to the handler for the hardware menu button + } + }); + popupMenu.inflate(R.menu.ftc_robot_controller); + AnnotatedHooksClassFilter.getInstance().callOnCreateMenuMethods( + FtcRobotControllerActivity.this, popupMenu.getMenu()); + popupMenu.show(); + } + }); + + updateMonitorLayout(getResources().getConfiguration()); + + BlocksOpMode.setActivityAndWebView(this, (WebView) findViewById(R.id.webViewBlocksRuntime)); + + ExternalLibraries.getInstance().onCreate(); + onBotJavaHelper = new OnBotJavaHelperImpl(); + + /* + * Paranoia as the ClassManagerFactory requires EXTERNAL_STORAGE permissions + * and we've seen on the DS where the finish() call above does not short-circuit + * the onCreate() call for the activity and then we crash here because we don't + * have permissions. So... + */ + if (permissionsValidated) { + ClassManager.getInstance().setOnBotJavaClassHelper(onBotJavaHelper); + ClassManagerFactory.registerFilters(); + ClassManagerFactory.processAllClasses(); + } + + cfgFileMgr = new RobotConfigFileManager(this); + + // Clean up 'dirty' status after a possible crash + RobotConfigFile configFile = cfgFileMgr.getActiveConfig(); + if (configFile.isDirty()) { + configFile.markClean(); + cfgFileMgr.setActiveConfig(false, configFile); + } + + textDeviceName = (TextView) findViewById(R.id.textDeviceName); + textNetworkConnectionStatus = (TextView) findViewById(R.id.textNetworkConnectionStatus); + textRobotStatus = (TextView) findViewById(R.id.textRobotStatus); + textOpMode = (TextView) findViewById(R.id.textOpMode); + textErrorMessage = (TextView) findViewById(R.id.textErrorMessage); + textGamepad[0] = (TextView) findViewById(R.id.textGamepad1); + textGamepad[1] = (TextView) findViewById(R.id.textGamepad2); + immersion = new ImmersiveMode(getWindow().getDecorView()); + dimmer = new Dimmer(this); + dimmer.longBright(); + + programmingModeManager = new ProgrammingModeManager(); + programmingModeManager.register(new ProgrammingWebHandlers()); + programmingModeManager.register(new OnBotJavaProgrammingMode()); + + updateUI = createUpdateUI(); + callback = createUICallback(updateUI); + + PreferenceManager.setDefaultValues(this, R.xml.app_settings, false); + + WifiManager wifiManager = (WifiManager) getApplicationContext().getSystemService(Context.WIFI_SERVICE); + wifiLock = wifiManager.createWifiLock(WifiManager.WIFI_MODE_FULL_HIGH_PERF, ""); + + hittingMenuButtonBrightensScreen(); + + wifiLock.acquire(); + callback.networkConnectionUpdate(NetworkConnection.NetworkEvent.DISCONNECTED); + readNetworkType(); + ServiceController.startService(FtcRobotControllerWatchdogService.class); + bindToService(); + RobotLog.logAppInfo(); + RobotLog.logDeviceInfo(); + AndroidBoard.getInstance().logAndroidBoardInfo(); + + if (preferencesHelper.readBoolean(getString(R.string.pref_wifi_automute), false)) { + initWifiMute(true); + } + + FtcAboutActivity.setBuildTimeFromBuildConfig(BuildConfig.APP_BUILD_TIME); + + // check to see if there is a preferred Wi-Fi to use. + checkPreferredChannel(); + + AnnotatedHooksClassFilter.getInstance().callOnCreateMethods(this); + } + + protected UpdateUI createUpdateUI() { + Restarter restarter = new RobotRestarter(); + UpdateUI result = new UpdateUI(this, dimmer); + result.setRestarter(restarter); + result.setTextViews(textNetworkConnectionStatus, textRobotStatus, textGamepad, textOpMode, textErrorMessage, textDeviceName); + return result; + } + + protected UpdateUI.Callback createUICallback(UpdateUI updateUI) { + UpdateUI.Callback result = updateUI.new Callback(); + result.setStateMonitor(new SoundPlayingRobotMonitor()); + return result; + } + + @Override + protected void onStart() { + super.onStart(); + RobotLog.vv(TAG, "onStart()"); + + entireScreenLayout.setOnTouchListener(new View.OnTouchListener() { + @Override + public boolean onTouch(View v, MotionEvent event) { + dimmer.handleDimTimer(); + return false; + } + }); + } + + @Override + protected void onResume() { + super.onResume(); + RobotLog.vv(TAG, "onResume()"); + + // In case the user just got back from fixing their clock, refresh ClockWarningSource + ClockWarningSource.getInstance().onPossibleRcClockUpdate(); + } + + @Override + protected void onPause() { + super.onPause(); + RobotLog.vv(TAG, "onPause()"); + } + + @Override + protected void onStop() { + // Note: this gets called even when the configuration editor is launched. That is, it gets + // called surprisingly often. So, we don't actually do much here. + super.onStop(); + RobotLog.vv(TAG, "onStop()"); + } + + @Override + protected void onDestroy() { + super.onDestroy(); + RobotLog.vv(TAG, "onDestroy()"); + + shutdownRobot(); // Ensure the robot is put away to bed + if (callback != null) callback.close(); + + PreferenceRemoterRC.getInstance().stop(prefRemoterStartResult); + DeviceNameManagerFactory.getInstance().stop(deviceNameStartResult); + + unbindFromService(); + // If the app manually (?) is stopped, then we don't need the auto-starting function (?) + ServiceController.stopService(FtcRobotControllerWatchdogService.class); + if (wifiLock != null) wifiLock.release(); + if (preferencesHelper != null) preferencesHelper.getSharedPreferences().unregisterOnSharedPreferenceChangeListener(sharedPreferencesListener); + + RobotLog.cancelWriteLogcatToDisk(); + + AnnotatedHooksClassFilter.getInstance().callOnDestroyMethods(this); + } + + protected void bindToService() { + readNetworkType(); + Intent intent = new Intent(this, FtcRobotControllerService.class); + intent.putExtra(NetworkConnectionFactory.NETWORK_CONNECTION_TYPE, networkType); + serviceShouldUnbind = bindService(intent, connection, Context.BIND_AUTO_CREATE); + } + + protected void unbindFromService() { + if (serviceShouldUnbind) { + unbindService(connection); + serviceShouldUnbind = false; + } + } + + protected void readNetworkType() { + // Control hubs are always running the access point model. Everything else, for the time + // being always runs the Wi-Fi Direct model. + if (Device.isRevControlHub() == true) { + networkType = NetworkType.RCWIRELESSAP; + } else { + networkType = NetworkType.fromString(preferencesHelper.readString(context.getString(R.string.pref_pairing_kind), NetworkType.globalDefaultAsString())); + } + + // update the app_settings + preferencesHelper.writeStringPrefIfDifferent(context.getString(R.string.pref_pairing_kind), networkType.toString()); + } + + @Override + public void onWindowFocusChanged(boolean hasFocus) { + super.onWindowFocusChanged(hasFocus); + + if (hasFocus) { + immersion.hideSystemUI(); + getWindow().setFlags(WindowManager.LayoutParams.FLAG_TRANSLUCENT_NAVIGATION, WindowManager.LayoutParams.FLAG_TRANSLUCENT_NAVIGATION); + } + } + + @Override + public boolean onCreateOptionsMenu(Menu menu) { + getMenuInflater().inflate(R.menu.ftc_robot_controller, menu); + AnnotatedHooksClassFilter.getInstance().callOnCreateMenuMethods(this, menu); + return true; + } + + private boolean isRobotRunning() { + if (controllerService == null) { + return false; + } + + Robot robot = controllerService.getRobot(); + + if ((robot == null) || (robot.eventLoopManager == null)) { + return false; + } + + RobotState robotState = robot.eventLoopManager.state; + + if (robotState != RobotState.RUNNING) { + return false; + } else { + return true; + } + } + + @Override + public boolean onOptionsItemSelected(MenuItem item) { + int id = item.getItemId(); + + if (id == R.id.action_program_and_manage) { + if (isRobotRunning()) { + Intent programmingModeIntent = new Intent(AppUtil.getDefContext(), ProgramAndManageActivity.class); + RobotControllerWebInfo webInfo = programmingModeManager.getWebServer().getConnectionInformation(); + programmingModeIntent.putExtra(LaunchActivityConstantsList.RC_WEB_INFO, webInfo.toJson()); + startActivity(programmingModeIntent); + } else { + AppUtil.getInstance().showToast(UILocation.ONLY_LOCAL, context.getString(R.string.toastWifiUpBeforeProgrammingMode)); + } + } else if (id == R.id.action_inspection_mode) { + Intent inspectionModeIntent = new Intent(AppUtil.getDefContext(), RcInspectionActivity.class); + startActivity(inspectionModeIntent); + return true; + } else if (id == R.id.action_restart_robot) { + dimmer.handleDimTimer(); + AppUtil.getInstance().showToast(UILocation.BOTH, context.getString(R.string.toastRestartingRobot)); + requestRobotRestart(); + return true; + } + else if (id == R.id.action_configure_robot) { + EditParameters parameters = new EditParameters(); + Intent intentConfigure = new Intent(AppUtil.getDefContext(), FtcLoadFileActivity.class); + parameters.putIntent(intentConfigure); + startActivityForResult(intentConfigure, RequestCode.CONFIGURE_ROBOT_CONTROLLER.ordinal()); + } + else if (id == R.id.action_settings) { + // historical: this once erroneously used FTC_CONFIGURE_REQUEST_CODE_ROBOT_CONTROLLER + Intent settingsIntent = new Intent(AppUtil.getDefContext(), FtcRobotControllerSettingsActivity.class); + startActivityForResult(settingsIntent, RequestCode.SETTINGS_ROBOT_CONTROLLER.ordinal()); + return true; + } + else if (id == R.id.action_about) { + Intent intent = new Intent(AppUtil.getDefContext(), FtcAboutActivity.class); + startActivity(intent); + return true; + } + else if (id == R.id.action_exit_app) { + + //Clear backstack and everything to prevent edge case where VM might be + //restarted (after it was exited) if more than one activity was on the + //backstack for some reason. + finishAffinity(); + + //For lollipop and up, we can clear ourselves from the recents list too + if (android.os.Build.VERSION.SDK_INT >= android.os.Build.VERSION_CODES.LOLLIPOP) { + ActivityManager manager = (ActivityManager) getSystemService(ACTIVITY_SERVICE); + List tasks = manager.getAppTasks(); + + for (ActivityManager.AppTask task : tasks) { + task.finishAndRemoveTask(); + } + } + + // Allow the user to use the Control Hub operating system's UI, instead of relaunching the app + AppAliveNotifier.getInstance().disableAppWatchdogUntilNextAppStart(); + + //Finally, nuke the VM from orbit + AppUtil.getInstance().exitApplication(); + + return true; + } + + return super.onOptionsItemSelected(item); + } + + @Override + public void onConfigurationChanged(Configuration newConfig) { + super.onConfigurationChanged(newConfig); + // don't destroy assets on screen rotation + updateMonitorLayout(newConfig); + } + + /** + * Updates the orientation of monitorContainer (which contains cameraMonitorView) + * based on the given configuration. Makes the children split the space. + */ + private void updateMonitorLayout(Configuration configuration) { + LinearLayout monitorContainer = (LinearLayout) findViewById(R.id.monitorContainer); + if (configuration.orientation == Configuration.ORIENTATION_LANDSCAPE) { + // When the phone is landscape, lay out the monitor views horizontally. + monitorContainer.setOrientation(LinearLayout.HORIZONTAL); + for (int i = 0; i < monitorContainer.getChildCount(); i++) { + View view = monitorContainer.getChildAt(i); + view.setLayoutParams(new LayoutParams(0, LayoutParams.MATCH_PARENT, 1 /* weight */)); + } + } else { + // When the phone is portrait, lay out the monitor views vertically. + monitorContainer.setOrientation(LinearLayout.VERTICAL); + for (int i = 0; i < monitorContainer.getChildCount(); i++) { + View view = monitorContainer.getChildAt(i); + view.setLayoutParams(new LayoutParams(LayoutParams.MATCH_PARENT, 0, 1 /* weight */)); + } + } + monitorContainer.requestLayout(); + } + + @Override + protected void onActivityResult(int request, int result, Intent intent) { + if (request == REQUEST_CONFIG_WIFI_CHANNEL) { + if (result == RESULT_OK) { + AppUtil.getInstance().showToast(UILocation.BOTH, context.getString(R.string.toastWifiConfigurationComplete)); + } + } + // was some historical confusion about launch codes here, so we err safely + if (request == RequestCode.CONFIGURE_ROBOT_CONTROLLER.ordinal() || request == RequestCode.SETTINGS_ROBOT_CONTROLLER.ordinal()) { + // We always do a refresh, whether it was a cancel or an OK, for robustness + shutdownRobot(); + cfgFileMgr.getActiveConfigAndUpdateUI(); + updateUIAndRequestRobotSetup(); + } + } + + public void onServiceBind(final FtcRobotControllerService service) { + RobotLog.vv(FtcRobotControllerService.TAG, "%s.controllerService=bound", TAG); + controllerService = service; + updateUI.setControllerService(controllerService); + + controllerService.setOnBotJavaHelper(onBotJavaHelper); + + updateUIAndRequestRobotSetup(); + programmingModeManager.setState(new FtcRobotControllerServiceState() { + @NonNull + @Override + public WebServer getWebServer() { + return service.getWebServer(); + } + + @Nullable + @Override + public OnBotJavaHelper getOnBotJavaHelper() { + return service.getOnBotJavaHelper(); + } + + @Override + public EventLoopManager getEventLoopManager() { + return service.getRobot().eventLoopManager; + } + }); + + AnnotatedHooksClassFilter.getInstance().callWebHandlerRegistrarMethods(this, + service.getWebServer().getWebHandlerManager()); + } + + private void updateUIAndRequestRobotSetup() { + if (controllerService != null) { + callback.networkConnectionUpdate(controllerService.getNetworkConnectionStatus()); + callback.updateRobotStatus(controllerService.getRobotStatus()); + // Only show this first-time toast on headless systems: what we have now on non-headless suffices + requestRobotSetup(LynxConstants.isRevControlHub() + ? new Runnable() { + @Override public void run() { + showRestartRobotCompleteToast(R.string.toastRobotSetupComplete); + } + } + : null); + } + } + + private void requestRobotSetup(@Nullable Runnable runOnComplete) { + if (controllerService == null) return; + + RobotConfigFile file = cfgFileMgr.getActiveConfigAndUpdateUI(); + HardwareFactory hardwareFactory = new HardwareFactory(context); + try { + hardwareFactory.setXmlPullParser(file.getXml()); + } catch (FileNotFoundException | XmlPullParserException e) { + RobotLog.ww(TAG, e, "Unable to set configuration file %s. Falling back on noConfig.", file.getName()); + file = RobotConfigFile.noConfig(cfgFileMgr); + try { + hardwareFactory.setXmlPullParser(file.getXml()); + cfgFileMgr.setActiveConfigAndUpdateUI(false, file); + } catch (FileNotFoundException | XmlPullParserException e1) { + RobotLog.ee(TAG, e1, "Failed to fall back on noConfig"); + } + } + + OpModeRegister userOpModeRegister = createOpModeRegister(); + eventLoop = new FtcEventLoop(hardwareFactory, userOpModeRegister, callback, this); + FtcEventLoopIdle idleLoop = new FtcEventLoopIdle(hardwareFactory, userOpModeRegister, callback, this); + + controllerService.setCallback(callback); + controllerService.setupRobot(eventLoop, idleLoop, runOnComplete); + + passReceivedUsbAttachmentsToEventLoop(); + AndroidBoard.showErrorIfUnknownControlHub(); + + AnnotatedHooksClassFilter.getInstance().callOnCreateEventLoopMethods(this, eventLoop); + } + + protected OpModeRegister createOpModeRegister() { + return new FtcOpModeRegister(); + } + + private void shutdownRobot() { + if (controllerService != null) controllerService.shutdownRobot(); + } + + private void requestRobotRestart() { + AppUtil.getInstance().showToast(UILocation.BOTH, AppUtil.getDefContext().getString(R.string.toastRestartingRobot)); + // + RobotLog.clearGlobalErrorMsg(); + RobotLog.clearGlobalWarningMsg(); + shutdownRobot(); + requestRobotSetup(new Runnable() { + @Override public void run() { + showRestartRobotCompleteToast(R.string.toastRestartRobotComplete); + } + }); + } + + private void showRestartRobotCompleteToast(@StringRes int resid) { + AppUtil.getInstance().showToast(UILocation.BOTH, AppUtil.getDefContext().getString(resid)); + } + + private void checkPreferredChannel() { + // For P2P network, check to see what preferred channel is. + if (networkType == NetworkType.WIFIDIRECT) { + int prefChannel = preferencesHelper.readInt(getString(com.qualcomm.ftccommon.R.string.pref_wifip2p_channel), -1); + if (prefChannel == -1) { + prefChannel = 0; + RobotLog.vv(TAG, "pref_wifip2p_channel: No preferred channel defined. Will use a default value of %d", prefChannel); + } else { + RobotLog.vv(TAG, "pref_wifip2p_channel: Found existing preferred channel (%d).", prefChannel); + } + + // attempt to set the preferred channel. + RobotLog.vv(TAG, "pref_wifip2p_channel: attempting to set preferred channel..."); + wifiDirectChannelChanger = new WifiDirectChannelChanger(); + wifiDirectChannelChanger.changeToChannel(prefChannel); + } + } + + protected void hittingMenuButtonBrightensScreen() { + ActionBar actionBar = getActionBar(); + if (actionBar != null) { + actionBar.addOnMenuVisibilityListener(new ActionBar.OnMenuVisibilityListener() { + @Override + public void onMenuVisibilityChanged(boolean isVisible) { + if (isVisible) { + dimmer.handleDimTimer(); + } + } + }); + } + } + + protected class SharedPreferencesListener implements SharedPreferences.OnSharedPreferenceChangeListener { + @Override public void onSharedPreferenceChanged(SharedPreferences sharedPreferences, String key) { + if (key.equals(context.getString(R.string.pref_app_theme))) { + ThemedActivity.restartForAppThemeChange(getTag(), getString(R.string.appThemeChangeRestartNotifyRC)); + } else if (key.equals(context.getString(R.string.pref_wifi_automute))) { + if (preferencesHelper.readBoolean(context.getString(R.string.pref_wifi_automute), false)) { + initWifiMute(true); + } else { + initWifiMute(false); + } + } + } + } + + protected void initWifiMute(boolean enable) { + if (enable) { + wifiMuteStateMachine = new WifiMuteStateMachine(); + wifiMuteStateMachine.initialize(); + wifiMuteStateMachine.start(); + + motionDetection = new MotionDetection(2.0, 10); + motionDetection.startListening(); + motionDetection.registerListener(new MotionDetection.MotionDetectionListener() { + @Override + public void onMotionDetected(double vector) + { + wifiMuteStateMachine.consumeEvent(WifiMuteEvent.USER_ACTIVITY); + } + }); + } else { + wifiMuteStateMachine.stop(); + wifiMuteStateMachine = null; + motionDetection.stopListening(); + motionDetection.purgeListeners(); + motionDetection = null; + } + } + + @Override + public void onUserInteraction() { + if (wifiMuteStateMachine != null) { + wifiMuteStateMachine.consumeEvent(WifiMuteEvent.USER_ACTIVITY); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/PermissionValidatorWrapper.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/PermissionValidatorWrapper.java new file mode 100644 index 0000000..a0094bc --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/PermissionValidatorWrapper.java @@ -0,0 +1,91 @@ +/* + * Copyright (c) 2018 Craig MacFarlane + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted + * (subject to the limitations in the disclaimer below) provided that the following conditions are + * met: + * + * Redistributions of source code must retain the above copyright notice, this list of conditions + * and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this list of conditions + * and the following disclaimer in the documentation and/or other materials provided with the + * distribution. + * + * Neither the name of Craig MacFarlane nor the names of its contributors may be used to + * endorse or promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS LICENSE. THIS + * SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, + * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF + * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ +package org.firstinspires.ftc.robotcontroller.internal; + +import android.Manifest; +import android.os.Bundle; + +import com.qualcomm.ftcrobotcontroller.R; + +import org.firstinspires.ftc.robotcore.internal.system.Misc; +import org.firstinspires.ftc.robotcore.internal.system.PermissionValidatorActivity; + +import java.util.ArrayList; +import java.util.List; + +public class PermissionValidatorWrapper extends PermissionValidatorActivity { + + private final String TAG = "PermissionValidatorWrapper"; + + /* + * The list of dangerous permissions the robot controller needs. + */ + protected List robotControllerPermissions = new ArrayList() {{ + add(Manifest.permission.WRITE_EXTERNAL_STORAGE); + add(Manifest.permission.READ_EXTERNAL_STORAGE); + add(Manifest.permission.CAMERA); + add(Manifest.permission.ACCESS_COARSE_LOCATION); + add(Manifest.permission.ACCESS_FINE_LOCATION); + add(Manifest.permission.READ_PHONE_STATE); + }}; + + private final static Class startApplication = FtcRobotControllerActivity.class; + + public String mapPermissionToExplanation(final String permission) { + if (permission.equals(Manifest.permission.WRITE_EXTERNAL_STORAGE)) { + return Misc.formatForUser(R.string.permRcWriteExternalStorageExplain); + } else if (permission.equals(Manifest.permission.READ_EXTERNAL_STORAGE)) { + return Misc.formatForUser(R.string.permRcReadExternalStorageExplain); + } else if (permission.equals(Manifest.permission.CAMERA)) { + return Misc.formatForUser(R.string.permRcCameraExplain); + } else if (permission.equals(Manifest.permission.ACCESS_COARSE_LOCATION)) { + return Misc.formatForUser(R.string.permAccessLocationExplain); + } else if (permission.equals(Manifest.permission.ACCESS_FINE_LOCATION)) { + return Misc.formatForUser(R.string.permAccessLocationExplain); + } else if (permission.equals(Manifest.permission.READ_PHONE_STATE)) { + return Misc.formatForUser(R.string.permReadPhoneState); + } + return Misc.formatForUser(R.string.permGenericExplain); + } + + @Override + protected void onCreate(Bundle savedInstanceState) + { + super.onCreate(savedInstanceState); + + permissions = robotControllerPermissions; + } + + protected Class onStartApplication() + { + FtcRobotControllerActivity.setPermissionsValidated(); + return startApplication; + } +} diff --git a/FtcRobotController/src/main/res/drawable-xhdpi/icon_menu.png b/FtcRobotController/src/main/res/drawable-xhdpi/icon_menu.png new file mode 100644 index 0000000000000000000000000000000000000000..6b9e997c5624fad049a7192d54034336f5216eaa GIT binary patch literal 975 zcmaJ=&ui0A91l2#lnrl!!r+mCOj+|@vb9~p;^^|4t!PT=I_)m9G#pWdU$RplDe*&&7KHhQB($9^!a$R3dVwx}KYA+y&(+2IISW~#L0aB&Cn77IO5N#f zIjSoz+y?WB#tD4FY>@cdL98XZ*yYvuIlYD==(?~iT|7^!VO4=aBLZ?#KIkKb4NO_K;|1yE%`VEav~mzLJ8(!D>muioJkQlktRkF8ra7k=vRs-iE*7!L?Zr|q zUo=#?kHys4@kzk?Sa*Px(NJtE2tVYJF^RlK#5E)8gKpuPH#`?Sl&^<%hvtGxL$Q!2 z1Jm*THSYcu*HC&Kh?g4!`ICTaKH}nj-#mN6ABJAW#d>n~?W3F#z}Uz!{`Gr5_jnqA zUORkw1MD9>KQZ@4-u-yB1}Daz{yaHcMcW7Kr}vK0-lwbUg8E?e#p==5Tj>)kmn!)6 H;`)<6M=di_ literal 0 HcmV?d00001 diff --git a/FtcRobotController/src/main/res/drawable-xhdpi/icon_robotcontroller.png b/FtcRobotController/src/main/res/drawable-xhdpi/icon_robotcontroller.png new file mode 100644 index 0000000000000000000000000000000000000000..022552f409c06b5c93aec2300324913fadcaac51 GIT binary patch literal 4777 zcmaJ_c|4SR7gtJDDs*K~O=GQ?VayCO*_T04wire3n1zWk%#3}hTW&>TUt(;PtwgS+ zK@45X5GEnJQfSmIhA7wmj^4WO{oLh!o%-aYecmU{T7`i=$NDstP2|`AAiaP;lNyd5*>mTa1=5ayXgZCBKr!^LPEL*LB3d=7l96RCwP#2^g#UD1`v>h*8@4h ztu?HDO$eSOix4WoKIF6mF2oCm#DfgrU~f>wyH3{&x#x-#=)5Xy4K#NEjpt>kH8YYiw@m zyQ8)B|92&m|L~^K?Fj#i_aBLA4#B z;L%hH8TfVMDAK>j0{vIMZ&>`l$D;MGSco7P$ma3>>A1g}1U0lN{!zRF<&W|w_y}sA zDk$y~mGR|5LVFKbni@F-4S&p9O8TMiht{r)7yuC?vd?Irt?fQ-MI%jFznc<2z@oee znm1)}`NvXDuE#hOpKMV(yQEYm8N-2c^v>CKn1W>C=l1Wjk%&39-^dxCrZg4QbzCIv zGdtpCRjolz5k*|s>P*8(PJK9KaXR$4u(0sfeZuNj*GKcxAJkxIQN$=B)NGshxB~Yk za|e8{*DHCo2@v-zvyV9ecTGM3i}&Huxe3f6W^EC4*v;jBjjh&o>XqbQc2e&1e7S~6 z8~LuxQbW@=(fOI^BF3#{f-La3^jU)L*)sd*2Mc|@tnC{OfoZEzN7JZQF;OL9PKgDuNcat z%fTC7jFtpn$2nn$mNN0-6_MJ)=Wt2grmHEu0B%66)1BbfxC+AlHurm^9P&~pO~ITLm#m)Cwb?;}p?yCW?p ztLL|g+}#2Y-0>3e5<>r1_`8SX-Je1Cc4hQ8IVMpS%PaXSuuynyBp@_Fv*kfMlxxP! zXEwpP%!85z3F{eC3eh_Oa+Xz*MIiA`5 zx`^0QGabwoINvU;Ee*yx{i+nck*uNWuIL>#F?Mn04qFGMyQjD$Ags}>&V|O}=^$qh zjXR2cARF8cc9=iXa*Yw=Ud59rUCQpw%yqv1{zY-OZeB2v@ko#ekoN+n_IdW%uHMnN zx8}A)Uq|4i7k(!^?o~21#dq#bdd|@qzQ=#Me+QHprN_&dd^>c~4~R=myWBSyP5x?) z;oOgdHl_pA%NDwt(iObI~6%oEXrGWa~ADOx69 z+jkaJj?{1Y2uZ|T5e2n^- z$B+B080CsSaidqM7<_hmU$ag8N*#K&csJWU)iNlNN9H1;aL{`fhlZY(_4VFl;9B>5 zc_f+I5ko4fi7>D&{&FJh#-*wnS#mwUV)b>a!-J8^XQda{Rk6?J*ljup{nDk>$j=$` zk$sUg-LmY-0AoFgBt}`_kGx5nW?h@Pf-??M8Hl&HpWjv_$kj5|bnLUs-|8y>6@;K$ zp<97}77Tlm{V{bhA1cbi)~>eSLEa4?nQ}W`@5XJzSOs|YpOSG|PKZo6jzD=P@NO^` z$+g5>9-IpnD+PDX`x>kEV`(DjyTETwbOpk6t<<+$-JV)*tGtPWz<4b+}VHeB;I3$VlFyM0?uMq6s zC9}xA0H`*jrI;dgqygrfyk#~|pK>+kXZ;2jrlJ;X3O?Htmcc*YC3A^`a`jTI%Ovt& z4SE5{W=M=#{UaQ^+kCC3?!mPj*NMB{K2Pey?*%t6QtDQSDjj2MiX)DB!u<|EN|Y$f zCSNM?ms9T45+7_(k4UU@o9N;%bg`a|G`GoUhO+fo`Ol_{7lpGI_N{5dH)LDi23I2{ zhe3U@W~qKA2jN|>hr2H{ZvfxCT01W}bF{xnWgTE($f+JuS>Y@6cRY*q%6VDO4)Nf; z^Op!bqR?!-9WcR}X&92%vFv<6uCgsqs-|QK9F)pOs9Jf3T9eXe(Y3a0n`_z0q`J4b zxbeqQquNQHK3lIi8BFZPGa87M9@X<7Mq>|tY)M>g`bi;t=YFNlr9lt}6HA z<(%Af*9nBTP)kcsnvs;F)`dIi&gLc6jc1Y_@txP>k1OpCd3W!Q%d_XjJ@R9h1%2|a z8>XrLQS}YCGq(X(7)Pm$!koNmRpc28 zE4G7hkER2QB5lF_N6I`NBiuRJmmQ`?vL`ous4gJv&dEw0?wRBX*qObfXu@V_? zKKqDiRk8}z#xGB8;Xgq0Poe;YMwB|DgM2-Chuj?P z@vfm*hTroUa!bCQzW1~FQ>(|J=;vu}YHtwFUP!ikS|=v0&Q~emSc7a9P039dm`J?l z2H+jRv8?<69w``30KfWR3}YxlL~hr;nd#@|m-S(*Tr(T()|7anB*X{JWx<_|4d3!KfTiOU6`Ww`(k?SU!+Zboa@lKoB zA@pUj$b(lVl@tgldEz0OBq86qA94V1Rr9>okG~Ja4k!lVLab63cr8z*5Cyc0uVN{$ z2;PMU^t|WucL93yxx2E>j_l3r>(~yke+a0tI()Lqt%&BI$YUS4DCIS*=2|<)7rM~v z-M?@I7vi0$@v^l`VDJWKM?4)GjaA>Sx=>%&CuGx!`11h$Hd6mSS^sMJlWu&JAU|uk zWHC9YZFUTi{EPc3yW0Uz=Nd=i*UHY=p)*WE9!A0~ZtK#Q{c>cI7_5M?bmlG{{rvH* z1~dMpFVhxa?W`9w*32xX6Kn*=mM?9@zOw4goNNuXw3D-zJRdr_OEUH4+D&2%@%myV z=0Uy;n|LCYHTsmwJ$9f>Ju1dYL_~i&;kSR-3jDQx5Q6qM?O@(g()Hk%*cL=Uyh5`>%ZaF#XvV~^7aLn) zdr1h3pa_axAM`cui4qIzvZxJRrnjSL9ufosV`GqOuWIE zGydrs?*Ow + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/FtcRobotController/src/main/res/menu/ftc_robot_controller.xml b/FtcRobotController/src/main/res/menu/ftc_robot_controller.xml new file mode 100644 index 0000000..657c1aa --- /dev/null +++ b/FtcRobotController/src/main/res/menu/ftc_robot_controller.xml @@ -0,0 +1,78 @@ + + +

+ + + + + + + + + + + + + + + diff --git a/FtcRobotController/src/main/res/raw/gold.wav b/FtcRobotController/src/main/res/raw/gold.wav new file mode 100644 index 0000000000000000000000000000000000000000..3a7baf88159a373d490064e774d8796e02bcf0bd GIT binary patch literal 104016 zcmd43cTiJb6fSx~OX$5Rz4xjVK~St92q>T^im2EtV!?u9@4YK3f}(;53ZisWdI=;1 zNFj|t0%>#zCFiZ*{o~D@H}B5<=e?Obdm@l?&faUU^{sEMz4kt?3tqW$^-~P8VQI{Y zT?doRG!O*AATr2>QuswAgUBHo$j*f11S`152wjV!(n;xiX^FHNZ9>zf@1(=%97>n+ zq-+#HeBt^!bUXSBC7_*Bp;QquL}sO0=o|D0Dvw&D!RRhj12sUON3V5C zT7Z6$5~M58Q1l|YAFV*E&}j5L`a}9xdI^1i#!8RFJ!JH_^tv>dL4B^{b2O7Fpe*Lvk(nI_94SkR2qx~BZjCQ-17lVL!Y2CQVsMN zdJV;(2+BtV=u)^=g%+a2QZ=+6ZA0&(S?FTuMGG-UOi(XW53xjap~ps)jV?f!ql%~$ z|h!k~2)nEoLXenqT3w?^_pt)$fv{KrKQqgK@y)+s0 zAcOWv+0t_9IvC$qx&q|6h)zkds5P=2NkXI07wBcUYYS+6DLM{gt%fvxfxbr9LqAlMfo=thSi#)(A^VYPX^!+V zdKCUzh(3flCZkDk)fII60-cpkNLM1hh|~WVOM;$6kE7o~_7|X&dg*|42z0anbw{I+ z9S9bklNurGkk?=j5saaSEJDJe&x^=oBpoe5Un5VEUuYS+5Q#-zp!Y$}Kd3q4fII}9 zn8F-)g0>za&ydrgMNJs<3CQu^bs0nznGf>#BVI@s;B*%niw;T!QUno$J`2zj=mqG# z6MoADE3r`(u%;%o&`&UW1I+^)x1lUlg04XifdyhvdBhpvflrt~e`Juy z7J9e zHL?aY=7ekkpEW?{!JGq7D?|%^u7PWD;4^7x0n&~9ME{|>(61egu?p~W9=QmzFGCZc zUvKdEgUEH@DP6<>S&RrlL&pG_MljPjK>vPZBVeou6(KmxB+S(qV}v<~oJI;k?rNk9 z7-BzSh*^LsMdgrNNG@_2P`d{?h*ZH0e<6R7*T`F>867|aki+nE0`))=kaaNc|NKQN zRYrdU16cz;o&v@lz_zWxWjI71@ZBTTggH^r9(ZaPaFdTp0ipXq*8e!~D)h?*k9>=K zK_WnJVdxQbw{*93BX}JatQZQ`z5=K>1df>xGw%ieF$WCYfM*fJ2T<;hbOR;~!EQN# zwaZ8j5(an)2RSQ2_cdU{co;1keE%Er0Z~LkkP3u^u+a|m2f{}eBc;Lj*MK7y!K^Uw>`rtK`XA;j!G{b${#fwBbgIQy}K}FJE(3?B79<&8G7Y9CkA5dZiTHgh%n1D`7%^;%e zN4Ge!q02cG^qlNdsX@f=Cuc&StwmD^^!(Q&qy&+M4AF|OjbHFcMRrvO`0kF zBCV0Sp*ztH(j!tk(9c=vUFkK5m#)$XkX9KTkx~K8z0g-2S|%-!?gQMdkR}3;@*#c} zpzeS=ZNw3AfVpge&#ZJ>x)%8AC-~4w~Y%ueIgZ>}`2o6%CzladQA}o{(ev*#7fykbS`~yDuiWZ~ehzh0z z_@fJk?(A7qW3$75Uj)6~d z(O#5*^noXAh1st_`Vl=$2J#L5b;s1e7<_~b8Ie19<$mN5=)DK-e+_;s@>GBN^QvlAFA5j=bgaKXEs2atHodrUCMF(2cHIS=05gD7IoL5z5T zJc9XwW`QFbpdTf8(g5R#F^1ek3AA$&c%T`TL2Lm5XCM~L!|cJBA)62m;*4=ec0~0TYP11~!_5j8G5bkMV=p)PYEVe}_>4d?Sn4fZc2`B`|~gpr>N6 z$r50#GSJ3fh{A1PB^t!ydtlK~@cmz4i)7>>viUzu!!!F~77s8lG4aTD#0j$$^BnNI z26G5=5_l~KSUU)_3-cEFid@7z!>k7DuLqffK!zC1RZJ!F1EC-_phI;`4Cs~*a?L{A zCL-++d$7oRXw{JK1j8s0VIjM80bPs%=Qp7S7z0cjT;qb~?!dfzkO{ED1CU}1g27Ax z`ZvH#B#?hxKr4|tc-{me<2i`rUjJo9NDEj}5wubW_-sVS&@n^}Qw*{^0{bigM2!Fn zA0U?zAJiCK1|C@sV^u*v8W6pD!Q0Bgi(-HS!eOK=$bzzwQqYe)U~UIwc$*>Wxr292 zfSw1@+ekh_2HkxETN)r{5UYQIHj9x+h%b@gOOGLrF2HzVsOS{L!eW?p9x#3=L`iqx z1Yi~D=_h1%p`fD&;G?d9rJcx0$TiE*RPePv6bIfq1l%G<_X0ldq1ot1$h&eOPcVVF zDusA3E;WKkI}DkS9YnJnbT8z=eBf6D;0rTY4V(v$I0YG0JTOl!#6>+o!4zaJFM+oX z0Y|)m9`ArY6cBmvtXQN1BFQAG2)^(FY&3}00k<53oM#1K=Ns@~I^^Ja5I&mo&J zcQFHy=}3Xc9s{mTF&^NIOTo^efZaOCeLErNiU|2qSG?}6Eh(FPr7BP?VCW*=rA z=uZYCgV_M%QXmJw0w3Q%t|EGn`#M7of`_>H9CCjy%xe(VG+nR)QibSQ2D!#E$Umzg zF1JH&L5DbW5>_tR(md%?$iY=1G6caIWHYSM?t+XukhP9W=U^Rch3t}^lpcV*uL!c@ z|6;~M$Q=Zb-~5G~>KoX)4iO_wU_;0#!0!gZhKIq+^f5O7W_93#;JtNtO{X*4Q(NgU@*(UOO-Gd7%a$t4*WU~)-tn@zqtdhg3)v6Z@^0n zWHVI2hY@%x5g1`DU_lP<4+9jPf_(f6at+pMLFi2+3$SH^l)y8&AcYKgKOgwzG5G6R zu(dO+RI4EZa3Dk32I~hK_}dhs=u7D}Sh-)78bHM$dG7Gsxw)HjUbEq|D`xl4%Figx z-gZ9E9Je~j%I=r#AZL*AoGqMM4UxvAd7i~@ zSs%G3QKpE7G$W?!@#+_ceh(E?1XlFsb$|7*aryT~U?g}hmnYW(T2>Muf`eoXnctbv z`L=g)@7f{Vp@0$WNZr_@u{W@O4>34zaNOS3;i$Er%?FKTnsLDPi?DH6vhgC5e3uQb zyB+Ku{VYkADQ1S|m&~;-dW{d7q-*oEPhcNo`=M^K95C&POTZmbzo%Y^oxui)mWU|$ z-S}-a&;C_4%QZ)~W#Ee%G#gU>ysfrs+|y7^@F9FBnh@7CziXmj80gLS{yW=GQVM}ZT8;Y&VjNdYpGe-%C$1f zRaay=?QxDV!JCB5ubp3L;$q4%+&Pb+ZmMCg5UoIxa;465E9Y!RJw`9Hli8`Q3a!q6 zPE~*U%l+elPj5ZdIZnDv7;5)ye%n-DuT$^Q^1j)u;|(!^I!V!HzGD#jlKcJ)zZ=HO z^vd{ZBx!_bzScaVUabC5ygi`|FB;-llc$8~2H&6dkl$aG-)uwF`Al~$_kRk@^6qe9eB z)I6=(psAp_M{`dxlfm2|8WsS{ifeD>U#pJw-FgA_;MzU?7mA z$l1-e;%kj`4`&LF3&wg1d*Vp=j%Z>#;aJBQ@d&=A^>|%tt##AM#>L%5U0J*>yiFic zp765pn7~P3F+>wbN}MD+A!BM)q$)DC)U+dwD~%&91}q*~DOg+Be6$I+;o8(%t+w_u zDKR-@RAIElsMaXXm}z`&p5pv*-K%=WYAI^dGY+%z1AHEb`GP6r=E3~m^VAsSbY;?; zj&GgIJN4=HJqNlyx)YjPnmX$&>+%RT1V_dXI+J^ydrg!enjYbed>?x?wtsxK9%bI^ zwd`Ha9$*>ulll?{lm>S5t@);t=)?$CfVI?5(l68X(|fElp(B{@XH;mn)PAKi)+NK* z-Uer$WS(fLVfEP7(yqiJ%2GxDw0<)71$M{8sfqoA%Lf1O?(uqrRlJBWZ!X;J;O@l7;r{dS6EprZ&o0e z&@Vq+J>)FoC^M(wsJU+b^Z9FSe%N5Vv%E6{UIyNCy5Q`t^h+sjDt;jj1!uY zo4T`Vk=lUz19efvQo_d0UeccK@vaZgVwQz{;(jA!yb@U9Km4zNZuN85z?!sDG9 zoo4lkbq0jkb_^?nsoGn^rgt6gy4%@D!ca3P{jK|2M=Crk49e$~?I5NTOa^%a<%1sw zz4`@xZ-%xEErdGLG1+6Xzovgq-yh#MehF}{q~WHKZQ^e_Y?W%AXsK)!ZqjdJtb0{= zvqq-I8kI9DVd|OcO={li46Gfl7Bhl*FZ)6EncN?_TuHa&6eo;hLU_@h*QV0u-CfdU z+Vg>Sm+C<6qa3FE?h5SMNN8@q-)hi$yp`0_Li8j?bk>s!>H2hKe(hlH$iWf$iHUKC zY3=EVIrF&_)5+7u5(SCFtnsWF&I^atjMV(8t)hd~I;d5lp{$A5vey2lKBB$@_Xu|! za+@2n&t$t5a9F&85|%8>maUQPk?oe5khwSYbxKvx&G+Uq`W=N#L6i8VIABP5$Ur}rTU6Kifu~j$_>gm6+Mk)4I_i=28l+qMn3u#`k^{y zI$he!brcNK3~lCT&yShkJ-^m)!91MaE4?=LI1Rdj9TqF&B{MiXJ6kI0m$ZzQjE;*3 z#D7KUqFci4LOlN^pV42`Kf|tPi`duMBfN*aaefOwOynp^8`(BuJ%yV}f?9+Z`69!6gO6p4Mm1J;B za3L~RWh`a`X4g(7PbCZ134XIv*xt-`Ms06Q?+B-kQ$yQJqmx#V788EA6A8ftwbt`3 zjGB;|z{)2TIaS=s#QIlt-A(pQtj4&;SG7vD^BT6)i(2ek{uB z&$azx+Y7QhIjEzA_!4h`uOhk;O)1aGZnQttBC->CQCCLiz3$mA9ODjMwl9aXaZqRQ zjBttYm3W8PZ`5YAZ=5_nF-@A@J9lcX66(f#pdNJ%J{TEonJC;%T#1^N`dzSpm6C#T zCD@}}m8B|B^Hs;GEL6!*u2TN1F{a@)@65cj#tJ4s44vjVsXb9MzgOe$lt($Fv^@t;kAXdHMLFSMdU>Cce(*Rg(0M4d!EwhtgWm&JO`ev zuw3vReC56Ppjd6RVq{>fYAk>3!}S#5&~gb@gl%|I>#ug}_HjZof!n;kdAKgI?o2JU=0fw2rcLejZMKA~?cC4zV}3Pq8nVBcNupV43w*PG4ev3Ia<^$OVmeFV;N?tCtl`;dEl&}YzEK;vJAJnt%S zUIG6ef9rtjfb76O9((ZN;BWqR{sNf$MV>zo<>qtoeOvqZ>}+-jeXeJMdYxJiT(FEr zphi&$WTfkQr*Zd={~CNFOcbVwc8Si4*NAWN6ZzK%mk;{#g@e}x zdjy*NlEHUeOKu8RkDJ#Y+^;zBo0rJ7FS|}|Fa?2g; zK~_4)i{r{`=2rEm_21_BbLv^)tT)WX%=5jfy)B$1&cZ$}CynLF`o>tnXrf=G&oFN? zPq3$0hRi~SF8xDK1v!AM)_uF{bZ0l|UYA@~J9#^qL)k^~CbxBeC#n(o?aSI@+MeKV zwEe+X;Q6frydhqxEevnkzOBu$;{Z{!V;^x-M-4HNklkKJ^dcVYY$0`ZTXw&OPjRPN zXH}O`S8{h_7q@#;cP-JGsMa3WCLwGi$aSyodQP3D@W^Y)LF6yp2Ps6dkg=1ogUMmU z&>E<3x(&LYk!8tO7(;Yxb~WoJOOLgPT1@%T>DOsT{6Ls%_iy(jo+A_zm566ZZ5>ZL zM8tNIDXEyGNP6BWCSB<)Cxw!pb;xx_kS>!tI<9tpBQ5TDL0mz4-r>-7rE{9HgEHQe z+Ow?Z1+AH?MJ*v~l0B$dluC*RrHF>1)%Q5|Br zpB6&%qIOW)VEw*~T;82V!DLc)JbT_N7g; z?NK|q&6;?CKx<#vPHp#US8uz3$Fv@9k>K~^uMy-4($4Lj{arh|jJhv%Q96A)GhywJ zMfIcx_0-We(XsUTviE_{A#X{%f- z7w>`(z`t!>PI%Z}OVA+P?U*HYb~tq$Bndhi$kAj1Z8z-*Q--;nJusip6zV1@&YUpz8(kI>MNP@K;N;*kGyHdLT z058~+pLNfW&jC}Ibl)eBcE9Vk>qg0GSpF3qk$98QR(~1c>uKxKNg-z>OI$c zwNIlj3wUT*FP9y`mSZ2_Ea3?IuJu**-RetXYBAZgi!^@M=Pn=8_YMi+D8Yzuy;+AuB^XXx3)g5ZdcR0M%R|z&5`&= zt;YDXt-ko@t$*;g_!D6BXG96Xw&Nu6R5!EBk(x)z1}vFVvdOkx<(=%V<6Rx(Nb+gw z90fzZ&@CpNCk?d+w>z~xz}K``wxl-wX}nn9Sx2druT8H1Qn$bEL#rot#ncwmGwK#JB-T6EC)Ek+J?jnXgX_LDi0f_Yx76)uxLg0Eskw16{(b8NA)Iiw zgF$@UIY%<>e%N(}Dx#S5JfLYZKGDs49@BEc4|dQf)GIxzJqZkLh84Y`htRXVr?_W% zPaR_|V>4r%zQ1Rjwu#K^o+Bla)I0Bz1f)GAZ0B`SJh6}9M0`xRM;L8CN~|N`2pijv zxBb8?wfnYBwA-}HcC02EcO(&=I#WpZU>##Ye&0Pq4W{bS5B7X!STRx-XJG!oGsvU24BX+X@<;i1`OgPk1_!ykgm6#GQ>56orGKM4(*9;#CEmz>NfQ@BmB13SFQAx z?AFef7i~iPCwxV#Qv3Ec95I4G?6mH5gEh|AF4?ZTU1Oc+DGc%wMjqXlS;pAFs$(j% zE-)(qSEl`6`y6>xu0F4v`11AD9_{I)70~VJLyUL^ff>dW(xPcCWLt84XAWr- zDYs(@a8EmMpnS&$q7E^Y@Q|QRNNMZA_qJQLtB{g9nBW(ysRfj9S|>GyPV1=zF8|WM zsb6{EEAPEvui(7MOJp(JHWW8vH=;Y*Ig$hQxul7Zi9M3#67qEYG-;+_rc!c8;yJA{ zy>MD_dT{#3wC1!>@@MkVBx&;3bYe%x?!W@6%5GMB_Q%wxDW}odks`qex3}k1p(wTh5 zWzg?Gsy(%q=19w9$}%4?#~7Kg&J6DU+9f8fCe4!8lN?F6J2rM)A$H*{@LjFSt&En@ z=5MXNEj_K=7FoPWt6rNQo`auhjcV-R51r zWF_(--IIQY$!6%Y_p$<6Png#12P_9pUatrDWIujz-QW;Ep1)kwCCm^w3oZ!dg}tIZ zqIW~4LwknC#NMJZVV=-TxK!|gA0(_3JQPv|r^U)*rs$X`O4ujxh3_8oUHEE(a=s@2 z$zY%$pFbfyE_^p+J0ux-IpR0U8F3wJ9L0|Q8(BFn7~48-G|rj$JHe1-NSx8#P`IjKCa)l`s^F&ZT>hSXqwFVHL+S9`%jrYY zu@ZC1dC5M>PFM%;9JL=^IixvMAPN=n#Baqv1P2Axyq!FwzDy36vy;=*pW6Q$@*Jyy zFFa>%W&bmtH}C3z$G|oLR*)c+6N&|C0*3IWFc<2%$6);sF|0mpFw#4`eZ+O-(&(yD zgR!B}`J=p%ve7f68Ka4#h2wk2Eyj`Y-qG^WxuJ}qwPJ$kme@&rS7a#KFBXfq!cW2$ z;aTB4K|TM8z(Md?uvG9$uu6~$*V&-Ak)iaV^TU3_lCh6t?8z^auCt7pEZA$WfL*DZ zGVwCCvJ_cutQqz+HWgc;Sg2^Kyh?eO+CjC$S{JqQwP&qTf~lUFx5X(7fz<}<0_z}~LpIYkem1{s?CmP-3hcb>6CH*f{yA)MOmj4JGIcJ3 z>TRw|g~wjc@1F4sqzis86fZ3E`rs4o8}Dc6e|>Ryz^%X?OGX0AmvEQ;S{}AicNJwd zHTdtUw$-m!X9gFAmWK65POP(w+7;coekQ6VW_H8(Sn8(0trz2OZ<*Vwx8>+o`>j{v zC%46KpTFbSPS5Rkci8NBu`?#|>#lqI6c0r2ryY2^PvwBd-kbY=?K3*?@{sY7?T6cr zP>yU*@;_RdG;{3qao3a8r;1N+JnMFjo(kzrDjr%bv`T2D(0>0$ML5$r-1 z1G2PdlsWQw8k;)K*u}`|RqR#jf7n;ox2`XYOYFbNHRH+;-WfQ_9~qn$<%+t8uMIyK zcOS2uc{7tGb48{~@w?(>)z7New2HMlbhLF93|1So8Ymflo>xAPZE9|YwT-h~hHufL>jD0O7gk2EG79rsdwbpE_1D(_h^kmuvi{Ec2T_mK7ewVmWkd%=My-op zn;ISv8W`5SmKxq1!HDcxM_Mmf&yRA53W#1A6SIL4B_CrD9T+1YfGzSb$65 zE`O87R|0$kodXq@ykBx{Y2o6_0d9Ws{@Z=#`RaMKc)fSu>~X+x#IeKD*J`g(xly~m zv%v(^(dsqEHGU~qE0bZ*MJPv-Ym)gUV~FS=v2$v3w3+WSC#F_St(nxFd^&b~%xsK1 zDmPp+^h6Xcq72?2oa;N^x45^M{eWS_cuGfmZc}F|zT`XIiJh$w_YaXcoeMe@x>j^D zq5k)vtGZK}5=@?@gi-E8etHD*hRv|r_)hnrThaDX6Dc)h!|wB4Rj?)(by;rcmr*cMD3Z&S6U$=c7GQ!f+_|E;*qQY+ z&!#p{4Uf~uzYbj(+AGo$r3xYi1|pX5nkZ1zBDyL%G~_U}bJ%WJX^c9$VqAW_aO%?3 zs%gXNjM;?QeArj4#yrLBllPa`QLt1P$IasIDitV|LH#LK6K zzBM8tGI`yO^>R^W(GOyqHzjSo6(5oKV;6Cc=f2^CVTWnQ;*YnUNj_JW`u}}PI3`J6 zwfo1Ol-T}F?jft!l=@ZpTe^0-*4ZtzA2-=xdQ>M*~|EjBcDuYXxmvsCj7HRxo6zCN2zg@^%^3^vF`_&^)^=ndo z^tYYgR}{P~N-XUA^{?2!)THD_`QUl-=O ze_?&P{%J|}vyY2D_-2oNi2tOJ{VHd{m#(j$b8GU>eGM*<&(HXA;QQ8JzQy0lic1~; zdi~kmaJl|fdskZ^qwS&uPRwo^W=^GYYrM8Wi;y^2Gj z=h_8N0|S>F5B3UK0o-suE@R8C#233J4ul;NB&8)EJ!zV9A!Xqi^mNwQT`7~NM^8OH zt$U*3vF0p*WkZ_cs2Gt&ARv*&TUwolm$|gd_wB{j}=sYLn%&%QKc-Uz!}SG0@vj-~Wbh z!lDyi3%vJx>3JVsfGjljDE3(CN^#xj%y-sws&>NLgxJ=Yd6*CAn;O_?9oG7#Bvlg2 z$jSbc6iBWNyA40)ujS|Vx%P##{xaLhUgYi$i;i}@w3XOA-L$G8y)*I_OTAX9cYf5oT44cjtiZeoartpu9@yD zJQ^09Ubxp=%jemmU4ABhEI&Jcu|M7S{i1a5FrUvJOFb{RXt_%5uQ@bZt+BQ?zi;kr zbl%9qAkpBBW|8Kc$|BV;g$RW@ILC5!dd>9lvDITiLz80B;FUo$?#uoKtb5G#o?+UN z?(!}z(ncsU$h3WK>1pn1@@=wh3U9pBl+)1YP zST$v1We&u3OPydHis780p=qLNt(A%OPkSwgKW4y-rtkMUiw()!8YkC>%6CGW|bXa@uD7{f)_g#{o@@3iQ z>mQvz9DS?vhW>oPv$02E4@d8Z-RroI-VM3;^^W?3J@oRFcdd>ZR(q{8!Jwkl@dtxtPQTv{@S^QG`!nU3G{toR9J?b*b zxn4T^ChR$WnFyOW1LxWOW%XnS6z~e0U{798eTjOE_Ev3ggBXL`#(Bmh3k%Du_QUq~ zoRwW}F7R4-Z_yIJ4a;noCxosGtBC#*vv%8|gt@)$y8gOWs*6>7<*Vd#X3x$V zPt1&K4_6H34=x=%!5!!?<`{CG^vytB%c1uvYlN9j=l5)%$~kE*-aaJzn>L7~C3Ikst2^W!E`3(}I@a;rJ7MbJ!c zp5Hvy;M%acaeqTf!;$*E&DWYvx8}4QBsLJHx?{Qv=_+)SzIz<$;OW8bBU8h+lcb41 z)2Y*m6OI!{#M2Vfo?95a$gslSsV0pS+%`qUsBi3X^zeg0r?qo`=BYgzG5> zoS>J;mA zC{HO5!kM?76P^?9B9btZZ_i)KZy&S}7m1$2+P-k&)2$K+L5{el+UbBc7Ei82liXbiNErjPEOSum3;drr0h_XJm^+N9d3 zF{z=VeLJ!JluOFL(}gK3&orF-oBDrBOFZRu`s*&o zJ&EfdMI8w^5V*&dVLN58%J7F;kD5K!8S6dcHxoIiHTamKPFdTuqwz}ly)w1j?l1Gx z-@a13AADDGWB&Ets}@%h)Apt9O*@!&Anj<{;k4wmq_mT1C(=%*rKF{%olQ$iJD+wr zZTt0GS1cyQ~L%}c$^?-_?a&;O)U)b(Sal3$V4^rKOOxQg%_YV@ai`f0JfBz7Xy zvPz)NM&n=SUlcDDhfU9)wwKkHJ*Iq2`GH!#8dq~d^Mc+>y&glXwHr@v zy%V3kL$EV*@6f)P!>5jxA9p|b>TKx$)?pWSgv{q&P0`{cR9@$m9 z#X9~|G$lqDq8us=8S}zPO+v-}Z;> zckLgvg4!R~z8e?o{Ji-4?5~K5^W~w{m#YHmHr4ty?QcAdFKpF=x?WmWOXnEnGDW55 z4(%(`9dpM{K;KH|uwq$K z_IY*_JC^OqE@7o|JUJ`+NBaKolll9hezs@CY2@+b_Q^%F*0Z~0B4sAzN%ES?3zZAi zN7WbUU)6tZj5E1nIc;fT_u1~7<2t7w?x#JFg+CWs_>g@ne2)5NEjr=)^O*>}~V z@r5L>VV6ADh4y&+7V|?EfkyX@NV-0HcQnd14#3H*3Ty{ZUTM|XP=PPWH2@Xug=Imd3gs$gFDpM)s8zdHo%^%7`r*I#f8A=<|C?WLUpKGOuR+ja)3UGSZF6#S z7Gyc+8+Jln{aam6?exFtzq|ik{5$=3!Qbe*OSP98it6K<&NUuxKHntU?BDbQ&JT_> zIyUZW$gW>nd#pyS&bD^C&aJMrVW9p}b9Pe`eyX(&_G6Y%zLQ;8Uzi%)SN&_mc+t~| zhzaKG?^$iR8Cfr-2qitOAgwzFjRs#$ab_yk$=0b({Z7puhdo_a z_5|-L-hE})&~BB)y}N$ylq5Rt__DJy(PMYep5J>L_N_d?+MjUn?LpVW6GyHkeNT!` z-kfZIVr{a)iRzQbQ|wa&r&pZOI~RNIzY5O(d;169SEtsbmZ$znErA+OF|6MTQ-7uA zrxvAtP5qwwCG}fsc4|&)X6mQZf~51wb6YONm#mo%{pOwLlWJ{e6Q@hiWvDo)T4RH; zkun=(vf<1SP4GvcOUtG%sef2^x;X8p#)sid_a{w{RIZ=D>YMhzYm4BHfV9OIm!{pk zS$zHO6XB!FZ!FVWK3&XyR&X=Fv8<}J6XNwzVg{j;vV`)ZXF-oC>l^cWFRmBId(EvK zOdO<$&x;MF1||tIzh#c9r>QF$EHtn-d1EqR8UtCxsM$l~5#w}2reTYYqi&BnRzptJ zPqkLLSJ_Q%wb}>uLG?dy*n3ddUhl*_>b$F_sHuh3ODjjamv#mYR~>#iuXPc-UUySo z@NWUb>#_HxMS*_!#XkbPgRGX#3!V%%4bxkz6k!q>6n;1&XKi12c<70+n9wa@GNC46 zwpM|_$vn}*?*uyn1LY;%{Lll-+E?wp2;BDzdatbx0nZ;?hYp;}#lFyek zN{U7AMB@Gz{cQR{`T;Vz`v*~h*o-&9|7|vIj;lReQ(M_vG51sBC!yebJ};;0i}R|1CKziJ5ho4W09zE1S7HGc`RuT{M|Gc>wZ@hT$i}Hp5**YQrr< zmq(V3jEw&pe*)+4CTGWHJK)T@x!gjz4+=F3ZHhca1LbAP7uCM1xoT|FC<8l0>1OHD z^#b(w8x|P`8s0RlF*KW}ZV+MMtiMUWM0-j*SA(IEr^;1*sl-=u#0BFpa#*?Nm@63K zT=m?~>4IsU@sTl!h$@=w|InYr)McI^v$|c|FSlvJ8oaX-Q;92eExBHFtMK*rGX>xB zPJZplt;`9}4b5@SMZdJ>zR0Z~RUCKr5~-8efdp#m~bZZT-_K+xoJ(w+U_WZ}Dt7*PPl~(y|Z#ruA9d3_gKa zPLROfgc~^n&NYm8cT)YS>h!dpLo6Y4HSF_$;skMo+|yiRe(qrEklRq>SkBnz>G#ue z$RxT~VW&d13R@*zs{{B+&d|i@nvtIQGjo>ZW~)E8?snmh^^P6RYg~feaUMq&=q_~f zW%&;K<@j4I5iFS>gjp83+w_3uQmzR4$)h)DA+cnJ*0Zg&Cm;BZs9f&+;D89 zcBI2PoAt}1*y{|Vilcg?lhzkUg+!f+9*OddL1WA|Zi#)f3A;HUu4Id4{F?aR+q8FH zOFX@cwYzoi-u>GT@(w8`-94soqVS~TRMzRJ)W-j1tA{H!@TpF%ORa#2UkYo)U%*_2 zz+45XKT~%e_Br|@-Xr1u+M@8)i#GcX`DRr5{rPV-M&lopt_I4vk`Y1+!P*qfs3 zT@SY1?|QNS`M~>{jP?0$-%=}ED&EwO)rG_E(RVoeP~Gj*{eroWY06XORR}YME5^LX z&y7^EoSi=amicu$C5Yo#JFv=SQo!OcJ=*Wwtvs!P*+bky47jxEJNja57cn#E|5bM zPJEfj7JU@SL;mO6JIiKJQYbRSmjp?}@`jxd*OwQc{TZJV_C@{u(7O{aEnYahXnV$c z`sm4(r{+(Zo+dwe^4$2D$7?wL^yXW7WoFX*l8?tf=;hIKZHwFr3(Lk!TmHEHSqwgu z-74Q&-QnMXg_`d$>pW}YfWm+Q>;)Vh-Z{JsYW0DWT9c8IAjybiKr%TeBej)Lllg|F zVMi3bl#-MmEB{nxE88fqQTBqfbrLx>c}L6^%+RdP+}oLNGxsHLCA=vnL=5I6Iz^q@ zJiBD}t@N8TP-c_N6WMCnj|wb>IP7OE2WyD?j+MZvvJq^ZqOMY#GNN)qy-VFu^m7P?AaA$DpvhJ{_ zwpywq@t15HjvcNUEE&AdN@86j$C2Y<5BzttOY{1grhnd5NtLB#qouRI*Z=-pT=R3} zyV>_A`Ay$^zplz%p0ngj#3#$__aEZ3@-uVa7iO$`clW(-hH@q^WBW(T4`)Azd{WFO z=V5+M7w#@iEpe=huFU;It0vZ})M_@Yt2b>q((K)K6~6`QJ}0_y-OaQ#+AgLObCOle z;m|ljhBKp{ag$+AFX&m@L#I8X&7;-R zchGP3MD$Q;*Jx#wO%zMuDWm|?{maAc+r=|&jWrg`L@(}x#f!ZRrgm<2b--q8}@1K<1p^p^zhaQvxuNb z?s}i-u$XHbtYa^2+8^7xDP>dB=K47At+Xxf@x}4R2|@8Mx4lc4+b#=xXNwb?cG>T0 z*!^|)!o6?yF5EBKmwjO5(ETH!NmIv;p4^nul#+fX^IS{n|Fbp1H+88^ssB>zft~)# ztEz#WDj=^aP5l9E6@O;Uxkm@14tH-$+ddMtGiJ$((<|9NG+(~+Uze5E5^MeWx$||@ z9;;=+yP^Fe8Szv4Hu~Ep!zNLwcj=bA#IJSl-n`}Ae|FC!?SI!6q!bj0%JK} zIdjSE&YN3y&#yjxm^Cl+Y{82BhEie4{_3w)@(pqId)iyu%4vbL5zZ6NoUmTFcFJ|i zQO-*)SmUwACjA%shNcfp{cUP&-nlGy?Q!RND7p=~ZFE}T9AU5PU}R6RUutb_L$J7S zdBWVmf@yW#+R?VpHpteBuE+M4-6w~gjuiV}4$tg2In>%_+Qr+s+uPZ(>`H9T z+b**iv}v@tWBb-=r}IzO<4Xg$%(?1(o?`Mav7L zORoQxRyb8m*Cf`+L*2-Yl-1!$Z|My_Ktp&_PY%v) z-hk#gw`p!Yyh~XRZ&zM{cky?^r$xeIc|a{pw^ zWp~57ctx}2vnQsmO(l-r8f6PQ1s-q?@(44OIZeAk>+ihQc?nMJd~Q)~(P-FR-%z7d z(^DBxsZthOs#U!5XYG&H@BZKH^KRr^`_k~S{sT2L|NVwc*Z0rfM`q+^o_jCKD$Shw z)RA42^XZE|?8BVN-}^22hvj#>V%wk9zkU?ID{CoLsf?~z{CDr4$(o=VYpB!JHaj%4 z@E-WR9X#UAP6^48a+Taq38lnA?fV5?iQWbKyjz&2OflmHBaAhl)z5m&s_VVbd$>2S z_bA7KbD-C$w~&3By_LOyeVw(QwUo_f;o;2LU~gV;8RrTond`)j7*rjk2s#Ak#3|yZ zuqQV(N*kS;5KpX{*)-!gr#(l3cce?@X61G(UQ^rv?`v>0yfl3vMmNn{JKx&kr3KS^ zzfGV0I|rOgg^P(N)05!U?oIId;G60HV)4I#+CZb_Ml0^D%3l3*4Lkw^*<=zox+y$P8h31~U`uGce*A}RPVvvSVYlr_c$c8Lqj9_6&Z~)LyRCOU z+U>EYbnmG>6MJ9p*Epzg@XMj?u$^&+4Z>Iuv=tZhP^L3sKDI$yNHn%ltzYzxGUCu-Jv;GGTYd zUe_w!>Y(AEp|Y~4^0CR6lREvQeT|)0I(uuAYWMuiC@RRx$TWLi@-*$X?5)*TDlfUD z{m;!k%{^@aG;e5r&;p<>g|-~pYG`ZH9By8`KKv-*p+fqlSMr}aKWY?c<{$fg?bmFj zSEZzWQ~mFDT>Dm96V-ty;?hU+N7H38WWKAB)r#gJ^Iu!2TE;pkIX-q??efyu)}_@k z&FQQiYWKiy%I*i$=Zo!=9n2kC9560#q0;K+CgWb@KIB&8e%NJ`Ypj#9^XdPKu=fsX zy6fJ4rT1P!4=vQtdkMXRH0fOh!G>7S8$@mq8+Jvpfr?V3_uhN&9YPO;l8_J*0)fE0 zpLgb*IcLs$p5Lr-)IUTfTG#ct6!nyJ6vzttvh%WylEG3Ku>^4zv0GwXz(X`D z>LunOJ}oW{ZGg5*d&$Jhvch2UDEY_oC-M~vVTz*)FBEy;b8xikpc+L3soAZQtGi`T zWtd|89^qu9Z>(%6Z&a?Et(U3wUVBibN!3P?M=4O29i}GPC%GtgS-eTeQh0{nQNV+1 zi0dyK1A82O7rp0k>~Y&}?e6h<&ic_@=3M5m>~LKdcb9)-enWKikE$1?9VLW9+CrH; z)m#YZ1In`AWFr2^{dW2dOAq^H_LDt*`KNcf-Y>I1XTUR7_9rDfGE1z0A-}eyu=q20 zuHcR58d5rD+avlWdhJGD4Hr(zPx=F$W$-H9s`Msx<1hXN{>T2u{R!#{wF-D=NKC5C z`>f7v6713J(j1{2Ev!6j+N@VtCz#n;;+VfN_c5(7Su@>d(q;B!KErIp?9B3*C7Y>^ ziNc`77)_f>8w3~{BS(WrTx2D(5J`fhMj-8u?N;sH!QaM*?}qIz@22l&?&jr!fF6}JLj%YP{Wl(`eq$yxdc`W+T+Rtc_9uCD?} zf%o9K?uL3u%E^w(#wf`tOR2A_JLuBtnHY;8ILyS&+pRln4D4MU!p^>SR&w=pOZJ%e zyyH{h%j@Uu{~)j-$S1fxWc9rD#kKQB7uhcOUerEcb74Nj_JU1F*?H68vXGSElMuI{ zuHZ`{$n!MkA6!_!cqQ!Q())IVJI!W(usxo+0qYQOdIj_yAj zF|qLz|HrC~&b!wW790NB_sH+9%Z96lt+}0^xtRr{F%KfbFv#eAhCoYal?aqmo?6+LlEa7l1Ya8Edw;04MTR6s&t zLU2Mz!bMP*KwSot^J`#o9tG+)sJozI5`MniNn-u{_><9ZZ|C`>iTM%>C9-= zXp?nY_1eKcxYzg-qRXt`Jlmqi(#86!&5^l?C5tK6^rzulBW-;JgU{OcbmmkVR1-jt zg_C?GH7V{PF)LysiWIpZ`bdl@<|Mf&StVm3TO$XTzoDR@xG8sEJ_@EPrz0~a69}V| z6Oo^k*HGS49#(s#o~Y%a{Y*DVPYLwXVS4xU?`oUsU^H4aAHv;Kek!>u-;h(3ACkHw ztu9e1AuaMvuey!ndu@-%^ZIYCq;B8-EZ;E_?s>J?p#f4~FmA zli$7nlcM-3F7@bJ;h*!r)eDGuG?i23ht2%Wti6z)RNyPP2Q}`!XZ4Ua)!DIM^Fm2<)!Rj?9p3G%Q4hDua+6 zmUWP+kja&im6ekIF6|>ZEZHe$AwCPS6I$g(@VT--XK!W_VwMLQoyXK7lPT)r?03Ijp?TM{D>&YMf%_Nt@6w8 zCHtxQ{P3Ojk@m~;q4m@BedAZ)i}c&|mk$gI>I?o7%zqwxKJ?;apqbIUI25WCS{lZG z>3+CVctZp=BJhg)_1Z|LsJ@#!cQWq2j?uh76aW9N+Ye^qYwokFGI+p`uIEpyHDEtbuu&7}-!jP7dKX{Jh9NoR0FdH$iMQ0umnw@r`>bKPJr@UXF? zA-pKI&?nO|Bl)}LH@@Vm_W>`ao^vLKfio%soKpWeqi%pR>J~VoqW||96$3VV55Q*c zVZx(?-N%qbiMMZGXMD5%n)dr{dRtymE>qcFNkiRmt!|rj>p^c=Z}imiWHZVO0pwqyVP0uI2wNiCOPWQRFw%XJ{I<|G!(!=z&cy2YO5e@C+>V`g_13r+sm9<2 z+1i+z_A*RqX>oQDF@GvgFchlk^fD5seZ&60G9l=6=h1$ZE?_%J7pGMJow(Ajv1~Cotgg6FZhYhJyRx z1Z9r0dANG`jGRVxA*+(V5Tl46_HOO{*v;I12z2}vyKTFV2p0+NBt6m?X_5p7Oc8%_ z0NDq423``s5l!~^_b%=l?`8mPH47TK1=Cc`F|hRgMGb!Th0YnW;>tI{ezR~A;Vm!B!SUKUpx z3iNmaMau=A1G(r2>B8D2B4JF3*4Ss+zd*i6)<4iYs3sSY&yws&U-!!QIR2x}$MRzDY{qYa|H$@qe?k7J+1p6?moI}xPFOG#m=1z4!uw$fW=FEx5~idLz1r|w%lFC!D< zk0w7%Z(H56eqi^-UgE5a^RAoFIXZ7M9}~Za{t$n!fa?KufocJXf#-r6zzkO^I6SyN zL?Ptm`O))`i`5sHLrLcYE?&D(dhz-BGZ$?yj)#g}d>g76R&{AS%rN|WIP^01^3$th zSEH`SL_(u9qIPdi-1&O9ASU5{Og!@cSpBihaRIk{?+9F(z4|Fs?@~3;4(@rHdK2BW z&p}BQ;2(@WOBWpLZ%f@!`*ndJrR%QhnR?hYg;xU;qEZ#P$$ zFRr1^nJ#or0cQgo3!IqkOzbaP)>vIN{c46VG&lO9Q>=SaHBik;iCOs@&<;G7vyj)3 zqm%z3XD+WHKP7K2N0ti#-Ij>#s;q%bhfJunlng|6S{5dED7T}~tk|af27Vc?qjE#l zUu{#RPE}29S`DdEs>-T@QE5<4g;ywDRAy41R=%pBq!=ZyqQC^}hNa6`%5FjvCEZ2( zMPPh!e3w~~tm3qnX)lqlkb`k&af563*YX!s7M#Gn=jrg_(D~j!J%wQQyVNMw$WWtH z-B#XF771ouHF=+Mg>qQ_)@Ho>qX%a6CO`MnZvBG&983TB>ol_^<4T@Gu2+e8F<*^j z^;i?Su?6UHMuD&F&cyUMKQeJn57oJP84JZq14e@_pdh_r{>d!M^AArP1PgJNXpvZz z4w2zg_^40;7&?Y(M7391VcM%Yb-JepwT39eYNI2g7GrfIdgIFmu7(|YFZHdpM6~_X z*wp_h1}m}4<;kT=>Pz*A-V{@TWI=-X8u`fFPCRMshwQn)U;mh~m2rVyfZ-Bt9Bl?* zGQK{3b1Zm(C#&r9>`xP#2`9UZgxB~Bc(dJ8eEV+RZWkeo@E+VJ{q~Ia*od4&7(p92 zr+Emk@QI+j@ddbYu-P93YHPinn(gn~zFQ(&;p=Mao+w(B)zb8$0r0L-XEJ7V0G|R5 zbYIVhn1^5k#r-iC2nCOJz!N$-R|JP|{QWSItG;MVnhk zOrNNqjksqrVwP=(U&WkG8sWd!iB$UK3AW#vbVk(LJjiFCC=8P9ns!IDPjvvFO4{t z2A7yfqDYaf4veV$5nirwQ`1W4NXOR@JY9%xgubbY*&XxC7MHA?tY6rCw=J-lw)MA* zvp;Wt*CE$&(W%hB$wA51&Mwii)Cys0Vn+q z8-Q6qy<)y1Md_pRBPD6&UrHv*mlUyzx{58}OgojYgBi;Kl?ALvN)wPXE0|w;|^mVU~5N`d`T~{=dlyu{V4|aPFFym#xwM1kfBGbJE=XZZLk^G7wcH- z9#y@pbS~8?X)nqww8`VmJw@(uJHqMITGa5GXBOxG0(v>?}*-6|AR)L|3%55PJjpd-W+DOf7*6h zcd~rq`N;C{<$;oZ3E&A90J^BP7VVbqMvlgT+S8ivRe6;*70TtflD*C0KTbTle}q3+IN&`Xk<<5Y?Pu&K?`sob#D?9EyY4`DtBECGpxel;i)b-4 zajj_0Z>4|v`tsUR7vLE{7M{+tBMaw@=DKDR=lFqJ;O^`laL3XCe*MFxr%R_RKUWym zU#@Wg)&mWuV!Hu&(NX|Q&+9<=;MY;|Q4h@@8Y4zuMoU%?)bi}eEwk;GZHQfjJ<@K#UdU0>smGzuQQA?}Y1i?@$;KJsQs=hj{>N>^-TK^B zk8Y2BPeb2}e%t;AftVoOkcIPx7q5l&Um{-(x~>qFc~k99Tl7$L^gUwi&p1^4e=n~G zsCb6x?z>knvt4!a?+ZwEnQ(7ie_C{N$dBVnnsLK5^w!7peW5 z`J2?4A2W}7u{}bymud#Hnlb~vLcc`5+kf*ciRbzG1n~q(P_hX!3GxZxBLI{ND2)Vl zum||hhEfk~C=LIk_yT(Xvjp$w-=1cCWJ~r)JCk}YTRbbS)Uu?vF|XlEZ+DN{c+!~V zQv2c><^|>*)q^^~I>%}QNrBWzNlG`ty;PF4J$3%-Me0`?ela=)dWCiK4U2y*k}NH( z?5zc?6>aux=4_?yr5xb)(hg1z-j4S6PaK-SYrJEHwPrI%nd1#djhwU;wYQa%;TL5m zWZyzLC2K|0L`Q{TBJYK}g`bKZh^C9NiU$Iw+W-_HDF>~Dwt#!uiCDDwQz)HeuEdc< zj>MrvE7VN#L^@t3T~1aWrm(1xujr;Ep){o=q_V6s1%Ir)pI>Y;Xj99kWOOy8Kgv9LLRZ%u4X4!eO#BfTey zo(P}hF#cq`!u5=+Sa4bJ2LQNEzPJR?*Q{J5(D`4uhx2Nyz^I_xyPABk@zw z8)A{dv%&%pJ;;5&e7;pKdG0cxTjFOnW}c<*r_Te-$oZ4zlhNbBV|oDU{Q`8ky+>(B z>{KS|GBus5d?Il2;poGW+!5!|nWK41{_%I;zTt&9n=i84aI1-F; z{5VO#Rk^<T8WvLp8o{+5LTnLBa#C*`cJyUyI*yzw9mFOw)!3bW!%uNLLj|E_(& z>SBrDtcLBMkg9<1{_#=7Q4{b7W}TFsNS(5to;$HU(E{A*b-;1&1SH}5ll~K3kfE@8 zLOf9hy51woK1H6aO|~Ghl1}ma_)*}9KH2=ase@)fpI_Hpw_Y7t$z6sodo3)_@6KNX zED{!EI}(QUMsCcd%`?s;mcA`AFTYvx0h;bxs|PFY>jP`TfT0kM5x@xJyK%5R);-)o z?!lSkwWD#s<@TlbpufW6&vKUIJO`P_lJ`7h5JD9-6KjytkPZYhmuvEK@^#?-Kd2l6 zcT(9>`KYR^=Al8-hzFjT4edgmcpVMhS-n*KGls*!4W(*qju1ByHNlzens-|WS`J#i zv5K~?vCOyn4mA9)tzKJS27iyWd2dU$J+%{cGC0fNEbem2Ey;ZloP)x?=l!&U9tJC4 zd>%?45q%kPrR3_;jg*@=?ogxEV$t`X#p%ay{6A_9oQEs%%khix3-NQHrsJo;o?$$G z6zoL9W7XnLuKd2H5&R=$(_7d#)1%E3=4{{+ZJlVNV8WCkQ2oC5R?K6C}V% zDV;F!oaR~9d%<^6-?3kGytd{K-#W2m8=FKp(|fd{C%Y zX;9%-f4)w$S)!?_tEZEHz@gu3ymV}R#&(8j>E+^IvV_@fsKJ* zlpif|7AgSrg<-0vs=u^@bSU~o23d$3CeEgUX16TtEUPR&SyouTv-xV1U`w(=+tLGX z_G2r5Yhz1ED{)hjDZ4SsxJ>t<9!2Y?c7PhEdXI7>d`Uh=ffdXb;oC{Y$=lOMr*kI@Cz+>dfWv+Dv=8u!`)MX=q5-@35hae2 z2lO2$fZp*C=ZVw6;I?&9m8-Xx_7>e2yB3^~sJSS>M~&+p?8)!k1lxj~wxeeG=9|^e ztFDxJmR<$A4Q9{-4*pHbX3LoU&6t_~$03L3FH=E(9wJBNuh(BOurH0yd0k{!$X<*t zuCuTov zA48ds`JcBAj8_O_@0PutHa;kCiaVc{R@*MEU@LcEl!QIY%o8u7&o?V0^ zn(Z}PCCda$3&R@2Gnz*-5eUa2awKHW>~X zXCT^4OU*8u`J1PhM_BZlr&!2X9avdg!)=yr`|LRExg8`Osg73Qe6#_#=_-#-&u;G# zpLKtRKuEB5h67}C+d)@by`-|3}oOFSL!n}`_K^529&b3|wpWi`%6qUTT zjSAQ!5h`htX#6>JIh=*B3Cqbu%4{op!(-Hk)GuhyYp3eT>GL8w5c!r-R(?R~OmG}^ zl66*ak#W;=w{rJ6x8pA1(d|BY&cIE_{fw)a+aHI1#{#Pg>-R?g7_VrgX{G`VoP}(v z?1q$@^k-=r85y}^x!-bp^3n1{`PV?xr!DlR=$0{ZGM=*GXE~W z3;0kVLDYoQg`$LGm$$)8Z_?m?!-3JHLKLvSNoMImkJg1@?aPNJVnuVjGyj=Ze$yt1kKth%IDzgDQOzTR0wMx)0F zdy_hF|AAPtTS0A9ZR4z8*`!)0*}S$4up*fam=jDoOy`VV8{abUGd!bRrhQK1jAph9 zLe)ZPQpp(HvGOIi!A_?_%mMNiQp?N5_n0k<&63%d`2qb$dUZNBx)YFv;Ct!-l7V2S z!5}Y%_k>7wI@UW@JGMOjNJ*vW9H||-9x5Cjl6T2($+6_yWNWheq1NHG!=S_cqxK_- z6V8*@ATy$m_BX9FtqbiqO$*IykbF~kl5xUD**|;*{>4*b5K(9Elpu~f*x}h2MVBvy zEUnFW%y><9OmL6h8QB~(9_;BA?)}x<+SAZQ?KEk9*mA4xTJ3s8T{(9tviMBFBybuq z<@jc?W#(piWIoQ+&w&2vN|#Ts`nj2Y=hss9m8_P6d->UAS4$C+PUA!4 z%XoqDBhzoDSmxKv-r!du%P7kyz(p2d(`8#?GWAj=>~!%TAz zd|tezKBi^^T@vPS>TsBpM-tqdA^Zz?iBxO@mb_W9sfZp}FIm-Hl~{SbJh)`H#JG?% zFANxN7PC1sEAs*KbBiI1{;ORp4^Z@|G_(r()~3W}?pE}cW`tL&>ap_PF9c=^%Cf! zo}*wWwh8}n**1q(_OgQ#|15^g#kAPez7+pY4?b>usC+N=F8Lkt4dP{DQeI+X0{4F@ z{ijin@4RYx!TTvDnJ+E%`_pXQEJQJF5nUa+X0?;9Q*1nNOlA4!5*Oyf_R9m)11G=~ z|IGE9iv^+sVG_3&Kb7{AsZ}siysNgW=BhKNvuRw65VyoxZriswymg8`i#v;T=5~d- zJ#c&HzUj{I@yRvS?W)TKSF{7xvB|2zy2|LD@v6oRO@8nWw*dwvArs% zQofqLs;#!U#7ONAhfl5ls!HQu%iff8oY9G|XwCJ@H^|cLHjr)w{OioOE z%nZ#X%+$?`%~H)jnqD)r1ApE|%$TeiHW}U5H#Wd(=jf1uCxjo~2q!9JC<@Ew$d*fr zO6iJbiQW|G6lmjN<%My0a-6b=u_iOrF;voZ(^Y~D3p~ggi3NRTI`FdJIS4%HBi$ug zk!VQ#gwb8MT?!zT{l>B2n{b@?C0q+W3Lj4}C!8UU>~)jhk-GuQxsdXLqDo<NqDlY^yf{M`sc5m!+wH&-N=5*P0y zNpm?991}w$yd#lA>4PPGv%P8^5$!CEf(^6v{dMQ-ylX$!e5m#)GboKN7AzVrd=IoR zwR!Zp$$t^qEtxyHw{y-E{aa96`nV*v#J+gHq^;PwQlw(KE))DboEst5|; zdfwSFd@%HKta!A2;%K~ghJA*#;;~|fS>6`KweNf(co6bQ&q&9B8*+S#J*9$_5?2~E z8f|)BdML9Na}i4&OEhaF>j#!tmIAgMwj#D#Hg;Aj3mYRdBMm)?Zk(=$?mkT@jXKZ` z`;g_xs-$J&T>>v*3;z?}xgD_Gk9I=at#zQ3Rx?-JmPeMHk==9Ornje50mG(g0y42U zVKBi8^f(RRv-{1&+_=Pa+>|Pq*Lcq4Pyd-YnNCM8&y6fvEZVI0tO%~+;*B zTXj4BJ1Kw>QcM0r4xu_wFVQ*CA(;o62ifM?9&tS2IN@gIxy|>J@3K&Z5LLuXbV5Q4 z8YYdBHdDw@s8RL;iu`qz8(<&TseVc0iH?$Pk{(Ro*9d03Zd7XAi}+|VY?5ngV=8av zWTt3-&RpAK(X!I&s+Fa+tR0g*+@8aM-;vV^<=Ew9;vDEgb)^9mtWl3bpLX9;|J4A+ z5a08?7nnk~FKI-0U#`CLSb(bTtMW^ zjnRt=VJQIxfnU9HymQ^E-FcnZ&aQ$vDT^K1uE`?ZlA>LwV+k9E$wQt&npwA4g{Wt! z>)ZL;*XMR;!}=t8k*2!9g?T?RdWa8qzlL8>ULbgSG39wUdIdLnXvU3U%-I2T$x?Y zo%Nor9@iL89@yTByR)V`@9R?=5SRVGwit9V%c1pKa0saJukT&(z7 z{krO~nphQ4`=Exd4qCg|FaoxiacsUdBl69B0l$w{?lth%emF$)56bF`>l?DTD7Onzau3jr%li4)f z=+%C^&A9t^mrwuGKF5)d!xtudCd#Hwr;CyHNYSNdAou3sTEv$078|w`BZ(iy4T1T# z} z!(LMpso9i|6hF|fgb-e>(jT3Yt z`1v3)e=yRx>7BH_$9oa`X8V1VT8hB2&@mlIZ1tnNL^sbs%gDqk!iwh54p1#8CCH!b>=yMeM(7cD*3V@s@4$#$D$a;rr#_=3We{090-~kh#Za0bl>0_?_t5 zd(g|hSE7REgMEE6d$iD??Co}cGFJ$}FU*7aq1 zQtZ>|$6&4T|665`SQ2Pn>%BDj{O!|jnoFuR_*w_~-}1t1&(=hC-sz|tSs(s6b2MGP z#<|9~TaAwc^w@NUFAN4?{_HN`E08ZXE@ldAf`u!uDqCx#w402=jXzmfSeDsl**Q8| zJ3VnpaV~Y;Y$;Zh%&S%Dp&gX}F)j9pMzRvbGcDA;r2(tzK3WFEgCOU=M20G!o z6y39iTt?9b3Wf{7?_Z&QRbyWH7JL@=K&}8PAgL^pAW|oAEbv8OQUJ}X!pF{Yizkg^ zj>Cl`jDwDgi%XU_gtq~BLO(->AkHG0A{-)DMQDV5gh%+91b*?6_zZayd4qXA@caZm z_7B`W+;+V0c;|pFafVl)kBQ5XtA~A$eGTw3IhYxknd!Ocbx)zEl}DLJ9pqwiD$pJ; z!s=t4Mz0<&sF!d;othC#?%HGpasrs8)^N}ozulMY&z6337u43 zDqhq8**Xckw7Z*!e-DE|9$+v-41*Q36LTbI0w*`0KA(y3J>e+vS#dqUOVCm7R1N_5 zbS}M3z59laM&YJvW@;8h3pQ(#b)>Z;xZg+EY+DUmJA&;T0qp6f5Je_o#@2{>gET`s zeTV_K&ZbU<`eO}MFeCn;UV~YB8N81ZI)2_ z2lN88qO>L;V>SIK?C1^o0r_Na0-Tg%gcZ5*iGb~S8E|G#fv)i$$dI51d|dB6 z_}<~($7yWrt(O)pq@s3(ZBq%QM~B+_2do&_HNlYDlb2s9CAosa0+es?Tc5YFui@ zHYKzsw><4C?o{YE0%z%~;R|EAqn=Z*CZThWX6F~f7Ry#_S8UdDP#Rmsn`+p0j5=X$ z_dJoZMKepNtud zBMb)&_rNYg2zaM|fxL3$anteB!|=l?@Lm%mVTnt-S-VX5UpOS@B_;qce9vv-H)7FT z=`w)eK$&M>wS7lB(R#1RhmhxfY=zaPG!I#PR2znro%?9$_ydzt4r z8aSGG+IX7y+xQ#7uJBmYL2L`E3Y=OmrOwHImSvZ_F4wOBSHvsF!^>3|RDY{mXryb3 zX^rdL(0!`Mp`T;mZm3{1Xq1AmHMt5{TBhbS7Ri?XSUm#$YKm>L9lia8eW0VdQ?FCj zS$P+TYoL3`IWtdfFIHbYKV!cLf4(5a;J)(*7k-7$Mx4F+`P#k6+8ctmGw!IyFHQ$?2Mc;%@L2-)<4Ct~_gL2q zH!p{O9Se;zj02Tf;BsP_Vs6|&xRXGp&L5l)u4Bo62|aE!ZVm1znUxh4p1Hy~_P=+2 zC8Z1e5=e{re(&?hr;)dMZ=7EKO!9p8@(E+Yf3JxohUfVoPTq;8e*ET_zVnkN?{$u5 zIjZzovsY7g-k2Z?qIY+WUyTGD^6ch@30&`XwQ^DzG z={1_(GqbW4wVQP4cHDEJxMsPxpVRX;@_FuC;iv5%6OiCv7*Ov+^d0b6^Stkz<01?w z63T!}TVnhKLD8=>Ffc?L<{RHaq!`B{VhnAKo@ra@G^)9&>njW@#7S>RJBhv#4T0oB z!ug-_&-3DVqrr@`l&glzmD`J(gUg&tkH;0{X8prkBv36-3ps^k2~mV-AYKp)epmi6 zJ`CR{-Xh+&yxF{M+)dmeAR}s@kB5IzfI)CfPzd4x`r1`KR{mu0TI+FM;|ylG!g7c4 zHe)l*3C*RGppzKN1&Z+gF)5T_N=U?5VCvC+Xzr!Qizk3_pgNH{P8t>%#`m%Jg>=8{ zGU>S39@p}{xvqJ(Nw|@vp{HTFKE1BGmZt8sMz@Bq+Ox)|`h87m^+C;6wPu}Btu4sj z`qlcj#i`4_Q*g+7@CcxQg65IPUu&vs(49Uk18IwBL7|~gPqt1H>8{f`u-dR{aQSn| z3wjEwh+h|f16zj~!dKxF8gZHs{UQCkpfBaK-~`=*vULv7cPZKO+WljfZvWjj*KWaj z(8k%y)wl>}8SOe%XEk-@B4r9JM6OM3ZFce8<#b!Dr-JNFUS^rM$2(>LM2dIC@=Tj_W6mEdzC;dPTlF-fdW>o zE|!8R*}1W!y@SVo#OdJNb_9U4C3^tu_dM@ zrP&F*&$XJ*HF-4oH|8`~Ha%~tZtiYBZG(0pIsw9|2iGszA2#%15H}Vu_I#2up*0&n zBZRy?$FuZikqPyBwH0V3leVGTh1g5jT9ALmPFg4K9?l;+gG@3BkPK=>XG~|paF!v8 z`Cn!#voK2&>j*0+ix!I+3xXw>={ZviLpOsVy*|A&?QPm&ntZU!dwMcO-K5?-_B6-TZyzLnI%_6wqJz09f=I%biPct75An>kVrXnB+@co}q?okdwKD0HR8C^D0HxrEQCL0}x6UQI!4Q^&WO};V+5fTe@ zFfC$+;+LR>&{~;OnYX|X@{b~e(k47o#YAmMtxhXeyG-YrZaLr;8XJ*~xDXo%RWl~@ zkLH&w@D_uX7|Q~yU7KFpO}k}#AIBgk9%mXCq-%v+sOL4WBjCj$`ttd^`o{;@1wRR? zzc?6*33s{t=_=dx_UninMz@A;--vX#a=0{Vd^0=?BL5zrP55X8!Rrl_je*L#t$>=tW~>1ENc}b8pyS7%>+=`(+cp z8Auc-a-9rNUo*xrc5+Q{^@xawegV0XH1HenC9Uh)qKI+CZ;O8|aW)gSbdFPwv(D)* zJ^&@|>h15t?&sq#?5E?8_vQ9~2b@i1E`6@;_MHxTmPeKYrWt0fh*u_6rjN}0%@oXM zOiRr2j6#i-blY`t>M0tE%Ab{0U_W6763G&3qEAIX2$cxIAy*(`fECKgt;bypI49}c z72K-4VZ2a26TZiSj|9sg4c{sCa#VD)mv3vm8n&aD%GlFDto}^WocDmWq)-=l}3Y3ys}j0TYkGwwh!$NfVS4p#>%$R>Y;V9^-G%zRzB8t zR_Cl2%o@!n5!EILBS~Wxz=a#p`mH^u@<^3I>6=otEJ60A6iG@}^qFWK&~n7_6!Elj zta997$zTy>;AOyrtnh!21CQ^5`S29!H3_})i|o3hn{xq`We6UWivi*O>lO?Z7Aa>oMX z9*XUhV&}2{Vn??UwD4 z!^FwB+4#3H)3JX?p#v}ao4fhC@f`{sqn#@qH#?I$uD2((oon-GwP^EiCAIIj8H3%d zVXH+8b1P#DXQymOPR~(yYTrO_)Ie$f;^3J5d#h^Sv7Ffs@X_E!_ zea8??Yd#Y#h=vC&2Xn`T$5&}$Y2MK#(d{vgF|xAAvvhLoaAo6>oT_5Eq8$7${4>}cd20=6ozZF1VKgW;{g|H9qd5ae>A85N;&=b?alHlDCl&5K3PdT zefZxC_89)?&dcPa;P?4&FMcXX9#4a%#{H$q-Y9!l+Su@*o~QG$9WtUZECF%`j?wqf zkMMMOBTC<)7{~#@b8d1nK{O##P*%w+N*zkVni-mh1|JOX17&x&MYiQ*yD{t9WaqO^xk=71H{f zd7wqIQG{{2uAv@}W~!z;=;f{hy?~j_ZcKGKoG>xLAl#j1Vux5OlO@5Het? z&kG3)-xewt5*E=FaS=`x77@BEB*|~bpUKn0^Ny#R$D8LBk1x+vp8xFKV|Y_}djTWQ zULZl>fR`TRqLy=cbKc^7#9qiA#`>JKjiG_z3+*r3;?tj}A5UUWo>KfM`XE;T1AH)b zTY+0CXd|@F@^9cl9-HHwx;L3J);=mcdzL9w9+$U)*)CJ#as6oEyS)uc_HZHw)FJ4^c``v$uL`vKcb zyIVH4wry4~taHFrEfFDaB4$`(D5&#OCme8-`{5xf|0N|>7Mcno!$5Bn5D z0PI}&Mmn0herMfpy=84^-FiI^#fXYVt*;ucwW5;NhZiORtBG#Tdv+3J z9yU*&PSB2>8+{FOs~f>;XSkoSzr35$MeI=N$Oe3m*zQMNMqSpO5}hU;^=;#=e>(oQ z7j)FL2ex~+9k!BMPTH8;s(U5@Q)j0SF%&sS8B`m@f~+r>slSsNb5XNrk>zuj7AohN zQTJBeHXAo4c9M1qahGt0d)tIk@PJ^gMRMNrf z!2&Ukm<{%cZ^8X{5R;4{Z>4WVgN*PBlqG6;ITGa6RW69l$00Rl%BMG{+^3u-6DA5K zHz!0VJjT<#zwm)m?mr{Ursnp|DCc0yR!51L&)6)y9M%6#8UT4z{(iv z9*Q0G!rw43OdtL(-f#~|Xb1XMqhsddJEz{Kg|r`N$@I1KcUj_D=r}-fGdG0iJnutZ zIzf^ER#-{IRy;}kwIo6EU+EU&)go08Q0v4bk*oob(7^v9Y-QA*~ zVqpSGNOw037#r)}ciVeE??3RncIkuL9tPj|eD3?4bDuLYZPM~76QagXb(v-x>k-$R z&@f};%nh?D=QPdjnlD^{Neo%?b~&;l3A|uS*ZXZ8x_SSW1v`q8+IDW=-IBa*U&;Q> zDeF@t})FHGs$c5uhDwGQh|%crjlU-D(?f(4fsPMNWG z=Jn~R@kOy8;u4~~qaTktH+tnz)zBSIRZdx4CHFHp+x^p|YK(oi`%;?%n*P(6^K~P7G=T+c4-j55u#Py-4N3q8jj(;;TWfCt&8)J;4#b?HUOPDnyX6D)W zl!QssW8;ISg~levESx%L%A%;$$@`}Kmpd>JRT;f4dP_{f#J`hd;aL&PF`_YT!B<0q zVecI8o8=om zydZw0&1M@7VB#w|k2$R@E_)B;t&}ht84XMwvz^(*^x`^mv$=n`QM{eJRL*aX2Rnex zV@I=V=xy}-)Q{9wf{0Lp%fpSs48*`2k~-^85U+dEX{xvcJebJ;}K6f%Kv2-M94fX=~p2 zrpjKYzWV)Y&&zplxT*GOsyBn*_owgp7?yDie5-{&^M54dBxVoIkIuVX`1;@dvLB^w zRY;{(n`b<2)PIx*kjr{XLTOv(sb_YR(81x9(E7zc?=NyB|%;9+}?_wDLt=ydA)0Uc zUSRH>+?Kz|xtIRl&;9UsW!|dX#(&oT+)EOQ11b{BbE{mc=GQ%~O=>8u_icI6{HUGQ zeyua0GqI;rP%7LfOqcGG&R3F@RoX)Bb;EkYWy?m(AZ!3ufLGviK*vR(GN`ePos5;t z=S&&+wn*56I703y-elfx-e+C|uZHW(3kSjFGj1dIF6S#}9s36R9j%47g7T2kMPL(! zSR#%KnZc`&SVXI58|G=fwY@N(l_0m1dku7nFZ9OsZtt?|a&EoYlGiY#!4uq;T&t2R z3o7}Q4&}*Zlgd&{SC#xKK3~#Me5g3T=vHxW(bA$Dg-wNn3O$Nv6kRVe7Fri4Lqeu= zA**CvaZuTr(s=NACe-b$l{T(!q(F*Pf9L*AWAEqQc<7TSi?QNxz?SBN?}$K^p~}@| z=!m8^xG2&4r}q|TybIN8oz-`eh@{XP^|K(|lGKvi{I<-oEalJJ z9G{OA*~?d0mnYxPy(@X~ z-5E<;PmAY$;`urrc3kIK?>XN`>!S&{KVoq(KVd3ygqV0=;jII zBgSyYDuPo(G%$17<9BG-z9A!r>OCKLb-Jx`&vF^(dJyJvi=1qn-#gp690zTv3Ve$) zT)bQhor7GW_#gQ++h|)y>uuI@9>;1w_bK--U=%3sbnY>36SvXY+h&S&y0wG#TI*%J zx4gBSEY54TnC-%t%7~yWq6{a56V_u-Vr6EdS#8uADiw7?@KX-av{xTp{_v%&JlDFsI?|+x}>BPq<&^n#{{p8ok+&O;- z6)ek_l#DESQc11!Zb+{0Z(Z7&EIe6X=|+2^v% zv&S<4vYE>LPX~ktQi9HmiXVMp%#N`(;hYFQWX9c^ax7}rRL^OoSbE&i>Br+gPX8Bw zE#cz~^K{>Y|2$OPrbbRPOd&O+Ej5%{kMF@pf@d(pY;V48xNNuuS)zH6L3%;4T9GC> zDH%23Fd!8^6OI;Y`mB0C^ppxV3Fbl;@B#2J(+Um?wh8cpG^mNs3tR+WdS>@T_p}RM z!F>87!DvA!=$rN354y{{c63#BF9tT$2Qqv!ySu)|2e)tZA@5U(f%PmzO8XeV;4-yRq=azx$98+5uQzQ2EiaKNVILK@BPO zJ*~;D37vwDQkW;+DX{8s(tkZ1J1%lS04%vvEh)t$kpgg0NP!s8==xbTe zS!+R)H`n&D?Fn0Q+U|8yI;&tWwCQ;w@P*!Itf;+T3AKBq#5mwCM`R0? zc1lmJSwq*A1J=CMaL2gSxY9hsECydwCo~IPhW>|EVoEU~_$m0K_-FWH$fxkd$Kxk~ z_TvQUJn0+Ezr=&zU_E4sEu^K;qG*e0QS|NfTeKqDFX+|pvj$jE++Ey{+#+tZmD%bI zRAivF^p5W4*>h8t3t04#O1zzeaowS`(Z<`pg&>;M387*F{E0#yC&+ zi2pwG;jFa_jx8)mL@nL8{KbmY)%>-8AusdyX7{Zd0auRMUbmluB_b`cKamn z+wGYgxLdU6K~l^v;`X~c?AO2B(6B6F#fYpFVdo`}nY*~k*+4El}eIyIgST8x-St59aon!;6O}jIfN@8tfKA8dWfA z&A72)JHqEg^hC5oeu{iI!70Kia(wuL2p|N)C}aMPsR_9gS{HmRL>P2GI5@}{^e1q4 z(9!@)K$w4{f3xp8zop(vZ#&2waUHa8&^GW8@OQf9#CAO9INtt;eGLB(Kic-Jtt(`% z8SGZ`Kk&)+uN;OrhB_8GvYie&ZE@5&K6I2gKD8IxU*fm(6Zl#Dzjk!~Bfh)6z%HE6 z1f5s6!xTpcr@ioAv16(|q^a>c`6V`EZQoegSeLVc*$e1z=;uf!Bmro57#K398u^B} z>j&wdC{vZ4(jU@(VU6&qV2j{nyJ!29CPkxD?cJI+;Nf3aG`eVBUP`XdpO~D+U!-5o zKjOcaeQgBwALrBGkMA?+AF%H?ykGiGo!*&d|6ZS7_x|O(h>wFZVn2I-%F2BDb=R+3 zKL_WG%5H${x@C~dcD=m3>;dG@zix7BitP}!heBqql{8cGTD?vE!F1gOA=ua*v==lb z*N4{uGv=4wi0)jkV_qcR`@TPhuklX`vi?1(pPDb=gJ2_pCXkMNM=je5}Ej{SR?)`juxv$TJclyOYs!(LSE9`tW^Ty&rp~gRbB@_?*P`R0#}S2f8-*IQO7?fAn|={&b@{&b7a7b#Gk`+0Cu3 zn_E>)dz$=e&)39NXIBlYd{A+-w7Eq3FY4dAyeqkX@-=yL3-06x7p%$e`_rFOmD89_ z|LgjP{deY{{lNLf{aWzz%I~MYva?gOY;%YF4gIq+=VMMswtw#Szds8_|BjZ`mrk$v zR{o;eq54HVx!$?`Y1`ZG*zTEqUA-TK8-z&`FG;d0MkUe-bONLid0;tW`AYamh@5af6zLr4_R~%6e!(Mq&>Byf`yP)$?wRiCZo%$G z*K=;Eu6}N-9rGMV0`JYRTW%+_U1@jNdcgW0htJJr^f4|&9xIA6h7v{CO8AJ;W1K9G zmhH%PBw4pl=L$2wTscm@8nRFKi4(;m0qb8O3Kg{r9|_IP<7 zQ~RcX_M%bvR7ewxMTMeAqB8L9lSwW}EHDojsv@dPnoi9`eYn2d_{aDd<^_|{>(Tw7 zW0Yc@ad!#r1R-%Wshd2U@(S>rx6~r)8Tt?UXvTiVD%J;96zdTyf&GR(nthu+6Z+yZ z_At(7_Aj=MmB?1H#<5A9`5YQ+1uK;Gl9k2ufuz~F>?f=q(Dt8X-DZAbE@ho#N!jrn zPwq-?Ansct17EgHqAEg_>T4|pz}gGCpgb=YjE2;$ko#wa)?rfI(V1+?)94h z*x-_pLjz9)9SFvb`Y|eT?AUSp!nZ`wCSfM8nX)yiYih}~k+BYOY4HsSgJ&$9`Cw+% zte{y-XV0E_YL*{(0`?}zW;Dd3XRu-;;!Z?gjhQ@Q*2L}O4~K`3@f$le*d`NpUx>mZ@^W*K$uo&!9#4p5cmgAOm?KAB@>1JuB;Hto>^+$`KW?#)qNclh& zEY8`kLpjA#mmaHhqO)RALhY07n}*^!q&0Avzl!JY??aX?*P@|bNji&c-0&YXBqc3x1a6CiHFzju|KotaQ1T|fp7HS zOyKO}W^iZ0wa~_6S(&*3yo;Z81uY{9@9X-iCyv`3D)h+QYnTge~$eJYV?p-=l)k{1XK=`4I(+^4AqU{dc(V-#-FqvTl@! zivQDatOJkV+tq@qjCyu`P}74(e_)C(btyYN`fm3&i`R-@f% zgNPw8b8(B4#__Y6X0sONr2}E6X`cHZ_hFttJU0w&AF2Z{pV^@A-UD8-FGfrn854vA znS)*hzYEzH`Zi=~Xjte#XjAZ|5KM4MaLvdIf&Kxt0rkUo`HzLH@ur~%ywknH2hSVS zJ?NL~2)FUhmz`HQq&qCLy=QB*nr;1;-N3G3_AsNUQ>Zbd@gxzhAD52)iY~JJuv8)y zNT)H+*sZ&zyRKWN8>DGc)6`|EJmo<^8LAaSlqN-wJX3x^u~tzcyDJM(bj!EOCd+<; z#~w!fKom8g7q6Fcq}+k8;yGfGhz~gTKgnFll7VKiUU*hm*%QveZJiy{Iu`c~2*!!(`g0_)l8^Fba<%e^QVkl%PskA@4rPm?Vg%?u z{8zk~jHigH2-S(nX42v9)Dd#8+q)O5k{6G8<+yvZF)B)6b zQ>-Zpvivf1$8<}yJx@Q^=x(etmw^BOfTalY26G4ajLn3# zgcqcDBsImIN~62dRdg2PKKmsb#c|~%^U^?RC*p0fF0`(+_OYq4akTwyOR|f$dui9r zpKHH{f0v)&aMR%r-_8CczX0kk0^c9>unzp?e3`9}-BR1Hw!3T!Y~pR7+4fqy+MKi2 zTW5gobvQ4X7Xjb<9Z$$R!h6Y^0RDBh+?m|B?0j|z-1GC<_t>p$D(4GZ0F0IpzKVmd z!Rm=)0xefJN5r+}HF3-w6W0UKs~f!cR-IOvR&7=pRtl@J)(5PwSoK;-ts`v;toYW` ztS?&sv{G8dTc5Q~h4*c@`e-%V>Z;W#UO7+C3$*&qZR1|%7IRS?9;_l+oH^|6>|3lS ztci?S3M|S5zUGnUT;ngJzfliUPaX!bzD7S=KT5BGeW3%il>aolHCMHhwK{d4TCM&I4oy|6 zm#XcmMXCv^iO?h61rN?+idaQAXQEWMk@2x{y79GP zCAhu}Gi)}jHcZgd_5KFAeui$iE>rhLcM}{xFKbW0BVK*V^-Z;p zTBJIq9uH2iGt}qd&rEfoI!29C6Ep>o*g;a4X|`xKL!Rp#z&?L!&qJ3q9vno|wR^Sm zv|IJt!E3Kk7igdvk_?+*D&U17*Z9PkWL#);FriJ4kOPRfnPQ%Ab~PWjECt5(pye@Y zJ<11Tjk$>4gEnBYF%vO^F<-IIuHwiRiVYorKK)gHtDDE&W0F(>ScyBmA z^@N#(IKpE1yOPjCh#*7}SVRW#3*iI7pXfp?$N#_=f}Yocug9Mx>>^N!4TMtYrPXlV z*5K3e(S*?i0^SaP0(Tbo1)GJfz~(|4$RjwmYccb&!B{oA2)z!o1Tz3$7fh7W5{HaI zO0}1?XVr_nyi~nH{Pr>)<)IcukCG^ z*f0TjN2lt=RWB=Y%c(WZ}*xrlKC%o={Ch!)7_ZIhz6xWHihz+7MFa!NmGDk8~;i`yJ`KxM__m%(E z0vExPdlfh)&eo37TC^ybAi?S44TB8tkqZbF5gLygRu~Xnp3WC^)GrN}3|R0Mx~My+ zQ^9kwMz(PtG7)h>)W!??CHkX=-G%^zr=brr5QVxL@Fb(?-fFkN{eDJcqbUMK=M`0e zR;fwRcxtBUtaK0)tgD85HO+X@7zy*}D`DMdA!6eJWC-~4ij7lEekPu|%Y-$zn4Vga zEOuxM>Ns{U)*8>lZzOI3=CqA?1hPk16dWau`i8occABQ3QfRJpe|kREOkGDmP9H@N zpnqkQF{U$jFrTtYSxea0*+<#m*`b`BoImVt_6|-Cr;0^n&tNZNPh=&s0$9sg&a81P z8fd;Ag6?lJGn%=KwU_mP)y^8v-ogIO#Ier9cI?VL&!n)Hvclk%B*quUS>`Kd8AHVo zFzs0ahBb2=^C9y%WT(G@&*m{#GkqD$7-Z&X=4i%t1{eOXRL~;mm;{zBGZNBA?O7!( z1M4@llZjysXRT!%U`%0dW!f{wGsZI3Fgj=^nuJONyk`Y1k$REZ4!K*t)bZ4j)L808 z@)`1E%2bLyIgosWw2gF@c!7u`FbSFXr=S`y!{q|6#=yE`OW+E>0bDL0>w%qsafb&U zgP@%-B#Z>zgKk0p*TY^w??aD7Q_#`qiRep~O_rmU?Uo=&+)4;#l=GC8u%EA2u2(V?7)892t~@WFE-#S1mmQUb$oi!xrC~A=pvunD zg)+X3B6}iL$WmnoWT8-N*(z!jcNIqES0!I9QQcJSRY_nUsZ>8!kJa+DSnVf`9#*@- zhE9E!A=&U1IfNVn-{)hn?s%iPC$U0pT&M{A9vI;!olZ z;(nr(BqvQHttY(%KiAWwo20FzTcChIlgB|`5RO8p9EIynPNq}XWGC`Y(tgrcqL>J> z1Hv@m^2In2ZUlZJ-X6=qzQtU{#G)smV^P7VtCowFqp(u9nJP`QkQvBn({7X5C^tSe zE-;QWnhiqz7rn3EN&ghQmtAx!?L3XUW{zgCW}4bTyf&HZI#}1B|Da!_8>#a!=%Ir) z=_VUE2E4(bpJy0p2tjP1woWx}F|9V$A`cL$DaT|n<(u|_x7T9J1WOF~c{!mhmSngF z)!^}U5p@t1jvj*!#{^?ASP5n&b~?5f^Bt3nTYx(b8Q2xL@3>94|I&8n;)dZIaA&a> zv45~nu-7qHFhjBSSU=1ZOd+NZqe3I-Z|FMoV)SY>8Z}@^L@hxjn&+A)m^@6`hI58e z-Bn#CL^yPpQ3HbEjTicezei`>jq@Yg@OsHmu=n{raY_jc1$c8>=Dn zGP>zmwvzUl(CzlNjp@j0U)a^z*(XR6oCM6My=Pw!L73W?D~uP;5vls$ z0;al693Va^StO~ET$K3AOj5k;n=~BmxkRO-^0{iaYMg4Q>Y;L_k_)>+w8}BGnI7n2M}grgl*eSF2Tvz?1W;CRWp_ey<*-c2hruitnIifo79twC130 zsm>MFi+KHXeUTx{@XEN`Sc!Z@ZXrp?4D&=Y8y*v(7rBYLhkA^@jy{fAhl#_^!cNCW z;A`<+_!Gnw;tb+sqCa^)*@rxnjG=f@R#W#=HLwr((x=cD(dN?F;E_aOI53KsVrC^% z$o#;{V@X&ZY#Q4KTx?x9VeBnz8poS66lN-BaSn4Dfg}C^o|abJ@!-h!hnoU|*v0=o)Tw-g36EAF(fTDme?;ui2|P$2iYf=`15_ zGTR<>4_leHnDul8{UPlO&5AaR2ICi$0Wv~vf=tCd#3#gegkJVJKtjDmw*aTawk_>u00I(;EIhP($?>qZ}oG-7mv@-#o)K z)bz)6&6J3YMjDKD#!tpH;|i#>k3fyou6w6z0H0}%=A-7X=7a{L=>m6$1hu1Dp>kIC zEB1iz_7C}S`F`-+9)NkgF|t0XRGKDT3b^nrDN*Vs$sO1ragtcXzr=3`62axy6I2DC z#2EvN2l63D!eIEf@h-kJSS1D#CWaIV9j@SPk|T)Xo*K z!luCNTnAc$K7x4#$4-LUh1!GqZ24rlW8PqPGZV}gkqt-y%&R8q=jwMuHu4YUOC_XT z$r;jaNxA5R=udxg|C+vWeHd83#Jv}LANEe{tpf$hhpv#Wj~ydB7PX05>8&SP5*wur zy82V~?`sCt&}ueUZ>}n>jIR7z@d7epUR3o|j;TFY^SXvq^P(1AyS}!eW@c?>&H9>3 zK*&c_O|Sk~HBdFJY7uz+8fp*Mw$}&NuWl@Fm<<}w^44jsC&5pT+;t9gGFQ8P_ayW< z^w#&pLw4=fzK}ka@LS&j(GYM+ll0$$Gt@Kibs$}KLiSsJO+Ho0S30QmDzoaB>aivP z+~9N6Pr#2@t@)`DYE!kNbpg5s`r-O5hN%Vxcn8fedKm9Phn5I9M;!RDoiwa9j5Y=s zYm9GQW}|1JhvUcMX_R3UcX|YU zJ^K!OIByXz(C&^MA26en&PwO4ZYcMegA9YtfuGRmAvr_7dN1|aK9uP_%sbmV+Plr$ z2~g?l-h+HLd+WXB-WPpdc(3+J@^SOM3mjzakmEzwc%cVd20iukaDVH5(gkrj4qm)5 z_HFiMcFXu-Hfc5&tVPy0c&$7UH-h&EW>2oMTiAP9?^##rHT3ng2Q&%Emh4EJPt1XP z*@!`72OvL60iLFZVD9pY`2@Hm{xTl{udC$t{1|+}onTU(2iKv%pnY z*tcSUJ)n@jlTHV8qeqvo3o`|oR$69RaaRMM`n9O`;!i@j`WV8 zj-t-Zo!HJx9lJWIopZYSJ2N^HIy<@!cWvlIcN)5Ob?xoy>HOU_rR#HdOm|lIqHc-c zntdl7QGP^Ia&k?e14hb#b&EW=aW$yiJME0UU(U1OJVv=~e7%#2?w~cJc z5{X6nRk}!SFaHgDQMK|F>_LB(|J1M4BQy>gH!Vhc1iX=!=>zpW#&Y8z@SHkh-eu;Y z)dCcKLadJGBFqw3Pluc5QBB;sKr_=`Ob^2d=BcqBTWqE_s?omz>_Y=3k%E7wL z3S<2l^4Kx~XLh!`Wp~@I+wQ6zi~q~chM#Qr!OoU{lt07njNNRz%XZ^!ci9Ho9<)7e z-EU2|O0=@Iinns(q;LjJRFxfpplk9+BWSC~)Gtqd+%# zu@lv40p0dL@PwP$QP*w-zN%xOUnm0~_VD(ywzQ5(9bbT7zuOto8P(a>v94R*^}BmX z_jZA-JHKaX&$V7|Z=-OpuvxfD*df{~$`z-G=fh_@$m*r>a-(dfQm5Fg8m@|mjK31i zU5x~MMD7D7V*}PwEaPe{KQYhCc#bgTQ zH@+7?54#o{ik^YKk9h(=O%CP);K<#UI?Dpf7|VB59%>PIo!`)>=&wOVWv@c17Ahl^ z>tqvU>j%OHEGJ?CJ$omueK%Lu$9y+^l8R23Oy#dRyI7Ranic_5p-W+7Q&RzNw~> z)0EUWr|ECw=a$G8b<4IEM%$g%mu>Ug{&vmi8V_zN_Web|&0UjpvMsrZFazrQfVW=}}XlcioIj!nNaF2}22!2?c;*d4Y#SIC%}( zLK;R6CC?@Qpva*DZlRo_eFQwqpEilUn*J7WWIO5t>Uhd7%4zaX@(noNc|-xx3(iwC zeloB##rPCJuzjH)3Wq+p2g|@&a5i`c$gobp?ZY8xCMFsE5RF3dP#%EwNX!G~$L1^M zaEqgyPy$Kuj)0f5Np#XZ(pS=C(ge~cawhpR+*dCt zzbUTNcxpLCNI|F(G&!)ZLV1N;35Xbp_=(WjFOQ z^*nS23Di^68I%K*ai9UPr$kU@lW&saDC;SnQ0qHUrcwOJi{X>Z}oN zD)|7pkW@!{NBl@UMtn+~O_)K@5@du_nEx3?h$jri2Z1ur0IqNS_!yX5j>L!I888Dp z0LM@Q9yv|6Z-|$QK z45~dVU5lnj6AL{+mg==irASrSs}Nj1q=HHV;Pq6VYvP@d>OGzv{XKgC?c zWCHHK5IYU~9Qy|Q5PKE762B9=vIF=Bgx>@|LKxu+;T^#n<^@y1V`d>CnLxuMxLNp7 z_~CdGyl+1~0yiF#dcWf);1lsVFn_oMc$6jBOV~re3zeWd(F-w&m>K9@fVc1f(ZQp> znO{Q!cNcg!uQpM@i8RP$WePL-n0_NUNE;l_OydjVBmHfC5imF_bbf$FS7|$7MLVWB z4%n3xx|n%tgSt!oUUf$`1d=AFsV1ocR1PYtvKEf;1;tIpX2l8lWqH3mUw%L~P8J|1 z%Dn)i-zSZb&VYG*iKIe;hFQ-e&`&kNY<-b9PaHikVZcVr5&Odo%vdp3JVAsJEfJ3r zR_ENz~5QHfs98Yj76eh>@B;O_e4c_TRn0;h;4A>JRGA>KVX| zMgYf?0e#_Z)p}JEV2eAIGn5(N?Np$AsO(ha144TZkRq9UC2(-1@&fr>MGzd{#qvAQ zU*48akl&HT$S%lZ<*_n_R4BVDYm;7=zLqTpHv_C}lgv~0L6#&lN`FfmAUnhb+!Dvg z6;R1|%5vp*K^=8K{zHC5J_`EOK!piBX%;G%C=wM`km)2>)+rULSMWQR!XL3p2wHox zrcC_|xQD?yrtXw}oxWC|rmr#l1D@%jQDm$)I+|=vlT2PF6iNul^*Rd;Z9)zG56=e8 zZ45RZ*4?8x8o{4Xhi}EdBbE|X#4)7lfEx^=EC)_cO6j3G((0);v_te#`catArZN{Y zxy(f7UzUh9opqQ+W>031WAA7C!KC4EPBP~g%tGB`_pq&5F)SZ;BHM?xfQ1L{U_G-C zu>XF>7RFb`GQcfP(FC-UltM}`Ig&D(bd7`}1`@Tnk@&gTmDnaU0W-?73w8%CXbuDj z4Y5bo>UZexLw{^gx2qvyMj4`@E0)W=Wx-Odq($^p6e)5L_4a-3D}q@7bk7Gte7C5} ztGl$S@iu&0X3MyiNiEgQg67T5InDE$?>6(Ao0}FkWi^L1S2xXX z8VufF51I!zKWp)B`PnkHC9pNCWmg-aZBv`2HLWAG<6;N1BdH75Rn)bv>#D$6K<|Ck zbG&zW?^EGI;r@Q#{;gs^u^teKY2eYIlKzp(;B0PF4pp92El~YZXR5bo=4-fmM0Z`E ztUqVmVazhVGCnpPG9{R!&2fP9E=EP8W}`==S-?p&Kt|(k+-`7N-HsbdSO6&c0zxD) zj%b3aV;k`laUba$$q5j7EjUW7AT1|hV1J>Kf=J$EKeCdIi z{004jzmRav74&ys>P89PbtiOpz+BFY?(VLOUE_g6eAXS-?a)&%s29Wugn|ul9&YqR z0@8aNe3v%&ar!3pYlOdri-n1z;i5@mwP@S`X&_M&Byo{eN&bQsB2_*c@Ch)|0gI)s06@L|LH%P!cp6_~t9i_XZ+2s*Qfe zNk+MWVDtu7WT*a+{yA{XYv9Yjg>mf2(llN zwUUWaI1~eo1N$U1C2f*y$!W0{MCz@ub?MpUNNY0WyOzFgm= zCqm88VK5r_h&57e6dSk0J;H}M(39rp=40lY=H-x7u+?(fG9PsevI<(zGtr6Y|9HG} zn2(sLxUF!cj+;Xf1d2xp-#%puef-V(|P3_=Lu1R;yi z482tr0U=z$f5YqXctRdti_gSU2tvFc;UA8Jr@`*7f-~j;{K{SI7wipe26hYPB&H9| z!2H5gW1hgddj=eNnMGs?M9oL#fSzKr<$&cKRFsXTdf*5H%mMK8|1dR~WJo7cZ2Dq) zi=+did<|KN%tR&`hZs9xb|l5P$LNBv5idxr*V;u|i`@rQ^19)o@s4pL z)H7c}M{wK_Whe*V$Pb3Mh9b}wEHf?#ZO1s{48WJa8Zr&1j7N+(qtGxIF&b|JcMuQw zS`p|r1B@e$hmqaLE5OdrLq+t%SZzcj{l*l~jJ6t>wP?mH7qmTQX?SPAKo4q!`sktIkm0s| zKh&JgfXy7$f7HFu&Cm@2#;y*yIFYtNQ>;nXJ^~E#s|KTmT0k=q*uQt`1%QPlszecYP3Vzo2z;Q-vVxaa@ zt4kr>Y^!!As7=ObrMhNaIOP1?&>aQVs9AeTH%qq!a>W~Tg*v0IM^^#!a0m6L^!Ig# zb%pw$`cBXz81x{$(-9zn_MSdTzfT{gpP-lP2?iHE2b?eN>7VJ+b$9fK^;liI_Ny*O zw^6rAhu5KX|KRy6x;xOmIqCQ7cI(dSF6j1whDof^X^vvQ5)rv2m zMNE}~^;Y!*^+WX+%_CU-XKRk=7VFBi<=TV# z(|TKjkKw1G)-Va#iX1~eBOA=y%?r#iW*a~WUIG8%jtPV=p%>$fbHyn@S4t!V5h`GB ztszyB{3v0R?UXkZDb<;_n)a5qhJJxw4=M&M%w1{NBSFiS%I0&EV7ht?FWaiw%G>&e z^)IWD*3)c0+bpojvl(yq&W;3`6mI-`dtIidb`aQUOlgu zJC8RFJ|E2vgZZ_c3=?$-Z3+24`2jJ5=uQ|9!?>L#r?*%;cNph!sje3dkM3G`4qG)r(p)^0i&C-fOdmMp)I2g zrmUt!fY0IyQUR%--~lL7FR=-(pGk!01Qjj>zYg~W7lplxMbJYqSJ8Rs$Ce7qLrb&e z4D1(J*h7XQ^O4`6;VU*Y7-|8}GQoX4TFck&1jSjox&aiSKI$Uyu546vL6>Z;Xp()B zJ&~Q2{g$Lj%7N>?JTPv~X4MUMQ#LdJoy@ucCEb0eJhq1+u#--pM5ef-r zB9GJub?iF8J3dh9v?q)PhCMTuIfwm~-OQfMna#b;J;x7(_0F?QGN8f;_jB0*y+v|VKPz=mVH(zf6By|s%?wRMEe8qk0i^CGRd zyj8p_;Hb(5ACKRVp~PWrV)-zV8D{!4h9m6+4Fh`dZqf+yI^q?;`^1FnxC*G)v^Zzb zOdLmlN9S9FmN&o&P5=(C0Cr3_P-cZ0q7B;&Ck$_34ZaTbunTC%_G`9ltThzS1VpNv zp!VmgxT+FW86Xz5u;M>eUQ!%abOVNgD0-l)O$9!5lX5cnt|chPCvX_tZqSZ|nL-_?XL5B~-CX@VaHTWrl?J*o2eRHTAb*ghPzy|geSM;Ntoe`SrzHdx zih6^}LW$58v=zn=G(ojkBQ_kj0Dl(h+(i6KLIuG{7($#%x*4-6IAj}l1T2YA&8 z>PcEL?Gm+%>Hzp0ik?8H!i?-F#&gho;h9wCM&^1(24f-fCo_U^laau@$4q8qG1@^5 z_Lfe9-tZ|XY(Fs8(BIQZ^x5`tQ2;Iv>D=ytVd_>%JY% z#w~R;nD+L>Z`}sz!rmWD(R5vov9va@eH>+8#*Wo}#VSwdhvsw(3gt zfAq@?Q$Ve7&G5@uV!RLcaTZdG+(Ql`_e~E>X{LM7XH2u~g}SyD?vX%LIO;igeA!@z zVe+BIEyfIB4ubFIPV5V858%^tajS7d@Eh@SaYt}fcpHKz%t&TK^{T^rK>gZ@-$zIT z$GEG6*Tf=X6M;y)1f0Jg@gT99s3nSs7}8_nZ_pUrBt0e85YtG%NPmeE;$>1gi2;~) zB1~6qBmE{lB(@NhBwzANVil1FK3dVl<=_l^ffx^{VFuwZD8y3Xvr6#G2ulecaZRA| z^2S$SyRr8{Ke`@!8f%U5!W_XI#c)6iZ3h@zx5Z*v1U0+G%(N7lYs_Xd#u9H@0tkhZ zX}mGacpWkv-#|V0!|=@TRDWN;QojN+ckb%BdiPj8fm!^e{4(gDjnhDxi?Hs6uRN7kYJKa5)SlF%G1UV& z7Tb-{V&oVHY%FFO<`kv~a|3+~(C?9$29y@H2Aza1K{bFj;V5c9>H#VS6^ptJIUm{P z2J>UfKT812Ch&$qq+=N`tAjqeC1$V&&(EU-M|G5YKPy_Vb zQvqGLhrERE#5FO^D6fA=Wqt`pX@Lm&TXyTDf5wbdBvs0&)nljokr> z+hW{ed;=OZD>#c=O>5z&C_)Z`{-@8RF~JZPTo+1M$4X6rW|^x^g>XM{Es?Od?6F+2 zxLSr<#z9wn&r)fjTkI`kLF;}QIzdbpZO@^Po}Hny7#~TsHqO-2fzFCtUTOrpd5wTt#jp2aJ!5Gm+&;nW5iMU~Ge4 zJ=G8mUAxFYGg=#|#`loNINz|`P;TroPBM%!oHSm5y|)J5v(9+OPzj2#0z*4=;1VMT zp_qJ4o}l}VHO(<;5H3_pp1`d5nKpx;p#YBXU*s3kZE7%y;9pJ_zUBW&ItTDL)31$> zVzdov+upXRt*vbvTie~*wymwU+S+!LBF@-0zhD2~l`Bn}%p{W+&vVYLGdwsVxGc1v zBik)x;U3) zUVVgnQ9 z3v-3xoXdc)EDnb@qItr#!VWz9i7*4DbswP`jLjl23wnzh@H^`XFOmZ=UbsW}jGr}H zxJ0;CFiFr{Sp9$8U1ebv=8!E7RVIr_9g0L>^@Lrf8hoBT=wJ=M6}%Fx6RICt72Hg! z?(Z|YJouP;x+X`hT(DA57-|$M8Y~@L$9*Is?^0bwlk~lUlHMp9)^3>qrXavaf9c{~5H}5 zqGCL!t7xKdx3CNMVK?D6;Y85}?w{tuUvPluh40Z0>xIpPJ%u%xd-PEFS=a}S+z8=p z;XuJqfkF@=Y|p*48|B;-!CFDpP?!JRQ?a44talXzO*n4ZpKm@Ud=r!6k-P6hOu_30qz>H#R>I`LKUU!sMgR^p-J4WcU|EBJv!6ff>A>Lr>F z9{d&FBTQU|k2%5(qNS{IB}8T6m)i;F3aH@~-{yQy2| z`sq66IZb6h-ZL2_=%Q_{t%{?RqmUy+zj4^!%Tmws+_2lwM<1=vW!_N{!!P|s_`5m! zQTil(CbO*4jkU}sQ$tfZQ%OU#;WuB`+L&ToX8q1C6r8>3n z-?||GOm5x!RQ~+gQNFF7lFUt z5OX``Osp*S-)MbwVR&tKWECd7hDG;|To+jboxlM+OpoYDDn#^+m<}I%D=a-M0uMuT z$rOn}R7Cue`{yY8%W$q0Qz#JnMo)bbx0Ng?50(qn4$VQ=wLx%A@KzvVrD-Ny#U7*- zxCPa@&O@O>fP+p;+L{`+nnENrh=(@Q0YR6WI?HxNa?tEOQ_@(jIxTN?x`P$}d9=|yLV7^EBO7i)A zJU_1Z*7*v<2gcDo{50jJHOWd1jqD90v#1F(XBvU1K#h=7Y!up0) z=9m{ki`tp&%U=?co~P&`fCRMJ)A<@f&u|J#;2IS(j z%Jekj?L9biF;_wSkl$QYoON+Hc$ugyaa?ljb)3h4JQj6ALA$}$0RHV3ex+O1z1D2o zZreZBRbU{^Y@5JotJ;!nZXB*FZ6j>=?K|wf?M>~|&^hdPY;`Q)nn-pRLs1{$-sQOq z+CR;6iTO9{eMfzAzm}TpgnuJ@^eQU0exMrrLZ?HwLN`Lu>{2hOyw{1Ai1x9j|A5~d zBwk2Iw?%v%U)U{F#3v;N$wr)E4_U7+iB&knbbM7Qo+G|1P7^0cqQr^fRgznh#^f{E zIGUTptHnaj|7VW(Cw^v8QG1b3kR+6dii>)4>~sPHQ?|m8?-_w5g#TBh`dEk0EHT)e z8YLM%CLs_L=n)(h{5x?j7#3KE_vou%MK{p_UTU`gFaJ#ca^Do+5jfw`zTv*U z{z?8QUw+?szGLFbvGuKRD(gG}-DLBn#=|DV4Bd5o)>$JHQE zmRFaJin~VO8vM@gz>dHhyyp*scY}=uRZ$c57o25}J}$UNwKRj8{4e1K!Bv4*7$#aT z+#)Q9|KKw9`%A9-bDWtpQIdEcn$>prA9dpLk}Q#!E4V%Gr^~4B&x;pxPu>(g77gag zU(J5|f_tVG{qH&972#6hM4^oiBTkqsoG(}}xWg>!vz(*G!v4ZYYUCP%HiG)p)I~#; zLxVy+LY=shio-)SgM~cJyxec_WTAjEFv&mB|B^SoKR!@7&<2L@k3bS!+BE+v{}JC6 z-!b^LX1*@ISbueYci#kGgg=55*(!dJAD`W4g8jed^K&0%`fmBI(H+ZuHs55tgOlL7 zA_LU|_kQ=%{xHAO9~Rh2ukki;5Oq*tTypbJ0QL&34lWMHP_y>se@);g+Ov8N3oZ?M z1LcE#`JO0PGuV!*NR0=*C;4y2V3E)Tj`?b8pR8acHR|QiVa`R@&3q27DRFW|K8FZ#M8=;o3|6~P}|q?wjR&1e)|<@~wm5d?UdSK&>2BbJKOMG8@Zq>f|) z_hfJJ5OGm-R6WEC#LFc6BsTW(7)cFDEo$2i)JiMGJ9xZKv{rmjJdPEso_GWwcd$C9 zk^nkW)Kb)kov<^wQ&Z~bcu|HRQCO1Q{Rr1rGIwMz!C*l;_2MV`&jrEt!Ed1qSjQr0 zWJZSm40gm_>k8BhR>aY{H!uLiVHF(hiNF@laEm}~d{@ip5NdILItE4uwzFR-1E2A8 zpZ5R5wb3|GH&7a1cjI8;;FG{rI*1EgFOU5*SUb;f^;G4Y+~zzM4;JMdb%Rf}1>RDN z)CtrM^b0Nu+5(Qiso>>cD{z%*fx&@poY(Dv^MT^@6~A$pN%SwL!3TZ>yn%PYykK@f z7RZlA;TKr$ChqFW^o-;2zkh{iJ`eKJDbP01)Zd78akc+1-$ZbiYW`fG!8gS}#xLR8 zYUQi#>*MPSI#3PQ@!!76RF}>*9w$eKt{JVqqr1!k{f#<5{zIUg0qNg2v>{{;vIN4&%#BPiWcDrY$XRNok z_lA3^`?cq&NACXQF6Gg>pTlJ=au0NGA>ZO(*BsYJ_dEA7+?7(-SF~<^XEi)JPn>&5 zA|C2IkLR)qd&+oMYcyUvoe?gBb0yh%_tA0{b%r}{J9atV;OseMp9f!>O4X1b7wizn zHgtz0@YpqU`aoybI{tN@0H^)eQNfw$+<+FLsxyEev;$mVW#>o7f{)QVI z4!@e|lsgx@Cb-JF67hVtakH8VGUnHj%RD{bI{Mmys6;hPxyBE$}mGP z!~fA=)nAGG-|r7ck*{0YOw)eLB z%wkJ$nC&h{rb7o;{=|8}`5i4!H+;nh-5cCR-R0e3@UA62FnD%emEXEl+NqUJSw@e&cbMcOOvfpgvze2PKC)*m@ z>e>}(3X0f!I$AkyI@UWnpzVC?cu2y>bH_Md+jTTk6>xrMI*6-&Au zTz?l_F4iHL`;Pmn=P&-+fhZ!Tdq#Mhc^8A3YJB&7dT*+?lRt^P2a9hj{N@_wv8)G| zZAvY+hgJAx@KkUkd-a*%Dsb~ZK;~8k^Mig408`)396MR?U9dg7(K#?He<()q6>Jno zeCP~%|MQ{qq5Ek4Jt1#swP1_jX6R+e2xk19{*R3l-|q;)LbAnnvp+uy{Tpg7Xdw6r zqS7hU4@6{n=vwenuvTao%A2C0DqwnN`F&Y*za!A<48-r#O3+Dgi8@pjGKacQD@+WH zf&GXHeGV#vbEyk92hRr2gsz7U1aAe$gwBRK2gg%O)CWn~8PWw~z-*?4YK97g3edaB zg6YAx{PbqvIKN+;koRe5IG^yketSi2{B=D-jYAbtTmK8Ay^p`G1Q<{j`|mqJmS6+u z&Npi1I_MvF2{uz{JVhN{88lxj*uz&Y32!q=z$X|DR`w6yJuJM&zU~2~TqB$#IwkrW z|Fe|(p(Qf|zJi(U7hM@eKb z4)m{K;fo`7MYIkd9WG>6L40`Oa9MbAM4N~n5tSqAM|6r{N>})`@Sc%FBj3Y9W<(Z? z(nnN`tQ5H>@(4WHEi{$|>2v!=EQ-)ZmWX?9vp z2qQX0o6s57#vlHa3OX6BoEYz9w78@=K~zOF39P&knAQ$@+S;NTA|;*7HSog|!tcUc zqF17P)Nkc^>`%O`t3}W0*qe!hc;n=vII#zQ>?-*CZqUFvqJyIDpby2szK)7+i$36u zSOq?u2=Y5xG(!}F!ZC*brPCoriEP5X)Q{7t?JMxMMQ~Qon>@jrg3|&q*{^O;!tUr# z&w$Nbp!!t9o0Q_Zss;XH3kd{Y@Z|-#iWUm?3%-QZDCq@)W?Vn-xd$o=I|`rw?%Kh$ z$J4tG5sc;jxgE-ZyYC1lTbFD98kl&IQ0dTFo_`HiVh5bp+t6d~5Do0B5r6iBkX;}U zj-o284J+0i_stBh&A$W_`Q0kAEADYeeh#IA8V==ay974{XYmxY=RWKS&RAJcU7$k~ zm&X0{k)PX(w?edJnL!hITg3%SLhD1Tz>zedmq~a|iiZ+H`d~B@p{4YR19?_}`mjOp zAspPk;EmwS;B?StXJAQiK`;faa8=fm>%q6dfmETXXmek|ybKEt36`ZUJxcF38GhzG zd%$+;g$vXZ%~+*6p%8qBE_^C`axOY?z1QU3?_J~_LjCcU85c`E^E^4Md|lkV-0fVo zslFb&8j>IK)%lTBsZ%KB2caw&;S@RC_HvG(JryNGK1Vue%qiOe+Zwp=*0#0Q@z%b! znzr8PWAK7e{;%x$%fvi7!Bv2C-BwiUzUP#FcpVcRI1&-TDJ z($>Lt%s$?}z}|~wtYvnILf1i4Pv#Y zV@~2d;~A=zLQJyVU=$e?`ZCOl>}jy*Uy;i(4Q);*LkVUuI`ww_1ATL5F&@_q)(z2@ z*MHUR&{fv^bspV)omO{^xicCj7=F+O!H9ooS8I#uUTI5dZ)iH{g4!(Y5v@qKSzAi~ zQukatS=$CAxq>cLyGq|s-<#b1N4iD2^ZLp9>Uybew4t=2i#|VI{CoO;@n(KtTH_AG zAe^)Yfa(bjt-xinSC~-*dboBD_Rrt#d3R zPzvs`B=dZ?<+G)s#b_2(fIgEyEh9%)gTCtI3ZqRn#CAkqhFn4`?E@GJK> zmp5NGE#PZa&Erfdrq9e4+ska5RMS7E>S&x|Os{xX8YdfT86!<@e2($PiKeEe?xsqn zNVCG!4o^+2>4z~EZ^!R*l#bV9Jb1dw_#7Q|Wz$kqXVZ7nzdR$KX$P5{Q_0M_L5fd* zbAtH=E|T%w8&!Fg715S1wDh%Xw)U}>MnhlQS_1v!zm_M~189QQSu3MK`DlHDN=Rm{ zXiu_-*O^4l-a=cLd94YRkhe@FFF-bfpo?f1go{7wvi1N9-4XGpNdWL&Kp0}RKp4Pa)c6si*Z@GWxtQ~Z% za}{=ZoKIc5Tsxfuz;=(I!M*Hw!aep7UG93vSYEltK7^!{+jyG1Drr5xZ|>x_=wiMGA2 z#b$nrB5nz@MAOWd%%{zhc}z!((2{A6M#rl#tIRd6wX7?wf3e2229rE*U1IBw+Ukw1 zFZ$aZjy;Zi_I&n*RIDkEV)Td0oG-zqHXc*d(I}6T$=-WM ziqTb1H*X2=EO?S=uZzCPgF|kZw}?N=Kf~9~ciVr#zY|yaGdy4Q@H&rU&T$SrdI;R- zBncE%g4KeOkT~=gd~XTx=zi=BOTeTeQA^)|m#!_SDQFjJ7rH38D>xQ96pFxF8!vE$ z>IlM6KyDWl6hwh&H5YV+AKT21IVRLTGz(tlP-rW=TxzHwyreDUfK@9Xkc3>p2mFk< zP$bO96>8N^q{PodjZmJfx*p&as=!Tfky~Js+rSi#237`YpzaU^vivRl6~PUM_|trk zKmz;VUQ_v`=*<51%|wgP&+7m)c} zSfg07XN912bNSi_;d$XK5Yj*3`ELtP3B~Zv^HB#?20xbzOw?vo(Mt5AR!<^Yei;sc zK*&R#RESQaEy(E{Dkly4f*bG!52&P4sk$~$v;5Y(q=8r6qbDfH)l~xxNH@BIf%HPP zLKQ+i;S?rne@|qDTIO+`X81mm z5c&Tui-Yi&uYB*pVp8x^-1KI{XKV$>GJCWhkK2UHW}7?S74B;1?%?iA`qxjYqc8Yb zUCd6n<2>rrfI4kMh{V~nE<`I27yY{y*3 z7uJ6@FtwDW|Molxr+Sl5^)|iT?itROS*qOFLwvDV$ zUFd};QWcf5#oKmRXW$pCW*up*XZ?;2UX4$2uVuRBxOE<@;}uI8TZnye2@~x*Tc=uo zqaz~gdh0T4j^(W-WHqv{?6dqtgF2s|@`6XnnKL|%M^(0|s79i#H*EcFU8%MLwp9A? z4(LuL_Dr&k`rAtJciH*tmf7ao8`+=m@v?mcuE}LE&+SQM>u%4oudp|EMmk$M#EwH` zTm6lvA=#Nux@JE|nDdciCk}zK&L567s3mJUh0ay3A>^%1b8d8XVy9S!)~~i}nR|{~ z<+|xIxL=cr73VJQDedV(M&oVz@$sHEtQQaP)XisNg2O$6J!-0lu`%B6-cO!ep2glj zK_(MHPTIqoAK`mFJm-8HeEUEwOZip45@0C@&^NX5wer2jYum|R-+vio{+<6{GTDCm zCZbPV4t_QvM6`r=D#ep|=z4^4I ziKQi8v?kO*<;*TqnB_D7zA(KrFQii2W;$u=U|MS$VybTvm^|#}D@}u$PPv1b0^f~C z@vi+c-ZAztRWwcI5o?U2_`I0u6KaNWrXnUQ{${Q5I9C7fhE(HD<7i_&;}ydmLnrEI zvEioPO07D~P@5E^U4}W#cmGGfhy)Lf?z8T+&ZNu3Wop#cVjgs^_M^6dE|YYo2HKOP zemvH$Bl$z3O(yLnR`XOnNZUj!Rqt2FY96TnP={0<)E?dw)VZp)sv2qqPR0L}iGOcX4 zY(H~Zs__5CvZ}J1vYs-d^q_RJbbxe@w2d@QdO5GKOhM|yxxC4;3bLKj#?o!l+R{=? zIlClnCYAEP($X_|KIvW(IJV~9B3)y_?uj;6}x7w)Q zu9h)NpqnPYW~44kw?WrX_e1x$PD)>}LElOL#IO}NLIJ~N<2-tTvc@&0_NJMpex^63 zQ>L!We9xxtR$4w;x?9>mY`ajmEVBLfMV}=*HIY=k+m3(04)@tNIZnbc zKXa5pbAQfx9@M_AbF!jQXW5qB)Pj8|OE(8B-DU~Psg?4A27|4wxm#!Gn=jqvFI zttGQTS+98nUXi!0w;RgI8opt08*{-)?Vc!VwmIIn)EaTr!vFcc_-=boct4VRmg!aS z@jc3%6~00KelWKO{byNC;sa&S33ep2@0|abZ@cfI|AT*%e=*6+7f?Spd zYt&-L(XV}jhe}}$Y7lG=|I|Hr08gC^3}OoEg*xoG=i&V>2A>BD!02s72{ja4aVSWl zCL|~OaS3|NQ=!(vabRp|f;p@yE#X67phdHx&f7^&-3D}J2g!@<2A1Z7>B|yHVbN7+ z#WbRe=otFJr5D2^d5y2;N9{713^_Xp)NDAu*XY7$;b-0~J_?tf2jVnetds;KJ*#l9F(KI*BOkZ^=&9#gZeE36i~_VGYANpnvEqo{K(yisW}L*kj2z$yDmL$KvZS#T(%oZQ^(00AD$U zN~^B8zi5K!u;h*8JsP`eaE=Oi<@cfou#*X55&6=^#P!98L8^i<(8Z_^dvkoof%j}e zTlp>YAykjrrMRGlU{&xO>g3(Qa&Rj%P5wLn7u3Y<{l&-%PK7~ffaYcLICsDe#TI9kU%P(MWWqKPm1II9fW}J0)-=)yNoX>?q@y z4ek+35??2NY8Pi~rytMI59fb;*4`QGigzXRl^i^h>1gV|!AfYHsm{;f;bUEcT?^^om{|aaVpW#NL*2_v(AIgo8+0_<+;1z09HH4&==idH=pZV zNPiS^h^db@I4aYJ#*<<2`(BmWf7m5XqeE$bVV^>6w350&Wxrvs%_c?RV$_ zvuri#W53gh^*{@<%C;HAXR0lORpBJq$tda*vCVG1OMP;f{(pq6y{)=6*;auor>Po>xrfFy(_IqwpBtB$MwliWdm77_qD+lI19}+S8NV1V z8j=heeIa9{ak!zfVXbk7@jt_RgBbt$YaCcr`1&e*)oMeUA<1NhMVe^5MIuLEV`F1W z@PqQE((G|NO}|V(O^r-Rrmg11;67JOZFyxcOt(#s%rDG?m>qEsf0xc8v<~L;GbDX1 zqXJAs=k$B6oKBsl!EJ5EA9NKz)pTR0Ed^X+8kKT6tI&FyjFP&xC|*&2kjf6W+Gv_~Fe_sb$VIm8sqMb? zwe_d%Am6Egi|7f?uARKnA-47O1JiBeY?JI0>@962ZC${0V(e<$7_zJefFgc|>ugQ0 z@B}{bpnW*s7250b??8JAru+@DTdBFmaxA9faXn+7Za?YR#0p@xA8|~AeLD#Q7R5@j z3|CrHM+!CCOzyX#oU>=(Lv=Z4Yrr&q%#=aY>h+zaVc*NJ zzjk-_fd_v^u45Tjg6qEHjAONPku#3(s`&ggyYXlH6MHMPsQ=ow+kWFvU2#c0vaci7 zz-}vp|7wc8A5(>twmtR@_V?B!)=FgU-mspr)`H)AW_@NYV0Bo^g7x(S$=kx28)}IG z2Me=~v9u#&wF*bO1Pp5?xL-Z%4ltlwJog>i)J9-qX(&~9(>rOcce(EiTHoUly{?}V=unrEyJ z-Z_}J>c&k}ZSxJS4XyPaT{VMJ|Am^ar#?on)Nas*>F#OoYxk3S_*s)*o18f}`MXZ(8R(zr=T_97)H_Io<Fifzie%4FqDMJd%o%&-IQ4~Uo`79-8Dva zUu{rxQ!_%dQQJ!UOM6W_k!nAdS<4S~D|D~*YxUpAG%9Q`=~H+Q8svr}hFOLe#?{8{ zrn&SN8;mziM>%?nO|Q)Vn5&qLruO7(OeF>253AR*lQklSOyVJ2TTkFJ1gu@PZ85gJ z^u|K?3#aX)?Ua2F%E$ut-OTcM#5(5)iiN+<@#;q(`3{J3Q&>7bRCcb7+^?URK z^;`A5NL4rkMtTR!7E*VeN+8#-JiNN-4k6b>0jTqe`|Z_TIy0Y2Q((_4Q9&q z&^*Rh{)@XRMQhiL(YDvxG+#AUw0@07eMz0E*{unx->C=UFJDAz=Nxrg^<;H3^$FDq zReg25`WW-0)pV2XRN<<_s@^Jva=EgZQmZJg%v5Yulvd19)>gJ*jrdP7N|7jkBYPp6 zD?2Z1D=Q&;C#@&Tq!WIbmm&+7dGq$>m6u&1(eqH=tGp3;W%KUj=F3}^dpxgSUW?q+ zoXfdgbBpBs%y#FT$Z4GuL;B?R?8DievI}M3%Nm>Q&RU%nku@p%YnCUoU*^fIdRZ?s zJ7t#6T9x@MvutL`%%vGWG6!e2%6yZtKFiK`-enBT)MZ@HsFkrO)19#*GdlA?MvaWJ znfEi+XGCN?&+M3aI-^xarOam;V>3eOCwN?Ep1VD>LgvZLdYLOToaw(Z`|y)@WfaT0 zl=(PwLS`^?O{O7hSytEVk69I%!+JAoN>(uYRCecVRaTjtr`ZFt!?OiB=d;J=6wYa% zQzplnvm$45PR*RRxwCW2<$lQ-nJ3KKk=rZxcy909DtR|@cjea4{ggK@?=yM%rKDf; zTIVTpH%XgH|B@z3FH46?pG*IgYNflRQrQw&g6x^Jj$9#2=J?f>UzeQ#Pv|E%%RX?d z!{quXl>aI(f^C#g$&_zZ^Hh1t^~x`*{h$No zR4M9M^}nids!!^<>Z9s$>Y5t8`n_hMCSH@KPSAeQ%+^F}x`3sGYtE_DGT)3*}4YIHg9hifiw=B1Umhewu6VoqVCZy&_4`K^~N?loyoGlvj|~kn3g7 zsP13f~Q+eI<@=Gt~yHWNGWPHvd)wY9FmG==$#|kzyU793aEGsJ8AT2K)!x>v5 zOOZ9^ye=T)K23g1epb;+F^?T|GxOIvkm-@2eyb|0{-L_1Ua2msd80nBS*5u^u3r)D z3(afYYF&L@KAl&8SHIEF%<$5%%FxbO3KXXv_}d6F1&f+Wn=|1!&zNfdkGah=*SA!# zBwDqW50;abY}V30tof{iZEbBAt(UBSq0^ec43r|a%4F^2qfYtDI-WZ57WH#wdxCu* zi1Ptv^IQXEzHD1h57QeCy(o&W%C>E20oIbkmreyf%-R^eLAG@}&u;|>`jIrglAzx% z%R9?U%OcBFIKjH+Y}0P@aC2po-&mcjexdQQA;+-5aFSVkCs^Zpz-4D?PiX(p70~U` zw9?Gf)Y5!VEm0+^->8yQ50nYa6S62CDJqgj=9Qn9Uy^kvk!+AmB2CZh%Na0n&&B0l z%GsAwD@V=BF*jS7{Wn>1j;sS&^|KyiF3hZ!*_m}glDR%(cV_F%Mj5x#*Jm0tre;{u zhiB$xSTojUoX)6_5tex_V?u^E{dX4K>x?!T8JTl4H)kei*3EjDc_nLT)|sptS=V#A z=Pb`Dms2ZGp1U%)a_((jfh+H9-X*TFWcgd!ZP{p91!|!(q#!$#XO;JrOO?O>mdWI{ zA7HKhm(04AntGZ}+8FI+%~VYronEU4%{DtGjLjY_*m z+g>B%UMr@#uU?@3Ozky-*{+*Z#Z=+s$P`fBR_;~RVWMj*jjzO>xaIZ3%51R;zouaZHWK z&-upvxgjdu%1fvZAs3}j_HoGOpcsjo?BiZS1!V#za3F*Y+6HmQtj(UG(?N{nOmW%Pd-RQgK# zPr5Z^h(_v7I<;=8?zwJ(uAffG=a+R>?Rl*q-O3s5H0>eXG+k}2P+MC2M)Og#R?}Q# zQP<*HY^`~&&ep8gR3{~}nI>0VTJug_S94oEPhD5NSe>H&o#m}i9Z|K=80cE$D!ckA zi2Z%lCCvcME_Gk^RrMHkLH3TT%rX0^Ij7m6Nz&}o4$*F9pZTagt=-2o-9*l5hW3zl zzOJ6GoIXc4n{?E(+&drX9Zu_~=?m-ax(LH3{YL$KeL=K3GYpLk;|*mDDMllc?ydSI z#_7fhhI)o>RAt4CTC~PzjNc6p3=S}(kH%d_51&8eO4(@WU|0sybeNph48s<~Izv;# zTXM!@jL!{q4SD+UhN^~pG9h+s%@?9rhCf0Jx=>VJ6-!rw~N1a z2|UPgZC9;K(^Y5FDzvM#i*&Vg<#k`R^?6^RE1_GDUZ^)2v9H*j-|1HAqIGYz<8|RW zjdrtkB|A!6bVe@Cc8yp2kM<=ql*6QU#+l~<#$TqY~2cq{KqpV>!Yk(W_^ zRJ>Gl;%7HeuBH0?q}oI++EX=HT}qv!-mIR%>q{Z`y^{8arjIUJmtU8q9jdRQZ%J?5 z)gUyCHPkd%Q9i9Qwgwf9FxEHOjgO2wjH{U0HQHRp+!m}+Zaz+`*75(<^~EeQbV|=G zH_0(uW$jMVSu%;p$<~J4XA{{o|D@-VdLz{WXfH! z-6tc+Y8}n&t0a)qL=cvnXsewdOrPvmQK9{`RR>S&PRd+ox~J3jt@hH6G`p5$HZzI0 zOF?#Sz|IaKz3m3BFUjQI?EUS*Zy09 zclc*={Gaf}-3?sFZ8sV&CL%D7IcbT3lt2RWZQ{`74DgTgr!f0ah;OD3U#rLGJx~xN z@0J2^J|oDbIb)vJMPYFcw^pA=6h{RxTQY5ZZXRza|EbdZ)P84fVua!7PWq` zo(7|@WwSE(?mhKgZR)?goyxAS=h*YaWd?jP`et-(@*AX(|)v?c60 zASp$t2D{l>a<#W68~VC!zU>mqux?=fBR~a<+OklcHYTrmFR7*}6(R=8b*|vq|Q%+W*0uT{o7@;$FHqT~obN*A$+1sjjf@2mRO;`t_r9OO4p` ziqo4tR!vc@R98@URXLQg%oj*feNy&OLd*DyC4ARa3m8zi>#u%T~Z9 zZ)=<~z+W8sg~?=8M}{_*2`(^6ZcJx`5Io0-}wb?>)}sYO#~etZ430{Mic zzMcHq<=cN>AABwGbt8kYAAU{ws{7LGi}h=dubsXw_@ew;oQdm|zRdgj0hez zwO3!ue!cR=_xZ>-&DW@JqrQ$$z5A_9>bP&>)Y;!gr+r9GO|6xxNo$(6KP@M<~Z zOCOg$BqK9@VY=;Cj|^A(9F9_vjEwY*^r`8|8SB!gX9UxGr0ai;&Jbp7P51sP$k+O$ z$E80>Z<>B3y-E7XbV2&V^v>zE(qI0nospA1H@$ZH;f#_Q(HV!*=Vav1xCKHnJtHh* zZf2Rx0U74>r~-0-v(vKjvJPeq24$(A{a@DU>@nE` z_?ersi)Yu(O3Qqo-86eIk6MsjH+yzYY|bL4T+Pc7<~+>q$2GGCgl0(g$D9c{&2yzW z!rUV{A9Ii9F3T;O`#Nu4UWkd|UWs#^O%u*#Cw?%dUtwcZBefelQ-#xOa ziee;9Uy;vLc;w?0RTVb*CV6#w-=&H&inY`d{S_v89pz_5IptSHree9`EEvEPrAzTc zHAnSE^|xw|dV=~j*oHxUPQ6r9RdZ5PTC-W(j8y9tnkG66h=WVBTvt;!1zchwwc!G7 z719;2Yi59s57R_wYHIbG_L@v}18t^8tzM`et#PYgs+X$osXM8=YgAOJT~+a#wd(xp z)vAT+g6avXXw?B#B~=ShiQURF%B89Z)ly|~Wr!N6r_!O&DZ9`m^`ci=rKqL6%aoKU zauc=*NHH?bEvoUV5@6`r>ecEkU^t&tlfdQ(&w|v}FTr~rsRpZJG`E=2vr(0yUZ?KJW8&30sx*4j z`t*W5)naui)fZ(0^;gvkj&p6zd-YrO3iVx*yX!Tu%Y|v>FUXv zhMM14Qa6oBt>t{^)&Ht(;6OFV2`#IAMtbFWbvurCtmcY(yQaFPl;)ATz9tn6a-Swa zb5=bO)M%Ziisq+wl6Ix0fu^@MU1QN)(+IWKG!wPa+7?=sW(#%gSZ!VHIbB!XLGY!M zx{m+Tv$WFNbRYCPsh3yjS}>t*pgvxI+0fO{(O}d!0974<7JnptOuBKm@sn|yv6A_d zX(apSC^YSh%zv1#TQiLanSdjhJV_N zyJ0*0eiyhRJ-q8PJgTkW^rtez^pbP2a~yNTz0P!W-lIU*C8+A!xk@k}zzs(~&t2Vp zkh;z0mb$CDy{-f9DeebM%lLsRyMw#5JJOv;P1oAJ5RASJ8oO$!?K0g8cUyOg`?32J zI`OL@{Jq>6{O>#dxEbzt?xFmi-e?z^Gda~yer{2>)g?nG(1nQ~16|3kf^HK`m(kf4 zZ|q)PjoAIhbqycw6LfwiG&wz8^>}@sTr%etr-^Ll0WP8I5-Rj2%wZjj?{=oEnd=}r zf-0`3IDk7iYrvY-=P&O;x@H_&hycf6DjCDGP-L%zHQ0)VK*t>QHGI5`9%3-uz*M+_ z_wd#Y>1I#BX@6u!>|*>HHOUj{f_9=f(=$x=)9B^@CIR}0{h4Dm%K0jeT-dxOP6a=s zh*QDL&$GM^1$AH+(`MrCeJKFOOt@kk|bxibl(F19`m%TA42pzt3 zUm4Q89t0i*+L3d3ki6_}_yfm>T8E-Sq2N^9c;84t(uB038+hCP`aN?7uX-o^eF?&J z6n;kqC4}DuE6C|fA&o1Zbf$E4e#xR#;Z$KaQkO0ZSEAJk3AMtT!aKr5oN~kP-q#ZP z1+#^NgrCq6Mhidlj7@^s_?UK}<(&{}NTSIrYUbfVU+@`|9Q*T%M5IH61x0~(er@0m z{_*4fm_P;@ealGYyT}}pAtcSUL@7BQKe)xS$k!SF@+v$;Z%|96dveiZn>=4U0@$Lv zp5^39-*O+oPh@oOaJNCFp`w5qspGr=CY-QJ?Xw@ig&-+#~qenOyH>kuxQn>YF}wX3>C+!jAr5iZ z!9m`Mx4P~>QPSLS&2lXw16$;N>(Z0Sng>5-kU7pWX)?Lm02inFm_(22cUeaCX@rX9U|2>LMXr?QK_V*J$^&0Ts zulY(pd_i|$uo}YBCgXCgftI2S_vRw5@}YE5wfLEX_}UGayr1ynXB_L8(R>H(%YFMW z?qW6lSth!Nq4?Bilbt>8_uNE&?-d-;Z&0;)$W&R!z4I8o+9LN@cSrmgCXb2~l+&Km z-fj5nmv|1MUH;|GL7(=-_X!v8AiTfJeN%lEd=aEgocI0VZNYqy-`cff-Vwe5z6NwG zb1BP`BWd3{gr>Iyvp9xq4!l2R5#Hg)>eI04rMRRvQ@{2;sjMq-eNM&b*Qtnio zR1{HMme&L|n5KwUs1?iDB?~L+DsIb{C{h${+^igJXLXaCpQ>pFKs|G3f%8G z+*mjAs;}s}>F((!>+0#%y2BtoK@gdI#)l{&Qw$4?-Hm$;(+$tTcUFTItYG&sKAqLr)!Yd_vp!8F=j*?bnRZ;N?>xv|M(Y-A3@ zF?NsdXPIoKccyw~sp+P9CXe21>SZZy31^o05pae^^l4`;Bbm6g-jW7NSJ90X%!X|$nP7x6)j+4=K|ISeLO#U02vx*hbJ&kcxNvIk}Uigym6IP7|&ZE+S=)@JB(6 zuoKx!vG`+UWF77jZYNc5JWiDVNW)wsTuOduYjRFIi3W&@Fux~4lqXEVpD}~%lT&z| z7n6#31E0!Bav4{NHjB3NoN$pv7%M6!x=RMjHT=2#$xTcl9c-|0pm3pRq^JzpauH+> z&L9`_KK^Qw=0rMD`_>94kS-Txp+cmnY-GlA7cyNo@NZ|lw>9v#&*IG7K@00Z zHQ$Ep*kQufLOWS+DSVvI@jpg_%xY3&bIC@ull!!ve5Z~~Q2!!%$8=enL=+xQ*1v=) zfF4{u9l~W1ZzIY_R*u{gIg4pK#z;q`AgWRHu;}zCebmjEOeVSYVM?7cW@yZzn3XXH zVwS`{h@BL>F*Yf#aopw#B^SMRfyo8D2`PzX6M7{KOx%>HPiU4nF>!X{ z=)@g~=Mr-g&m`q0txdd~I52r@azs+Uq+LnRlj4(FB+X2El$4d!AX%PNGP!^9mE_}z z^2Et}{3B^eQmy1g$;XnuC8Z_BBzH-kmwYtIn6xqZOLBvx<$Tm64@x?eR6luC^5&!$ zNhgx@N&A!DB)v=wC90E}Cr@W0c(0_LNzD@fPTZELPh6gGHDP-~YJw}FKw__ixl9dj zkXRrgC1F6~^2GKDOA^jA4?HTNX2P!mkqMs)h!XDQf1W=gp;W@N{9p2W^VcfyD*uoC z>G?JJ8|Pn@{}xAMPrlFjF2{>G64UaZj{iimz9e7%_?GeMaS`z!V;jb0$GT%r$9{^P z6>~DCeeA5*G0a=ciZR9d-}4%~ zMgA2zJMv6qa%4)RG9o^*UgXlq(GhDSQX*U9*KZrqIC5a*TXM5I@NphK{%sLgBC168 ziQFCWIN~I8m~;`&2u-9u@?ga6h_B>ncZnDo(TvQ1!MyJzhr3V2gowG}OTz1rHJm@Z zP53@?v;PdA8=f5=2p`Qn$*W{Y=a4*n6=bnQ^u`u3>Di*#2=#xi>Gy{udV)zcua#8TRettH%$G zzY_N=ZX5aZKjTF4UE_Dew`U^STypFi#xIC372k%8`!Dfl$gbZScRH?b+{U;T@qfh+ z1dQ^XY~4S!nPthl~>uVGxPIAOdn{v7jF%f;7-{~9NYD;C!-?p>TS z?gp=FY23xQRvr^W4yJNtheT?Kg5&eN`+Uvih6?9R;2T5hxNyy{$-EiS9o8b)&nYz^1c&`9cm zYLBL+X0_^)YLa@ano?I&C#Z&~?y9m>ja2cfNvgT3t`Jkx0CD;IDW@qjA*N=!@|^O6 z;-_M`a*47!L>`}{+^=k?=&0DDIIj3CSHVb#Q?yg`QA|@TR;ZMb%Kve1wo)zx?$^)q z*O1apF;ekf{!1PW{E?l2o&LD|v3!B-gzS@CsrV&R%6rS_$m8T=)de#zB8ig$VRNLlD`I5JL6{;$oD91VUVlrSf=LoNT3Ro9q+BPwyd{Ci@#=lO2To zZGq$LbVx=>f$WJaJ!DTv81&kT5QnS*#N*f~yDY0Mt0mhbKP;aQT)~^<>2kB2lZPo{ z6b%$ZAreP|qJwg<^1I@#VguB=iL$k_BlzPKaN5mQomXY3g4BmqFI5fHQECQwHoL1A zs(YwLs}87ds3X+%;ong8C$&yfuJQom=y~;g)m7Cr^*wb2^rcw!YV`wEg{r%1n(7M7 z?fxp6Y6WodE>d1oMyaN#dH{c(68dEgWr}jXa*^_n5?4J_lq>EjOO(^0f2))hr3_e2 zhbU$#(iPhkK?;Q;P!X=s$Z2`G98)}(Rm$$bT2d9F@c%EZ2;%au1Adl{^6~P2G7YeJ zmdJa`lHija##*qvrEIROHN+FK$ds~OvK*OL76Bvmfb2`iH}IB8wiDVQ3wW;@$f99B zX%@0wb`#oTSx5)qlItiNBYO_3*;V+r2Y4;BWM5=*86~?a%az5!9KeCEcLOt49*p|k z5bs|Z@+-KGtfs7g$kdQrS-I?XNMXoWm<#1F6Fdg4fNJtq@}{z;aE*o7Co^SN<+<`? zh@<{lW|x;j+!Pwt$}e)QVvqc~{GQ^g;tKGfL@L`U^~z?-hsvkQuF4t8qwpz8HB$9a z`B&LUT}Ay*sZ%AW7pQ-#-l~qN->9dl52%l;v($Gr?=(F$Yc!j*YqVLK5{(R@kP__= zZF6KGG89>e+(I58F0>(9j>us(>8-Sl^Le|2wl9{oMwZZYfY z>Wi=l{HXqiz8Ag}FT)z*Uc40^hS$P7;6v~acs0Wfyd1x3_-D9;f5R6WvJLeO^9?Hu z8HSs<*FYN<8j=jV3=i>AdDH2X!H%|6w3!PddP&A!<7 z#`e@!+kOP@H?>c+kFaIg>e`Rk&)Br~FSbs0&c4L{+*Z}Da3ndZ*eBcX*;&V8Tbb<& zTov~5_FZ7f^xC7}9$7+Yye%v1K$aSo;`#JtPQpq3oVU9VDAC9h$ zBaSAH^^O8ZL-L5--|^egjEr{dakL|sk_nD1M`LmW8A;9{JCiraw&Vr!fWt)gA$WNN1jNC8>61lMS3satvABdCaLL=aA<} zvvV3*L8g$;Nya(LdCSq09OtZb7CDm0Sm$Nuc#!d%obHTqUU7aRyE&IS&8{0{8|MM1 z%GI1)MD}r>a;A_aAj#jE;QZ>0b7neO*DhC#^Pcmcv#TrAxdlqLb}5~6oo$>~o$Z~s zoXg2TrxU)91D#i#7FP$5k?hKK6*;@RemaM{w!8H1k5JW~;qz@{-W_AS$qZNxleK6vBd+Z0yL ztYVI{wYbAfGj<01n)R@I*{4hw_6ggS+sOQ3QrSD~Z)Prgl5NR#h0hyUC%1?-at7`K zSB1OAWwRr>5^gFV!Y$w)a&zFil$*-Q_0&u zODjQEx>#Rw3;zjygfgLxAQv|YO~r|#Qo19I5l2e2?}s>FDwf9kc8S#`PC6=m5|>Nq zVlyc~IxSrn`$!?aGd^6pFS&fTeMRCjDaQBMSHqX(i;^BoZ>5307vgB?qqNbtP^>M@ zk*fGsh;5{WqDfjPi9W4#OWGi{@wE|ui*KbxzAa+3R4E>o-ivFcUXtDSpETUpNO~l7 z@KyN6OOd{zJ|a{h?U%Bot-je(9bb1TPdW%16D7`P^F4v@GT%mD%g|D3ov*Kt3_UB& z@?Dks`AVcsz7lbRbU~W#6Qyg?LusUMq-d2Qr2@$-J&~q}gj6Qpk!pw^MV)j=GK$AV zTskMU6U#&j>TzC7kmibRsZ=~ARTuAz7U{NhPV6hS0^;p-vAI-7d?Jn)L@^fp`d!#9 zUgEn7mBM228sAfxAVi89AxEeN?bTT*7EHn};hS(=Ea6WI=Y&*o7*7dF!f)X|-(Pse zCkQfOqtK9l&tK;|3*Wi<{A^wlCIi6V!e8cRaDn_1&dOhB$8!5wo_ov=><)G+8^qOy?~mDeTrM+~?ZNr+ zJlIyij^=)_d%64UFwV;raO+u)({bCm0B#RAnIrhlY!NGR7r8NPDch9$%!%Aij_20F z|Bd)vEXBR$2J`?S;SG%8F5+Ebkmwb2#XtNE;h~T$o(8?Cf=qnR z&lfEGGmsfA4B$WU1^hJO65mM>gcrhaJ`~3KATb^0hb}yTLizK;T)qkPwQs^9t_!~r z`qVovm2V05E#lhqyC7{4Zx&*>uUvhQ)q=aib>_49wVcFv=Ck>eTpdUY;IG5HR0X^; zpVtf3*tg)_7u-si6L-Lz=i*LqXV|{n7|zUJWBYKMxW@ed=Dt?o)#=d3j<9XHV{8KV z4chQH%$Z6y9m*f(ny`0au1#ZY>IN z3)tC=lfA(7W2-}78p0~LrpyVLpNFz$b|LEq$)W6S_J?;MbDF8n-uJd8Jb-L z{rWqz66WnO>@l$AAG04c_T(a=Z!6g?>}aNvna_M>w!w7xpSLB0vAfvX%pGPh`;M){ z++=<;%h}b8nvG>LV78ynhB0ShtW;r}d!KoOnB7cs?@jM#?@VSeQ_i5Unk<0%zYUC| z*(}Y?WCGY+@QI1N#BKq*eC%~r1KO@JUD;M3?-tvI%VdVLgTSKKOfnk)D@=d(2m2nz zu$7y|z4H!ZwlX!?2FyE#0MA4*cbSo3%NEwobzqJ&23S)zLe4L+#>BD@Suc$FD=_-6 zF#}kFTgClv%=~3XasAjFR^aw?X|NJe+(vFIw5yEU!D;#3d?~w)%i)&r4Y_w*e;7~G zIWP46m(UZ&Lz~~=szEWpUWr1O0*IB(;B{|P?HZ4?(w4p6Tg8E6K)EFVYNC3 z{cAVhUHHl`6?zNh!W_P-a8l?G^WPwt)AXVe)}z@%l(>^>axC2(LETNCMPwX$v5*opXS}M`v3-Kg;zaY+( z4v8_+cBz#wO|(Jzd}$oa>n`b&R8)KH%+E*xT^EssF66L$;+|^XeV@ct z(r;01YRieU}RkYc0~ zX`B=W`nvdh;yJMp*2H2lQCce1g6j}z8myDcz?M!@xztknBW-|s9+3L`E{GkadeUp@ zu{d3tAbEVh#nsXSah}9T+0t&YgH%(}`09zDA*Gt{j5t6-qz_R4@zN}jgU%7p#~wrSHNfXzwKHsxS;%e!MhHWT9R1#T(*OX@k&C{2=a=+~RfdEwsTt znA5h3VN#-~kdnm!X))+sDR5$*kS_KU3}UdjS!^i05`x5=qFFd0OcQ+Kd10bhTlgY` zi2Fq!%&CaDTihtbh#TNJr!~yOTZOLTF(FXQE!ardUtmnCWy3k1YDLmxI376q{=rrF!xGu~PAM;a%H9`l`Bs>tzP<|)W zsX5HPU-*N99-iAA!Uw^~KZU1&F!3XQ4xZEY3ofvB1K&naftEqM0mjT0p()Jfc34;1 z!CYJjVmmVPlj2ch~3H+atHX$Y-4T)$ME~v z8e9=OiF?g1;ikeIXovN4AKQqF<%(ck9>S$^!Tb%jIjk4^U`-go)n)&&5!_AgB0P83 zfVDXSo}lNlVcam7vEDO%+0ATic%J^nR7L%h zV0j)jhJHw|@&?goLE=QOi#|?Y1^XlDZ**t+7u}Qg^B$lQ=mB&oJ(g0_m%$ft@H_M^ zHH#+b6Yx9qBHe;Uz5C#|=6-r0ThJJ6N% zC`zDn>4n}j%1r;Gw$KLp8hxJ{M^B|2dmB(Xx|}*pbJQoQEnPy_qjISrdLE6@2k0En zED8ndPI(4UmDCyTUUEx_pU7|YB=R8ZPB~&>5z|)LcNY$c^ z)N!g8^^XdtGN~h;KGa?+g`VSSNNu48({gG%^_~K&DGi-Qx#?TfJo=mG7L@NpccGMY z1hiiy^_`-iKXjl%={#x@9YpV?^E`X0r_@rYS53MTZT3ci&D-dP-VyM95@n@}slD_e z_*H(DN~6Q6d(>xYI~_osqh?bQ{n|5~y5Z?hHKcA)Up-SO6ZMEn@ci-ksAtp+Pb4)4 zeyh)*TF?`yI<%6yNnP;_r#esu`k5yQ%G>A#p5|05DwxjnjHc#Mt>~|w%~Sw&f~p2u z(mXDTr*G2pJQ8S|Mep%+r*3*CQ!hOWs6+5M-?M`X_vCm~u*dM(y~HC>h15DvG-ZMB zMPQc)u7{wl8bB-C=`-|VPXyHzJfG;HC?57C#(Jt!{?sn2EmcNsgguW$@JWA~ zxaYQqq|Q){Jz>-<&l+mJr!_U+(}h|He-6(JPqt?+b;~`}Q|b9%-W#6xo?{f^Iqx~_ z?&ewUZtWTFHh4yZo-uB-r>)1I3W0qTKiCDi=$`NC=h0Ewo<&r9&s)zE=mTchXGwrM z&U82OT=q<&s<_{|AGs%Zp1Nmw^4)7aFWgCX}S@%JAAI}YU ztmm40E9h6$m;BA=Lx2cyX$#K&m~V|u!iuQ^fYkacmL<^=Sg>` zd2-wXJrmqsPY-tp*f`3Q==$h#x{KUzTpQdM-F-brz!uv5#GMW{gu9=+FT00(-nrJg zkHKEkG`AM~6XJR6n&)oq3UF7tPP&J=F!vAl49`thU-vn8e@{Jki9699Jo+^}=C)FX3maKCbo_q=g+a}RdEcUN~_Mvy6)!g5AN-**6t^+6nCWiv%A1G8UA1FndmaQ zw?X;ME(xwRJgEDydj;&VslkHLE<5N`c{aHs+>^lHQLfjnc$eVLg0@`dY6iYr?sm8e zT*KT|JWX6mx6XamJ<(O$eHT)K!Gk}Xr(7MOU)^(#aM|4Z+)CFy*9e!)?RI6lzBu=} z{M~8p%g&~*L#`ff6#l>AN^+;VaQ8*9cZ^Hn9^m@!iiI``bgy==bccJMx|X_IxQpF$ zT%7xnYqoo@tFwC@ozQx&i!O`njO((qu`3Sz)ZOXlN_WM%yE;Q$D%Wn;b!Vc>;X3K62X)SN zj(62?J#|IGZt5YBf0c}J#yj6S{hV8!Q=KBnI_-*do_8u-CtMrIu1=k6rR%t}zN^GJ z$;G?!Tng7RS98}Z7wX#L$^ac-os(TI&^Fqsab0qDbje(sT$7z)uJ6t*uIWy~)yehK z)!M0rvO8S$K~DqNPrd4lakX)Nb5;Ypm%;w+E9WT4>u?=&M!TvzKRD~cy#vk!*DrD} zd|T|gO^$^<*J#&yurDog^IfI?ooL9-&&R*eN*+oVUm{XFca#XLsj2XEN*(KY@B~fE>e}l&ioon(PEx zn~|T$4rDR;mYnL`4g15r{y*&lX@q^|GRGRSweyLyk@LD!=3MS92K!P$j>WmnDTn=R z2GR$S-^n#(duJl)a_%5&JCBjPvjuq{{=e@WK$6a9M+ z6qMQxJ~<2f)*s0s&TP0YCoyLjd6{fLo+OjNrV7VYaxLt0&u}2*UXpgsauDPQ*ekyV zd*xNhi{xOklq_%zBpW%;Ir}?`!P?*CXorlfOa4bjkyps}k%oqrtrNQS&aE^$zhegN{{B9A&eqzm${cUZ^(=SJ9TKTI}<@*5o@`O?vcbb{R{ z98s`4k2>c$^yEUYHNo)r&wPcWEGnCdj$2&06 z1N;5c9ab{Z@zk-~(TJ=Ad;hWE{{fEgjyq6~eES$jzN0PK$5HNxf|Lu6u4F&QH%Dvo zA$iw6$+5{%i=1G$JDNHw9IZflp?!~|7u@$bZaC^Wg30doQu`=JJ#wPGHk2Jep0P(c z7CUJ2AZW>l@>z~)5I}jgBMod^W3TC0;)o*!c$cvf-c<;WyN-5_KynJ$H^EVt{N&hA zZnAf9bcDPQ?9&|c96Y(m-W=@u}{EVXMLE_grF6!JZ@jkCA1SJ>y59Kq23NA2w$g#D^r?bzqI0Qa)&`!|+n3s6?WgP`9ItF?_B*zY_LsJG_B5NyzTFmTzi;hn zv)ixOaoc&@IC!Tu(iUtVVEbUZXN$M*wzjwB+UD3h+X^AiYWo|I8)V;YH`@=}m)q*w zo%Uz;>b5JkrtmJyV%u*kw=K0-v%R)$1i4>rBkVHU0h`Z$-+t8E!8X$N-KMr(x21rN zcDB#9AJ!|jiMD3;dhlM&33Yp7O|kX0DeS4X65Dp0V1Htp4*ol9`)Au=*Me*>=;Cd~ zAUn(+YAdjX*>Y@`p)FFa;kL=P>h>Sj4K}%bl|2yT{RTTs_H*_R)&yIrZL)o_wGGr` zpuNaC$2P)-+v9B-dwZ~Fr0tK*-?qv&3`*X$#zLNC`&Db2EzL%N?mISz?V!zPyK7r% zb=fA_%=UDU=Wjb>OSPi5_SRzSHfw#`RO=V(H|r=HX}w_egZG86EW@m#Z3m>=Z4IrN z@ILWBTe_vS)nWb5`i@AooUk;sMp`aeK3bMq2U@gNjb*E4yM?xXA%V zERC!ii3XNC#5Rl3a>x>4xo+8SX>WZ-EV0b72-Z0SW?5pXYJEziS*Ba8){mA6*1nc+ zmfx1e*8M~?OEb$W%OFB-iL<=6xItHfMQL4Q3AH{Vx?B9Mv#gCR*^qM3no4Lb4MA&7 z%Sq621!PB9r$bsN>pAOgqOm0v-lZxm>n(cAIg8q|!7`RmS#A;IEk))zL=#J%WeVZ6 z+_Fr!o&eirmcy13u&*_dLp&v>TEd8JP?HerUP823EUPU6md%!2qPHcV=x=#%oYPZ(ELx~_9pVfTXUVbLF^?uPiGG&O1W+Xr zV=M|{Cb7gEOdJF`cJqGoS)z?4-CU2zCR$jA5}$~>L`O?~@UoTIMWEoV*2HZBv+l4S zCYoA$TRvIxh&h&5&>o{Ln3V;cZp(AaJ@Yi;1u@8C0^9c!(Uy8d2C;%5EH-$X+#Brc z3h$N8#8IM%7-v}p^@_4&Sw0cdE!8alSu&vws#?xlMp#tV-^6V2MznPc(Hi`E*7A^8 z1U^W&+_ChuT8O|0CKQ&l=5S&H z5nvf>MhP=nEXq9m3OC~=V(1NRn#k6M_s%oD)Y@n#LN+gyuqns1u_ znU@f4iQmLxc;DZR_&}tZZ<)ss|A>5$vl`lDv$+!y4t>oBvR|2p5tE5pmORr!$kmhJ z%@56&&D{vne8k+#{KfpyG})YH4k7lNn-I55?ck)rU81G=K1e=5Oo8(S_uxE1g!zHF zi}|g&run*gpt-;-fQ@?dZnGb>f!(~-{KhGq)rn&6#kXVFZz9nr>bI+KlF@@XtkzGilBFrk3UprU~Xx zrhev);D@888s?qmF2p@ZPXt*@OrhqdrcUPHrcvherncq?^IdbC>Ah*R87JDCa!hs2 z*UYs|uT29?C8mF-xn`Rw-6VrrRv3qyewwGa1KBta-7~W_oFgH4iZTGOaXKGY6RFnA2~X!D2jaWK1iKqRC|3Y5Zc0H7zn2j8#o1OudXZjBQOBrUc_%<4of}V~H`^ zBs2DcduvT^4E>D_P4i4WjJJ&^jnz%z#zV#=<1b^8ah$1@F%QaJH|dSbj9REc2l&3y zXf&-c)-z2rhM2m-_ZZ`ANZDpuYfu|M8b_D{jAM;y#tX)F#yiG5Ltpq*%VdV~QKknb zxp9thD`@Qj_B=7PG5#>jFb;!rHsg%E>4KpPSRHG6U`Q|;AXiP}H)C7l8{;eEOp|Cl z4Dt$%PYolC*Ni<(O^uI?8Q_mt;}4_4I18j|O&<(BjJFKEjZye>{F@=w*vat1unp?J z%@A(vV7PAhXc%uS!226o8qXSo4U-JB4FSf__)Iu4Yc*Ck+=TP8H4SqOLB>h=7yJm6 z>5AWjPmSPwZ6CuYyoI4Teggl4CF457V1ooFW^IO(hUIYf)?!#`up71-p5dJhAMi1T z3M>v+8&(^BV*TNK?leO?+~05ruVTOrrws{s1>PO^GpxrLD8JnhjBkN+ycRfzTY|5^ zCBp{ztH37Vg;1_Kz7zk5rx^;cf%sg|+T2iNxCH*ZV;Ep$3`Y!-@rtp&;WBvZgYk-? zjS-0xz%Z0Ka1W(YRs8-^Rd;-d|T;KjH2dc$2qlra@fGEFp~#)Ag8u^b;~ zC^dA4`d>Gs8^Vme!FNp!S%w<$*Vs^Ca2swJcEcIt>d+^6!$v4I&o~da8oC;a!3(zy zv%p^`4ULSwAz!F*jd35;{Gfq1`WZ$W2EkdW061Iq79RlTs*W2j<1Cy?{$;3(pT-fx z4ns@0zQSV+2k_>GZ}?clBJhY4&K*Y?4jb~ZSo|xV2!7rHXSa6ZBt8{RJZIrEz?XY4 z3qA|3VbBHzco%~XPlEI;ye)nScjAlj|Jf(u>A1(R587ZD_^_4XJWd&A;$`>(TyA&@ z->=|K1A{NY2jVaAX4rLXF)kW*V>Y}S{uFPHAHcsuxfvkg16&i}eDn;=jGx2_!!S5w zjpOt2jhG1^3l_G(f8brQmslG-7ss(3(DEPfXe=8$44)#gvv87{!gpZx@%va;{3$jB z|EeF1VfZG!2Fa8kgfxo~y;_)DF6P!gmgL&}d;QL9~KWqfp z&=k7_5-o7{doq3k)8TFKyLe;l2I#(qyRkJ`1L#4Qv8K2m-XC9uY4C#>iAUlG@wRY= zygiTZv3!&p2MB#!}$+GB6A6`(I&Z^z29$#}kgI`$pwhX;e) zf7o2SC-xNUjXlDyfQ*m&#n?HlCH_}G4!ecb!f)xLv0vB}+={Kn0(O1R( z!+PRv_1E=xuta>Sz6#b6dx2HKu7RAF*hnl8|Dj)iT>xE)aI$|EAxI z{nSszR_b|dj6PR?THg_Ss!N1Z@vATlPNIL*kHsANE&2p~p8k}+HJqEjsAsUp*l@iT z>j1L*>jSWV`UBW={YcCM8dAXzJa!Sh&==pVcVKNGZKu8&oL+b0bM;E>Dah-KY4JSJ zIS}LUQr&93AGQT+s=uY*qmRO_f&Om#n|c9i@6sR9CqQkF>uX@$vG>>={QzvD-inpz zmt$@9uk|gl0&JN6ojwTL3h9OVZF&*gqi>D{V;eCCwD}lvId-SqE4zX#i{uaCv+4H%1U!+z=}=&yp#7=4lcvwjTL zKz~%9t#5%f0b2`o?ewelHf*I%(l68_Sb;8HABH{0f*>_j|5e{Ye@_2Q*9*??&&6)( zqV!YsA=q8r5IE6~V_)F9N7q7+=r`++>#BlZy%?@v24^b!U>dki)oZY7V3%7z13XX@ zi_ZMIw&q@N3I$?A6Nh66U?AlNLxoeJGd_*`8drN5zHs?&ike=J8=Q$Jo` ztXD(dq4l%$ar&qFOkHRFMDS4+-A<4+Rlizi(TD2i=>G$sy#$LcfV@BGB9PANOVJH* z(lkWB7DaXQbXI-1?v*Y`w-D|{=xw^yy3e|P`nPBwIFUM3Uqg3Rw@WAJ7wRZ|5!zR` zR>$a9p>ohMLcam6t4r0P`j6;DT{qo7K!ND>Cde@ys23Zf=g>G^w$6htLR*5Q3up__ zF;%x2u90x>Gde_92YrYR(*^3+p-prX!QTC7M_oDCHWXx5MbDsXbz%ApG(k67SE9R& zHr9D{J9IVCi|7FKG`bAc>mDE?S_jfPpqZ#uH(1vT%G^L}=|-V4T^jlx-2tgQx*FY! zinsJg5Dy?(%FtT~i6-eD=o+CH(2igmfzCoX z&~z7-bWU^xT3feQ7mlt&!$IavL_!Cn-_b#6F`zz zdyF(if1_h`FOUZ4Ewq)c7@3I9fm-wfUj?9DLAzXc0jY-`Lv_0UklN@x)U5jkNTU1D zw}zl^(eCJL^f*fBx*>;=aC8;=3Upsb8$v&tjxGkDjDwYK^fT73w(S8@q(9| z=u(mI$Q1M$dI7urq^g8(h2>9v_TDE%Ry+jVW^*uhV$Gsx(l6y z1cAPv=xL+|nuRn$pCCQaiAW)Mst?%b25(`y5@>S*#=}aa1S|!uNPX0bEI}x!Sv6!m z(g$j4L8c+qpkJ&&{Lvz$IeHnXf}Ybx0P@O)I-oViKwqzm>_uv!CqP#lbRXCp2rWq? zr;)~B>vE(V`HFVZMI&F3&&W7*2NH;0LA219c=Wrr2lSR|XdJTp|E+zd?E>e~A0QUc zZA8{UYo?$hw7<1oq2IjIwnKV=yvE2bcKD8o93FAs2}()>dkhwZ+=)P=|{er}i4Ol`9I{f zwignL97X18pJ@%qAS6dqPn)f6jC9w2*7gFdO*idLZ7ASyWjDY)G|jXpwM~!? zn$wzA+GE;C%{k3+Z8PK&X+(fP`*grL3322)V9+c(y*F2nkkw$np^6sn!B1d zT0yfylc2o_IHX5VBae13)Uy!k;n8l^=EMCZS|zkq18raJ8SQrPMsMvk?MO|QX1Hd* zW{@UVvrYY1ouO%~4OcG)jFeZa(#+C~)ZEj2R3~dt%}mV!jigpHkP- z+*dUQWR6~QN##?|R9C8}sVk)?dGWy0Y43+kfu*`KUC%bY=Ov@^yYWe(i!s2lof0=odv7 zr2?>V1;J#94$!y1fou1fKNpY$EWV+^M{cPiaY`Y+7D&CDph$v86)2(-vr2<@3L^fc6pRdH5)W}fLhxH{pgaC0?g_o z)dr!>!WnkbhkPlOm`)8)71w-uOj1Jv6Uv@~d4;J3iNvUjrUfKmIYq*XJN`AVyL4xsdY zsGDoXXyl5z3M!~e&{g1d`5jm@=men8)bgJ4AbB(SQrSaUFZE{iDg;HF{SV6pc&b^! zH-m2~t*R~RKkA#xNL6#iX2o#muNz@LQYvBdcAI@uD)ue>aVOBphb7fuL1hB zllFx6f1`eu=BuVc6{(JgvEN$zAIw?pkt|p*H0tr{1FBD|&B`L>V!$T01`KP8B1ho| z_O}8&+iI8{#%Nz?GvV2yAN2P>vf=W(A-_Y0Do-oT$VQ|9=FENS78*pEq-+fH!bok7 zwl=aI@v3I3b^yi{1&rs1;P8-c%2UeznoP}BtplkKbIny%xVnvEzhZ^FOuirdk)iad zf|b3L7lH#rItT3yvH?foTt$UqrM47i{t<{8#&F}{UBP5PT7V`vJQ&wZ*3?E1p)Fy} z`KIck&HyxYjPi_f4q&xvs*b331C|d7jsX_L%fZuSA7z`B!77bngkrkj9o_4J@yLGgfL>#01iyrFhzx+y;^0~Kk2iK`=j9#TVgMb<$6QPxy` zR*9-=tG1}FYDDcVm>a&zqZETfazi!;R|NM|TvY6U`!TA6s&;@)8y0duq*&fo@lqY5 z*{PKw-2vm5sbbXgR0XP|fQ~#H>;4MEYW)2bb4fbIg!Ve1q>6k9_6h5XO9 z*YY}wJ%Dd~1Y@NZV0+``r=e}v%X5^iRcinX_zIAOv~s1gUɍ)X7ft-z(-0Z_L+ z)qB)MXbs&cKr0t28>sXl@gX$=cLu%+v;{2-dKc6c5R?l6TUlGNTd_lKQk<9T6oceh z^2W-gO1*lxdOy_XePu1bo@JBD4nYjos>O7%2Vxq9ehmA#s%UD_MBsGz`KRJfM!=td zbih=$l0T47@_*sq3nC=U`8EGnx1XJU29>kr1^%Xhr;1SJTfj+=)m+uAR{T{o3``HS z0vFu73d`TY6%V{y1=`n*)Ob;sfRb&r#*ST9rR4BLA-V zd-ZS5zaRhRLv(_E!Ha{#Wg}%qRW}t4$k(;-43-LOO_{1c;ILn-8Y}lIV-*(^!O#b$ z1QrG^^?&B?1eU0gfu{n$264fAgZ~Dv2znbN1dI(h01;_!`rANblfdGDmVtXAK0sJS zuZmv4+1C!@NZKkoR*e0-?(YkTp76=v5wJF}BCvhn{=kZW_`vD@%76|3O8@2m4f)se z?~=bdzrGNSV5wiZe}jOG%1(X*D~?wTFFRQl1)N@mf4}^t|26v879vg#E#F+e{okQ~ zzx=HJtAOjC{oDQTpE3`yl`brQ{rk`F?GXKB8bl-N0*rXq{rv-ufzFK(Bf$qTEyX`k ze{54c0RJ^Ek`W*re(04(Nf{*ws0t){$FIx}12E{+HpYuzv zmHz(YDXRl9b#fuH(Z1A|?4!|k?u|og13&dsdudq}&D%=$Tz^R%9 z@hd}s!?no&H?aTZ`iBJ83cLvX_V;0q+Tfq*pYiVt#6LLmkM!^C-x%b~2)Y>bKfd?Z zeinZ}h<n zpH*2}xuEhCaCp9}>>9Wz&;nfZ`vda>xwh&k7hHbT()U z)cFc9(--@*0d;^geH8GKzX7e6fTs*t(#ksgj{S{7ES=xw+6t3j9lsv_bN#>n3$HZ( zW&W-O_Q5_-gCTw!{3ZfVdqe+e{zLuF`&IsvD!stdnO|lo?_IH};?rN`A6gk!sr$!5 z9MJle+y9+{nzNNFAzsl2V5IF|F|lHN*@m+F&~8z{e{;3;dg)o<(wYl>XfniT3I%Rn zrc@{m`Be`%tj3qFDcxCm8{&4J0A|`!h~gPrJ{Y2k5EbVkI??*l(-7rnHpHtk|CRy& zEC=y;(!XB-&Sl^L`g#9nO9k zE#&(Z;ISF}HSX)QFQb4*dim#@pZ0$WD4`)HXV=dK-;053X5Ei+U=LafoY!Z7&#>yZ z2H&=RIr*hOaIaK>qXr>`Pd}_EOe?%tbg`%r#2#A#(d23t*Diio^tMO~@p>tUEBC7C zMbW9EqeV?1mgKtkec$hSH}YN24-&+)fAoG5#HnlW-td0ohrS;!K@{64h|L&Ppek6I zKR7?*&Dl4nA^y&%_gCLHdc(hY_IknV*>CDVWb>3aGhfBOYMw6wKSE-`ao_}0Lp z&-Xm<_`KH>|0lt@=W<$Sf6lVyUd}oCbi&i8dF%6bz3A{_KCmXFJ+J-T_3ZZ3h`i6w zXFg+|HO>8)vm>iTR-^3OSx28VebVMhVQyv4{+yhQxfu^L<1#JTd$X=&$7BmxS2Ooy zG|!k0u@g&k7Und6@;UcJR#aB!%)1#Av*cMZ|BC^cb0J%qvHfw;!=#7V54t^Am|33j zD|=A(@9a6*o3ad9M>A?=Jj>{wap*Dn_~+wJkI6^V9x{&>JVYK1eYoba{_)GlQI7{c z{QLlayyVf!Ohu+&&Zg|8k83=x`(W+;G53GooA|K!!O~29W>{7Z{N2yYeLUdtsLW3p z)3VfAXvXr#2k!~@kUQgU&wTLgzWec-M{6>?8Q&j8JyK>Ydb}87K-S25lv$8DIkQUE zu}uHmIXRuPbF-RcJb(Q3e&74c?#u5#dyx8IK*rn0$8wuMyvxZ?3{P(7j>&Ro8j^$Ncyh1hbkDBHiUIx6P&O{}ea4#1kj$1K%btBQ3(Z=TStUCsOP;$Vr{`1K z(`HZ5C;cFfW>StKryzS$_VgT6PT!~RpD13O%v+dOFYn#6EzeqImu4->)@3I=rJi2M z8|bNO>c{*4!HUZO8=zFhgTPM#$%|LLNqN?^4}%rAdYUa+^IN&dGNC-YT5ce&1J>FNeLU`DQ6_BJ_UK@=g0Uwcpfyi@Y5RaohF4 zMRDMDzt`!&=TZA@#hc@=hQ0E=QNI!4{@-`G5OrzwThsgOcQS~Vc)Vy`Q4QcAnEas` zMDP9qu?auC>s;t6M1XDNKj5nWSa_>&9>fc*2HYaiAF6!V330%a3Yo$~MJtN>7xyeS z6&Dq~`1t5!-@@jF_lvg`FDXeWY4N4)muJ9#5(?a*xu5esKl}Xl^SF}PB}Rz9`l|SR zv9aXyr?4+5L@WRIx!O1J+q-Y?zt#H|@lE!*sN`GG{h}_gHs3EkS-kpl%4ZeC*6j*B zaeGQelx+Vz;q%cibH7BFG%H~~Ss^a>%HlSkrO(s8#D2*D_PVd%UVl@3*L|M>@th;S z)&eHC=&$pD`>iK1(e3@b`m^C{;g|ZL=_+u9?f8)nG3vJe42KAGhkl&=5&cd2R$6kc zWbK#fU&wEM-;#jEYCiD)H3FX1IzL+id)Jzubs(lv+^-f8<^ANZ%Rit0tOYz?b%0a1 zAIxsk|MdG~Di0}7hM33yL2R(~Wyi{Tz*?(fAjnf`XyE# zsC*0T;?Mko{5?UvgO0*8%r$?%fNqteDjPzq~?W*3jM)R7JYfY~`zRuvf z#`;wo7#c=4YT3A36SiTmM*4=XM$rwU8`WtV*Q{5|(XFmTJ!$i@Ez`bXyFu;M?K^i^ z+BUacLfd)m*0#CV_H4WN?ISx(kDk*pr_;1fcREv@#&$03?1>%K`A7^F+b7N%8y{Dt z>&R}4y4!o!>vf@bgWlPF_}-WLl=V*PJGS?eK1X`n`ug>n+Pg~c?S1a{?$Y;QzZL^l z4`>#@eqi+kO~ST8o#P7=)+U4ww#45_7!m(7;rD>_c)$3y36=3126Y-NPr9EpDW!KZ zlX^X6bXvb;PpUKdV@m1Z!%1@zYbS3=?4CR{NuQFCTt77|`BBR0l(e*P`DNq;IM)WJX%Yp&e53A*$5Ow1vsFQu`-iDP5CHDVd4$k~PV< zQzj*~OzDwaH8nKpNAi)Rgp?IYjZ?A{XD7=N&m=t`6rIQ=Jx_j`JUMkoT92Wl(qe{= zNNYDVI4x_)!jy=#uPLk3s-%2OElE0+vUbp*MD5_ciSrU&$*Sa)DNB-ir3@UbOsX=d zVDR#R)e@==d>((jf1iQf2HuPR8NYwfvxG&19fJx7ZyQiE{!zd21GqkUeOL8q(643R zPyN31TQH#BfTDpT`?o-|u?|HSCqUZWvLweYHDSFK8Ikktb*R$?} zdM@wb>$RwxqQ{MHQjegni@WXUV(eBP*QaZnF0Z?mby?NzT5MdG332{ihsPGjy^8JD zWqeF#Y-G%e*!-BdxH+*UaWCU~be$0MH+ENNH1pvEzeI zBRdv$TG_Fv6V`D^r~GKX)0s|0%+ro-J2&h&zEisnd!nN{tcmW^Aw4>w{pJo!+CFHf zY16!|sx93vtX*{bCGCTv>$F?l{!Cj<`z>w1x4YU#)lS)FXxlxl-$d1n8qy}d)%MoU zS`kqLTb^juqUE4g5iOHjU26Wf#k*#|o3C%WyVJ;drb*3i zHO*^=H*MamtVvk2{f(Y9b~ftN*jGQM0bVbx{^7bu>m9Fmy?#)`;6{ZFG>s-T+|X!Q zy{+~C)caCDzV76DSe=l%k7^ax_NyIGXG84+b*!~()mc;PV(qDsi)$>3OsH|C`jyDq zHLBJ;7kQ#aVdV1~h8iU`=0;AdF}Y@PEup5e*87@QYE?$Qs4+4!t;W>qX^~0QnpdA) zZCdpcRo7PgQ2lviaP@c9+eg}KFxAbG#noF-QD}SP?O^iXvPY!AB^nB!t(9Xc1wr@+f?N#QN~z z5fj6EM8t*>;j-`x;fumLho^>jidY<$6n;Kze0XwbQ#io)GIU93r?54lBg0(bAHw&B z^$K4To)FP1R1uaQni6)+cRF-~FFJIzuWM*Sz%5>qruhcJ{8n4?^Ysx?X_eH>7b^w( z&Pk(ucYF&%U-)*0)(S(ztA&<@_6e;LruCKh6rm48=Y@6*bBE=J1&6&1tMFxnYD0g7 zZjpxhN_`hXze^8&t9%KeuY9{h>x7bFexaG6&wTqrr~0~t`b%e|QGm@~4Y@RRpNWV&o2Z_eL6o^ zhydJP7Q2?)&JO1o_5q-3R{;Xn!5`z=@L##f{93jqd~eUcVAlX*6%jH4gRK_s2>k&o zI8vx1&f(h%huL_pIs1j3!PH?Ja2mb~;JXe0ek_PR%I;+6azWf}u8h6NedOlx&A6xB zM8MNUf_+M`<~CrldBEly0UsDEI7O>iDJlTVZGt!fdjPw?6XF8&5(U6b&INqxAi%PI z5e2}ZMgR&?CEgVm@lnDiAyJ$lv=Wm=URnnD)}eqsGzp)C6Z|T`d9M}+16DN%u%Fcd z=Q+~XB6Nn-(pN=7eK!HG`Ba=HjTf6r{h{5SLHvRZfM>iY&XQV)8o-Yx0-p4Q^boML zOTmsD;g}c?w%CRD!Vlq$Siv6nrwE2+zga!cwt?pc2!B|Lf>Hz?-(-E-uO2mSj1Lk|IJId!!9SUb4oOzQgzA0Y6sktLs?0SNFU>oztPF z6@4Fpeom~}6Zc)tu9*X$hmsomY7VN|@AD>j=NN3ygRjHa$(3r@a1UY>?nmoeyM^*>esJ>NNoSImCG|)im{c!$Is6Ess=veiZkdT};^V}EsyWrNt8&#&R$W+aB<@br zs-COX1fC+ptJ$x!=Ec0-D9nBXTa(X0sWd4Q#rO%v=m(~=15FT z%z>Ev*iLcP;VLr(O!7Z5+hUuD-WJbkj6$`+D=7Bq%RxvF4 zWQCR$<15^(@JqP{;LfI&pBUvXR}s8aM%3k~u2IjUI)PBC7PUQU6dqH+xO@qAsZQix zutCGY5v`4~M9qpC0Xk|xB!fJ&V2^~?}OA(^jS?r&#FW3ssY0lrAv96BLQC>Q# zIxjj?Tw(Cudg*BFY;E6WU+qvF>)>&f?}&4Da~^T_g`d|d_*%7LkFq-dm3xc3obWH| z(iczI2p+^C|N>^AmGpW;Ig|o|76B zh)B8v-H;i^JfIc2jHzuZVm>gP%nQv~=JQY(T3U8PGiq+$ZjQA~v2f^j&cju5k1PZBTr z+~{v)AU|Y{alp6OonlNNTyhSI>rz=q!D=ewzfzPV8rn_Oq4_kIE5b5vgjV z8fW2&+05t(CuXm4#&}>9QKi%;570TxVWuII$?Qj_`X=TglVYlBdIzn1JiML{(S**V z7Q)ZDfLZ{ZZnu6||4eV8$3oNE2rcU}c|;13q4HEqAWqVRELMM0^N~GHsR?jzo~;hk z7HAP#Yb`==qCdrTeG0rxpXd(5O0}i7L+86?=*ANCW`aH+St@glM(~F2ZcH^!LO1(~ z+#{#7r`k$wz4id>gK(`ryj1V0dzAuZllr?FuK4BS%2j0-T(CLCqCAm(a-n=e4l0T= zNbR9sBzH**(vxVUDZEyflXsd|OGA>%0kur6qkW-`RdbLLLsUgkm7VHAwYAbvxi8<8 z*M?Sws!5Hd7ePwu9%vJ&23PLO$cXL<5A-L2SAq7y=D|hCzShA44Q`RW6e6hmMAh$YVNeQL}v!!{G z9oflgQWBh{J4)N39UhdnNO^D$-x%D09MkrJ9|9W!uYE6l5BfWyJitp3*^Zr{% zH0|O~Mz;HZ{;+@*soBxU@S21i)E7uhv0-i5#W&Jd9uB>2kY&0O4)ZR*)889eGW(H^ zecM;+TZMeRZT?;U5})Mj85kd!k1X1(z^_Q3eeeGXb*xVK%h-aIk*jnWZm zF;a{Rr4#UN4rL`=ahJ*in1Jj?w_>x zWTl;Q9Io0;hOfpW( z(TQw`{<8#QD3BQgXix&xF*LM^Wo3i7{1EajFSdU5rZ{& zV<0t^Y6S1#|54kh`^FtOCubN{;8p)0#>p&WrZG>S318p^ct2-E!-+V|sA_Oj7?n$1 zqb9@WK9e3x-+(%?mEKB!K{cibe3e(iKmQ#SpqA3V(+yFtUBYZ&I?)3#lHP`o{#~jS z9F`x`7RCc_eG|>lgQ#?>E;R7b@HPI0xzY0vdUs!ag8ovkY9zpq|0+?*Y5l4G8e?-ieLChRiM-V#jTQO@y_>PjDAdow6ZvcX zpnhLJN&@62st+*4BQ`x%f1uH13Ry^olj(50c9R6shNNg+v{{&af?5Q*N+R{9q!*m6 zFOf7dl)T3nejM+=j_+|S*-jp5A+3QvUhhc;;fVN3?}r)X0ruh1Tk6}%IWke7t#=_k zQ0?fT4!$* zQU(0fPzS5Gpxd`sN2?~BUtQG+>V3tlIMe{r$Ijx6`dn#++RQ#=S+7517B}e^5 zwJ8axL=-E7l#w{^S}A7CisO{|$}6Qr8LVU}cN9^Xi~NQdrH0ZQnO!I4pD`*dMG8T# z+)>F;>f=1-lo;hhs5rC^+fwA(@+tX%ybyT@Utvty8oCs6gu;-7b}Q5x*#YM8Pf-BrCEU<1luf3|+`DKpTx~#2XAlhOmUpp$=k;(mBowSVt(5y{w{VFn~ACz z6thty>4C(%^JT}%vXLL=6s59hVqbBwI9V(%dr`JcJS|3wHN|P-0&x#A>KBL;M1NVJ zY`?f#d@X(u$A~?}VcyQ(iD>7&Sd2uKcV*>~tTF>BD<6=u_ZnMAiG##tNapS@b`>Y0 zhozz&&h@LkUbxv$LVHi~8EPQ2cD=Y3ncGLp=85w}r7Rr2_|L^m?;`I|ak|KRBfOPF zx4221FS6nX^lO9o3T^L#i~d;eLT|J;8L4I$y-!8KYsUON+q=wr1CIEAc&{Q6e2q5& z8GW2L#+!}wvc1R&&+?7+?Lv)6_Qv~K`o{a7dEa}#_b>5}_s#Q7_OI|SM*8hJ-!osK z?*Vcbs{3#I|MFMxH}H2wnn@kxmgixf?gxft6|zhILxx}LK;r-vbObj9wgf22Db+;+ z1uHRY|^)DOq#N2x?A2;C07mH5ze zTr+jUaeY9(A?L{laV8YVsvM#0#`6N@wi2s!#FgI#g;L5Zj})RD!?_o&wnZham9`CY z*k@W_tv{|g$D?*PM?I?+Xpgk3nuC0(LW?YKS5;j8Ai2#F+OGM-|FM^ zJ$hxmtsd0t8NKyo`YhB%gL)!7_a7Pe4HuP4^+8TbK9!BJvoa@1k}K5bF8_$ z`J&lm2{UJy*IS-ju9)qXVV0d1+S1(;Vd-P}+rnFYsCV~-WA|%I6*y|YfXDV6YliKh zO|T8M9kBYXhiz87VeJan+zU3&+S57${@(*_hv4M<)|Ox&ZeL<6vhB8WjxX)|?Nc0Q z9k=Ysjt7qN&X4x~j&#Rv$05f@hs!b2G1__BndVrGmEj2IaBT18{1I!z{m$>5Pn~IC zaW=tCdj&Wur>l>v1H6pCL7o1tt2X-{EC|PTWQVb5;d8vgHI+TW4#4`b)b%-g$Q8@} z&bqjLV88NRpRqMS-lX8Y`?yN{QuZ0Ukb4Z)D}#%J&-I^dGWR1(b1CqzzQ;BNP4|-T z&K>6Jf&MGz7V>Sd{``we<IToyGYtMO4EU4#zXHeKfu*!2iC7w;f2hhul@%v3eijXIq5|TV!g=2yt zO!r&@L%c~y@H}=86&?uPJwt^Op|fyR_*3ZbsSTpJxu+1kvCEU_+36wf)xvzi0;c&- zp(#k_$>5x0g^hwHQa{+906ZDq$ zL<%#6S$O>0JxS<>cE*B583~s6n9#t}0c3Mu&l^uie7YWC`C2qsL25nw;w*=)|-_s5x=~u#e z!Q`IoUXQm248v;+Qk*{kNxmnK>b``1UUWYM!&(Y1w;{iokHU8u#m@s7ddR(-m-!(0 zs*d25O7MHZJ>R{K=iD9LMeaG^JVX3qcQ25lf8smM#xdf={)7Bj_X}-*#8=1v;pp#nwgOifEM+~AnRB_P+$8P> zmk&1a6^{G9>;blzYtJ8JD{)uAsp{-B@R*6bAICe*jpE*MKk=7vUZf)5VHH=NxAJ{> z6F-+XxY=MVQ}`KRzPj^8{A8{aL}xd?jBUa#1S>Y2d&t?j@tmE{=9A$h&+=3F&Y;xF zBP-%Kn91jGlCRDG!fyp1mcsAkeQaNFVj}+vq^W^_W5Bh>gW1}`ar`Zuizo2eUcynn zD?I13*aA?QeYrGl2X~iEL<>{+O>AYj)PKo`*dbg7n9p`x85d$_;5_ciHD(X8kHK~R zfH9yZXwVt#WwshOi)#h`v?BU+nXAsP0i)TRJIehAma_`CojbxtaV;>O5Oy?|&i;>m zfzQRVqw)UPoQWOF)@7GtyxGMzfG_`dtce@tI^x>E2H8ulI!JTq!2RKB$drGQbD>^WUo8hx{5!kU+@e+;Igtio$sM& zZH5L_=1O!;cP)jFdoO36^R2U=>y4v zgpdAA@QOX5!1*wij&YoHtVT9LvAsTsrf}y@y9F(*b5?MS1#xD8Qag;)kNVEnj;79w z_6m;Wjt`FS?QiXO9r4a*;KzD9j)HhgfroyHqZqo`TYD!*v~#jE$MGC4@HOBK|Hx73 zNObmfTy(s$PjDn56JY>2v-XY?j+J)GvCqLfJ7FIq9bUM_|LVxGm!c>6j;>%O@7Y^A zjv=ig+g@xRX)m&mvp=zSLN3K+Xm~g5@s3pc5s+mw9Hq9FaOXb=m-g*;kG+LG)BcBj z40yW<_H*_E8?l|RGmg2oD@ZkgyS#0M?W6Sz+doLUNQTEeu?@CwgQNT#TUW4nE#T^3 z8T_cr&R~{m56AdVtXu~;&zB%IBFFZ`wiZz-xz;kPVx5M}il3}wtjDZ7Erw+@yyk0L zzp-XR|C@#Eino?p$W$3@Jz+tVyLmV9$Gn6!7MC+51AO6ET1k(PayOz4aA%_8(j+5F5@$J`qFp_l1w>TWJH zH#2=>x@7X3?m&;MWm*ins*-t|NiofWMt2=duG6&Kl*^PbNv64`M$97SB=d&JVg6v6 z;rRscZUU3WykKgW!kM8=xGBpt7@TY)(|FSuWX_Cajxxuf@2#Ux)03FZ%p`h0y@ol? zEWmY2C1xDcgPFq|rR_`)tW|%*I&&s-i%CMBk6;>Y%B1(uJ+P;4$o;v4*BDrtZUF&3B=@jr{sZ=ZK9Fl@Ep(w^W{S zdM))7>gO{mo4!aFQW01i_oAyI^~XfFr~fi|Dh_Mi0&sKJ(dJ65qi0gvu>vomb{JQT zE?~o&VYNG(dPP-(wmJ@Z&8nVmoB=oZ7`#?5JqPK{jf`UOdM2Z$@tTy=OZ8}MU#*Ah z&GdX|2mg>LD3JNOOHbCv>bd$lQb>;KPxNUZy~2W)%Ly{9Cp@pv{EbRdoE+ab_zIOrgwaDBT_y`+Yz zL)4l|d#FX%l=1QwIUSnPL-~>HR_Z8IkY2SJ*?1RaS}{URa1Gr=sUmlgiQH6KBJYvI Vv0|mMjxCTc%YVx6WRILE{~xn&YM%fA literal 0 HcmV?d00001 diff --git a/FtcRobotController/src/main/res/raw/silver.wav b/FtcRobotController/src/main/res/raw/silver.wav new file mode 100644 index 0000000000000000000000000000000000000000..25918a72ae51c412a9a6268279b449fd19639272 GIT binary patch literal 99068 zcmW(-1DG6H*DkB8syVhY@x=CSY;Lr%y}`z|t&MHlwvCz0WX$fKo_3f1^Zn00vy)9# zU7UOPp7(CIwyj#Z7NN*r&3m;RF@9>S0YMNH!H@;F;14zoArJ#HZ1B{<@8O;A@;3RQ zI!^7NBq-OFt;z*urt(ZVs{B%3E9aHb${KZsIzs8J^iV6RLzKEoyy{f;D>D>Ld8>q| zkCm26h;mU`48JQViK<>*2=7)^S#`G3RGFg2s*-YAaVxi#aw?`iQ!Xn<)Ol(Zm}{9j zR=o%F)l}ounaXelRldp<6iRs_?~}VLjg@xtpYm~Ky7E>&B9DeK8O159vRxjaBq=%a zR(XZeQ8_D5mm?IboGcsR*&X@4d{3SrSC<>YX!Ybod5}_G87jAsJ1SL_P4X)FzI;^9 zk}t!Xn`F0~BmW^M$X{W{yWy(^FkWw^0qmo?G8lf}mzT=@lxoTed8WKgSqa~-mPw^V zz9H|FKgqk~)3Ek`j_-Uv39;35rc_s)Q>s@KaAwB<+yQN}0R^-uWOul4r{!@7 zyjxzWbXQ!mOa3a~l5fg)Wu0PC&M7CANcbm2Emf|{hvh@czaXnzxvf%BsiYW{SFrEd zN)K2^6_8^c`p10V6PWslbWrpl^4RdGv$Bb z-6isHIQK*H3VDiLSK*Z!at+Xrzm)B;gTwMXc?PVnhTKN3qUe>&@+`RntniV1K(40H z%2Rnc$Up{Nc`F~656T;q*)Ycgc_ipc0qpP}I3H5^4d*maIi&1RGUONXZekMN_FxtX*$TFfoza7K=+E|+aQh3AXkrEP4-I!oWlsYl3XC4mV3kRz492? z$pZN=c)APr+zxEY13J+f*8U65Xr0^?M(GLjFO&b4LqHy#l!2@g zXXIxfGZOTxqU@GdDkBu5Tq5Pd-p0rlxsMzt*M*;CS&}lqE@Naw=HR7Eo0GN>&p z(ofIykr$E`N~@ zOH)ASPe}8n_3}WumK-Ec0$V%@=k1anNo!%Jt)&!c0jy-b)KuCn_XRt+EHUy6iIzV| zInovBH28^h>74WowCAWaQyLFaW93Y#5zHGRQ}Cyq(lt1nC(;V(j67LhC5@Hlf$nEZ z-(cK^@*KGnXjvVQ_kFo0oIz*M0~OZypFBvu0^cr>Tgm<9SUE?!ApHZ9Op}gEzd;9% zNE4)fN}O^M{KH*1<4rK{EqQ}1f%jShr-FlpcLA+k4gTPNKCr2h0yc9HeA8t(QxU#f zFP8_u^!k5mQ+C6%!yu7cV5ObF>RZCfYQQQJz{XF*zw@PDvM8UICQ6@RU8CeM@Bl~U z#vtQ0VDpFIZ-&biorE)bC6`GjKyE$dW^kUt@;B)Q=-*9gy7WX| zDL<7~OIA1!t8_%VDUX+Xz#Ml#_8-8%Uyvt)eYs_abWB>TOj8cRuBs>o(4ju?bfP>@ zX{Gc5n?D5ap9Z-%0k0PX+WZmxLOAGaIoSUlm?Km!mf~Ptwc%+O@S_t!*4M#%((-rd z9QeJx;FterOR9`1-{Cxef-l45Pf{NxS?MGT@+t5U`SMfH)P?dFI5zs(iLXe zDffa1Fcc(T2~NMROv)LcyL&)RbEPXF-(=8F9r&FHh(<#oUaf(+IS@2`xzYvXzY=~A z2R}GP{s?E%5Y93WPUn=eRAIs93c<%cgBjK+L%>=O%ZzeQegwX86?lnyYJ_?gJlA4n ztnwW^u~B`e=+#`M0{Hhz;77Z_2n`_yuUCdD_0>>yB>1av)uBvQ{{WA(SJ|YlR+lKF zl#A*twFS%@rrMMu#-|$@>^$0|{-f9aq5@gD158oHC>x!|_liSJw_wnJO4-P2BJ^|VlJsM=Un)FSmyHAsy@ ztT2vU^=SyQTAQcs)_Q4;kQii?)=BG*#35Z_9uCRXSgk<)s%_TVXvx}9ZI`wI*@TFY zR}~={$an3smWw!$W=Koqwzfihr(M%7Y3sDz$QER_HdQ-?96_dP1GH$wt7X8GwJ;9~ zU+>h`Yrl{W$PZWPFrnXOL)=M+d``*T{Y3J^CD#koU+$v>7@Pt&ZM7Z=z20C;BH^11&}`qOH*?=v8bR zb`;%)au|zs$Le7S_7JU%#bHK_#+t%UbNKEv`W}6OTt_aW%TWUJqa(3CSUy&S^~L_e z>f>Sfacn#`8SjsGz-nMKu(sG;Yz_7XJBxM0TjCe76WD3|A-)-#jcvx);Md{5S=^1S zz-GZ4o3PpV0{jBD8)J!VJO+=)SL0joins+Y#9!be@L6~_ya9e4KZ?JHx3A#K@fX-F zYz*E956AsjF8%<22~Y3ir*Q-E3pe0dSTIqDPsLl|3SI)^5O^wn9^VFIp2BzH>xk(@ zFd-6t!cJVncjHdtH*u3VM+_s|keA3~q>jubrVvAjqcC0)NszI9&Y*@+zsXyqgM3YTh(h8Pd4x zfeNCcDJQ9r`>B)ENNPBBnqEQAqNY&K=?8RmCXP8vAEM__Q>pLdHL@Swfqq3_rvuDQ zrZJsB$1p+6W_mrHMyJ!`=#{jE*3--3`xVrF>I}1k*+Czn1GJej(q_7Vw$Uq?!^}}; z3xl#D>;U>7dIB?%nG9d_XGgQY8J2y`Ubs>-OU=f zC~hXVltb7`tc~$8t6;2U%o>Jgo3dM(t&D-I%$2Y_H;voNEnpVG2;-TH%n{}xdxhQ4 z-Q*^*lh|(T95$LsVhk|GI_4}>pKZftGr7!H_BVTkd%-o*{iRFeK691XPV6D(8gq)? zLLXpvu(`}frXAOa+sSR?j&mnDg)L*RGH)3(8(?fqI+M?$+)d^MvzC3s-iK$yxT)Mm z?ihE3y~Dm|9c&+F2IFBu*$vE2W*c{#Tf!aWCUWz+RJMd|&h}+Dz?wxih&{-jWy@f7 zFS+krJKbR2KyC&%h#kkKu&vn>+;uLG^TSSFaf{g#?06XE26vcS%&mri9QGtLk4>8~5FJHXxH2J5=$KCne>Gp;|E&t-8_ zbbsrT`SScu?gF<~w_mqMcT-0QDZ)r$z7Q)^7aH@U`B1Jd_XOng6V}&S7%rUTU-K>= z6}IpP_{w}OzCPcF_j6|5T7D}(kzc?QVtH}0uvu6woDt^pYxoSVn0u}Jq>JJ!@8U9p-t zU)UE#T?OKXk&ON7KkH-8A45=y^z5d@y)~*qF3+MQFvi;u(y!grw)-;;j@QaK0Tbulb4Ow}LO z$LMS6dkP~2oj?jTgpNX@5GO3xZ_`KUo9Ue*uaAds_VO3`kD^!XWcb@K#<0n7L--&p z71j!oq9|TApKix~wFAg)@F-$SuGL93+iWP+pFlsL$MrNc4#*`m;irFdFv!m#|pauivZRWV~qXW0+|O7aNN+xh>pN-Ai2| zj2SS-ng;5(=sW9r>ZUW}m|*B)m|-Z<#qfXeqxdW#L)fOj zt2gOW^b!1@{4{()3*ue<8~rX=gFzRm^Xnq`-NHSg(7+n! z8?G2m2oHpComtnI590^J_;n4v3=f4&p(dBiRp52}4^b5z2D7oE*gjp+oR0+U z7-~FhY@lDRpDZjC=IFNSn(_TX1M3R~0xrhttLcY|+eAi868poNRK9``&v)c=coOuW zMEEYs;yV2W{SiKwf2(upWG+N!(ueC;!g}U`To3a%__tgk*G-rt%o10M|B2VdmpZ$S z6Dx`*^sn^G4XX_O^qcgku8Qs#o59}K-PNr%J~O^BMVigV7-Nu7LD&bj9?kv1m2lrU zRxgPMjXRACO&Ok|UeIsp8hkbW zso{%Zy6K2%zy6c{sP3vRTvu6lMt4~^O`IV{icunB5DYnDkbVv5gi)8GyTe`L=7Y|d z4LU<}Lsvr(*k}y+kx{}pp{c&H{(|sPSgc#6ThCwRQPC`J(*Mx60eiosyRU1h>#K_p ztBaTQ+4`5_Pw^`Mncu{2Vi_H&yT>2l%k+X_yZ(&+wwNd0;-BzwJjMqLW}%k8f_}7d zv2merSZK{mXCkR6N)ZF%qM%bjyG{2@+2Gv|;#crP%y}kX@6gXTKQ=!ByPOSv|1f=k z{t7mjW2|r5Yp@Nw=Ke#?xSRkA*y;Bj276W0P1tYhl~--S`CkH2o93 zP5(DPlkWvSwxh1Iu8X)t{Gcz=?=YM;bQPzHW5Clhj6!GAMf7rhG2h)V*zmWhzo{1J z=|Z|Yy^tP6e*-_}6^aBC=-*0i8D|IIKZ+mA*8!Uu#sABDSUY=*Im1{WqD8R`TaHfw zpKw;_Cd?E%u>IIK%uD7Go5`-#9oDfrQJ4Dv-hVgiVItWqHjf>`4q%(;8tR7XCh2yt zTUZ1lwS!8da6W=3^sV&0!MfW}9Vwcy;=iaIY7E$5cX5b#l)25+B`Om`iB`l)h?k3n zslo=~oKTZ($UY@6khjV05l0hfqXma#^~b6 z1oAnWjV?rIqeIxCtQq{=R`H%Vk=jm;gTFhA@56O$7`uo+#kXO4Fk{ILWO*WHQF|XVJUpBr1X$PEV$fFi)9CrX4dDpNTibo8xPVgG39MuK_cL*+wiN!f_Jc1ew-u z<`y%E9!#Gg&JraUiA$tLeq|VT2(^x?hgHE46h|jAGnrPxWML?`gL5JY=ys?8Cu7U8 z4{SC&hTqRO!#m@vrD2jDYOUk+2l}^;<4+SWi393FwG$SG{i$oF>p^d$duT&6zw%U( zK<+biM|D2RP1V4fV4Jax*bHVHbBalnC0EF)Fa)I?v%ALLGmB5m0HY_S@ob@JEq&r>_71zeo& zIRBCVgX&JrQO~OtG@Z5|-Ht9r=c32a4X6!GMYqCPZX|XRZpuREL$=qJXiLo2mTHsL zLF!a;Bzb~+$<<~5Vhf=DK%p-Es(w~);E(WT)Zf%lauB(bIz=rfmy`9dc37wyqyDe{ zI0t#||K#`qWcnC#^Ul}=3_}T24>d(Y$n$fkx0I2Npf77@waHR^PAFw|^o0XGdsf{^==A5Ws! z&{yyecsHr5R4q_5P!qnN!|Z3elat8~fKP@8$_FyEpW1txW-d|;jly>mypz3${WJaX zSUJp2St%FV2%8T$%T=i3thg6%PxK}HP}~0w+zFhAHBP2xQFp0VR0`Szoej0BtYj#c zw6|ITQij~1u26-b|JAXESOvf+9{|R82ifue$aowYfzHN;VeiRYvLfW?2Goom1dJ{X zknoP=-z3G>Ve^UdBYJux+ByR z${2_URP$)=3V=UW28&8%39cDh2Tcl841~%)X${!dJkSLPYDF)rS5zD5O1K)VZc-Ph zE3k>!BsinLh%v-Ld76AKFgI{VJD{ob56GH#(L11K&G!EG&h|F(c7|Gc3bm9v#XMn- zqgT=HYArPdNkX1rhq1@%6ZITmB1`{QztLs1NQ4tp0)ql$rAm?s*`T|s=(*lzMg}#@*L)cEt z0C{VFW)@RUt)Pzab@9~)Jc!3NtSdQ%j3jCk%ZN=xtXf;0nnHY5)QW(SUgpZSKp#{Qt%QE#;$+C}xKnt-cV zeX0f(ho|5J;f(cxBL5cuWdB$+3GD&CZ5mWJEAeS~Tp&Ks+MnnjgLFWiG9Q=~g z^{LgR+ERJNtY`sBLgWJJHgS&#)z#6B2b*~7{o!5Y@8zFJ%qEI>LYPMECoX%Rdm}u& zrybG}X{s9mK8oX(V*g>k0Fgb5?n8GGkBK<6G8zlW^L^!k5)R1462QeyBS(?FNl04!)cXm#MQ+Ihe36nT zAQEmoiS9~QRr{!w{d#|flB#?mB1t<{p56?I#Byn+RLNJ*2S}DY1M7{QBF+;Ru7~um+DUCBqh`?3z(zX|V~H)uMP$2lP#UZZQ`Vyk&^lCm>R)sV`p);v zSKxW>=>a}wDtDR71^-w8a;E{=+YH#HQA(5=0A`-GRSVi+T)V#TCFj>PUY|nIM}y z=qxm$>QU3_b@URj&B1`CHATyzAF$_u#?--T08T}dX+(L}%znc$;&$L<;G%bu_lPz} zs|J|B4c&8HIi>|;L#&AGclf&pf&&N9Md$_m68;{!gp>nZt`44ze*@n)ADfC527U&H z`;vS&k*!EN<7dKzDgsLR$ZDWZe$Qji7w;Btrg~Q$0QqGL%Sp><$Wcy}&MDnkSfg-_ z+D+{i)F-H$afflLv`kuTYipYr=ol!}W$Di7X@ej2qgTDlz0ahrQWjZ6hOn*JV#TLi zbscxTlIBSL408>ormp58fH`(5WZJ*ek3%V1ApS=kdR8N(Ocjn?}u}^3XzbQ(#fx zy#Kg=As}88s3p`{z>I?Z#lC-h<$W{pKk$xFBdbh3wl-kp-J~j#hF(B-P|K+cfZGj# zc%2NfK=04??E`GzO?)FJ>UZczh_l4!^f&rBp!|njLtImwiOx)IkMVQ{Bn#P&5r02sJ~d zF~&5ITuY8}^>F2uJ}iw7IQ>?pkogRF1r1nQW2v(AQraf1Mm8Wlv<_O4_FNM%6P7?$ zBP$bih%)pyDl1Oqhjd4}t^B9-MS39Ju_@R~$KS*8Zg-wU&mFOxotxMKLA{7zJZna^y3)XMc zE^1Ejwcr;F$9^k2TDG7}aomP1BhwITyvShePu~;Y8dqOe7~uK^bSa(6RAZCW1hozH zZ=%SSt|Au)kIN;~NY#sG1wHU1b1V1EFa9b z54sX`hY4p#yGOYnlwByR4%j+N$I-cbf{@EbaAlBFy4?YX4CWsjYzH-tc|(z4Tu9cHtUuMA(wBy?V@0&D+WAF5XZ)TB4_X4ebm3`;^WBk|Dm+E+OCHXJEnwPmIx-`s$&-5NIkFH6Q-%^0B$%SPrPnm zph=*}y2;wa`_5z6BZeNqD}!Umn&blKpUxxj(-)7$tFg7&{={hFw|q^m556x_s3i;l z3~)R6qMxPzmU4I+7HXVkEa4J#`Tk4(T;~*Lb?h@5WlAymjEzk(khiSJZ=0_cFD}lb zZ`0R99ia~a(`cy-QtJA2zNZkM+AF@zUG&m+CHctN2ztLg&)Iz1O@I zT%7A3dI;?a`ARC8PQJvSO$xfv=n=Zc1yjW2*?ee6&@;_Uc?o7 z(YI(QU@t@9{Uy*(x(L;X$_cu`fPylR^+rJtGg;>45#X!*ST^>Dt}aBP2)?b>Thm-$ zTsJ-cc`Udd|HC-kxFje)XbzXkQLZfKY1`kn{w+NY_&(o{!aF1N_xMUXa1=yg@L8F7e1>2Am^p*92wYv33i2(LC zjo-m1>l*9&s~c6s=kjV=sg}wXvb6xQd`WO*b+wAB*F^0-F5x*4dnnDUP1i0$hh+xT zJ}i1gy#SfeCAFo>slD(j_%8-$kC30qN{}00gML#}ssXi)yhz#tandx`KvxH?rN(k8 zTy0_;VfE$uL~ogAHZl&W0(}A-hT#Ze*APrXcVO4Bwd!*9B;+sw=)AqbZ-9n6z;~u2 zAJG@+O!%iF^vgT@WBuop?aDr412GgZ-_}s;ZPVsz=~|k0THUQy^&;L9*LRmgd8mwL zhAV3- zQeSB{`7fD8`pCv=b#-s)pwgGN*|u4Mp#e%~(M=T3iRZ{{vai2~|7q#5(gU7R9zR}y zhjE>`AczM;{mK3#+G_37|IlXsGT+40(sQF|P*J9Dvacs#nuV4sL4B#Ml*{qcp_H61 zx#^ki=|NT?-|1EG!KK_?`JKGY*2=cU-pjrddcWKFTl^y7x?lmBJucf_Hpkw;UJ2@| z9R4?NH&!vVgg#1!Kf~{`U9(vncS<#^0CSkDSVkK^8E>eJ7V4H=6(JY@S6VKu3G53r zQkp3@v74Ba^phvZDv63;>(G1#V+I?@}2vk`*iW(;_*(i^8nNUvzWWgD&!@SEX7Dp94Btk zc6uRn%eUGW+PnH1`sqRMZtMp@fl(<9ump{%=2d37;qTNLmi@z3~h4=1-B!v2zaqP*0Jp~6d zoar4kPCF1586I!EYz!@{SavS6T_)w&UMh&;A`?9~S`{bjJGth#D&=S8h1*}){>MwG zOLQlkL>##YI-*y+@4aDVzZ?yq?>n8}%`ao#F%A7q{exVMTn(X1AA*<>x5{hne2KoX z{`Qbl562UrudqyaNmm~7rep^VvrQ~bQ___pJb{?6u2;RvPbHku1G;#X8R#739F?of zWlQhc-?5+A*M@q=B=ld@>3QHu0UiYh-RT#O6Ar@u&9)lq|Lx{`=5m&EmP6Pl?33-i z?QZ_u{Gl$vm5kNIdO+?JgQlPy)E0}FgG_m?f;P_9(AL=MEcuri&XDoT;{BnA!@N#| zt7&@4uXDK#a%X_Aw28s`aN|s4U8*aU?62VeGf*i25R7&kI2pfY%OTIhQOX{gH z)byV|LqFWR-n%3#CToeKmZLCK4b4fsp0pe@<7t`qGT-F?lm91RktXv5^E0rTm;THC z8-eA4$$%e}$#s;I{@eaX$_FKj`c2gYoGb&epnU^v10}xCzCFlnrOqpgrY~IR+>Sj7uI?p(VIET~S=&6RihRRS& z%q#9vT+63=uLYAK-q4LiLNu>)!ofKt+Z_^4qSw> z+zReB#nHq6mrr37mP2V20vPCbT`WHm>X%ubqn^jM^|nUP8($xMDfpSGi+P!UxBrR# zy#1PTT$#z8;gZ2mL;>#72>L&EF)t@!g0Xp2s4G6X0RvOMUJ+P4gSXd?IDjt=7w5B zHBUpYlyNlUsH2zTx#_2AW>jI67aN3oie42}aJ(TXI2ITo@X@W@7Brzxc_do!daGKPJ@R-=c}x78nNOR5!h3v&HJ zz?R0jd%4dCx&|E3N6`n34tj%NXnFf*TdsS(+fS4cVPX|=Jb#Sufepci_`CT7;5%Mv z_qB7(C1wP>mK{y)q*emf*&lG4-_9-0T!=ksx&mFBki#J-3<1M2z(DB2Ck3}0jU7*n zH;g|cn@0u6Zd8W#w6&|x?>z!>AUt$MXkTE=^vRa8u+nw*QnTCKDDiCK34OfbI^YsQ zX@PyEue0wEegyv$;uWH=t#2!?7H1GA2+{eWtcBa|`j0)u?i8PkwWNO18P^Dxk8)85 zg3^P+g`q+@capnuX}-MxeT9Coe6U;)^x|3XPA^k>&mMw)MNS*bj6d06EhJqWNnGwbxNx0 zA2NRosS>&wNkign#n%0!J zS_c$(^D6*{ttMTUeoek1J5hb9_VjT2f$~k+Y3pvA<@fm9A-<5%gq;aZp_U2G`j%Ok zS|xRXpYyj4?G?H`!X2@Wd`cFT?kT z@7U;^?HrOz<<@k4aK19VHYG)Gi>_nbZye>D<)q*2PwP?vUJf>ISvBp}oN)P$DY)DNJA% zGYdU4Jc;=y^Y%!MC0A5z^slI%(U*W9c_+Vq{!^$U>{K3AOV>cx88Fyu%0ZEKy$GJFi48p5#b&Gyx80G+l{4E!+{VLM-@? z&0$-*8@fXZ|0}o;7!3lw=OpO;DOxS$1n|#R0Y>{b^qxL>vON!T{?1tqmD&C9qVTQp z%@gL)FX^b_pG8F(Q!}iN!4A>rG8~F!;^v#3=3sA__h3P@g7nhSrDK>xW?RVpkU_?I z#vW2{Nwl6TDRv!nJ;P7n&BZoioLF023>=TzQiQbB(adqs*T(mWea0>a|5?)=;x@QX zx}x+p@myqSc@w!S*{q?z&Q2fex$~eA~Qt4QD z^N54?U3Mb9+piT)&bgG|&et}LHm!r6^AcdDYzOQs)}84Z4Q$~o;kS@t8e^IZ{H2Mm zp{_#DEl+o{BUuYLE5%4EG7Ra8Tte@n-L*+tp6XUfBm~Kn@+BU)g5QAsxLup3k(`%Z zpgX7g3AMlm-$LICz>+%wCK$k-_)}mEO-IM06QJI18!!g4{IC5_eOrBXp`R8JHZtrU z@8cJg{9Tfr(JrHT;HUpZm^G|<)a|Gkh#6@mOG{ps^(#9B43~_c#Na}rE;%|cE^m9T zK9}TvvNxj9m@9k<|DSb)^+iF`f&q}LoM6whb3p%$&`G;2Tog>e$)5!|M%=HNX&G`i zxogstqyrTU6{F}rv?c9qYUS^pz76nw^==5?7akHnJl-U95}fWI?xf;dMeWNzIbKtX zsfVUalZIm0wz8D65aqqx%23Vd4s)7L|DYf=&mtyA!i<&&*kUm zPs~ri?_=*GeUUfJCd+#JNIRW7Cx?`_2O@&H2GtBZ6gD4tI9CFf1Kq8xb)P-iegvP4 zhlJG)>l$(~wee$UBCEB(RI^FQ+&*>7?r3=-gK>T z-BhM3v!H(3L98GqLH-^u8Kp|l-}y^wCOt)upu_mUB;fkNx}p9{=MG6yw3o8pT!nEjphx~3DMXePRV!NQEbnY) zX=+&-VTdgA>V56A?`1VnGnKfw?r{sFd{OBh$+Ih!N*z{u$38h`eawRjeJlPZ+fhlG znZL*7?#x+8)u9>$^$zL}eeNtgA14D({I4K?D|OFyw-qDA5y2;dC(`rjQD}3t7E_tA zqmkHpcUN}@zu+HWnr?a*RyVv2z81IVug$++lw5R@?ZJjbc8r{6_-ZKj<@i!<|Ju%Y z`g;0uO}RH)-i3F!$BoMwdfhN zRD8eqTz>idYUEv_Ri%xU&L#CLhX)b^|9xY=^~&7)TPCg%>*FrQbxAsx^p%ce?&J>3 zCDJSZDi;{hkZXFN+iOZ<`k zGVUqIm}AUdoFjS_h814TPt4!I#dGhgc1ZbLaek$akZ=6Rn4PiY_os|F3ZXiir<+5d z`}+&MhE5FH6*PwzgxEYScV+&oynoD%%-0gCCeD;LN{cdLGJ@3u%IOMa6%vvQk`=en zv+3KfugwbA7ksmvwVX|ESiXT_kzv1Mk^?E7Yo88&(BRMWO$R>nPyJ>6QN zrTANsE_Xx@CXbg^1+5P1VcB4rOim=P`=C`Qu4fcULT$hujLg6Lu>qGI|YQ)zveeruWEM zpM45&-Dj0*R=yC`K6;XExh*MUa=KYPuIviW312B>3uAnJeVNb)`y>{LBZNbOpwZe= zPkYZwSAW+Y=%GI_SPcftpO(dj+Xft~i+ylyb$u_JU-pk2A&&wU$?%Z*A$mTFAMWqz z-)b#qo$d6Lo#y`Gej9EX)+6(geqOV82jp>6gAN5<1(w$v$5Y3Uw2!ID-VL5+F~edi zfqqQI9%7HO2WJzxy>p_lgQzdc9K9%{EaU_J3g4kk*1BrdwJFwmR(;9BVh6D3MwPo- zZeMUx$SwOJyZU?V?7M z+S-2G|5b0BV67Px6*M~WRbmIjQbU^Om}iM2zx0kWS&2o95hGPXJ{B_ompKQ1X_@N) z+8%u!RUEY&xH4ycwflAZx0;a;a=ub~Or0ddB}3axK67VUQR)}CxsYu}EZ;(!hNf|m zx*Gn@{+C5Fizb&D%AOi77)DokS>Z#}tmr7ng%&+O{;X=QIp^ORBWt8|>fU*`zLR0+ zW5=V7kD5Fj&*$jm=Equ?lM0hw_SOC++uPk+KC)S)8ox3jQ8!1|x-h!%^|zRB z4?MW%b%K=eymHSfF~B+ug*-APZ)$EC^pm?q{Sy@wGAN`W+*KM;XezAbz2G@2ZWeKU zOZ_-wI-7lOY^nlv~kDCfSlinTIpV%tR=jkpK3cz*H5;wH|tvS{FO zy12@^v94aO8_=6NuYalE0Nl?d)@jy34y}|os)ku%AHrs{N7(_M?w-BQ`A(1fvD=5{ zqA8|-OnGd5?tlHWgtE_$nts){3S{Sm{^4Hp3-jON330M>m2M?SWue^T$OO{@MSdP~f~!sCT=zF0rM@ecI%iC!B$GxS|(9jTGj z(^1~xEBjCu8TjIVL|>-+>c{D`36>0kn@+32Rykjh_XTzVt4KGdpCfOPlt55mqTErA zfm?@w$Z8g;JVf|;ftt{Vbn7b_Bwz|mF05Gi!7<439QaPvO*>7iluOF`+={u4i|-aS z0{M7+ReVgY$NBwM&IsSKu<%juVm4B=LOL?gmKL-4mm3A#PfUU#MMq8tA#`cQq zhMYyN!n+qz^MAI1zEkCh2@x%$u11v!lf*8z!L~-%X_<{PV}B3G z7^QEh-&CbZ)vA_HmPM&;QnR0=Js$13>7I~sEv0?6+ST*q7-eZ{jnv+Sp9)$5pJB3T zzv;G_uyD+1Cc&<=Z_cWcRo~@tvXK=cZRNLCSO^@PMPE!`9%e}CA59lcl`Gt=FhYDM zF3YQt*XH-4jB`kLdqmHc;J3j~==RLSf_4RK z-R<3NL#Br~^kWS^7vpw2?>Sp*&9#l>PVyb?p>xq#?40YUYZdUL!h%`{Z5Jv1OWQkJ zLH5AxL5Qkti7bn3Z~AH)ZS8AqlJPnHfNOwjr|Fm}J+4E1BjZiuJ8OycbXtqFpt4gA zgQ>Dusr)nVlMm4EDQt49X`M)2)WEcQY2QD8`81e(L$r=L z5c85V>Sj8cIC_Jvj)R+Nt-%AXXg5IAH8?Id*^B4nOd>1;*qLrQ%(h{NKN10eRm}HN6v}ZHL(TNPu4i##{xB; z&3sDch}q@@HUTaFq&%ZkR&HfC%+B&H_T2*v{7c|>U~&mlQcJQ0wwkY*zXK!P&6w8F~oTfeJTyt-&+%-EP)^`|yiVyP4~=vT#G2R~o` z^u<-r^$K{lYr;N;EwwziEPx)>(4v|}SF#hc)BNlGb3#^zRE*sd+X^^U>ueou=Szf= zO5|mtz)V@}gpo}59r8t}S;|n|Al(NhpE)WmmC6S``pvk6Jp!KFa%vK_irhu6!6)F| zfhDE--})K#rqU65IKy=#b=}dr=mxAYwivj8GocT)*;c`}(No7Wof<`rFb*?T2VVGi zxY^aoZm@^=UU?4~w;Odab7B-=>`yAGP@*fGSx{g5sh+f4w=^?PHMfPkKTS)5OWu@C zEPJ4Tp?602i6?-@F{}TJK({1HPMUSo*1B)a@mWN=&x()(UtAHZJ^Z z_+`M}_W<78)@HR{@VD@{W$H35V9ZkWvHDL)uaH%c3*Co%xxLfH^whklJkos4bf9+o zI#nTF*DS11c>3L=w;yvua(9GI41G|gZ`DJ_>ZVChcMY~(v%WLkF>MQBLi_vs_zUwU zVE3F%Zy?c2Tu#WuKT6y=}qy{_6N3U9@;a4 zt_j@8Lr?_@>_g%STn)1OF1v=i#zO7B0&Y@uuq?Ht1@#Cv!o9O3Yj#PRd!hS|>6vK+ z)NZlfc<-uAB$M(^^jwV2jV=kB9j*tiW7VSPg^t4Rg-LJ&%oLdvxga(*whU{CH-wxi z?>qVZD{$GiN9RQM7B`BQ%GQ+qv<Squ!QleVZPUUI}W&7W!WL7r>R@ z0{fF3dmSfqOLVUzVj|n4E76+2-laXwVRHWBHJy?;CTXykFV4@eo`33BxnEJFk>J|8{6wQtdp?bM?Buo~k( zbo(&wD1D^Y&xjurQzoS_-CA`g;v8|?evJOH^zDi_Hv$QPL-D!sZNpxNH3b~G;qRLn zZHmqnwlPK4} z@>AZ$u7O)P3_C@?PJd2(AZiMuKFA%E+c)oNZe`?^R@KnTa0U8FHtm!)gK5f~5Yt5h z`a1rPlRut%wdv&|<`*5={&xFL^%mD}nmsam+0DOiv`(v-x-Dcv$oHy!QUc~hmaWAj zisxnC_+1S-p%q4YBj<@P#8cUSW{=O_kVTnp8M{}0U-=1A3+?{w#?xItW`9o)-yim-UcmK5v_yeyZtP#`AU(I9lhvc_;H}mZmTc))`^4#PaHTasdfWJyS7<+%n zL-l@7!zKOuj{7?$)u>wYV)okX5BD$LtDV;`_eaFrh)1!F;+%o}z}6CXaU%K%=@0jT z%GnY7OIvl@F7q67&7`?WQE>NZ)Aw56U9UVZ`vGp1981M+tzE9p8iUc;JS{V|X=<;Z zDe8MAgM3MTk`_w?D4r^c)FQjU&6a|!^;!KtOn)DdUp?=bxwARAYR{@w<3Gk5{0aW( z)cHUEDZW`WmuO5}gj<(=p)T)XKn*dWLqk>Ix$P=!Sa?3&{p+) zwS#s;?3I61{L$BS%Gm{OR=hL2%!lD#+e_eD9b^_T8M=qMBwx7iMtb%18YQ!euY?~7 zuTW!OjT@FWLA^6{8RfrJ{Jc(8l_PPlJ_CzT{ea^k*9)x1`iWYinU8+ zd%27S>8HHep5f)Xmy1cbk}$Y%V&T*0&z?PUo%6&mUqC8s0HgE=_S3V|HmzJl{%9M2ks3m3N?id3S;5^@~48u1#yK- z3f_Sn)8NkC&qO3?ellHNM#C}P&l5kUryl${7%9;f1@8{7AF?1s1H`o_z&QzBqK1$4q>>4!=xJ3iM-TnHs zxMJ}d!+1l(#C?gkqbf$f1H7eGFs>? zRB01(KE!Q3YrUUFrwwtOE`38B2R3M1B9+=p?YCUC{0d44u3uWA6o1?NO^08#)0mhW z(P4iW|8$Eh^}}C3d$s7*^p}5m*0|#n!xLB6imv^Js7^BPM!o%(_8?UZ&JLQ9s3ux{ zmHm6a6n!??>)PH%-ikaI8WI-nM4cbfE2USoEwH{8_6T>&;}t%}&WfuGz0M6!_dV%Y zJgCS~L995iLPW)1z~jKu2Bg+?%qu+*vL|G-*>BFzy_9SFN`Eb*d(-LFnpH0d-X4rLhpoDa37{|NmNNOdyM@_W@6?(pUL;g^3c*CbaH4b zxFb0PdeYv46$L+Y8s+%VA?U*dcf!xGq2ZAwTuHUee|}HHO3?e^zr*9G+SII~8bupi zM%Nz0FvHJqG~yjrhKzR4uGhX0VbO6-RIo~+^S>fTGU89#0Q9V#QW4GYPdbi_P`cl zdns%ai16Xz(Z2S+N4d>%4`SiisL21L=&Zw<+}|+1-dJxefH7)xN-1C<*w~F?AG;M> z>~6*G?#2eO1EjlagY{sYZ2LXGzc|-*JV)St_x(O`-=F)Gr#tvHC-*qI*}4bXEYSW; z250U$P!a{;WjHIbgg6)U%jE9QsyWI}iag9B%t78=UW-R4uiBaK>{6dnf3S|XMxY$X z2vj)g1}Y1+7<5JF$a~2Y7luo*CR3Bw9oQX$)FLK5Q z5oj!3@I|}jhootFL-t}aNBgBI;oZ?h{QoYjj*PKL; zKtFWebM63r>Ur>GlQ`Y%;~vc(D3<{)tzEae`d2QfC{QVsuYlNuu$Y{+%UKEbPkCj_?#KgRsUOn|da30e#V<{oIXcN3q3BY&BhZXWDtwhPG> zWGO9|X2(lE@ZOk^h`!zTJp`}Cfc-fxS zwHdXoaF3-EJ6uv=PIeiSj~R{XMh^0t?nUJ;<&K1U`g?t2T?i%{T^5K7`j7jJ+X#2l zUr>uJ$MUdOL3h#a*zds6%-~C1Me)U;FxlW+C@}0W7(qv!KntLKq!v-%p-!V@m{JVR zmFAl2vdE=Em?fN0x1^R&*nz(}!fm8)zn}!Mut7K_zwhT-*yn{u42?Jwml7XGIz^h< zu4=1kZ*HqY3`hL&`0X*p`>?keI{;@?-BsmCL!_OcnJM>r==CprT7(aJD!R8axiYpf zy5Woa3U~XU%gIs9Qs#p?uez|BwCbPWqgxY#2yMqF5YE9o>IrB)C`7f(Wm~3AV%lTc zLt#>md;a#68iI_WHKVKT!0$O3*BtjIU_;=1`51Xr!-)F*wo2=1-{-z!b{q$7`e@u> zCYv$%FE|87f}U>bHQG!kO;yM&7B z&M{S+N}4A&pO6+w9N?{771S2g4}6eUp?+g4XDcM^4mQzkwHwZ4G=5eNQ;N{*(8J-L zVqh&8A7CQXfP*`Uww0#nwsr?sW>@S`=k^ThyR&ch@ahrXpmR+AwC>}W3QBo(!0iA> zQhWa_(3@wqW7`i)J0t<%xtIf5$%)jJ;4F5dg#xc=3Y>+HjFHAJvXJ}|cGIcO=}v~c zQijzWRGWz!;s(waPByj?yAWm~Y~>b31?MX#ikHWW0xjz?v6uLmj<4gA3`7$vl|2o- zbp(S%zsbNh>_+WFxr4SKu)A8tLlijs@dxs6df<61O_b(j-M8As7Mb~_=XXyNdkCjL zb_CWF`s@{&Sj{0RQu-F=Z1rxp}#f2e{?jRCA2EpkqLXQ4}mfAPzXAeWv*Q=l#|@ z7M|~b_6Kbz6{YeW;HB>GHN%SxK1_=|NB*BOSFwiTNhtuGpa3}y`M!hSu@v+%ix>fn zE_WY~J>aU+^nC8wuOsVXxlHa*@0;EujoXcg*7YrZ`W?Fdf&@Wy_}6eg>}897H~wPE zol@6uM);7v*nY{#Y-DGlykK9$xO#u@a<8fZy9Z7Oe?FmjO3})?(Y47w67RHeBgeD3 zHf~vARN;@pBLynLdP4rdp@YT-B?qtVoY*-Q-sh2oclZ+mh2S%gA4pBB8huscl}SO- zL2mOI?sE=6~wKs zM_jqk^Y*HLsdrm7mI=Ityd&HmE?%$KAv@2CM7m@hgMy>Dcun#;?rL-OQE8N4#8~lf z(o)i5uQOg}Koct0ZPkU_MZG3e8On#GCDxHeWQy5fdaRtN)Drg)zl2f3TfKMtFel2RS4C$4!;hv{Y&po)()18PG$UqJ%1$V5&pA`u*CF~bhiTks+3d4>C4^7 zoe4fPhH8(p%eKgtYmBm$=v6`!$OM4QIx>eA1#CG=GF$`pWs^iO-wi%I5()W=SR=a26lr=o+dw! z8;tYf#&Km(1Bs9wNFVJl%_HEVOkzx8D9{AVE#-9OBz>FiFE9=;oB+-W)PJb^`gQtW zrd1|2+-Ec=#u;G9*GD;8?3>`Oo*P&g7)01ZsBYZd_@k+|QNen|s!F_?w1DU9sp=@| zSW=x?RfL_3tq2_&cEc^!UEZzk-Yv0-WvFK;0dX4fw(*d$KsiHMj^2W<_rdti2s8x# zLc3s|*2L8OsQp?q7+a3%7k(wYF<@8V3+RXEw7+jlG+!`1^WGYeaw4f$GQ1ZOnRaH=j?=UrtlYhL&0hN+{_*ZhT$)0_d{qtv{Kk=I*g^p*~%it z7>p7v;(EFrCj20zXi?fc{Z#!+@UBe-9>hlcYW#8VK^HSMOpR4!O%axK+*LExCplX= z)tpf7L)1moJkSNL#IAyOzgqX!ILR3Gf7dvbPW1;a&Mw$pz9JtY&v#qvwg-6<`9YK> zde+^jy5fG%{b5W%><8KqdO*Xw`UwrA>kCkEC|hWB*vi20ppEwT_K%`bq6Gb8-A<28 zk5@i?-(|+5#?KY{^8B8v?z^EEL(BSINMIxOBc^}L`nst;wQf?lU-yCNRtE z`_%iipqf4L=Ws@t_2hwGZyhkQ@<6AS4ts<&<{jo4U6#(b>riJF_|20ePDE_sfzV zf88di@r{GlCi4R<0rMoIC8uldSI@;a;TABV>{rO@(Ctuw>%v$!gYr0JkX+M_D<|ACp;lM zpyksJIW{?{>T5md-sd(V-41QTP@ai3f}N{5u9@5TtYL!ku;RMM439gpqvBR@>N%-$ zgKUqmwL=Z0ukGCRT$R^&?{<6~VVtH{z0-^`_k+IVjB}6kVvkqPbbKw&84?$&_dg$S zU;9%lsb*ALKyw}7>SVqSA%rG`*N0C7-R;-5qis3gUwqrqIYX2kekYth$~0;upW#&_ zdLdd^f1vIQ=mVWFTbM~-Lk|ZHzknIYjNokH@L`ua%{b0DPSh++R^3tN(;v|{MRY{C z`&9XiQ_WBX7w!9ZtMyyUEYCF0S)<0Kg@!E*ud5bUvpzg}cM@^ONgVNHgsP7;x*9a2 z$xUY)$%LEuvYV@F|DpZfu6QmF4<8(U zC3(l-3g*VHa#6|VA;jbr;0<{sYYeak0XyF+Vh%V;Y^Ek@P( z1o~QlE|e>rEBpjn$ZT#uE-_?v$TJ{uZ0}C({w)oV9O|9?HNEL(^|ZT`ed-auu!18{3@R&?+{c(hW{6PfEDvPE#xOlZRQ&JmVH2JT4i8qZ=+Kh02$sb)|O1HA)vCds9LQ^>Q>yV48NXT`6IvNUhi zLHw8cX>|zAHfZXVS~*&t>am z=;?o0S({0f!_yQJrL&x;CMd7}Yq_!-BE=fx4?amYzXN$c9yKd@WA3;N>^ z(8E_Uf|!BCP+~ZagF9=TWhEIO7?OZiHI2K6`w8 zRfL~}nyzhKsXfKrbBGf`fr2Le2fDyE(@fJk_H6c=U~0%qVqcQHDZ8ntozN~NT_AM| z#`_aM<6|-DOatV9Wlrls>r{`;9t!{60sS!7Ftq9`RYU6M*L5;?Gp{C1=>O7th0h() zAra}X+nE^Q+u($Vj8;}Wl zr6o-nP5nMo^3v>Mdn?i|jK;;RinX*`+C8iImCIoloRKb_&{WB$(i%zA)CqVBg(Zog-7 zx5R`!2wM@D98_VJnWL4j6(KkUwgY%gHO@Nc6X#T?mR<=QoE+v)m?ISdS9Cx0kXqmv zyg?pBTA&Yz4Y(UH9UL~?mi%VCctn>LyrEGMg%Rbp?7{5AoWvi)dxAH14fqC!A{_{C-VokFRx9f?c))XjKR3#2n-`9KkG-#ZQTHv- zY`h1asyM_y^i99@2{!tBMt$-2;=ocx@o?CW44JfN@?=g1x9#tOzn@F*l(0R@+_B?- zjmJlE`owgN=qmZ1^UX~^NH;n4cdB%-Zz{)l&=^rXsAz)ybnmZ}nW;H9bb-3E0UQ+(CiB~3(Li>ajR^P7PlNXiy9Pz?A zW7O|ae^Y-BSr6VKLuudAu=;zR;tvS!$I`j+_%&k#avZ z3%FW|CB?-$7}a;s`_seHjp<9$3Pz9ZdDheM>hH_jbT#3_SwORRS!De;NjgJvBJyS=uD`Lr#4^ho@;M`KXX)?~en6G~#Myi2 zT#2iR*9eg!-Y5LWCylonK6&o-q$QtAo)q3M!r=&ZF6#UvIxQE=PC%`ApZSqF0T^5N zT=u*CpNYoA5HRn-Z;Zy0u^r|XGg|vrGmAtcC57Gz75VP>+Xo(+@$IbkS;AKx4157D zCS+$wc0g$02Q(5RhM&V=&NsU{Z9w?_Xy1t4iya2fzYk*ugGlxyQ$Rn7R6LRI&}p^b zfvt4b$Le#i0^?foWX4(QI3&qJJ`03k6%i3miWsF^p_|$A zp!tJsvTeL)rso4Mo!50Z*GKgD_k5_vRU10-ofePp9?0a*IU)9j znFg9w=UNgH89Iz{jbqqZmPN$f$RN=Ge8yp(iO_(nMS zk;}sg^{usIVyDHHr%6YDWUXb-Yrr?WY0PhUjtE3}x{Yx2cgt`~rOczW=-%s=Dca0k}PO z+X{2RRp7gS0y=>n%{9$w>p<%vw^eRA3G))SamTvtD7TkwEQtJj7-n9d64R4f1o!;g zP*xPmCbNp6zo<8^FfO9RQ|1JI3vBRS>62{UXg=PqYujU_7-M*&c(J~xeSLse;S^_y zOM%~w3mzVv8$3Vcr^RAvs2W-2*5=-dbARvVnNphaS#Z;TTIa~lPn9JV_spH99KSlh zyZ%W5{o!X@tM8*fi0F$D(%b3lfNxm?p2$cbk`58f5!`}5V>Dt-O-*i1xoV<11H98S z1-}K8UC+37YbrD~UH)Aa;6a|jxxi5nQKSiay8Z~TUWlGyp5?w7e$Nn!d7xfCQKP2B5 z7}E9IhSUt%l=QOyLeXha=KIihi}k~FFNO>picjDquIoP0{j})gzqy**>g|G8f?)42 z-evl7y{UU^w>zbm9PPc)n@k=>c2oXUY*E%JGKd`FLhco=5Li7z%vFp9=!kW_&Amx7 ziFB-?T0eyy!)Ei>@Tb9h-etUGv{*k_Q;=au4R~H&fev~kZwl|RZDG7yiUwY>g9d({Lyu?Yjn%PW;6N) z`aB=uB_xKCF2ZN@H+BcMzt?%McMK*|pp|Pl>a#sfE^l0%oG9)y@MN%@YR7ZaY!k*_ z)!RgUOWp2q$zv)05k9|XOAkeRN>fk!NjvDR_a2B}gBNtkJ4b8YsLOrxdn)F54+V8OZH+2ebqH7mrvtwRmiey; z7_D8Yt*XFPM5)*HcnG`&fze&jo{X~$vQDqPCY>vtDA&p!Ld|f?HP>|%?;~#l?JVtw zqt<~m|1gn&a7(3#$yZ=LUGKr;ts~tbm70o7Rp4tfy8L#T!`%svuR5+f5EjD>$%a<> zY57jjW|h13x@~qZaCaq+BmOcyF^z}$-d|vZd;o6VNMNb01#h$5eBHbhc3Ih^LnMFl z7;-1m$n;{&V?-hZh%2TcCK+g#A26OVsyzw(C!n7lub!&L3D2X!gw1v1wUbbJH_>oE7pp#F7Ed1UVtK@KJZKU$6V@NMs-xSJG&=# zcLoQ9gd`>OKaTapZK+XK&lb+@XmY>gu8-Xl=fb$gaPRysnlF7Jc?2BFm7YzWNwhPx ztV26x)P?9wW=^npcrvmj6LqHEyDM zfO@7*qP-38^;y_^eE??YfABs3CAyFbZA~_{`KLJzm^yq$ETe(t$({!Lo%@#i7O7&p zLSedM!g%!ccpp46#7WrUvbQO{sj78A>lxUq92&qIcrkEO&}hwcjih-%v%|E;#P^H! z+YYx`bptKjt*=4#*Dl(1vJ#H}y6y zMfM;*$41BH2JH>b*XQeVI!GNCz&D@amG53&ZZ}mgqcNDoUM8X8u3?k`Z+ME_hs&hN1@aF zX~j$h69MeQ*TBs81culi{BC>(V-uqugT|W8U(H)U^LU+pnSLAyoXfT8TCyZt{FE@6 zFekDma<8Aiz{8qo<%vEElT0PXE9}2)HhT{{8d%hQtTM|zmt!tB;4BOz4E$0pMX>&f1f%&unv_`ZOy+YGhbJIM+Jee_`amm-qFAaE}pEUWJpYmUGr1N|{DYhO>Si zXgk-!Oyf1|D9eG?*)1=XFYl}qkCEA z6%~i|!cGIf+c$JR`ls!xZ6RtnDjMc_5Afyq|6r#l0Z;8BOfp7CwvsD=*YOD0(amHT z`6{seZUZN+uNI+AF)K}fP)|@p2#EwX?2CPgLy3COAP-lFX@vg_N#jXQ(OCjn3>KIU#H};P2U27a- zJOJE@T+o_i1APVop3sS!YIPsFhIT!`9H1pkbeY`HQU6a=DP*xKT%!fo1eI7Uu3MX< zT@QSW|0iffo4Mv4&OuIZ7atcs__O{3@kIgb@V8K(WD&0ti?lDaXH>4Li_Y;*CN-A& z1$a68%uCI_`a+%D^2}mI8_~0Xw_ziNk~i8<*hQ+xsw_2E-GI4|NrRbgDzLc* z(9fU*O7KA1HrinDv85|IF!|kGMoLM?-A1K|1RuYhTaoo^9>rZL=`lXrHk4VJ)5;`3sdJl}W;@9aC`4SS8ODd{24I zUFe-?Uxd)P0-21g!F+*AJq*J@D4gL4qw_7W25Z5QZ3SNAUTw7YqiKohFmNZh^dNc+ zdMNrEaLpG0i)|ui118;^XdYv;TmSD5Nn{{YVVx#@5eVB=2itcT+_ zQ-I?%!8XVit(9t?p{AgA)34EY1NZx~VXomC_#z&XZjjPRJ4i3#{%q78)b&DNJ`DKn zT84vB!SZF3?Sb~|iU@_ue$qaHeTKae{}^AZKBq1M-kO<1;9iCu+y>)rwy#Y3mBFOz_L6_yHD%yGSsEXRt^ap*|u%uC1f)zh>a(X zA@0zh(eKx#=qBKa_+~EGEtPqixfN=VUO0Eqkc@DZ{)CQWO=9&Y9U-+iemFX>_yPQ$KlfbMdhadrkex|x23FW ztVr+_`GdYM`iSlgbU&EBC%RD?KN9W6|OYaI0=^(gdcJ}}FxHD5N* z!c4`?0bXPl@XH42!gcAoFx@g>E@|uv`(xn6>hOB}bt>4rbgoya3Cgt2Z#$a z`cQOV^i4>?xCvU5R`_1sX_2%|9;F_1*l+Gs%~D;HdCQJMjsu+?#BK#v1OqaVp2+vh zeKmX4r-1L|0lS31I3!*H%$|9`a;pW_iOf!~MkMaH6@>n}mIvRywF zatOY_E`JevJ9=GDV9%m%*X|(T4}J!=b2%{H4d5AkU|3=3LjFW{k@1xOv*!H}K8Vw% z)23uI&%6M1`%Lg^R08{HJ!EvOvZRAbI?SR1{vij6Kr-R^Jp<3IH?SX1>c;7Kh9bQM z*?{aK_7HF5zTxNwmEKKtL#Z+!FrS9L^dPX@e?sjel<$*I(wlX&pa$s!{AWuq(ymwM zs^1wCjCVoXauf3n)7P48E!N!BT-7hpF9Gh}cF>gUgj#zJ{Qb?qmDvs?)e7q^E5`6t zKMHarMyjkdTawPEz1lsf`+;G#Aq#Rt-a%f;UfmSk#BPgf z3w)hmi?!_o-QjJ>*%3G?&i9~CKL+(|ullb#Nn=sZGDn$fY(+K}WD7-`NT$1*shZGk zO!qoyEE*j4p2U})aa`eaa4~!d)YG;*mj>~G7 zuaE)Z3-zm)vQgoyF6#LU>^vqVoWcPP$8pUO%}vQP$wEV*VIJ^v&M|ZhF^D50y3ML~ z-6b6dL&Hn~R_kHNc%g!ZGXT8OXJ9_E2eS=x0D3|ml8H26x-c#{7hD&lMJ$6^<6PkM zjD>S~G_b`&?Y7>Zz=dK0WBUa3d}N?~&qMD;=R&&0Z%h&9C%PHUga0;2ZUOGAiMNS=UjjS=i@*YH;O-*K>Vz;m(IrY%<9|JbtADV== z(DSwDelN4vKY9$~Gv0!K0^HWslv6;}xI%fukTHI_VccJHd2U{m<&;)bFRC7rAtnI> zUiqTqhJSGMc2j{~>`*M2#WLH75n*BTMKhL6;qw3LZkZJM~ z^%FRVe(>|%0w#n2vZo>(RLB2Qj#fbiksAVsn2cVC9*iA>&4#;T9ppk)!CrqHjTnu%X1i%S0eMIN&ugl-Uxlnaj^hX72jVd7sSLdx zy=vgao-r&pWY|J%V&EEoN4!OJ*@U(P@HL|0vzZHw-IehB6IpH64Yo-(5`0fi6bmTb z`_ZcrhY=x=6NR-}EC;|JzuUCQgtJ+!^^PvbDdZL8B*^(Y3H|CT;1~U$IVeZ;B7hR> z?6qQTVMYv89}A5W%tOo_aK}Faz4;Prrgfj?pymJ1Z43N!t@WMt0Ax^g*p!fE7YM}2 zYxd`$ZI6OHuoN@LdI}hph{(!)yVzBI`rzW$cOsPXwb9FR?~L4qe@J< zrZ=FeBkMbLD08)mYL=LO8V(ycx;ib}pwZ(j5I<)UnHGWu@i_S3rs!z8d7zW0>%Zu7 zdUo|3?Vi?6=*jCI3VO{l%`uG*W`nLejTQ$?D7@m4{2S~g?Q*c#6?7f^4XyS^QRhOuh>| zQ1!qKb5Z<|AC>fxjF9Q2ZHhaJo2rqjTG=z%ebD?iN-`yRiWv$nFaqW%-IVDHE|35Y z$SQp^sstiDtS6b&3FHkl|35w>nB3 z|Djg`BTk6?4mpgK&U$AE=zsW-k>Tbt(B%qndsbr?VvhiCV<`O~ok)$NV#tHYQ%<;_hYw_FWL-R%R_wg^eo84Xco%Dm^P*rt(!4s&}hXdMw>9{fUyYZYs{W4jIdQvGPlNK2XJiNy*o_dzhrxD32Wg3V@qZ@mOAKsW0-=(F9N zK29{`W{m}1;2W4%zcM^DY%nY{&OGNJVBB*E-h`8oneqSplFhWI zvP1Q;rITu;UZT~|wviu@buMh89FHJegI*>Md>6|g&!mIy%{asO!RYUL()AeJ z6DCFo^EIW6k`FzzfH;GAmsmi&PW(z72>RzbatC=MWS^XYc_AJ=uy}eRy`IiyoM9F* zy+LRF8?+pu%o$88@O;u}nY2nWnlhZYiuivn{!n1}2Se{v3arIBj&Y83(B}{#!E3T( znIi@<5^)bv47Bhxm^7?K@SWbyY+&&>1JnH@`1>-9F~$L=P*ba^6i%!$rV{f5^JicL znQUs?2;d9^Sc%rJ&?`PRY%`>QmOVoouFY5PR3B5ODDNu9D>ldj<@1#!hMmja))<>ujLqq)Nz0KUzW=3V9jn4kVJ=a?IfABV+{lz?holp~}c^Ck-V%LGlN=8ks1mI)r1{hEoI7UgRtPp*^& zDiw-NiWJ3SS-9+nC{t7@oF_Dij){JDZtXlT>L>E+IM*K5QPh5+-L1W>eRcbX*7VkQ zt+RptV`(O}UTXOY`tyaYwiZ{=O#Erv)aKd#uWePEM_Z?GyKqpaRdiJx1ug?imqUD2 zJWI-uK9r7xEH5z3__gU)Q>>)!<*=CZ8;SAzuUM z?n%{xp8h@GHD_TOMAbah?9uSF4h_f9rSF9eo5I*=JOf1M)0QGX5HMm$A$ zqclh|&WL>uHQzH}yU!vVCfIFM;>!1(9`JNFMzj}7!V z?~(5H$7{2n!A~E&AY^nTA&M7U85`TzwVyEYX;SQ9Udqnl_eSg=8f1l5|@HcEle&Mydr63|Dce%5HnCM%WZ#b12N+;@7Z#; z!~Lj-kurt4fU%k}Br-qp_Lw_kvqlI<-U(U}{0X$%4BxrFk#W|z|6oVE3%uy~SDIA#qwW#nLJLBSndFgq@<@qO)yDZS{2-b@Qu! zS9&!(sn2fd-_qH1zsaxmbj{Tox0++EJuNm#wj@Sv?8BXejcTLL$Tv_8 z5irXeA!G=5R7X@Nl$?Q0>l z2pX?9?5Ri8uWK?jcDJ2wbBOMWke$~>hor-#x7FL!2O*pCJTSP|;;!NL0=sWLc8+Ag}TmWXYz3C*%L61Pzyp+r=zo zUUzr6_kb5P#OJh+GvG|%_mF9!Kf>0BF9@ZERRkRkE((|(cm-z9m3|8ZGQocTD?tgt z`N5MzHV0Ee9tDjHz8#Pl=;E zjIWI0uDe{_-7?+EVUB)=kMo-8rS#^4`egXe2U*J^%USK-x1CNq_kjFxw z@&anRY4(}+!%%Da8)$~1I;PI1u2H|zZq%*=jSNz&(4?x})kM&qqq`ea$)L0PDa(@W z5{HUQy7qPLk%UWp6vc7`)IquM=I=1fGpsetF*NCa=_hIh+H&h4mc}!UzZ#PpJsQDn+9Yor(Olo8Z`sjO*RiT&RVSx&j!Z56r+lIupmy{S zwH2CD!&w6wdavtnPWOY1ltbpt=2gIj?domseG3VX-jLKNMN}cOP*+h3;PN`L6xa~ z7noL7FxSAd_)F8$JRqZ|ip-~Mf&1>S3!YeyCFAyDZ(-};?;Qv}iwCIhs4Vmm^j=_W z%>WMI2H+g*gPvm#S5Jo zHHkHub;Y&MYocq$)V!+hQ}eJoqV7X&Z|&aN+11@ugR1USmR6uE0;?WWF0TAu5m%K{ zdB2idSx^;HwW?ZEb+wLGm)WdqN@&k-69CWRvSg0rlj4ZtwJKBf9_~nu=A9;0>(qSF z9@5U%(zGiKBMjN5eWnnrht+A;m=9VfSswxO&lM72R)ZdSG9(TRMhBxAz)HqIQlSqn z4tEdol=_kqN$pf3P0!454d-s>9_Qn|p7{v`p}|*!--JyF&+6kHZ3Of2gQSK1l>@5= zZ5})~g+GKabl9-;;Y}l}My(umI!!(XGw$>F$_Y(VdDBfZV`s(AU$#KEbn9~Qx>cE3 zTLZR7Y^~q6d()ULxoak_vo6V4);o2}^aF{Tl1kmjdDu~Y=-a)odcR_xV!lE8)(Y+e zE+t@gKuN;ZME*GSIMT%UNtc1&P#my2aI(h@k9GWd{#g$Ok55}g>jON9x9HpGTi|cP z(8thWyGC3Ab;vMVKU<#Zz3GbPy5@`MhA6S_d+n=YapUHRIROUW(x(DJ_I zUHRJ!Z?@&kdpY{WjaO%1cD=897xHK9?};@z)w%64?bpNu#W!H5n=PL%Z<1V;d~BQ3 zcBa%(g84e?%kuYo-c^41_df0Oq`bB-b@>rL#(e+tNAf$MBCb5N{%+m%rh!d}hC}tc zYoyim%9RzNo2 z-vGZs{(1hp1HFSjgzO0|gfnc4U!&hro|)&$na%l+{hQ5aEn-n!2fDT~16`xsv)s=K zi2l@wrxE1%r}0qp?|J_l{2KZ5;I~I#|NDOUo7b-;KUoDM|2{5jE{&|aR4ZsNYFpaH={g1es!*9s%9Nj! zWlIIp$j&NJd)v-7W7Gbooy|Q>OPUQ$>XtPv8(KN7cYy)eBRnIV1RC-l*=Lzf@j?-! z*7VHLCF-()>(~KY_#KdUO+?v|tx&5CAx$Hlf!cXA@OosN5U!0k!860B)5l+MQBV{V z5iASY6evA;&|4LUlpggLeg50{Q;e{LlH31Om`SzV!9; z8v>pI8*hu}UXMzTX18JP$Jr`&fBF*oQeX>>1zrD5n6V%oJ`RqVU@p+*>XhKyxFw$@ z4;4$g#GMB^mr5h0tE4{CX7NRFm+-XkOV{|W3(y;#5f2ylwtQ>psajaox9Vi&uZp1+ z$BMg(x_|WhF)n|{=dv%x{Ma9}zJK_E`7!p}rmvg6Km6A3=g}Ygf93o<_Wjeh>tAH~ zv9Kvx-r-r*=tA zVRde8bgiX!el4Ov}DD*EV0^3~T}qmq7kimZRFP+M}MJ zjx#v*bD?jrLz-|3DiY-nxmF?QY3Tisbv70>9#aV$3B`CJJ_iyw7f^0frZ6usr@3Bm z9l*W;d*%h~JK$UC$Gy+>U~gf6U~$NDW2nfL_XfW zPkbAE`}!pbJ`2VM_XO_`^@2}CP()qCj>z`t%ossmW8bXAi_=$>uROUrbggsclhuk<$JRVvm9}Qo zGX3(I8R}WfMtw~?k$5qQ@2~a$faBqRiNA?2x3XKMs-3E4VBuCnU6%(eR3zv}za;ER zoIYq$a%yyCH`=(0Z5az*datHV~jlkyW22S`~ zu|WI?JQv3cn+m+X_50eIXUScdGv;;Pll70kKUjW0_sNvURnP7{J^W1PTi*H%>UEp?mU@g4VH^W&5tBxw!AZV}Pc{2A7gq&WZK?cOv7qu* zMQcq+&BIz`txLm(`r|G3W{U7+he$F-qL5#b7b>nOYQ_7-Kj6r~4+E|bjP5V& ze>OTgW>QF7$TYulzdYZ5ezUx)yvo7z{Lvlhf%oEi^$$EAm>7~DLJT_;_9U!tcv_Sr z$`E}u=5)+}*o}SI(bpqAqr4*iL|hMx2=5P{!5QJQ@Wwvtqt8YUj9C*ch~Y%Ojg)}? zeZQ~8_b6M){to%MDBzxq*ALbw18e+(bb|DxXtij#kRtRHR(1S?b0wkEB+3Eb+XL|; z@li>VI%sk5++Z)$=7kYL9WI|tn{OX;^m&hE@@VWz2*aW#!-@u#J zgYrV_(QHf&7J>VQF<^Xf!*FO^BF=}90o1G^E^-3XB?U4MkCJoAW0=>N+t~_socl%h zOivg74ZlHxy5NiuPQ=@Y+Ndvm2+`yh+DQw*Pa6gr`bg4b zk`yuxiy(Y?J+Odka2WhY@X+&srGEyp8x?2_CKQsSgOEdzqrn^7$2h_`O6{r6R1H?` zlcY%!gw-AET0C1m)ce+RYaUe}1@_XZGE*tObXd^A1*NqmK_wfCFaN7998vV<-5@GqQ@iJ; zes*r?Y?Qo|M9KgPrR-E>!+yM1U8sJly`v@T>a{Bk{)X8wBfVwXVtQs?0(0;fD<1Y& zlOdyJkZp`DwKuYNBzS^1LO-+@X3ifiH!NGNSyqRQ+&crFdjxPFTYD9~N1O+p^C4$S zgtcN@fxSitUgr?t+@2-eC0L0=ND1KA*$Mf>Wq2}SIBF_ z61Nn40ISEj# zjSc@2wYZNx_F3HEe$<4;gQAixLli^5jw(qrL9dLQ$e8qG`j7NJvrf<6Hn(}+>$zd` zo&VimG-qksGS!mR%O=ljnID?|a0VrP#f<*xk7q0%-7w}!(CgsYkY`cdQ`~dMaKJDQ z{7wdg&9K-IZ}6nbD2tLhldepBGl@IIbLeW{c)$CMPKJh^#u){A`~A2AoU_MO%>y>m z1N&xsGHE?22D=OU#(c#*7xt=i4MPkcfiZv;)e7A!UsO!_ee2iMFMIMm-!6F*@*@2C zj@O%BZTWQkA_ z-O?u_8Woch^FDrC-#vY7Gh~ z>XQi^My=mvfy`^E_dU-+e3{QOU#b7nfVn});C=yL11JHX0zL`)`%m`M_*Hms_3`k| z_TJ#9@*C-2=07Z`8r*gG;I`oGkhq}NLG9jieJ;7yxfVe}<7CHN2Mcmf1mM;1mCI!L zU0b>yi3W(6(EBHfiiDdxTsnAd-&(azyPLR8S&g*~LmO^1MK|?pzSH!vWkE|&>(>@Z z+x)gF(E-sg;Gnik9!XX}oi-KD)J+|}9g91h?ImrQZO2*#tpUw8&);;G-TI1 z>RM}O*4iqzR;++J<9?aGbVlW!3PbJT+WF0;O;N()j%kt>aYgs$?kfFd{g&RDy*C|~ z9FKtc&q9Twa4^$;0^IXspq(^P>9qOu6ZCRdEYJ~R*o(Pexsl+bOoTdkA?Okk=!@xX zG#S(%;1f|7lPb_5kUuoY~V{B2X+2qUspdVFU_->8{oFnwcOQ{xsvG*Y2F<~2k{Y{ zb9-S6)PW)+Y0C6l^$Sc)Q(r@{ z!J#kKAJ&c3{evBArRIfZFL?0I%G2d{q&#>tgQXWbgF20^hg!pHj#jIS;)`N_AN+Ou z+u5%NzueBBpEol%@w0DU+}HkJT7I?tocnwEFXgZJU*3OL{YfsqRkWwvP&U8pdFl5G zdWETadNsN6V#EE0VGWZTl=ZJ#Vq2)9C&D1Hxa+zwKv>rPU;BkN|F(_5F&f<5*mR|y zQlD1$s+M2Bz3xeqN0YsAd!sMt%xmh_)_FG^t~WK$X~uNiX+P1`ud6_MLpr7h(-Wz0 z)Sa^&vMjbG+wOvQXC)#NF%R@fJ>W061=$%rh%Q7X@WY=w?>IvoY)7Zftu zi_K!Tq=RlXN3%ooMYS5-=RQi7vQ1H~*spMdoQBVEXHGFOO#7_Uty~8a?o0}D59R=7 z5oBCGgUp8+!0RT`1L?z9d%%f#fbD?1rfQa*MdK{sOyWM`j$t2X?*J`hFlRpJtvk!( zDbLgM0^i8r=v(1?-QO)BAmmv{e7GolYjmHOe{e^BPukPpH3dr8;l|-x$8H^mo`#=3 zdEV9e)0Z?arL8%)*0>>iQ|2~gR!`RRorG;4wm;uAYKwf`q0Edm`n8zlOI8f|@9n}H zQ;bvZCvHu;!{%`y{KFIonJz~_^Ys$u-WJd)Y=C{?0DcDFE&f%!IQ&t>X3)nK_crzN zfg4LC_z=c{Hls$eQSw0SBaSc?7{h>xvCV$W-mNQzDe*_`K*&bn8N3al?VW9F|Fsq3 z^Fux(-y7cEeU$l-bK~u`(3{t{EHdjqmdG`lHOcjkYavw+yx|>`cd%Qr zxI`1Df>-G(=!p_=<8TjQhO(Bjjo^R(rvrBruI z_eyn0^|&j(>q+~V_RMBZ^U#Jr^}OmcReMW1ioFX~{$2I+{tw)bsPFf_{K!A_`9hu} zkCZ3RcYcOZ_gd|aTO{t?t8@t9GeGlG~C*(Oscm$JzFk!20!SSX|##<6e_d zccnHLYA;c(M{N`MN-4D&H52Ra)wwjxsV`~pZO~T7R~G_5a8U92qC-Wm{?UsD{0slL zsBlTq^?w73GmAW7wvDKmTb^3+sywyyV9C{zlH&Cx4~j>Z94XE!`B&Ur>{>jx^km7w zlApzurPxwz8KTs$a!Ex;^?>St`hj&n8(kVlv>tB>5Iq&LJMV~a;zM1FyR4lLr76;n z^2PFQ1$gY_O!-8qO%fvgDcLD?N_^zivhkp|RDhm&s6sCPtlFU315D``%1KJb|2R4e zsHoPri=XKpx*G{;MHE5W?(XhZ?C!dDT)X|--Q8WNAl(BK49qay`R)6yS+gXrUYI%O zeV_Mv_Wo^IZHuG%L$kEyP|Mzy(JlXzt&m-6>Dw|JGwNz&se~zNTmMzgQrc8+RC~2^ zkQp>XtH7SO8XBU>m<mXSfpOwVEEZtgj!SEs&d z{ZjiU4~QSqbLh)aKS%Q>4V=7UvU1AXX}&WmXXeiKUN|?u%d(>7=_{I7Ca!E;^>Wq3 zHPhB{Hn6cS9$uWYv~k9cS;I!18l8|mEN4?-T2OE3ZOf4(`IhpG(%F&dAPnF1WamrA zN1qCxUv07?l&%SGdQ+pR32Y~d@tuLCVyfEJlvEFcFUt7WQIuQ1x2{&OP@scPrL1;1 z7*)rbCO7k&BjK3}D4ANka457g)IF?6*x0DsQ3vBF@!9P^xBt^&R0^&0@y?FS zZ&_JgeY&xFRQ7n>eS44k9zT0Bx_{}uH)l+)fBL)h&Z!ltFFFlO?Uh`VyfdycZaiv} zf5Yp;H;4WTeHds9Ovg;a74zeJ$WbpQZV=y0$4xtxDa!RN2`%x+VKL)et7$sgREM7F z5zKIl!ER1d>J)8cuUfL3?@H^WL!|qpj;7<{II*}Px1o(-e?7NuL+xU4H)|>qDpr=f zEgn)dr?BJS`8biK6kaH}^E2gp-)|$nuKD8mBldgd_wL_x->KjC{=E5P{GYSGC;dHM zbm6a}NK_JEQiwC~;W}oWwQ+XiH+aw0$hC|!-7`LL&TwuZk0%o}2lXla96g2|hScX( zY=W&|xv?YIOE~j6t~?GelRJpJihmjDd1C%hx5aL|-Cwzn_gLaF-{X$Q7mo(eFJE|u zcuw|A_MYfX@fqmT;2#pODM%5-4;>TgA2udzWZ37hJ`w+mSQJ?pNklJ>E{hlvDU8U8 z92|Ksa!q7o7k*N|N$z5?b@#BWs4XXNi^`3(5_2u=0>Suz(5LLaq z>PK~0^*QulS5+D+Zh!}Ips@N+=I@Qadi|PEK>anTU|zwopFV%u{MHvH7CtX#6i>u^ zbg0Z&`ntSJ`Rt0yazWLcs+(X0R@NuicdENx`?Mykrn;(g)#|G9%HYbK73}iiWybO$ z<&l-^DkL>SYHYQOYV+zo*Lv4%t2ThbdAOQZU0FG@@_q%kLRXesW+?Ade!ZMpUR)7d z@uxDbvY|G!c2h&6;5&YYwO}TgTK2R&Y3&OiaZKw@WI&vS_BuxXNj6$8l*P1OlJ{u+ z1B%sS*-!aWsORbO=g{W+wO*Ig73W$Rc)o_t{-m<2vWKF+bp_a3gB6Zelj@b~xO%Ai zf5@qI=F z$aAfy%*)f8>e^J;NyzkQ^|3p56r#h_dm9|9*;}S{9*OEmY z4|YoG^u1GTnl)`!R=ez=9$r0}eYk!1_xsU5aY*9O4Wm|#uASsD<=nJC(@)P<&dFVP zGk?$uVpZ7M#&u)YGd4KZJJuWVfV_GAvUSS!*{j5>4=!rABx7dPtTzLr2J;fhNr&M< zGTNGLXKZ_HqoG#mp`mHELQk;~y3o5W=Uh&?QQV76Cet&)7(qzGX2DJQFnLOoL6Y|C zRKe5N_g}HUdVYzjzFHLvKmON}*pkLlZt2XXhmsH2Pb#I?rS{TIrE%B?s|y1Qvwt4^ zk@Ira3)%zWJ@2Q)le@28zI=N3&7CV}W}Lov(({Dn*4>*OUPZm!_G;P7ycg}ChkoII z9##LbZVgz>ubQ?s-KuL__eGK*sW-kcst6hJ*VaJ0+%`%9x+&Op~V1PI77YtL@+za>TZX#^637T%V~vKafu&_a5idF5oB<0j35P z`*rkp4c-#mE%H)iw}j?|&q?#!MJAOc^=+?hU({i3N`E%9j-p748-Jic5et*!Y`osQm$urjQARaJFmMy0)CdBv>KrV>)gkmC37Y-a!2|NGT%m*1*C34f-S z+%Ikmw%^#QE0uXwPb)vxB-R|Nv(-)&2lHQljlRiVwZeD{~ z(7mp;Hm2rE^^qDk{EDZw9c$mz46o@@^}aG1_Z44ow*CY9#0PQR2TJP1D)?Z#wKO(&YIT8X(I4vY zt*t%eUhbt~Rndj|?D|vnsr9=AQGy(SwLU{&tB-`{rlDa^!(@S}URA%U zUSGGrE~4SL;HfZISSs2mdM;ie?jtsdx=VFUXPa}Iw_tCc4KB)4&0I|}=A`GXm#md` zv7PL2aS+fs9VYe?aEwvHskzkMw8ON)^nvuf^egmAx`DnFiOP3quW1M1J8+{V(5}+X z(GD=rFux$f^(m)>^OD!X^9RFVn&*AbuUm!~_>mDO z#%jiuPUKIXG40p%`Lh(WE%RnCurA!b=b?e3?6N)}7gZ zrcRqK95-yj<>BO!6S|)6MvJbBe#ner9YZ$!9{U!%-0td_3GKy1g}36V?Y3lBsA^29_+R<1a#@)d_yd8zRu*J_n(>kSIsDUrf*n7feXsv^`_0eSf1gU9 zB)?63v+&2i-y?p{{1yFs{jYC@?80r;(yDjDbm1B)8&kU1O|x4(TO8^i>ao^f>lFB( zp4d*?`r0yW1`?N?>Xzj;IQUd>O7yAdH^{g0NIjmKo8_9_E~8E6u8zYy%}byqhQ{2E zVYfNiW;QZ5n*6h&sQ3^-4jLb13CIq-7sL%76!to-AZkW*Le$Twxe@%xoUn?p-l5+^ z`v+YQVu48+3ny~A*AcJNK5`$aZ*RX%fr>zO(2AhgSG}O39tlQ z_f7IU??LgL>3-9FzRMYx|Mc-&ozI=Ow2!q0bXh)2)=M5>?IOUJ>sVb-6;k=3VqUeO zN?pCI`gzsVs*>`V@eilZTJ4{og8pBU}=anBr@Vkgd$>3WLf0IsH!&eA{K>f!*9YjdO5&7 zu*Nsh&*YWo{lnYOXR_CAuRxF49ur*exfZ#)fXcYot$?TIJ!I)wQ@}0iMCnUuB{YQ4 zMz&ut+%PQD4bYXKcia)Y-Z6$5hB4aC+7LZYe_wY;mjdnHe{0e3Mp|Q%P||2?yx6!} z*hc8xxT)cXWRBzt*2M2E^IL`q1B6#=vTGL9@N2lW*J@VPrqrfZNh{Z*)Aj+XylcWd zVV+1TjKMWt3$Bl!q)gnk#jj;n%aE1^=p4R^Pm1447D}#SVzm-pq4v<3Y}7B*ueYwV z&Ok2O7wEALBFF2aaJew0v8W+aq!x~m#!Ew+E2Ilij|tW9)TyQ*(=g;7)0KEfnr1bv z6}892>$5PR_GXR8-#tY)3TOqx{yh8rZ&`3zWyM@v50vU1_00M{5|Oxz#2_A3udKUU z;azdGL|I%@l2XDdSy{ZY#9ka(`fo`=MPkKw@Q1Uji>f|XrB?-1pQy46_6od&m(Wpv z+`wzNEjZbj*mzemOj6RcxQQY?g{o$cWS=Zi7K?iCn_`n&d`+=CuOzeGMxRyh^UnYM1WrM4-y3~Pkd-^R4j>`I%-DRNpV zX3BSZF+BlUAj{b|*z1vfZQ(?62eNmt-_W)6e<_U=6D5dxi&{iI$au}@$y&==!@mu- z-46GK$VHF!neKDQx5PIK-cuFwE198FLvIATg!Bx57d|<4ASu}Rw!{gOz@ zH5%`|uvO`n~PX9UM6%Z`jP?Q^!0Sdt!ocV%e08Y2{OL zr?s8Faz_5lud^1EY#jgpV1a?91r7iSA|1kyIAt#G>sv-(|C zbm65xsUK&5sCdqQu6Z8x-1)lg>-=|T-ah*x{ru;5`tRPApDQeN$LqET+6iiEme+Vy z-L4F%JXLYGvR&ng8lReg`s;Ol8x#WX+AlTHRUaz-%l|1GS^m2$zg$%Iu!LDMzw}4R z&hpB#`1*r&S&fYiBJ6AbLZ|PKK5nV{nR=8iP`3m*$5YYA_|F?1ZDbkm*&o|`FuE~L zdp`Ee57mVpgTA63oET&0KWPUt*Jg1uc$uQkx4e?cTK8m5`hm z9QPnDJ3*9Cny@r+d?G8UO>9MMLiEk(oQTp0Rml3#IsWhbM|xcM;9{@)z*Rwo|BL&S zrD0uXN|~{&*{~`lvHCN2GXDXinhc%r52l8>pL2^-$#La==UI3PezMC0ehq&pSTQfj zR&tUf((w`Z)jItceLqzjRlin~e5S0cY^E$lw!X!`B}7`-^bb7HbV-Z&n`nXP3>XA8 z;0g9C`&1fHa;Dg$Xl zgM7MM?tUJFJTG_-azElOfL>kWIuNe$zivPH43{~`M_vXVh7eq>SDYfwB-T!l8lRvS~fucA-Iv2t_O z{3>1DnmShXqpCL0?TCNp{%-gk@jIYsTH%NS;m_!Ai@)CfQu=wx&ypXP3+aWDzhnNc zC}Wi!tPZFiD&z>`#Z{tFBC2SkFif~XOcqaTW;ZWw32#xz56InAuar}fC!1!V8Tui2 z;J$X7b}#4%cEeZ0Cgkm{Fk~A}=?3Y>8{>`L%s%GT<}v2u@FguWEimb=wbrS&iMAWa z;C^X*2iH)4Vo1$l z;Y0uahGk^=usXA*VI3WVxf~CynsQznet(=HzPVA|?QT6mI2r08aSL_Vy7%@t=iUS* zaFoXu^u}I#JH7YeKg1HUGxTaio5;Ghb7LxF$Hc8jpe7y#L(Y-f*m+t; zYNjCTcXrzxN6zT3UAv9%y{wN{|I7itgT@Rl8A2V_cFfSRl8IT9drrMRZTU3y^l`H) zX5U&cFn`w~|0Nrigf8=1{A9`SMY_dZ7L+e6o;hyz*m0%frG4Z3`((Ih?oFE0Zcc1W z+zl}0j`%kDe&ejjXw3f;@ZFQ1zUa|`+V(F zhmX@f40yly-RAe_->vzu`u)9cgnets(w(PzlUm_&=Sp*x`8r?*?|me<5bVh{5&vxCcamk^)%KDz=Xfn{M6 z!!xlK{zm`dII{oJ4BUo}`p^zRB`r_vD7;K^?qPf|7N~d)p67CfYx2-z7 zaO`w^vOTl?V_OYH>ne*lQg>9DQwh(1owHDlH1jRBOKVGQ9Vv%$v*u zFe}x8Rc1CxO&^V~ji>(O5F3g>FIr_dpkJ@oYu;(x)G}3Mt6eTdH~m%9#HJ^b5t4P1 zJV}Z8UvaubDDKtNDk+q%kcuRaC9@?i=&GF+=QUfUk7RRYjMhT=LB&Kx7W@cZjp@dV z=C|gdwi&i8d!pTy%p_lU2iUqhgX0W!fsu>LC9@;UQjLM-MpxmY? zDDKoo!bA*mPIVr4>~NGjnjGKljhGVOw(rH~7o69fWh4VBmXt(lg1>6BeXISsy}~ZE zm)YeGm?s==&bRh!_MdnSX-bei z5Ll=M&-|DDr#*m#U8!b>w!fCAX;zcfk5&1q{;GaTv4R1w@>j)b#bC^Brzqohd zRMXU7l^d1YTPL)-G`*7K3)%=qq3_tgw!G$LZBA`S&4p@1?f%*;f(*e75l!?(G()tg zA+kY<)3pRM@dQC#{eimBx|*7;HR}+s)2Fte<`sC4PXu!W|A_KL)aDD)5M_&^BlZlN zN~H=wUdw*<4E1PrXLYKQr)&dFeFE4SkI^fmz|))t&DM1 z)<9}dTVw{03pIwGYSSSiG5T}#hqgUp*2bm9XUEsa_e_!|9Z2re;ZVxxj`f{hr%p>t zOaGdAH!CwYI4`|>M2`>M|MvLVyQoinKW+ct12u#AL;DO94NDqf8P$KxKVvG!TE<09 zNEyF=!p{k;Nx})8Cv}-{bK{h3IF|KX8pPHDPN*#i2*q5D}juhDP3xh>tuR z85wmxGB&DfWL;!<*tf8a!II#dfYSjF{pkKepMky$y$ZdKp*Ok%Yfm~d*+SX9*>joO znSJQ%=;^c`G&}N&22*BJCXiN>mN`Z^df2_}Rn}Lar#`ioSbka7Sk_s*@yIntnX^H@ zm`Mx3(wL$k#=dL&C=IitH!MNAsP=m#QC01uq zS$f*L+YdPoI+C1T&LzZJ;yY1Egi(?xBCr8WbQeZAd_zr4KI8m-fIhUYc-^KpRb~4jgDe#Qs z&}Pz_C~lxz+@(s$TJmGkS5hWw8CQqR-p8J9|6{ue3PrMQkuBHeus7J94xOXO`PF#| zJ)0-it=0+fi9WPkw9H2Twb=a1+{@AhPS<$LBinsjtS#84vX$Dpz=>jZ)Hu2k$ykLq zAYr8+aR|QdPxeFh2iV)BV0@%o{4C{Im&2`I*2UId(6@B6E;o-r^_6K}gEiC*+N-Vl zdHUV|<>eaah9doG{bT(h{dw&&IIs&e`!VND)AiOp(XQ5Bfg^RYajG%V%rW-?=jOC& zwTWa_n2wotn{ut~twA=LZ8SVf-)v7od3|MTho4kpsj$4X+=2^TWA0|lfn$7&ZLB@R z{y+SlNwzTC2un{(H&Z-_%ukKy4d;-y_eOt7bx3sznIyACKB6~`Ya2fbrU@3<7n3*)6#O=Rhtrt6Z$l5GwA2A77DSgBvucM}vrEp)eTe=Sjav9@h(Y0cysYu)C$ zeP9p;h^`3(n!ib(D;6vAREa8zTB7Qpicob{rYWB*A1SlI5K*ZLRJ*hzv|=#67lX-h z$T-M2!PwW>Y~Bv)1K3ofvNnCCZ@I0yy%{c?&x$cV{~WFW)I_T<96kb#Yu3w%RThOC-YD86TsJO z;Q8>${49P?*Hx~g+|Qt@J?x?P>ghcK?5h(0VF4=wn*t98bKsk*3b`ES74FvNQyc%t zGm)v$U!vQ!t!n!-ZcDsd!mWhJ_ATvA$*moxr}XGJyHir?>C}~-&vjXvQI~lk>tN2@ z+?%=A^Xj^5dmQO~s!wU(AN}G6bQ<`3pm@;qq3egShjkj>I;{PO{zESh`)BC$VRb`; zhmRj}WoY(*g#&rrLwopUakIx_9nR>e@3<*3J!ufMgJ;71+T@1Z3vq+@!RS8QL&xaF zYzL--1b*BVn$eou@)hzNbYir1TkF0ElhUr}7u=|ItGy+R6~31)lirk%!?b0d+*R?q^^f9|;;VMO zHp=WY1y~u@E#QjPBe$x7B!Vu?Ntg*a@s@Z;yd<9?Kc&B=XW$(ufUa&3bqTeQT1Q<( zJwcsFT}T~C>q>jiJj`_CCGyz(aDFuUevKT0dmn4YU}$_SJRZL@@{?8If_(E#^h)!d zfh4R#pKabX-t&CRe4qL8kQg%~pmX4>z^{R!K}ErNA(esALEeE&16TM}_*s3&!qpb+ zzsUcle?-92fM(x3a6f!}Uwb$_BHiY>jpkkBC3A8(ci3;(3T8Nq%=BY6QpA)EU`U*V zt1iJ7W%II_%=@4T?v49QVEAk}ixU}Jr^g-MUi)3WPd!J|NyFFLG!K895|olUjOw4_>-tzabJt9rB;#Uhb0$&_8!PmnvdrM>rCr1`#L+z5$HIEoYmd-!*(BVG2HD68y#eh zh46R|w0E;_!Ju-Z^MF$eR)rk*(Mjh&&J*x1SD`ZLO`A%aN*_Ys$T-G0#VTNpU@c-v zSOM(MOoCO-cEyf35gn68&L1|3a{)ZkXB;QzHM^CqfG#o{xyf0$hr-y2?D^<3P?!wn zN7_qT1T}`blr)&sg%kxY#xTcc)WSAfnQf~iW)Yt6P5(0uGpCvdW5(Fe zBlm5*!5g{;lOK9(yhAtt{&k z%OlH6tm9dz;RKG~j&Mi1W2t?+y#)J!n~iIe|F?Vo$Mi9$neX8Q!Z({tcfqL~XNk6S z2S2FDbk7uJR-00cP@@>H83{wFUS@b`IBJ+;XvX_?Q@;d+qC2{CV0f4e-wkI>^G#ok zZ;cGI-1OXZ5U%e}NOp2Hbug&)QvFB$Q^PgGPV+|dN%KGEACIQ z`uRAn+ZDL(F4)i6Z8+upc3g7oA*K)~plve~R-%chC5DpXNrNbZDP?%%(Pq*1)1J_d zGrlnjhV80wcZ~wmjS%Jp_Yl3ou*N5B)*@l{J1m-LS z;k_^k>lQINN)eS4Esd_mJnR8-e=i|(f#2a$hi@rMJH~;rZ%Zpkugu(^H6h18SK0MN zw@2Og_88f#XK&YjmVW;tANA;^92m@_e}BYs5;4>%A|>PmBKW@%Zq)KcnnFwRaRJ1<|=Rwa@i zk|s4g5d5i3s{H5g{i1Py&ldeH;gl#Ud@D|rRhI7l`?_dV;iEr);XS!n`16ljVUIt8 zzaD=_l`kt>TUS>*7M|4+b(OWxYdhEOZIB7B!k;IRFOa8cYShh!+lJj>)GPx>Y$o=G z7R(;|IYjo2;1O+QO=J16Ev%i4^NjV3)r@RL2ID#PAyrA1kYABclJ_%LFl&+JIh{L) zJBBrp)e{V&AO?dm5o^dh+FjZ^%3I2DFqa4>9UWI4;~n(|wTk?VJeC$q3y0>lfGg*A zjgZ;W6Ip&|do4u{x$$0lp`*cLM+#a7ApZz{% zK6^Z`cpmfo;<>`}tmnVTQ-A3B*Ymu4xqGI|SeIp-4V-+|KP(MRL}O8<6g{CJxFkAB zjy>pKFxg6wU3dUz-ebCW-4LxCPN}Kd2kORG6DyaLN9qJVX)|#I1lXa(ee?w~hz`Vl#~Dn#7CCx@qkjkc<#PKF z`%&vMu#K9mA1(VVdy#K>#4^GXV=J;Av@N$Sw&&S*Ah+?XeTn_OeJv)a@%Gu^e@KWQ z#0t`Q(k$eg@TfA14IJ1{)DP4f^aJ!BsDZqhOlC*;PCgud}RDf-%O8ZIOu^ig4UT5Lb*ZSOTG(@w>wem{Ak|=S9!Vp3IIV+Uh}VJA2;9}N8spTOi@g*ASQsSBL# zJB_bR!%bsMPGcV=WNb4hn)A&8W)&E1mn};z2{xzI+xFKw7zd%9$VL`}=@ej>qs|>^ z>t^TLGi)SVrR}TjIJDC@tuw4-s~AM3EtVblI2YZgMPNHcSSrmkz%C6i%S>U|WfDL< z`Uah<5j~8{_*uG|c&3>~s<90iH3PuTRGHqJHlqSB!)v|@`|DPm3^rQ_S(7bdGuisp zvJ?J$8r(c5Z2N3iY;$dk;8*Vr-p2;;s&cG0%Vk?W^3k4IN8o4gW-W$-on+CN&zZL) z+hHO8?_2e8`as=1t%v$7D4tK1iKv(E%Ccl7&10K8NRKutpuJ(BOFmjsBTf+iPc%;a z7jwFQgkwZEgmZ-78ZR`^8WuG)2~O5?>&1fpf-&{=b!iRP1VxSMjkiQ?MOl)c;%kzQ zlH*dW^vxN~;qr&Fb#gcPI7L${Te(f~Ks7@31pT5;nsW6a?NqH&`(EpA{9$M?UN-JE z=i)(_eJyonvE`D*V)<^##*TRheZEKs38&^T$0(3HtwhK~k0 zQZctU?Of&T<2-|WsZ*#c@^NO`L(C-h!546WvLEEEIh5yE1I8kAwHVjrAk4B>QC3i9 zp{9R^?*~((;CgGMy3^?(0T$CEm_E!=@V@m0|1cO?oAszQZm?_FgE{>{j{Aa@$AMYK z93Ge7mcPJtuIo@__sZSlK;@u&j`up|wI5l7b9`?4l=_AFKlT^;?+OwJT?HnB&y zo@Ko%d++PBrf;vlJ^O9zv!SoOm$3J-t~0wyvQOu%PEYL8KD8j#Bjs*N*LL69RmU$$ z*c?NSJqXYEY-Fy}e8>AXqc@(9OxLxve`zreisK3Hj@`&(zK=|yM9fIvqn>6XKR2p+ zMU|nlJtlO46`!D^>|b)U_)@X?@BYH#KSv6`{@GJhP}s9%Zt?oE2c_T3yvn9k?x`q5 zmUZ{qt2H(C1MA<4;zdu9l@hEt&^lHvR~-gdW)8SsV#6`RTuYM0X)UvQI~4Xjf<~++ zcP1YM?= z3hG&~2+8Co(jU?*QZ4bC*g~E}){&l(Ok_2=mHLdD#MCl^I9{A(oNb&j+!P>L`?-%EY3F-=e7yU5 z3%%R=#Gw!K8lI~yE}LA!!O!II!}ygP5|?1xSR1fIk?0QEaPm0vXW|Yq!QRV0#g=WG zXB};QY+h&fH0g||!RX-YYjg=(r{<_;jAjG+qwf`m6m&urn#@*s^4oEYp63`Fg}Ma;5SaNbB$TXGUHw2WUz~_px1KBGTf45 zb+-<*cd_&AbaZw1+aK6>+QS@fju=NY&I=K?c6Npxi?YoRzSTAVt><9sO~HBpi}{}U zGID?NF!LS^f8ky8L9-5geuw!d>f^8GN4Ou4T4!6o+wa>?z=M%O_z;VnL!E!X{|F)Z zk|-nQIAwkc&7b1FC!z+1e8 zuFNg^8+r%&bfgLPre}jOHIKf6&ZALi`SiKS9?(#&=xgVYhQg^rCZ!YE#6`zB$8Gy7 z`%-WNH)B5yL*6pitT#tsFhMUMw?P6?eOfp%q`pONb(D!gfP5#Wh+uR1fJ=d%S9$} zB5XD!S1z;q+fBCB*5Ouz1;I6zpO#w78Otm%Dvp^Kn>)kVx!aFTGU$N~D($HqYoui#v%tPiVyQ+KOAuwGTOriQ3p zUGu3bxN1~&X_cZnr+QzFsM=XOwsu1O%et?P!y5l53>8L;&xkIFl0`!#GBLUNtduO< z-(r-VkX6Vx%MU6iEA6U#ssL@PW(=kYT*D9jT;yzLTYasBt;{+YJ*UZ7=jFHtA33ig zJ*L$$0JO1_V2iFJZy@g?Uq@AVoP3YGlYE+Vkkpr)PHszbC8dI^))l_jQt~wIABQQcRK4YG^y?d+1%5sZ2gQ zh%H5?8`mYoWv9y*moDy;knYR(NcCC7?Mr|2->olxKJhC@Aibx=K zqTi72=;7cxIF4VKsLVzmU@3B{LdZd6PpUU{J#<%JX&-6Pv^-ieEt_VfIce``^|Y;w z!wfZp#oWQ!3yMi!ZZMiCUfn&n zdv5it^*ra%#q#6ut7Tx^cQj{e69&A;C~&dS+UN?y$o0%(4L) zlT`aB+e}ndO;}6KI2FqrMGm!7?Cj!9!7Ch$Zn_Tri(q_R<&fD0b}w*A38%vG7|CCM z(a%l;m)YB?b_{pqID!xWxy!!P?u|O_I9Q>xzJMh=5lU&t(B4q&chUPb=N05~(ALr3bz?Z(}XT=Kiz zR_;PD`Umot;c<+Ai(kY)jhyZz{w)4f{vCcDm&x19{m9Knw&!>B_y4lwtRc*SOd5mE z@TVuzMKmg1Pw_$RaDY0DvJoi=XDI#1{mEZQ^&}n<2TO?5-p*mMAF^(-zOx>~iv7(Z z##3*pv|O>Cwsyl@xsx^BIt|({57hoNWCm}u9kBIArqE#Yz3XfrKrUKiE4O~Mo<=Y1 zH%LiA3u_)A zL+ES@i8t7nCz5)Q&LX8g2G{v@%3Vq|=6$bd6*Lwt6&l_l^ae}_vliD}XA{d5fqnL;;gx{}EkPFMI*T!PFly>F z3CJexr3zO~K)>EwQP8?wF;?+Ven?JM{QOVx+&WFcRV;5!Z|w&zXJ2TcU$(Al9jtId z!SqAEO_8c7lW&txf8J)A!x}~T? zj~k{KQ;ip~w;VDbGiR86%~Nr%dSp3eNwP)T9$EibAK9VZPPmo0bw?J}Xs<0^@4YNuzkL(@eErA!=LH=N$_>60OoYw}9Rnt8n}}-> zyP{*;o@g5%_7N3BeQSTwe)jc_I6s6y0}9~N<;@?hyCqx+Gi!I6Ki6R#66FB7`rGcB6@uoExb=a zNr1*{fcG!AJolUMrATr5Z%c#`Ba!`ZS+_$sM>SG)NO5^1?E?Kcy*D&uF32?bWiG*=$>u4j&wg90aI#-* zCEHZiU-l~dBw`kUZncwvbBh?YM=X&})IvAGvy<%aZ1vEg_*$ziS_@(`pkCN#vKrV% zUrasQS@;%`)oOWf`)oUndc1~k5VLXb7m<{>-#Mh0;L$rMp43ZVe_5#QXM)f+sx*3PC-R=g;B+5riZnKeeIaj$+eCbX&gX3Zy!1NRMEV}w(Cle!)9GLA#v-b>RFWPyX~2b$fQ@9Jx6 zjpm8QrY=xNXqZT7*o@?wpK4bfUH1sN>YWT`z1;A_;9}O9Ji&K0nH$YAs{qt!=pE3} zt;Y&`1v621t0zd>nbvz)Gkh$aEL}nJ-hm#-ePri^ps)VJ`pKGXvm*V51nT%%+bru! zYbE*|E3EUaXKW{JuTUoh*!gyvy~S2;yJKsCs$~b*9xm9imLOOC2I)DeA9*-=DrGm4 z$vM=4w86BonB_(>k{PR6XVE>}%sRv_#gx~^_T+Tryyd7lQQVQ-3Z9Oq*lPAM-1{M{c-ABIxF$0eF&5Bg;_*LvEVUiA3#~m(g?X}(#2}}T`jJe8 z9w#0EFR+$RCeI=V;e2(FypKEs)vGr-jC=>{eQ)$GPT~A~nf#Fa5zPHUaxwV}w3c(w z+2~IlL*-GusJXN;v@6h^_N6bS(~R5C4zI+$7wjY~aPl;6lrWHpq^v7|nvyUt&za4MW@ z9V>8hn&$Y82|<&s#TE;0d>`{vbCF44S`3ZiOw7Xe<1XMrA?7et8vLOjb~QE`b{e`H z+>Is!1Nxo)#(l>5$bYy5F3$tQR?v+j3^IMGUZQ`g4>fo}8JPpldIl6efu_^ut>%yB zBJ&#bX75_AS$ct6QHJ&41+)U+@iUyntYSavrSV`3i&2Xeq0a4r^`(JO68A{&NxdjT zC=FyfC679VdKE0;wX`!dIhBiD=soQX?K7<(eFPji74+{k8Epl!T|ALLb%1_>ewy}& zcAowT@BK=ARzM#IE}9Pe{y4@O#yXtO??ID39@CDKw9B-+lorYpsDeI_uae)BN=Uf) z$PS{Bn1_DuYMgxn@%ehva#A$egS?TnAE&Ner2eEh(s`Vpl1Pc94nz>Klax=|KrALI zNCKpQkjT0Cxz~Y!tiqahjD8n8+Xeaq@V`4C8*4u5G&{0^w{UN8|Hpg5YvRT8H@SXx z-Q)JsEfF5dQ=Ssfbv_?`j`vdGCEQ6^mZ(d7n=n4HUE-C*`n;&;uB+ZKs`xZs{nHCbHo< z4QTo$c`VKqXNqe?#{@BgDe!=Of&ZYPazN$Ia%Y*mtVda9`I|DLVtTn><&lc-c`c4H1288Hg0PCAf6~bD#?(j(R&@-Y?kgr&C^rms;UBi zccLyv7la(LQHEf{7u_D+WBnj~F4S#DpoRpqGjUD@=#$n%>^o<}}OoF$ylzud*XtApAl&KhJri!N7^Da6D!xEsWJ z5oq6H9aFE=CF{NP$93~`_0X5x!7TfbezJa=E?M_Lw?bD9=FNJ2H+?;3$+AU4*YpV!+WF3T&O~Q#=QLskl6xOHJ#g0l;=Jp$5^AD^R8E>m z8i7Q_0@8i>f+nE`--c>jMNmm8I6JrzTueTMgwCmSu7_hT&>4rn??S9VM>>n}C*~m! zXeRQKStJhWzpRKC#8=R8dk}L;n@KLQvP~f{cW*`IZ5!SCu z$amU}{dO1Di|2TqJcwYTnrOzW_yRo@Aema+%Kw#Dx@?~ejvA`7j-03O@2~C zse#l!ApF`XWa@azOo}hnovNgSpjSMI>Pt;PI@tfvIc~;YIG*;ERz&l_X}^x90_n*g zX_Eo;w#e>Cqj#fYYKZmp4b4L5;Z-U3M5B!eT;YJ-mpF`h9zxMyv=_lG>&_ETm z-PD8B)6f}Y($Z+2GzRS#G{fR} zZCDPKZcJ zHQT9`s%z?5>SM_39IEz3LfJCa2DOhmMP*QaQms%ORZUSDl`oVc)fUxpc=*FrWlDc# zz2cT~q%s^nv)TBXi;|^eE99-ml>OoAI;mK$ic^hNrK{$s`>WTe)1Y0Rqu#9P1t#; zZN16QTx%Ky4PT*`!qGN=Tb=!~eFE515l*GUiM1h$OeYH{KPgASl6t~8&bY{! z%NWn^&%ueHF@s_wAg??a$+e43Hk1)@9o>P2|`aJTf_sR16$FJ6}9n!iy0u%vB zf&2Ym`9Jmd4fyPn=)1}Lh4(9POZR(T_N?6|91WLA?zGG3&Cvg&i= zWdU1ovEEqsp{`?Ha{Yxy5-w3m_euHk z-?FEzi(9WMhbYC$1In&=PuFWFY9GVD_@Dm1o3*#~iTw*!U54FiSHMBI#@^qNfgaH| z$9?3v?I2bXS)^g0comTPILA02J4>8dj*gBm_P=(so$c6+Jeq9uXZxY|PP4n&gRN0k zKdaO7&T`k%8yVgVoX6P67HoqKR;lB)<1_ZIXeZ6tlgJ{*kOq+MK?7i-M$%r>AJG@H z_Odvv5LP#2kOX3VbmxuZ_2*yY4?wnTKewB18{Nv?-nhGXG`S6QZ+1`jNOeEv-rr-E zht6ZN=T@*D+Mvfb8Lp+xZcp9h$mnq6r}GV5e_lTK5_cx&9_Kczp4A5@{+VDzeW7dU z=c$!Qx4(k5Y8GV_vXimz-#bsD zlQsmspE^{pl~_^t5f_NVq<={JFc;U5RHOu)6?o91ZeNFjBky31wLeM-Tsln)s zCQ&z_vbzIsVkKEhuBAvQBanZzi+qT@4?O1g(7*IY*Xg(8kK?uTq0<#vMKWYAZO2rZ zV)L}sSdB

;VVQ7wbIBbW0D+qhBJ^gTTalgL#*k1Qn|X>ZHTwmF5GOd*oT#SgN5D zj0|-L33J@uQ{zAt0pueb+PKI zYJ@r-%F!FDGz~}dMfFLQj5ST9x}v%a|3HiCrs|S%hVr~>jcU7coN}l-Kpm@UR&Gab z@_W@@6D4N&mGDCp2yCntg0lr0y_rnLIB z+GHPO?A90ZW3s+78?uQm$R^8{$=b+_vh%Vz@Omx5+}zx9p=GChpnSKigDgqKfF-gZ+5cK5v>b)k*Mxl9mGbuBotR{uTp>FmTO~|PgW+oEIZdSy=9-Qm+UckQ3vsF8)XkFKsRP{XjAn&YE4?GVC0#D1gD2Kg z%9XBeN@^;W?vajaYLtY-NpoD>U7RTSEw+f4i)G?X;#=Z*;u~P&z7h`=Gej4JD@0+U znL>tep@;)ARu5r<=(>=D=S^Wh;bx%&+Ji0)NsT`nVjErvQX4-ubZn3dIE`-_iW)XG zENLJb_TfA01!;ml4b>p%D(kN_WH+1@bP*^8JCIWHq~2J+xL#You)aG9ztcrpVJEDGJw&gBPSH-0UUW%Bm0T9j5Oc(A z$p^7YbWIc`euWfIjwnv_Ryani5xo;F7llA?;S^sK*Gi^INbsjbHwh$eU@4bN=1T4~ z?QghR(CVkU`CMmeFwM{#lo;K$<@06xWOPY^1vs->O50WWc zj>wkFT3ZTQ+Q~{=nvt4fg`Ox~<}I5lZ-yGOwNO6 zu}uC%E@-{qIskpwWJO)8R&i6oRZ12A$I)4UM|Evcc;?D@l8F*XkRZXG6n81^TD)kn zLeT=nU5XbiF2!Ao6?Z6BTtajvHj{CEf8NK}#wIs&&popD+H0--0bN*H?)Kc;^d7F0 z7m~NcGR-pFTHm^bjsvFr>!|K^`A2Q*h{@A!-N=)-p>MIlo}5o!?IN!(sCKUOZ19|f zL;ffAob`b%0dw$O@B{uXdZ{Qeol&Yp{iU?rMh>dO=!nx-tJ1gC$Ab`0nDDQf592NYt(eR(% zfCh%brJM(EVXS?my(M~tXK0JpfgARM7dMd?`lmqNRQPDP#bYuFLE$5lgTg(^aN zgftAD61odE;t2gc{Slo__eyIZ+JB+j4W`@{WNQfhg7P%AwF%@}bBQ~0z^r>{)@b_R z*{jo*()x5s`VCY}iqJ2y4>{bX5Ld|V&=ioG!C?o`h8#A!jh9eW?u`gU^ou+Z`5-bp zDkZ8~bjg@$F$1EHNAHi33Ta|S#;l1k$85nj{Ze$1nCDTYqPs_Jh{}&_7S%4|M8xcf zyAjunLU?p|@9;@s_rn^8Z3)|i-g#q)BV;0dk4CROlmAQZ}S2pjEC7ZVDzU8Ys#HV}s=sp^6T0 zkLCtv2EPPv2Ak4@=eOY2;LmXCOo4Y)#WMYU$uBPv-<+m*N`+vVpem>gJ_{Z}Yh5U~ zEBHH^XEAgJhZT9jyx@f3NUl&bc!R#76F@L$2Koigq058C!Ec`qjPVclPk^PT;;dZf z`vVo^GT&LxHO~}y*el#a-6yCY{@`xm&T#$ZYU>&d!!6WR-Wlf{NRNq9jxfh~dndBu zZuT4br}8`7s=^3ckbfk95)6Q$bV<&z-pap{UlAm?z(-?)b*%Lb$nFcvVM|TA)P1wu zwuGRQyaaDousq2Nu_jwi=Pk$!x4yMZre|nVd?^ZB9O(ZV=H1KNop*;meCPAV<&DX0 zL_ByAht-`q7jh=%DB(D^%U0oSQ!)E!*66IEd^B?o4b4i-s$_m`T54`#)|>w|9W?)7 zzHVM+4mUqGjWWlYkK${25O0c&nLlKHGOagl&8(a`(Nxq_ntd@?)AWRhPOlrGJX~+o#lFsTp`Gyht0Ib}+Si z>iD$qv`MKEsn1edrFOuNBgpApEB$3!+q93V8+kXE)2614Nsqzt`ZxYkkJQ>}x9G&N zH)UyBwKQYe1^h#rq#eS)H#F@CKf93HEbVjZNxr8~+nQQ6{cl_hR@0TBBj@9cl&q9d zDV^}Tew{KP<=@m1sTES6q{wOOaJm|pQYCdO{E@;iNbY=FlUj*8-H{TPwmJ0-_w!q7 zBzJf|Wm=jwwPBhiHNg2>DfJrn^XXe5cqemGRjCKkTBeOj6VvXej!Ny5T7us{o^q02 z1j*^E(-&t%W)x#AR?!3Ue){zEBk2{>&1eOaGA^h8lIh6!24A5DanSS3DVgtaN116Z zOKeor)CRA)H^fb?>0J<(eI#o{wj%plR+p^p*-6>8bN``Z=DGEbkKi-JWCob`A~KEZZpCLHi%}R`3Om*r(W2sJ$Em zwZ89M;jH1zcKq$0=pNvz?RxB9?cMuJSCl!$v2o#+yD zMz}2`fyWf|bt){YkMQCC)m(^d?`!$a=CF!Epo;v$a+DY0mnkkxpv`@6- zG;1}r!1sp0pfvFBx%#>KhUz(8>{hDGl0&M1?zElMj4Z5}yk6Wcc7dz&QSlheFCV^u zKTrc+YgMwiOWwb{wWziSJa;^U=;i#tbrAM!1J`BvzMY+k&K{thwOr9ua+6$1_*{>o z55-I8NoO;c;JSydVxGIZyNb66+~rxGDrgA)AU~<@)p#8+iuP0Ie}tNCIh?H0{u2Jv zzFWR%e@XhBCc-N&HWMP?Ef4(x)JVv?&t9J-h*Qf2Fbia@5f9y zYCpTXx|?_mo@#ELXm2Igng>UF4x0IMFuM%_DRA1q*B|eX zqTlT(|9JXS{^MWbuRx}^**}Y@r-%OqXy^erXRmw@eXYpLzvJpIznu=+cl_4^ivnHX zg!H4gW8YxW;9+{=8WiT>{9sRT&*5-wdXQ%iRXhmZ5B8=HLn-n!2!4gV5U==7 zSzCFY--=Y|6f@D+O;-$5~{s;c%`i;xRjf_XDk&&b?2)k9|9M zCQs4stny{!U0sj-@`Y!==Z*WGySO(D6d=cAfm=G4T7EC`)qP~FC&(AWy?XB;w3Z`% zm3*6d=GXX%=zZ(4dY*e4k=eH9I*%CB)2T|?HpmWzu=V(V4M@dJ% zeUziVBhTq^E^>5p{O#BYuD{z}A3di?wQ0L;j_o05_`mcw?Q5@LZ(|c}Lu`?@kJgpe z&G|p&huNOwAF&R$9%a8sroY?~TVLB_1u`ekGW}+V2|vDiAy z8q4d~5LX||U7kA!+|Zo&J}=hl!ojPMxIK#q=b&w&Z7q0d2sM;T zutAb+QMUcyrrQ}8y~9t{y9^bUs`m2u;%u?cw5Qr1*w=#hm1nf|4v)Q~qq)Ok`)2E9 zuVSBTYi_IHaNsoX+AdIocw*mg??m2H(N)vc1;yNM-z(2z-B$Ih`Pu-LcOHzd$E z@H+4|FrT`fHyC8yPY+HZ_U)h;MjlgE*hSxj9IAbLl&6&4#Af1Z`T^E~q0~-jFPv6B zS9TTV37?g(l;dDreH1{5~HKq`~5E zV!EIfPm5Q@r@|xQ1Q^y#aH|F2>MkJ>#H*QDN9-;R7P<&Wg=@lV#cRb!`iq}L(bX3H z(k5{Cf0ce^BcX*bn*HY^1665IcUG(Yv z3I0#0aDK-3BZIotEXMCOdL*C=U<82QH2?$s+F^&p)0Yz|)#~I>H=z zhz~?rSh|OBEScwBk47)T>%z69h36`@s_ig?qr6J*8sQ52Fu! zEimQL{xR^9d;4ple_u@3g;sO~b_ArL!>5fA>+^hKRQ&&Rsd zlzx%-$c?CjAJV?i%DM_@Jeq+xEFixup_TOI^q+MhddU!H*r|IJ@q(T`6X3lsPIUx?gmMnDsHeW4FXAV!Ou17g<^4Op&Cx zH*q!NYs77in;7SgyB}YxXqTcJijFPzZ?WRV))#wKqE5p563r7zmE2V_H{th^+e#`* z9i~5=vecYXfl>!b4=QOYS*_%?l23~_C=n?3Q*md}g~iqui!HvTsK4l+g>^;V6lxW_ zIp%AODW*}O;gL@xmxn(MZ)IF&RMR(cU+A|`zplN$pKgJ!lXj!F45P9^byKwjP2D*Z z(j(=cWSPA9J!|a?E7&N-Nn_z0M94|<9_g&~2W*0e>RfdR_-}XBY3gR$KH5-SWt~GG zXYlIF8*b>5VT#nzHzP0qm0nWK^rQ9P89KoN_-be!GA3l2VVj|+evG~}-)m@SVz`RG zSZzZ$(21UgbbXY;rI!u+>F;@8^Atp24Hwov=K(YB$^G^$fVq=JfJ)c z_q4XcsHmz?D|)eFjX^PJKqFKm5XZkatnC*5z3hQyI6n*lf%q)f_s;)-Q{xwuF5#dM z75wvjLwtMb#q!0M?%Nz#5f}+}zt2C=Ux!%UO`Q6DpgLT+2;%zEeyjI|w~*ffQ)?Bm zcBn7G>%`0A56>3w&)zGbgr_*$8o71uQ|_5Ki-Z$lC%I?1y1-9p=UhhqQk%YpZCp{V zwbYcpQHgtuE@m?x>1CV_m^qK_R>v*JF!X*y$PRWn7CTx~yQ^xq*$jB^C>@XN%j}ix z6VSY0C4LedcX7s8Y@bHIFr72mfeO!2)D=K67;tQME_Nn3((Gd$wH=-Cn0slzXdeR8 zCjh6qx4oJ@6UUMz_8#`jwyyB4+u&<5*Y?0R-8KwO^(c z;~cf#!_I*y8s9rg5-+^9pR*^TW%=gVq_i*=gVpxOwGgmM>ySN2BUVe2|A*M}q=eoAJescN1 ze2uPTr^fx*)!3zXtwP`4A3ks%?k6AIXQ`WBcSU+`yYq=_9+BgC-9$H@-9+W5JkdU< zw=+3TJ+G1I{T}+5PVl8R`+DKG`I4Ng2+Ewzp3$DJ-g5A=EBXp@pFQ4j-ep^+@^5Bk2R?v&$u;alME(4;dG2cHO&zSvzVko6n^Z8fuiPg;VqbR0k z^158#6He~2zJ+ka!a>lh{=Fbm|2_(`d=or}FgTSG{ ztYG(GI~ep=&?634v{S?>gNo1eDVeJ1PUk(iAqs_}1AX_dDt0SY!|NWe?8Exoq`aow z#%Z@oSwpBSe1JQVqI{!lt0<>^=fQEE7VH)50~#4eW>$vx+69brCHFjmTC9%T{3o>Yu0Tq_0D{>*&>l6` zPXRX-%icsmgZzoamjiq&(H)5alfNl=Iy}{gh4bHjUJ;{C8`-iuk zr;O*hdz-tGyBPB+-*uBcq#N^MH=NRy_!C|AEGHfsfpfz~cN;WM>zKDU-RH>K+Pj;2 zjGij)qV5&$MsC0Bx$7<-5LLL&M3@)V+{NARKoSaE3~R%=n1NSZANHSRnC!bz-E~Ga zw%pyu-3M;pao1*C7;3rtxC-OMQPuSaD1_a0$yEfc?|2lg)!3^NskgTPo0yII_<(DJ zE6x?+y5^b!hEc}3*EQSK$yL_%&3VU}&HAe1ayh5D2e|#tSFA~u>!4${V=aF_iZztx zyy0BOI;-g{=A7r8;!JXcP-A)KypPAuTLT!SPk}evy!X7HydG~qbdS5qLE?x~fEF5iQ*eCMyoYSjAMF=yff0sm&|{BE4NJM;aP z)cb35&KOZLo+AE^3y>4I{#e%0RC1DpWwUeJ??$uJ5J8?k*vR- zFNtfsBn}_t8|AZm)ZWkhdjZ4y1-X@t_gsp2Je5e-<#Brcgo7M{r{Hepo&$dJYVR^{ zC0}JL z_^m8q4UF^l|37N69YkW5w>O<1OL(L4Woqwj$@{s?m1mM0tRxrM;jPPitI2pTL$@#Z zvb_bGKZ7raD>U>rL?PZ3d@BX-sH5J+-u-A6)m&!|xW_SSb88v9@_0&>f~76;dwW4t z+IU)f{_$?|&Vj$Wm#15mr@M~7Hk7YZ8P|e*(&W|q64-aQk*&UjJAdDMo0(A_-ga4E z7jJWKn%Cle$l6`X-5l{8^sMks_nzeAX;$e~&t?#{GM*aVX5Ms9uIC(oOM+9do%NgV z&TxMt$2GfOyPt!p90V^}1ovh(=!wl~b{^$>^ZC3OTFtt66vnZdYq`6)pR%4-<3e^3 zZTU6U=p6RzU@bQeft^PJev)_*N?U%ww?AloW`B(w>afx;)x1&ExJ;heU*KL{ZGey$3>i4 zKH=~Biu1g^Bhk^&QIyU@yX{Zy=k48WWo@k-wHzgh5uV!j*p2kEbLJn*pNkKKC;uLG z!#%JT_u69ZpKY^=8LHZ1Z5yl$tx5R_ILJP<&ZQ=^E`Le>V{~9S)~nWdaFR6ZOKW#a zSxa}zcb4jT0*K5{ATsH>UMev8mirbFtR_G2Q{JVVWjTYgt7iXVDrDMknqb-ojxC#R zWNyr8m=VnQJ7bG^rn#Cqi>gN(Q$Ktdy3{t_ zw%Iz{`W#&Qyk)(mq}5?5j&5x_jI9Q_DPZCK@;0E2{!G8By5Qtjb9U#v&fS@N0(|=! z*wYJo^R2b?w|um2grPOn`o(hB63Xv4hxax-zgvD9I)sU!Pk+$o%7?eW99uKnY8>Ty z+Ku)sJ8>bA;_uF(&SQ?1Ft$fKHZYG?y9a~TRU~Jg;@;dQe}{FedcR&;=oP zLUx8Ght{T#+&|%w5g8Flk_MMOuiCSrjPjXj83Qs)W`2SHHP_tCd=x*f^SSeLFIpy8;_yTC&~s4*$6f=@ z{e<}Q2w3?Zyb)K!alGU>;n-7up+8Dfr1fxc>%$|Rt5~ck&@?Xc{p{Q8 zyX&jZ{@;K&^9H+`=(Bjo`^Nb0z((rrY3TWtb9cRK7x!_9y=0H0I;d?0d=5X`p4ndD zyET+PLQ!b=f3~%^J!C(4W8X_`-+&R{>xe_Kp6Pst_r*+in(MLaH_sYQ)K#8)p#Hr$ zNsf~TH7DkE;Ia3W{ci`JQ5T#KoXvuTZp zO5X4dKY&T@LGEYt3{UZX;N1M#7fSB7iqUX@GESuiqVoAU`5Jm_`*hSLW_uyTaGp6m z4!7U^J$P0pya*FLanv=+xvP3gdk(q(a94&2lH_jYp6Tr5v^x*dX>x~SptG*CH~Zl; z=0PDa)_IQhWNZT+r|Ijv!!d|`c)R_eb0Z3j4~_`(iCNCxxI>+Bo^ma94aaS=Kh>XR zc*8a)D{Mudgw9mDnsL3UbORoNwrizrnC&Ng4y)xy=Lhp|<@W}c-e#L@`vg|J9PUal zYeQ>AQ06*tH}>cLlG{5kng3VI{gk^a_ef5koa;FY=x&gdvpKsfTEwzBzhzg>Ud7yd zoINpnX-?gogW03At=W6v$xh1F=KPucIcIs!%j_xHnK^TEGV!=e%UPLoDZ69#P32V2d-8t^HbBGbReB;PB6bS|7ae`etQ_+)c`MgxXYOxuXYQdEex27eHor6VGUu4Kb4qM8S2d3_3uZglZDz{K?8oV#;GOQr zS6YD|>IANH!raUJl$v^Y)=jSNGHuTqn5E5nYTlXEGOH@RuKz`kmzA|Y>sHq6tTfIQ zMb;IwCi{F=p81%$At%Y5thrgXtixFovW+kW_QDaUo4pE-+hWGwNf*SfIr^M3aFllE zbjrDrGa=_`PQRR}+;=&B(TNV{S>zhATUn1-I$PdZ|FTZEMqBS%*I66p2RLyD zS?A?9q1Le88f|;Ve2U30ZwusKL|b~sw#+8m-@<`&RUB~^$hYv_ivZ-?ZL;lm? zFZsR~{!9Mf$-VC3*fbp`@Fp}zX@Q%8BluYE#EEYRz42avy!wK!U?04p@)Y@Ghl%h^ z7vPcbhoXQxx)%=2YJ3*zfT`>8v~EpbAX%s+tP=N#-Gxs0=zO4K%4l&l-YjeydrR(Sh zIZw)#Hp1B71I8ud67mBcfo-Wkua!5+o#bBfS6QW+D9@GmsUE97;i&mZWmg4dy-HTc ztN%mq6sszR#>s<*Hj(PHUA2#1MKjPkyJaIN+zQou*)P}P-+Fns`~zBn!Faz7kw)SI znT2*PmUq`iDk8O%{)M+~M{}?i|Ab_!~eiTE3h;+fqD-lBprj0}Ds z3iFG>6?l2=N4K&ka3HWfxFa|&I0%-}3979>2UntBT^w8n%HEXRz8YGDsNggF$%Y0y zqU*>HHpOXHfPr*dag@wIIrtwa!8UM>CEyG`yigy~8^Xl=m`;s11P;_B^etw^JH-ar zPTSzlx%uf4#cH%Lm#Gf*q4R@A*$7mz68E(bElE|Ox=>wN7iWoNpzlG+glR1=dwa~!^mqmrfM2JwFKL(rdNoa)I-YWGrwNoyl5?D)?kc7~a zMw9Vxyyyp`N#7muQcu9oZ`S@95XDy$f$H z;xDo@u6TTr_!aST+_1RiasIdpaa-e-$Lou3i%W=4jT;c(Gww^AuE^>lOACK0+_KQt zLJbO?D6}l*e$3q>VqA;(^YLTj)8k{~m&bQ3x~J%X;;!O1i?%4HE4H{;T=CV#_mnDM z`cav#iE(9*lvS4BQGO>Ka_d#euhOeZ^7SZ`udtM9hr4LP-(s(hE?r;A&nhsG>2JTYFop?Z+-3^`G{AJpZ-Z*9V_xe;)U#?5DY(;y#W1V*Ik?bJFKZA2V?AN&3+7 zqx3O8<<_?&^f@SOO3i#mkLWI$ip;vX&YS`H-Sh9-AK43Hcm=hBtMY5Pjru3`GS*VM z;-O-{cvk!%y_6!Q8q&|;qJQB6F@zqb2Z)cq+P>KQdDrr;SQc6i&~s)zJsHZ-S9~}c z?)mf`ND^8LZE&5Ks@|@?hu2zVSg&s)%SJVdxf#>H(1AkDV{gZ5aLcV9e;NnP-1r{F zZWQZK>_M>x38xYYm0Vr2d&yJO!iJVyTOv9kBVlDpZNje!o)WVXUKjnL*x|S$@zJqY zV$Vm`jJg?GJ*=5_y7r9fKUFLK=5sj64ZK~wuU*Gn)$l`YORhc=Z0#OiQBNJG9QTMz zr{j&?j<|Ck`QR4ZvI=<$QLVj-E8lf@F+NXr*<4-Vet2Bb?s;Gi2c3U9GeF!0`d;;M z{N(5b6QIAnp8cj{2|2<}m>n$~qu_IR^55i#*sS^Gi9_0;OPZ3?Jtr*ZRrbd0CFrjr zvL(DM>Y~FkTKG2x*3}Nnrw}z%_1{x zLSOAloU@0d`_eI<=RV~z<;~z%>f*b>+#dUH;9_;tdx<*DT~CVVF#f+|@G|d%Pf8x0 zq3+Ndsv1acxX+09tbVX&Xco(Y~rSAr{y=zc3z znXN=h&26=9HDPz0M>F;dovGhj4uM`A%}dXFl6O9Db>5!5fq5n2ow{-_=l%_jkV`DL z$x^{m#QMk*OP;kdzi<99>M?Whk-g+t?>Nic>+de(9!q^{wtt$xIH%tRI1A5|x0FeE z0k$DtnM~~QMf8eus3VV+R#16s!ufDXEHB2Oneht)sW}&%(N={Azo{$AlFHK5q35B7 zo{p1HfrhiP@F(bfyjVmWDhv}Iacn9!%=Se%H z2DnYPl=@4>Q8Lbu&clE2BA4guRd}d16o-hJC?OlNA6{0bDFd*k-cy;YhNn!)|NAh% z^#1F9?kwKH2;njn;kPr86F1pTD1>u7(Unu#oXY|U}N-6ce&Evys8-* z(4VPO80g8+nA5ER@zpiX+$0o~Eu@}OQMmt`JQf>>P&z%U#g>f_d}0hy!&~aX)#ZN7l{)fRxuY~$n#8y*!}m~+8dfQVN>6YeC@ zx(mrd7V7FHL_#m9{w~KC?H3_LtRc3<8>2oeVHS0$3BnrTU*>!dUUycwFZ5InRqhpT z;xw^; zT_sAZiRCV`uQd_J^0#`3jhRDF1gr3u=%EWtZ?PQn#w&E?3nf@e2%S+@hFnf2% zUnQ4xSyrfOO6#P};GR41lpc#`#ZmIP&r+ytkn79$rF`j%<}ADyZG!O z^SgnxmT?;{9QGIbABzmwD-@tv8WK9A?o7gaqJx}h2co?8>u zch!efW_Ths$z6M@hpF$V$EmldN2?C0V%0;``|wITg!5A~)n;DPL2Xslpudkn-IHDu zL-DM5ppMiGhhIIBD^Gy$5~6Oa4)PAxsNUfk6{l`R-W!GMOs<@uny-FE&jUeSl_>wP zO2zAMsLH^D`AFtmpboms_cp6vs)wi!sJf_jz*gz4D)0zff``XzaO)}R7wY}0Tvb=z zUl06i&hWQat5Q^>)u;K{W3`cM-BK6U)KCvlm(z6Byi-M^=o+LsuKoZcsIumfI!Y6- znFR~ygGSOc)O4Wd^C^`^JxX(1vqb$`y%|lEUtI-1t3{d@XqTciJ$O*4(_{H+PmHdXJ#&8EQHEEY%U z`eZYs)eBYk@XuPI?yK61!oR8dnN&n>PJVnb-uDqT=0MFH zO(;25chyi;N#S$H61$bRm5ZqTH^+bbK3UL2y{7xW3{! z(8KxGcHh?6R>~$&VR=Mv!Xr`box zFkjm0Cg|$%7b3#Cgw-)lHV#5}cQ#xdu^{4o#K_2-k&mM)N56|nD&&rCLR^2UP?JK( zp&pEiyQ^mP?XPO_ zNCdjn>08%ZH?!X9`qvsJSwfruk_M#dMYi_F6srvTvgDSi)I=xtu@u2Z0&P?kI@O;w=)0UJ8DZzL9 z-sQji?WN<@#8>~k{{Hpje+T^={cz-i(NESr?)v8ae<@$Kei@w{k=*H<`dh>F@bvOo zAI0Q$&W;*BgDn9BqOE9n&FyyA!|aS zq9;YqEu39=PP`@FUt&f=QsVB!u@$2$b*&syrDOHT8b8!3pR}M}&-#-XkFgE9Hmq7N zrvAh4w$*-B>q^qGD$}ZdF1M`wvJ#FGfg=@r_L9KJ8O#RxdS z#la3|f-8Oq*wM0w1s?{U2lm707=sog)Sd6@;A!vq50=nT;?(o5b@bf0iK3&Bv!$(q ztxA4aeu18POWttmn2Nkxx%+a3yt}v+G|X+9S0gVe*O+?+H;O}9i?dc`jm+w8u4vYq zGBbamTXxUXQmNHav#3?w`1UdNRO+O(`e}b<4$AD5Bjs$t;h`sdCjq^HC8$(v=RByV z9j)yg@^i?Wh%%9X;q%<5$o(QmiW-Y8E*?{2Y=S#sNr{kzPYDxCE-KL?p;(DiCA>wK z7i$%t6>p5&7UxQ9kH8Y!szngDYSro0j8LKg_2N1HjiE#y)?=mwK*a@@<@1% z2z}V3u+4a@CgaMt-0;RQiteUYbdtV8ib>siW1aJHi(X(Y%u@sGi9pJii)p z8mDtC=kOx1j@!Y)it!-2b$tDN=`PWIk?d5-SY4w=x+iOBR*%d^ndj2?q@PP!nNl~k zTx#7E&$qj&D^kPLa#B~JFN-iaGjE{yK9sXRCl@u|u>1!3?`$`0)mp4v zNcbn>(ir+4-=|ygF!uLj^vCgN8lx9&sI97-i>INIZY+Q4KX5OVLWYMN2=&7n4Gn8y zTxmQX<_bG*%rvetrW=AX1aqCYX#P1@uNBkUq2d}k$Mmh|jHXbNc) zON4{gUZK$n@t&FV}{&ny1vTI^3L#?N$^-s$_iR zAob`etg&>fl>gcK#9H6h()Kd{X?`yJ{+qUgw!g^LOVdTGr>l{x5Kh_W-8bCz@vbO| zS942vjYIkG@4%zLQ)1YWin)r`#H_QaDelC9crZCz3b^*a_}e5(t)wy1D59gzplP*L zeO14!3Tv99oD#Km{XoM*gBVgVY)ROD#>EicGdwh64BboXM5ab|kD3zsCDInvH##S> zMpU7w5mCy>0g*2vDn<5)SRXOp=rrC6i!i`6_r*{xma4Ovs?&H9kf*!{V>NXq4jc@+2Kohh@=Y4N{9-N^Mk=M7Ts&@%qOj9tC zNpN5+UIRU7^F7J9?}=azm#7E!#edJ@&G*W%?LNQ&sfUgz)LWgt1KoYc@ah{&kBIs( zZ`RVMa~&+uT$t#W*q_RwEbs;kfhs>#7=&iRIoy0$!sibJa0mE3$1UEVH=pTw1T6D5<$-^aITv@lUvq*zX;xt)rE!KJ~Afn+Ku zCj$@gvycK60%vh`i$z~G+cy>Wi$lJ#-i7dS8o?O)<{iW5z5FBn_weJM5||eVh38Z; zSef3Adc_;C*N5bQxA9KfD()px?!I>Lg7K?NqHq zUxI&iK@go}+|=IbKI#h_@^vNk4t;e)guXSNxgTLouQIeXOf4Ss;nG(pWvTt+{uGYg^? zbvMSBj8HSMqI~d<8}wWGD0~qFAw7(8aP03@_%NGLI zQIOT)?$@GYUXHH7j;5?*pd=Z0Y_J(4x+GW$r|nAgcQ)WF*IqH1-1xpy4exO(o+W}1 zM^^Zo(43s6E&0wY;{Tg?*PW7I$Zv6)TaI7tKltDsq2HPanpH>rqq?K!7mZO}Tm6(? zQcYlb?*RMFhLyfZyH6X`*49;HF08|C_%uDco9QR&r{UCC6hD}3U0wZJyoBq)Ro|qu zgPTp!zr)q_58Wf(2d!Q=M7vy@OZrwB2ZLD$_e4UIb53vx&qcMQ7L&wB)HT+K zPsA{MAdg8}vgEgnOo+-RmrxaW5V&MwPxasUUHzjfM2v7p?N(PK_I#u>Oy9nBrlgMQz!W#8sMgPm4ZSCp6e{JA^ARz zhvHfM)vMqV_FfSU+IEh3^BO+KF2w@nS>;H2edK`jn^59k<+Bta!FjhpC3CD#<6sZ&a04 zvs6P_xm8t@Rb8pct|sm%#(w;UdTlB64Nc`axU|ogccCFJjp8AKll3fkOnv%1T0S?#HGqFYHiyaaAo0pSrZtP3PQjWesI&url6k zWtDFgNy-Iya8ID0NKw$bc6j}pgQXRx(6K#G_!O;_tLY^-RH?!9x;;3}BV|CDArujR z5GDzu`MYDlAwLQR@d#cfM;VtL(iyz3(JX>z!+U<&QQiC%+v{ta#* z%U#(oBDvSbeEnIjtQxKwO`n$Dc+l12b-h(vR6Ed)6y#Taxrb^KweGia8P!zAaRB34 zS)K@HT3;2;eCYtrQt%9VgQgti8TiFUoFYTy!i?2H*+M@iExla_$cMp08cQQklsu76 z@_v7m_R=5gJ5ftLvNn6wGoiHDh#Klnu)puAnl=>H32%g0@u-k3v=vu~=LL^Yhuq?o zSXL^)k{*gW$tE-bZE7Ta5o(jchDiT`<-7t{nk5|u*O77Ri;=y!m>#1V8W3A>dSv6@ z7cIXBhgpE1-dU=p7g+_v!Lau6wTDsa%IQ~vYQ4;}56XR270Ge;f?e$8=N0iiRnj%Y z%Lo<2pYJ3d?>AHt>Ui*}dU#5nVSGNxTGbr+JqXSfP@>wZ#_|b9ypL)x)!W;QS-RAg z%86U9${2pnh&AJN3J}CNuF)7AF$_$;2&2@Tx`2v`?o2%Xl5i9pM%+1&*rgGjg_eLx z&0_yhOMRu!Vzg9)`8a@UEfT-Lf4C^Mf-?{f|KXZg8I{^EAZf`Ux62uaFOm)&{0{mN zO{6BEW33#M(y0&!K(AWkQusZU)&$i`M(!+o_6p7u4Lpd|%$76qM_Et}Qf=kjbIT3D z%cn6TCaLP8aI46@A7Ra8q6mtopYK)Kpo*lQN>h0ubLg0=x4a17!zrqM@*~FmCNu4> zJV{=|sZ~ss3(DD@-L5bmy1T$hJAsf|`CfY}+Ly$5u*})gd8+I#v9XkZZgUs?F1m~J z#Bp#6mx(v2;0_dLv;Oqd&{vD16wiwM0uI`daf+52Nr!lSN9GG|*Hl+}fcnjZ7t&W; zP5tAJSXzqWyevXpelKU1n>Et|71NIevxKu?IUmb&ozrkJuJQXTSYspjz3;(RKQXt; zgBebxcb^E7nMViDOwjQstc7y&c+Qxgc$V#Co%9E zr#TRh`&0bRGWovznA&zNPR!F_gVD^724L+Yi?{OHh9i1rvKm< zp(W8wL&+y}XQ!_t{m#l-#b~8c3wp%roGjgwntOeN6lYQaVV= z5-UpGq?2Gsxr|>ud@ySe#lGV#Q?l<|gpIS04oW|Q>b``<@IX4lKJX{q0Yv$`|Kr|1 z*5Xy}_BZC_XQ>q9)nA$^UyviE_KeMDc`vnxpNO#rv(L1UXR@Y&#Jf! zdflCI>qJ#7k*DKhEmY&#o-OFmB}(iY2RIRLODX)Vm+b!|r1iX4y{v*i5e1hsnsvGa z%)UR@ngYuIRH`g@;94?B?^jX7Q*SJfp}tvE9w@hg*HInR+m9=S7XQ3I*fG-hNe6K3 zOunb14pp4}yC3^c8o2Ucp5qevusn+>Dn}l!nkzpe3%$yjoy>R~mJ5@|oR*x@33B+) zVAhM-tNqew*(Y^p)xD8_XZB5FkDt$6+raAn#2CK^|L(x`n=nhBb05XI*BDk|9HU#6 z88@FhSjD(kmWRq3Mx~)VS>DOr7nPe+H(N;+qX=B7>Tsy;F^gCHf3E+^`FWOorYS4? zGI)H3R7;L!29M*(w_tXsFj6OoJ#K-KA17XJ&g%?vBkrP-syMM?B~>qCk~%17i*i6E z(Gh$)cN#=-SfAB&Lk?#QOR0LOlqeCuf|IARx|S2S)MFpoEB(vrJCDc5&r%Pn0tJ7+ z8qcK<6@*>v?ax?GUDyX3!uvAwJW9z;`~|8b=;uN@P(Dpo_cPo_%T zN;$Z1_v0O%mS>~DJkRrV^2(h&>w)a%Rd|+VxVJPoKW5g`FwWNH%$@_R!&+duW>$nk zl|)RmxnOsZH*)4!q_P9@RqD%q*>hU(zFtTxcvYrUoH4IQ zPmVI|^~32r_JispQKXVMYrcF|zNiw_hlvorsj8`8a-I$X!^xImSI7tWsv~D$K0&OKG}_#KZ-j@V=vtyzmnZjd9MCKW|wB(Wix}cs`EsM zTjUI$_%{A#sOkK-dP&4Gl@-;M@wqRjGb+VY1@o#dI^&_-LnOVzDlrRFnVVj@ zj_ML?_7P|Q2L8W{=kpJ{e=_?~dEP}L=VeWv-6CfBWMa#^{A?Y*=nr}Nxr|;Sd)RjF z;)s+g&A5Qsfsz^VVD^xV+Fn`)E=vuw;0&Gu(=Ht@V1}Zk zQg(H96$43bXRZ!A*=SD7xRz1w+qdL*U$^4ysQSA3YsatB=Yya2d~WkO{M&=%qoz@& zkCu;?skRBW5%dWi2E(O4n1hDe(J=BjQIn_{8FD)0akE`H>3waW9A+$)?-(fY3vqJJiehz&T`XaPh*dTB&wc&R|JI!IuB=v1| zVJdDrsUa?5d`7Z%`{OVByY!#*f!xd&tgF}+_!_8=?sX}Bnd+ign2Yw}6ka7S@HXj- zPf35?>7MG^X>Vu~wClCYsO-&BJyF%7r{)0Rh)_{EP^qUUtQ7ny_&xl)Kk$~_5d1gj zRF)Nb;OFog&#sz!gSvzEl-8tc4YKk`?+NJ}stcVI+7vWY3>zJG+E^%jSj6LqlM$sO zLn7u!oC&EMTHG+h@J(A+cU`O0?MK6ZgSD}p{pFb|45huOS;0EqLu}KIT_px@sxg|2 z)E%?cc_3%A=;<6#x6oWeKi`3P>k1r&TX>7iBrY3{50ze=F5ZC6b4$#EJGw(K!Bpu7 zS0h~z#G>kU>IU?Hey`DLjrazP(Om&0dZ0T(Z%i(A05y1)(IfGYC|8qTeO!-2>^3ueKufz9xZ)bx+6qiCU+0C&ov zEKLuSOX5j<+4u5%>S*U^!?nG&3-tf!tAxx5*%kIREGBGN*xj(=#$Z@m95Fu_!@?(q z7dQ4bRy1xit_=G?9ihE3f*vAG!VZLO4=o;M2<;krHYAL-ILL5c@1plj23XZWZ6D3= zAY2i?;Q_z$TfC=aD*?mRm z6L!`w2eLtm3$VQeI7tD84(u)zF2^mwF8m;_B`!Up%A}J2Qnf?#R`Zv}rMF^LARK7aTw>5bWL5Ym1_C`dNNPa-of!h)?LtKY9h2hYva_v zsU=p>br=o3h#_Xv)4C_I?r8ZX@$)kx=eyDs!3~DiMcl2tt2_=%t`wN>8a%rtIOaZo zC_N|-1eVcb?m<8kTtYwg_VBO1r|)zlykiu>YQe*JbdRD=STQ(0_+AmFOojaLoT^!J;rd8oM$%6T2Gz-;Xo?Oe@A z%_2UYp{L9O?OUxASCQgi?z7=pv?s6rN0p3s$5#0rC$5Po{S@PnK%DsitfL#a{d)Ek z2`$8QxJ)NeQ8QIy_BuA?NQ!EoPTY(Q|aG+Abm;tyW|teyOYNy7yt6< z^UIHGKEC)c^n>Bc{m-4#3a8C94K;=2h&h=#|K+sBbL#|pU^5)OTKEb^fh=9o`n3*Y zr|^?e5z#Y^FO6x2av?v_N2mdQ>2>J>8^lwpEe<{};F9GjMWK?vga2pGV2_OQdKw*b z@|Y)oQjc+pMWoKeP3b5pKWmHYhSBGDuHm(zZ0PLJY3!j+;|SOTkHb$G%Z9g%xEWD6 zd`I|!5Pj%6-4}e>x6mE%2lXQuKGlhH!@&%jf|*{%gH-{e{2Gdyh4glvgl4%HJ_7UI zOWjW($=7pC#d%~UYT;Rq*697qIT9T6@c2{UL{Jy6trB=<9R_o~4OhzyruGZ*mJKYX zD0Qt8hVh1CVPiPk;=xsk45JfG-lkq8e{eMzj`9JzNMH5n z`Yl2qc`UL?lp1f~ABUS_=b^m5_LJ?WFvr+-ZNcon_#!&!^7 zy1?i4E($N&;MnVENKG*c&)a&cDXPC&sV4IjbE=`UajNFLrUN*BJX*P{=?M4<-sOB{ z4?I)1U}mP@21V_~+RxTB(Eg%3r<-ibG9C7x97Ez^#EzgX%G_{`C#;OSBxYtT)<>c2xhbXo9sH$r4?3)kyAIQm(y}BgPhO zyG)IFFB;MFzt8%96>aTpxyy3P<^<&wD}m#ht2Orql?DC z;!Sm`TpuL`_H?53MY2hK@t)M9HSi4OuglIXXEpqsHI7)ic^5iL)8}3T4T;;R^M7&0 zy6;E@QZ;&Z{9yo|Cr9|A8>aumkYfn5jI#Km`}8s(F|elfmUUz?uh5rav%*`^<6wzg z9N8{BGh7vMJmROY$6{ z9LH_fZ69F5TrYYD>w7#*nm(v@pD4O*)7UrJQfv-~LI-j=G%CW}Pu<_#HYr|SjoxS? zom+o^GsNM=Rtk6bqxilZ#>XKWg|ExLaegHNRt9|c?;LPCuv*ZTfQZ0H_&%rux(6)x z&+{*XuJtwF3VyBdtBUYF=KGJY)$a;^a3|p=E%f>Bqw$&J^RK0XkJ-G-{Lb9LGR^cF zr^8<6#^_sBgnzZk_yb>_mj;pjE>5>gm#QtJYeCN7OWr*JEY*+wK1lpc9FHDaz8tDd zrvJPT+I?-&hAW1DSSb7%v$FzPD9;={?0?xsM~ve%S|EGvPwkU!%WM@ZYxKQ!l|&p4&3@=XhT(w+*Us-|i>to=xQs3WP)#9md&_dU306Jk;iCesY= zsWYM16prd>eKf^F0_lAZNMCAR*kCTYYn=qG7=>rU(brr~E z*66*6)voBO>z5Ou7ip{Nn#0)j0iDpJJJ*D2YPfcq_8__ODUCrJ3iqft6{9&I=v9cN zD}aTk!YiLhRp9_$fA^_chR|oXmOWqq>J?vj%5_xTRXyl%DM8Oh8RFLjJkviau`qY+ z!XA1~E1{*?L|7!~g+_FMT*qm9FdZHtVlb*w1E|tlVer>dB~qKNjR)}%-a8Xk;8oRf zsy9CL#Eby{zX|pm0t!_CBApCwwE@g>FLkY!)IF`#^RB^4j-uZ;f}W%vnl+kG?HC+y zH)uO*kK(>-)OFG>)gI9X>q_zSP<*3jYM&D=X?3!uBO~!j+ZnAtwYC{Vh+*1!K zWyI0=ZjMtWakm4--QqlSG>kYQ7NI)&Cq2BY=qs8)@5>F*F0R01emdT9ujuiJrf;G& zy~1Tx{Zv+ZEt-QTH9&h}fNBN()5p|BXrX3-4_u&bRG#`+8a(+D)W*ELYj_{v+7gIy zOEQllsadFCl7I0I#y?`Kb~w+vEd20QkBv?JK!eXn3J>?E(Ippy5=f4lPE{ndx)Cb3et z>Z<;Z_UcR6H#_L8jRN2IB}#vXhR^)$4&Vda8MC$;U#x$;d!Xd*kPp>uvY0sQC@7v^^R1 zeQ@)O@G4(|m3O1pD-Cqm3Pv0PhVH@ZcY*sJ<^KJsxmoB`ThMx!y&p2UNtP)sM;2u8|9m#qp99~YLSVMK2 zzL#KlC#TS4E1_zs>OnvKES$e5!Pwb>yWk%5aVIlYXH<7p2XT#iDC+0{d7_#|&;0<- zp%Hw40cZS>N=tXhY@S7XSYk`nBf%qH&=(WKSQ+RU+Qr!1REJO%dEvEz(TSy6R@{4$ z_X_@d0QmMS#&0{7ok!rT>%jfvKtY;pz6MAjQAMr@r~*q#%zM^z#-sfZoMLao%BKI{7buf1OYuiniyj$j?d zGDb;^OL^|0I(40VulCf$HdCpr?=3RhUsJR3%@M6b<~TM3sXlSd7~8Lv^56Piob@sRzMGRY!o1&xJL(2Uo%h?6f9&n!Dlh*hlrZ zDiR*eQ|5gjPf<(Px{AH!9l7yd`hSk#>|T;Cuo2>RxSG{Cb@BIti!u}?fKKS0ugyM^ zJ?>qvcMa3U^zNBanduovGM1J4~CAUZ}~rRq)@|b!+9zlE!mrF zjtP*CJ!g-2X(T&++W97$bwCeD%v&C!;%aH-q- z-}LX{f6c$A-$TCzK|$67!NWuTvvvu->OUypf=?Y^i%*hIJU%Tm!D<|G6J@{awd;xf z9TEK+dkGZr^yvBbu`ROQM<1sQ_FZJ{mF)zIHC*-Y8y+Lp$?+kKalEAO%zf_vcWw>zkuwTk~X z|D~32pULJAW)-}{gYYm(J5e5M%bP0lSSoWv;8wYaL8};s;{+6JjL{9n`FWtOEVY7EO*!pa*1;6^ zwj_F$JdQv$v{SU#!9d!vNBjgf6s{Sism&UgMn&fw6`PGzg;Vf?eogmNp}GQ`mu2cr z)MXCRgZZy!jYBQZEWOMR%?sg*C!0T- zwLVQjiJSUXv23-BX3n3pn0?F@CRJA zZOpqWbj!Mxa#SEvgciyQB_6$xH%chn&sWL^AzXARQG$_9TDw?J6+*A#GwSLQ-oL;+ z=tEc9FPcGU5_h5=5oqXYsErrYb$z(OgS(~ZRQlt(eBF1Vrp-j3<)~E_XOF*6O&zsu zb&~2fIM_z9jM!c5K^AaDQ47aV^j=S1R2!w3t@u-~l2dRq%8&=a?j44Qd^&JpAwALYtQNwAy7a9CTxZ><6+@d5YN z7G;o=%%{d^e&|&Lh!}l1-%hB5)dOMubnqGhx2TA{*#t)-0iMfSbrZB?>Uqso=fLB9 zrFy1TdBwuNAE~;8{=9~+$)=pQcJY+pLQB<$_^qFKi5}+DaA}Xg`unU@MG>sNFqNL) z#&iqkDBi*&a-}2W0VR0$|KM@Ei9Yu3D8*Jo=X}50=61QuOYc!%9E-;O&*(1t;1rU8 zXGA4b!s@$6xF@0Q{tB0otIkyC0JO8lxYoIL;9c>_T~Qh+okj_D8))V@X&}AEk`y6t zQ*1bB98}a|3-LXQv2{>|Er7GQ1wOce{@sPD3#!SSSy}2vR39pPXQ^9*IB%uOdBtlD zG1?vYMSpU?cU5M3c=r-fIK)^qd>l$Ep&yFs7iAeO=Xvr=d>u_ntkNANAT5Y{Jbe5l z`M5j=-P(ij*l)07K46BmB39ic$Y|M}XFjHYs8u5C9x2K~OK~_mqMA9N6{3U&;P}^M zi?T&}kFVn)S#md)&Z7@sK%HVjDwA{jEUqxU-dl=1L^Uz;ww`N zl%gUW6;V@H^TRuV9HFnaCHvfOhRud(Jj5zcJNn78%ks>xhJTNM+W{x(q1Z%+Nq#_& zKriby>*=64YbEO(>!LtaP=ER?X81j$_hO8{(QmHbB>!~(u7URhuToW8I`j^PBk(1DyP{kdNYZR-COOEXrw>GwETyF8TCGN#7 zEq*DcSM1}6Ns;@*6Cy-VM!8sW=xgiH;BNr~0wdA#o8b2Wyljw9I=bjJ$ZHefFLZ$a zy&kWISX*;j)BGm+Gjo>YtfL<>>`UO6j+xfX66t5(8q>DEsrdGL+WoYow1Me?>60@` zWTa;Pm-+eA;ZJkF&igtEU*FIlem~9^Ju1p}I33+sqw9&CeCcs`NG=deUQmNhRZp$F z3$v)2YOrcGSbBGm)RJ-?StD7bSLk^@W~QG-D?lgv%6CzR>H!X-r-$W*-p6pl9A`P} zm+jZtdfi$SGPzjgh}{vJBYQ=?j9C&pC$4_+--=f*ai{p)5~bqq#|g1(VuwV^k*=_R z!~Y7YT5M-fC2LH;#(<^NvOC~;H${J2ABXNoH6pa8@cTTN|0sCNg-T^%FT0FI_LJw* zO|gXDibbwt=mE$6AHUg%Ui5P3M(3Z7xoC1kIX8fI%|;*hq;0$HXZpNW+RpO5x2>@~ z!QKts$7RG8<84dPifdzUV*k}%8BI_hht)C8KF7YtanA7zudU~(={SeO&?EbI`!z=n z>RU$~|4OJI;7ddHvBS*KGhnRo29JAD3ur><57@Qe2z}l1kuswmnK>>k(2QK#e=6A}crSBlT_M)lh&opKjWnFXq z0s5NilCMUCCLM?8x<+$Lvw^I(1GT3|RA^pnjIitnaGEFS*6E(>I_hTY(%}%iq^EWU ze>+)oT~n28wgxe0Lzv9BaqsXbcN{_Qwy~>*>#6;P{g(ZpJs)4fnfT0{EBd$S3mSoy z&>zd?dnfeAZlXQh3yk%aBh_&k4csm4Uq8G1y01~si=>7*guP^vJX_9Vzi0-BydORK z%js*_ORPQz9%~sQd{0le4n!u;yMbm1bvHp9N`Ju#_yl>{ zSlxX%40E*?r~nt&jo0qdPS#z}dGgb5Ou}nrv@0IH!}jhm?i%ja z?Bva*EAG3fAC|(WcAS*OdU;K~=o}GR6#Y>3-QC=y=>h7Dw-!+%k-#DJa6?^f+&{a^ zy1Jq%+>rO_1o~J-N|gV=>90sWf?~H#u?W6Y{U&lQuBNxP8~g4R@hx5pf;s>d1S?(s z%ivF_!Sc(IdvD>aE{?uJDtW_H@Cub!iiqN-=psHCPd{27=iW>-{#OwLR{@*x;4`Js zEGkhV(e3U@UH%BYT~*P!4CJnQyV|%!_ea+i@ZzKBvp)klj%K`S@Y_G! zGu;R1?b_}>>E4H)+#B~tw?-~5x8-!JE_aiCr4lI5)se2dzc3EHQBA&y_tIAMg|E36 zpfCIr{fqr@KRC3v+ zU;x_~^V)E{{m@`I!&!R*f19%4qRn~5T-8&QFb1oVssBdNKk$@11wLR`Pmx>$7Al(?*Gtc`vI<&f@WGRv8~t;=294lW;V6!zF=L`HCHt?b^CS6L~kxr z9rJj4*wz}p7{-E&_BX9I{YnQ{HTH&)uo#oLkLh$-yi=F*N)j#$KZEgJ7cvC9%3ocX z46-*m?wxVt^6(9N5$o30{H@sw*Rd7C-g6HA7UQsEkcOVu68T*aY*XM#e zoCmdv)cvO02hYcgexj1pZ5!$b>dV9E80ygjU?0pvbHEA~=HVZ^ylQ$cMz7+8m*S-& z_xAK^Y(OV&k+>7BxLc|V=&PiHx1FO4s~U=LAw-iU;Tq`pew{hA3SM3uIw0T4i^jwI z?9Wxi!<&&sJ@{5L)is{$F*q?c6wRkeTck(0k`=lmrSei!X_G6(b<-_K2hbVcK(C(~ zHkBT2@(J!_cLyTBt;D}i-D-Bz2zev*i{Z*bNqS&B>-q6_xMbC?L;f{vcNCd84y zQA5xR#e^zKa}e)_boT8-!EL$_Nw1NM9QzVcV=H->oT=!9^5n_ol~&4LdSj>KGWUuv z8y%5#;1m5#-rj^OHM5I^QiB@KKGMQ9n9k7_uEl8ImSpbrYAdrNPvs zPr>&YgtvWf@`w4-ZK)AnM+3nqH0;0%bx2P>HJ-jm3**+9p1(&z46F~mY99S6ZHeJ-NwrFVDom<%VP-57;H&kgM^m+fxA>g^phg{h7&dzMf0@ z(gP_+n#O(nLEbVRPq15Pj5?$cxf}Q3>EYZZA0WD~s`Q~3ZwtDUW0)76l{rcRWAp^n z^A53ob9PWGJrjqi*PJGP*{pk{>qGBk3&S`=1dcm1^jq|aR5^SMoei0K13CE?{T#zN zLs$KBx-1jP`!DK-8g?7(@H@Wg;|)L4i`WjP(p9=yXB%G_&E|3DeWnoeb+gqn)pXC) z%CsG;v*MUM^ScuYJPIwyu6g`lP>d%#cFyR|s9t?SFX|M%h+o0% zOmI$v!4e9T`qbqXDW~u<>CcMvXmI(WuXUOjVLv%%N7g`NR#>iEagV_3@~k@@AA%NA z6fsR@_D8#9%|_rJuzThX06Bt4bXjL8D&2q*FlGWtc*B`IC%;BLnrktiu-@k!J~ zA4u@EAW-&S@zk|!`Rgo&0GjT%trzfU8 zqsgMvKb1&yn#KYelS(Z67=F7iy%e`u6PI8PHUbf@MvYA2$&6=@=ms*-AOFhIQa7m~ zYw;5}L^WvudJnBx&k1;xX1J5&tyFb=l^4QuVz~feF}>9V?8<7~UGl*WcHlo9#va`O zJu;(GUFm=hV`cnYeiDWW^{D!;mLG%NJc8GINeB`eQ^!o<#pTs0Ft=B6#(txAQXDSi zTk!{+#J#Bd)=`=8S{saSjF~;FfD`FoG!PSTC_Rogekb(Rezu9eCLBa`*-Gdp9HuX`7xDNw?G|kzHOtEIXE*3C>u;igkcX~BGwQ@$s5*=zes!U7 z5v6og^zLx?edjA@6kaD|(M*1pe>XoQ-}&Qg{`{HCRT44jQ0)M}G8so=ofsjcr3Q>nTy5N{AKZNZn);Er;a!aw0K9zY8m z2XVRl*YTFM+rmAH6Kah6CFr&bhrx-s1O0{0P%8V-Z84LYYd_U_@VY#;hOUD3UL&bV z8qiq2?e)X!jP{*Ypv$kCdH{TXqo`H2R#gGJ-y`3YH!^M!L>rUPvgnDYYen$-VvgdD z-|-*$%e4nim5DX6fiwSsvXcCu3R=K7;M=W-7v$l5ccT}6gW*5Uo-&5h>|u$fwWdn2 zGs27mjQ++x#_q<|#-H`;KvfeAui+q?ILQX;kL!nlyk9it8l$P;o3)AB{=~%7(9fE| zp5X)fa)unn8y(v1Xnw`e|Dh>_h^{D!AEGWZH4WQ{h9p}Sax%FRXo^2T^a7L|MzElz**2iZXs8J1#yfsKt|`$ zGshqEES|t=JqbsozuZGEMV@z%*motn>924^FA?({1QC2e1bIWg4<>v_DTeZ5F*G-q zDeFBd(aJ93(UNFFR|D((Otd+Ryr&IX*8`P5m7VA)_Qz9dCGV0lh;G$jP@hb6hSu5svbD2Uu@~B|*akQT;2iVT-pSblq~n>h9KJe#fi!J*?{I5D z{ukh-TnhZ_5hp?@{zL8Mu0%b1)9J>k+pvovV>z| zuDBlc?czd+Fa`{Kg(r??PaY0_k|W#XS>V7Ld@1HS=i~aY$f?4+W|*Thu2kNRZMG%0 zXZS;RbkuP?cRqHu$EV80b-uDcu$vuO_9F7}r_NK(m(DA&VtYD&a}9FMM2poMpS5!A z$qBIKB!>}S>XY^i#}z!UIy;6t$2o(X2Inx&m?Zp>3f;ldJ0j0C{JRo_*1{V6&8Bdb zcX`g8&{1SKMveq8e&));56R(-!Ns8l-dD5m-Kvb=j%>F&%EN1P+P>mQamO|Y z=ZN~w^3KO(_62xFjE9{S%k#elk7k-MAJ5f0N;qfL46-MI{*vP0C|`Kr{;bBAIQ=wr zG;{1NI$U%PKfJE^H=JfiiYSaLR2OIqz7|}^DX&q%pM{I?=DA*YplB!hu4nAqo$H;e z!3N8?qunEkuYXl$DwWY0&m|x5C_;RN6ZI5bgU!NGbkiDf{_T)sIqMsO#f-q4N$aqn zmwLs%%eDsBtYfyDMX!sF*v{aiceW@FFTk#NS^Vob?Kq7GnYUz;?$HsGEZ!CKs1yFq zKGdG>q{{e9{z6PA63-sgIdv`QH)x|v)}7G#=)KXvYYKYxjW}T?`FR`7Y|V9Q@2_C7 ze?j&2Hk^fzYJ=A{ddnBnzxITDyBIm{Dp0RvG@^D<)0$xZY#wBOV18<J@NQ%l%mH^z;N(Z{Zh=^gt4zH8^`kI}s&rO5dap^>FRwuh`od+CF@jAf2~zkV)U zfyd+lHoMVL4+j=gR*9^#nE{#pZ%(G}d0zJU;Kwf?t$Gyw$o8<$!#PjBJzn_K>uJP` zJHjHt-)-q zNBc-N_p3o)-8$ za0YeAYo!t;-pjbL-ohZ_cmBzK;Cd<#VmifuGBK z^#0i9Q?XAuA5VRBf4ao>dH>6A_+&V=f8YAo8OoVU@Q&JAOPsT2xFDxcauV|*_AO!Ry1m*v;Rzqx;k ze=q-+{zY1tM0rvvB4N$CRk zz(SCo+45I%dLwGOI;w2t(S)svzRGeKC?~+wa;Voo0)soQ_0<*X>gzYau^5Ez{t;cC zuCxB2K2rBL^|0Q$1o~R$(Tf@nTPKE`?Hv4pYH%E{2v3CJaAjALH`M0S&VfgLBOhvs zcbg1H))S}y4F2-T`PzAgb7GXUk8>k@k%rEi&Y6y19ltxCemMUP_@I zu@1f2J+QXoV6z?7+|*>S2EM_x2?WQQERU0alSW8wiR+$_ha9CYRN(mN=!^TnAJl#> z6+J0hSh%*ZcA>FQqAvCrE=vRY-99yn^Y?a#&48MY_0KK_u@U7hr0qyO}b}!E5FZ}z5{TnCsH(Xpwpl+Cs z6J(mr<;Zv3L?1oNo#UQ_Pp-!ocQG!9ov9V1;VQnJPP|!ge11Yp&VasnU2?M+C5qml zKKNy4Di4KsuvE(lo$0G=EW``*sOvW8iPn(}yLaMx?@nnK@z$}PLrN}Eu>e$8sEMin|m&ZW1Y6tR~|HWQl!DwEi$tmHc( z`1KxfUvhs#?=2l2I3MEo7I0k#aIyz*;?9GUUYxlf;I^Xp*37*F4*witi$!4m8=3J_ zz$a6|@>WX6q~TPJYSH^t1D}XET+`xNBiE@5x0FZ8t?(Vqk&W~}ZB=HI(>@_S`j3At zV_ZJtJz|yp;b^~;pMa;Gg-1~Zf8jnLh-JXJ>fyKU(WhUFqIo_wm=e@hhPub&1RDiP znm~;uM!H69dsqI#h$WC!S5Y!?g1tl>J5ZiVZ(<4gfN~Witbu|;G* zXTHvQ<)S@N9G=~9?~_#UAA(M=rH^hadatcDziBRc7kS?TX}(L}eo1s$LgkZrWZv795{KWvok2O&gT9AU2#g6H6THK!3ob`({W}=YS%b=0j?Q*FBbp?ymCwT0 znoFG8##zc4;WoQX%$ydEzK(|Wc>5~*GJ7KW_Cx4n+p9mLH=3)MJNvHmjrX7C-!X7o z;4!@KPFscG?!}UdwF>zsWI@P>kavOAgCxJE{u(+C4(aac#?#FiM7?JZ8AoeZu&a$D z(6Oy>LSc8jm^43f^FHMs&t0C=F{f@;{EWSbKXJE?Xcj0 zSN}-T$|2-camv|p_iA(S~CZpZF zPk&Z_*_dTKgl3_O-jNu%3=Qd?_+lz&R+$@_+u%ulS^q&lo9OZ=-T1$$W~x4;vg$%( z@f9u%4IJ@~Mf7s7!0YWP9@RcSzU6)Yk^W<8UeCPGKQ8^4n?FAP-+~zh`l9!R*NUzc zEq6?CJf-hu1HBRZ>7lqtt=$FMkicoBkRR`pHiJ}mmDQ3j`$eE!9gpNrU{(95XL#Wq z`rfUPmQvklOP^LV_W-vSfAhOM64lo#u58^J%Q3Aer>HPJ6%W-Y)z z4zh#PX9qE}5BjstXW`$P?Fx0@#l!Xv-lPj$7wH2rx^}xxx+HvBm$28@B%9oeE@c-~ zJU#VGgB&6s1P`nTOGYb&@V%Ve7p(jtILrn2TlX9MpcjKih}>%pu%J2cdxp}D=X}qRB zYItijeTnI!(5CVD0rjH?y$4*UfAEXSfCc2~109O1O>bqQqJyDSk2-;h>V}^Zpqv56 zFDr*~?hTMCGYbB)71s>`|6Hql&wbHdK&Mz|@-Lg@k~ZS^b&-147%(Xv3 zNV{{tP!gqScq~l92lEl}WLT!k3Cjg4~4@#7AiX&e@t8R08MBQNEs2-?>DE-3$`3ma`^;?&Q*P9Q`ln7Z zIx5VUz%hc*UVKjXL?u-T5UA;@WALYTs7|R~@FY|I&-I=!9;Sa+R7I<()UB9YW z1-{k=xW1=^lJuzP@WcwlJuFNt4Wr^5qZSDtZlACS6zyMXK$pQ!zR;nvovw?IaJ4+R zz6AF!39d(7y6QJjefnJ(j_!iM-8NC2=#pl_ORdORYv6Q02G&)9^|^%c*dhHRb&*GK zm(4+h(>Mvo$Q|%5cfdukf))M)Ut+$pn0!4-D95`!^VE|0tlMY@W|CW{Qx!YH7{rR- zgxz$1Zxkj8KKR1r@jTDMCprYHv4+@4>h{cX=N_mI5y63jSweH zqSn0`O>&1+h`0NE*#^d&O?>mOe3c&j#c=JWfT#LX2`Mdm=IsZ@q>0j9xy%01mHD=f z*OpZ3!c%)D&lA>wM>SCfF(2dj%0YE=Gu_QWE~JCRj>1q-<1FFfz9BjJgRmgxhF&4nk}BEO_+= z=DQV!Mt$aA6~0f08`%>a|2VVz9eqb9sYS*U58hMGbJqiS#{C5iPwKXE5q`lnIZ~0y zW^TxXiExInZn|-fPG(JBk}pydo=qLIDvs?oJmD58{Hg3_?u78~gIsej-Z78!c@Y2g zCp}<6up&1rAMm+*Wl^VNg6 zbr=1FH#H~iIfO|*)HJWz#w47nKdV1&s zgyO7`^U6>h3}1pfbp~Dc~tN zsh~M{cL`US+4z>ZsoJy#i>k{0G>rS$M_s!H&ohs|?8)qjz^zUbg|Cw9_$YatE(Ovd`6Q7<96Le{GXBBx zdN}(g_)SasS1q2tz;kL1+arTWLoI7zV};AB;9>3O-6wFm9z;DO1>QhkxsPm@o=bLW zwP|oilBnsmU`4l+o5~}>X(#ifzbjvquk70^nYT~bcgnIuK4+gCDR*RUKTtk%cJ2Td zeM-%=A>((OdH+NupT!OdcA!3d^*@Mwk;aOJ#$G$C} zlfUrlZ(K*N@{RS;l#y-AE;0+w_Y3sBA1Z1xxq7U@7-sSkWdc8cDer_6FjDEt$_rz6 zoT>c5J=~|4aw$3>bs6(cJflX8?F6p37i;_&JJ4aqp+498kY|>}r)-y3$hTNm-FcTd z_TIi+)iAEVke#d7&j5PyM9jgS*~HmkvAHgoNNG2;$XyC}wd+Qv0q=w^Bb%wwupTijUfm`yPD5pBR-XCB;I~bV*%+cHUxv7l_tWU z#0r0)B>aHAH<6wGD>c8Bd}cpllz62U3NODhQxdqsXG&kzb8p6}1u<=q5<#Wm3Zv3c zY0detg8Z^S<28(^FP(c!X2ztlA9(i7+44m4u;c7CSLH2?Qg_zDSJvtTc9?Mbn8P@m zT-2}TasPYSv+6OoJkO&K>!Ad@coQX!GFz#`K0gh9 zjgh@SiRTl*9Tww^`O4|Bf=FdLG0`?5Sx5twxlb(inq9OHCtD+W2oAIIdJwI*<}Sz4 z&(Mtf@kQswj}Bwcd;G;2(S>JShjTKSYyX>{WWX1i#JN(E=&e2I&pyu1$DB@Y;c0}T z2c84(p&K#Nb7C$#XXu|ikxqEc|Hs#Jy|d2b0_6EZC~Xo=_SeXi&naa$tuA{wufC~7%F={&E*dLM;{*HJi9H@J#)-m|*U0teeY!W70h z1;y#!TBJxyEpYkjjAANzc?05`_1r}QPhlk3+&TPt z)wsD;5d*~mj8`x!+kV7%Z+XZ2e9a)%c}JbP60c}STx^gD&_huDL#IxDXzw^A-!6JV}y}5)~OuRr>nanu((}BK=XSH5jEKcKIR>R`#%kwG_yu>}s!}?+c zaV6`hGAk&G*IBvuc+ro%IRxdudT1fMP^Pl3ykS;*GovaB(Zp+W*tN8*r}xS{IMM?D zl>}Ge3g^gsB^9nrXV!b6l8uJX8TPV4!a7bxZ^p>uRp)UzY=9%+c%e1(^*+7-%^0_- z^t?ag3|+|gPR#prjJ8<_XUe`-Vgd3Mf2Th2m6^{v5>HP9Rm;NW7 z$x&W+LAcLa)QVwZRdNpx2Hu^o=gh_Vtcm7m>Xj4A!j01NdaI}xim1__W%Wg{hQBGh zVI)2zL)ytRYQPC`h6+nPcDpp5*A4ul?ogW#XMfnu`M#Pte}$iUi)p;R5e$)ESsBxb z^0oixVD;n9@|itH8N({9jZS>F=VwhAf!B=FA+D!D{wlX;9mH`yHep;|{y%enrMOU& zpT;=<&Tdi`b%i8Yk{-@kWyYi#>-RHf!!BO`4c6&I_Jnzys0$e9tL&jxc1svc?4Q+? z3gpKr>=m7ugB>|B>+o#Gu*&AL7M(Cq4{}!~v6Oh9n)OZY`5C!bWp<2X%;`(~?lgEr zE26=_nRz0Nr@iFq&Di6ba=%N-={7T`qT$5;%Q$(9ZnDrA;cr%aTYkTe$haJQ`3S{I zE-?l?_zZLY4ELDL+1Zg(?4=S-M$(L@Qk#fmGCRpSVx_ZWYkACl3(v0&Bf6hh{|7tQ zCterFGke3ls!o@e2JLyz9$v`q_JQY9mbLeeS$c{&(3LssIXSioCmF3K;36M{h4A!d z!7(k)?(#SDD~Gu5730+aRr+bLLwlf0AHkJ-=Ba_1x&{vVY4*ol-uub_&yn`Raj?t% z%%$1*AT&m|U>`HCDtqcoa-5IMs7N?&{_HK&c>iy3QP;8__px{DaO?JXD>q{9mORX!j+(kI1&Al;oRAG_VfGP@jX0M|K(Y%Vqf^k)2_vSH5r`fgs=nMjYP1e z-SnQ+5N~il9?T(~akYp=jBO(b)36gt1r9n2IkIU?tc~Ellk3N z_KzN7g1CV_E=rWC#W|U^&4nrAOtG`jR~XCuXwJNgVb51sk%E}Py;k9MM>%s!i{se4 zMpBn~$-k$tj^D9=w`A|>$Tfap9X#PRr+5}d)X#QuJ}02-^N^G4nDB;CJ3=PBke%!d zx_NVy6Fj@a)ONP;o0g1aG&}YeVy&Lcq+N{HL!S5%#&Ium%Ec*BKx}vooMjvH=ri8z z+t?|Z@Q$%O-5NZDXWVlES?N+v!#dp4NMQ~$A(x1yCeNfLzU;%?OZ@&#Uc%Zg#yd=99Vx7hOs-QD@|32m#_5cE7xtb!*1-$n%Ru(U{{c$u B7J>i( literal 0 HcmV?d00001 diff --git a/FtcRobotController/src/main/res/values/dimens.xml b/FtcRobotController/src/main/res/values/dimens.xml new file mode 100644 index 0000000..63f1bab --- /dev/null +++ b/FtcRobotController/src/main/res/values/dimens.xml @@ -0,0 +1,40 @@ + + + + + + 16dp + 5dp + + \ No newline at end of file diff --git a/FtcRobotController/src/main/res/values/strings.xml b/FtcRobotController/src/main/res/values/strings.xml new file mode 100644 index 0000000..6ea191e --- /dev/null +++ b/FtcRobotController/src/main/res/values/strings.xml @@ -0,0 +1,72 @@ + + + + + + + FTC Robot Controller + + + Self Inspect + Program & Manage + Blocks + Settings + Restart Robot + Configure Robot + About + Exit + + + Configuration Complete + Restarting Robot + The Robot Controller must be fully up and running before entering Program and Manage Mode. + + + + @style/AppThemeRedRC + @style/AppThemeGreenRC + @style/AppThemeBlueRC + @style/AppThemePurpleRC + @style/AppThemeOrangeRC + @style/AppThemeTealRC + + + pref_ftc_season_year_of_current_rc_new + + @string/packageNameRobotController + + diff --git a/FtcRobotController/src/main/res/values/styles.xml b/FtcRobotController/src/main/res/values/styles.xml new file mode 100644 index 0000000..07689c0 --- /dev/null +++ b/FtcRobotController/src/main/res/values/styles.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/FtcRobotController/src/main/res/xml/app_settings.xml b/FtcRobotController/src/main/res/xml/app_settings.xml new file mode 100644 index 0000000..58d3aa9 --- /dev/null +++ b/FtcRobotController/src/main/res/xml/app_settings.xml @@ -0,0 +1,93 @@ + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/FtcRobotController/src/main/res/xml/device_filter.xml b/FtcRobotController/src/main/res/xml/device_filter.xml new file mode 100644 index 0000000..7b75350 --- /dev/null +++ b/FtcRobotController/src/main/res/xml/device_filter.xml @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..88b776b --- /dev/null +++ b/LICENSE @@ -0,0 +1,29 @@ +Copyright (c) 2014-2022 FIRST. All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of FIRST nor the names of its contributors +may be used to endorse or promote products derived from this software without +specific prior written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + diff --git a/README.md b/README.md new file mode 100644 index 0000000..2e5ef29 --- /dev/null +++ b/README.md @@ -0,0 +1,1481 @@ +## NOTICE + +This repository contains the public FTC SDK for the CENTERSTAGE (2023-2024) competition season. + +## Welcome! +This GitHub repository contains the source code that is used to build an Android app to control a *FIRST* Tech Challenge competition robot. To use this SDK, download/clone the entire project to your local computer. + +## Requirements +To use this Android Studio project, you will need Android Studio 2021.2 (codename Chipmunk) or later. + +To program your robot in Blocks or OnBot Java, you do not need Android Studio. + +## Getting Started +If you are new to robotics or new to the *FIRST* Tech Challenge, then you should consider reviewing the [FTC Blocks Tutorial](https://ftc-docs.firstinspires.org/programming_resources/blocks/Blocks-Tutorial.html) to get familiar with how to use the control system: + +      [FTC Blocks Online Tutorial](https://ftc-docs.firstinspires.org/programming_resources/blocks/Blocks-Tutorial.html) + +Even if you are an advanced Java programmer, it is helpful to start with the [FTC Blocks tutorial](https://ftc-docs.firstinspires.org/programming_resources/blocks/Blocks-Tutorial.html), and then migrate to the [OnBot Java Tool](https://ftc-docs.firstinspires.org/programming_resources/onbot_java/OnBot-Java-Tutorial.html) or to [Android Studio](https://ftc-docs.firstinspires.org/programming_resources/android_studio_java/Android-Studio-Tutorial.html) afterwards. + +## Downloading the Project +If you are an Android Studio programmer, there are several ways to download this repo. Note that if you use the Blocks or OnBot Java Tool to program your robot, then you do not need to download this repository. + +* If you are a git user, you can clone the most current version of the repository: + +

            git clone https://github.com/FIRST-Tech-Challenge/FtcRobotController.git

+ +* Or, if you prefer, you can use the "Download Zip" button available through the main repository page. Downloading the project as a .ZIP file will keep the size of the download manageable. + +* You can also download the project folder (as a .zip or .tar.gz archive file) from the Downloads subsection of the [Releases](https://github.com/FIRST-Tech-Challenge/FtcRobotController/releases) page for this repository. + +* The Releases page also contains prebuilt APKs. + +Once you have downloaded and uncompressed (if needed) your folder, you can use Android Studio to import the folder ("Import project (Eclipse ADT, Gradle, etc.)"). + +## Getting Help +### User Documentation and Tutorials +*FIRST* maintains online documentation with information and tutorials on how to use the *FIRST* Tech Challenge software and robot control system. You can access this documentation using the following link: + +      [FIRST Tech Challenge Documentation](https://ftc-docs.firstinspires.org/index.html) + +Note that the online documentation is an "evergreen" document that is constantly being updated and edited. It contains the most current information about the *FIRST* Tech Challenge software and control system. + +### Javadoc Reference Material +The Javadoc reference documentation for the FTC SDK is now available online. Click on the following link to view the FTC SDK Javadoc documentation as a live website: + +      [FTC Javadoc Documentation](https://javadoc.io/doc/org.firstinspires.ftc) + +### Online User Forum +For technical questions regarding the Control System or the FTC SDK, please visit the FIRST Tech Challenge Community site: + +      [FIRST Tech Challenge Community](https://ftc-community.firstinspires.org/) + +### Sample OpModes +This project contains a large selection of Sample OpModes (robot code examples) which can be cut and pasted into your /teamcode folder to be used as-is, or modified to suit your team's needs. + +Samples Folder:    [/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples](FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples) + +The readme.md file located in the [/TeamCode/src/main/java/org/firstinspires/ftc/teamcode](TeamCode/src/main/java/org/firstinspires/ftc/teamcode) folder contains an explanation of the sample naming convention, and instructions on how to copy them to your own project space. + +# Release Information + +## Version 9.1 (20240215-115542) + +### Enhancements +* Fixes a problem with Blocks: if the user closes a Block's warning balloon, it will still be closed next time the project is opened in the Blocks editor. +* In the Blocks editor, an alert concerning missing hardware devices is not shown if all the Blocks that use the missing hardware devices are disabled. +* Adds Blocks to support comparing property values CRServo.Direction, DCMotor.Direction, DCMotor.Mode, DCMotor.ZeroPowerBehavior, DigitalChannel.Mode, GyroSensor.HeadingMode, IrSeekerSensor.Mode, and Servo.Direction, to the corresponding enum Block. +* Improves OnBotJava auto-import to correctly import classes when used in certain situations. +* Improves OnBotJava autocomplete to provide better completion options in most cases. + * This fixes an issue where autocomplete would fail if a method with two or more formal parameters was defined. +* In OnBotJava, code folding support was added to expand and collapse code sections +* In OnBotJava, the copyright header is now automatically collapsed loading new files +* For all Blocks OpMode samples, intro comments have been moved to the RunOpMode comment balloon. +* The Clean up Blocks command in the Blocks editor now positions function Blocks so their comment balloons don't overlap other function Blocks. +* Added Blocks OpMode sample SensorTouch. +* Added Java OpMode sample SensorDigitalTouch. +* Several improvements to VisionPortal + * Adds option to control whether the stream is automatically started following a `.build()` call on a VisionPortal Builder + * Adds option to control whether the vision processing statistics overlay is rendered or not + * VisionPortals now implement the `CameraStreamSource` interface, allowing multiportal users to select which portal is routed to the DS in INIT by calling CameraStreamServer.getInstance().setSource(visionPortal). Can be selected via gamepad, between Camera Stream sessions. + * Add option to `AprilTagProcessor` to suppress calibration warnings + * Improves camera calibration warnings + * If a calibration is scaled, the resolution it was scaled from will be listed + * If calibrations exist with the wrong aspect ratio, the calibrated resolutions will be listed + * Fixes race condition which caused app crash when calling `stopStreaming()` immediately followed by `close()` on a VisionPortal + * Fixes IllegalStateException when calling `stopStreaming()` immediately after building a VisionPortal + * Added FTC Blocks counterparts to new Java methods: + * VisionPortal.Builder.setAutoStartStreamOnBuild + * VisionPortal.Builder.setShowStatsOverlay + * AprilTagProcessor.Builder.setSuppressCalibrationWarnings + * CameraStreamServer.setSource​ + +### Bug Fixes +* Fixes a problem where OnBotJava does not apply font size settings to the editor. +* Updates EasyOpenCV dependency to v1.7.1 + * Fixes inability to use EasyOpenCV CameraFactory in OnBotJava + * Fixes entire RC app crash when user pipeline throws an exception + * Fixes entire RC app crash when user user canvas annotator throws an exception + * Use the modern stacktrace display when handling user exceptions instead of the legacy ESTOP telemetry message + +## Version 9.0.1 (20230929-083754) + +### Enhancements +* Updates AprilTag samples to include Decimation and additional Comments. Also corrects misleading tag ID warnings +* Increases maximum size of Blocks inline comments to 140 characters +* Adds Blocks sample BasicOmniOpMode. +* Updated CENTERSTAGE library AprilTag orientation quaternions + * Thanks [@FromenActual](https://github.com/FromenActual) +* Updated Java Sample ConceptTensorFlowObjectDetection.java to include missing elements needed for custom model support. + +### Bug Fixes +* Fixes a problem where after October 1 the Driver Station will report as obsolete on v9.0 and prompt the user to update. + +## Version 9.0 (20230830-154348) + +### Breaking Changes +* Removes Vuforia +* Fields in `AprilTagDetection` and `AprilTagPose(ftc/raw)` objects are now `final` +* VisionPortal builder method `setCameraMonitorViewId()` has been renamed to `setLiveViewContainerId()` and `enableCameraMonitoring()` has been renamed to `enableLiveView()` + +### Enhancements +* Adds support for the DFRobot HuskyLens Vision Sensor. +* Blocks teams can now perform webcam calibration. + * Added a Block for System.currentTimeMillis (under Utilities/Time) + * Added a Block for VisionPortal.saveNextFrameRaw (under Vision/VisionPortal) + * Added a new sample Blocks OpMode called UtilityCameraFrameCapture. +* The RobotDriveByGyro sample has been updated to use the new universal IMU interface. It now supports both IMU types. +* Removed some error-prone ElapsedTime Blocks from the Blocks editor's toolbox. This is not a + breaking change: old Blocks OpModes that use these Blocks will still function, both in the + Blocks editor and at runtime. +* Standardizes on the form "OpMode" for the term OpMode. + * The preferred way to refer to OpModes that specifically extend `LinearOpMode` (including Blocks OpModes) is "linear OpMode". + * The preferred way to refer to OpModes that specifically extend `OpMode` directly is "iterative OpMode". +* Overhauls `OpMode` and `LinearOpMode` Javadoc comments to be easier to read and include more detail. +* Makes minor enhancements to Java samples + * Javadoc comments in samples that could be rendered badly in Android Studio have been converted to standard multi-line comments + * Consistency between samples has been improved + * The SensorDigitalTouch sample has been replaced with a new SensorTouch sample that uses the `TouchSensor` interface instead of `DigitalChannel`. + * The ConceptCompassCalibration, SensorMRCompass, and SensorMRIRSeeker samples have been deleted, as they are not useful for modern FTC competitions. + +### Bug Fixes +* Fixes a bug which prevented PlayStation gamepads from being used in bluetooth mode. Bluetooth is NOT legal for competition but may be useful to allow a DS device to be used while charging, or at an outreach event. +* Fixes a bug where a Blocks OpMode's Date Modified value can change to December 31, 1969, if the Control Hub is rebooted while the Blocks OpMode is being edited. +* Fixes the automatic TeleOp preselection feature (was broken in 8.2) +* Fixes a bug where passing an integer number such as 123 to the Telemetry.addData block that takes a number shows up as 123.0 in the telemetry. +* Fixes OnBotJava autocomplete issues: + * Autocomplete would incorrectly provide values for the current class when autocompleting a local variable + * `hardwareMap` autocomplete would incorrectly include lambda class entries +* Fixes OnBotJava not automatically importing classes. +* Fixes OnBotJava tabs failing to close when their file is deleted. +* Fixes a project view refresh not happening when a file is renamed in OnBotJava. +* Fixes the "Download" context menu item for external libraries in the OnBotJava interface. +* Fixes issue where Driver Station telemetry would intermittently freeze when set to Monospace mode. +* Fixes performance regression for certain REV Hub operations that was introduced in version 8.2. +* Fixes TagID comparison logic in DriveToTag samples. + +## Version 8.2 (20230707-131020) + +### Breaking Changes +* Non-linear (iterative) OpModes are no longer allowed to manipulate actuators in their `stop()` method. Attempts to do so will be ignored and logged. + * When an OpMode attempts to illegally manipulate an actuator, the Robot Controller will print a log message + including the text `CANCELLED_FOR_SAFETY`. + * Additionally, LinearOpModes are no longer able to regain the ability to manipulate actuators by removing their + thread's interrupt or using another thread. +* Removes support for Android version 6.0 (Marshmallow). The minSdkVersion is now 24. +* Increases the Robocol version. + * This means an 8.2 or later Robot Controller or Driver Station will not be able to communicate with an 8.1 or earlier Driver Station or Robot Controller. + * If you forget to update both apps at the same time, an error message will be shown explaining which app is older and should be updated. +* FTC_FieldCoordinateSystemDefinition.pdf has been moved. It is still in the git history, but has been removed from the git snapshot corresponding with the 8.2 tag. The official version now lives at [Field Coordinate System](https://ftc-docs.firstinspires.org/field-coordinate-system). +* `LynxUsbDevice.addConfiguredModule()` and `LynxUsbDevice.getConfiguredModule()` have been replaced with `LynxUsbDevice.getOrAddModule()`. +* Old Blocks for Vuforia and TensorFlow Object Detection are obsolete and have been removed from the + Blocks editor's toolbox. Existing Blocks OpModes that contain the old Blocks for Vuforia or + TensorFlow Object Detection can be opened in the Blocks editor, but running them will not work. + +### New features +* Adds new `VisionPortal` API for computer vision + * **This API may be subject to change for final kickoff release!** + * Several new samples added. + * Adds support for detecting AprilTags. + * `VisionPortal` is the new entry point for both AprilTag and TFOD processing. + * Vuforia will be removed in a future release. + * Updated TensorFlow dependencies. + * Added support for webcam camera controls to blocks. + * The Blocks editor's toolbox now has a Vision category, directly above the Utilities category. +* Related documentation for associated technologies can be found at + * [AprilTag Introduction](https://ftc-docs.firstinspires.org/apriltag-intro) + * [AprilTag SDK Guide](https://ftc-docs.firstinspires.org/apriltag-sdk) + * [AprilTag Detection Values](https://ftc-docs.firstinspires.org/apriltag-detection-values) + * [AprilTag Test Images](https://ftc-docs.firstinspires.org/apriltag-test-images) + * [Camera Calibration](https://ftc-docs.firstinspires.org/camera-calibration) +* Adds Driver Station support for Logitech Dual Action and Sony PS5 DualSense gamepads. + * This **does not** include support for the Sony PS5 DualSense Edge gamepad. + * Always refer to Game Manual 1 to determine gamepad legality in competition. +* Adds support for MJPEG payload streaming to UVC driver (external JPEG decompression routine required for use). +* Shows a hint on the Driver Station UI about how to bind a gamepad when buttons are pressed or the sticks are moved on an unbound gamepad. +* Adds option for fullscreening "Camera Stream" on Driver Station. +* OnBotJava source code is automatically saved as a ZIP file on every build with a rolling window of the last 30 builds kept; allows recovering source code from previous builds if code is accidentally deleted or corrupted. +* Adds support for changing the addresses of Expansion Hubs that are not connected directly via USB. + * The Expansion Hub Address Change screen now has an Apply button that changes the addresses without leaving the screen. + * Addresses that are assigned to other hubs connected to the same USB connection or Control Hub are no longer able to be selected. +* Increases maximum size of Blocks inline comments to 100 characters +* Saves position of open Blocks comment balloons +* Adds new AprilTag Driving samples: RobotDriveToAprilTagTank & RobotDriveToAprilTagOmni +* Adds Sample to illustrate optimizing camera exposure for AprilTags: ConceptAprilTagOptimizeExposure + +### Bug Fixes +* Corrects inspection screen to report app version using the SDK version defined in the libraries instead of the version specified in `AndroidManifest.xml`. This corrects the case where the app could show matching versions numbers to the user but still state that the versions did not match. + * If the version specified in `AndroidManifest.xml` does not match the SDK version, an SDK version entry will be displayed on the Manage webpage. +* Fixes no error being displayed when saving a configuration file with duplicate names from the Driver Station. +* Fixes a deadlock in the UVC driver which manifested in https://github.com/OpenFTC/EasyOpenCV/issues/57. +* Fixes a deadlock in the UVC driver that could occur when hot-plugging cameras. +* Fixes UVC driver compatibility with Arducam OV9281 global shutter camera. +* Fixes Emergency Stop condition when an OnBotJava build with duplicate OpMode names occurs. +* Fixes known causes of "Attempted use of a closed LynxModule instance" logspam. +* Fixes the visual identification LED pattern when configuring Expansion Hubs connected via RS-485. + +## Version 8.1.1 (20221201-150726) + +This is a bug fix only release to address the following four issues. + +* [Issue #492](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/492) - Can't create new blocks opmodes. +* [Issue #495](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/495) - Remove the final modifier from the OpMode's Telemetry object. +* [Issue #500](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/500) - Some devices cannot be configured when the Driver Station app has been updated to 8.1 + * Updating either the Robot Controller app or the Driver Station app to 8.1.1 or later will fix this issue. +* The Modern Robotics touch sensor was configurable as a Digital Device. It can only be used as an Analog Device. + +## Version 8.1 (20221121-115119) + +### Breaking Changes +* Deprecates the `OpMode` fields `msStuckDetectInit`, `msStuckDetectInitLoop`, `msStuckDetectStart`, `msStuckDetectLoop`, and `msStuckDetectStop`. + * OpModes no longer have a time limit for `init()`, `init_loop()`, `start()` or `loop()`, so the fields corresponding to those methods are no longer used. + * `stop()` still has a time limit, but it is now hardcoded to be 1 second, and cannot be changed using `msStuckDetectStop`. +* Deprecates the `OpMode` methods `internalPreInit()`, `internalPostInitLoop()`, and `internalPostLoop()`. + * Iterative `OpMode`s will continue to call these methods in case they were overridden. + * These methods will not be called at all for `LinearOpMode`s. +* Deprecates (and stops respecting) `DeviceProperties.xmlTagAliases`. + +### Enhancements +* Adds a new `IMU` interface to Blocks and Java that can be used with both the original BNO055 IMU + included in all older Control Hubs and Expansion Hubs, and the new alternative BHI260AP IMU. + * You can determine which type of IMU is in your Control Hub by navigating to the Manage page of the web interface. + * To learn how to use the new `IMU` interface, see https://ftc-docs.firstinspires.org/programming_resources/imu/imu.html. The `SensorIMU` Blocks sample was also updated to use the new `IMU` interface, and the following Java samples were added: + * `SensorIMUOrthogonal` + * Use this sample if your REV Hub is mounted so that it is parallel or perpendicular to the + bottom of your robot. + * `SensorIMUNonOrthogonal` + * Use this sample if your REV Hub is mounted to your robot in any other orientation + * `ConceptExploringIMUOrientations` + * This OpMode is a tool to help you understand how the orthogonal orientations work, and + which one applies to your robot. + * The BHI260AP IMU can only be accessed via the new `IMU` interface. The BNO055 IMU can be + programmed using the new `IMU` interface, or you can continue to program it using the old `BNO055IMU` + interface. If you want to be able to quickly switch to a new Control Hub that may contain the + BHI260AP IMU, you should migrate your code to use the new `IMU` interface. + * Unlike the old `BNO055IMU` interface, which only worked correctly when the REV Hub was mounted flat + on your robot, the `IMU` interface allows you to specify the orientation of the REV Hub on your + robot. It will account for this, and give you your orientation in a Robot Coordinate System, + instead of a special coordinate system for the REV Hub. As a result, your pitch and yaw will be + 0 when your *robot* is level, instead of when the REV Hub is level, which will result in much + more reliable orientation angle values for most mounting orientations. + * Because of the new robot-centric coordinate system, the pitch and roll angles returned by the + `IMU` interface will be different from the ones returned by the `BNO055IMU` interface. When you are + migrating your code, pay careful attention to the documentation. + * If you have calibrated your BNO055, you can provide that calibration data to the new `IMU` + interface by passing a `BNO055IMUNew.Parameters` instance to `IMU.initialize()`. + * The `IMU` interface is also suitable for implementation by third-party vendors for IMUs that + support providing the orientation in the form of a quaternion. +* Iterative `OpMode`s (as opposed to `LinearOpMode`s) now run on a dedicated thread. + * Cycle times should not be as impacted by everything else going on in the system. + * Slow `OpMode`s can no longer increase the amount of time it takes to process network commands, and vice versa. + * The `init()`, `init_loop()`, `start()` and `loop()` methods no longer need to return within a certain time frame. +* BNO055 IMU legacy driver: restores the ability to initialize in one OpMode, and then have another OpMode re-use that + initialization. This allows you to maintain the 0-yaw position between OpModes, if desired. +* Allows customized versions of device drivers in the FTC SDK to use the same XML tag. + * Before, if you wanted to customize a device driver, you had to copy it to a new class _and_ give + it a new XML tag. Giving it a new XML tag meant that to switch which driver was being used, you + had to modify your configuration file. + * Now, to use your custom driver, all you have to do is specify your custom driver's class when + calling `hardwareMap.get()`. To go back to the original driver, specify the original driver + class. If you specify an interface that is implemented by both the original driver and the + custom driver, there is no guarantee about which implementation will be returned. + +### Bug Fixes +* Fixes accessing the "Manage TensorFlow Lite Models" and "Manage Sounds" links and performing + Blocks and OnBotJava OpMode downloads from the REV Hardware Client. +* Fixes issue where an I2C device driver would be auto-initialized using the parameters assigned in + a previous OpMode run. +* Improves Driver Station popup menu placement in the landscape layout. +* Fixes NullPointerException when attempting to get a non-configured BNO055 IMU in a Blocks OpMode on an RC phone. +* Fixes problem with Blocks if a variable is named `orientation`. + +## Version 8.0 (20220907-131644) + +### Breaking Changes +* Increases the Robocol version. + * This means an 8.0 or later Robot Controller or Driver Station will not be able to communicate with a 7.2 or earlier Driver Station or Robot Controller. + * If you forget to update both apps at the same time, an error message will be shown explaining which app is older and should be updated. +* Initializing I2C devices now happens when you retrieve them from the `HardwareMap` for the first time. + * Previously, all I2C devices would be initialized before the OpMode even began executing, + whether you were actually going to use them or not. This could result in reduced performance and + unnecessary warnings. + * With this change, it is very important for Java users to retrieve all needed devices from the + `HardwareMap` **during the Init phase of the OpMode**. Namely, declare a variable for each hardware + device the OpMode will use, and assign a value to each. Do not do this during the Run phase, or your + OpMode may briefly hang while the devices you are retrieving get initialized. + * OpModes that do not use all of the I2C devices specified in the configuration file should take + less time to initialize. OpModes that do use all of the specified I2C devices should take the + same amount of time as previously. +* Fixes [issue #251](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/251) by changing the order in which axis rotation rates are read from the angular velocity vector in the BNO055 IMU driver. +* Deprecates `pitchMode` in `BNO055IMU.Parameters`. + * Setting `pitchMode` to `PitchMode.WINDOWS` would break the coordinate conventions used by the driver. +* Moves `OpModeManagerImpl` to the `com.qualcomm.robotcore.eventloop.opmode` package. + * This breaks third party libraries EasyOpenCV (version 1.5.1 and earlier) and FTC Dashboard (version 0.4.4 and earlier). +* Deletes the deprecated `OpMode` method `resetStartTime()` (use `resetRuntime()` instead). +* Deletes the protected `LinearOpMode.LinearOpModeHelper` class (which was not meant for use by OpModes). +* Removes I2C Device (Synchronous) config type (deprecated since 2018) + +### Enhancements +* Uncaught exceptions in OpModes no longer require a Restart Robot + * A blue screen popping up with a stacktrace is not an SDK error; this replaces the red text in the telemetry area. + * Since the very first SDK release, OpMode crashes have put the robot into "EMERGENCY STOP" state, only showing the first line of the exception, and requiring the user to press "Restart Robot" to continue + * Exceptions during an OpMode now open a popup window with the same color scheme as the log viewer, containing 15 lines of the exception stacktrace to allow easily tracing down the offending line without needing to connect to view logs over ADB or scroll through large amounts of logs in the log viewer. + * The exception text in the popup window is both zoomable and scrollable just like a webpage. + * Pressing the "OK" button in the popup window will return to the main screen of the Driver Station and allow an OpMode to be run again immediately, without the need to perform a "Restart Robot" +* Adds new Java sample to demonstrate using a hardware class to abstract robot actuators, and share them across multiple OpModes. + * Sample OpMode is [ConceptExternalHardwareClass.java](FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExternalHardwareClass.java) + * Abstracted hardware class is [RobotHardware.java](FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotHardware.java) +* Updates RobotAutoDriveByGyro_Linear Java sample to use REV Control/Expansion hub IMU. +* Updates Vuforia samples to reference PowerPlay assets and have correct names and field locations of image targets. +* Updates TensorFlow samples to reference PowerPlay assets. +* Adds opt-in support for Java 8 language features to the OnBotJava editor. + * To opt in, open the OnBotJava Settings, and check `Enable beta Java 8 support`. + * Note that Java 8 code will only compile when the Robot Controller runs Android 7.0 Nougat or later. + * Please report issues [here](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues). +* In OnBotJava, clicking on build errors now correctly jumps to the correct location. +* Improves OnBotJava autocomplete behavior, to provide better completion options in most cases. +* Adds a QR code to the Robot Controller Inspection Report when viewed from the Driver Station for scanning by inspectors at competition. +* Improves I2C performance and reliability in some scenarios. + +## Version 7.2 (20220723-130006) + +### Breaking Changes +* Updates the build tooling. For Android Studio users, this change requires Android Studio Chipmunk 2021.2.1. +* Removes support for devices that are not competition legal, including Modern Robotics Core Control Modules, the Matrix Controller, and HiTechnic/NXT controllers and sensors. Support remains for Modern Robotics I2C sensors. + +### Enhancements +* Increases the height of the 3-dots Landscape menu touch area on the Driver Station, making it much easier to select. +* Adds `terminateOpModeNow()` method to allow OpModes to cleanly self-exit immediately. +* Adds `opModeInInit()` method to `LinearOpMode` to facilitate init-loops. Similar to `opModeIsActive()` but for the init phase. +* Warns user if they have a Logitech F310 gamepad connected that is set to DirectInput mode. +* Allows SPARKmini motor controllers to react more quickly to speed changes. +* Hides the version number of incorrectly installed sister app (i.e. DS installed on RC device or vice-versa) on inspection screen. +* Adds support for allowing the user to edit the comment for the runOpMode block. +* Adds parameterDefaultValues field to @ExportToBlocks. This provides the ability for a java method with an @ExportToBlocks annotation to specify default values for method parameters when it is shown in the block editor. +* Make LinearOpMode blocks more readable. The opmode name is displayed on the runOpMode block, but not on the other LinearOpMode blocks. +* Added support to TensorFlow Object Detection for using a different frame generator, instead of Vuforia. + Using Vuforia to pass the camera frame to TFOD is still supported. +* Removes usage of Renderscript. +* Fixes logspam on app startup of repeated stacktraces relating to `"Failed resolution of: Landroid/net/wifi/p2p/WifiP2pManager$DeviceInfoListener"` +* Allows disabling bluetooth radio from inspection screen +* Improves warning messages when I2C devices are not responding +* Adds support for controlling the RGB LED present on PS4/Etpark gamepads from OpModes +* Removes legacy Pushbot references from OpMode samples. Renames "Pushbot" samples to "Robot". Motor directions reversed to be compatible with "direct Drive" drive train. + + +### Bug fixes +* Fixes [issue #316](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/316) (MatrixF.inverted() returned an incorrectly-sized matrix for 1x1 and 2x2 matrixes). +* Self inspect now allows for Driver Station and Robot Controller compatibility between point releases. +* Fixes bug where if the same `RumbleEffect` object instance was queued for multiple gamepads, it + could happen that both rumble commands would be sent to just one gamepad. +* Fixes bug in Driver Station where on the Driver Hub, if Advanced Gamepad Features was disabled and + an officially supported gamepad was connected, then opening the Advanced Gamepad Features or + Gamepad Type Overrides screens would cause the gamepad to be rebound by the custom USB driver even + though advanced gamepad features was disabled. +* Protects against (unlikely) null pointer exception in Vuforia Localizer. +* Harden OnBotJava and Blocks saves to protect against save issues when disconnecting from Program and Manage +* Fixes issue where the RC app would hang if a REV Hub I2C write failed because the previous I2C + operation was still in progress. This hang most commonly occurred during REV 2M Distance Sensor initialization +* Removes ConceptWebcam.java sample program. This sample is not compatible with OnBotJava. +* Fixes bug where using html tags in an @ExportToBlocks comment field prevented the blocks editor from loading. +* Fixes blocks editor so it doesn't ask you to save when you haven't modified anything. +* Fixes uploading a very large blocks project to offline blocks editor. +* Fixes bug that caused blocks for DcMotorEx to be omitted from the blocks editor toolbox. +* Fixes [Blocks Programs Stripped of Blocks (due to using TensorFlow Label block)](https://ftcforum.firstinspires.org/forum/ftc-technology/blocks-programming/87035-blocks-programs-stripped-of-blocks) + +## Version 7.1 (20211223-120805) + +* Fixes crash when calling `isPwmEnabled()` ([issue #223](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/233)). +* Fixes lint error ([issue #4](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/4)). +* Fixes Driver Station crash when attempting to use DualShock4 v1 gamepad with Advanced Gamepad Features enabled ([issue #173](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/173)). +* Fixes possible (but unlikely) Driver Station crash when connecting gamepads of any type. +* Fixes bug where Driver Station would use generic 20% deadzone for Xbox360 and Logitech F310 gamepads when Advanced Gamepad Features was disabled. +* Added SimpleOmniDrive sample OpMode. +* Adds UVC white balance control API. +* Fixes [issue #259](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/259) Most blocks samples for TensorFlow can't be used for a different model. + * The blocks previously labeled TensorFlowObjectDetectionFreightFrenzy (from the subcategory named "Optimized for Freight Frenzy") and TensorFlowObjectDetectionCustomModel (from the subcategory named "Custom Model") have been replaced with blocks labeled TensorFlowObjectDetection. Blocks in existing opmodes will be automatically updated to the new blocks when opened in the blocks editor. +* Fixes [issue #260](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/260) Blocks can't call java method that has a VuforiaLocalizer parameter. + * Blocks now has a block labeled VuforiaFreightFrenzy.getVuforiaLocalizer for this. +* Added a page to manage the TensorFlow Lite models in /sdcard/FIRST/tflitemodels. To get to the TFLite Models page: + * You can click on the link at the bottom of the the Manage page. + * You can click on the link at the upper-right the Blocks project page. +* Fixes logspam when `isBusy()` is called on a motor not in RTP mode. +* Hides the "RC Password" item on the inspection screen for phone-based Robot Controllers. (It is only applicable for Control Hubs). +* Adds channel 165 to Wi-Fi Direct channel selection menu in the settings screen. (165 was previously available through the web UI, but not locally in the app). + +## Version 7.0 (20210915-141025) + +### Enhancements and New Features +* Adds support for external libraries to OnBotJava and Blocks. + * Upload .jar and .aar files in OnBotJava. + * Known limitation - RobotController device must be running Android 7.0 or greater. + * Known limitation - .aar files with assets are not supported. + * External libraries can provide support for hardware devices by using the annotation in the + com.qualcomm.robotcore.hardware.configuration.annotations package. + * External libraries can include .so files for native code. + * External libraries can be used from OnBotJava OpModes. + * External libraries that use the following annotations can be used from Blocks OpModes. + * org.firstinspires.ftc.robotcore.external.ExportClassToBlocks + * org.firstinspires.ftc.robotcore.external.ExportToBlocks + * External libraries that use the following annotations can add new hardware devices: + * com.qualcomm.robotcore.hardware.configuration.annotations.AnalogSensorType + * com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties + * com.qualcomm.robotcore.hardware.configuration.annotations.DigitalIoDeviceType + * com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType + * com.qualcomm.robotcore.hardware.configuration.annotations.MotorType + * com.qualcomm.robotcore.hardware.configuration.annotations.ServoType + * External libraries that use the following annotations can add new functionality to the Robot Controller: + * org.firstinspires.ftc.ftccommon.external.OnCreate + * org.firstinspires.ftc.ftccommon.external.OnCreateEventLoop + * org.firstinspires.ftc.ftccommon.external.OnCreateMenu + * org.firstinspires.ftc.ftccommon.external.OnDestroy + * org.firstinspires.ftc.ftccommon.external.WebHandlerRegistrar +* Adds support for REV Robotics Driver Hub. +* Adds fully custom userspace USB gamepad driver to Driver Station (see "Advanced Gamepad Features" menu in DS settings). + * Allows gamepads to work on devices without native Linux kernel support (e.g. some Romanian Motorola devices). + * Allows the DS to read the unique serial number of each gamepad, enabling auto-recovery of dropped gamepads even if two gamepads of the same model drop. *(NOTE: unfortunately this does not apply to Etpark gamepads, because they do not have a unique serial)*. + * Reading the unique serial number also provides the ability to configure the DS to assign gamepads to a certain position by default (so no need to do start+a/b at all). + * The LED ring on the Xbox360 gamepad and the RGB LED bar on the PS4 gamepad is used to indicate the driver position the gamepad is bound to. + * The rumble motors on the Xbox360, PS4, and Etpark gamepads can be controlled from OpModes. + * The 2-point touchpad on the PS4 gamepad can be read from OpModes. + * The "back" and "guide" buttons on the gamepad can now be safely bound to robot controls (Previously, on many devices, Android would intercept these buttons as home button presses and close the app). + * Advanced Gamepad features are enabled by default, but may be disabled through the settings menu in order to revert to gamepad support provided natively by Android. +* Improves accuracy of ping measurement. + * Fixes issue where the ping time showed as being higher than reality when initially connecting to or restarting the robot. + * To see the full improvement, you must update both the Robot Controller and Driver Station apps. +* Updates samples located at [/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples](FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples). + * Added ConceptGamepadRumble and ConceptGamepadTouchpad samples to illustrate the use of these new gampad capabilities. + * Condensed existing Vuforia samples into just 2 samples (ConceptVuforiaFieldNavigation & ConceptVuforiaFieldNavigationWebcam) showing how to determine the robot's location on the field using Vuforia. These both use the current season's Target images. + * Added ConceptVuforiaDriveToTargetWebcam to illustrate an easy way to drive directly to any visible Vuforia target. +* Makes many improvements to the warning system and individual warnings. + * Warnings are now much more spaced out, so that they are easier to read. + * New warnings were added for conditions that should be resolved before competing. + * The mismatched apps warning now uses the major and minor app versions, not the version code. + * The warnings are automatically re-enabled when a Robot Controller app from a new FTC season is installed. +* Adds support for I2C transactions on the Expansion Hub / Control Hub without specifying a register address. + * See section 3 of the [TI I2C spec](https://www.ti.com/lit/an/slva704/slva704.pdf). + * Calling these new methods when using Modern Robotics hardware will result in an UnsupportedOperationException. +* Changes VuforiaLocalizer `close()` method to be public. +* Adds support for TensorFlow v2 object detection models. +* Reduces ambiguity of the Self Inspect language and graphics. +* OnBotJava now warns about potentially unintended file overwrites. +* Improves behavior of the Wi-Fi band and channel selector on the Manage webpage. + +### Bug fixes + * Fixes Robot Controller app crash on Android 9+ when a Driver Station connects. + * Fixes issue where an OpMode was responsible for calling shutdown on the + TensorFlow TFObjectDetector. Now this is done automatically. + * Fixes Vuforia initialization blocks to allow user to chose AxesOrder. Updated + relevant blocks sample opmodes. + * Fixes [FtcRobotController issue #114](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/114) + LED blocks and Java class do not work. + * Fixes match logging for OpModes that contain special characters in their names. + * Fixes Driver Station OpMode controls becoming unresponsive if the Driver Station was set to the landscape layout and an OnBotJava build was triggered while an OpMode was running. + * Fixes the Driver Station app closing itself when it is switched away from, or the screen is turned off. + * Fixes "black swirl of doom" (Infinite "configuring Wi-Fi Direct" message) on older devices. + * Updates the wiki comment on the OnBotJava intro page. + +## Version 6.2 (20210218-074821) + +### Enhancements +* Attempts to automatically fix the condition where a Control Hub's internal Expansion Hub is not + working by re-flashing its firmware +* Makes various improvements to the Wi-Fi Direct pairing screen, especially in landscape mode +* Makes the Robot Controller service no longer be categorically restarted when the main activity is brought to foreground + * (e.g. the service is no longer restarted simply by viewing the Self Inspect screen and pressing the back button) + * It is still restarted if the Settings menu or Configure Robot menu is opened + + +### Bug fixes +* Fixes [FtcRobotController issue #71](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/71) + Cannot open OpModes in v6.1 Blocks offline editor +* Fixes [FtcRobotController issue #79](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/79) + 6.1 causes a soft reboot on the Motorola E5 Play +* Fixes issue where the Control Hub OS's watchdog would restart the Robot Controller app if + the Control Hub was not able to communicate with its internal Expansion Hub +* Fixes certain I2C devices not showing up in the appropriate `HardwareMap` fields (such as `hardwareMap.colorSensor`) +* Fixes issue where performing a Wi-Fi factory reset on the Control Hub would not set the Wi-Fi band to 2.4 GHz +* Fixes issue where OnBotJava might fail to create a new file if the option to "Setup Code for Configured Hardware" was selected +* Fixes issue where performing certain operations after an OpMode crashes would temporarily break Control/Expansion Hub communication +* Fixes issue where a Control Hub with a configured USB-connected Expansion Hub would not work if the Expansion Hub was missing at startup +* Fixes potential issues caused by having mismatched Control/Expansion Hub firmware versions +* Fixes [ftc_app issue 673](https://github.com/ftctechnh/ftc_app/issues/673) Latest matchlog is being deleted instead of old ones by RobotLog +* Fixes ConceptVuforiaUltimateGoalNavigationWebcam sample opmode by correctly orienting camera on robot. +* Fixes issue where logcat would be spammed with InterruptedExceptions when stop is requested from the Driver Station (this behavior was accidentally introduced in v5.3). This change has no impact on functionality. +* Fixes issue where the blocks editor fails to load if the name of any TeleOp opmode contains an apostrophe. + +## Version 6.1 (20201209-113742) +* Makes the scan button on the configuration screen update the list of Expansion Hubs connected via RS-485 + * Fixes [SkyStone issue #143](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/143) +* Improves web interface compatibility with older browser and Android System WebView versions. +* Fixes issue in UVC driver where some cameras (e.g. certain MS Lifecams) which reported frame intervals as rounded rather than truncated values (e.g. `666667*100ns` instead of `666666*100ns` for 15FPS) would fail to start streaming. +* Adds support in UVC driver for virtual PTZ control +* Adds support in UVC driver for gain (ISO) control +* Adds support in UVC driver for enabling/disable AE priority. This setting provides a means to tell the camera firmware either + * A) It can undershoot the requested frame rate in order to provide a theoretically better image (i.e. with a longer exposure than the inter-frame period of the selected frame rate allows) + * B) It *must* meet the inter-frame deadline for the selected frame rate, even if the image may be underexposed as a result +* Adds support for the Control Hub OS 1.1.2 Robot Controller watchdog + * The Robot Controller app will be restarted if it stops responding for more than 10 seconds +* Adds support for using the Driver Station app on Android 10+ +* Introduces an automatic TeleOp preselection feature + * For details and usage guide, please see [this wiki entry](https://github.com/FIRST-Tech-Challenge/FtcRobotController/wiki/Automatically-Loading-a-Driver-Controlled-Op-Mode) +* Shows icon next to OpMode name in the OpMode list dropdown on the Driver Station to indicate the source of the OpMode (i.e. the programming tool used to create it) +* Fixes issue where the Driver Station app would exit after displaying the Configuring Wi-Fi Direct screen +* Fixes Blocks and OnBotJava prompts when accessed via the REV Hardware Client + +## Version 6.0 (20200921-085816) + +### Important Notes +* Version 6.0 is the version for the Ultimate Goal season. +* Requires Android Studio 4.0. +* Android Studio users need to be connected to the Internet the first time they build the app (in order to download needed packages for the build). +* Version 5.5 was a moderately large off-season, August 2020, drop. It's worth reviewing those release notes below also. +* Version 5.5 and greater will not work on older Android 4.x and 5.x phones. Users must upgrade to an approved Android 6.x device or newer. +* The default PIDF values for REV motors have been reverted to the default PID values that were used in the 2018-2019 season + * This change was made because the 2018-2019 values turned out to work better for many mechanisms + * This brings the behavior of the REV motors in line with the behavior of all other motors + * If you prefer the 2019-2020 season's behavior for REV motors, here are the PIDF values that were in place, so that you can manually set them in your OpModes: +
+ **HD Hex motors (all gearboxes):** + Velocity PIDF values: `P = 1.17`, `I = 0.117`, `F = 11.7` + Position PIDF values: `P = 5.0` + **Core Hex motor:** + Velocity PIDF values: `P = 4.96`, `I = 0.496`, `F = 49.6` + Position PIDF values: `P = 5.0` + +### New features +* Includes TensorFlow inference model and sample OpModes to detect Ultimate Goal Starter Stacks (four rings vs single ring stack). +* Includes Vuforia Ultimate Goal vision targets and sample OpModes. +* Introduces a digital zoom feature for TensorFlow object detection (to detect objects more accurately at greater distances). +* Adds configuration entry for the REV UltraPlanetary HD Hex motor + +### Enhancements +* Adds setGain() and getGain() methods to the NormalizedColorSensor interface + * By setting the gain of a color sensor, you can adjust for different lighting conditions. + For example, if you detect lower color values than expected, you can increase the gain. + * The gain value is only applied to the argb() and getNormalizedColors() methods, not to the raw color methods. + The getNormalizedColors() method is recommended for ease-of-use and clarity, since argb() has to be converted. + * Updates SensorColor Java sample to demonstrate gain usage +* Merges SensorREVColorDistance Java sample into SensorColor Java sample, which showcases best practices for all color sensors +* Improves retrieving values from the REV Color Sensor V3 + * Updates the normalization calculation of the RGB channels + * Improves the calculation of the alpha channel (can be used as an overall brightness indicator) + * Fixes the default sensor resolution, which caused issues with bright environments + * Adds support for changing the resolution and measuring rate of the Broadcom sensor chip + * Removes IR readings and calculations not meant for the Broadcom sensor chip + +### Bug fixes +* Improves reliability of BNO055IMU IMU initialization to prevent random initialization failures (which manifested as `Problem with 'imu'`). + +## Version 5.5 (20200824-090813) + +Version 5.5 requires Android Studio 4.0 or later. + +### New features +* Adds support for calling custom Java classes from Blocks OpModes (fixes [SkyStone issue #161](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/161)). + * Classes must be in the org.firstinspires.ftc.teamcode package. + * To have easy access to the opMode, hardwareMap, telemetry, gamepad1, and gamepad2, classes can + extends org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion. + * Methods must be public static and have no more than 21 parameters. + * Methods must be annotated with org.firstinspires.ftc.robotcore.external.ExportToBlocks. + * Parameters declared as OpMode, LinearOpMode, Telemetry, and HardwareMap are supported and the + argument is provided automatically, regardless of the order of the parameters. On the block, + the sockets for those parameters are automatically filled in. + * Parameters declared as char or java.lang.Character will accept any block that returns text + and will only use the first character in the text. + * Parameters declared as boolean or java.lang.Boolean will accept any block that returns boolean. + * Parameters declared as byte, java.lang.Byte, short, java.lang.Short, int, java.lang.Integer, + long, or java.lang.Long, will accept any block that returns a number and will round that + value to the nearest whole number. + * Parameters declared as float, java.lang.Float, double, java.lang.Double will accept any + block that returns a number. +* Adds telemetry API method for setting display format + * Classic + * Monospace + * HTML (certain tags only) +* Adds blocks support for switching cameras. +* Adds Blocks support for TensorFlow Object Detection with a custom model. +* Adds support for uploading a custom TensorFlow Object Detection model in the Manage page, which + is especially useful for Blocks and OnBotJava users. +* Shows new Control Hub blink codes when the Wi-Fi band is switched using the Control Hub's button (only possible on Control Hub OS 1.1.2) +* Adds new warnings which can be disabled in the Advanced RC Settings + * Mismatched app versions warning + * Unnecessary 2.4 GHz Wi-Fi usage warning + * REV Hub is running outdated firmware (older than version 1.8.2) +* Adds support for Sony PS4 gamepad, and reworks how gamepads work on the Driver Station + * Removes preference which sets gamepad type based on driver position. Replaced with menu which allows specifying type for gamepads with unknown VID and PID + * Attempts to auto-detect gamepad type based on USB VID and PID + * If gamepad VID and PID is not known, use type specified by user for that VID and PID + * If gamepad VID and PID is not known AND the user has not specified a type for that VID and PID, an educated guess is made about how to map the gamepad +* Driver Station will now attempt to automatically recover from a gamepad disconnecting, and re-assign it to the position it was assigned to when it dropped + * If only one gamepad is assigned and it drops: it can be recovered + * If two gamepads are assigned, and have **different** VID/PID signatures, and only one drops: it will be recovered + * If two gamepads are assigned, and have **different** VID/PID signatures, and BOTH drop: both will be recovered + * If two gamepads are assigned, and have **the same** VID/PID signatures, and only one drops: it will be recovered + * If two gamepads are assigned, and have **the same** VID/PID signatures, and BOTH drop: **neither** will be recovered, because of the ambiguity of the gamepads when they re-appear on the USB bus. + * There is currently one known edge case: if there are **two** gamepads with **the same** VID/PID signature plugged in, **but only one is assigned**, and they BOTH drop, it's a 50-50 chance of which one will be chosen for automatic recovery to the assigned position: it is determined by whichever one is re-enumerated first by the USB bus controller. +* Adds landscape user interface to Driver Station + * New feature: practice timer with audio cues + * New feature (Control Hub only): wireless network connection strength indicator (0-5 bars) + * New feature (Control Hub only): tapping on the ping/channel display will switch to an alternate display showing radio RX dBm and link speed (tap again to switch back) + * The layout will NOT autorotate. You can switch the layout from the Driver Station's settings menu. +### Breaking changes +* Removes support for Android versions 4.4 through 5.1 (KitKat and Lollipop). The minSdkVersion is now 23. +* Removes the deprecated `LinearOpMode` methods `waitOneFullHardwareCycle()` and `waitForNextHardwareCycle()` +### Enhancements +* Handles RS485 address of Control Hub automatically + * The Control Hub is automatically given a reserved address + * Existing configuration files will continue to work + * All addresses in the range of 1-10 are still available for Expansion Hubs + * The Control Hub light will now normally be solid green, without blinking to indicate the address + * The Control Hub will not be shown on the Expansion Hub Address Change settings page +* Improves REV Hub firmware updater + * The user can now choose between all available firmware update files + * Version 1.8.2 of the REV Hub firmware is bundled into the Robot Controller app. + * Text was added to clarify that Expansion Hubs can only be updated via USB. + * Firmware update speed was reduced to improve reliability + * Allows REV Hub firmware to be updated directly from the Manage webpage +* Improves log viewer on Robot Controller + * Horizontal scrolling support (no longer word wrapped) + * Supports pinch-to-zoom + * Uses a monospaced font + * Error messages are highlighted + * New color scheme +* Attempts to force-stop a runaway/stuck OpMode without restarting the entire app + * Not all types of runaway conditions are stoppable, but if the user code attempts to talk to hardware during the runaway, the system should be able to capture it. +* Makes various tweaks to the Self Inspect screen + * Renames "OS version" entry to "Android version" + * Renames "Wi-Fi Direct Name" to "Wi-Fi Name" + * Adds Control Hub OS version, when viewing the report of a Control Hub + * Hides the airplane mode entry, when viewing the report of a Control Hub + * Removes check for ZTE Speed Channel Changer + * Shows firmware version for **all** Expansion and Control Hubs +* Reworks network settings portion of Manage page + * All network settings are now applied with a single click + * The Wi-Fi Direct channel of phone-based Robot Controllers can now be changed from the Manage page + * Wi-Fi channels are filtered by band (2.4 vs 5 GHz) and whether they overlap with other channels + * The current Wi-Fi channel is pre-selected on phone-based Robot Controllers, and Control Hubs running OS 1.1.2 or later. + * On Control Hubs running OS 1.1.2 or later, you can choose to have the system automatically select a channel on the 5 GHz band +* Improves OnBotJava + * New light and dark themes replace the old themes (chaos, github, chrome,...) + * the new default theme is `light` and will be used when you first update to this version + * OnBotJava now has a tabbed editor + * Read-only offline mode +* Improves function of "exit" menu item on Robot Controller and Driver Station + * Now guaranteed to be fully stopped and unloaded from memory +* Shows a warning message if a LinearOpMode exists prematurely due to failure to monitor for the start condition +* Improves error message shown when the Driver Station and Robot Controller are incompatible with each other +* Driver Station OpMode Control Panel now disabled while a Restart Robot is in progress +* Disables advanced settings related to Wi-Fi Direct when the Robot Controller is a Control Hub. +* Tint phone battery icons on Driver Station when low/critical. +* Uses names "Control Hub Portal" and "Control Hub" (when appropriate) in new configuration files +* Improve I2C read performance + * Very large improvement on Control Hub; up to ~2x faster with small (e.g. 6 byte) reads + * Not as apparent on Expansion Hubs connected to a phone +* Update/refresh build infrastructure + * Update to 'androidx' support library from 'com.android.support:appcompat', which is end-of-life + * Update targetSdkVersion and compileSdkVersion to 28 + * Update Android Studio's Android plugin to latest + * Fix reported build timestamp in 'About' screen +* Add sample illustrating manual webcam use: ConceptWebcam + + +### Bug fixes +* Fixes [SkyStone issue #248](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/248) +* Fixes [SkyStone issue #232](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/232) and + modifies bulk caching semantics to allow for cache-preserving MANUAL/AUTO transitions. +* Improves performance when REV 2M distance sensor is unplugged +* Improves readability of Toast messages on certain devices +* Allows a Driver Station to connect to a Robot Controller after another has disconnected +* Improves generation of fake serial numbers for UVC cameras which do not provide a real serial number + * Previously some devices would assign such cameras a serial of `0:0` and fail to open and start streaming + * Fixes [ftc_app issue #638](https://github.com/ftctechnh/ftc_app/issues/638). +* Fixes a slew of bugs with the Vuforia camera monitor including: + * Fixes bug where preview could be displayed with a wonky aspect ratio + * Fixes bug where preview could be cut off in landscape + * Fixes bug where preview got totally messed up when rotating phone + * Fixes bug where crosshair could drift off target when using webcams +* Fixes issue in UVC driver on some devices ([ftc_app 681](https://github.com/ftctechnh/ftc_app/issues/681)) if streaming was started/stopped multiple times in a row + * Issue manifested as kernel panic on devices which do not have [this kernel patch](https://lore.kernel.org/patchwork/patch/352400/). + * On affected devices which **do** have the patch, the issue was manifest as simply a failure to start streaming. + * The Tech Team believes that the root cause of the issue is a bug in the Linux kernel XHCI driver. A workaround was implemented in the SDK UVC driver. +* Fixes bug in UVC driver where often half the frames from the camera would be dropped (e.g. only 15FPS delivered during a streaming session configured for 30FPS). +* Fixes issue where TensorFlow Object Detection would show results whose confidence was lower than + the minimum confidence parameter. +* Fixes a potential exploitation issue of [CVE-2019-11358](https://www.cvedetails.com/cve/CVE-2019-11358/) in OnBotJava +* Fixes changing the address of an Expansion Hub with additional Expansion Hubs connected to it +* Preserves the Control Hub's network connection when "Restart Robot" is selected +* Fixes issue where device scans would fail while the Robot was restarting +* Fix RenderScript usage + * Use androidx.renderscript variant: increased compatibility + * Use RenderScript in Java mode, not native: simplifies build +* Fixes webcam-frame-to-bitmap conversion problem: alpha channel wasn't being initialized, only R, G, & B +* Fixes possible arithmetic overflow in Deadline +* Fixes deadlock in Vuforia webcam support which could cause 5-second delays when stopping OpMode + +## Version 5.4 (20200108-101156) +* Fixes [SkyStone issue #88](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/88) +* Adds an inspection item that notes when a robot controller (Control Hub) is using the factory default password. +* Fixes [SkyStone issue #61](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/61) +* Fixes [SkyStone issue #142](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/142) +* Fixes [ftc_app issue #417](https://github.com/ftctechnh/ftc_app/issues/417) by adding more current and voltage monitoring capabilities for REV Hubs. +* Fixes [a crash sometimes caused by OnBotJava activity](https://ftcforum.firstinspires.org/forum/ftc-technology/76217-onbotjava-crashes-robot-controller) +* Improves OnBotJava autosave functionality [ftc_app #738](https://github.com/ftctechnh/ftc_app/issues/738) +* Fixes system responsiveness issue when an Expansion Hub is disconnected +* Fixes issue where IMU initialization could prevent OpModes from stopping +* Fixes issue where AndroidTextToSpeech.speak() would fail if it was called too early +* Adds telemetry.speak() methods and blocks, which cause the Driver Station (if also updated) to speak text +* Adds and improves Expansion Hub-related warnings + * Improves Expansion Hub low battery warning + * Displays the warning immediately after the hub reports it + * Specifies whether the condition is current or occurred temporarily during an OpMode run + * Displays which hubs reported low battery + * Displays warning when hub loses and regains power during an OpMode run + * Fixes the hub's LED pattern after this condition + * Displays warning when Expansion Hub is not responding to commands + * Specifies whether the condition is current or occurred temporarily during an OpMode run + * Clarifies warning when Expansion Hub is not present at startup + * Specifies that this condition requires a Robot Restart before the hub can be used. + * The hub light will now accurately reflect this state + * Improves logging and reduces log spam during these conditions +* Syncs the Control Hub time and timezone to a connected web browser programming the robot, if a Driver Station is not available. +* Adds bulk read functionality for REV Hubs + * A bulk caching mode must be set at the Hub level with `LynxModule#setBulkCachingMode()`. This applies to all relevant SDK hardware classes that reference that Hub. + * The following following Hub bulk caching modes are available: + * `BulkCachingMode.OFF` (default): All hardware calls operate as usual. Bulk data can read through `LynxModule#getBulkData()` and processed manually. + * `BulkCachingMode.AUTO`: Applicable hardware calls are served from a bulk read cache that is cleared/refreshed automatically to ensure identical commands don't hit the same cache. The cache can also be cleared manually with `LynxModule#clearBulkCache()`, although this is not recommended. + * (advanced users) `BulkCachingMode.MANUAL`: Same as `BulkCachingMode.AUTO` except the cache is never cleared automatically. To avoid getting stale data, the cache must be manually cleared at the beginning of each loop body or as the user deems appropriate. +* Removes PIDF Annotation values added in Rev 5.3 (to AndyMark, goBILDA and TETRIX motor configurations). + * The new motor types will still be available but their Default control behavior will revert back to Rev 5.2 +* Adds new `ConceptMotorBulkRead` sample Opmode to demonstrate and compare Motor Bulk-Read modes for reducing I/O latencies. + +## Version 5.3 (20191004-112306) +* Fixes external USB/UVC webcam support +* Makes various bugfixes and improvements to Blocks page, including but not limited to: + * Many visual tweaks + * Browser zoom and window resize behave better + * Resizing the Java preview pane works better and more consistently across browsers + * The Java preview pane consistently gets scrollbars when needed + * The Java preview pane is hidden by default on phones + * Internet Explorer 11 should work + * Large dropdown lists display properly on lower res screens + * Disabled buttons are now visually identifiable as disabled + * A warning is shown if a user selects a TFOD sample, but their device is not compatible + * Warning messages in a Blocks OpMode are now visible by default. +* Adds goBILDA 5201 and 5202 motors to Robot Configurator +* Adds PIDF Annotation values to AndyMark, goBILDA and TETRIX motor configurations. + This has the effect of causing the RUN_USING_ENCODERS and RUN_TO_POSITION modes to use + PIDF vs PID closed loop control on these motors. This should provide more responsive, yet stable, speed control. + PIDF adds Feedforward control to the basic PID control loop. + Feedforward is useful when controlling a motor's speed because it "anticipates" how much the control voltage + must change to achieve a new speed set-point, rather than requiring the integrated error to change sufficiently. + The PIDF values were chosen to provide responsive, yet stable, speed control on a lightly loaded motor. + The more heavily a motor is loaded (drag or friction), the more noticable the PIDF improvement will be. +* Fixes startup crash on Android 10 +* Fixes [ftc_app issue #712](https://github.com/ftctechnh/ftc_app/issues/712) (thanks to FROGbots-4634) +* Fixes [ftc_app issue #542](https://github.com/ftctechnh/ftc_app/issues/542) +* Allows "A" and lowercase letters when naming device through RC and DS apps. + +## Version 5.2 (20190905-083277) +* Fixes extra-wide margins on settings activities, and placement of the new configuration button +* Adds Skystone Vuforia image target data. + * Includes sample Skystone Vuforia Navigation OpModes (Java). + * Includes sample Skystone Vuforia Navigation OpModes (Blocks). +* Adds TensorFlow inference model (.tflite) for Skystone game elements. + * Includes sample Skystone TensorFlow OpModes (Java). + * Includes sample Skystone TensorFlow OpModes (Blocks). +* Removes older (season-specific) sample OpModes. +* Includes 64-bit support (to comply with [Google Play requirements](https://android-developers.googleblog.com/2019/01/get-your-apps-ready-for-64-bit.html)). +* Protects against Stuck OpModes when a Restart Robot is requested. (Thanks to FROGbots-4634) ([ftc_app issue #709](https://github.com/ftctechnh/ftc_app/issues/709)) +* Blocks related changes: + * Fixes bug with blocks generated code when hardware device name is a java or javascript reserved word. + * Shows generated java code for blocks, even when hardware items are missing from the active configuration. + * Displays warning icon when outdated Vuforia and TensorFlow blocks are used ([SkyStone issue #27](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/27)) + +## Version 5.1 (20190820-222104) +* Defines default PIDF parameters for the following motors: + * REV Core Hex Motor + * REV 20:1 HD Hex Motor + * REV 40:1 HD Hex Motor +* Adds back button when running on a device without a system back button (such as a Control Hub) +* Allows a REV Control Hub to update the firmware on a REV Expansion Hub via USB +* Fixes [SkyStone issue #9](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/9) +* Fixes [ftc_app issue #715](https://github.com/ftctechnh/ftc_app/issues/715) +* Prevents extra DS User clicks by filtering based on current state. +* Prevents incorrect DS UI state changes when receiving new OpMode list from RC +* Adds support for REV Color Sensor V3 +* Adds a manual-refresh DS Camera Stream for remotely viewing RC camera frames. + * To show the stream on the DS, initialize **but do not run** a stream-enabled opmode, select the Camera Stream option in the DS menu, and tap the image to refresh. This feature is automatically enabled when using Vuforia or TFOD—no additional RC configuration is required for typical use cases. To hide the stream, select the same menu item again. + * Note that gamepads are disabled and the selected opmode cannot be started while the stream is open as a safety precaution. + * To use custom streams, consult the API docs for `CameraStreamServer#setSource` and `CameraStreamSource`. +* Adds many Star Wars sounds to RobotController resources. +* Added Skystone Sounds Chooser Sample Program. +* Switches out startup, connect chimes, and error/warning sounds for Star Wars sounds +* Updates OnBot Java to use a WebSocket for communication with the robot + * The OnBot Java page no longer has to do a full refresh when a user switches from editing one file to another + +Known issues: +* Camera Stream + * The Vuforia camera stream inherits the issues present in the phone preview (namely [ftc_app issue #574](https://github.com/ftctechnh/ftc_app/issues/574)). This problem does not affect the TFOD camera stream even though it receives frames from Vuforia. + * The orientation of the stream frames may not always match the phone preview. For now, these frames may be rotated manually via a custom `CameraStreamSource` if desired. +* OnBotJava + * Browser back button may not always work correctly + * It's possible for a build to be queued, but not started. The OnBot Java build console will display a warning if this occurs. + * A user might not realize they are editing a different file if the user inadvertently switches from one file to another since this switch is now seamless. The name of the currently open file is displayed in the browser tab. + +## Version 5.0 (built on 19.06.14) + * Support for the REV Robotics Control Hub. + * Adds a Java preview pane to the Blocks editor. + * Adds a new offline export feature to the Blocks editor. + * Display Wi-Fi channel in Network circle on Driver Station. + * Adds calibration for Logitech C270 + * Updates build tooling and target SDK. + * Compliance with Google's permissions infrastructure (Required after build tooling update). + * Keep Alives to mitigate the Motorola Wi-Fi scanning problem. Telemetry substitute no longer necessary. + * Improves Vuforia error reporting. + * Fixes ftctechnh/ftc_app issues 621, 713. + * Miscellaneous bug fixes and improvements. + +## Version 4.3 (built on 18.10.31) + * Includes missing TensorFlow-related libraries and files. + +## Version 4.2 (built on 18.10.30) + * Includes fix to avoid deadlock situation with WatchdogMonitor which could result in USB communication errors. + - Comm error appeared to require that user disconnect USB cable and restart the Robot Controller app to recover. + - robotControllerLog.txt would have error messages that included the words "E RobotCore: lynx xmit lock: #### abandoning lock:" + * Includes fix to correctly list the parent module address for a REV Robotics Expansion Hub in a configuration (.xml) file. + - Bug in versions 4.0 and 4.1 would incorrect list the address module for a parent REV Robotics device as "1". + - If the parent module had a higher address value than the daisy-chained module, then this bug would prevent the Robot Controller from communicating with the downstream Expansion Hub. + * Added requirement for ACCESS_COARSE_LOCATION to allow a Driver Station running Android Oreo to scan for Wi-Fi Direct devices. + * Added google() repo to build.gradle because aapt2 must be downloaded from the google() repository beginning with version 3.2 of the Android Gradle Plugin. + - Important Note: Android Studio users will need to be connected to the Internet the first time build the ftc_app project. + - Internet connectivity is required for the first build so the appropriate files can be downloaded from the Google repository. + - Users should not need to be connected to the Internet for subsequent builds. + - This should also fix buid issue where Android Studio would complain that it "Could not find com.android.tools.lint:lint-gradle:26.1.4" (or similar). + * Added support for REV Spark Mini motor controller as part of the configuration menu for a servo/PWM port on the REV Expansion Hub. + * Provide examples for playing audio files in an OpMode. + * Block Development Tool Changes + - Includes a fix for a problem with the Velocity blocks that were reported in the FTC Technology forum (Blocks Programming subforum). + - Change the "Save completed successfully." message to a white color so it will contrast with a green background. + - Fixed the "Download image" feature so it will work if there are text blocks in the OpMode. + * Introduce support for Google's TensorFlow Lite technology for object detetion for 2018-2019 game. + - TensorFlow lite can recognize Gold Mineral and Silver Mineral from 2018-2019 game. + - Example Java and Block OpModes are included to show how to determine the relative position of the gold block (left, center, right). + +## Version 4.1 (released on 18.09.24) + +Changes include: + * Fix to prevent crash when deprecated configuration annotations are used. + * Change to allow FTC Robot Controller APK to be auto-updated using FIRST Global Control Hub update scripts. + * Removed samples for non supported / non legal hardware. + * Improvements to Telemetry.addData block with "text" socket. + * Updated Blocks sample OpMode list to include Rover Ruckus Vuforia example. + * Update SDK library version number. + +## Version 4.0 (released on 18.09.12) + +Changes include: + * Initial support for UVC compatible cameras + - If UVC camera has a unique serial number, RC will detect and enumerate by serial number. + - If UVC camera lacks a unique serial number, RC will only support one camera of that type connected. + - Calibration settings for a few cameras are included (see TeamCode/src/main/res/xml/teamwebcamcalibrations.xml for details). + - User can upload calibration files from Program and Manage web interface. + - UVC cameras seem to draw a fair amount of electrical current from the USB bus. + + This does not appear to present any problems for the REV Robotics Control Hub. + + This does seem to create stability problems when using some cameras with an Android phone-based Robot Controller. + + FTC Tech Team is investigating options to mitigate this issue with the phone-based Robot Controllers. + - Updated sample Vuforia Navigation and VuMark OpModes to demonstrate how to use an internal phone-based camera and an external UVC webcam. + + * Support for improved motor control. + - REV Robotics Expansion Hub firmware 1.8 and greater will support a feed forward mechanism for closed loop motor control. + - FTC SDK has been modified to support PIDF coefficients (proportional, integral, derivative, and feed forward). + - FTC Blocks development tool modified to include PIDF programming blocks. + - Deprecated older PID-related methods and variables. + - REV's 1.8.x PIDF-related changes provide a more linear and accurate way to control a motor. + + * Wireless + - Added 5GHz support for wireless channel changing for those devices that support it. + + Tested with Moto G5 and E4 phones. + + Also tested with other (currently non-approved) phones such as Samsung Galaxy S8. + +* Improved Expansion Hub firmware update support in Robot Controller app + - Changes to make the system more robust during the firmware update process (when performed through Robot Controller app). + - User no longer has to disconnect a downstream daisy-chained Expansion Hub when updating an Expansion Hub's firmware. + + If user is updating an Expansion Hub's firmware through a USB connection, he/she does not have to disconnect RS485 connection to other Expansion Hubs. + + The user still must use a USB connection to update an Expansion Hub's firmware. + + The user cannot update the Expansion Hub firmware for a downstream device that is daisy chained through an RS485 connection. + - If an Expansion Hub accidentally gets "bricked" the Robot Controller app is now more likely to recognize the Hub when it scans the USB bus. + + Robot Controller app should be able to detect an Expansion Hub, even if it accidentally was bricked in a previous update attempt. + + Robot Controller app should be able to install the firmware onto the Hub, even if if accidentally was bricked in a previous update attempt. + + * Resiliency + - FTC software can detect and enable an FTDI reset feature that is available with REV Robotics v1.8 Expansion Hub firmware and greater. + + When enabled, the Expansion Hub can detect if it hasn't communicated with the Robot Controller over the FTDI (USB) connection. + + If the Hub hasn't heard from the Robot Controller in a while, it will reset the FTDI connection. + + This action helps system recover from some ESD-induced disruptions. + - Various fixes to improve reliability of FTC software. + + * Blocks + - Fixed errors with string and list indices in blocks export to java. + - Support for USB connected UVC webcams. + - Refactored optimized Blocks Vuforia code to support Rover Ruckus image targets. + - Added programming blocks to support PIDF (proportional, integral, derivative and feed forward) motor control. + - Added formatting options (under Telemetry and Miscellaneous categories) so user can set how many decimal places to display a numerical value. + - Support to play audio files (which are uploaded through Blocks web interface) on Driver Station in addition to the Robot Controller. + - Fixed bug with Download Image of Blocks feature. + - Support for REV Robotics Blinkin LED Controller. + - Support for REV Robotics 2m Distance Sensor. + - Added support for a REV Touch Sensor (no longer have to configure as a generic digital device). + - Added blocks for DcMotorEx methods. + + These are enhanced methods that you can use when supported by the motor controller hardware. + + The REV Robotics Expansion Hub supports these enhanced methods. + + Enhanced methods include methods to get/set motor velocity (in encoder pulses per second), get/set PIDF coefficients, etc.. + + * Modest Improvements in Logging + - Decrease frequency of battery checker voltage statements. + - Removed non-FTC related log statements (wherever possible). + - Introduced a "Match Logging" feature. + + Under "Settings" a user can enable/disable this feature (it's disabled by default). + + If enabled, user provides a "Match Number" through the Driver Station user interface (top of the screen). + * The Match Number is used to create a log file specifically with log statements from that particular OpMode run. + * Match log files are stored in /sdcard/FIRST/matlogs on the Robot Controller. + * Once an OpMode run is complete, the Match Number is cleared. + * This is a convenient way to create a separate match log with statements only related to a specific OpMode run. + + * New Devices + - Support for REV Robotics Blinkin LED Controller. + - Support for REV Robotics 2m Distance Sensor. + - Added configuration option for REV 20:1 HD Hex Motor. + - Added support for a REV Touch Sensor (no longer have to configure as a generic digital device). + + * Miscellaneous + - Fixed some errors in the definitions for acceleration and velocity in our javadoc documentation. + - Added ability to play audio files on Driver Station + - When user is configuring an Expansion Hub, the LED on the Expansion Hub will change blink pattern (purple-cyan) to indicate which Hub is currently being configured. + - Renamed I2cSensorType to I2cDeviceType. + - Added an external sample OpMode that demonstrates localization using 2018-2019 (Rover Ruckus presented by QualComm) Vuforia targets. + - Added an external sample OpMode that demonstrates how to use the REV Robotics 2m Laser Distance Sensor. + - Added an external sample OpMode that demonstrates how to use the REV Robotics Blinkin LED Controller. + - Re-categorized external Java sample OpModes to "TeleOp" instead of "Autonomous". + +Known issues: + * Initial support for UVC compatible cameras + - UVC cameras seem to draw significant amount of current from the USB bus. + + This does not appear to present any problems for the REV Robotics Control Hub. + + This does seem to create stability problems when using some cameras with an Android phone-based Robot Controller. + + FTC Tech Team is investigating options to mitigate this issue with the phone-based Robot Controllers. + - There might be a possible deadlock which causes the RC to become unresponsive when using a UVC webcam with a Nougat Android Robot Controller. + + * Wireless + - When user selects a wireless channel, this channel does not necessarily persist if the phone is power cycled. + + Tech Team is hoping to eventually address this issue in a future release. + + Issue has been present since apps were introduced (i.e., it is not new with the v4.0 release). + - Wireless channel is not currently displayed for Wi-Fi Direct connections. + + * Miscellaneous + - The blink indication feature that shows which Expansion Hub is currently being configured does not work for a newly created configuration file. + + User has to first save a newly created configuration file and then close and re-edit the file in order for blink indicator to work. + +## Version 3.6 (built on 17.12.18) + +Changes include: + * Blocks Changes + - Uses updated Google Blockly software to allow users to edit their OpModes on Apple iOS devices (including iPad and iPhone). + - Improvement in Blocks tool to handle corrupt OpMode files. + - Autonomous OpModes should no longer get switched back to tele-op after re-opening them to be edited. + - The system can now detect type mismatches during runtime and alert the user with a message on the Driver Station. + * Updated javadoc documentation for setPower() method to reflect correct range of values (-1 to +1). + * Modified VuforiaLocalizerImpl to allow for user rendering of frames + - Added a user-overrideable onRenderFrame() method which gets called by the class's renderFrame() method. + +## Version 3.5 (built on 17.10.30) + +Changes with version 3.5 include: + * Introduced a fix to prevent random OpMode stops, which can occur after the Robot Controller app has been paused and then resumed (for example, when a user temporarily turns off the display of the Robot Controller phone, and then turns the screen back on). + * Introduced a fix to prevent random OpMode stops, which were previously caused by random peer disconnect events on the Driver Station. + * Fixes issue where log files would be closed on pause of the RC or DS, but not re-opened upon resume. + * Fixes issue with battery handler (voltage) start/stop race. + * Fixes issue where Android Studio generated OpModes would disappear from available list in certain situations. + * Fixes problem where OnBot Java would not build on REV Robotics Control Hub. + * Fixes problem where OnBot Java would not build if the date and time on the Robot Controller device was "rewound" (set to an earlier date/time). + * Improved error message on OnBot Java that occurs when renaming a file fails. + * Removed unneeded resources from android.jar binaries used by OnBot Java to reduce final size of Robot Controller app. + * Added MR_ANALOG_TOUCH_SENSOR block to Blocks Programming Tool. + +## Version 3.4 (built on 17.09.06) + +Changes with version 3.4 include: + * Added telemetry.update() statement for BlankLinearOpMode template. + * Renamed sample Block OpModes to be more consistent with Java samples. + * Added some additional sample Block OpModes. + * Reworded OnBot Java readme slightly. + +## Version 3.3 (built on 17.09.04) + +This version of the software includes improves for the FTC Blocks Programming Tool and the OnBot Java Programming Tool. + +Changes with verion 3.3 include: + * Android Studio ftc_app project has been updated to use Gradle Plugin 2.3.3. + * Android Studio ftc_app project is already using gradle 3.5 distribution. + * Robot Controller log has been renamed to /sdcard/RobotControllerLog.txt (note that this change was actually introduced w/ v3.2). + * Improvements in I2C reliability. + * Optimized I2C read for REV Expansion Hub, with v1.7 firmware or greater. + * Updated all external/samples (available through OnBot and in Android project folder). + * Vuforia + - Added support for VuMarks that will be used for the 2017-2018 season game. + * Blocks + - Update to latest Google Blockly release. + - Sample OpModes can be selected as a template when creating new OpMode. + - Fixed bug where the blocks would disappear temporarily when mouse button is held down. + - Added blocks for Range.clip and Range.scale. + - User can now disable/enable Block OpModes. + - Fix to prevent occasional Blocks deadlock. + * OnBot Java + - Significant improvements with autocomplete function for OnBot Java editor. + - Sample OpModes can be selected as a template when creating new OpMode. + - Fixes and changes to complete hardware setup feature. + - Updated (and more useful) onBot welcome message. + +Known issues: + * Android Studio + - After updating to the new v3.3 Android Studio project folder, if you get error messages indicating "InvalidVirtualFileAccessException" then you might need to do a File->Invalidate Caches / Restart to clear the error. + * OnBot Java + - Sometimes when you push the build button to build all OpModes, the RC returns an error message that the build failed. If you press the build button a second time, the build typically suceeds. + +## Version 3.2 (built on 17.08.02) + +This version of the software introduces the "OnBot Java" Development Tool. Similar to the FTC Blocks Development Tool, the FTC OnBot Java Development Tool allows a user to create, edit and build OpModes dynamically using only a Javascript-enabled web browser. + +The OnBot Java Development Tool is an integrated development environment (IDE) that is served up by the Robot Controller. OpModes are created and edited using a Javascript-enabled browser (Google Chromse is recommended). OpModes are saved on the Robot Controller Android device directly. + +The OnBot Java Development Tool provides a Java programming environment that does NOT need Android Studio. + + + +Changes with version 3.2 include: + * Enhanced web-based development tools + - Introduction of OnBot Java Development Tool. + - Web-based programming and management features are "always on" (user no longer needs to put Robot Controller into programming mode). + - Web-based management interface (where user can change Robot Controller name and also easily download Robot Controller log file). + - OnBot Java, Blocks and Management features available from web based interface. + +* Blocks Programming Development Tool: + - Changed "LynxI2cColorRangeSensor" block to "REV Color/range sensor" block. + - Fixed tooltip for ColorSensor.isLightOn block. + Added blocks for ColorSensor.getNormalizedColors and LynxI2cColorRangeSensor.getNormalizedColors. + +* Added example OpModes for digital touch sensor and REV Robotics Color Distance sensor. +* User selectable color themes. +* Includes many minor enhancements and fixes (too numerous to list). + +Known issues: +* Auto complete function is incomplete and does not support the following (for now): + - Access via *this* keyword + - Access via *super* keyword + - Members of the super cloass, not overridden by the class + - Any methods provided in the current class + - Inner classes + - Can't handle casted objects + - Any objects coming from an parenthetically enclosed expression + +## Version 3.10 (built on 17.05.09) + +This version of the software provides support for the REV Robotics Expansion Hub. This version also includes improvements in the USB communication layer in an effort to enhance system resiliency. If you were using a 2.x version of the software previously, updating to version 3.1 requires that you also update your Driver Station software in addition to updating the Robot Controller software. + +Also note that in version 3.10 software, the setMaxSpeed and getMaxSpeed methods are no longer available (not deprecated, they have been removed from the SDK). Also note that the the new 3.x software incorporates motor profiles that a user can select as he/she configures the robot. + +Changes include: + * Blocks changes + - Added VuforiaTrackableDefaultListener.getPose and Vuforia.trackPose blocks. + - Added optimized blocks support for Vuforia extended tracking. + - Added atan2 block to the math category. + - Added useCompetitionFieldTargetLocations parameter to Vuforia.initialize block. If set to false, the target locations are placed at (0,0,0) with target orientation as specified in https://github.com/gearsincorg/FTCVuforiaDemo/blob/master/Robot_Navigation.java tutorial OpMode. + * Incorporates additional improvements to USB comm layer to improve system resiliency (to recover from a greater number of communication disruptions). + +************************************************************************************** + +Additional Notes Regarding Version 3.00 (built on 17.04.13) + +In addition to the release changes listed below (see section labeled "Version 3.00 (built on 17.04.013)"), version 3.00 has the following important changes: + +1. Version 3.00 software uses a new version of the FTC Robocol (robot protocol). If you upgrade to v3.0 on the Robot Controller and/or Android Studio side, you must also upgrade the Driver Station software to match the new Robocol. +2. Version 3.00 software removes the setMaxSpeed and getMaxSpeed methods from the DcMotor class. If you have an OpMode that formerly used these methods, you will need to remove the references/calls to these methods. Instead, v3.0 provides the max speed information through the use of motor profiles that are selected by the user during robot configuration. +3. Version 3.00 software currently does not have a mechanism to disable extra i2c sensors. We hope to re-introduce this function with a release in the near future. + +************************************************************************************** + +## Version 3.00 (built on 17.04.13) + +*** Use this version of the software at YOUR OWN RISK!!! *** + +This software is being released as an "alpha" version. Use this version at your own risk! + +This pre-release software contains SIGNIFICANT changes, including changes to the Wi-Fi Direct pairing mechanism, rewrites of the I2C sensor classes, changes to the USB/FTDI layer, and the introduction of support for the REV Robotics Expansion Hub and the REV Robotics color-range-light sensor. These changes were implemented to improve the reliability and resiliency of the FTC control system. + +Please note, however, that version 3.00 is considered "alpha" code. This code is being released so that the FIRST community will have an opportunity to test the new REV Expansion Hub electronics module when it becomes available in May. The developers do not recommend using this code for critical applications (i.e., competition use). + +*** Use this version of the software at YOUR OWN RISK!!! *** + +Changes include: + * Major rework of sensor-related infrastructure. Includes rewriting sensor classes to implement synchronous I2C communication. + * Fix to reset Autonomous timer back to 30 seconds. + * Implementation of specific motor profiles for approved 12V motors (includes Tetrix, AndyMark, Matrix and REV models). + * Modest improvements to enhance Wi-Fi P2P pairing. + * Fixes telemetry log addition race. + * Publishes all the sources (not just a select few). + * Includes Block programming improvements + - Addition of optimized Vuforia blocks. + - Auto scrollbar to projects and sounds pages. + - Fixed blocks paste bug. + - Blocks execute after while-opModeIsActive loop (to allow for cleanup before exiting OpMode). + - Added gyro integratedZValue block. + - Fixes bug with projects page for Firefox browser. + - Added IsSpeaking block to AndroidTextToSpeech. + * Implements support for the REV Robotics Expansion Hub + - Implements support for integral REV IMU (physically installed on I2C bus 0, uses same Bosch BNO055 9 axis absolute orientation sensor as Adafruit 9DOF abs orientation sensor). - Implements support for REV color/range/light sensor. + - Provides support to update Expansion Hub firmware through FTC SDK. + - Detects REV firmware version and records in log file. + - Includes support for REV Control Hub (note that the REV Control Hub is not yet approved for FTC use). + - Implements FTC Blocks programming support for REV Expansion Hub and sensor hardware. + - Detects and alerts when I2C device disconnect. + +## Version 2.62 (built on 17.01.07) + * Added null pointer check before calling modeToByte() in finishModeSwitchIfNecessary method for ModernRoboticsUsbDcMotorController class. + * Changes to enhance Modern Robotics USB protocol robustness. + +## Version 2.61 (released on 16.12.19) + * Blocks Programming mode changes: + - Fix to correct issue when an exception was thrown because an OpticalDistanceSensor object appears twice in the hardware map (the second time as a LightSensor). + +## Version 2.6 (released on 16.12.16) + * Fixes for Gyro class: + - Improve (decrease) sensor refresh latency. + - fix isCalibrating issues. + * Blocks Programming mode changes: + - Blocks now ignores a device in the configuration xml if the name is empty. Other devices work in configuration work fine. + +## Version 2.5 (internal release on released on 16.12.13) + * Blocks Programming mode changes: + - Added blocks support for AdafruitBNO055IMU. + - Added Download OpMode button to FtcBocks.html. + - Added support for copying blocks in one OpMode and pasting them in an other OpMode. The clipboard content is stored on the phone, so the programming mode server must be running. + - Modified Utilities section of the toolbox. + - In Programming Mode, display information about the active connections. + - Fixed paste location when workspace has been scrolled. + - Added blocks support for the android Accelerometer. + - Fixed issue where Blocks Upload OpMode truncated name at first dot. + - Added blocks support for Android SoundPool. + - Added type safety to blocks for Acceleration. + - Added type safety to blocks for AdafruitBNO055IMU.Parameters. + - Added type safety to blocks for AnalogInput. + - Added type safety to blocks for AngularVelocity. + - Added type safety to blocks for Color. + - Added type safety to blocks for ColorSensor. + - Added type safety to blocks for CompassSensor. + - Added type safety to blocks for CRServo. + - Added type safety to blocks for DigitalChannel. + - Added type safety to blocks for ElapsedTime. + - Added type safety to blocks for Gamepad. + - Added type safety to blocks for GyroSensor. + - Added type safety to blocks for IrSeekerSensor. + - Added type safety to blocks for LED. + - Added type safety to blocks for LightSensor. + - Added type safety to blocks for LinearOpMode. + - Added type safety to blocks for MagneticFlux. + - Added type safety to blocks for MatrixF. + - Added type safety to blocks for MrI2cCompassSensor. + - Added type safety to blocks for MrI2cRangeSensor. + - Added type safety to blocks for OpticalDistanceSensor. + - Added type safety to blocks for Orientation. + - Added type safety to blocks for Position. + - Added type safety to blocks for Quaternion. + - Added type safety to blocks for Servo. + - Added type safety to blocks for ServoController. + - Added type safety to blocks for Telemetry. + - Added type safety to blocks for Temperature. + - Added type safety to blocks for TouchSensor. + - Added type safety to blocks for UltrasonicSensor. + - Added type safety to blocks for VectorF. + - Added type safety to blocks for Velocity. + - Added type safety to blocks for VoltageSensor. + - Added type safety to blocks for VuforiaLocalizer.Parameters. + - Added type safety to blocks for VuforiaTrackable. + - Added type safety to blocks for VuforiaTrackables. + - Added type safety to blocks for enums in AdafruitBNO055IMU.Parameters. + - Added type safety to blocks for AndroidAccelerometer, AndroidGyroscope, AndroidOrientation, and AndroidTextToSpeech. + +## Version 2.4 (released on 16.11.13) + * Fix to avoid crashing for nonexistent resources. + * Blocks Programming mode changes: + - Added blocks to support OpenGLMatrix, MatrixF, and VectorF. + - Added blocks to support AngleUnit, AxesOrder, AxesReference, CameraDirection, CameraMonitorFeedback, DistanceUnit, and TempUnit. + - Added blocks to support Acceleration. + - Added blocks to support LinearOpMode.getRuntime. + - Added blocks to support MagneticFlux and Position. + - Fixed typos. + - Made blocks for ElapsedTime more consistent with other objects. + - Added blocks to support Quaternion, Velocity, Orientation, AngularVelocity. + - Added blocks to support VuforiaTrackables, VuforiaTrackable, VuforiaLocalizer, VuforiaTrackableDefaultListener. + - Fixed a few blocks. + - Added type checking to new blocks. + - Updated to latest blockly. + - Added default variable blocks to navigation and matrix blocks. + - Fixed toolbox entry for openGLMatrix_rotation_withAxesArgs. + - When user downloads Blocks-generated OpMode, only the .blk file is downloaded. + - When user uploads Blocks-generated OpMode (.blk file), Javascript code is auto generated. + - Added DbgLog support. + - Added logging when a blocks file is read/written. + - Fixed bug to properly render blocks even if missing devices from configuration file. + - Added support for additional characters (not just alphanumeric) for the block file names (for download and upload). + - Added support for OpMode flavor (“Autonomous” or “TeleOp”) and group. + * Changes to Samples to prevent tutorial issues. + * Incorporated suggested changes from public pull 216 (“Replace .. paths”). + * Remove Servo Glitches when robot stopped. + * if user hits “Cancels” when editing a configuration file, clears the unsaved changes and reverts to original unmodified configuration. + * Added log info to help diagnose why the Robot Controller app was terminated (for example, by watch dog function). + * Added ability to transfer log from the controller. + * Fixed inconsistency for AngularVelocity + * Limit unbounded growth of data for telemetry. If user does not call telemetry.update() for LinearOpMode in a timely manner, data added for telemetry might get lost if size limit is exceeded. + +## Version 2.35 (released on 16.10.06) + * Blockly programming mode - Removed unnecesary idle() call from blocks for new project. + +## Version 2.30 (released on 16.10.05) + * Blockly programming mode: + - Mechanism added to save Blockly OpModes from Programming Mode Server onto local device + - To avoid clutter, blocks are displayed in categorized folders + - Added support for DigitalChannel + - Added support for ModernRoboticsI2cCompassSensor + - Added support for ModernRoboticsI2cRangeSensor + - Added support for VoltageSensor + - Added support for AnalogInput + - Added support for AnalogOutput + - Fix for CompassSensor setMode block + * Vuforia + - Fix deadlock / make camera data available while Vuforia is running. + - Update to Vuforia 6.0.117 (recommended by Vuforia and Google to close security loophole). + * Fix for autonomous 30 second timer bug (where timer was in effect, even though it appeared to have timed out). + * opModeIsActive changes to allow cleanup after OpMode is stopped (with enforced 2 second safety timeout). + * Fix to avoid reading i2c twice. + * Updated sample OpModes. + * Improved logging and fixed intermittent freezing. + * Added digital I/O sample. + * Cleaned up device names in sample OpModes to be consistent with Pushbot guide. + * Fix to allow use of IrSeekerSensorV3. + +## Version 2.20 (released on 16.09.08) + * Support for Modern Robotics Compass Sensor. + * Support for Modern Robotics Range Sensor. + * Revise device names for Pushbot templates to match the names used in Pushbot guide. + * Fixed bug so that IrSeekerSensorV3 device is accessible as IrSeekerSensor in hardwareMap. + * Modified computer vision code to require an individual Vuforia license (per legal requirement from PTC). + * Minor fixes. + * Blockly enhancements: + - Support for Voltage Sensor. + - Support for Analog Input. + - Support for Analog Output. + - Support for Light Sensor. + - Support for Servo Controller. + +## Version 2.10 (released on 16.09.03) + * Support for Adafruit IMU. + * Improvements to ModernRoboticsI2cGyro class + - Block on reset of z axis. + - isCalibrating() returns true while gyro is calibration. + * Updated sample gyro program. + * Blockly enhancements + - support for android.graphics.Color. + - added support for ElapsedTime. + - improved look and legibility of blocks. + - support for compass sensor. + - support for ultrasonic sensor. + - support for IrSeeker. + - support for LED. + - support for color sensor. + - support for CRServo + - prompt user to configure robot before using programming mode. + * Provides ability to disable audio cues. + * various bug fixes and improvements. + +## Version 2.00 (released on 16.08.19) + * This is the new release for the upcoming 2016-2017 FIRST Tech Challenge Season. + * Channel change is enabled in the FTC Robot Controller app for Moto G 2nd and 3rd Gen phones. + * Users can now use annotations to register/disable their OpModes. + * Changes in the Android SDK, JDK and build tool requirements (minsdk=19, java 1.7, build tools 23.0.3). + * Standardized units in analog input. + * Cleaned up code for existing analog sensor classes. + * setChannelMode and getChannelMode were REMOVED from the DcMotorController class. This is important - we no longer set the motor modes through the motor controller. + * setMode and getMode were added to the DcMotor class. + * ContinuousRotationServo class has been added to the FTC SDK. + * Range.clip() method has been overloaded so it can support this operation for int, short and byte integers. + * Some changes have been made (new methods added) on how a user can access items from the hardware map. + * Users can now set the zero power behavior for a DC motor so that the motor will brake or float when power is zero. + * Prototype Blockly Programming Mode has been added to FTC Robot Controller. Users can place the Robot Controller into this mode, and then use a device (such as a laptop) that has a Javascript enabled browser to write Blockly-based OpModes directly onto the Robot Controller. + * Users can now configure the robot remotely through the FTC Driver Station app. + * Android Studio project supports Android Studio 2.1.x and compile SDK Version 23 (Marshmallow). + * Vuforia Computer Vision SDK integrated into FTC SDK. Users can use sample vision targets to get localization information on a standard FTC field. + * Project structure has been reorganized so that there is now a TeamCode package that users can use to place their local/custom OpModes into this package. + * Inspection function has been integrated into the FTC Robot Controller and Driver Station Apps (Thanks Team HazMat… 9277 & 10650!). + * Audio cues have been incorporated into FTC SDK. + * Swap mechanism added to FTC Robot Controller configuration activity. For example, if you have two motor controllers on a robot, and you misidentified them in your configuration file, you can use the Swap button to swap the devices within the configuration file (so you do not have to manually re-enter in the configuration info for the two devices). + * Fix mechanism added to all user to replace an electronic module easily. For example, suppose a servo controller dies on your robot. You replace the broken module with a new module, which has a different serial number from the original servo controller. You can use the Fix button to automatically reconfigure your configuration file to use the serial number of the new module. + * Improvements made to fix resiliency and responsiveness of the system. + * For LinearOpMode the user now must for a telemetry.update() to update the telemetry data on the driver station. This update() mechanism ensures that the driver station gets the updated data properly and at the same time. + * The Auto Configure function of the Robot Controller is now template based. If there is a commonly used robot configuration, a template can be created so that the Auto Configure mechanism can be used to quickly configure a robot of this type. + * The logic to detect a runaway OpMode (both in the LinearOpMode and OpMode types) and to abort the run, then auto recover has been improved/implemented. + * Fix has been incorporated so that Logitech F310 gamepad mappings will be correct for Marshmallow users. + +## Release 16.07.08 + + * For the ftc_app project, the gradle files have been modified to support Android Studio 2.1.x. + +## Release 16.03.30 + + * For the MIT App Inventor, the design blocks have new icons that better represent the function of each design component. + * Some changes were made to the shutdown logic to ensure the robust shutdown of some of our USB services. + * A change was made to LinearOpMode so as to allow a given instance to be executed more than once, which is required for the App Inventor. + * Javadoc improved/updated. + +## Release 16.03.09 + + * Changes made to make the FTC SDK synchronous (significant change!) + - waitOneFullHardwareCycle() and waitForNextHardwareCycle() are no longer needed and have been deprecated. + - runOpMode() (for a LinearOpMode) is now decoupled from the system's hardware read/write thread. + - loop() (for an OpMode) is now decoupled from the system's hardware read/write thread. + - Methods are synchronous. + - For example, if you call setMode(DcMotorController.RunMode.RESET_ENCODERS) for a motor, the encoder is guaranteed to be reset when the method call is complete. + - For legacy module (NXT compatible), user no longer has to toggle between read and write modes when reading from or writing to a legacy device. + * Changes made to enhance reliability/robustness during ESD event. + * Changes made to make code thread safe. + * Debug keystore added so that user-generated robot controller APKs will all use the same signed key (to avoid conflicts if a team has multiple developer laptops for example). + * Firmware version information for Modern Robotics modules are now logged. + * Changes made to improve USB comm reliability and robustness. + * Added support for voltage indicator for legacy (NXT-compatible) motor controllers. + * Changes made to provide auto stop capabilities for OpModes. + - A LinearOpMode class will stop when the statements in runOpMode() are complete. User does not have to push the stop button on the driver station. + - If an OpMode is stopped by the driver station, but there is a run away/uninterruptible thread persisting, the app will log an error message then force itself to crash to stop the runaway thread. + * Driver Station UI modified to display lowest measured voltage below current voltage (12V battery). + * Driver Station UI modified to have color background for current voltage (green=good, yellow=caution, red=danger, extremely low voltage). + * javadoc improved (edits and additional classes). + * Added app build time to About activity for driver station and robot controller apps. + * Display local IP addresses on Driver Station About activity. + * Added I2cDeviceSynchImpl. + * Added I2cDeviceSync interface. + * Added seconds() and milliseconds() to ElapsedTime for clarity. + * Added getCallbackCount() to I2cDevice. + * Added missing clearI2cPortActionFlag. + * Added code to create log messages while waiting for LinearOpMode shutdown. + * Fix so Wi-Fi Direct Config activity will no longer launch multiple times. + * Added the ability to specify an alternate i2c address in software for the Modern Robotics gyro. + +## Release 16.02.09 + + * Improved battery checker feature so that voltage values get refreshed regularly (every 250 msec) on Driver Station (DS) user interface. + * Improved software so that Robot Controller (RC) is much more resilient and “self-healing” to USB disconnects: + - If user attempts to start/restart RC with one or more module missing, it will display a warning but still start up. + - When running an OpMode, if one or more modules gets disconnected, the RC & DS will display warnings,and robot will keep on working in spite of the missing module(s). + - If a disconnected module gets physically reconnected the RC will auto detect the module and the user will regain control of the recently connected module. + - Warning messages are more helpful (identifies the type of module that’s missing plus its USB serial number). + * Code changes to fix the null gamepad reference when users try to reference the gamepads in the init() portion of their OpMode. + * NXT light sensor output is now properly scaled. Note that teams might have to readjust their light threshold values in their OpModes. + * On DS user interface, gamepad icon for a driver will disappear if the matching gamepad is disconnected or if that gamepad gets designated as a different driver. + * Robot Protocol (ROBOCOL) version number info is displayed in About screen on RC and DS apps. + * Incorporated a display filter on pairing screen to filter out devices that don’t use the “-“ format. This filter can be turned off to show all Wi-Fi Direct devices. + * Updated text in License file. + * Fixed formatting error in OpticalDistanceSensor.toString(). + * Fixed issue on with a blank (“”) device name that would disrupt Wi-Fi Direct Pairing. + * Made a change so that the Wi-Fi info and battery info can be displayed more quickly on the DS upon connecting to RC. + * Improved javadoc generation. + * Modified code to make it easier to support language localization in the future. + +## Release 16.01.04 + + * Updated compileSdkVersion for apps + * Prevent Wi-Fi from entering power saving mode + * removed unused import from driver station + * Corrrected "Dead zone" joystick code. + * LED.getDeviceName and .getConnectionInfo() return null + * apps check for ROBOCOL_VERSION mismatch + * Fix for Telemetry also has off-by-one errors in its data string sizing / short size limitations error + * User telemetry output is sorted. + * added formatting variants to DbgLog and RobotLog APIs + * code modified to allow for a long list of OpMode names. + * changes to improve thread safety of RobocolDatagramSocket + * Fix for "missing hardware leaves robot controller disconnected from driver station" error + * fix for "fast tapping of Init/Start causes problems" (toast is now only instantiated on UI thread). + * added some log statements for thread life cycle. + * moved gamepad reset logic inside of initActiveOpMode() for robustness + * changes made to mitigate risk of race conditions on public methods. + * changes to try and flag when Wi-Fi Direct name contains non-printable characters. + * fix to correct race condition between .run() and .close() in ReadWriteRunnableStandard. + * updated FTDI driver + * made ReadWriteRunnableStanard interface public. + * fixed off-by-one errors in Command constructor + * moved specific hardware implmentations into their own package. + * moved specific gamepad implemnatations to the hardware library. + * changed LICENSE file to new BSD version. + * fixed race condition when shutting down Modern Robotics USB devices. + * methods in the ColorSensor classes have been synchronized. + * corrected isBusy() status to reflect end of motion. + * corrected "back" button keycode. + * the notSupported() method of the GyroSensor class was changed to protected (it should not be public). + +## Release 15.11.04.001 + + * Added Support for Modern Robotics Gyro. + - The GyroSensor class now supports the MR Gyro Sensor. + - Users can access heading data (about Z axis) + - Users can also access raw gyro data (X, Y, & Z axes). + - Example MRGyroTest.java OpMode included. + * Improved error messages + - More descriptive error messages for exceptions in user code. + * Updated DcMotor API + * Enable read mode on new address in setI2cAddress + * Fix so that driver station app resets the gamepads when switching OpModes. + * USB-related code changes to make USB comm more responsive and to display more explicit error messages. + - Fix so that USB will recover properly if the USB bus returns garbage data. + - Fix USB initializtion race condition. + - Better error reporting during FTDI open. + - More explicit messages during USB failures. + - Fixed bug so that USB device is closed if event loop teardown method was not called. + * Fixed timer UI issue + * Fixed duplicate name UI bug (Legacy Module configuration). + * Fixed race condition in EventLoopManager. + * Fix to keep references stable when updating gamepad. + * For legacy Matrix motor/servo controllers removed necessity of appending "Motor" and "Servo" to controller names. + * Updated HT color sensor driver to use constants from ModernRoboticsUsbLegacyModule class. + * Updated MR color sensor driver to use constants from ModernRoboticsUsbDeviceInterfaceModule class. + * Correctly handle I2C Address change in all color sensors + * Updated/cleaned up OpModes. + - Updated comments in LinearI2cAddressChange.java example OpMode. + - Replaced the calls to "setChannelMode" with "setMode" (to match the new of the DcMotor method). + - Removed K9AutoTime.java OpMode. + - Added MRGyroTest.java OpMode (demonstrates how to use MR Gyro Sensor). + - Added MRRGBExample.java OpMode (demonstrates how to use MR Color Sensor). + - Added HTRGBExample.java OpMode (demonstrates how to use HT legacy color sensor). + - Added MatrixControllerDemo.java (demonstrates how to use legacy Matrix controller). + * Updated javadoc documentation. + * Updated release .apk files for Robot Controller and Driver Station apps. + +## Release 15.10.06.002 + + * Added support for Legacy Matrix 9.6V motor/servo controller. + * Cleaned up build.gradle file. + * Minor UI and bug fixes for driver station and robot controller apps. + * Throws error if Ultrasonic sensor (NXT) is not configured for legacy module port 4 or 5. + + +## Release 15.08.03.001 + + * New user interfaces for FTC Driver Station and FTC Robot Controller apps. + * An init() method is added to the OpMode class. + - For this release, init() is triggered right before the start() method. + - Eventually, the init() method will be triggered when the user presses an "INIT" button on driver station. + - The init() and loop() methods are now required (i.e., need to be overridden in the user's OpMode). + - The start() and stop() methods are optional. + * A new LinearOpMode class is introduced. + - Teams can use the LinearOpMode mode to create a linear (not event driven) program model. + - Teams can use blocking statements like Thread.sleep() within a linear OpMode. + * The API for the Legacy Module and Core Device Interface Module have been updated. + - Support for encoders with the Legacy Module is now working. + * The hardware loop has been updated for better performance. diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle new file mode 100644 index 0000000..cbc76b2 --- /dev/null +++ b/TeamCode/build.gradle @@ -0,0 +1,33 @@ +// +// build.gradle in TeamCode +// +// Most of the definitions for building your module reside in a common, shared +// file 'build.common.gradle'. Being factored in this way makes it easier to +// integrate updates to the FTC into your code. If you really need to customize +// the build definitions, you can place those customizations in this file, but +// please think carefully as to whether such customizations are really necessary +// before doing so. + + +// Custom definitions may go here + +// Include common definitions from above. +apply from: '../build.common.gradle' +apply from: '../build.dependencies.gradle' + +android { + namespace = 'org.firstinspires.ftc.teamcode' + + packagingOptions { + jniLibs.useLegacyPackaging true + } +} + +dependencies { + implementation project(':FtcRobotController') + annotationProcessor files('lib/OpModeAnnotationProcessor.jar') + + implementation 'org.apache.commons:commons-math3:3.6.1' + implementation 'com.fasterxml/jackson.core:jacson-databind:2.12.7' + implementation 'com.acmerobotics.com.roadrunner:core:0.5.6' +} diff --git a/TeamCode/lib/OpModeAnnotationProcessor.jar b/TeamCode/lib/OpModeAnnotationProcessor.jar new file mode 100644 index 0000000000000000000000000000000000000000..4825cc36d9606badc3c0eac0f61079f9c5a4d196 GIT binary patch literal 6032 zcmbVQ2UL?;(+*83p-BxzlwKk&NReKJgwU&k8X!OblRy9)0@8(msC0-@1f+)|MG>X< zBH$`gY&1cTZUOaAR##Ye@n645a?X2BX6D{^X6}9F88a9aH9LTomKK1zJZ=WqA?yHZ z09@Zn`;d`|fec*R#K=J3(n=a`@TCg?;OLfP3O)d&86bqIVkx3Zi~EWfV>yTPjt};K z&T063*y0U7^^AWM3hrs|vD|!Hc7VwW=< z^USZe+R{J+$(@&@(xgzGcK*h3phqME+wC~xVm~4Yd|4%D)xvuo6tduY12Xnl0ytqU zxrXyh{-f-AUiDB9>U^0ioy2BWCPcWSz0G8Z$C!z>+dmA!72djV%N`src3M8*kvwj! z8ZK4mT;=98(Ukdt0hs=Z5LtydWbYGpqsJ$|mE-6&OO0y{C9-?2gDPDnhuVllRMSv7>=Eo06tDbqH+jLqAri*`wQp}j|{^x&)vPV!+G;`Q5W4eh9^reuRZI? zPm!>hCzRFWt*`r4jlJYQ_9z@4X+nWKPT&ZQm`(sg^;%}qIu};K8HZsuaTwN$kk#&M zhK9Dl_0(%GqbT2TR3E4{lfE0M#I->yGYdI<4!waD{uTGKZYxf? z^wqg~t=cd14-1tqadBi=E$@qitc{^KyB$ut+Nn#$5fTcyS=65Aa?}dfMO&WLZ!J0t zZ=FA@+nR0>0PCA*Qt{X-a{e3XtWqyNR4 zVH_8d6IkafIz&Ll-AND69vB~XqT=Dd6d=f#gko;(xny_7doRspP(m1gH!gA}N~D}( zC|X4~!@yDL%Dzx1m3NdLOzpF7i3`lWX?X{H-wR}q(`#6JbKdNv4D2HY#>FDs?}D-{ zKUX`WRPuWo{G*fjnQ9b{O7dc69JSvs9XJyZGNK`LHn0X8G0U|4+!kFZ?Rks`UCZf_ zCo!AGa@}z5)zmZ_?bPAJw?-TTTD;4A&)d^5+Sa-?H56A5rm>*83~n4;x;+Cs>?&W$be#pGtaO!;J`zjZ6RkOi$qkig* zr*a<)AfZS`?T%<2^O+M{W_^&PPX(1G?|SV==RCTiAa{!N6+73DU_8bdEVHHx;vap8 zEx{f4jBy@YfLwI70zxD7Uh_jXR0NsFSZdT))YWGKdjjDwt<|zJzKho%jmWr%c{y*j z-CZOcEK4mEARsWhGD*%f7el*m0#fRYXF*~TMnXAqRi7xIzF+jAQkQa^a<+q;K88Hh zb-#6Wg2^zR<22J&h@|+fBj;EzH89+s^(bbH6Jmd;ED1^rT^E+XpD+sbv1!H9J?_o= zG@#rBbdJhs-H0*U6!(SWkQb2`XZKCVzLu_2{Hn%j*sJVuaTzTE{raA|gE`hbV)eFX zVHsa@)A&p&0afg{rn&dzpH8eccHcAgtf1S06lIbYE!=vN&IqQuUF7q2gkr5lrGs)KePp*LABX z!o7|4f&=QNdmrSqQIEHQ>~#8FJ&dIe7sk4#6G0MduZ*x&L+Us4JU1okLzn_$7HWg} zYw^-saps+%4ocX?TF2l(6b3&M1bWyc6!7+Vd=D!*I|8sAnptE^(vkcVl9bw zI-Enpk-%zpl&62ryJF>!XYLDh$=!3$?QWmC9x}|q_lep|=?NYcPiQqg6uvg@z2g_k)T} zef9tVlT6=$_us+I=6?q_VO=t~X`@}Vf553B#>)li^9}V%ERUPAX|sRX1LaDRC}|ap z=CT^D7LT#ji`O#5i%w;Wi#DWW@jq)^K#vpBg5{Pm9|Qula#B9LU|u>3>i4oez*w)a z=;}Z1Fs(YHI`j6Ore+XDZD{00qW6e_W}DX$$8v9uCwS@SF**X#+)7P;yh^eN#9$LV z;;rkD-%Z8vBE`gF@Leaj$b{TO%O=XDOND)DObNCG#H>UOo~mgz;84}deWZCZ(8$s9 zOuKXpmxl@MWj3aedN4Y%0!i`&W0#W z|GKtI^5nEgg>?~HaDh8h+4q+^_d1(1ln;!OWu zWlwyCZ0|Av!s!aK`Kn|OT@3E+YD)rpMl9BY;*3?{h0y`9W>Z4%6l~#1b_IV!CBKKg znCt_i_`0MAD+KG1L27Y*!kpsSu(U=G?#}gRl&y~yJK=tIwD_j26|E4?qx93DsP|PP z5YQ0cwYw;!9POM6VR}Gc<`h@^gHVp3%y|YQ$BsuzMjF8}2R||0smId02^y>FKsH!2 zV#}wU!5R&bi-INU!WuPMZ*<)Q1Y%$F5%7dIQG*L`4Fkxrt%w_5*N7SiH#s;j-@h;m z87-AtFv##>m1+)5ADFNVV@?=p7w5seYrTG3jM9R6D>NSRN5>TPbYQabJ|*JG0W`g%7P)0QC!+; zEi{75FACU3%aM7iYrfQ)FlPGwTL4XHF^EcX(n(;>ysYrVgA*DW&nGC}BARtsA<`Zx zhY)?uYMgxFBX&;W`wc}F4(gVck03e(-taccKE4LcV0=|7n_SP!YKeNY|H_fE4h4a0 z=Prn`MT{pYBCt|$r(Ec#!6@N@&nXGD7SDX7?#%Iyvl(T>v0b?ygI>lRvZJ-=@zuTi z^&G;rM|ldGT&Kg**+sR;JNYNHRG<5Uyvf6S^7I%HZI zwQ>8ffg|;t_`vI2Pas%bKtH)6IG&ZW`b6W6ii)Gu+9W zeUC4PF4BAypt!;^qLF)2P`WEc^wyPlKbr?6-msUpj$(%DGz9?QKuYTFEP;-c&>!q( zcQrDZ_@RbF`uL+rEMNx((EF)Ql>SxU1L2Q2E4@u7q`$KX6xvPt2LujGHRRuUa)CSz6cuPZ+ z$#k*~q4e+X!{mFE-ZEEA5=ut#o}_m_VGlD=fmg@Kbn%J-sI6Y(10*vQ1#jw~En$QW z>nc1oRjtDZH)qwDh<&*2cHRqH0Aa(fbVENYxFieS%=A-6%;Y$UTG`{{HXql?-*7aI zsk_nWAn9OJ@r12S|N$^^D?{81gdDjudb^4MM?69gpX7(jzCr`mE$jq)k1?A zih}5SN~eT}GyBS0ar1mi+#>;dE)i)M-KRlKif;q{EYq>H$v6$tq2&2~D8Ka>Kj%mJ zH}a$H=ZisOPGkIV+c|31Vu^6uL{tSUPg8E2c6+EJ^9J=WKH6MsX6>ziDJRqe zsiRVka%qyHSZI)QNsR3PE?KLbrU?!{pK=XGtspu%2q+Wk-PhYt=^`$gOqj8tWNPbs zD&Cea9+StzP-6UWSw?kzD4vSm0pIotKhE3@I+=U$*y0l|sH(q)#7qPaP9iAvK=a|R z=M>nnR()}gaY|g3KbTS z>owz4-8UfVwLcFBJX*ztW^|ir}0)qo|CQ zuUMy~%%VHwKJ#aqsK~d;sEZGqHYrVlf2N6ok{z%;8`?RaAoXNAF zZILI%@OM1_V-iFT+h4&aFSWBvA$Q{2!b}>H{f|riKYjVmJ&^mMoxn=&hqi^0;Wzj2 zqdWRld5|G`TPA=zkoZ?-f3Fc560rTK%^d)~9bPho??h2Dl5Wf4f3^Qlfc~pZ{{8s$ zkQVs{&%2i{8KAdi@w>$ixZd5A4A|QOBGEkZVn0lG!uRe@$N;`ACc8V?3Fo`JBLn)j z#Qqm|Kj3{kcVqzB2`yxR-*csNk*u literal 0 HcmV?d00001 diff --git a/TeamCode/src/main/AndroidManifest.xml b/TeamCode/src/main/AndroidManifest.xml new file mode 100644 index 0000000..3705b31 --- /dev/null +++ b/TeamCode/src/main/AndroidManifest.xml @@ -0,0 +1,11 @@ + + + + + + + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md new file mode 100644 index 0000000..71ce35a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md @@ -0,0 +1,117 @@ +## Basic Ideas +Pedro Pathing is a reactive vector based follower. What this means is that the robot dynamically +calculates a set of vectors that are required to correct error as well as to move forward and applies them. + +The robot calculates: + +* centripetal force correction +* translational correction +* heading correction +* drive vector + +These are then applied to the robot in this order until either the robot's power is maxed out or all +the vectors are applied. + +## Why Pedro Pathing? +Why use Pedro Pathing? Why not something else like Road Runner or Pure Pursuit? + +* Why not Pure Pursuit? + * Pure Pursuit searches for the farthest point on the path that's within a certain radius from the robot. Pure Pursuit will then go in a straight line to that point. This poses several problems, as a small search radius will cause some oscillations on corners, and a large search radius will cut corners on paths, which makes the paths inaccurate to real life. + * Pedro Pathing instead corrects to the closest point on the path while still following the path. This ensures that the follower will stay on the path while still being able to move forward along the path without cutting corners or encountering oscillation issues. +* Why not Road Runner? + * Road Runner is a motion profile based follower, which means that a set of instructions for motor powers are calculated for each path beforehand and then run. After reaching the end of this motion profile, Road Runner corrects. This can be sufficient for most situations, but if the robot encounters an obstacle or wheel slippage, it may be unable to correct in time. + * Pedro Pathing instead dynamically corrects throughout the path. The movement vectors are calculated at every point along the path, and because of this, the path can even be changed midway through and Pedro Pathing will still be able to correct. Since correction occurs throughout the path, the error correction isn't concentrated on the end of the path and therefore the robot is able to better minimize error. + +## How Does Pedro Path? +As mentioned in the *Basic Ideas* section, Pedro Pathing calculates a set of vectors to move the +robot along a path, which is defined with Bezier curves. Here, we'll go more in-depth on how these +vectors are calculated and used in path following. + +### The Hierarchy +While following paths, sometimes all these motion vectors are demanding more power from the robot +than it actually has. How do we deal with this? + +Our motion vectors are applied in order of importance, which is the order they appear in within the +list in *Basic Ideas*. The centripetal force vector is the highest importance, since it ensures the +robot sticks to the path. If the robot is far off the path, then the robot will not drive along the +path, and so the centripetal force vector will be reduced in magnitude. This is why ranking the +centripetal force correction above the translational correction doesn't produce issues. The next +highest, of course, is the translational correction. This corrects strictly the robot's position to +the closest point on the path. The reasoning behind this is that it is usually much more important +that the robot be on the path, and so avoid potential obstacles, rather than facing the correct +direction. The third highest important vector is the heading correction, which turns the robot to +the correct angle. This is higher than the drive vector since we don't want to drive forward if the +robot isn't facing the correct direction. Finally, the drive vector is applied. This ensures that +the robot only continues on the path when there aren't any major issues with the following. + +As each vector is applied, the robot checks to see if the sum of the applied vectors is greater than +the power that the drivetrain can produce, which would be 1 motor power. If the magnitude of the +combined vectors is greater than 1, then the most recently added vector is scaled down until the +combined vectors' magnitude is equal to 1. If all vectors are able to be applied without exceeding +the power limit, then all the vectors can just be applied without issue. + +### Centripetal Force Correction +Have you ever noticed that your robot seems to want to swing outwards when taking corners? This is +due to a lack of centripetal force correction. In order to take curves effectively, your robot must +accelerate towards the inside of the curve. If we can approximate the region of the path the robot +is at with a circle, then we can use the formula for centripetal force to calculate how much power +we need to allocate to approximate a centripetal force. + +Because paths are defined with Bezier curves, we can easily take the first and second derivative of +the path, expressed as vectors. We can use that to calculate the curvature of the path, which is the +inverse of the length of the radius of the circle we can use to approximate the path. The actual +formula for the calculations of the curvature is the cross product of the first derivative and +second derivative, divided by the magnitude of the first derivative raised to the power of 3. + +With this, along with the weight of the robot and the velocity of the robot along the path, we can +calculate the force necessary to keep the robot on the path, and then tune a scaling factor to turn +that force into a corresponding power for the robot. + +### Translational Correction +This is as simple as it sounds: this corrects error in the robot's position only. However, the +robot's translational error is actually used within two different PIDs to move the robot. A large +PID and a small PID are used. The large PID is used when error exceeds a certain limit. and the small +PID is used when the error is within that limit. + +When the robot encounters error, the large PID, if applicable, is used to bring the robot within the +small PID error range without much overshoot to avoid oscillations. Then, the small PID is used to +bring the robot to within acceptable error ranges. The reason for this double PID is to allow for +aggressive correction to bring the robot within tight tolerances, while not correcting too +aggressively at larger ranges, which would've caused oscillations. + +### Heading Correction +The heading correction operates very similarly to the translational correction, except this corrects +the direction the robot is facing. It also uses a small and large PID rather than a single PID, like +the translational correction. The heading correction will turn in the closest direction from the +robot's current heading to the target heading. + +### Drive Vector +The drive vector points in the direction of the tangent of the path and it is responsible for moving +the robot along the path. Similar to the translational and heading corrections, the drive vector +uses a large and small PID. Using basic kinematics equations, we can use the velocity of the robot +along the path, the length of path left, and a specified target rate of deceleration to calculate +the velocity we should be moving at. Additionally, after finding out the rate of deceleration of the +robot under 0 power, we can compensate for that with another kinematics equation. Combining these +two lets us control our velocity to both move along the path quickly and brake without overshooting. + +## Additional Capabilities +In addition to following paths, Pedro Pathing can be used for a few other purposes. + +### Holding Points +Pedro Pathing is also capable of holding a specified position and direction. This can be useful for +improving on the end accuracy of paths, providing additional correction time if possible. It can +also be useful in cases where contact with other robots might occur. For instance, in the 2022-2023 +FTC season challenge, Power Play, robots might come into contact when scoring on a contested middle +junction. Pedro Pathing would be able to recover and correct from a robot collision, allowing for +more consistent scoring. + +### TeleOp Enhancements +Finally, Pedro Pathing can be used in TeleOp to enhance driving. With regular mecanum drive, robots +will tend to swing out when taking corners. Pedro Pathing can account for that, allowing the robot +to take corners more smoothly and efficiently. Using the same localizer as is used in autonomous, a +first and second derivative can be estimated from previous positions. Then, with a modified version +of the curvature formula, we can estimate a centripetal force correction and apply it under driver +control. + +## Questions? +If you still have more questions, feel free to contact us at `scottsbots10158@gmail.com` \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md new file mode 100644 index 0000000..a8cf0cc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md @@ -0,0 +1,29 @@ +## Welcome! +This is the Pedro Pathing path following program developed by FTC team 10158 Scott's Bots in the +2023-2024 Centerstage season. + +## Installation +The quickest way to get started is with the quickstart [here](https://github.com/brotherhobo/Pedro-Pathing-Quickstart). + +Otherwise, take the `pedroPathing` folder and put it under the `teamcode` folder in your project. +You can do this from either downloading the project from the above quickstart link or the 10158 +CENTERSTAGE repository [here](https://github.com/brotherhobo/10158-Centerstage). + +For this version of Pedro Pathing, the localizer used is the Road Runner localizer. To install its +dependencies: +1. Find `build.dependencies.gradle` in the main folder of your project. +2. Add the following code to the end of the `repositories` block: +``` +maven { url = 'https://maven.brott.dev/' } +``` +3. Then, add the following code to the end of your `dependencies` block: +``` +implementation 'com.acmerobotics.dashboard:dashboard:0.4.5' +``` +4. Find the `build.gradle` file under the `teamcode` folder. +5. In this gradle file, add the following dependencies: +``` +implementation 'org.apache.commons:commons-math3:3.6.1' +implementation 'com.fasterxml/jackson.core:jacson-databind:2.12.7' +implementation 'com.acmerobotics.com.roadrunner:core:0.5.6' +``` \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md new file mode 100644 index 0000000..a8e3f11 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md @@ -0,0 +1,91 @@ +## Prerequisites +Obviously, you have to have a robot to use Pedro Pathing. Also, Pedro Pathing is only able to work +with omnidirectional drives, like mecanum drive. There is currently no support for swerve drives. +You must also have a localizer of some sort. Pedro Pathing currently has the three-wheel localizer +from Road Runner, but we have plans to create our own localizer soon. Additionally, using +[FTC Dashboard](http://192.168.43.1:8080/dash) will help a lot in tuning. + +## Tuning +* To start with, we need the mass of the robot in kg. This is used for the centripetal force +correction, and the mass should be put on line `114` in the `FollowerConstants` class under the +`tuning` package. + +* Next, we need to find the preferred mecanum drive vectors. The rollers on mecanum wheels point at a +45 degree angle from the forward direction, but the actual direction the force is output is actually +closer to forward. To find the direction your wheels will go, you will need to run the +`Forward Velocity Tuner` and `Strafe Velocity Tuner` OpModes. These will run your robot at full +power for 40 inches forward and to the right, respectively. The distance can be changed through FTC +Dashboard under the dropdown for each respective class, but higher distances work better. After the +distance has finished running, the end velocity will be output to telemetry. The robot may continue +to drift a little bit after the robot has finished running the distance, so make sure you have +plenty of room. Once you're done, put the velocity for the `Forward Velocity Tuner` on line `25` in +the `FollowerConstants` class, and the velocity for the `Strafe Velocity Tuner` on line `26` in the +`FollowerConstants` class. + +* The last set of automatic tuners you'll need to run are the zero power acceleration tuners. These +find the rate at which your robot decelerates when power is cut from the drivetrain. This is used to +get a more accurate estimation of the drive vector. To find this, you will need to run the +`Forward Zero Power Acceleration Tuner` and the `Lateral Zero Power Acceleration Tuner` OpModes. +These will run your robot until it hits a velocity of 10 inches/second forward and to the right, +respectively. The velocity can be changed through FTC Dashboard under the dropdown for each +respective class, but higher velocities work better. After the velocity has been reached, power will +be cut from the drivetrain and the robot's deceleration will be tracked until the robot stops, at +which point it will display the deceleration in telemetry. This robot will need to drift to a stop +to properly work, and the higher the velocity the greater the drift distance, so make sure you have +enough room. Once you're done, put the zero power acceleration for the +`Forward Zero Power Acceleration Tuner` on line `122` in the `FollowerConstants` class and the zero +power acceleration for the `Lateral Zero Power Acceleration Tuner` on line `127` in the +`FollowerConstants` class. + +* After this, we will want to tune the translational PIDs. Go to FTC Dashboard and disable all but +the `useTranslational` checkboxes under the `Follower` tab. Then, run `StraightBackAndForth`. Make +sure you disable the timer on autonomous OpModes. There are two different PIDs for translational +error, the `smallTranslationalPIDF` and `largeTranslationalPIDF`. If error is larger than a certain +amount, the `largeTranslationalPIDF` will be used, and if error is smaller than that amount the +`smallTranslationalPIDF` will be used. If you need to add a feedforward value, use the +`smallTranslationalPIDFFeedForward` and `largeTranslationalPIDFFeedForward` since those will add the +feedforward in the direction the robot is trying to move, rather than the feedforward in the PIDFs +themselves, since those will only add the feedforward one way. You can change the amount of error +required to switch PIDFs, as well as the PIDF constants and feedforward values, under the +`FollowerConstants` tab in FTC Dashboard. To tune the PIDs, push the robot off the path and see how +corrects. You will want to alternate sides you push to reduce field wear and tear as well as push +with varying power and distance. I would recommend tuning the large PID first, and tuning it so that +the PID is capable of correcting to the point where the PIDs switch with little momentum. Then, tune +the small PID to minimize oscillations while still achieving a satisfactory level of accuracy. +Overall, try to tune for fewer oscillations rather than higher speeds or perfect accuracy, since +this will make the robot run more smoothly under actual pathing conditions. + +* Next, we will tune the heading PIDs. The process is essentially the same as above, except you will +want to only enable `useHeading` under `Follower` on FTC Dashboard, as well as turn the robot from +opposing corners instead of pushing the robot. Naturally, instead of changing the stuff with +"translational" in the name, you will instead want to look for stuff with "heading" in the name. +Otherwise, these two PIDs are functionally very similar. The same tips from above will apply to this. + +* Afterwards, we will tune the drive PIDs. Before we continue, we will need to set the +`zeroPowerAccelerationMultiplier`. This determines how fast your robot will decelerate as a factor +of how fast your robot will coast to a stop. Honestly, this is up to you. I personally used 4, but +what works best for you is most important. Higher numbers will cause a faster brake, but increase +oscillations at the end. Lower numbers will do the opposite. This can be found on line `136` in +`FollowerConstants`. There are once again two PIDs for the drive vector, but these PIDs are much, +much more sensitive than the others. For reference, my P values were in the hundredths and +thousandths place values, and my D values were in the hundred thousandths and millionths place +values. To tune this, enable `useDrive`, `useHeading`, and `useTranslational` in the `Follower` +dropdown in FTC Dashboard. Next, run `StraightBackAndForth`and don't forget to turn off the timer on +the OpMode. Then, tune the large PID and then the small PID following the tips from earlier. For +this, it is very important to try to reduce oscillations. Additionally, I would absolutely not +recommend using the I, or integral, part of the PID for this. Using integral in drivetrain PIDs is +already not ideal, but it will continuously build up error in this PID, causing major issues when +it gets too strong. So, just don't use it. P and D are enough. + +* Finally, we will want to tune the centripetal force correction. This is a pretty simple tune. Open +up FTC Dashboard and enable everything under the `Follower` tab. Then, run `CurvedBackAndForth` +and turn off its timer. If you notice the robot is correcting towards the inside of the curve +as/after running a path, then increase `centripetalScaling`, which can be found on line `117` of +`FollowerConstants`. If the robot is correcting towards the outside of the curve, then decrease +`centripetalScaling`. + +* Once you've found satisfactory tunings for everything, run the robot around in +`StraightBackAndForth`, `CurvedBackAndForth`, or some paths of your own making. There's also +`Circle`, but that's more so for fun than anything else. If you notice something could be improved, +feel free to mess around more with your PIDs. That should be all! If you have any more questions, +refer to the OVERVIEW readme file or the README readme file. Best of luck to your team this season! :) \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java new file mode 100644 index 0000000..79a804f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java @@ -0,0 +1,69 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.examples; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +/** + * This is the TeleOpEnhancements OpMode. It is an example usage of the TeleOp enhancements that + * Pedro Pathing is capable of. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/21/2024 + */ +@TeleOp(name = "Pedro Pathing TeleOp Enhancements", group = "Test") +public class TeleOpEnhancements extends OpMode { + private Follower follower; + + private DcMotorEx leftFront; + private DcMotorEx leftRear; + private DcMotorEx rightFront; + private DcMotorEx rightRear; + + private Vector driveVector; + private Vector headingVector; + + /** + * This initializes the drive motors as well as the Follower and motion Vectors. + */ + @Override + public void init() { + follower = new Follower(hardwareMap, false); + + leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); + leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); + rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); + rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + + leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + driveVector = new Vector(); + headingVector = new Vector(); + } + + /** + * This runs the OpMode. This is only drive control with Pedro Pathing live centripetal force + * correction. + */ + @Override + public void loop() { + driveVector.setOrthogonalComponents(-gamepad1.left_stick_y, -gamepad1.left_stick_x); + driveVector.setMagnitude(MathFunctions.clamp(driveVector.getMagnitude(), 0, 1)); + driveVector.rotateVector(follower.getPose().getHeading()); + + headingVector.setComponents(-gamepad1.left_stick_x, follower.getPose().getHeading()); + + follower.setMovementVectors(follower.getCentripetalForceCorrection(), headingVector, driveVector); + follower.update(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java new file mode 100644 index 0000000..f2cfcfd --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java @@ -0,0 +1,152 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.follower; + +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +/** + * This is the DriveVectorScaler class. This class takes in inputs Vectors for driving, heading + * correction, and translational/centripetal correction and returns an array with wheel powers. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/4/2024 + */ +public class DriveVectorScaler { + // This is ordered left front, left back, right front, right back. These are also normalized. + private Vector[] mecanumVectors; + + /** + * This creates a new DriveVectorScaler, which takes in various movement vectors and outputs + * the wheel drive powers necessary to move in the intended direction, given the true movement + * vector for the front left mecanum wheel. + * + * @param frontLeftVector this is the front left mecanum wheel's preferred drive vector. + */ + public DriveVectorScaler(Vector frontLeftVector) { + Vector copiedFrontLeftVector = MathFunctions.normalizeVector(frontLeftVector); + mecanumVectors = new Vector[]{ + new Vector(copiedFrontLeftVector.getMagnitude(), copiedFrontLeftVector.getTheta()), + new Vector(copiedFrontLeftVector.getMagnitude(), 2*Math.PI-copiedFrontLeftVector.getTheta()), + new Vector(copiedFrontLeftVector.getMagnitude(), 2*Math.PI-copiedFrontLeftVector.getTheta()), + new Vector(copiedFrontLeftVector.getMagnitude(), copiedFrontLeftVector.getTheta())}; + } + + /** + * This takes in vectors for corrective power, heading power, and pathing power and outputs + * an Array of four doubles, one for each wheel's motor power. + * + * IMPORTANT NOTE: all vector inputs are clamped between 0 and 1 inclusive in magnitude. + * + * @param correctivePower this Vector includes the centrifugal force scaling Vector as well as a + * translational power Vector to correct onto the Bezier curve the Follower + * is following. + * @param headingPower this Vector points in the direction of the robot's current heaing, and + * the magnitude tells the robot how much it should turn and in which + * direction. + * @param pathingPower this Vector points in the direction the robot needs to go to continue along + * the Path. + * @param robotHeading this is the current heading of the robot, which is used to calculate how + * much power to allocate to each wheel. + * @return this returns an Array of doubles with a length of 4, which contains the wheel powers. + */ + public double[] getDrivePowers(Vector correctivePower, Vector headingPower, Vector pathingPower, double robotHeading) { + // clamps down the magnitudes of the input vectors + if (correctivePower.getMagnitude() > 1) correctivePower.setMagnitude(1); + if (headingPower.getMagnitude() > 1) headingPower.setMagnitude(1); + if (pathingPower.getMagnitude() > 1) pathingPower.setMagnitude(1); + + // the powers for the wheel vectors + double [] wheelPowers = new double[4]; + + // This contains a copy of the mecanum wheel vectors + Vector[] mecanumVectorsCopy = new Vector[4]; + + // this contains the pathing vectors, one for each side (heading control requires 2) + Vector[] truePathingVectors = new Vector[2]; + + if (correctivePower.getMagnitude() == 1) { + // checks for corrective power equal to 1 in magnitude. if equal to one, then set pathing power to that + truePathingVectors[0] = MathFunctions.copyVector(correctivePower); + truePathingVectors[1] = MathFunctions.copyVector(correctivePower); + } else { + // corrective power did not take up all the power, so add on heading power + Vector leftSideVector = MathFunctions.subtractVectors(correctivePower, headingPower); + Vector rightSideVector = MathFunctions.addVectors(correctivePower, headingPower); + + if (leftSideVector.getMagnitude() > 1 || rightSideVector.getMagnitude() > 1) { + //if the combined corrective and heading power is greater than 1, then scale down heading power + double headingScalingFactor = Math.min(findNormalizingScaling(correctivePower, headingPower), findNormalizingScaling(correctivePower, MathFunctions.scalarMultiplyVector(headingPower, -1))); + truePathingVectors[0] = MathFunctions.subtractVectors(correctivePower, MathFunctions.scalarMultiplyVector(headingPower, headingScalingFactor)); + truePathingVectors[1] = MathFunctions.addVectors(correctivePower, MathFunctions.scalarMultiplyVector(headingPower, headingScalingFactor)); + } else { + // if we're here then we can add on some drive power but scaled down to 1 + Vector leftSideVectorWithPathing = MathFunctions.addVectors(leftSideVector, pathingPower); + Vector rightSideVectorWithPathing = MathFunctions.addVectors(rightSideVector, pathingPower); + + if (leftSideVectorWithPathing.getMagnitude() > 1 || rightSideVectorWithPathing.getMagnitude() > 1) { + // too much power now, so we scale down the pathing vector + double pathingScalingFactor = Math.min(findNormalizingScaling(leftSideVector, pathingPower), findNormalizingScaling(rightSideVector, pathingPower)); + truePathingVectors[0] = MathFunctions.addVectors(leftSideVector, MathFunctions.scalarMultiplyVector(pathingPower, pathingScalingFactor)); + truePathingVectors[1] = MathFunctions.addVectors(rightSideVector, MathFunctions.scalarMultiplyVector(pathingPower, pathingScalingFactor)); + } else { + // just add the vectors together and you get the final vector + truePathingVectors[0] = MathFunctions.copyVector(leftSideVectorWithPathing); + truePathingVectors[1] = MathFunctions.copyVector(rightSideVectorWithPathing); + } + } + } + + truePathingVectors[0] = MathFunctions.scalarMultiplyVector(truePathingVectors[0], 2.0); + truePathingVectors[1] = MathFunctions.scalarMultiplyVector(truePathingVectors[1], 2.0); + + for (int i = 0; i < mecanumVectorsCopy.length; i++) { + // this copies the vectors from mecanumVectors but creates new references for them + mecanumVectorsCopy[i] = MathFunctions.copyVector(mecanumVectors[i]); + + mecanumVectorsCopy[i].rotateVector(robotHeading); + } + + wheelPowers[0] = (mecanumVectorsCopy[1].getXComponent()*truePathingVectors[0].getYComponent() - truePathingVectors[0].getXComponent()*mecanumVectorsCopy[1].getYComponent()) / (mecanumVectorsCopy[1].getXComponent()*mecanumVectorsCopy[0].getYComponent() - mecanumVectorsCopy[0].getXComponent()*mecanumVectorsCopy[1].getYComponent()); + wheelPowers[1] = (mecanumVectorsCopy[0].getXComponent()*truePathingVectors[0].getYComponent() - truePathingVectors[0].getXComponent()*mecanumVectorsCopy[0].getYComponent()) / (mecanumVectorsCopy[0].getXComponent()*mecanumVectorsCopy[1].getYComponent() - mecanumVectorsCopy[1].getXComponent()*mecanumVectorsCopy[0].getYComponent()); + wheelPowers[2] = (mecanumVectorsCopy[3].getXComponent()*truePathingVectors[1].getYComponent() - truePathingVectors[1].getXComponent()*mecanumVectorsCopy[3].getYComponent()) / (mecanumVectorsCopy[3].getXComponent()*mecanumVectorsCopy[2].getYComponent() - mecanumVectorsCopy[2].getXComponent()*mecanumVectorsCopy[3].getYComponent()); + wheelPowers[3] = (mecanumVectorsCopy[2].getXComponent()*truePathingVectors[1].getYComponent() - truePathingVectors[1].getXComponent()*mecanumVectorsCopy[2].getYComponent()) / (mecanumVectorsCopy[2].getXComponent()*mecanumVectorsCopy[3].getYComponent() - mecanumVectorsCopy[3].getXComponent()*mecanumVectorsCopy[2].getYComponent()); + + double wheelPowerMax = Math.max(Math.max(Math.abs(wheelPowers[0]), Math.abs(wheelPowers[1])), Math.max(Math.abs(wheelPowers[2]), Math.abs(wheelPowers[3]))); + if (wheelPowerMax > 1) { + wheelPowers[0] /= wheelPowerMax; + wheelPowers[1] /= wheelPowerMax; + wheelPowers[2] /= wheelPowerMax; + wheelPowers[3] /= wheelPowerMax; + } + + return wheelPowers; + } + + /** + * This takes in two Vectors, one static and one variable, and returns the scaling factor that, + * when multiplied to the variable Vector, results in magnitude of the sum of the static Vector + * and the scaled variable Vector being 1. + * + * IMPORTANT NOTE: I did not intend for this to be used for anything other than the method above + * this one in this class, so there will be errors if you input Vectors of length greater than 1, + * and it will scale up the variable Vector if the magnitude of the sum of the two input Vectors + * isn't greater than 1. So, just don't use this elsewhere. There's gotta be a better way to do + * whatever you're trying to do. + * + * I know that this is used outside of this class, however, I created this method so I get to + * use it if I want to. Also, it's only used once outside of the DriveVectorScaler class, and + * it's used to scale Vectors, as intended. + * + * @param staticVector the Vector that is held constant. + * @param variableVector the Vector getting scaled to make the sum of the input Vectors have a + * magnitude of 1. + * @return returns the scaling factor for the variable Vector. + */ + public double findNormalizingScaling(Vector staticVector, Vector variableVector) { + double a = Math.pow(variableVector.getXComponent(), 2) + Math.pow(variableVector.getYComponent(), 2); + double b = staticVector.getXComponent() * variableVector.getXComponent() + staticVector.getYComponent() * variableVector.getYComponent(); + double c = Math.pow(staticVector.getXComponent(), 2) + Math.pow(staticVector.getYComponent(), 2) - 1.0; + return (-b + Math.sqrt(Math.pow(b, 2) - a*c))/(a); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java new file mode 100644 index 0000000..cfdd2e7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -0,0 +1,879 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.follower; + +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFSwitch; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.forwardZeroPowerAcceleration; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.largeDrivePIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.largeHeadingPIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.largeTranslationalPIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.lateralZeroPowerAcceleration; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.smallDrivePIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.smallHeadingPIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.smallTranslationalPIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFSwitch; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierPoint; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathCallback; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants; +import org.firstinspires.ftc.teamcode.pedroPathing.util.PIDFController; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +/** + * This is the Follower class. It handles the actual following of the paths and all the on-the-fly + * calculations that are relevant for movement. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/4/2024 + */ +@Config +public class Follower { + private HardwareMap hardwareMap; + + private DcMotorEx leftFront; + private DcMotorEx leftRear; + private DcMotorEx rightFront; + private DcMotorEx rightRear; + private List motors; + + private DriveVectorScaler driveVectorScaler; + + private PoseUpdater poseUpdater; + + private Pose2d closestPose; + + private Path currentPath; + + private PathChain currentPathChain; + + private final int BEZIER_CURVE_BINARY_STEP_LIMIT = FollowerConstants.BEZIER_CURVE_BINARY_STEP_LIMIT; + private final int AVERAGED_VELOCITY_SAMPLE_NUMBER = FollowerConstants.AVERAGED_VELOCITY_SAMPLE_NUMBER; + + private int chainIndex; + + private long[] pathStartTimes; + + private boolean followingPathChain; + private boolean holdingPosition; + private boolean isBusy; + private boolean auto = true; + private boolean reachedParametricPathEnd; + private boolean holdPositionAtEnd; + + private double maxPower = 1; + private double previousSmallTranslationalIntegral; + private double previousLargeTranslationalIntegral; + private double holdPointTranslationalScaling = FollowerConstants.holdPointTranslationalScaling; + private double holdPointHeadingScaling = FollowerConstants.holdPointHeadingScaling; + public double driveError; + public double headingError; + + private long reachedParametricPathEndTime; + + private double[] drivePowers; + + private Vector[] teleOpMovementVectors = new Vector[]{new Vector(), new Vector(), new Vector()}; + + private ArrayList velocities = new ArrayList<>(); + private ArrayList accelerations = new ArrayList<>(); + + private Vector averageVelocity; + private Vector averagePreviousVelocity; + private Vector averageAcceleration; + private Vector smallTranslationalIntegralVector; + private Vector largeTranslationalIntegralVector; + public Vector driveVector; + public Vector headingVector; + public Vector translationalVector; + public Vector centripetalVector; + public Vector correctiveVector; + + private PIDFController smallTranslationalPIDF = new PIDFController(FollowerConstants.smallTranslationalPIDFCoefficients); + private PIDFController smallTranslationalIntegral = new PIDFController(FollowerConstants.smallTranslationalIntegral); + private PIDFController largeTranslationalPIDF = new PIDFController(FollowerConstants.largeTranslationalPIDFCoefficients); + private PIDFController largeTranslationalIntegral = new PIDFController(FollowerConstants.largeTranslationalIntegral); + private PIDFController smallHeadingPIDF = new PIDFController(FollowerConstants.smallHeadingPIDFCoefficients); + private PIDFController largeHeadingPIDF = new PIDFController(FollowerConstants.largeHeadingPIDFCoefficients); + private PIDFController smallDrivePIDF = new PIDFController(FollowerConstants.smallDrivePIDFCoefficients); + private PIDFController largeDrivePIDF = new PIDFController(FollowerConstants.largeDrivePIDFCoefficients); + + + public static boolean useTranslational = true; + public static boolean useCentripetal = true; + public static boolean useHeading = true; + public static boolean useDrive = true; + + /** + * This creates a new Follower given a HardwareMap. + * + * @param hardwareMap HardwareMap required + */ + public Follower(HardwareMap hardwareMap) { + this.hardwareMap = hardwareMap; + initialize(); + } + + /** + * This creates a new Follower given a HardwareMap and sets whether the Follower is being used + * in autonomous or teleop. + * + * @param hardwareMap HardwareMap required + * @param setAuto sets whether or not the Follower is being used in autonomous or teleop + */ + public Follower(HardwareMap hardwareMap, boolean setAuto) { + this.hardwareMap = hardwareMap; + setAuto(setAuto); + initialize(); + } + + /** + * This initializes the follower. + * In this, the DriveVectorScaler and PoseUpdater is instantiated, the drive motors are + * initialized and their behavior is set, and the variables involved in approximating first and + * second derivatives for teleop are set. + */ + public void initialize() { + driveVectorScaler = new DriveVectorScaler(FollowerConstants.frontLeftVector); + poseUpdater = new PoseUpdater(hardwareMap); + + leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); + leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); + rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); + rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + + // TODO: Make sure that this is the direction your motors need to be reversed in. + leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + leftRear.setDirection(DcMotorSimple.Direction.REVERSE); + + motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear); + + for (DcMotorEx motor : motors) { + MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); + motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motor.setMotorType(motorConfigurationType); + } + + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + } + + for (int i = 0; i < AVERAGED_VELOCITY_SAMPLE_NUMBER; i++) { + velocities.add(new Vector()); + } + for (int i = 0; i < AVERAGED_VELOCITY_SAMPLE_NUMBER / 2; i++) { + accelerations.add(new Vector()); + } + calculateAveragedVelocityAndAcceleration(); + } + + /** + * This sets the maximum power the motors are allowed to use. + * + * @param set This caps the motor power from [0, 1]. + */ + public void setMaxPower(double set) { + maxPower = MathFunctions.clamp(set, 0, 1); + } + + /** + * This handles the limiting of the drive powers array to the max power. + */ + public void limitDrivePowers() { + for (int i = 0; i < drivePowers.length; i++) { + if (Math.abs(drivePowers[i]) > maxPower) { + drivePowers[i] = maxPower * MathFunctions.getSign(drivePowers[i]); + } + } + } + + /** + * This returns the current pose from the PoseUpdater. + * + * @return returns the pose + */ + public Pose2d getPose() { + return poseUpdater.getPose(); + } + + /** + * This sets the current pose in the PoseUpdater using Road Runner's setPoseEstimate() method. + * + * @param pose The pose to set the current pose to. + */ + public void setPose(Pose2d pose) { + poseUpdater.setPose(pose); + } + + /** + * This returns the current velocity of the robot as a Vector. + * + * @return returns the current velocity as a Vector. + */ + public Vector getVelocity() { + return poseUpdater.getVelocity(); + } + + /** + * This returns the current acceleration of the robot as a Vector. + * + * @return returns the current acceleration as a Vector. + */ + public Vector getAcceleration() { + return poseUpdater.getAcceleration(); + } + + /** + * This returns the magnitude of the current velocity. For when you only need the magnitude. + * + * @return returns the magnitude of the current velocity. + */ + public double getVelocityMagnitude() { + return poseUpdater.getVelocity().getMagnitude(); + } + + /** + * This sets the starting pose. Do not run this after moving at all. + * + * @param pose the pose to set the starting pose to. + */ + public void setStartingPose(Pose2d pose) { + poseUpdater.setStartingPose(pose); + } + + /** + * This sets the current pose, using offsets so no reset time delay. This is better than the + * Road Runner reset, in general. Think of using offsets as setting trim in an aircraft. This can + * be reset as well, so beware of using the resetOffset() method. + * + * @param set The pose to set the current pose to. + */ + public void setCurrentPoseWithOffset(Pose2d set) { + poseUpdater.setCurrentPoseWithOffset(set); + } + + /** + * This sets the offset for only the x position. + * + * @param xOffset This sets the offset. + */ + public void setXOffset(double xOffset) { + poseUpdater.setXOffset(xOffset); + } + + /** + * This sets the offset for only the y position. + * + * @param yOffset This sets the offset. + */ + public void setYOffset(double yOffset) { + poseUpdater.setYOffset(yOffset); + } + + /** + * This sets the offset for only the heading. + * + * @param headingOffset This sets the offset. + */ + public void setHeadingOffset(double headingOffset) { + poseUpdater.setHeadingOffset(headingOffset); + } + + /** + * This returns the x offset. + * + * @return returns the x offset. + */ + public double getXOffset() { + return poseUpdater.getXOffset(); + } + + /** + * This returns the y offset. + * + * @return returns the y offset. + */ + public double getYOffset() { + return poseUpdater.getYOffset(); + } + + /** + * This returns the heading offset. + * + * @return returns the heading offset. + */ + public double getHeadingOffset() { + return poseUpdater.getHeadingOffset(); + } + + /** + * This resets all offsets set to the PoseUpdater. If you have reset your pose using the + * setCurrentPoseUsingOffset(Pose2d set) method, then your pose will be returned to what the + * PoseUpdater thinks your pose would be, not the pose you reset to. + */ + public void resetOffset() { + poseUpdater.resetOffset(); + } + + /** + * This holds a Point. + * + * @param point the Point to stay at. + * @param heading the heading to face. + */ + public void holdPoint(BezierPoint point, double heading) { + breakFollowing(); + holdingPosition = true; + isBusy = false; + followingPathChain = false; + currentPath = new Path(point); + currentPath.setConstantHeadingInterpolation(heading); + closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1); + } + + /** + * This follows a Path. + * This also makes the Follower hold the last Point on the Path. + * + * @param path the Path to follow. + */ + public void followPath(Path path, boolean holdEnd) { + breakFollowing(); + holdPositionAtEnd = holdEnd; + isBusy = true; + followingPathChain = false; + currentPath = path; + closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_BINARY_STEP_LIMIT); + } + + /** + * This follows a Path. + * + * @param path the Path to follow. + */ + public void followPath(Path path) { + followPath(path, false); + } + + /** + * This follows a PathChain. Drive vector projection is only done on the last Path. + * This also makes the Follower hold the last Point on the PathChain. + * + * @param pathChain the PathChain to follow. + */ + public void followPath(PathChain pathChain, boolean holdEnd) { + breakFollowing(); + holdPositionAtEnd = holdEnd; + pathStartTimes = new long[pathChain.size()]; + pathStartTimes[0] = System.currentTimeMillis(); + isBusy = true; + followingPathChain = true; + chainIndex = 0; + currentPathChain = pathChain; + currentPath = pathChain.getPath(chainIndex); + closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_BINARY_STEP_LIMIT); + } + + /** + * This follows a PathChain. Drive vector projection is only done on the last Path. + * + * @param pathChain the PathChain to follow. + */ + public void followPath(PathChain pathChain) { + followPath(pathChain, false); + } + + /** + * This calls an update to the PoseUpdater, which updates the robot's current position estimate. + * This also updates all the Follower's PIDFs, which updates the motor powers. + */ + public void update() { + poseUpdater.update(); + if (auto) { + if (holdingPosition) { + closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1); + + drivePowers = driveVectorScaler.getDrivePowers(MathFunctions.scalarMultiplyVector(getTranslationalCorrection(), holdPointTranslationalScaling), MathFunctions.scalarMultiplyVector(getHeadingVector(), holdPointHeadingScaling), new Vector(), poseUpdater.getPose().getHeading()); + + limitDrivePowers(); + + for (int i = 0; i < motors.size(); i++) { + motors.get(i).setPower(drivePowers[i]); + } + } else { + if (isBusy) { + closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_BINARY_STEP_LIMIT); + + if (followingPathChain) updateCallbacks(); + + drivePowers = driveVectorScaler.getDrivePowers(getCorrectiveVector(), getHeadingVector(), getDriveVector(), poseUpdater.getPose().getHeading()); + + limitDrivePowers(); + + for (int i = 0; i < motors.size(); i++) { + motors.get(i).setPower(drivePowers[i]); + } + } + if (currentPath.isAtParametricEnd()) { + if (followingPathChain && chainIndex < currentPathChain.size() - 1) { + // Not at last path, keep going + breakFollowing(); + pathStartTimes[chainIndex] = System.currentTimeMillis(); + isBusy = true; + followingPathChain = true; + chainIndex++; + currentPath = currentPathChain.getPath(chainIndex); + closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_BINARY_STEP_LIMIT); + } else { + // At last path, run some end detection stuff + // set isBusy to false if at end + if (!reachedParametricPathEnd) { + reachedParametricPathEnd = true; + reachedParametricPathEndTime = System.currentTimeMillis(); + } + + if ((System.currentTimeMillis() - reachedParametricPathEndTime > currentPath.getPathEndTimeoutConstraint()) || (poseUpdater.getVelocity().getMagnitude() < currentPath.getPathEndVelocityConstraint() && MathFunctions.distance(poseUpdater.getPose(), closestPose) < currentPath.getPathEndTranslationalConstraint() && MathFunctions.getSmallestAngleDifference(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()) < currentPath.getPathEndHeadingConstraint())) { + if (holdPositionAtEnd) { + holdPositionAtEnd = false; + holdPoint(new BezierPoint(currentPath.getLastControlPoint()), currentPath.getHeadingGoal(1)); + } else { + breakFollowing(); + } + } + } + } + } + } else { + velocities.add(poseUpdater.getVelocity()); + velocities.remove(velocities.get(velocities.size() - 1)); + + calculateAveragedVelocityAndAcceleration(); + + drivePowers = driveVectorScaler.getDrivePowers(teleOpMovementVectors[0], teleOpMovementVectors[1], teleOpMovementVectors[2], poseUpdater.getPose().getHeading()); + + limitDrivePowers(); + + for (int i = 0; i < motors.size(); i++) { + motors.get(i).setPower(drivePowers[i]); + } + } + } + + /** + * This calculates an averaged approximate velocity and acceleration. This is used for a + * real-time correction of centripetal force, which is used in teleop. + */ + public void calculateAveragedVelocityAndAcceleration() { + averageVelocity = new Vector(); + averagePreviousVelocity = new Vector(); + + for (int i = 0; i < velocities.size() / 2; i++) { + averageVelocity = MathFunctions.addVectors(averageVelocity, velocities.get(i)); + } + averageVelocity = MathFunctions.scalarMultiplyVector(averageVelocity, 1.0 / ((double) velocities.size() / 2)); + + for (int i = velocities.size() / 2; i < velocities.size(); i++) { + averagePreviousVelocity = MathFunctions.addVectors(averagePreviousVelocity, velocities.get(i)); + } + averagePreviousVelocity = MathFunctions.scalarMultiplyVector(averagePreviousVelocity, 1.0 / ((double) velocities.size() / 2)); + + accelerations.add(MathFunctions.subtractVectors(averageVelocity, averagePreviousVelocity)); + accelerations.remove(accelerations.size() - 1); + + averageAcceleration = new Vector(); + + for (int i = 0; i < accelerations.size(); i++) { + averageAcceleration = MathFunctions.addVectors(averageAcceleration, accelerations.get(i)); + } + averageAcceleration = MathFunctions.scalarMultiplyVector(averageAcceleration, 1.0 / accelerations.size()); + } + + /** + * This checks if any PathCallbacks should be run right now, and runs them if applicable. + */ + public void updateCallbacks() { + for (PathCallback callback : currentPathChain.getCallbacks()) { + if (!callback.hasBeenRun()) { + if (callback.getType() == PathCallback.PARAMETRIC) { + // parametric call back + if (chainIndex == callback.getIndex() && (getCurrentTValue() >= callback.getStartCondition() || MathFunctions.roughlyEquals(getCurrentTValue(), callback.getStartCondition()))) { + callback.run(); + } + } else { + // time based call back + if (chainIndex >= callback.getIndex() && System.currentTimeMillis() - pathStartTimes[callback.getIndex()] > callback.getStartCondition()) { + callback.run(); + } + + } + } + } + } + + /** + * This resets the PIDFs and stops following the current Path. + */ + public void breakFollowing() { + holdingPosition = false; + isBusy = false; + reachedParametricPathEnd = false; + smallDrivePIDF.reset(); + largeDrivePIDF.reset(); + smallHeadingPIDF.reset(); + largeHeadingPIDF.reset(); + smallTranslationalPIDF.reset(); + smallTranslationalIntegral.reset(); + smallTranslationalIntegralVector = new Vector(); + previousSmallTranslationalIntegral = 0; + largeTranslationalPIDF.reset(); + largeTranslationalIntegral.reset(); + largeTranslationalIntegralVector = new Vector(); + previousLargeTranslationalIntegral = 0; + driveVector = new Vector(); + headingVector = new Vector(); + translationalVector = new Vector(); + centripetalVector = new Vector(); + correctiveVector = new Vector(); + driveError = 0; + headingError = 0; + + for (int i = 0; i < motors.size(); i++) { + motors.get(i).setPower(0); + } + } + + /** + * This returns if the Follower is currently following a Path or a PathChain. + * + * @return returns if the Follower is busy. + */ + public boolean isBusy() { + return isBusy; + } + + /** + * Sets the correctional, heading, and drive movement vectors for teleop enhancements. + * The correctional Vector only accounts for an approximated centripetal correction. + */ + public void setMovementVectors(Vector correctional, Vector heading, Vector drive) { + teleOpMovementVectors = new Vector[]{correctional, heading, drive}; + } + + /** + * This returns a Vector in the direction the robot must go to move along the path. This Vector + * takes into account the projected position of the robot to calculate how much power is needed. + *

+ * Note: This vector is clamped to be at most 1 in magnitude. + * + * @return returns the drive vector. + */ + public Vector getDriveVector() { + if (!useDrive) return new Vector(); + if (followingPathChain && chainIndex < currentPathChain.size() - 1) { + return new Vector(1, currentPath.getClosestPointTangentVector().getTheta()); + } + + driveError = getDriveVelocityError(); + + if (Math.abs(driveError) < drivePIDFSwitch) { + smallDrivePIDF.updateError(driveError); + driveVector = new Vector(MathFunctions.clamp(smallDrivePIDF.runPIDF() + smallDrivePIDFFeedForward * MathFunctions.getSign(driveError), -1, 1), currentPath.getClosestPointTangentVector().getTheta()); + return MathFunctions.copyVector(driveVector); + } + + largeDrivePIDF.updateError(driveError); + driveVector = new Vector(MathFunctions.clamp(largeDrivePIDF.runPIDF() + largeDrivePIDFFeedForward * MathFunctions.getSign(driveError), -1, 1), currentPath.getClosestPointTangentVector().getTheta()); + return MathFunctions.copyVector(driveVector); + } + + /** + * This returns the velocity the robot needs to be at to make it to the end of the Path + * at some specified deceleration (well technically just some negative acceleration). + * + * @return returns the projected velocity. + */ + public double getDriveVelocityError() { + double distanceToGoal; + if (!currentPath.isAtParametricEnd()) { + distanceToGoal = currentPath.length() * (1 - currentPath.getClosestPointTValue()); + } else { + Vector offset = new Vector(); + offset.setOrthogonalComponents(getPose().getX() - currentPath.getLastControlPoint().getX(), getPose().getY() - currentPath.getLastControlPoint().getY()); + distanceToGoal = MathFunctions.dotProduct(currentPath.getEndTangent(), offset); + } + + Vector distanceToGoalVector = MathFunctions.scalarMultiplyVector(MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector()), distanceToGoal); + Vector velocity = new Vector(MathFunctions.dotProduct(getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta()); + + Vector forwardHeadingVector = new Vector(1.0, poseUpdater.getPose().getHeading()); + double forwardVelocity = MathFunctions.dotProduct(forwardHeadingVector, velocity); + double forwardDistanceToGoal = MathFunctions.dotProduct(forwardHeadingVector, distanceToGoalVector); + double forwardVelocityGoal = MathFunctions.getSign(forwardDistanceToGoal) * Math.sqrt(Math.abs(-2 * currentPath.getZeroPowerAccelerationMultiplier() * forwardZeroPowerAcceleration * forwardDistanceToGoal)); + double forwardVelocityZeroPowerDecay = forwardVelocity - MathFunctions.getSign(forwardDistanceToGoal) * Math.sqrt(Math.abs(Math.pow(forwardVelocity, 2) + 2 * forwardZeroPowerAcceleration * forwardDistanceToGoal)); + + Vector lateralHeadingVector = new Vector(1.0, poseUpdater.getPose().getHeading() - Math.PI / 2); + double lateralVelocity = MathFunctions.dotProduct(lateralHeadingVector, velocity); + double lateralDistanceToGoal = MathFunctions.dotProduct(lateralHeadingVector, distanceToGoalVector); + double lateralVelocityGoal = MathFunctions.getSign(lateralDistanceToGoal) * Math.sqrt(Math.abs(-2 * currentPath.getZeroPowerAccelerationMultiplier() * lateralZeroPowerAcceleration * lateralDistanceToGoal)); + double lateralVelocityZeroPowerDecay = lateralVelocity - MathFunctions.getSign(lateralDistanceToGoal) * Math.sqrt(Math.abs(Math.pow(lateralVelocity, 2) + 2 * lateralZeroPowerAcceleration * lateralDistanceToGoal)); + + Vector forwardVelocityError = new Vector(forwardVelocityGoal - forwardVelocityZeroPowerDecay - forwardVelocity, forwardHeadingVector.getTheta()); + Vector lateralVelocityError = new Vector(lateralVelocityGoal - lateralVelocityZeroPowerDecay - lateralVelocity, lateralHeadingVector.getTheta()); + Vector velocityErrorVector = MathFunctions.addVectors(forwardVelocityError, lateralVelocityError); + + return velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector())); + } + + /** + * This returns a Vector in the direction of the robot that contains the heading correction + * as its magnitude. Positive heading correction turns the robot counter-clockwise, and negative + * heading correction values turn the robot clockwise. So basically, Pedro Pathing uses a right- + * handed coordinate system. + *

+ * Note: This vector is clamped to be at most 1 in magnitude. + * + * @return returns the heading vector. + */ + public Vector getHeadingVector() { + if (!useHeading) return new Vector(); + headingError = MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()) * MathFunctions.getSmallestAngleDifference(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()); + if (Math.abs(headingError) < headingPIDFSwitch) { + smallHeadingPIDF.updateError(headingError); + headingVector = new Vector(MathFunctions.clamp(smallHeadingPIDF.runPIDF() + smallHeadingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -1, 1), poseUpdater.getPose().getHeading()); + return MathFunctions.copyVector(headingVector); + } + largeHeadingPIDF.updateError(headingError); + headingVector = new Vector(MathFunctions.clamp(largeHeadingPIDF.runPIDF() + largeHeadingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -1, 1), poseUpdater.getPose().getHeading()); + return MathFunctions.copyVector(headingVector); + } + + /** + * This returns a combined Vector in the direction the robot must go to correct both translational + * error as well as centripetal force. + *

+ * Note: This vector is clamped to be at most 1 in magnitude. + * + * @return returns the corrective vector. + */ + public Vector getCorrectiveVector() { + Vector centripetal = getCentripetalForceCorrection(); + Vector translational = getTranslationalCorrection(); + Vector corrective = MathFunctions.addVectors(centripetal, translational); + + if (corrective.getMagnitude() > 1) { + return MathFunctions.addVectors(centripetal, MathFunctions.scalarMultiplyVector(translational, driveVectorScaler.findNormalizingScaling(centripetal, translational))); + } + + correctiveVector = MathFunctions.copyVector(corrective); + + return corrective; + } + + /** + * This returns a Vector in the direction the robot must go to account for only translational + * error. + *

+ * Note: This vector is clamped to be at most 1 in magnitude. + * + * @return returns the translational correction vector. + */ + public Vector getTranslationalCorrection() { + if (!useTranslational) return new Vector(); + Vector translationalVector = new Vector(); + double x = closestPose.getX() - poseUpdater.getPose().getX(); + double y = closestPose.getY() - poseUpdater.getPose().getY(); + translationalVector.setOrthogonalComponents(x, y); + + if (!(currentPath.isAtParametricEnd() || currentPath.isAtParametricStart())) { + translationalVector = MathFunctions.subtractVectors(translationalVector, new Vector(MathFunctions.dotProduct(translationalVector, MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta())); + + smallTranslationalIntegralVector = MathFunctions.subtractVectors(smallTranslationalIntegralVector, new Vector(MathFunctions.dotProduct(smallTranslationalIntegralVector, MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta())); + largeTranslationalIntegralVector = MathFunctions.subtractVectors(largeTranslationalIntegralVector, new Vector(MathFunctions.dotProduct(largeTranslationalIntegralVector, MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta())); + } + + if (MathFunctions.distance(poseUpdater.getPose(), closestPose) < translationalPIDFSwitch) { + smallTranslationalIntegral.updateError(translationalVector.getMagnitude()); + smallTranslationalIntegralVector = MathFunctions.addVectors(smallTranslationalIntegralVector, new Vector(smallTranslationalIntegral.runPIDF() - previousSmallTranslationalIntegral, translationalVector.getTheta())); + previousSmallTranslationalIntegral = smallTranslationalIntegral.runPIDF(); + + smallTranslationalPIDF.updateError(translationalVector.getMagnitude()); + translationalVector.setMagnitude(smallTranslationalPIDF.runPIDF() + smallTranslationalPIDFFeedForward); + translationalVector = MathFunctions.addVectors(translationalVector, smallTranslationalIntegralVector); + } else { + largeTranslationalIntegral.updateError(translationalVector.getMagnitude()); + largeTranslationalIntegralVector = MathFunctions.addVectors(largeTranslationalIntegralVector, new Vector(largeTranslationalIntegral.runPIDF() - previousLargeTranslationalIntegral, translationalVector.getTheta())); + previousLargeTranslationalIntegral = largeTranslationalIntegral.runPIDF(); + + largeTranslationalPIDF.updateError(translationalVector.getMagnitude()); + translationalVector.setMagnitude(largeTranslationalPIDF.runPIDF() + largeTranslationalPIDFFeedForward); + translationalVector = MathFunctions.addVectors(translationalVector, largeTranslationalIntegralVector); + } + + translationalVector.setMagnitude(MathFunctions.clamp(translationalVector.getMagnitude(), 0, 1)); + + this.translationalVector = MathFunctions.copyVector(translationalVector); + + return translationalVector; + } + + /** + * This returns the raw translational error, or how far off the closest point the robot is. + * + * @return This returns the raw translational error as a Vector. + */ + public Vector getTranslationalError() { + Vector error = new Vector(); + double x = closestPose.getX() - poseUpdater.getPose().getX(); + double y = closestPose.getY() - poseUpdater.getPose().getY(); + error.setOrthogonalComponents(x, y); + return error; + } + + /** + * This returns a Vector in the direction the robot must go to account for only centripetal + * force. + *

Hnj76Oym1QVh(3qRgs)GmgnEt-KxP|nCFY3uezZn zmtR0CZ$Z_-+f07?lu_tr~IC{&U6+QOth>ZgYk4V2FI$B2V3`M`Jk zsr>>lupymPeK129PfpDt9?GA2;I>03Ktz8NxwvTroqu8oaRB&bXT}G=^2UyOW}(4H z;9sG^YwV8K7pC&&viM^X_pfeFoN!cIhrE>OPQ5E<4KKDyPhRV^BGb_^Y6GO6#w}c= zu`0fC-@F4qXQtnB^nPmfI7Uw0bLhY^09TCO+H2(nvg8jdPjMAi4oSX%GP3oeo0`ks z%DoV|waU-Q7_libJCwnnOL9~LoapKqFPpZx?5FygX zsA~*ZR7X=@i{smf?fgxbcY6Y`JvD50P=R;Xv^sANPRp-Hc8n~Wb*gLIaoZJ2Q^CFe z_=G}y&{_NXT|Ob??}$cF7)$oPQMaeN_va1f%>C>V2E01uDU=h~<_fQKjtnl_aho2i zmI|R9jrNdhtl+q*X@}>l08Izz&UJygYkbsqu?4OOclV{GI5h98vfszu2QPiF?{Tvh19u_-C^+NjdAq!tq&Rd`ejXw#` z@U15c$Nmylco)Yj4kctX{L+lz$&CqTT5~}Q>0r-Xe!m5+?du6R&XY|YD5r5C-k*`s zOq-NOg%}RJr5ZWV4)?EO%XzZg&e8qVFQ?40r=8BI-~L%9T7@_{1X@<7RjboXqMzsV z8FiSINMjV*vC^FCv_;`jdJ-{U1<_xjZg4g?ek z4FtsapW_vFGqiGcGHP%?8US~Dfqi8^ZqtHx!}0%dqZFg%nQB)8`mE$~;1)Fb76nFk z@rK#&>2@@)4vO&gb{9&~R8-_{8qz6Rmw`4zeckD(L9xq}{r(fUO0Zh-R(d#x{<0j| z?6xZ2sp3mWnC}40B~g2QinHs1CZqZH&`+x2yBLT8hF7oWNIs_#YK2cyHO6AoGRG|RM>Hyn(ddpXFPAOGh~^0zcat`%&WoEQf9)!@l*3Tt@m>Lb z6$+$c!zsy_=%L9!_;jfd`?VXDd*^Vn%G>n~V9Vr6+_D@#E+dWB#&zAE+6xJeDMr1j zV+Tp~ht!M%^6f?)LBf8U1O4G#CutR07SB>8C&_&;g3TdIR#~e~qRtwd>&)|-ztJJ#4y0|UMjhJZlS8gA zAA260zUh+!$+xMfWKs|Lr23bcy#)JNnY|?WOka&wTS7_u%*N7PrMl1Lp9gxJY%CF? zz4IA@VVxX{knZPlNF+$9)>YIj#+(|$aflt=Wnforgn6`^3T+vaMmbshBjDi&tR(a7 zky~xCa77poRXPPam)@_UCwPdha^X~Aum=c0I@yTyD&Z!3pkA7LKr%Y6g%;~0<`{2& zS7W$AY$Kd}3Tg9CJgx=_gKR59zTMROsos?PU6&ocyCwCs8Qx1R%2#!&5c%~B+APu( z<1EXfahbm{XtOBK%@2a3&!cJ6R^g|2iLIN1)C2|l=;uj%tgSHoq2ojec6_4@6b<8BYG1h-Pm_V6dkRB!{T?jwVIIj&;~b7#%5Ew=0Fx zc(p7D1TT&e=hVt4spli}{J6tJ^}WL>sb`k}&gz+6It`Yz6dZdI53%$TR6!kSK2CfT*Q$`P30 z;$+G$D*C$U(^kkeY!OWn$j@IUu0_a{bZQ=TCbHD1EtmZ0-IBR<_3=tT%cz$>EE!V}pvfn7EMWs^971+XK}~kxSc_ATJJD$?)1Gz^Jq!>Hz#KkdCJ~jb-Y*Xv01_}}=T_V-A1<3O!V9Ezf z%Lnjihb3>=ZV}jSeqNu5AAdVbe|`;|p<%W#-<$s1oDYrB;C({psqV>ENkhadsC{cfEx=teVSB`?FOs+}d#pssxP z(ihudAVu3%%!*vOIWY11fn1M0&W|(|<2lEShz|#%W|wV2qM%#+P9NOy1x8jytHpfU zh;_L^uiL<<$L@~NpRXSrkJgdC>9R=>FmVu3^#C?3H>P{ue=mcv7lBmnfA?mB|L)EF zHv%Nl|D}0Tb~JVnv$ZysvbD8zw)>|5NpW3foe!QHipV9>Zy`|<5?O+rsBr*nZ4OE} zUytv%Rw7>^moSMsSU?@&a9+OdVgzWZnD>QXcUd{dd7vad+=0Hy)4|0A`}rpCx6cu!Ee5AM=iJ?|6=pG^>q(ExotyZP3(2PGhgg6-FkkQHS?nHX(yU0NG;4foCV|&)7 z1YK!bnv%#5n<25|CZ>4r1nK=D39qMzLAja*^#CN(aBbMx${?Iur3t=g2EMK|KwOF?I@W~0y`al&TGqJ zwf#~(?!>@#|JbDjQV9ct%+51l%q|lcY&f{FV&ACRVW*%VY6G5DzTpC!e%=T30mvav zRk$JOTntNoxRv>PDlJG1X=uep&???K00ep|l_#7=YZPuRHYoM46Z$O=ZZuGy_njgC z>P@gd+zKH5SjpWQ!h_r*!ol1s{9DS@sD4}xgFxaw>|av!xrKzg?rGnhZ#uZeU~iod z3-i*Hl@7cge0);y{DCVU(Ni1zg{yE&CxYT7)@zJ%ZZABj-Fh}0au^)*aw`vpmym;( z5|JZ!EACYenKNXH%=Md{my$sI3!8^FgtqkMcUR%w_)EBdP5DZ64aCIR%K99tId6SU ziT8Ef)K%7{XuIpPi}N+&FCm$elE>oKY;3c$x+*mXy?~wt6~?ss$HGqCm=YL2xzVTQ zr>*2_F;7j{5}NUPQ(aY0+h~rOKN|IA28L7^4XjX!L0C^vFB+3R5*1+s@k7;4d#U=5 zXTy8JN^_BCx1a4O3HMa9rf@?Fz>>dq}uvkY7!c?oksgs~xrpCo1{}^PD?w}Ug z3MbfBtRi z$ze~eRSLW^6bDJJeAt^5El{T*i1*v9wX{T7`a2wAVA z%j>3m*g^lc*~GOHFNy?h7>f7mPU*)3J>yPosaGkok}2#?wX5d$9moM~{NTzLznVhX zKa}bFQt#De`atoWzj4Lb@ZCud_T9rA@6VcmvW(+X?oIaH-FDbEg#0Slwf|7f!zUO( z7EUzpBOODL&w~(tNt0z|<9}Filev&4y;SQPp+?kIvJgnpc!^eYmsWz1)^n`LmP&Ui z-Oi1J2&O|$I<^V@g2Z91l3OArSbCkYAD0Tuw-O(INJJ>t%`DfIj}6%zmO+=-L{b!P zLRKvZHBT=^`60YuZon~D$;8UDlb-5l8J=1erf$H(r~ryWFN)+yY@a;=CjeUGNmexR zN)@)xaHmyp$SJcl>9)buKst5_+XomJu34&QMyS zQR(N@C$@%EmfWB8dFN(@Z%xmRma@>QU}!{3=E`wrRCQ~W=Dwb}*CW8KxAJ;v@TAs3 zW}Pq5JPc)(C8Rths1LR}Bgcf6dPOX<#X08^QHkznM-S>6YF(siF;pf~!@)O{KR4q1_c`T9gxSEf`_;a-=bg6=8W zQ&t`BK^gsK-E0Jp{^gW&8F9k?L4<#}Y0icYT2r+Dvg!bnY;lNNCj_3=N=yd9cM9kY zLFg|R0X;NRMY%zD*DbAmFV`(V@IANtz4^_32CH*)XCc$A>P-v49$k@!o$8%Ug>3-- z$#Fpo9J>eUMKg>Cn+T0H!n0Hf#avZX4pp54cv}YcutP+CmKC~a745-zhZp`KNms;J zS3S49WEyS8gCRAY|B~6yDh*cehY52jOSA#MZmk2dzu`_XpBXx9jDf!H3~!`n zaGe=)1VkfIz?*$T3t>-Pwhrw447idZxrsi;ks;(NF>uVl12}zI(N~2Gxi)8yDv-TLgbZ;L&{ax&TBv;m@z6RcbakF^el{!&)<___n#_|XR%jedxzfXG!a2Eyi)4g zYAWkYK{bQzhm|=>4+*SLTG2<#7g-{oB48b05=?PeW;Jo3ebWlo5y5|cl?p8)~PVZqiT^A~w-V*st8kV%%Et1(}x(mE0br-#hyPspVehofF`{gjFXla1lrqXJqQKE9M)8Xe0ZO&s$}Q zBTPjH>N!UU%bRFqaX(O9KMoG$Zy|xt-kCDjz(E*VDaI={%q? zURR{qi>G^wNteX|?&ZfhK-93KZlPXmGMsPd1o?*f_ej~TkoQ#no}~&#{O=>RadgtR zvig@~IZMsm3)vOr`>TGKD&fbRoB*0xhK7|R?Jh-NzkmR}H6lJiAZTIM1#AXE1LOGx zm7j;4b(Lu6d6GwtnsCvImB8%KJD+8z?W{_bDEB$ulcKP*v;c z*Ymsd)aP+t$dAfC-XnbwDx3HXKrB{91~O}OBx)fsb{s-qXkY<@QK7p-q-aaX&F?GS z2};`CqoNJ$<0DuM2!NCbtIpJ9*1a8?PH#bnF#xf~AYOIc4dx1Bw@K=)9bRX;ehYs; z$_=Ro(1!iIM=kZDlHFB>Ef46#rUwLM%)(#oAG(gYp>0tc##V{#aBl!q``!iIe1GBn z+6^G^5)(nr z8h#bm1ZzI450T?!EL)>RWX8VwT1X`2f;dW!{b~S>#$Pa~D6#Hp!;85XzluH%v5325 z730-aW?rY1!EAt;j7d23qfbMEyRZqxP};uID8xmG@mGw~3#2T^B~~14K5?&dP&H@r zL|aXJsEcAAXEXfu2d-!otZTV=if~^EQD*!NkUFQaheV&b-?-zH6JfjKO)aYN=Do*5 zYZ-@m#)5U0c&sUqu_%-Editr5#%Ne&bs)DxOj2_}`f;I_ReEY9U&Cf3rb>A3LK(ZD zid0_-3RfsS*t&g!zw}C_9u(_ze-vc1L59CdBl(IS^yrvsksfvjXfm>(lcol%L3))Q z@ZT;aumO3Q#8R!-)U697NBM@11jQ>lWBPs#?M4_(w=V_73rsiZh8awEm>q1phn1Ks ze@D|zskeome3uilE8-dgG(EojlI(@Yhfm}Xh_AgueHV`SL##I@?VR+bEHH=sh21A_ zhs&pIN7YTLcmJiyf4lZ;`?pN0`8@QbzDpmT`$m0CTrTMiCq%dE&Cd_{-h`I~f8Kps zAuZt4z)}@T>w$9V@iLi=mh({yiCl}}d>JN)z;*G<6&mgl(CYhJHCAPl=PYK2D>*F zy;YK=xS@1JW7i=C)T04(2P#|fowalY=`Y`G8?eRMAKt|ddG9UF^0M5 zW=ZGZ5qb-z@}iS`4RKXvuPIfzUHT)rv<8a|b?bgB3n=ziCiX4m2~CdVBKHWxw2+Hz zLvqoAij9(0moKoo2$`dqS0?5-(?^RXfcsQB6hU2SAgq8wyeasuyFGcK+@An?8ZzVw zW8wwbZB@i=<<4fA7JKPkki6y>>qO3_bW>-uQ*>9g+g7M0U^`RV)YTrGu2Q=2K>fiI zY0dFs>+}xuOZE^efLK2K6&X@>+y10Oqejnnq^NjfXt9JpK4K_E=cl29 z(t2P;kl4AK_Jg9v{1(z)ESpyo_(Z`74D&J1A#J?l5&J^Ad1sm5;Po@s9v7wOs(=_T zkutjt`BaxT09G{-r>yzyKLlM(k`GZl5m+Tgvq=IN|VjtJ*Zu66@#Rw;qdfZqi15A@fr^vz?071F5!T`s>Lx5!TszI%UK|7dDU;rUCwrRcLh!TZZ9$UMfo z@Qzjw>tKS3&-pyWS^p4mMtx`AvwxVc?g?#8aj@jQ#YKDG0aCx{pU+36?ctAiz=f$k z05S(b&VPQgA(Sm`oP&M^eiHvBe&PcTb+j$!!Yx(j3iI5zcQLOn(QqfX5OElbSsQBUw7);5C92onieJyx`p{V!iwXk)+1v zA6vStRZo0hc>m5yz-pkby#9`iG5+qJ{x>6I@qeAK zSBFylj8{FU*0YbFd2FZ6zdt^2p?V;3F~kap`UQgf@}c33+6xP)hK)fmDo@mm=`47* z9S6rnwCSL&aqgZs959!lhEZZp`*>V8ifNmL;cqajMuaJ~t`;jLPB?X~Ylk_Z#Q;%} zV+sAJ=4505-DdnIR=@D_a`Gy#RxtSX+i-zInO@LVDOd*p>M-|X(qRrZ3S(>(=Oj>} z89d75&n?m^j>;SOXM=)vNoum|3YmzxjYx%^AU*V|5v@SjBYtESp^yz?eQ#>5pnCj} zJ_WCw23wGd2AA-iBve8Hq8`%B3K4@9q@a}sf$49IA^IPsX@QK)36mrzqOv?R_n9K@ zw3=^_m#j{gNR0;&+F~wlS(i8IQN8mIvIO)mkx|e)u*y+xDie}%mkZ*m)BQM^$R@-g z1FrP0{8A?EcxtxxxX&J;393ljwwG?2A2?y-1M0-tw$?5ssoEsbPi?sd2!s~TrwPLF zYo-5XYV7AU-c|Vb-v;>pVi^CwX(Rpt<9{Ic?@<9SrNu>F(gwij%?dC9^!Xo90o1-| z&_aPKo%+xyw64e&v<}F^-7sO0Cz-VOF@7**i@v&(Oy4Q8PbV+4&rKwmYyokM z48OZ|^%*mC_Q)RJ31D#b4o4Jzr{~BX4D#swW<31;qCil2qlim;e=9ymJAEXfv-|h3 z)>uqQ5~S+8IgiWW28Fqbq+@ukCLy+k7eGa1i5#G_tAUquw$FjFvQt6~kWa69KXvAj z-knF`5yWMEJvCbTX!K{L)VeNF?(+s?eNjtE5ivg^-#937-l()2nKr#cHShB&Pl^l8 zVYws26D^7nXPlm<_DYU{iDS>6Bq0@QsN%6n>XHVvP<^rDWscC!c+LFrK#)T@$%_0{ zob%f&oaq>1_Z8Ata@Y2K6n?GYg|l8SgUr(}hi4D!@KL~hjRv<}ZZ`tCD^ev=H&^0pP%6q2e+t=Ua`ag8xqWvNnIvCU|6ZA^L5v{DD)!mcQ@n6{=; z#Z)PrAz>*+h-|IV!&J*f@{xb!L7h3{?FEs*ifw5z2U9$&OkYseI68yb=V4xv*VK3- zVxGhtmedujX32y-kC{5ej-Wy#JvB~4oxTb{|1H825_B(A0#?CjUTc=PrGh6jAgK9h zoLAe`+NBdStZE@Y8UH^Rd*|R-|7Ke}wr$(CZQHhO+upHlCp)%n+fH_}S8%^%xqhu%20_1p=x#Dl9ia`c3iM+9Vh5?gyY8M9c$tJ5>}V_sidHN zoMl%rSgSK!7+Y8tQkYq|;Vh`4by2uMsUfnxkk2{S@a>V#d}fv}Yud*>paVi_~T zU!GoYwWbnG%92!Cte(zhZX-i9#KJ;b{$(aZs|{MerP#6||UUx$=y)4XOb zihyKn`_QhJ#~@_peJ*8yD4>I7wQyKkZG%#FTKZfb(@G+9x7-3@hG}+ZC&$7DwbaB$ zC)jLj7yituY&WpOWlG7Z4Tuxzdwo6k!3lgwhh7BYMyB? zO9Q5nvn77~g~c623b`Pe5efNzYD#2Sfmg>aMB5s?4NC|-0pIXy%%`J;+E{(irb!Szc8M8A@!}0zqJLoG4SJ5$~1*yRo0^Z`uObA+= zV?1sYNvzvWbP%AsMzoIo3Cwx~y%i8rHF(BgLS>tH5Ab|1wp$X_3o2_VB(pFxgQ5QQ zk@)Vy95$b%HVf4@ppX(wrv^Jwfrsu+9N_OUm}nD7Ch_7STj66EYsZR#`9k|Tf^@p& ziHwnO$p{TB#R(Q{Os>Un~0!r$JO zLZ&F%SP|%$TuG)mFeOhKr1?S!aa0jTV$2XIeZb_fgO&n{8HTe9s`L&(tKoy?OaS^$ zLHNrgYgq920EI~M>LyU7gK70$7*`nFKD^d>MoEAhsBU0%@*RW@%T(J z?+wVbz=mcN%4#7qlCpl_^Ay7VB%?+uW1WSNnQOj^tALyqTpV zkEN2C;qO_W)MYl^Ow5I;t3;z#iG82F(qe}#QeE;AjA=wM==dB(Gu+ez*5|RVxO4}l zt`o?*B;);-0`vR(#+Q^L4WH_9wklh-S-L-_zd%Q0LZ%|H5=>Z)-x#Z+m%p&6$2ScV zEBneIGo)r0oT)xjze*Q~AIqhB%lOM5Id}^eKwS!?b_;B&TouZsemyL&y`)#FX}ZKp zp)ZnB*^)1P@2bCoe+Z|#KhTBNrT)UN@WIuudw})fwHl)re1|b~E1F=xpH?7L77p>5 zei$aD@KO0<+zo1<&7OuZatNsPq24Whu%0jD_ z$ZZy6MzayYgTJulNEy8D$F%JDYgx|d6{6kpDg#s170<15bM#4tzvrDU$6bvu-hH@6 zgcjq&3aR3k(23$FaUA|iuoy*bO{2F6W0<+ZdsYvXjc?d@ZT8kM!GD}r@qr;TF@0Hb z2Dz-A!HZ$-qJ?F%w6_`t`8xk$f$MNBfjqwvJiVdD+pf7NVFGh?O=qp2vh%UcYvc{rFldib~rkIlo`seU%pO_6hmBWGMcUhsBSWiQYYPMX<-Cjp49@7U==iS57bG zw3T9Nbm`)m9<<4e$U74`t~zRo0JSfi}=GdQXGLLPyW zlT^I}y=t$j{Vx!wN^z8X4l0|@RNrC#)G>bK)7IT7Qop>YdS^NnI3gfP>vtp)pXkr2WSVcAAv8uN>@ z`6)kICvNYU$DA8pnkl4sQopDC6<_M8zGJ^@ANXJL(yd#n1XFj9pH;rld*gwY8om_I zdB55w@FUQ_2k}d%HtQsmUx_7Mzftky&o2X2yDQrgGcehmrDDDtUJj5``AX$gzEbMc zUj2Qzp)Lo>y-O*@HJ|g9$GR2-jgjKfB68J6OlIg;4F2@2?FlW zqj|lO7A2Ts-Kd!SO|r9XLbPt_B~pBpF40xcr0h=a&$bg(cwjp>v%d~Uk-7GUWom?1 z92p+C0~)Og*-N~daT#gQdG{&dPRZso(#{jGeDb1G`N)^nFSB`{2-UQ&!fkPyK`m03 z_Di94`{-(%3nE4}7;4MZ)Pmawf#{}lyTSs5f(r;r1Dp4<;27K=F}Oga^VsUs3*NIn zOsYstpqpRF&rq^9>m50LRORj>=;{CV2&#C$-{M5{oY9biBSoQyXvugVcwyT-19S;pf!`GSNqb4**TI%Y z*zyV)XN3Fdp3RNNr9FU+cV*tt?4L8>D@kJp^rkf_rJ~DPYL}oJngd1^l!4ITQN`0RTT^iq4xMg|S6;d}lznE$Ip^8pW-CHu zP*^!U>Lcd3*shqa)pswq;y<|ISM1g1RG#`|MSPNAsw*XH1IAD(e(Kgqp6aDHgv>fI z!P67$z{#()Pdo3;4dUoy*Xor(O?+YTRPe=g*FfRj*9q9!8p%1l>g3e^rQ_nm{(@4t z?^nMDC2J8@my5q0QyCljCSp_@)No+6bZ*y)lSdrkLFcR6YOHu*vZ-q(C);5$MmM_z z1WT>Gc8g%`Rt~6*!}JhWi0=Rc_z5c8GR9YXW+cdoK~Ea(@wyXf|89HagNuFAO-V7k zUb|9zaCCWH3^Fz(m7$8K$|0ZOP!SNpgP!ql<)!z8w$Z$?9gq2f<~koe3|zD=imLfD z>IV5?SkRZ;7JlOG%z%Tlze$GXr0A}ResyF63ZGZVDLv2k4HWtoqoCaq+Z&GaVKuLA z>@zhNjYYc=sexH?;DTe4&2vnQE}C@UFo&|qcLddvH0FwswdRUc(p*X&IT^Zu>xLpG zn(@C%3ig(l2ZPm#Fc){+0b+%O7nt4zbOt+3@GQVm|1t70=-U(>yo3VY2`FnXFHUyi zwiqf(akt0kEE5_Pa-a*VCS}Pi6?`~P%bvX6UT~r-tUAY%I4XF3^nC+tf3alyL{M`w zv?aVQ#usdwpZmkrfv19O39}tQPQM+oY**a{X?@3Qe>r$+G!>r#?Id&U&m^HU(f= zjVpSi9M||1FyNQA&PO`*94&(qTTMQv3-z`bpCXs-3bX}#Ovqec<>omYhB*VrwxqjY zF3#OXFsj`h#G?F}UAilxTQ|78-edHc-Uc-LHaH*Y(K%R#dVw>_gz}kRD4s#+U&Pq= zps)kMf_t9`GHR7CO4zI8WVj0%qiSqy50N{e_5o#GrvNhMpJf5_sCPrEa%a@ltFnss ziaWh26vEW4fQp}qa4oP(l4xIMpA)~VHD9!lP%;Tm`(HD$jYMM-5Ag>S(gC35J35$%?^gk(r|`4Ewi-W z;f&;B*fO=kC@N=r<-#nGW|yXE;`zb0Y3TJOAkw1a$SQgoTawHZTck+V%T=spmP`^BHihc(jc+S1ObX%6AYQ6LVVc+BfM*P{2s0T2z zVIs*5{ql%#CKAzv0?@S+%||z;`dpfj0Y(VtA51n$j%sG5I%A|h98VU}PkVZFrk1*G zaw75v3(N50lanvr&ND4=7Db;HS4fpi)2vTME7aD2-8N5+kcOXmYCrLE?*5&dWhvB` zbD5)ADuIwwpS*Ms;1qyns(8&tZ*)0*&_lNa`_(phwqkL}h#WdX_ zyKg%+7vP>*&Fus9E4SqIN*Ms`QLB(YOnJ|md%U|X`r#tVN$#q6nEH1|blQ?9e(3|3 z`i#;GUl~v?I6&I6%YvkvmR?*l%&z)Pv8irzVQsWrZSr%aoYuPJa#EjK|4NmiuswK= zlKP2v&;yXv3>LQ$P){aYWrb)5GICwbj;ygw>*amKP;Z{xb^cF}O@IeQ^hB-OjEK{l z>#PNyLuVkeDroL9SK2*ChHmJJSkv@YRn7)E49fy!3tqhq`HtHs_(DK|2Lyv(%9L&f zSy+H}Uk{nE2^5h7zN7;{tP3)$1GK9Xcv^L48Sodg0}ZST@}x607yJo2O*XCfs7*wT@d?G^Q6QQRb!kVn?}iZLUVoyh8M4A^ElaHD*Nn2= zkfCS=(Bg9-Mck6K{ z%ZM59Rs4(j1tSG1B#wS=$kQfXSvw6V>A(IC@>F;5RrCos`N{>Oyg|o*qR2EJ>5Gpe ze~a4CB{mmDXC7C>uS@VL&t%X#&4k<`nDx;Zjmo%?A4fV3KOhBr;VuO!cvM8s2;pG5 zcAs!j?nshFQhNA`G3HMS z?8bfRyy1LwSYktu+I7Hurb-AIU9r|rl5nMd!S&!()6xYNJ1EqJd9BkjgDH@F*! zzjtj4ezywvlkV7X@dG^oOB}T76eK=y!YZB#53LhYsZuP&HdmVL>6kH8&xwa zxv8;t-AE>D5K<{`-({E0O4%fGiLVI8#GfZ0aXR6SfYiPUJKnujMoTI5El<1ZO9w|u zS3lJFx<7XUoUD(@)$pDcs3taMb*(v2yj#G)=Mz-1M1q@Tf4o{s9}Uj9Yo?8refJwV zJ;b+7kf0M}fluzHHHS!Ph8MGJxJNks7C$58^EmlaJcp`5nx+O7?J)4}1!Y>-GHf9o zk}oTyPa>+YC$)(Qm8|MhEWbj?XEq}R=0NFH@F3ymW>&KS!e&k5*05>V@O*~my_Th; zlP05~S5@q+XG>0EuSH!~gZe_@5Dbj}oNIiPJpEOip+3l!gyze@%qOkmjmx=?FWJLF zj?b}f8Vet*yYd16KmM43rVfZo?rz3u|L6Foi*GQe4+{REUv9*}d?%a{%=8|i;I!aT z7Wxm}QJC`?cEt9+$@kSkB!@`TKZz1|yrA1^*7geq zD5Kx-zf|pvWA+8s$egLrb=kY385v2WCGL{y4I15NCz5NMnyXP_^@rsP#LN$%`2+AL zJaUyV<5;B^7f+pLzTN50Z~6KC0WI<|#bMfv+JiP3RTN^2!a7*oi+@v3w*sm5#|7zz zosF*{&;fHBXn2@uguQ1IDsh(oJzH#i4%pk;Qh^T zfQLyOW;E*NqU!Fki*f-T4j(?C$lY2CT{e!uW}8E(evb3!S%>v^NtNy@BTYAD;DkVo zn9ehVGaO7s?PQBP{p%b#orGi6Y&~<;D%XLWdUi}`Nu-(U$wBBTt*|N4##sm2JSuWc)TRoYg57cM*VDGj~ka<=&JF zo8=4>Z8F`wA?AUHtoi$_hHoK!3v?l*P0$g^yipOWlcex4?N2?Ewb1U=lu}0`QICA4 zef61j-^1p}hkA*0_(esa!p%dX6%-1e-eMfQsIp6wRgtE=6=hDe`&jel{y=6x5;78s z?5^{J|t!#x1aS8<3C`v%E%u{*wZwSXr$0Owl5_ zmXh>D>C_SjOCL^CyGZpBpM5`eymt{*rf~9`%F&&o7*S!H%3X)7~QFgn^J>6 zD+yV}u{HN-x9*_$R;a+k?4k*1f)rE~K|QvcC3dlr>!nftB?gE-cfcPMj&9mRl>|Lg zQyCe|&SuZopU0>IfRmcV3^_mhueN5oQ=J+H4%UsSIum4r4!`^DJqZr?1j3BU)Ttzg z6LwM)W&UEMIe*H2T6|{rQ;x9qGbp7ca#-!Egm4|ECNTMN);`>2Q&%|BpOdIJ4l|fp zk!qEhl;n(Y7~R1YNt7FnY10bQZXRna2X`E_D1f*}v1bW^lJorDD0_p2Rkr32n}hY! zCDB(t$)4YOd)97R60gfg3|wrlsVs#4=poh4JS7Ykg$H)vE#B|YFrxU-$Ae^~62e;! zK9mwxK?dV4(|0_sv(zY&mzkf{x@!T8@}Z6Bf)#sfGy#XyRS1{$Bl(6&+db=>uy-@y z$Eq~9fYX$06>PSKAs#|7RqJ3GFb;@(^e`jpo-14%^{|%}&|6h{CD(w@8(bu-m=dVl zoWmYtxTjwKlI!^nwJ}^+ql`&fE#pcj*3I|_Z>#y##e@AvnlSN4po#4N#}WT)V5oNP zkG+h_Yb=fB$)i`e2Fd28kS$;$*_sI;o0Xoj#uVAtsB6CjX&|;Bk}HzQ*hJ!HDQ&qZ z^qf{}c`l^h5sg-i(pEg#_9aW(yTi?#WH=48?2Hfl_X+(SfW)_c48bG5Bf+MDNp>Y#Mpil%{IzCXD&azAq4&1U10=$#ETJzev$)C*S;Pr9papU3OabRQk_toRZ!Ge(4-=Ki8Db?eSBq~ZT#ufL6SKaXZ+9rA~ zQwyTQTI7*NXOhn?^$QOU>Y6PyCFP|pg;wi8VZ5Z$)7+(I_9cy--(;T#c9SO;Hk~|_ z0tEQ)?geu8C(E$>e1wy%f@o;Ar2e#3HZP$I#+9ar9bDa(RUOA+y!oB;NEBQ`VMb@_ zLFj{syU4mN%9GF;zCwNbx@^)jkv$|vFtbtbi7_odG)9s=q(-PtOnIVcwy(FxnEZm&O^y`vwRfhB z7Urcums9SQS6(swAgl?S|WDGUTFQu51yG$8069U zviuZ=@J&7tQ8DZG<(a->RzV+sUrmH$WG+QvZmUJhT*IoR3#3{ugW%XG0s?_ycS6V6 zS)019<_Rl@DN~8K4#w3g_lvRm4mK3&jmI$mwROr0>D`mX+228Dw4r;mvx7df zy~$zP8NjVX?xkGFaV>|BLuXMQ+BN+MMrIB4S6X)p&5l$;6=S8oI9qi&1iQbs?TroDMfCmIeJ}pbVVtVqHhS(zutEy6#UjTk29-+3@W0`KfehW`@np zhhu#)O&g%r)hTj4b$CY41NYp_)7!bYyG;v(rts z^}YDJt2W88H^H;e$LSm3dh=~yi@)mzJtEfW8=4avbeOE&;Oc>-6OHO+MW`XBZ4rO6 zS;nAi**w3Yso4&Ty+8f$uvT?Z)eaLe$KW1I~9YM2zeTIT}C%_G6FPH-s5Wi3r`=I&juGTfl zZ;4qFZV|6V0c&>t!Y>mvGx#1WWL0N5evV=u28K9**dv`}U3tJ$W?>3InXiwyc)SA% zcnH}(zb0@&wmE>J07n#DOs7~lw>5qUY0(JDQszC~KAAM}Bmd-2tGIzUpO@|yGBrJyXGJk3d+7 zJBN0$?Se(rEb0-z2m%CBd;~_4aH04%9UnSc4KP!FDAM5F_EFujJZ!KDR-fn181GX` z8A?8BUYV}D9bCE0eV~M>9SPag%iVCLWOYQJDzC4~B~Ct0{H7x|kOmVcTQ;esvyHJC zi$H0R73Z8+Z!9^3|2tNut#&MVKbm`8?65s)UM8rg6uE(|e^DYqvoc15-f;u8c=>3;Viz*T# zN%!T+Hex0>>_gUKs%+lgY9jo6CnxL6qnQ>C*RseLWRpipqI;AQE7;LUwL`zM%b`Vu z%Sa-+?a#+=)HaD|k2%_(b;pHRF96(c;QyPl6XHL8IqGQKC$M8R=US-c8;hUe?LKo&l!{V)8d&55sUXEu z5uITcO~`ipddh+Nr{7ibp^Wd{bU)^3##<5`lkuqfckxEU*9{pgNpTB2=ku1c-|3dK z|LIQF=ld@I7swq^4|G1VA}BK85&>2p#*P95W`I1FF(8G9vfNJ6MoN$+C^M89u!X=< zJSS%l?Qj>$J%9?0#0&S6#*h*(-9Z$}q*G#hP?cX7cAvM0eiVFhJJ~$`iZM!N5NhDb zi<1u_m#?jzpIaOe7h|Kiap#mHA`L|)ATnPJ7du{^ybuNx@1jA+V1l8ux#{LJ#teM(6=%gZcMq24J$2p z`wcC!qRssmwUv4H6Psw{(YdDNOv$!sq&O1SvIS}fCKZa+`T=Ayt@uZjQqEC{@Uj+| z!;i3W+p~=@fqEEhW@gT^JtCR<`m`i|Htg<TSJ&v`p;55ed zt@a|)70mq;#RP@=%76*iz>fAr7FKd|X8*@?9sWOFf$gbH$XFG zcUNu#=_+ovUd>FW*twO`+NSo*bcea=nbQ_gu^C7iR*dZtYbMkXL5mB@4a3@0wnwH! z(fZKLy+yfQRd%}-!aPC z4GB%OvPHXl(^H(BwVr6u6s=I;`SHQ1um7GPCdP-BjO%OQUH!_UKbEGvHCY}{OL`8FU$GZ;Y$SlS$-0VjK%lCP?U0shcadt4x7lN4%V}wBrLEbiEcK-OHl+pcBNSqN#mftpRj2A4Q z+av@-<#t_Dj_FN^O2~wq(ij1O*+=RVl+6gNV^~CI1UED- zn^zN@UOq8?q58b^4RA>lV}x;jA2OE=SqMYV9P#RsUlI+pp!y*jpwHgp-w3i$V)%?L z>irn1pnRc|P@r|Z0pCeMZ*k$}$`1GVGCT&QtJ`V%Mq!TXoge?8Fjn$bz}NqDn*2ZQ z$p3@F_^(}IVS76>OLNzs`O5!pF=LZ$<&gyuM$HQzHx8ww^FVxnP%Yv2i=m*1ASF~~ zP=!H}b`xl`k0pL5byku2QOS~!_1po!6vQyQL#LQ#rIRr?G5^W?yuNvw-PP{}%m35i$i+I?DJ%RGRcqekT#X~CxOjkV1UQrd&m_bbJ+gsSGbPwKS{F& zU-`QNw!*yq#Co#{)2JvP-6>lY$J$2u+e=r0&kEc#j#jh@4Tp;l*s<28wU%r= zezVPG^r*a?&Fn_(M|A7^xTPD998E-)-A4agNwT?=>FbrHz8w~w?hWBeHVYM()|buJ zvGv4j<%!U_Rh^ZKi~2(h1vk-?o9;`*Zc}m5#o@a1ncp)}rO2SDD9y!nT$_Eb%h`>% zDmssJ8Dl=gDn<-7Ug$~nTaRzd?CJh;?}nCco$7Pz<#J8;YL40#VFbAG|4nA$co;l^byBOT2Ki@gAO!{xU7-TY|rujdYTaWV(Rr{Jwu?(_TA zDR1|~ExJBfJ?MAReMF47u!oEw>JHVREmROknZUs2>yaboEyVs$Pg1f6vs06gCQp$b z?##4PWI#BxjCAVl>46V_dm4?uw=Y@h#}ER4|ACU{lddiweg`vq>gmB25`XuhNai1- zjt{?&%;TRFE+2Y_Gn;p^&&|bU44M=`9!Mc%NbHv|2E4!2+dUL z>6be$Kh|Duz}+)(R7WXsh!m`+#t^Its($x`pqDaN-^E z?*a=0Ck^rZBLQV~jY-SBliN&7%-y3s@FB;X)z(t&D=~@U0vT%xfcu`Lix=W#WVE{{ z2=C~L$>`~@JCIg8RAyk= zYG`(@w4H95n0@Fqv16~nlDU!+QZw&#w@K)hv!V>zA!ZOL$1Iykd&Su3rEln@(gxO| zxWc++T-rQEIL+j7i`TeatMfp4z7Ir31(TE4+_Ds@M|-+cwQg(z>s=S}gsSz{X*Wm+ ziKJWgOd`5^o|5a#i%?Gvw~8e?Rpi7C>nQ5dvPHVTO$PI^mnJ*7?gd3RD{|c_a>WrXT#Es3d}(k z$wpmA#$Q^zFclx{-GUL_M$i0&mRQMd4J#xq-5es)yD{kYCP1s!An(~K5JDRkv6DUSKgo^s@lVM5|V4mWjNZp zsuw^##l%rbRDKglQyj?YT!nk$lNUzh%kH705HWhiMuv(5a<~yoRDM&oCqm+1#S~|8 zA$g2Xr=}p_FX%Eaq{tUO9i*Q1i!>$+1JYZCL}flWRvF0y1=#D#y-JQTwx6uP-(bC} z_uP7)c;Xd`C6k#JVW?#Id7-|`uW+hN0>OM=C2Ta^4?G zr;EvxJ{%l|8D-heRYRM%f*LBC)krHZJ@%&CL0)FADWh14&7KV<9km6gE=o9(7keg~^rIQtthK^_8%Jk&aZLY_bc6SbY>IcwDK9{sV*t1GfKwf8aCo8t za)yALEi^-WXb!k6n>W-62Z^n8hO|eRYr&uZiW5d_URi??nl*aGu?ioQ+9RF9u8kwD z6UZ6HVd(G%l9>y7E)uyn?gAJMKeki0@tG*jdcE-}K?8(D-&n=Ld1i=A1AI<1z>u5p=B z<1}|q3@2jNxW-}Q4z~s|j&^Qc;nXIdS3K8caP_07#ig} z#KAD&ue2jXc&K#Q`Hy#x+LeT4HHUCzi1e?*3w{tK+5Tij(#2l2%p#YGI-b~{5{aS8 z!jABC*n6y~W|h;P!kn(a4$Ri2G118!?0WHDNn((QDJP^I{{wPf<^efQWW?zS>VS?X zfIUgCS{7oV$|7z2hJBt+pp1CPx4L{B_yC3oWdE)d)20WG6m5qknl}8@;kjPJE@!xP zV(Nkv^-Vz>DuwBXmKT(z>57*D<$u=Blt)IS-RK0j89omD{5Ya*ULWkoO)qeM_*)jF zIn87l{kXPp=}4ufM1h7t(lAL?-kEq>_DE-in8-!@+>E1+gCV9Fq)5V3SY?**;AKq0 zIpQ(1u*3MVh#tHRu5E5=B{W-QOI34plm`#uH(mk*;9&Re%?|v-=fvb;?qvVL@gc|l z8^L?2_0ZrVFS-stRY(E>UiQeG_sMrw5UiO znGFLOP-GO{JtBM@!)Q37k3G_p&JhdwPwtJS6@R4_($Ut^b!8HP{52-tkue8MG=Zwr z7u6WaFranJq4oNadY)>_6d~?pKVxg$2Uz`zZPnZVHOh-;M|H7qbV0OF8}z;ZPoI+| z(`e}bn6u*kJpRLC>OZ}gX#eHCMEk#d8y$XzSU;QZ|An$pQ%uZC$=Ki!h@&m8$5(xCtGaY3X1FsU?l5w^Fr{Q-?+EbUBxx+b?D z80o*@qg0juG;aZhj=tO=YHjfo=1+-NqLME~Kw7Y1A*?}M7#cOyT(vd$1tVPKKd@U! z&oV!RzZcK6gPWj`*8FIAy2I&x``h_sXPe*O{|ih(Y+V3|o68MWq~2Iy^iQ8RqK76f zC$1+hXqd^jsz`U{+EFo^VQNrLZt#R`qE*>2-Ip&(@6FmtAngx@+YnG}b5B9Y)^wg#oc z24KlT2s!H_4ZR^1_nDX#UH4(UTgl603&Q3g{G4!?6Sl9Om=Sy|8CjWO>d@e9?Q%s- z-OS3*W_H7*LW|Ne{b+^#LqQ}UKDmiZDma@no2!ydO^jcm>+z379K%=Ifs{20mT|xh zP$e7P=?N(tW4PMHJOQ`a8?n}>^&@<`1Rgo`aRevPp^1n7ibeS6sc8^GPe>c&{Kc+R z^2_F~K=HVI45Pf|<3)^;I{?H}vU7-QK3L1nHpcn3!1_)<$V;e0d_b8^d1T==rVpky zZTn~UvKrjdr11k}UO@o>aR2wn{jX5`KQQM1J1A?^wAFvi&A#NA#`_qKksu`sQ0tdM ziif17TO<{wDq_Q;OM}+1xMji^5X=syK=$QdZnS#dwe$;JYC7JozV8KpwfV}?As|^! zFlln0UitprIpuzLd$`<{_XoUV>rrHgc{cUQH-Px#(_Ul%=#ENrfJe@MRP_$E@FLMa zI`(J)Imw$o427@Oc^3(U&vz}<3Lfmy7diVpJJJ@gA>e;q-&gj zcGcBC_luF%_;**EB?o--G?AkaruJ%-b*8aX$4E+-?V@RWMnjHJ;hx27Vd7l0nUUY( z6OQb&8g8cvN3LZ%^xvIav*X|Epqm@yrTZk9U{GSZXAUJt8Lh(%7?Eaf&AzmXOVvU| zmz<@l1oMe#^POR38KT6q3@c`{%eYNu4ccurv`q?b5DzLxENjSfYOJHAI$MbSNgB*D zJsP>i*BgrFlIn?x&DH9x~UbPBtMFj{_vJ#CaAF>1$oE&k`EF&L@HCa@mN>Q7~!RU>7 zW%fv84aCKSgBacmuvg}r@)YKqO$U{D5|!`vG-Gp%An}raz2gESWm0Exhux4C)zE}} z_@kn z3t}bvm?L+@@az@<*jG>(Xopq&c*;^mttlJ!mv;5k6o%Ac<_`o`4G3qzzo(GO{!&F8 zW+~bF?S;7gO1dQ@>gwZ?iIHjE#^@;Ix!Z`R6{RYLlGB&v4A)ha(2hc`RGV-8`LcvSf+Y@lhT%(Z7$tWEF;cZs2{B|9k#&C}sPyr; zd-g~${TqY7E$9X+h4_(yMxQ%q;tm(h(lKzK)2FQ%k#b2}aMy+a=LHYgk?1|1VQ=&e z9)olOA5H}UD{%nu+!3^HsrBoX^D9Iy0pw!xNGXB6bPSpKDAaun{!fT~Z~`xp&Ii~k zdac?&*lkM+k_&+4oc6=KJ6RwIkB|st@DiQ!4`sI;@40>%zAG^!oG2@ z@eBM$2PJ@F&_3_}oc8A*7mp-0bWng^he9UYX#Ph*JL+<>y+moP^xvQF!MD_)h@b}c2GVX8Ez`x!kjAIV>y9h;2EgwMhDc~tn<2~`lf9j8-Q~yL zM=!Ahm|3JL3?@Tt(OuDDfljlbbN@nIgn#k+7VC+Ko;@iKi>~ovA)(M6rz5KP(yiH| z#iwJqOB7VmFZ#6qI~93C`&qTxT(*Q@om-Xb%ntm_?E;|58Ipd1F!r>^vEjy}*M^E(WslbfLE z<+71#sY~m$gZvoRX@=^FY}X?5qoU|Vg8(o`Om5RM6I(baU^6HmB<+n9rBl@N$CmP41^s?s1ey}wu3r3 z4~1dkyi%kA#*pLQy0phlXa-u(oK2Dwzhuex$YZv=*t*Tg5=n~H=}fJA!p2L78y3D2 zimkqC1gTU(0q||k9QM#><$b-Ilw#Ut2>JF=T^qN34^qcBEd={! zB)rxUbM2IwvMo?S;Id^aglw}-t9et}@TP;!QlFoqqcs(-HfNt9VqGFJ4*Ko*Kk#*B zGpJ>tA9(=t|4#M!kBaf%{$Kfj3-uf|ZFgiU`Bo>%k_OuAp~vnE^_Tg8*% z*?)4JdzyMTzvNDy{r$c``zBw=Vr)6c4}CBIv#mw()3h7`?V-;LF?J&N5a>kjpy;9n zQyXvuu`n?+W84QV=(i`JEJY=}Ak+u4>!Lyt2P!$nBl}T=^|pG*z@)_l!)OKB{tIV&&E@hj=OIhSBHgPV~X=R3NrTMh?VzDm?1yW^IJ&zzAn2{8rE~MRX5EE)a(-T&oE)1J4pGXBYi+nexX-?5! z{EZ4Ju=Y8MQ87=uNc2t^7@X)?85KeSoc`?BmCD;Uv_cwQaLyc}vvnJKHV zuK)H_d)xhGKB!_pRXv{$XgfZ_(8G%N3o$ZI#_ zixQj~so0*m^iuA!bT>&8R@>b%#B~zbIlwt4Ba0v&>B(`*Z;~?6!>-aQ zal+Qt4^dCcjZZMd4b4Khg~(GP#8$3BeB8j!-6l?*##)H?J$PeUy)cA_I26#0aggao zaM5PweS_Sb@{OZ@Uw*(!DNV)KTQU+BTRi?AUAv0Vowth`7mr9)ZVC+TI?@; zWGL&zydnsuE3+D7#U~P%PrxpD3nTc9#mm621iX*?ZMS_Q#n9SzOJ~Hg@`rX{d?qJ; zt}`76!H)MX#=VKifJZP$3<8@}0-llthFpq3FV;(UP$-k63MkHHq~J&}d?C<+c~*Zk z<#G&>AD7EoiAVO38TO2TOBKN>6N|JS*{+`}V-)T0j(bAzGlEUWEvWLrMOIItYexh) z?he>SJk*#bywgDF6+*&%>n%0`-3tOY72+n&Q1NJ`A-bX*2tJV(@;%b6&RxMcUd7+# z@UzOmc9DolSHc-D$5(GouinaE%&uOVMyD&CTdKaEB{Qap4_wU7_=23CULKQ;jmZuV;+Y$(`#Gh0@}s7-!qk-^&#IG>7B{yft?UoA)H5 z|B0u3Tu0TF{AB0jpT|E&RsYB$3WiQU^5p*|f)^Si_#^j+Ao^|5(gNjn+!0|NtXDt* z5fwxpajl@e0FrdEuj2s#Pg>gUvJdko9RBwEe_4@?aEM?SiA2nvm^tsLML{-AvBWM7 z_bm7%tu*MaJkUWd#?GWVrqaQ0>B%Azkxj+Yidvc$XdG1{@$U~uF|1oovneldx`h;9 zB1>H;;n1_5(h`2ECl?bu-sSY@d!QTa`3DrNj_F@vUIdW5{R7$|K{fN11_l7={h7@D z4}I;wCCq>QR6(;JbVbb4$=OBO)#zVu|0iK~SnW~{SrOq&j*_>YRzU&bHUhPPwiy($ zK0qin8U;#F@@}_P_flw`bW_v^G;ct?Pb65%=%egDBgS#YF3?E36$9xzdvYqjAZoK#hcjctJu~MF^S*$q3`o2;!L|jPnM1x*Q~qF%BH(5UDFYglsJwO zEdEuB7NihnTXK6$)F~``nmSQNFP7x7hE{WuOjTAhEjGw#XxvL@S;aZYuyu9)!yZ~X zo35D6Cwb8`shRXCCR;xlR`n`cs4aie!SSM`0)x3ykwM*k zK~w^4x2u#=jEEi`3Q9AU!wE)Zpn#)0!*~)(T^SEjIJveav(d1$RaSMC0|}<)?}nSG zRC2xEBN_YAsuKyl_3yDt%W^F`J-TyeGrcfboC_0Ta=KcW_?~RLb>xbqIVI6`%iWz; zM8Kq9QzwO8w!TntqcB;gNuV$gd+N|(4?6A9GEzYs z5f4(*N5}&ObeYA~I28r;?pKUj4N6}iloE=ok%1|X()Ahdwir?xf6QJfY7owe>pPj)Me*}c^%W-pP6`dnX1&6 z`b#*_P0PeM+1FR)t)Rnr22f!@UFBW!TxgjV)u0%_C~gIbb_D3aPhZ~Wmex0)Lj`VoZKjoW)dUoKY6*| z0|V)|XyjiKgZ}s5(SN?te*muif87vD_(wYOiOjOKNI4L*aK||2$~;s25HS#iY6r=)WW8a^dkd0Y|pPc1-9jmy&wqoCbL84`C94At6$lm_o!8m*did^?o$m?ozIp{RmZ*M%YMX_i$KYkz_Q)QK?Fdm)REqf*f=@>C-SnW{Lb;yYfk&2nAC~b}&B@@^fY7g;n(FVh_hy zW}ifIO9T7nSBHBQP5%-&GF8@A-!%wJAjDn{gAg=lV6IJv!|-QEXT+O>3yoZNCSD3V zG$B?5Xl20xQT?c%cCh?mParFHBsMGB=_5hl#!$W@JHM-vKkiwYqr8kZJ06n%w|-bS zE?p&12hR2B+YB$0GQd;40fJd6#37-qd1}xc1mNCeC%PDxb zlK=X|WE*qn2fROb4{oXtJZSyjOFleI3i8RBZ?2u?EEL1W-~L%7<`H6Vp0;cz5vv`7jlTXf-7XGwp}3|Xl6tNaII3GC z9y1w*@jFLl2iFA!<5AQ~e@S|uK4WL9<$R^??V^aM?Bgy=#|wl$D2P$o;06>{f)P+X z91};NrzVV+)b}k2#rYLF0X0-A+eRul=opDju)g0+vd79B%i!Y}*&a^L$_|C&jQN^j z9q#4<(4)3qNst^+ZYpyVF2hP;DN|OMxM9w(+)%kFQRcYVI zO-frej9x6a%-D%Xuwedcw9#3VSVkOjNF!BYRoY1KD3wFJ%?ML*3QwcarMK)@v`o%s z$w=NLrO>og`nRJpZZ(%~*hNJU#Y~k;_Ci3~gc=4UQO!Ydje^?=W^DgCKyO;Zz4LgQ zKtm($MdY;UZ((U_g5*pMY+dYGyyT1ERkaj`U#S-2yyJ47wMonCpV+2rI8zPNHDfo& zc59dFz*2#^A-R?P6Np}jhDLi4&vP%$NW#8J>=CLj1mlf$XzmQezH*F1jNOiPgXl2j zzD07AKLT*h$CA*OsOba2etPLU%|p?=XhplXo?vOu@q0{QBo++)@6U?YKv_)GFK(^Y zm&uFBbrQyzJm;c49O00PIt;|{&ei%VSS%Y3m3#~L#(3%Gso^a4#9AaB$w@vnAvdr6 z%!2#)YS0HFt%o)q6~BelT;?%oUjX%9qQCn#-~+TM(a^s%Y>&aBkL(UY{+?a9@&Q+a;t%c_6u^6_r@>MEAN9ir5q=Yo|R8z4lKYd1sv^LyTozFn$KqaJ>? zoH&+`AX>E03Gv=71+NZK2>!-NasKeCfMp;@5rZ z*m<}q2!$AgKUwWRXTVHs!E>`FcMT|fzJo30W551|6RoE#Q0WPD$fdA>IRD-C=ae&$=Fuzc6q1CNF>b3z_c<9!;))OViz@ zP58XOt`WOQS)r@tD0IiEIo4Umc(5f%J1p{y4F(1&3AzeAP%V)e#}>2%8W9~x^l}S4 zUOc9^;@m{eUDGL={35TN0+kQbN$X~)P>~L?3FD>s;=PIq9f{Xsl)b7D@8JW{!WVi=s?aqGVKrSJB zO-V&R>_|3@u=MEV1AF%!V*;mZS=ZK9u5OVbETOE$9JhOs!YRxgwRS9XMQ0TArkAi< zu1EC{6!O{djvwxWk_cF`2JgB zE{oo?Cyjy5@Et}<6+>vsYWY3T7S-EcO?8lrm&3!318GR}f~VZMy+(GQ#X9yLEXnnX z7)UaEJSIHQtj5?O(ZJQ{0W{^JrD=EqH_h`gxh^HS!~)?S)s<7ox3eeb7lS!XiKNiWDj5!S1ZVr8m*Vm(LX=PFO>N%y7l+73j-eS1>v0g}5&G zp?qu*PR0C>)@9!mP#acrxNj`*gh}21yrvqyhpQQK)U6|hk1wt3`@h^0-$GQCE z^f#SJiU zb@27$QZ^SVuNSI7qoRcwiH6H(ax|Xx!@g__4i%NN5wu0;mM`CSTZjJw96htSu%C7? z#pPQ9o4xEOJ#DT#KRu9mzu!GH0jb{vhP$nkD}v`n1`tnnNls#^_AN-c~PD;MVeGMBhLT0Ce2O2nwYOlg39xtI24v>pzQ zanl2Vr$77%weA<>>iVZQ&*K9_hfmv=tXiu#PVzNA;M@2}l&vaQsh84GX_+hrIfZC= z0Se*ilv-%zoXRHyvAQW9nOI2C$%DlFH1%zP-4r8bEfHjB3;8{WH`gOYt zg+fX)HIleuMKewYtjg+cSVRUIxAD9xCn+MT zs`DA7)Wx;B`ycL8Q&dR8+8mfhK;a^Rw9 zh9tC~qa>%5T{^8THrj^VEl5Do4j4h@nkrBG6+k8CDD~KB=57m@BL-)vXGkKIuVO9v z7t_L5rpY^0y=uu5iNw0v&Ca-zWk>v;fLJ=+SaV&V#C-o^}8 zp&Xp$v?~ccnfR=&5Df)32^d6QJLg*iuF#s|0M4zJF@Hza1p`q|f}~K)q;HC*I1_9t zQ&1jr9-kdUi8)DGxiwdqU|rPxYWDQPWY&SI&Rxkhxobp~C=Y*`d?HD4JW?WjU7dBPeuIE`ABLq95b#lfKS52IB^6KoHmm60$R}TESplQt59#mboJj+Na!P)V{ic@$yQ-&Z za^JU0T+n0Lf2VdusoNr0?g~1DMsY)zdY-63yH!Ii#aWe|;0TO>L7#YlaDrH}xvYXn zh-NYa>O>f_NTTBG=|k0qWH+X?d5@+INsQ}WcI_3z1Z4-%Gj#_{P$0A~cAye`?j0cW z8)hd(V}7rattLUSMvgZ4g96P7n` z^{55A&&29;-P992{yhkGWa3v_Z6iB4a&~NmL)IpC&dsSwe$9jS(4RVJGt=Y!b-O~1 zSCl@wlaba_cA*yt(QvulMcLUuK z>(ys_!{vqKy{%%~d#4ibQ5$yKn6|4Ky0_ngH>x-}h3pHzRt;iqs}KzajS!i!Pqs8c zCP%xI*d=F=6za_0g`{ZO^mAwRk0iwkzKB7D)SaLR0h|ovGF2w9C9g8;f#EtDN*vBP9yl;n=;B2a7#E8(%Bw()z(M$_pu zQ+9uFnlJ!5&$kk^S_+kJ>r9y8MFPpSf9;o8v;ZxsMA!p>eaAIwt5xNiQ|2_ydGkbi zkggG;Xp&I7C8R{>ten^j@MsN#V5JPs1Ezc!74->Nh0a}U){OK@j=OIoY}C7IYYd8-V9 zQ6s?v=Y7(?Y$7=P#Wwub-*0DLqli?I%kT-D^jqK?c2~HEx<2(poRWAUoC}!~6$1=I z*M(IfPmdID8i+5l@=1(+`?i`G_ew=1Y!gF?tFbdgtW2etKLOFoNozkH(i!Qa7(h^| zF`9!VeqQQwM+yO6J`;oWUWq@9l6hP~FiG8-{Pj*T`XI3~s@FfjW2Tl(llpa901$&y`F}K1uZuHEo;=mr+_8d(o z2Be#yWHEN@euC$=VUSB+3A}khJdF$)0r#<5(f3n`kx>ZT8ifaKyX*OhffeHH1?6OM z*-19$j5tMNYQoB)>cGpz@11>J%q4KW`GLNj?uB>LcNg$0G@}XN#Tqf2F5@jv<`|~p zqB^l!%v!g{R_+0GX5z0>3Q~O``%T$NFc==dsPsTj-;{b$XUS0TGoJs2BUA*H;4S?w z|Nigt|F@9hf7QLSo}JPEK#CPgYgTjrdCSChx0yJeRdbXipF(OwV)ZvghYba)5NZxS zm=L8k_7Lb?f8`=vpv(@m%gzsCs9^E$D5Jn+sf}1lep*zz&5V?~qi_@B?-$Vd1ti(rCi*I0}c}slKv@H_+g?#yarVzpYZN zIk21Bz9Z#WOF`JG&TC&C%a*3*`)GJx9I!U8+!#J4}@5rm8*jK%Xg2VLjP-a;H zFydWO;nxOZ&|{yOW;ta$ZU^6*4vFP)idD6M*M0+9buB#hK4z%YTGBdSva?Pvxim2` zF-?QVGuRQ2-1eYzd1Y%}w^`t1S7|{{8=Es#ApC0<;pc$|NJ)IU%WVK+4gnTWA7-t1 z0K{DCESXb}!y_tzrycr^%%|G4T4)`$BC8+qm|n1lS?CO=`V`1T#ykY#5g5$dc$lGt zqGHyw-*Av%C;33nEiU(rU?w^3F46!dEz#cHd3IF<(XCq)>JG?Bi)4v26MQr1A-g5RqhFoPy%^TD3sa|D^9aS>>_2-X2i#? ztVp@ZkyMB;Uo#9s!R!@G#CCaFVaxx*8YYu$kGFk4g3|9t!1nKqOaDBAe;w!(6#w)0 z?{&F2BgctT1=Z;TvjOGL_!}Vlt=kaLA7#W`mv1h%hUg983!wA*K@_r6_cd6o z6LHiCE6qwlt2H&|Ica~%b9C?Z@$dreBNR_!NKcfL)%8kGr7!IVq|^&6PKYK%EhcKu z6+uR*%EOw=rF6Q42Mx|a> z$2XrM*NV2x9ci6|X^eh1UAbJ9Ky!#*Q5w7)#o#%}d!#-^k8To=n8{UU*LmFsS-wRj zi6-p76V6g?If3S&Bj~GW&QI_WtyPY0@u3hjKtqf9`8S!wn{@P&Tc8uu8cf)YmrX7+ zrC+O3V{9}JG6ihA&^2Q7@)Kq)j(Y_oTzsoBUYQDG!}`Ame`bbcr>J-6E%gaBPEDCU zflX#1-)Ih^HJV*lew*N_SdG-4!b2}G8%U&9_V0~Qt?ZS z@H3L&5ybV8X}A@KQADl93H`}0qkNm!jGHkCJUM%r8`mP1nV?Oo%^l;yDnU6IJtbuY z`X2Sf8|r00mB_f)Q0;S{FqS1Yq?otd-BVbw`#@SDd5}n5X4lqdDi1*vtVv8-Zi10q zexCj0eyngrp`UxjEOrdzUt`?%jRlj7zSU-V-%R?y+_w7P7f1ge%t1ozmN+&)%3xQW zT3u@)))(_a<6`lTJd`DIYw>(pkb=PMKvCNEG~zza+LVNqkY^}QoGMVdS0K;gS*A3f z;6Ua!^sSV-try(M^pB6D9dsX}c>$Da#NHucp9vr(fg4pbBR*uPhYq+N>q1X4RSOCl znIQj4=A+y+8{?LQ$3L@(!Yy~~Cu4Sx72*%@dW>eP%Br7=uaynV6Mqa-49A9) z|L&5r=4K5SClwc`!2J|>(#n$4y1>lmR~2Om8q6HkcpK>d(Fk!T^NO?hM4Fc+(5J{` z&K|vrBz;;zWlNO%=a~JkMxMiZa%wYz#G901lw#+2SUaMMHrebb&|1L8tKoGJK*QhJ zU9|WkDy^-4F6U&VYSc3ScHDk@kV^0801#I|-pSK%az5=DwI}gMm)@s2O+-ESTk?QY z;y9gyucaXO(Cc+cd{B>2)euMHFT71$a6DssWU>>oLw4E-7>FC-YgZH1QAbRwmdahD zO4KAeuA^0q&yWS|zLTx%(P4VOqZv-^BO`0OFAXdBNt9>LAXmPALi3b|gt{b?e-$z0 z4n7H$eg6y_zs(c>*4FT!kN*$H`43~1p!g;IZ8-mYbUPTejaLW#BZnAPFES?ApM{TQ zE*TC%O8)apqcX|PrNjIZE-z{q`I(LwIE0kf=PLjExEX>)oIu><<@lt>-Ng9i$Lrk( znGXl|i4dP;Mt^-IbEp7K0e#*c7By@gCo@VQIW$93ujLL`)lMbA9R?C_5u~7^KopaAMj#6&>n-SOWlup_@{4 zcJ?w_!9JKPM=&Bd#IQ37F*x39y!azm$;~IRlkm>bHdABcNwW-TdDKD$pkD{j6A8d* z{vP~|<}bj_Oz#83K$ieRtsA4a@4a5cRjJ}A01{PgxXn3;fx)5ElMEPwDX_mW9)9oB z*;scve~v#HHqUj3KdC$tdV3&0)Whkp-=hKKz{SzD7g0@N!wyv;ZAime7AjB7&)!)5 zp_iVblaf)%agwJqOG2e7WTCM1&khq`{b>fN4n8hOJbvO?Y;60>LIwagLXWC@@0RSR zo%lPo1cUU=g$ahJ8D=;`v~ORUSl(1-&a@yTAC5Y8E892@{P@MM=GXUGpBSXSbSs!N z;L~0D_s7{+^F6c!WW+^yz5~o7eWtsOE}8{hKaFlHgnyBeUJ8Zz2$k7Lrh?NuMU|No zVvsq@57)8zin;&ckR1;*Z%(xH2lBw z`x%N;|H1En8au588bPDxP^$kfpO!bIzz>K=5Jiq9Rg(NGde0g!rKagLa+&yC)jg7y zq}~2IH)N*FJC31qrIH-2;%3^F?=bDD^U2Y;%ftN(v71oY;od+vh!!2z^}GHR$43rg z0In@ki}TglIsMU^O1(SiLK#oiuyw zB>-@z?&uW`ILoPupw0_cs?C|2YoX&87~us+ny%eo{A!3M<-7O7mHUBCgA~{yR!Dc^ zb= z8}s4Ly!GdxEQj7HHr<}iu@%Lu+-bV>EZ6MnB~{v7U59;q<9$h}&0WT;SKRpf2IId ztAjig0@{@!ab z{yVt$e@uJ{3R~8*vfrL03KVF2pS5`oR75rm?1c`@a8e{G$zfx^mA*~d>1x`8#dRm) zFESmEnSSsupfB>h7MipTeE!t>BayDVjH~pu&(FI%bRUpZ*H615?2(_6vNmYwbc^KX4HqSi!&mY9$w zpf%C6vy@O30&3N5#0s_!jDk|6qjb-7wE3YT3DA7q3D`Q&Y*y>XbgE7=g#rPx1hnf8 zTWd{IC!Iysq*vZup5VGrO)UM<3)6raR`rOwk(!ikf3XPp!n|gz0hS*P=VDXAyMW(s zL??-`&IusEuOMrz>m(A1W5Q~>9xJwCExAcMkOBD` zD5BJSadd{0u}%z4r!9qA`FW4;Ka_Qk>FcHxiucGw4L9qhtoge|ag8jbr`7LHSbVQz z6|xUo*^LV1SLxS>?D`m=g{8IC&1YF$e}VRGD#ZOc_15QW%J@FbEj8tE-nGxo4?X02 z@|q#k*G4xMW>q84Xc09pRj@>Hz8t^fMm3n&G;Al6KU*;=W`7Q{$^|=bnZiJ7?(s)@ zB`vW>#zJ{}!8=*|?p(~fcXSanO^j8+q7V!q16*ic!HLRdz0TzNI6}m+=OKd2b8KX< zAcDTj*%~vQlcO+%@H01gjv-1zZaOXVoM*t-+KXTR#NoTf-#{dQAm?GqK6q8Ta zu3xW?t=NE$EfYa#=0HofLn5~c#m-U#Ct_r6~X-pg6k*F zYIP7De52BBwcAnK?O(j?YEs1;q60!-!hTuKzw3T;XcA_w5HvU;tO~}byLA^cggu8i z-IP@pxFjTy&ie28m}j66dm@g78xK7aG{QSR^bAcY+W*xWu;G~I08sf(GK4>K-cbfJ z-%v9DGR77He<291M~=fg>>9&NFQlboP)pC6fT;{>_!lM`A&&HWIMd)Y6e@IL;nvRdBE*Tn({&3{-XJ9helJa{G51Ck}-_Y=5C|fEo z)7fZlsHxN&SY&ZLTdYuBBZnwIh0#VTzmyK>U0|r&SXb&GP0m)1dGV8z(^x6s5yQ-z zEyniK${#U@Y7p@Yxx}E+jA?1@{=|e6UM;iyai=0=aItVvqieogZUq@sio2#9NLW~L z{w@^H!HEGU;>;T0lu{Ad20Hr6u;?-9YHKvkjEc)}wsb4Y-ArRK8`24uBT8N)8m%Ee zYJX21)|e{peL26}VUUKYQ3L@NSe8rEbN#AIo$tjJm-$B|IJU?mu(h$Sq`XNY0@NhY z0?WeMtPwP)sUdk}dWA4qBUV^x>P|is-kPgVe)*WV>dKDL>gOq1 zUYw(nU|N#dw>97A_(c3?VA_zDfF{^A1eE#8Bucd^ON(sv-{tc@&i)Y)3V~o7U~+AA zOwnXB5`WN^z$z<9^@(?LY%7?y5X_C(j1ip-Ug^f7Tt6suI3&a=&~#EJegG4r2^tKz zJoEXCVOc1QdOSNHp2d;t&smxL%CfK@mSl)Ky}`!6kCsi#7s5&G2Q!sM9S6o)&mdx% zz|2M~pav2;Th=DTN5yB@6HFAO!pl-y+tEJsh}(? z!tIyg01O*w@mWxsFhHMi7%Gqz!v(Osc5WxK+^1PGfsozw)FE}VIxk9GexmAohPNAF*SAjxG3Al#(xQoYXdI}TR zoCHAFS6+LDqsP8L1SZH{RxJjFK_=vy4nNH^?M!OsQWe^qC~$c1r&y`H9n5;D z2F$t-Htc%2@K(>opJHE{NytI2<_J<6Kz*p$wtKUTEH}zITx?H0L%!5%i@!rLphSBrkFs>jscP6?HVQovX8!~b~ZY|0h%&souT7e5nD@OxuSgC zVW*eo0B|1POwg7;6fJSUC`g+`1%XQvwpRc*&|AtV*h!#5nQM(@m!K)-Qop!Rt3F`a z9HUO zF3w{uI_==EpjFQWV4boF^A?wc@@@U+KrKPjn6sK{OLu-~1UloSqt-aHYo*^@kQy2+ zH(9*-mFz?YV4cL7EW)9hsdmG{5jaYXLvm*&3PZ4y?8z`$9z6`q9fgsJm@*W$-QSzu zut}57hroSbTd=&RJpuy#?K?A6!-;_MowpK8eb~5T-^eye%3O-T^ktSMbd%PT0j-B?#yAKr37u%gB z*2)WJMw6Y)6BvY$JjD`(06ci7u;u$hv}gN5oS&Q^*y$J6L)0#BD<>XL|;pZgtZaxp3~$0zxA(;6Qr_AP$?8l@S)C^Hoaz#rQFK^lA}3&)Gr}Fsca? zK>9BkVcl;c*E2P9UMppEIB&38dL9R?Xg9N{Nl~4*w!qsZJElz}Xc9gz#}cwnP4u{+ z6VNTEx*>u67?3bn{sWk*P`1_$YfsB+)Ax0+jt|)0p&VS?N0k8IAp2KH_#eY3I#{Hw zB$vObUDtXyZX)*wVh*@BefnUej#jv@%uiA=>ngX0kQXaz>8(WM)fX~v__@I}7|!Il z@J%r#I!JqqFwGd4JPhmDmL>1Bh}nn_BE;hgKUesNOf9zQhiuhn%4B}O8jnxEwJiQFDaiiuXw2sb?*8a}Lr;_#7+IPfIjhVDhazSpbQZECL+4)p8lO;)!y>Rt=0X*;O# zX{s(p-*d{#{Y3gVhL;A{4a(Z5sIfpk;WMCqdFA&Mb7mp;YMXhBF@p`}$ShAug+bo`;<9fm!~F z-;1yCj$GQ^mzucrfuatilXrYLr)`izjn_m(f~);txN?D7d?Kg4wDuPXilVyeVwjzf z=4Kewf=u}X_H*viVfPWZW?Sqa3G#h3|;b!Q7>BRc7-Wox0}&>}Lqo=0v;T_i~% zqB&h;14|~nK{W0N=$obGP@O%(c8SraYS^qiu%Q`B zBHdA!`Vk7#Bz*@_3eE#bizLzjBV;F0vfSA~+7@8+F{$7Y?fwI~Pp_X`2ORgqW6g@2 z{cQV!niSsMEVr1IaeRAj8~|*4yW~X5$6o`crw4uTHhgPs^qAk?9UPu;xy5wh2^jZ; z)@27Q=QKa?8w7_C0|u`@k=%b9Ce$D7x42CdLsckF2<$wLuV2kpik8PXex2^Co$n2o z)l#H*;#>?yrPw0x6LI@x(X$nezCBa0Obi%|I5ZV|4bJSPtNHjDkS|3S?fiv(i_(n* zFbve0g!B0!MMmakRsgg_if8nwImb=kk%|s+08xGQ)J?vpkdaya3UD|RJK+LQ72|g> zc4LnwInx!2pN-5Yvp7rvRF#B=(ZO8gyVB^0Dh#ZdHA2BjjppfV<=2Nm#w_t{%6O$W z`-?7N?LwL0DWgK0Y7L#ChSHfa{=DOpJpl8L@V70cd%ei)n%SQO;Z+Xw#li#%LUfbs z&hP%UzN(qM3cw#bWQS6_B@>1^ea-AqNA12xoiQeb_Zdtf>yHljqeIHqlyC^gzH)h1 zstXTFEb0r=l9;><<$a}YWlscH7VW_xeKVZ#*#v#HiuUOs7PPj8ml4#!BiGEK)kDpO zX=2mU0ZuIDDnhfV7v_Rs)0R#ff6I6_|MrzV(R$3Nt#S7D?GQy6?a^WRvA@r2~?7f~s99*9;fuqJ(843U`hRl2O|sk>J@WMsR2O zwyZt$@J)DnSUNkF@B3MPNz|<@`72{M*S5d<1Vkg+G=q~u{8OP84Yh6VCE5pNC*#m> z*jzHy5Tc82sBVw+6W7DoR5@LXZ|+>;)Q%czg%8pyMyeE2-)R^oHg~SrO~#I8MxNc> z6pWT&F&H1mX7#2@mBY>#rRoFKszT z(gvV#j3x|7sF|Dt0*CgsJTdH1R!>inYZWp*2RDbjjQCP98L_ds!$x&{t85NRYk4ii ztJ3HyC8h2A2&`kq^Cfci>N*r&btHg_|v6=s|v=(-MQ zK4kjqoI^~y`j9poC2r{Izdlehm8!AcMP^+SwDUce1Zon(%YvxK)x|rXsJRlO?-K91 zMsmHgI&PmqT_W}C0mdA_6L!EEjgJzidRvTN;vQRJ-uBl#{dEeN?24PRwx)7c5kF^ut=M0)e@zr?z_vpYf=%;;@UYF9>9-->Qf2FW*# z5*#VFB$$-k(zphh4sAElMiLbp`$+SKm*{l6qX;Q8GZ7b|J>OhC!yg$}8dt$dx3E8b z$FlaM*K@6mSsYCoe#*QjLEB3|_Vs4GbZI#!>Ya}dzh%uMn}sw0gFQQ{+V+e|_`q)M3nK27)nAqQ-viJoPHUKdr9HN`v0 z+tZo0ORLuv_d)x}gO|~s(H!12RM(aMfqLG>KSH#kGxC{sUUj>FUC(6;ds1cOjeDYu zOrd>q@bNFq5?0s&@5nbF3-rw{{V&YYf3o_9|K-X4k861UwZ&C2bH+A7^%7nizU>b? zC2@*VlrqprJiv$rx{+^+Op9i3RM;IHq@a;34=Gn%B+rXMZi=UsHC@TEFk4{*fs96p z)wNUY?AhVkdLGQmPESuh@-!iqSZrnxIT~Mon)J+i+B~9VdL8QE`^4=2@lNaKluUVx z_^i7~5E4dN4&gVMi%;7ast@WIY21Q`+^iTC*Gx@IMVYB`BLFHzPh{Fpc6LKZTk@>P zquo2E*Pgq(0MX>h>4)YaJYbIK&V?-W}JfL@&R0I2)TOA!Teg zNa4DBO&)`Nn0$Inb|d8ea|)qqOLYVbQIBRC4T4E<5#Nzc2 z57|Bq7mYsW8y?uLA$XMj%OeK+1|DAKcLYB98-vDP<3*+SKYcPcOkm&}H|!{9l*9%L zbiYJYJ^)Cql-&wPwABGD>Ai7SUXe15m zIr^wNEU$9)D6@atm z(w(1~GuLpHi?JGgIBj`Ovy;j4M`XjrCNs?JsGh1zKsZ{8 z@%G?i>LaU7#uSQLpypocm*onI)$8zFgVWc7_8PVuuw>u`j-<@R$Of}T`glJ!@v*N^ zc(T~+N+M!ZczPSXN&?Ww(<@B=+*jZ+KmcpB8* zDY_1bZ3fwTw|urH{LLWB;DCGzz$jD|VX#Af@HC%BktA8F7VJSy&!5iTt};#U^e0_q zh6j7KCTInKqriZ1`BiF3iq2LWk;gyt0ORIFc4Mi3Bx`7WEuFq{u^C49-SYVjnv!_40m1>7x*+<8~Xkq?056 z!RBfE@osP%SxzOw>cLAQ$bioAOC0V!OzIXIc};)8HjfPtc~8tnah$PtoAz`4k)7$FDUc2O@D)g_uAo&nXMymK$##V?gYUPt^l zj{6NFDL(l-Rh(xkAHP%bBa=($r%3Y~jB!eQ1Smuq2iuQ|>n%Y=p(26SE5gFu11*Q< zaPN5G^d;Iovf`VY&Gh58z~%JpGzaeUz6QoBL^J%+U4|30w7Q&g9i}}@l61eKEfCgo zST6qMxF_Eaj7;0OC)TSU{4_m}%FOa6B{AxS$QIcmmG~IVjjf;7Uk!HBtHfm{%LsLb zu8~5VQFyOZk&!VY(wxL__haJ;>Bj?g&n`+i&=X{unJmv&0whCitWfGlOr6+Tc-lMZ z(ZRXqC-=O+GAvTXKViA9vdwu{aifhk$tYh~-9BScg!Yr*M2zw&9`pHMxHGh`dUH-1;~^6lF@ep;X9PjQ!rqmXNWJ?#P-qb%*TB%xe&3 zX*5V>xuW7)$3!Yc$y>cwBqd8+p+u>WS7p7~O80ipG{(a*#=NJ`^Ld6k-`|;Y&htFy zIi2(Sm)4eD=o+CGo~M3%qF|O9P0+ahmc%EklI?NgX05W3+OdS`_Rd#wg-}hd1&txU5wXy zy`x)05?WVZvELw`XWetIAg6$|(^4ntaE;=f$Wcpwbxm7?bLDnPs-1!bRoMcy!EeOh zpIv8ewDzcIU}mv1NxV!&(Wf7~_kqGAk=2=j&O5FA)z2!APCcDQPnIaiqMkVT4fUyX z))R|WvOJyzcU6d=z0q8JDt42*`js4g+_t{YP7lVguX+vhEejJ3TAIo*Z6jizHm#S- zZT_}-STQAa-0Gn8+RmR7V}{Ns1@jJ{^Sb!9&RSXXP;^ep)r6;&PW++~XYXC9a=zSF z?sp(JQo&MROb~b1Y*Xw4!P)>PHT>Z<)*U=Ax_75^OUw97pNudbxS1XPtNrIg zQ5YB77E@i7$2Ia}(^JcCi@OX`9a|m}PY%-th2m~y+)eCl>fTVjCP^lDOBLyhg1DZ+ z)~G{&OkDc$!;t~`gq(wz@qW3lh9B^ic$>-h#nV!H8d#l+>C(M%g}u2g=I#&W|L!VD zqHYoQkBW;`r|fW02u{7X!X;}T7X4iAaWzkeOh}7&o!F1qt4#$1|BDF;(2VlgEqJ$F zy8Ba-y(%fs`MzpvyXlQLEhS^ed$7Va2hO%?$-D>^*f$b)2Hx;}Ao$UqFt7l26<7eP z!{!C7PVrq>=794Zqmc z%LKkzIBZq@%Ja8EkH}?>c5ILG(EAMS*JHu?#9_7TsELw)8LZzN>f2Y6YN{AJC?34> zh42sPa1%2JpCeS9&E1URm+Pb}B>A1M`R{+O+2~}c(@^1Rf&J9p(4QqHl;E^4w5;I5 zM{?(A^eg*6DY_kI*-9!?If^HaNBfuh*u==X1_a?8$EQ3z!&;v2iJ``O7mZh%G)(O8 ze<4wX?N94(Ozf9`j+=TZpCbH>KVjWyLUe*SCiYO=rFZ4}S~Tq|ln75Jz7$AcKl$=hub=-0RM1s(0WMmE`(OPtAj>7_2I5&76hu2KPIA0y;9{+8yKa;9-m??hIE5t`5DrZ8DzRsQ+{p1jk-VFL9U z2NK_oIeqvyze>1K%b|V?-t;Wv`nY~?-t;tMC4ozyk8CR(hoZTno3!*8ZTc15`?MFf zDI892&g&3lshOEv4E@w-*_%)8C_<&HhV`0D5lN$WT4Q^UWHNSAE+RZe(o z%bqR^hp1IsDr47e^AajFtlppT)2F6yPcrWO9{Kw{o=P6y^HOW$Wqd_)_fwzn`ikZl zOGVc0+S(*=xZ_KbL0Nr`Sx$$CWEbw$52udl1f=X6CZEcFMA*nl>`0gn4&tc5^`!!)tGw<}^Q>P7E}$ zialDUofH*XcB3r9@tA@lnS}dA(@nK_xuw0b;FPUnNGD0;MIySCw=cSzB#=3>F37V-nni3UNB)-;;Gkk;3l9fh6FIjSZU zk=Eo2a`6i7@i*4>ym5`R?i-uZFv6+iX*Gi^I}ZU1OrLAX8aGiT@`*YnjeF>}$U}ORP`+EY5`eqVC_&4yG z;Tp>+2QbZ?lt1GB+D}q14W3dWP8lWnN zf(nlT6+XW&(zme{FbyDpP^NakA<~TK=Y}H^eS%2rt0v8Lr)B}@B!cTvC=9FM;7q4@ zf*;vb4HG>RFpY5?vFCp27VEnVIGx~-na6biU4{+UoYe=}^R#_My6wT$5d&r*=kpAA zu;=-c0|~yqi(N8&*H;aNfhyey+HHQ7J_qae*_CgG2V8j=Tq936S0DC8r3BXBql3Gz z0pLo_`|4Q+oY3rPBNaLmL{QM};9dke>ujP^j@z-N;fNlKb|edn>)YaafDaJ>GWKP$ z5}l&#$QFhN!CMT;WH&z-5E)kvM|36lV!^#3z{@2FF>HsgUO4PMqO#U$X%+U>K!xJ@ zBFs|+woG_9HZQs_Tw*vnCPGhlXG@>y|6pJT$I67!aP&b0o$AF2JwFy9OoapQAk>k7 z**+$_5L;5fKof<;NBX%_;vP@eyD=Z0(QW)5AF7 zp|=tk3p?5)*e~Inuydz-U?%Kuj4%zToS5I|lolPT!B)ZuRVkVa>f*-2aPeV3R79xh zB)3A$>X~szg#}>uNkpLPG#3IKyeMHM*pUuV5=-Jji7S6PSQ9oCLo{oXxzOZfF$PP) zrYwlmSQ-~n94uO3CD{K0QTmj@g%Yzn7_xQ4fTduU0Yqvln`e_`CdXH5iQ5qRr1 zBC;}%YZ2!4I>*=sR)O~jBPx6sxmIEBnq)s-fHz_y0z8-gPl2Us4BiBXNR5CIF!YR@ zb9B305SilU*@4|+ x6JBtc8JSt5M0pkooaq!^FqtuD_KdXXTo>Mw54>`rP&>h&58!3a6l6r9{sG7g--!SK literal 0 HcmV?d00001 diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 0000000..15de902 --- /dev/null +++ b/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,5 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=wrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-8.2-bin.zip +zipStoreBase=GRADLE_USER_HOME +zipStorePath=wrapper/dists diff --git a/gradlew b/gradlew new file mode 100644 index 0000000..91a7e26 --- /dev/null +++ b/gradlew @@ -0,0 +1,164 @@ +#!/usr/bin/env bash + +############################################################################## +## +## Gradle start up script for UN*X +## +############################################################################## + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS="" + +APP_NAME="Gradle" +APP_BASE_NAME=`basename "$0"` + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD="maximum" + +warn ( ) { + echo "$*" +} + +die ( ) { + echo + echo "$*" + echo + exit 1 +} + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +case "`uname`" in + CYGWIN* ) + cygwin=true + ;; + Darwin* ) + darwin=true + ;; + MINGW* ) + msys=true + ;; +esac + +# For Cygwin, ensure paths are in UNIX format before anything is touched. +if $cygwin ; then + [ -n "$JAVA_HOME" ] && JAVA_HOME=`cygpath --unix "$JAVA_HOME"` +fi + +# Attempt to set APP_HOME +# Resolve links: $0 may be a link +PRG="$0" +# Need this for relative symlinks. +while [ -h "$PRG" ] ; do + ls=`ls -ld "$PRG"` + link=`expr "$ls" : '.*-> \(.*\)$'` + if expr "$link" : '/.*' > /dev/null; then + PRG="$link" + else + PRG=`dirname "$PRG"`"/$link" + fi +done +SAVED="`pwd`" +cd "`dirname \"$PRG\"`/" >&- +APP_HOME="`pwd -P`" +cd "$SAVED" >&- + +CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + +# Determine the Java command to use to start the JVM. +if [ -n "$JAVA_HOME" ] ; then + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD="$JAVA_HOME/jre/sh/java" + else + JAVACMD="$JAVA_HOME/bin/java" + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD="java" + which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." +fi + +# Increase the maximum file descriptors if we can. +if [ "$cygwin" = "false" -a "$darwin" = "false" ] ; then + MAX_FD_LIMIT=`ulimit -H -n` + if [ $? -eq 0 ] ; then + if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then + MAX_FD="$MAX_FD_LIMIT" + fi + ulimit -n $MAX_FD + if [ $? -ne 0 ] ; then + warn "Could not set maximum file descriptor limit: $MAX_FD" + fi + else + warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT" + fi +fi + +# For Darwin, add options to specify how the application appears in the dock +if $darwin; then + GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\"" +fi + +# For Cygwin, switch paths to Windows format before running java +if $cygwin ; then + APP_HOME=`cygpath --path --mixed "$APP_HOME"` + CLASSPATH=`cygpath --path --mixed "$CLASSPATH"` + + # We build the pattern for arguments to be converted via cygpath + ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null` + SEP="" + for dir in $ROOTDIRSRAW ; do + ROOTDIRS="$ROOTDIRS$SEP$dir" + SEP="|" + done + OURCYGPATTERN="(^($ROOTDIRS))" + # Add a user-defined pattern to the cygpath arguments + if [ "$GRADLE_CYGPATTERN" != "" ] ; then + OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)" + fi + # Now convert the arguments - kludge to limit ourselves to /bin/sh + i=0 + for arg in "$@" ; do + CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -` + CHECK2=`echo "$arg"|egrep -c "^-"` ### Determine if an option + + if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then ### Added a condition + eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"` + else + eval `echo args$i`="\"$arg\"" + fi + i=$((i+1)) + done + case $i in + (0) set -- ;; + (1) set -- "$args0" ;; + (2) set -- "$args0" "$args1" ;; + (3) set -- "$args0" "$args1" "$args2" ;; + (4) set -- "$args0" "$args1" "$args2" "$args3" ;; + (5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;; + (6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;; + (7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;; + (8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;; + (9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;; + esac +fi + +# Split up the JVM_OPTS And GRADLE_OPTS values into an array, following the shell quoting and substitution rules +function splitJvmOpts() { + JVM_OPTS=("$@") +} +eval splitJvmOpts $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS +JVM_OPTS[${#JVM_OPTS[*]}]="-Dorg.gradle.appname=$APP_BASE_NAME" + +exec "$JAVACMD" "${JVM_OPTS[@]}" -classpath "$CLASSPATH" org.gradle.wrapper.GradleWrapperMain "$@" diff --git a/gradlew.bat b/gradlew.bat new file mode 100644 index 0000000..8a0b282 --- /dev/null +++ b/gradlew.bat @@ -0,0 +1,90 @@ +@if "%DEBUG%" == "" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS= + +set DIRNAME=%~dp0 +if "%DIRNAME%" == "" set DIRNAME=. +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if "%ERRORLEVEL%" == "0" goto init + +echo. +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto init + +echo. +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:init +@rem Get command-line arguments, handling Windowz variants + +if not "%OS%" == "Windows_NT" goto win9xME_args +if "%@eval[2+2]" == "4" goto 4NT_args + +:win9xME_args +@rem Slurp the command line arguments. +set CMD_LINE_ARGS= +set _SKIP=2 + +:win9xME_args_slurp +if "x%~1" == "x" goto execute + +set CMD_LINE_ARGS=%* +goto execute + +:4NT_args +@rem Get arguments from the 4NT Shell from JP Software +set CMD_LINE_ARGS=%$ + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% + +:end +@rem End local scope for the variables with windows NT shell +if "%ERRORLEVEL%"=="0" goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 +exit /b 1 + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/libs/README.txt b/libs/README.txt new file mode 100644 index 0000000..3d34852 --- /dev/null +++ b/libs/README.txt @@ -0,0 +1 @@ +Location of external libs diff --git a/libs/ftc.debug.keystore b/libs/ftc.debug.keystore new file mode 100644 index 0000000000000000000000000000000000000000..a7204e66d948a1bcff16a75fe0a85f9577ae675e GIT binary patch literal 2146 zcmbW1X*kpi8^-5v24fj(#&%>WSsLa)3864$DI&&@?I1f@l3iJ@X(CIZK_eW?*urUK z31=)JY6u~UOd$+eLX@%f&U@bT;e38SJRk1+!*e~)b3fPbaP@E%0)cR_3;4SPuLXJD z4EFQ%^l}gPx$YIs_HpW`O6U*>Cjb&)2cQ5K?@2BI2B>lf0{|xkMu08l#)lw-&_qoa zYJ-jsL!5EZztm^+U7_(aIejHx!DiFjxU5UakXBUkm-T+E5(~g`i+BJb72ER%9Em{HaEQ> ze>hLO!;N9ICSdF!@4zA?nl4j$SZk3ISx17_Q-qN53Xw6@#PJA^&m~Tq;?T~);DHpT zFKzDFS}i^*@GWkdl)O+D`(8*R<9D?gQom_pnhK!_E&1ppZ%V0F-D2_q7uTyUk~Wi` z#8oe`X>;WxkWe<7S=q3sH2f_Zr`DkED;??9?y6D zg^D9a6Zsk6B=7ig=Lp|8RpxSzsohzHIm55INIrZG z0~}nUx<+=Y`N9poN^z-;bzj6^ytwmYsD7uvdEM$QH*Wt}$=t>Xhtnh>kYKV%)j0N5 zn-R^)7`TdDs~{PD675#GU$)}+NwIJyLI*nM5w$SxrE2yR%go;mj?>+q28A;8?U8p0 zC|!xk-IXEBH-nT*%t*ZXmAbGag36f-O=%s*edvNwYZvhi=poesOX#8=0~Kza_AaXSPWf?Ieg;kGO`(Y5a2FjV(2~ z)$m)Otv4#{*2Y%*$W+CSM>>DEgvOxwGg7D)dUWQcy=CZ9lf~f>^`$aNon~tU_uW77tnNj+V&lnV zOR{60cQ45UrVJmS*ghJtZ}$KyHS`YGLW(X3?HjRz8s0(m+e}3h@a|%_xS6!-{C(~poK#X1&8I=6S?)C7 zp%2lmX8Y6k4Q`198}js9sd};6$Haly`0=3%+$Y;Oa3-Do7M=;zqzqdr54)1N98wHpDCM2i5 zx+bUtfk1^p0u%uffbAkE2f)Drvw58g2N7I6s)-j9eYj}roFWB5QGE~qVxOO(Ub z1|!7ACtL7}a%2C8Squ_9@?QrkiulKoGxB1S7d$GCz-WOO3U8M5z-izl)h%HOii*2Jii7vcK+VFZeW(oQ$)-dM5lxB+F8Rg;%}Z!yuomfXaV zo6mTkYmtn)Z4t1l2~o^@MQx{SPxkpUi1~)$_swsOz)2AD*K+Rz^ZmqCBX>sexv->c z2iZ7LUuk~Ag`<6FiGxYrIMiuEPb|Af{rZcyA#bPR&tIPQZH)`=(2+SfJSJo2a>?T@p`}+^n zDHY#vn(rM<%aug~tT_Ehr3wcWfB+PEP!^P72O}c@L&A=H54pJaPN&F9?H*6~WOYD= z^1CIKe;<*ZH2a7EB&s1TJk73f^r*`0b*)ytmzrev*v?a~r?>Nm7nyQ$S{^u1sr`{Tb94ojRBvAcpr;16;$-DiEi2 z-M1pWJ_JwFaic}UVbSWf8Ax|bn^Amp;bGDZzBJ) Date: Sun, 24 Mar 2024 23:37:21 -0400 Subject: [PATCH 02/94] first commit --- .../org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md index a8e3f11..5834cda 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md @@ -2,8 +2,9 @@ Obviously, you have to have a robot to use Pedro Pathing. Also, Pedro Pathing is only able to work with omnidirectional drives, like mecanum drive. There is currently no support for swerve drives. You must also have a localizer of some sort. Pedro Pathing currently has the three-wheel localizer -from Road Runner, but we have plans to create our own localizer soon. Additionally, using -[FTC Dashboard](http://192.168.43.1:8080/dash) will help a lot in tuning. +from Road Runner, but we have plans to create our own localizer soon. Additionally, using +[FTC Dashboard](http://192.168.43.1:8080/dash) will help a lot in tuning, and we have a slightly +scuffed Desmos path visualizer [here](https://www.desmos.com/calculator/3so1zx0hcd). ## Tuning * To start with, we need the mass of the robot in kg. This is used for the centripetal force From 61615feacf2eb9c8ef08f8a20413ead4ed090d6d Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Mon, 25 Mar 2024 20:36:00 -0400 Subject: [PATCH 03/94] minor change to tuning readme --- .../firstinspires/ftc/teamcode/pedroPathing/TUNING.md | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md index 5834cda..cfc1323 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md @@ -2,10 +2,11 @@ Obviously, you have to have a robot to use Pedro Pathing. Also, Pedro Pathing is only able to work with omnidirectional drives, like mecanum drive. There is currently no support for swerve drives. You must also have a localizer of some sort. Pedro Pathing currently has the three-wheel localizer -from Road Runner, but we have plans to create our own localizer soon. Additionally, using -[FTC Dashboard](http://192.168.43.1:8080/dash) will help a lot in tuning, and we have a slightly -scuffed Desmos path visualizer [here](https://www.desmos.com/calculator/3so1zx0hcd). - +from Road Runner, but we have plans to create our own localizer soon. You will need to have your +localizer tuned before starting tuning Pedro Pathing, since Pedro Pathing needs to know where your +robot is at all times. Additionally, using [FTC Dashboard](http://192.168.43.1:8080/dash) will help +a lot in tuning, and we have a slightly scuffed Desmos path visualizer [here](https://www.desmos.com/calculator/3so1zx0hcd). +One last thing to note is that Pedro Pathing operates in inches and radians. ## Tuning * To start with, we need the mass of the robot in kg. This is used for the centripetal force correction, and the mass should be put on line `114` in the `FollowerConstants` class under the From 3af80e243bf1cbf8979bb3104dd46ca7274aafa3 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Fri, 29 Mar 2024 16:58:38 -0400 Subject: [PATCH 04/94] centripetal force correction fix --- .../ftc/teamcode/pedroPathing/follower/Follower.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index cfdd2e7..ca2a607 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -767,7 +767,7 @@ public class Follower { curvature = (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)) / (yDoublePrime); } if (Double.isNaN(curvature)) return new Vector(); - centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -1, 1), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta())); + centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -1, 1), currentPath.getClosestPointTangentVector().getTheta() + -Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta())); return centripetalVector; } From 2ba5639bbc9caca3b4076e5b27372e1d29b00182 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Sat, 30 Mar 2024 22:47:57 -0400 Subject: [PATCH 05/94] centripetal force correction fix --- .../ftc/teamcode/pedroPathing/TUNING.md | 1 + .../pedroPathing/follower/Follower.java | 25 ++++++++++--------- .../tuning/CurvedBackAndForth.java | 4 +-- .../tuning/FollowerConstants.java | 2 ++ 4 files changed, 18 insertions(+), 14 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md index cfc1323..7aa7279 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md @@ -7,6 +7,7 @@ localizer tuned before starting tuning Pedro Pathing, since Pedro Pathing needs robot is at all times. Additionally, using [FTC Dashboard](http://192.168.43.1:8080/dash) will help a lot in tuning, and we have a slightly scuffed Desmos path visualizer [here](https://www.desmos.com/calculator/3so1zx0hcd). One last thing to note is that Pedro Pathing operates in inches and radians. + ## Tuning * To start with, we need the mass of the robot in kg. This is used for the centripetal force correction, and the mass should be put on line `114` in the `FollowerConstants` class under the diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index ca2a607..3cadba6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -757,18 +757,19 @@ public class Follower { * @return returns the centripetal force correction vector. */ public Vector getCentripetalForceCorrection() { - if (!useCentripetal) return new Vector(); - double curvature; - if (auto) { - curvature = currentPath.getClosestPointCurvature(); - } else { - double yPrime = averageVelocity.getYComponent() / averageVelocity.getXComponent(); - double yDoublePrime = averageAcceleration.getYComponent() / averageVelocity.getXComponent(); - curvature = (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)) / (yDoublePrime); - } - if (Double.isNaN(curvature)) return new Vector(); - centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -1, 1), currentPath.getClosestPointTangentVector().getTheta() + -Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta())); - return centripetalVector; + return new Vector(); +// if (!useCentripetal) return new Vector(); +// double curvature; +// if (auto) { +// curvature = currentPath.getClosestPointCurvature(); +// } else { +// double yPrime = averageVelocity.getYComponent() / averageVelocity.getXComponent(); +// double yDoublePrime = averageAcceleration.getYComponent() / averageVelocity.getXComponent(); +// curvature = (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)) / (yDoublePrime); +// } +// if (Double.isNaN(curvature)) return new Vector(); +// centripetalVector = new Vector(MathFunctions.clamp(Math.abs(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature), -1, 1), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta())); +// return centripetalVector; } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/CurvedBackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/CurvedBackAndForth.java index a93dc28..6ff548f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/CurvedBackAndForth.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/CurvedBackAndForth.java @@ -47,8 +47,8 @@ public class CurvedBackAndForth extends OpMode { public void init() { follower = new Follower(hardwareMap); - forwards = new Path(new BezierCurve(new Point(0,0, Point.CARTESIAN), new Point(DISTANCE,0, Point.CARTESIAN), new Point(DISTANCE,DISTANCE, Point.CARTESIAN))); - backwards = new Path(new BezierCurve(new Point(DISTANCE,DISTANCE, Point.CARTESIAN), new Point(DISTANCE,0, Point.CARTESIAN), new Point(0,0, Point.CARTESIAN))); + forwards = new Path(new BezierCurve(new Point(0,0, Point.CARTESIAN), new Point(Math.abs(DISTANCE),0, Point.CARTESIAN), new Point(Math.abs(DISTANCE),DISTANCE, Point.CARTESIAN))); + backwards = new Path(new BezierCurve(new Point(Math.abs(DISTANCE),DISTANCE, Point.CARTESIAN), new Point(Math.abs(DISTANCE),0, Point.CARTESIAN), new Point(0,0, Point.CARTESIAN))); backwards.setReversed(true); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java index be954e2..bf0c302 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java @@ -114,6 +114,8 @@ public class FollowerConstants { public static double mass = 10.65942; // Centripetal force to power scaling + // todo: there are currently issues with the centripetal force correction, so just don't use it for now + // i will fix these in another commit soon public static double centripetalScaling = 0.001; // Acceleration of the drivetrain when power is cut in inches/second^2 (should be negative) From 28c3ee97fe0b549c7e57f7ae76ad22ebd221f163 Mon Sep 17 00:00:00 2001 From: Nv7-Github Date: Sun, 28 Apr 2024 17:24:54 -0700 Subject: [PATCH 06/94] Make it successfully build --- TeamCode/build.gradle | 4 ++-- build.gradle | 2 +- gradle/wrapper/gradle-wrapper.properties | 3 ++- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index cbc76b2..442471b 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -28,6 +28,6 @@ dependencies { annotationProcessor files('lib/OpModeAnnotationProcessor.jar') implementation 'org.apache.commons:commons-math3:3.6.1' - implementation 'com.fasterxml/jackson.core:jacson-databind:2.12.7' - implementation 'com.acmerobotics.com.roadrunner:core:0.5.6' + implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7' + implementation 'com.acmerobotics.roadrunner:core:0.5.6' } diff --git a/build.gradle b/build.gradle index b7a9997..8969a41 100644 --- a/build.gradle +++ b/build.gradle @@ -11,7 +11,7 @@ buildscript { } dependencies { // Note for FTC Teams: Do not modify this yourself. - classpath 'com.android.tools.build:gradle:8.2.0' + classpath 'com.android.tools.build:gradle:7.2.0' } } diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 15de902..9d86991 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,6 @@ +#Sun Apr 28 17:12:03 PDT 2024 distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.2-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-7.4.2-bin.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists From 99b02f52e9793c65f2fbb2aabf9313fd405b11a8 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Sun, 28 Apr 2024 22:10:23 -0400 Subject: [PATCH 07/94] yay --- build.gradle | 2 +- gradle/wrapper/gradle-wrapper.properties | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/build.gradle b/build.gradle index 8969a41..b7a9997 100644 --- a/build.gradle +++ b/build.gradle @@ -11,7 +11,7 @@ buildscript { } dependencies { // Note for FTC Teams: Do not modify this yourself. - classpath 'com.android.tools.build:gradle:7.2.0' + classpath 'com.android.tools.build:gradle:8.2.0' } } diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 9d86991..655d7d7 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ -#Sun Apr 28 17:12:03 PDT 2024 +#Sun Apr 28 21:36:26 EDT 2024 distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-7.4.2-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.2-bin.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists From f8fd55da434a7391e66c037196688f51509fe0d2 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Mon, 29 Apr 2024 21:24:29 -0400 Subject: [PATCH 08/94] oops, should be good now --- build.gradle | 2 +- gradle/wrapper/gradle-wrapper.properties | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/build.gradle b/build.gradle index b7a9997..8969a41 100644 --- a/build.gradle +++ b/build.gradle @@ -11,7 +11,7 @@ buildscript { } dependencies { // Note for FTC Teams: Do not modify this yourself. - classpath 'com.android.tools.build:gradle:8.2.0' + classpath 'com.android.tools.build:gradle:7.2.0' } } diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 655d7d7..d7de5fe 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ #Sun Apr 28 21:36:26 EDT 2024 distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.2-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-7.4.2-bin.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists From bdf183fe625df8a6be0bffa95ad6d0fe9a03f3f6 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Sun, 5 May 2024 20:32:06 -0400 Subject: [PATCH 09/94] fixed path parameter issues --- .../teamcode/pedroPathing/pathGeneration/Path.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java index 4035252..563197d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java @@ -38,32 +38,32 @@ public class Path { // Decreasing this will cause the deceleration at the end of the Path to be slower, making the // robot slower but reducing risk of end-of-path overshoots or localization slippage. // This can be set individually for each Path, but this is the default. - private static double zeroPowerAccelerationMultiplier = FollowerConstants.zeroPowerAccelerationMultiplier; + private double zeroPowerAccelerationMultiplier = FollowerConstants.zeroPowerAccelerationMultiplier; // When the robot is at the end of its current Path or PathChain and the velocity goes // this value, then end the Path. This is in inches/second. // This can be custom set for each Path. - private static double pathEndVelocityConstraint = FollowerConstants.pathEndVelocityConstraint; + private double pathEndVelocityConstraint = FollowerConstants.pathEndVelocityConstraint; // When the robot is at the end of its current Path or PathChain and the translational error // goes below this value, then end the Path. This is in inches. // This can be custom set for each Path. - private static double pathEndTranslationalConstraint = FollowerConstants.pathEndTranslationalConstraint; + private double pathEndTranslationalConstraint = FollowerConstants.pathEndTranslationalConstraint; // When the robot is at the end of its current Path or PathChain and the heading error goes // below this value, then end the Path. This is in radians. // This can be custom set for each Path. - private static double pathEndHeadingConstraint = FollowerConstants.pathEndHeadingConstraint; + private double pathEndHeadingConstraint = FollowerConstants.pathEndHeadingConstraint; // When the t-value of the closest point to the robot on the Path is greater than this value, // then the Path is considered at its end. // This can be custom set for each Path. - private static double pathEndTValueConstraint = FollowerConstants.pathEndTValueConstraint; + private double pathEndTValueConstraint = FollowerConstants.pathEndTValueConstraint; // When the Path is considered at its end parametrically, then the Follower has this many // seconds to further correct by default. // This can be custom set for each Path. - private static double pathEndTimeoutConstraint = FollowerConstants.pathEndTimeoutConstraint; + private double pathEndTimeoutConstraint = FollowerConstants.pathEndTimeoutConstraint; /** * Creates a new Path from a BezierCurve. The default heading interpolation is tangential. From 55b4b6b7f1d3b3c41091ebf826d1e622a1f92a86 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Thu, 9 May 2024 21:40:36 -0400 Subject: [PATCH 10/94] fixed centripetal force correction issues and added localizer stuff. an interface for the roadrunner localizer will come in the next commit --- .../ftc/teamcode/pedroPathing/TUNING.md | 10 +- .../pedroPathing/follower/Follower.java | 88 ++++-- .../localization/DriveEncoderLocalizer.java | 173 ++++++++++++ .../pedroPathing/localization/Encoder.java | 50 ++++ .../pedroPathing/localization/Matrix.java | 152 +++++++++++ .../localization/PoseUpdater.java | 79 +++--- .../localization/RoadRunnerEncoder.java | 250 +++++++++--------- .../RoadRunnerThreeWheelLocalizer.java | 117 ++++++++ .../localization/ThreeWheelLocalizer.java | 242 +++++++++++------ .../localization/TwoWheelLocalizer.java | 198 ++++++++++++++ .../localization/tuning/FowardTuner.java | 49 ++++ .../localization/tuning/LateralTuner.java | 49 ++++ .../localization/tuning/LocalizationTest.java | 114 ++++++++ .../localization/tuning/TurnTuner.java | 49 ++++ .../pathGeneration/BezierCurve.java | 2 +- .../pathGeneration/MathFunctions.java | 39 ++- .../pedroPathing/pathGeneration/Path.java | 7 +- .../pedroPathing/pathGeneration/Point.java | 8 +- .../teamcode/pedroPathing/util/Drawing.java | 39 +++ 19 files changed, 1431 insertions(+), 284 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/FowardTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md index 7aa7279..a41a4b2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md @@ -1,11 +1,11 @@ ## Prerequisites Obviously, you have to have a robot to use Pedro Pathing. Also, Pedro Pathing is only able to work with omnidirectional drives, like mecanum drive. There is currently no support for swerve drives. -You must also have a localizer of some sort. Pedro Pathing currently has the three-wheel localizer -from Road Runner, but we have plans to create our own localizer soon. You will need to have your -localizer tuned before starting tuning Pedro Pathing, since Pedro Pathing needs to know where your -robot is at all times. Additionally, using [FTC Dashboard](http://192.168.43.1:8080/dash) will help -a lot in tuning, and we have a slightly scuffed Desmos path visualizer [here](https://www.desmos.com/calculator/3so1zx0hcd). +You must also have a localizer of some sort. Pedro Pathing has both a three-wheel localizer and a +two-wheel localizer. You will need to have your localizer tuned before starting to tune PedroPathing. +Check out the tuning guide under the localization tab if you're planning on using one of the +localizers available in Pedro Pathing. Additionally, using [FTC Dashboard](http://192.168.43.1:8080/dash) +will help a lot in tuning, and we have a slightly scuffed Desmos path visualizer [here](https://www.desmos.com/calculator/3so1zx0hcd). One last thing to note is that Pedro Pathing operates in inches and radians. ## Tuning diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index 3cadba6..3a7fa37 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -12,9 +12,10 @@ import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstan import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.smallTranslationalPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFSwitch; +import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; @@ -22,6 +23,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierPoint; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; @@ -29,8 +31,10 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathCallback; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; import org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants; +import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; import org.firstinspires.ftc.teamcode.pedroPathing.util.PIDFController; import java.util.ArrayList; @@ -60,7 +64,7 @@ public class Follower { private PoseUpdater poseUpdater; - private Pose2d closestPose; + private Pose closestPose; private Path currentPath; @@ -118,6 +122,7 @@ public class Follower { private PIDFController largeDrivePIDF = new PIDFController(FollowerConstants.largeDrivePIDFCoefficients); + public static boolean drawStuff = true; public static boolean useTranslational = true; public static boolean useCentripetal = true; public static boolean useHeading = true; @@ -184,6 +189,7 @@ public class Follower { accelerations.add(new Vector()); } calculateAveragedVelocityAndAcceleration(); + draw(); } /** @@ -206,21 +212,34 @@ public class Follower { } } + /** + * This gets a Point from the current Path from a specified t-value. + * + * @return returns the Point. + */ + public Point getPointFromPath(double t) { + if (currentPath != null) { + return currentPath.getPoint(t); + } else { + return null; + } + } + /** * This returns the current pose from the PoseUpdater. * * @return returns the pose */ - public Pose2d getPose() { + public Pose getPose() { return poseUpdater.getPose(); } /** - * This sets the current pose in the PoseUpdater using Road Runner's setPoseEstimate() method. + * This sets the current pose in the PoseUpdater without using offsets. * * @param pose The pose to set the current pose to. */ - public void setPose(Pose2d pose) { + public void setPose(Pose pose) { poseUpdater.setPose(pose); } @@ -256,7 +275,7 @@ public class Follower { * * @param pose the pose to set the starting pose to. */ - public void setStartingPose(Pose2d pose) { + public void setStartingPose(Pose pose) { poseUpdater.setStartingPose(pose); } @@ -267,7 +286,7 @@ public class Follower { * * @param set The pose to set the current pose to. */ - public void setCurrentPoseWithOffset(Pose2d set) { + public void setCurrentPoseWithOffset(Pose set) { poseUpdater.setCurrentPoseWithOffset(set); } @@ -327,7 +346,7 @@ public class Follower { /** * This resets all offsets set to the PoseUpdater. If you have reset your pose using the - * setCurrentPoseUsingOffset(Pose2d set) method, then your pose will be returned to what the + * setCurrentPoseUsingOffset(Pose set) method, then your pose will be returned to what the * PoseUpdater thinks your pose would be, not the pose you reset to. */ public void resetOffset() { @@ -476,6 +495,14 @@ public class Follower { motors.get(i).setPower(drivePowers[i]); } } + draw(); + } + + public void draw() { + TelemetryPacket packet = new TelemetryPacket(); + packet.fieldOverlay().setStroke("#3F51B5"); + Drawing.drawRobot(packet.fieldOverlay(), getPose()); + FtcDashboard.getInstance().sendTelemetryPacket(packet); } /** @@ -757,19 +784,18 @@ public class Follower { * @return returns the centripetal force correction vector. */ public Vector getCentripetalForceCorrection() { - return new Vector(); -// if (!useCentripetal) return new Vector(); -// double curvature; -// if (auto) { -// curvature = currentPath.getClosestPointCurvature(); -// } else { -// double yPrime = averageVelocity.getYComponent() / averageVelocity.getXComponent(); -// double yDoublePrime = averageAcceleration.getYComponent() / averageVelocity.getXComponent(); -// curvature = (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)) / (yDoublePrime); -// } -// if (Double.isNaN(curvature)) return new Vector(); -// centripetalVector = new Vector(MathFunctions.clamp(Math.abs(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature), -1, 1), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta())); -// return centripetalVector; + if (!useCentripetal) return new Vector(); + double curvature; + if (auto) { + curvature = currentPath.getClosestPointCurvature(); + } else { + double yPrime = averageVelocity.getYComponent() / averageVelocity.getXComponent(); + double yDoublePrime = averageAcceleration.getYComponent() / averageVelocity.getXComponent(); + curvature = (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)) / (yDoublePrime); + } + if (Double.isNaN(curvature)) return new Vector(); + centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -1, 1), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta())); + return centripetalVector; } /** @@ -779,7 +805,7 @@ public class Follower { * * @return returns the closest pose. */ - public Pose2d getClosestPose() { + public Pose getClosestPose() { return closestPose; } @@ -863,9 +889,18 @@ public class Follower { telemetry.addData("x", getPose().getX()); telemetry.addData("y", getPose().getY()); telemetry.addData("heading", getPose().getHeading()); + telemetry.addData("total heading", poseUpdater.getTotalHeading()); telemetry.addData("velocity magnitude", getVelocity().getMagnitude()); telemetry.addData("velocity heading", getVelocity().getTheta()); telemetry.update(); + TelemetryPacket packet = new TelemetryPacket(); + //Draws the current target path that the robot would like to follow + packet.fieldOverlay().setStroke("#4CAF50"); + Drawing.drawRobot(packet.fieldOverlay(), new Pose(getPose().getX(), getPose().getY(), getPose().getHeading())); + //Draws the current path that the robot is following + packet.fieldOverlay().setStroke("#3F51B5"); + if (currentPath != null) Drawing.drawRobot(packet.fieldOverlay(), getPointFromPath(currentPath.getClosestPointTValue())); + FtcDashboard.getInstance().sendTelemetryPacket(packet); } /** @@ -877,4 +912,13 @@ public class Follower { public void telemetryDebug(Telemetry telemetry) { telemetryDebug(new MultipleTelemetry(telemetry)); } + + /** + * This returns the total number of radians the robot has turned. + * + * @return the total heading. + */ + public double getTotalHeading() { + return poseUpdater.getTotalHeading(); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java new file mode 100644 index 0000000..3a10761 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java @@ -0,0 +1,173 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; + +/** + * This is the DriveEncoderLocalizer class. This class extends the Localizer superclass and is a + * localizer that uses the drive encoder set up. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 4/2/2024 + */ +@Config +public class DriveEncoderLocalizer extends Localizer { // todo: make drive encoders work + private HardwareMap hardwareMap; + private Pose startPose; + private Pose displacementPose; + private Pose currentVelocity; + private Matrix prevRotationMatrix; + private NanoTimer timer; + private long deltaTimeNano; + private Encoder leftFront; + private Encoder rightFront; + private Encoder leftRear; + private Encoder rightRear; + private double totalHeading; + public static double FORWARD_TICKS_TO_INCHES = 1; + public static double STRAFE_TICKS_TO_INCHES = 1; + public static double TURN_TICKS_TO_RADIANS = 1; + public static double ROBOT_WIDTH = 1; + public static double ROBOT_LENGTH = 1; + + public DriveEncoderLocalizer(HardwareMap map) { + this(map, new Pose()); + } + + public DriveEncoderLocalizer(HardwareMap map, Pose setStartPose) { + + hardwareMap = map; + + // TODO: replace these with your encoder ports + leftFront = new Encoder(hardwareMap.get(DcMotorEx.class, "leftFront")); + rightFront = new Encoder(hardwareMap.get(DcMotorEx.class, "rightFront")); + leftRear = new Encoder(hardwareMap.get(DcMotorEx.class, "leftRear")); + rightRear = new Encoder(hardwareMap.get(DcMotorEx.class, "rightRear")); + + // TODO: reverse any encoders necessary + leftFront.setDirection(Encoder.REVERSE); + rightRear.setDirection(Encoder.REVERSE); + leftRear.setDirection(Encoder.FORWARD); + rightRear.setDirection(Encoder.FORWARD); + + setStartPose(setStartPose); + timer = new NanoTimer(); + deltaTimeNano = 1; + displacementPose = new Pose(); + currentVelocity = new Pose(); + } + + @Override + public Pose getPose() { + return MathFunctions.addPoses(startPose, displacementPose); + } + + @Override + public Pose getVelocity() { + return currentVelocity.copy(); + } + + @Override + public Vector getVelocityVector() { + return currentVelocity.getVector(); + } + + @Override + public void setStartPose(Pose setStart) { + startPose = setStart; + } + + public void setPrevRotationMatrix(double heading) { + prevRotationMatrix = new Matrix(3,3); + prevRotationMatrix.set(0, 0, Math.cos(heading)); + prevRotationMatrix.set(0, 1, -Math.sin(heading)); + prevRotationMatrix.set(1, 0, Math.sin(heading)); + prevRotationMatrix.set(1, 1, Math.cos(heading)); + prevRotationMatrix.set(2, 2, 1.0); + } + + @Override + public void setPose(Pose setPose) { + displacementPose = MathFunctions.subtractPoses(setPose, startPose); + resetEncoders(); + } + + @Override + public void update() { + deltaTimeNano = timer.getElapsedTime(); + timer.resetTimer(); + + updateEncoders(); + Matrix robotDeltas = getRobotDeltas(); + Matrix globalDeltas; + setPrevRotationMatrix(getPose().getHeading()); + + Matrix transformation = new Matrix(3,3); + if (Math.abs(robotDeltas.get(2, 0)) < 0.001) { + transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); + transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0); + transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0); + transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); + transformation.set(2, 2, 1.0); + } else { + transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); + transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0)); + transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0)); + transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); + transformation.set(2, 2, 1.0); + } + + globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas); + + displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0))); + currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9))); + + totalHeading += globalDeltas.get(2, 0); + } + + public void updateEncoders() { + leftFront.update(); + rightFront.update(); + leftRear.update(); + rightRear.update(); + } + + public void resetEncoders() { + leftFront.reset(); + rightFront.reset(); + leftRear.reset(); + rightRear.reset(); + } + + public Matrix getRobotDeltas() { + Matrix returnMatrix = new Matrix(3,1); + // x/forward movement + returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * (leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() + rightRear.getDeltaPosition())); + //y/strafe movement + returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() - rightRear.getDeltaPosition())); + // theta/turning + returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() - leftRear.getDeltaPosition() + rightRear.getDeltaPosition()) / (ROBOT_WIDTH + ROBOT_LENGTH))); + return returnMatrix; + } + + public double getTotalHeading() { + return totalHeading; + } + + public double getForwardMultiplier() { + return FORWARD_TICKS_TO_INCHES; + } + + public double getLateralMultiplier() { + return STRAFE_TICKS_TO_INCHES; + } + + public double getTurningMultiplier() { + return TURN_TICKS_TO_RADIANS; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java new file mode 100644 index 0000000..3f8f72f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java @@ -0,0 +1,50 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +/** + * This is the Encoder class. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 4/2/2024 + */ +public class Encoder { + private DcMotorEx motor; + private double previousPosition; + private double currentPosition; + private double multiplier; + + public final static double FORWARD = 1, REVERSE = -1; + + public Encoder(DcMotorEx setMotor) { + motor = setMotor; + multiplier = FORWARD; + reset(); + } + + public void setDirection(double setMultiplier) { + multiplier = setMultiplier; + } + + public void reset() { + motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + previousPosition = motor.getCurrentPosition(); + currentPosition = motor.getCurrentPosition(); + motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + public void update() { + previousPosition = currentPosition; + currentPosition = motor.getCurrentPosition(); + } + + public double getMultiplier() { + return multiplier * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1); + } + + public double getDeltaPosition() { + return getMultiplier() * (currentPosition - previousPosition); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java new file mode 100644 index 0000000..a16a3db --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java @@ -0,0 +1,152 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import java.util.Arrays; + +/** + * This is the Matrix class. This defines matrices, primarily for use in the localizers. However, if + * matrices and matrix operations are necessary, this class as well as some operations in the + * MathFunctions class can absolutely be used there as well. It's similar to Mats in OpenCV if you've + * used them before, but with more limited functionality. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 4/2/2024 + */ +public class Matrix { + private double[][] matrix; + + public Matrix() { + matrix = new double[0][0]; + } + + public Matrix(int rows, int columns) { + matrix = new double[rows][columns]; + } + + public Matrix(double[][] setMatrix) { + setMatrix(setMatrix); + } + + public Matrix(Matrix setMatrix) { + setMatrix(setMatrix); + } + + public static double[][] deepCopy(double[][] copyMatrix) { + double[][] returnMatrix = new double[copyMatrix.length][copyMatrix[0].length]; + for (int i = 0; i < copyMatrix.length; i++) { + returnMatrix[i] = Arrays.copyOf(copyMatrix[i], copyMatrix[i].length); + } + return returnMatrix; + } + + public double[][] getMatrix() { + return deepCopy(matrix); + } + + public double[] get(int row) { + return Arrays.copyOf(matrix[row], matrix[row].length); + } + + public double get(int row, int column) { + return get(row)[column]; + } + + public int getRows() { + return matrix.length; + } + + public int getColumns() { + return matrix[0].length; + } + + public boolean setMatrix(Matrix setMatrix) { + return setMatrix(setMatrix.getMatrix()); + } + + public boolean setMatrix(double[][] setMatrix) { + int columns = setMatrix[0].length; + for (int i = 0; i < setMatrix.length; i++) { + if (setMatrix[i].length != columns) { + return false; + } + } + matrix = deepCopy(setMatrix); + return true; + } + + public boolean set(int row, double[] input) { + if (input.length != getColumns()) { + return false; + } + matrix[row] = Arrays.copyOf(input, input.length); + return true; + } + + public boolean set(int row, int column, double input) { + matrix[row][column] = input; + return true; + } + + public boolean add(Matrix input) { + if (input.getRows() == getRows() && input.getColumns() == getColumns()) { + for (int i = 0; i < getRows(); i++) { + for (int j = 0; j < getColumns(); j++) { + set(i, j, get(i,j) + input.get(i,j)); + } + } + return true; + } + return false; + } + + public boolean subtract(Matrix input) { + if (input.getRows() == getRows() && input.getColumns() == getColumns()) { + for (int i = 0; i < getRows(); i++) { + for (int j = 0; j < getColumns(); j++) { + set(i, j, get(i,j) - input.get(i,j)); + } + } + return true; + } + return false; + } + + public boolean scalarMultiply(double scalar) { + for (int i = 0; i < getRows(); i++) { + for (int j = 0; j < getColumns(); j++) { + set(i, j, scalar * get(i,j)); + } + } + return true; + } + + public boolean flipSigns() { + return scalarMultiply(-1); + } + + public boolean multiply(Matrix input) { + if (getColumns() == input.getRows()) { + Matrix product = new Matrix(getRows(), input.getColumns()); + for (int i = 0; i < product.getRows(); i++) { + for (int j = 0; j < product.getColumns(); j++) { + double value = 0; + for (int k = 0; k < get(i).length; k++) { + value += get(i, k) * input.get(k, j); + } + product.set(i, j, value); + } + } + setMatrix(product); + return true; + } + return false; + } + + public static Matrix multiply(Matrix one, Matrix two) { + Matrix returnMatrix = new Matrix(one); + if (returnMatrix.multiply(two)) { + return returnMatrix; + } else { + return new Matrix(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java index d8d9b32..a010f6a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java @@ -1,6 +1,5 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization; -import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; @@ -9,9 +8,6 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; -import java.util.ArrayList; -import java.util.List; - /** * This is the PoseUpdater class. This class handles getting pose data from the localizer and returning * the information in a useful way to the Follower. @@ -26,13 +22,13 @@ public class PoseUpdater { private IMU imu; - private ThreeWheelLocalizer localizer; + private Localizer localizer; - private Pose2d startingPose = new Pose2d(0,0,0); + private Pose startingPose = new Pose(0,0,0); - private Pose2d currentPose = startingPose; + private Pose currentPose = startingPose; - private Pose2d previousPose = startingPose; + private Pose previousPose = startingPose; private Vector currentVelocity = new Vector(); @@ -59,9 +55,8 @@ public class PoseUpdater { module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); } - List lastTrackingEncPositions = new ArrayList<>(); - List lastTrackingEncVels = new ArrayList<>(); - localizer = new ThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels); + // TODO: change this to your preferred localizer + localizer = new ThreeWheelLocalizer(hardwareMap); } /** @@ -85,23 +80,23 @@ public class PoseUpdater { * * @param set the Pose to set the starting pose to. */ - public void setStartingPose(Pose2d set) { + public void setStartingPose(Pose set) { startingPose = set; previousPose = startingPose; previousPoseTime = System.nanoTime(); currentPoseTime = System.nanoTime(); - localizer.setPoseEstimate(set); + localizer.setStartPose(set); } /** - * This sets the current pose, using offsets so no reset time delay. This is better than the - * Road Runner reset, in general. Think of using offsets as setting trim in an aircraft. This can - * be reset as well, so beware of using the resetOffset() method. + * This sets the current pose, using offsets. Think of using offsets as setting trim in an + * aircraft. This can be reset as well, so beware of using the resetOffset() method. + * * * @param set The pose to set the current pose to. */ - public void setCurrentPoseWithOffset(Pose2d set) { - Pose2d currentPose = getRawPose(); + public void setCurrentPoseWithOffset(Pose set) { + Pose currentPose = getRawPose(); setXOffset(set.getX() - currentPose.getX()); setYOffset(set.getY() - currentPose.getY()); setHeadingOffset(MathFunctions.getTurnDirection(currentPose.getHeading(), set.getHeading()) * MathFunctions.getSmallestAngleDifference(currentPose.getHeading(), set.getHeading())); @@ -167,8 +162,8 @@ public class PoseUpdater { * @param pose The pose to be offset. * @return This returns a new Pose with the offset applied. */ - public Pose2d applyOffset(Pose2d pose) { - return new Pose2d(pose.getX()+xOffset, pose.getY()+yOffset, pose.getHeading()+headingOffset); + public Pose applyOffset(Pose pose) { + return new Pose(pose.getX()+xOffset, pose.getY()+yOffset, pose.getHeading()+headingOffset); } /** @@ -189,9 +184,9 @@ public class PoseUpdater { * * @return returns the current pose. */ - public Pose2d getPose() { + public Pose getPose() { if (currentPose == null) { - currentPose = localizer.getPoseEstimate(); + currentPose = localizer.getPose(); return applyOffset(currentPose); } else { return applyOffset(currentPose); @@ -205,9 +200,9 @@ public class PoseUpdater { * * @return returns the raw pose. */ - public Pose2d getRawPose() { + public Pose getRawPose() { if (currentPose == null) { - currentPose = localizer.getPoseEstimate(); + currentPose = localizer.getPose(); return currentPose; } else { return currentPose; @@ -215,13 +210,13 @@ public class PoseUpdater { } /** - * This sets the current pose using the Road Runner pose reset. This is slow. + * This sets the current pose without using resettable offsets. * * @param set the pose to set the current pose to. */ - public void setPose(Pose2d set) { + public void setPose(Pose set) { resetOffset(); - localizer.setPoseEstimate(set); + localizer.setPose(set); } /** @@ -229,7 +224,7 @@ public class PoseUpdater { * * @return returns the robot's previous pose. */ - public Pose2d getPreviousPose() { + public Pose getPreviousPose() { return previousPose; } @@ -238,8 +233,10 @@ public class PoseUpdater { * * @return returns the robot's delta pose. */ - public Pose2d getDeltaPose() { - return getPose().minus(previousPose); + public Pose getDeltaPose() { + Pose returnPose = getPose(); + returnPose.subtract(previousPose); + return returnPose; } /** @@ -290,7 +287,7 @@ public class PoseUpdater { * This resets the heading of the robot to the IMU's heading, using Road Runner's pose reset. */ public void resetHeadingToIMU() { - localizer.resetHeading(getNormalizedIMUHeading() + startingPose.getHeading()); + localizer.setPose(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading())); } /** @@ -299,7 +296,7 @@ public class PoseUpdater { * method. */ public void resetHeadingToIMUWithOffsets() { - setCurrentPoseWithOffset(new Pose2d(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading())); + setCurrentPoseWithOffset(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading())); } /** @@ -310,4 +307,22 @@ public class PoseUpdater { public double getNormalizedIMUHeading() { return MathFunctions.normalizeAngle(-imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); } + + /** + * This returns the total number of radians the robot has turned. + * + * @return the total heading. + */ + public double getTotalHeading() { + return localizer.getTotalHeading(); + } + + /** + * This returns the Localizer. + * + * @return the Localizer + */ + public Localizer getLocalizer() { + return localizer; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java index edbe337..35b8789 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java @@ -1,125 +1,125 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization; - -import com.acmerobotics.roadrunner.util.NanoClock; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; - -/** - * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding - * slot's motor direction - */ -public class RoadRunnerEncoder { - private final static int CPS_STEP = 0x10000; - - private static double inverseOverflow(double input, double estimate) { - // convert to uint16 - int real = (int) input & 0xffff; - // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits - // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window - real += ((real % 20) / 4) * CPS_STEP; - // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by - real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP; - return real; - } - - public enum Direction { - FORWARD(1), - REVERSE(-1); - - private int multiplier; - - Direction(int multiplier) { - this.multiplier = multiplier; - } - - public int getMultiplier() { - return multiplier; - } - } - - private DcMotorEx motor; - private NanoClock clock; - - private Direction direction; - - private int lastPosition; - private int velocityEstimateIdx; - private double[] velocityEstimates; - private double lastUpdateTime; - - public RoadRunnerEncoder(DcMotorEx motor, NanoClock clock) { - this.motor = motor; - this.clock = clock; - - this.direction = Direction.FORWARD; - - this.lastPosition = 0; - this.velocityEstimates = new double[3]; - this.lastUpdateTime = clock.seconds(); - } - - public RoadRunnerEncoder(DcMotorEx motor) { - this(motor, NanoClock.system()); - } - - public Direction getDirection() { - return direction; - } - - private int getMultiplier() { - return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1); - } - - /** - * Allows you to set the direction of the counts and velocity without modifying the motor's direction state - * @param direction either reverse or forward depending on if encoder counts should be negated - */ - public void setDirection(Direction direction) { - this.direction = direction; - } - - /** - * Gets the position from the underlying motor and adjusts for the set direction. - * Additionally, this method updates the velocity estimates used for compensated velocity - * - * @return encoder position - */ - public int getCurrentPosition() { - int multiplier = getMultiplier(); - int currentPosition = motor.getCurrentPosition() * multiplier; - if (currentPosition != lastPosition) { - double currentTime = clock.seconds(); - double dt = currentTime - lastUpdateTime; - velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt; - velocityEstimateIdx = (velocityEstimateIdx + 1) % 3; - lastPosition = currentPosition; - lastUpdateTime = currentTime; - } - return currentPosition; - } - - /** - * Gets the velocity directly from the underlying motor and compensates for the direction - * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore) - * - * @return raw velocity - */ - public double getRawVelocity() { - int multiplier = getMultiplier(); - return motor.getVelocity() * multiplier; - } - - /** - * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity - * that are lost in overflow due to velocity being transmitted as 16 bits. - * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly. - * - * @return corrected velocity - */ - public double getCorrectedVelocity() { - double median = velocityEstimates[0] > velocityEstimates[1] - ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2])) - : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2])); - return inverseOverflow(getRawVelocity(), median); - } -} +//package org.firstinspires.ftc.teamcode.pedroPathing.localization; +// +//import com.acmerobotics.roadrunner.util.NanoClock; +//import com.qualcomm.robotcore.hardware.DcMotorEx; +//import com.qualcomm.robotcore.hardware.DcMotorSimple; +// +///** +// * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding +// * slot's motor direction +// */ +//public class RoadRunnerEncoder { +// private final static int CPS_STEP = 0x10000; +// +// private static double inverseOverflow(double input, double estimate) { +// // convert to uint16 +// int real = (int) input & 0xffff; +// // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits +// // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window +// real += ((real % 20) / 4) * CPS_STEP; +// // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by +// real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP; +// return real; +// } +// +// public enum Direction { +// FORWARD(1), +// REVERSE(-1); +// +// private int multiplier; +// +// Direction(int multiplier) { +// this.multiplier = multiplier; +// } +// +// public int getMultiplier() { +// return multiplier; +// } +// } +// +// private DcMotorEx motor; +// private NanoClock clock; +// +// private Direction direction; +// +// private int lastPosition; +// private int velocityEstimateIdx; +// private double[] velocityEstimates; +// private double lastUpdateTime; +// +// public RoadRunnerEncoder(DcMotorEx motor, NanoClock clock) { +// this.motor = motor; +// this.clock = clock; +// +// this.direction = Direction.FORWARD; +// +// this.lastPosition = 0; +// this.velocityEstimates = new double[3]; +// this.lastUpdateTime = clock.seconds(); +// } +// +// public RoadRunnerEncoder(DcMotorEx motor) { +// this(motor, NanoClock.system()); +// } +// +// public Direction getDirection() { +// return direction; +// } +// +// private int getMultiplier() { +// return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1); +// } +// +// /** +// * Allows you to set the direction of the counts and velocity without modifying the motor's direction state +// * @param direction either reverse or forward depending on if encoder counts should be negated +// */ +// public void setDirection(Direction direction) { +// this.direction = direction; +// } +// +// /** +// * Gets the position from the underlying motor and adjusts for the set direction. +// * Additionally, this method updates the velocity estimates used for compensated velocity +// * +// * @return encoder position +// */ +// public int getCurrentPosition() { +// int multiplier = getMultiplier(); +// int currentPosition = motor.getCurrentPosition() * multiplier; +// if (currentPosition != lastPosition) { +// double currentTime = clock.seconds(); +// double dt = currentTime - lastUpdateTime; +// velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt; +// velocityEstimateIdx = (velocityEstimateIdx + 1) % 3; +// lastPosition = currentPosition; +// lastUpdateTime = currentTime; +// } +// return currentPosition; +// } +// +// /** +// * Gets the velocity directly from the underlying motor and compensates for the direction +// * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore) +// * +// * @return raw velocity +// */ +// public double getRawVelocity() { +// int multiplier = getMultiplier(); +// return motor.getVelocity() * multiplier; +// } +// +// /** +// * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity +// * that are lost in overflow due to velocity being transmitted as 16 bits. +// * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly. +// * +// * @return corrected velocity +// */ +// public double getCorrectedVelocity() { +// double median = velocityEstimates[0] > velocityEstimates[1] +// ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2])) +// : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2])); +// return inverseOverflow(getRawVelocity(), median); +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java new file mode 100644 index 0000000..ce879cc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java @@ -0,0 +1,117 @@ +//package org.firstinspires.ftc.teamcode.pedroPathing.localization; +// +//import androidx.annotation.NonNull; +// +//import com.acmerobotics.dashboard.config.Config; +//import com.acmerobotics.roadrunner.geometry.Pose2d; +//import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; +//import com.qualcomm.robotcore.hardware.DcMotorEx; +//import com.qualcomm.robotcore.hardware.HardwareMap; +// +//import java.util.Arrays; +//import java.util.List; +// +///* +// * Sample tracking wheel localizer implementation assuming the standard configuration: +// * +// * left on robot is y pos +// * +// * front of robot is x pos +// * +// * /--------------\ +// * | ____ | +// * | ---- | +// * | || || | +// * | || || | +// * | | +// * | | +// * \--------------/ +// * +// */ +// +///** +// * This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will +// * be replaced with a custom localizer. +// */ +//@Config +//public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer { +// public static double TICKS_PER_REV = 8192; +// public static double WHEEL_RADIUS = 1.37795; // in +// public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed +// +// public static double X_MULTIPLIER = 0.5008239963; +// public static double Y_MULTIPLIER = 0.5018874659; +// +// public static double leftX = -18.5/25.4 - 0.1, leftY = 164.4/25.4, rightX = -18.4/25.4 - 0.1, rightY = -159.6/25.4, strafeX = -107.9/25.4+0.25, strafeY = -1.1/25.4-0.23; +// +// private RoadRunnerEncoder leftEncoder, rightEncoder, strafeEncoder; +// +// private List lastEncPositions, lastEncVels; +// +// public RoadRunnerThreeWheelLocalizer(HardwareMap hardwareMap, List lastTrackingEncPositions, List lastTrackingEncVels) { +// super(Arrays.asList( +// new Pose2d(leftX, leftY, 0), // left +// new Pose2d(rightX, rightY, 0), // right +// new Pose2d(strafeX, strafeY, Math.toRadians(90)) // strafe +// )); +// +// lastEncPositions = lastTrackingEncPositions; +// lastEncVels = lastTrackingEncVels; +// +// // TODO: redo the configs here +// leftEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "leftRear")); +// rightEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "rightFront")); +// strafeEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); +// +// // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) +// leftEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); +// rightEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); +// strafeEncoder.setDirection(RoadRunnerEncoder.Direction.FORWARD); +// } +// +// public void resetHeading(double heading) { +// setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading)); +// } +// +// public static double encoderTicksToInches(double ticks) { +// return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; +// } +// +// @NonNull +// @Override +// public List getWheelPositions() { +// int leftPos = leftEncoder.getCurrentPosition(); +// int rightPos = rightEncoder.getCurrentPosition(); +// int frontPos = strafeEncoder.getCurrentPosition(); +// +// lastEncPositions.clear(); +// lastEncPositions.add(leftPos); +// lastEncPositions.add(rightPos); +// lastEncPositions.add(frontPos); +// +// return Arrays.asList( +// encoderTicksToInches(leftPos) * X_MULTIPLIER, +// encoderTicksToInches(rightPos) * X_MULTIPLIER, +// encoderTicksToInches(frontPos) * Y_MULTIPLIER +// ); +// } +// +// @NonNull +// @Override +// public List getWheelVelocities() { +// int leftVel = (int) leftEncoder.getCorrectedVelocity(); +// int rightVel = (int) rightEncoder.getCorrectedVelocity(); +// int frontVel = (int) strafeEncoder.getCorrectedVelocity(); +// +// lastEncVels.clear(); +// lastEncVels.add(leftVel); +// lastEncVels.add(rightVel); +// lastEncVels.add(frontVel); +// +// return Arrays.asList( +// encoderTicksToInches(leftVel) * X_MULTIPLIER, +// encoderTicksToInches(rightVel) * X_MULTIPLIER, +// encoderTicksToInches(frontVel) * Y_MULTIPLIER +// ); +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java index 88f1e7c..cdbc35d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java @@ -1,117 +1,193 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization; -import androidx.annotation.NonNull; - import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.HardwareMap; -import java.util.Arrays; -import java.util.List; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; -/* - * Sample tracking wheel localizer implementation assuming the standard configuration: +/** + * This is the ThreeWheelLocalizer class. This class extends the Localizer superclass and is a + * localizer that uses the three wheel odometry set up. The diagram below, which is taken from + * Road Runner, shows a typical set up. + * + * The view is from the bottom of the robot looking upwards. * * left on robot is y pos * - * front of robot is x pos + * front on robot is x pos * * /--------------\ * | ____ | * | ---- | * | || || | - * | || || | + * | || || | left (y pos) * | | * | | * \--------------/ + * front (x pos) * - */ - -/** - * This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will - * be replaced with a custom localizer. + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 4/2/2024 */ @Config -public class ThreeWheelLocalizer extends ThreeTrackingWheelLocalizer { - public static double TICKS_PER_REV = 8192; - public static double WHEEL_RADIUS = 1.37795; // in - public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed +public class ThreeWheelLocalizer extends Localizer { + private HardwareMap hardwareMap; + private Pose startPose; + private Pose displacementPose; + private Pose currentVelocity; + private Matrix prevRotationMatrix; + private NanoTimer timer; + private long deltaTimeNano; + private Encoder leftEncoder; + private Encoder rightEncoder; + private Encoder strafeEncoder; + private Pose leftEncoderPose; + private Pose rightEncoderPose; + private Pose strafeEncoderPose; + private double totalHeading; + public static double FORWARD_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963; + public static double STRAFE_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659; + public static double TURN_TICKS_TO_RADIANS = 0.00053717;//8192 * 1.37795 * 2 * Math.PI * 0.5; - public static double X_MULTIPLIER = 0.5008239963; - public static double Y_MULTIPLIER = 0.5018874659; - - public static double leftX = -18.5/25.4 - 0.1, leftY = 164.4/25.4, rightX = -18.4/25.4 - 0.1, rightY = -159.6/25.4, strafeX = -107.9/25.4+0.25, strafeY = -1.1/25.4-0.23; - - private RoadRunnerEncoder leftEncoder, rightEncoder, strafeEncoder; - - private List lastEncPositions, lastEncVels; - - public ThreeWheelLocalizer(HardwareMap hardwareMap, List lastTrackingEncPositions, List lastTrackingEncVels) { - super(Arrays.asList( - new Pose2d(leftX, leftY, 0), // left - new Pose2d(rightX, rightY, 0), // right - new Pose2d(strafeX, strafeY, Math.toRadians(90)) // strafe - )); - - lastEncPositions = lastTrackingEncPositions; - lastEncVels = lastTrackingEncVels; - - // TODO: redo the configs here - leftEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "leftRear")); - rightEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "rightFront")); - strafeEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); - - // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) - leftEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); - rightEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); - strafeEncoder.setDirection(RoadRunnerEncoder.Direction.FORWARD); + public ThreeWheelLocalizer(HardwareMap map) { + this(map, new Pose()); } - public void resetHeading(double heading) { - setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading)); + public ThreeWheelLocalizer(HardwareMap map, Pose setStartPose) { + // TODO: replace these with your encoder positions + leftEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0); + rightEncoderPose = new Pose(-18.4/25.4 - 0.1, -159.6/25.4, 0); + strafeEncoderPose = new Pose(0*(-107.9/25.4+8)+-107.9/25.4+0.25, -1.1/25.4-0.23, Math.toRadians(90)); + + hardwareMap = map; + + // TODO: replace these with your encoder ports + leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftRear")); + rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightFront")); + strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); + + // TODO: reverse any encoders necessary + leftEncoder.setDirection(Encoder.REVERSE); + rightEncoder.setDirection(Encoder.REVERSE); + strafeEncoder.setDirection(Encoder.FORWARD); + + setStartPose(setStartPose); + timer = new NanoTimer(); + deltaTimeNano = 1; + displacementPose = new Pose(); + currentVelocity = new Pose(); + totalHeading = 0; + + resetEncoders(); } - public static double encoderTicksToInches(double ticks) { - return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; - } - - @NonNull @Override - public List getWheelPositions() { - int leftPos = leftEncoder.getCurrentPosition(); - int rightPos = rightEncoder.getCurrentPosition(); - int frontPos = strafeEncoder.getCurrentPosition(); - - lastEncPositions.clear(); - lastEncPositions.add(leftPos); - lastEncPositions.add(rightPos); - lastEncPositions.add(frontPos); - - return Arrays.asList( - encoderTicksToInches(leftPos) * X_MULTIPLIER, - encoderTicksToInches(rightPos) * X_MULTIPLIER, - encoderTicksToInches(frontPos) * Y_MULTIPLIER - ); + public Pose getPose() { + return MathFunctions.addPoses(startPose, displacementPose); } - @NonNull @Override - public List getWheelVelocities() { - int leftVel = (int) leftEncoder.getCorrectedVelocity(); - int rightVel = (int) rightEncoder.getCorrectedVelocity(); - int frontVel = (int) strafeEncoder.getCorrectedVelocity(); + public Pose getVelocity() { + return currentVelocity.copy(); + } - lastEncVels.clear(); - lastEncVels.add(leftVel); - lastEncVels.add(rightVel); - lastEncVels.add(frontVel); + @Override + public Vector getVelocityVector() { + return currentVelocity.getVector(); + } - return Arrays.asList( - encoderTicksToInches(leftVel) * X_MULTIPLIER, - encoderTicksToInches(rightVel) * X_MULTIPLIER, - encoderTicksToInches(frontVel) * Y_MULTIPLIER - ); + @Override + public void setStartPose(Pose setStart) { + startPose = setStart; + } + + public void setPrevRotationMatrix(double heading) { + prevRotationMatrix = new Matrix(3,3); + prevRotationMatrix.set(0, 0, Math.cos(heading)); + prevRotationMatrix.set(0, 1, -Math.sin(heading)); + prevRotationMatrix.set(1, 0, Math.sin(heading)); + prevRotationMatrix.set(1, 1, Math.cos(heading)); + prevRotationMatrix.set(2, 2, 1.0); + } + + @Override + public void setPose(Pose setPose) { + displacementPose = MathFunctions.subtractPoses(setPose, startPose); + resetEncoders(); + } + + @Override + public void update() { + deltaTimeNano = timer.getElapsedTime(); + timer.resetTimer(); + + updateEncoders(); + Matrix robotDeltas = getRobotDeltas(); + Matrix globalDeltas; + setPrevRotationMatrix(getPose().getHeading()); + + Matrix transformation = new Matrix(3,3); + if (Math.abs(robotDeltas.get(2, 0)) < 0.001) { + transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); + transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0); + transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0); + transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); + transformation.set(2, 2, 1.0); + } else { + transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); + transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0)); + transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0)); + transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); + transformation.set(2, 2, 1.0); + } + + globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas); + + displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0))); + currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9))); + + totalHeading += globalDeltas.get(2, 0); + } + + public void updateEncoders() { + leftEncoder.update(); + rightEncoder.update(); + strafeEncoder.update(); + } + + public void resetEncoders() { + leftEncoder.reset(); + rightEncoder.reset(); + strafeEncoder.reset(); + } + + public Matrix getRobotDeltas() { + Matrix returnMatrix = new Matrix(3,1); + // x/forward movement + returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * ((rightEncoder.getDeltaPosition() * leftEncoderPose.getY() - leftEncoder.getDeltaPosition() * rightEncoderPose.getY()) / (leftEncoderPose.getY() - rightEncoderPose.getY()))); + //y/strafe movement + returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY())))); + // theta/turning + returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY()))); + return returnMatrix; + } + + public double getTotalHeading() { + return totalHeading; + } + + public double getForwardMultiplier() { + return FORWARD_TICKS_TO_INCHES; + } + + public double getLateralMultiplier() { + return STRAFE_TICKS_TO_INCHES; + } + + public double getTurningMultiplier() { + return TURN_TICKS_TO_RADIANS; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java new file mode 100644 index 0000000..26e46bc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java @@ -0,0 +1,198 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; + +/** + * This is the TwoWheelLocalizer class. This class extends the Localizer superclass and is a + * localizer that uses the two wheel odometry with IMU set up. The diagram below, which is taken from + * Road Runner, shows a typical set up. + * + * The view is from the bottom of the robot looking upwards. + * + * left on robot is y pos + * + * front on robot is x pos + * + * /--------------\ + * | ____ | + * | ---- | + * | || | + * | || | left (y pos) + * | | + * | | + * \--------------/ + * front (x pos) + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 4/2/2024 + */ +@Config +public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo work + private HardwareMap hardwareMap; + private IMU imu; + private Pose startPose; + private Pose displacementPose; + private Pose currentVelocity; + private Matrix prevRotationMatrix; + private NanoTimer timer; + private long deltaTimeNano; + private Encoder forwardEncoder; + private Encoder strafeEncoder; + private Pose forwardEncoderPose; + private Pose strafeEncoderPose; + private double previousIMUOrientation; + private double deltaRadians; + private double totalHeading; + public static double FORWARD_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5008239963; + public static double STRAFE_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5018874659; + + public TwoWheelLocalizer(HardwareMap map) { + this(map, new Pose()); + } + + public TwoWheelLocalizer(HardwareMap map, Pose setStartPose) { + // TODO: replace these with your encoder positions + forwardEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0); + strafeEncoderPose = new Pose(-107.9/25.4+0.25, -1.1/25.4-0.23, Math.toRadians(90)); + + hardwareMap = map; + + imu = hardwareMap.get(IMU.class, "imu"); + imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.UP, RevHubOrientationOnRobot.UsbFacingDirection.LEFT))); + + // TODO: replace these with your encoder ports + forwardEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftRear")); + strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); + + // TODO: reverse any encoders necessary + forwardEncoder.setDirection(Encoder.REVERSE); + strafeEncoder.setDirection(Encoder.FORWARD); + + setStartPose(setStartPose); + timer = new NanoTimer(); + deltaTimeNano = 1; + displacementPose = new Pose(); + currentVelocity = new Pose(); + + previousIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); + deltaRadians = 0; + } + + @Override + public Pose getPose() { + return MathFunctions.addPoses(startPose, displacementPose); + } + + @Override + public Pose getVelocity() { + return currentVelocity.copy(); + } + + @Override + public Vector getVelocityVector() { + return currentVelocity.getVector(); + } + + @Override + public void setStartPose(Pose setStart) { + startPose = setStart; + } + + public void setPrevRotationMatrix(double heading) { + prevRotationMatrix = new Matrix(3,3); + prevRotationMatrix.set(0, 0, Math.cos(heading)); + prevRotationMatrix.set(0, 1, -Math.sin(heading)); + prevRotationMatrix.set(1, 0, Math.sin(heading)); + prevRotationMatrix.set(1, 1, Math.cos(heading)); + prevRotationMatrix.set(2, 2, 1.0); + } + + @Override + public void setPose(Pose setPose) { + displacementPose = MathFunctions.subtractPoses(setPose, startPose); + resetEncoders(); + } + + @Override + public void update() { + deltaTimeNano = timer.getElapsedTime(); + timer.resetTimer(); + + updateEncoders(); + Matrix robotDeltas = getRobotDeltas(); + Matrix globalDeltas; + setPrevRotationMatrix(getPose().getHeading()); + + Matrix transformation = new Matrix(3,3); + if (Math.abs(robotDeltas.get(2, 0)) < 0.001) { + transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); + transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0); + transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0); + transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); + transformation.set(2, 2, 1.0); + } else { + transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); + transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0)); + transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0)); + transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); + transformation.set(2, 2, 1.0); + } + + globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas); + + displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0))); + currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9))); + + totalHeading += globalDeltas.get(2, 0); + } + + public void updateEncoders() { + forwardEncoder.update(); + strafeEncoder.update(); + + double currentIMUOrientation =MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); + deltaRadians = MathFunctions.getTurnDirection(previousIMUOrientation, currentIMUOrientation) * MathFunctions.getSmallestAngleDifference(currentIMUOrientation, previousIMUOrientation); + previousIMUOrientation = currentIMUOrientation; + } + + public void resetEncoders() { + forwardEncoder.reset(); + strafeEncoder.reset(); + } + + public Matrix getRobotDeltas() { + Matrix returnMatrix = new Matrix(3,1); + // x/forward movement + returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * (forwardEncoder.getDeltaPosition() - forwardEncoderPose.getY() * deltaRadians)); + //y/strafe movement + returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * deltaRadians)); + // theta/turning + returnMatrix.set(2,0, deltaRadians); + return returnMatrix; + } + + public double getTotalHeading() { + return totalHeading; + } + + public double getForwardMultiplier() { + return FORWARD_TICKS_TO_INCHES; + } + + public double getLateralMultiplier() { + return STRAFE_TICKS_TO_INCHES; + } + + public double getTurningMultiplier() { + return 1; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/FowardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/FowardTuner.java new file mode 100644 index 0000000..525d4ad --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/FowardTuner.java @@ -0,0 +1,49 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; + +/** + * This is the ForwardTuner OpMode. This tracks the forward movement of the robot and displays the + * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the + * robot's current distance in ticks to the specified distance in inches. So, to use this, run the + * tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're + * at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials + * and average the results. Then, input the multiplier into the forward ticks to inches in your + * localizer of choice. + * You can adjust the target distance on FTC Dashboard: 192/168/43/1:8080/dash + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 5/6/2024 + */ +@Config +@Autonomous(name = "Forward Localizer Tuner", group = "Autonomous Pathing Tuning") +public class FowardTuner extends OpMode { + private PoseUpdater poseUpdater; + + private Telemetry telemetryA; + + public static double DISTANCE = 30; + + @Override + public void init() { + poseUpdater = new PoseUpdater(hardwareMap); + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("Pull your robot forward " + DISTANCE + " inches. Your forward ticks to inches will be shown on the telemetry."); + telemetryA.update(); + } + + @Override + public void loop() { + poseUpdater.update(); + telemetryA.addData("distance moved", poseUpdater.getPose().getX()); + telemetryA.addLine("The multiplier will display what your forward ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); + telemetryA.addData("multiplier", DISTANCE / (poseUpdater.getPose().getX() / poseUpdater.getLocalizer().getForwardMultiplier())); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java new file mode 100644 index 0000000..2995a2e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java @@ -0,0 +1,49 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; + +/** + * This is the LateralTuner OpMode. This tracks the strafe movement of the robot and displays the + * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the + * robot's current distance in ticks to the specified distance in inches. So, to use this, run the + * tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're + * at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials + * and average the results. Then, input the multiplier into the strafe ticks to inches in your + * localizer of choice. + * You can adjust the target distance on FTC Dashboard: 192/168/43/1:8080/dash + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 5/6/2024 + */ +@Config +@Autonomous(name = "Lateral Localizer Tuner", group = "Autonomous Pathing Tuning") +public class LateralTuner extends OpMode { + private PoseUpdater poseUpdater; + + private Telemetry telemetryA; + + public static double DISTANCE = 30; + + @Override + public void init() { + poseUpdater = new PoseUpdater(hardwareMap); + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("Pull your robot to the right " + DISTANCE + " inches. Your strafe ticks to inches will be shown on the telemetry."); + telemetryA.update(); + } + + @Override + public void loop() { + poseUpdater.update(); + telemetryA.addData("distance moved", poseUpdater.getPose().getY()); + telemetryA.addLine("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); + telemetryA.addData("multiplier", DISTANCE / (poseUpdater.getPose().getX() / poseUpdater.getLocalizer().getLateralMultiplier())); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java new file mode 100644 index 0000000..a6c19e3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java @@ -0,0 +1,114 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; + +import java.util.Arrays; +import java.util.List; + +/** + * This is the LocalizationTest OpMode. This is basically just a simple mecanum drive attached to a + * PoseUpdater. The OpMode will print out the robot's pose to telemetry as well as draw the robot + * on FTC Dashboard (192/168/43/1:8080/dash). You should use this to check the robot's localization. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 5/6/2024 + */ +@Config +@TeleOp(group = "Pedro Pathing Tuning", name = "Localization Test") +public class LocalizationTest extends OpMode { + private PoseUpdater poseUpdater; + private Telemetry telemetryA; + + private DcMotorEx leftFront; + private DcMotorEx leftRear; + private DcMotorEx rightFront; + private DcMotorEx rightRear; + private List motors; + + @Override + public void init() { + poseUpdater = new PoseUpdater(hardwareMap); + + leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); + leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); + rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); + + leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + leftRear.setDirection(DcMotorSimple.Direction.REVERSE); + + motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear); + + for (DcMotorEx motor : motors) { + MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); + motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motor.setMotorType(motorConfigurationType); + } + + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + } + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("This will print your robot's position to telemetry while " + + "allowing robot control through a basic mecanum drive on gamepad 1."); + telemetryA.update(); + + TelemetryPacket packet = new TelemetryPacket(); + packet.fieldOverlay().setStroke("#3F51B5"); + Drawing.drawRobot(packet.fieldOverlay(), poseUpdater.getPose()); + FtcDashboard.getInstance().sendTelemetryPacket(packet); + } + + @Override + public void start() { + //poseUpdater.setPose(new Pose()); + } + + @Override + public void loop() { + poseUpdater.update(); + + double y = -gamepad1.left_stick_y; // Remember, this is reversed! + double x = gamepad1.left_stick_x; // this is strafing + double rx = gamepad1.right_stick_x; + + // Denominator is the largest motor power (absolute value) or 1 + // This ensures all the powers maintain the same ratio, but only when + // at least one is out of the range [-1, 1] + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + double leftFrontPower = (y + x + rx) / denominator; + double leftRearPower = (y - x + rx) / denominator; + double rightFrontPower = (y - x - rx) / denominator; + double rightRearPower = (y + x - rx) / denominator; + + leftFront.setPower(leftFrontPower); + leftRear.setPower(leftRearPower); + rightFront.setPower(rightFrontPower); + rightRear.setPower(rightRearPower); + + telemetryA.addData("x", poseUpdater.getPose().getX()); + telemetryA.addData("y", poseUpdater.getPose().getY()); + telemetryA.addData("heading", poseUpdater.getPose().getHeading()); + telemetryA.addData("total heading", poseUpdater.getTotalHeading()); + telemetryA.update(); + + TelemetryPacket packet = new TelemetryPacket(); + packet.fieldOverlay().setStroke("#3F51B5"); + Drawing.drawRobot(packet.fieldOverlay(), poseUpdater.getPose()); + FtcDashboard.getInstance().sendTelemetryPacket(packet); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java new file mode 100644 index 0000000..4b2838d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java @@ -0,0 +1,49 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; + +/** + * This is the TurnTuner OpMode. This tracks the turning movement of the robot and displays the + * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the + * robot's current angle in ticks to the specified angle in radians. So, to use this, run the + * tuner, then pull/push the robot to the specified angle using a protractor or lines on the ground. + * When you're at the end of the angle, record the ticks to inches multiplier. Feel free to run + * multiple trials and average the results. Then, input the multiplier into the turning ticks to + * radians in your localizer of choice. + * You can adjust the target angle on FTC Dashboard: 192/168/43/1:8080/dash + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 5/6/2024 + */ +@Config +@Autonomous(name = "Turn Localizer Tuner", group = "Autonomous Pathing Tuning") +public class TurnTuner extends OpMode { + private PoseUpdater poseUpdater; + + private Telemetry telemetryA; + + public static double ANGLE = 2 * Math.PI; + + @Override + public void init() { + poseUpdater = new PoseUpdater(hardwareMap); + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("Turn your robot " + ANGLE + " radians. Your turn ticks to inches will be shown on the telemetry."); + telemetryA.update(); + } + + @Override + public void loop() { + poseUpdater.update(); + telemetryA.addData("total angle", poseUpdater.getTotalHeading()); + telemetryA.addLine("The multiplier will display what your turn ticks to inches should be to scale your current angle to " + ANGLE + " radians."); + telemetryA.addData("multiplier", ANGLE / (poseUpdater.getTotalHeading() / poseUpdater.getLocalizer().getTurningMultiplier())); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java index 450da65..190bf8a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java @@ -164,7 +164,7 @@ public class BezierCurve { public double getCurvature(double t) { t = MathFunctions.clamp(t, 0, 1); Vector derivative = getDerivative(t); - Vector secondDerivative = new Vector(getSecondDerivative(t).getMagnitude(), getApproxSecondDerivative(t).getTheta()); + Vector secondDerivative = getSecondDerivative(t); if (derivative.getMagnitude() == 0) return 0; return (MathFunctions.crossProduct(derivative, secondDerivative))/Math.pow(derivative.getMagnitude(),3); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java index b66193a..0edfbfd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java @@ -1,6 +1,6 @@ package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; -import com.acmerobotics.roadrunner.geometry.Pose2d; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; /** * This is the MathFunctions class. This contains many useful math related methods that I use in @@ -105,24 +105,24 @@ public class MathFunctions { } /** - * This returns the distance between a Pose2d and a Point, + * This returns the distance between a Pose and a Point, * - * @param pose this is the Pose2d. + * @param pose this is the Pose. * @param point this is the Point. * @return returns the distance between the two. */ - public static double distance(Pose2d pose, Point point) { + public static double distance(Pose pose, Point point) { return Math.sqrt(Math.pow(pose.getX()-point.getX(), 2) + Math.pow(pose.getY()-point.getY(), 2)); } /** - * This returns the distance between a Pose2d and another Pose2d. + * This returns the distance between a Pose and another Pose. * - * @param one this is the first Pose2d. - * @param two this is the second Pose2d. + * @param one this is the first Pose. + * @param two this is the second Pose. * @return returns the distance between the two. */ - public static double distance(Pose2d one, Pose2d two) { + public static double distance(Pose one, Pose two) { return Math.sqrt(Math.pow(one.getX()-two.getX(), 2) + Math.pow(one.getY()-two.getY(), 2)); } @@ -137,6 +137,17 @@ public class MathFunctions { return new Point(one.getX() + two.getX(), one.getY() + two.getY(), Point.CARTESIAN); } + /** + * This returns a Pose that is the sum of the two input Pose. + * + * @param one the first Pose + * @param two the second Pose + * @return returns the sum of the two Pose. + */ + public static Pose addPoses(Pose one, Pose two) { + return new Pose(one.getX() + two.getX(), one.getY() + two.getY(), one.getHeading() + two.getHeading()); + } + /** * This subtracts the second Point from the first Point and returns the result as a Point. * Do note that order matters here. @@ -149,6 +160,18 @@ public class MathFunctions { return new Point(one.getX() - two.getX(), one.getY() - two.getY(), Point.CARTESIAN); } + /** + * This subtracts the second Pose from the first Pose and returns the result as a Pose. + * Do note that order matters here. + * + * @param one the first Pose. + * @param two the second Pose. + * @return returns the difference of the two Pose. + */ + public static Pose subtractPoses(Pose one, Pose two) { + return new Pose(one.getX() - two.getX(), one.getY() - two.getY(), one.getHeading() - two.getHeading()); + } + /** * This multiplies a Point by a scalar and returns the result as a Point * diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java index 563197d..5a2f453 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; -import com.acmerobotics.roadrunner.geometry.Pose2d; - +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants; import java.util.ArrayList; @@ -132,7 +131,7 @@ public class Path { * @param searchStepLimit the binary search step limit. * @return returns the closest Point. */ - public Pose2d getClosestPoint(Pose2d pose, int searchStepLimit) { + public Pose getClosestPoint(Pose pose, int searchStepLimit) { double lower = 0; double upper = 1; Point returnPoint; @@ -156,7 +155,7 @@ public class Path { closestPointCurvature = curve.getCurvature(closestPointTValue); - return new Pose2d(returnPoint.getX(), returnPoint.getY(), getClosestPointHeadingGoal()); + return new Pose(returnPoint.getX(), returnPoint.getY(), getClosestPointHeadingGoal()); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java index b37a140..7322447 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java @@ -1,6 +1,6 @@ package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; -import com.acmerobotics.roadrunner.geometry.Pose2d; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; /** * This is the Point class. This class handles storing information about the location of points in @@ -45,11 +45,11 @@ public class Point { } /** - * This creates a new Point from a Pose2d. + * This creates a new Point from a Pose. * - * @param pose the Pose2d. + * @param pose the Pose. */ - public Point(Pose2d pose) { + public Point(Pose pose) { setCoordinates(pose.getX(), pose.getY(), CARTESIAN); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java new file mode 100644 index 0000000..6d9af9c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java @@ -0,0 +1,39 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +import com.acmerobotics.dashboard.canvas.Canvas; + +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +/* +* Drawing class needed for putting your robot onto the dashboard +* Edit the radius needed to align to your robot the best possible simulation represented + */ +public class Drawing { + private Drawing() {} + + public static void drawRobot(Canvas c, Point t) { + final double ROBOT_RADIUS = 9; + + c.setStrokeWidth(1); + c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS); + + Vector halfv = new Vector(0.5*ROBOT_RADIUS, t.getTheta()); + Vector p1 = MathFunctions.addVectors(halfv, new Vector(t.getR(), t.getTheta())); + Vector p2 = MathFunctions.addVectors(p1, halfv); + c.strokeLine(p1.getXComponent(), p1.getYComponent(), p2.getXComponent(), p2.getYComponent()); + } + + public static void drawRobot(Canvas c, Pose t) { + final double ROBOT_RADIUS = 9; + + c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS); + Vector v = t.getHeadingVector(); + v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS); + double x1 = t.getX() + v.getXComponent() / 2, y1 = t.getY() + v.getYComponent() / 2; + double x2 = t.getX() + v.getXComponent(), y2 = t.getY() + v.getYComponent(); + c.strokeLine(x1, y1, x2, y2); + } +} From 2468dbb58ada6b9caec48d91e94611d884bdf798 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Thu, 9 May 2024 23:00:12 -0400 Subject: [PATCH 11/94] Adapter for Road Runner added for easier transition to Pedro Pathing localizer, as well as adding previously omitted files --- .../pedroPathing/localization/Localizer.java | 34 ++++++ .../pedroPathing/localization/Pose.java | 109 ++++++++++++++++++ .../RRToPedroThreeWheelLocalizer.java | 93 +++++++++++++++ 3 files changed, 236 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java new file mode 100644 index 0000000..4aa7702 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java @@ -0,0 +1,34 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +/** + * This is the Localizer class. It is an abstract superclass of all localizers used in Pedro Pathing, + * so it contains abstract methods that will have a concrete implementation in the subclasses. Any + * method that all localizers will need will be in this class. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 4/2/2024 + */ +public abstract class Localizer { + + public abstract Pose getPose(); + + public abstract Pose getVelocity(); + + public abstract Vector getVelocityVector(); + + public abstract void setStartPose(Pose setStart); + + public abstract void setPose(Pose setPose); + + public abstract void update(); + + public abstract double getTotalHeading(); + + public abstract double getForwardMultiplier(); + + public abstract double getLateralMultiplier(); + + public abstract double getTurningMultiplier(); +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java new file mode 100644 index 0000000..667196c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java @@ -0,0 +1,109 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +/** + * This is the Pose class. It defines poses in 2D space, like the Pose2D class in Road Runner except + * in the Pedro Pathing code so I don't have to import the Road Runner library. A Pose consists of + * two coordinates defining a position and a third value for the heading, so basically just defining + * any position and orientation the robot can be at, unless your robot can fly for whatever reason. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 4/2/2024 + */ +public class Pose { + private double x; + private double y; + private double heading; + + public Pose(double setX, double setY, double setHeading) { + setX(setX); + setY(setY); + setHeading(setHeading); + } + + public Pose(double setX, double setY) { + this(setX, setY, 0); + } + + public Pose() { + this(0,0,0); + } + + public void setX(double set) { + x = set; + } + + public void setY(double set) { + y = set; + } + + public void setHeading(double set) { + heading = MathFunctions.normalizeAngle(set); + } + + public double getX() { + return x; + } + + public double getY() { + return y; + } + + public double getHeading() { + return heading; + } + + public Vector getVector() { + Vector returnVector = new Vector(); + returnVector.setOrthogonalComponents(x, y); + return returnVector; + } + + public Vector getHeadingVector() { + return new Vector(1, heading); + } + + public void add(Pose pose) { + setX(x + pose.getX()); + setY(y + pose.getY()); + setHeading(heading + pose.getHeading()); + } + + public void subtract(Pose pose) { + setX(x - pose.getX()); + setY(y - pose.getY()); + setHeading(heading - pose.getHeading()); + } + + public void scalarMultiply(double scalar) { + setX(x * scalar); + setY(y * scalar); + setHeading(heading * scalar); + } + + public void scalarDivide(double scalar) { + setX(x / scalar); + setY(y / scalar); + setHeading(heading / scalar); + } + + public void flipSigns() { + setX(-x); + setY(-y); + setHeading(-heading); + } + + public boolean roughlyEquals(Pose pose, double accuracy) { + return MathFunctions.roughlyEquals(x, pose.getX(), accuracy) && MathFunctions.roughlyEquals(y, pose.getY(), accuracy) && MathFunctions.roughlyEquals(MathFunctions.getSmallestAngleDifference(heading, pose.getHeading()), 0, accuracy); + } + + public boolean roughlyEquals(Pose pose) { + return roughlyEquals(pose, 0.0001); + } + + public Pose copy() { + return new Pose(getX(), getY(), getHeading()); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java new file mode 100644 index 0000000..4df27fb --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java @@ -0,0 +1,93 @@ +//package org.firstinspires.ftc.teamcode.pedroPathing.localization; +// +//import com.acmerobotics.roadrunner.geometry.Pose2d; +//import com.qualcomm.robotcore.hardware.HardwareMap; +// +//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +// +//import java.util.ArrayList; +//import java.util.List; +// +///** +// * This is the RRToPedroThreeWheelLocalizer class. This class extends the Localizer superclass and +// * is intended to adapt the old Road Runner three wheel odometry localizer to the new Pedro Pathing +// * localizer system. +// * +// * @author Anyi Lin - 10158 Scott's Bots +// * @version 1.0, 5/9/2024 +// */ +//public class RRToPedroThreeWheelLocalizer extends Localizer { +// private RoadRunnerThreeWheelLocalizer localizer; +// private double totalHeading; +// private Pose startPose; +// private Pose previousPose; +// +// public RRToPedroThreeWheelLocalizer(HardwareMap hardwareMap) { +// List lastTrackingEncPositions = new ArrayList<>(); +// List lastTrackingEncVels = new ArrayList<>(); +// +// localizer = new RoadRunnerThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels); +// +// startPose = new Pose(); +// previousPose = new Pose(); +// } +// @Override +// public Pose getPose() { +// Pose2d pose = localizer.getPoseEstimate(); +// return new Pose(pose.getX(), pose.getY(), pose.getHeading()); +// } +// +// @Override +// public Pose getVelocity() { +// Pose2d pose = localizer.getPoseVelocity(); +// return new Pose(pose.getX(), pose.getY(), pose.getHeading()); +// } +// +// @Override +// public Vector getVelocityVector() { +// Pose2d pose = localizer.getPoseVelocity(); +// Vector returnVector = new Vector(); +// returnVector.setOrthogonalComponents(pose.getX(), pose.getY()); +// return returnVector; +// } +// +// @Override +// public void setStartPose(Pose setStart) { +// Pose oldStart = startPose; +// startPose = setStart; +// Pose startDiff = MathFunctions.subtractPoses(startPose, oldStart); +// localizer.setPoseEstimate(new Pose2d(getPose().getX() + startDiff.getX(), getPose().getY() + startDiff.getY(), getPose().getHeading() + startDiff.getHeading())); +// } +// +// @Override +// public void setPose(Pose setPose) { +// localizer.setPoseEstimate(new Pose2d(setPose.getX(), setPose.getY(), setPose.getHeading())); +// } +// +// @Override +// public void update() { +// totalHeading += MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(previousPose.getHeading(), getPose().getHeading()); +// previousPose = getPose(); +// } +// +// @Override +// public double getTotalHeading() { +// return totalHeading; +// } +// +// @Override +// public double getForwardMultiplier() { +// return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.X_MULTIPLIER; +// } +// +// @Override +// public double getLateralMultiplier() { +// return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.Y_MULTIPLIER; +// } +// +// @Override +// public double getTurningMultiplier() { +// return (getForwardMultiplier() + getLateralMultiplier()) / 2; +// } +//} From ccc74c8120a98fc792853e8a012539da1c9fc45d Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Thu, 9 May 2024 23:12:32 -0400 Subject: [PATCH 12/94] uncommented Road Runner adapter files --- .../RRToPedroThreeWheelLocalizer.java | 186 ++++++------- .../localization/RoadRunnerEncoder.java | 250 +++++++++--------- .../RoadRunnerThreeWheelLocalizer.java | 234 ++++++++-------- 3 files changed, 335 insertions(+), 335 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java index 4df27fb..5ad8cd1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java @@ -1,93 +1,93 @@ -//package org.firstinspires.ftc.teamcode.pedroPathing.localization; -// -//import com.acmerobotics.roadrunner.geometry.Pose2d; -//import com.qualcomm.robotcore.hardware.HardwareMap; -// -//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; -// -//import java.util.ArrayList; -//import java.util.List; -// -///** -// * This is the RRToPedroThreeWheelLocalizer class. This class extends the Localizer superclass and -// * is intended to adapt the old Road Runner three wheel odometry localizer to the new Pedro Pathing -// * localizer system. -// * -// * @author Anyi Lin - 10158 Scott's Bots -// * @version 1.0, 5/9/2024 -// */ -//public class RRToPedroThreeWheelLocalizer extends Localizer { -// private RoadRunnerThreeWheelLocalizer localizer; -// private double totalHeading; -// private Pose startPose; -// private Pose previousPose; -// -// public RRToPedroThreeWheelLocalizer(HardwareMap hardwareMap) { -// List lastTrackingEncPositions = new ArrayList<>(); -// List lastTrackingEncVels = new ArrayList<>(); -// -// localizer = new RoadRunnerThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels); -// -// startPose = new Pose(); -// previousPose = new Pose(); -// } -// @Override -// public Pose getPose() { -// Pose2d pose = localizer.getPoseEstimate(); -// return new Pose(pose.getX(), pose.getY(), pose.getHeading()); -// } -// -// @Override -// public Pose getVelocity() { -// Pose2d pose = localizer.getPoseVelocity(); -// return new Pose(pose.getX(), pose.getY(), pose.getHeading()); -// } -// -// @Override -// public Vector getVelocityVector() { -// Pose2d pose = localizer.getPoseVelocity(); -// Vector returnVector = new Vector(); -// returnVector.setOrthogonalComponents(pose.getX(), pose.getY()); -// return returnVector; -// } -// -// @Override -// public void setStartPose(Pose setStart) { -// Pose oldStart = startPose; -// startPose = setStart; -// Pose startDiff = MathFunctions.subtractPoses(startPose, oldStart); -// localizer.setPoseEstimate(new Pose2d(getPose().getX() + startDiff.getX(), getPose().getY() + startDiff.getY(), getPose().getHeading() + startDiff.getHeading())); -// } -// -// @Override -// public void setPose(Pose setPose) { -// localizer.setPoseEstimate(new Pose2d(setPose.getX(), setPose.getY(), setPose.getHeading())); -// } -// -// @Override -// public void update() { -// totalHeading += MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(previousPose.getHeading(), getPose().getHeading()); -// previousPose = getPose(); -// } -// -// @Override -// public double getTotalHeading() { -// return totalHeading; -// } -// -// @Override -// public double getForwardMultiplier() { -// return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.X_MULTIPLIER; -// } -// -// @Override -// public double getLateralMultiplier() { -// return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.Y_MULTIPLIER; -// } -// -// @Override -// public double getTurningMultiplier() { -// return (getForwardMultiplier() + getLateralMultiplier()) / 2; -// } -//} +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +import java.util.ArrayList; +import java.util.List; + +/** + * This is the RRToPedroThreeWheelLocalizer class. This class extends the Localizer superclass and + * is intended to adapt the old Road Runner three wheel odometry localizer to the new Pedro Pathing + * localizer system. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 5/9/2024 + */ +public class RRToPedroThreeWheelLocalizer extends Localizer { + private RoadRunnerThreeWheelLocalizer localizer; + private double totalHeading; + private Pose startPose; + private Pose previousPose; + + public RRToPedroThreeWheelLocalizer(HardwareMap hardwareMap) { + List lastTrackingEncPositions = new ArrayList<>(); + List lastTrackingEncVels = new ArrayList<>(); + + localizer = new RoadRunnerThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels); + + startPose = new Pose(); + previousPose = new Pose(); + } + @Override + public Pose getPose() { + Pose2d pose = localizer.getPoseEstimate(); + return new Pose(pose.getX(), pose.getY(), pose.getHeading()); + } + + @Override + public Pose getVelocity() { + Pose2d pose = localizer.getPoseVelocity(); + return new Pose(pose.getX(), pose.getY(), pose.getHeading()); + } + + @Override + public Vector getVelocityVector() { + Pose2d pose = localizer.getPoseVelocity(); + Vector returnVector = new Vector(); + returnVector.setOrthogonalComponents(pose.getX(), pose.getY()); + return returnVector; + } + + @Override + public void setStartPose(Pose setStart) { + Pose oldStart = startPose; + startPose = setStart; + Pose startDiff = MathFunctions.subtractPoses(startPose, oldStart); + localizer.setPoseEstimate(new Pose2d(getPose().getX() + startDiff.getX(), getPose().getY() + startDiff.getY(), getPose().getHeading() + startDiff.getHeading())); + } + + @Override + public void setPose(Pose setPose) { + localizer.setPoseEstimate(new Pose2d(setPose.getX(), setPose.getY(), setPose.getHeading())); + } + + @Override + public void update() { + totalHeading += MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(previousPose.getHeading(), getPose().getHeading()); + previousPose = getPose(); + } + + @Override + public double getTotalHeading() { + return totalHeading; + } + + @Override + public double getForwardMultiplier() { + return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.X_MULTIPLIER; + } + + @Override + public double getLateralMultiplier() { + return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.Y_MULTIPLIER; + } + + @Override + public double getTurningMultiplier() { + return (getForwardMultiplier() + getLateralMultiplier()) / 2; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java index 35b8789..edbe337 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java @@ -1,125 +1,125 @@ -//package org.firstinspires.ftc.teamcode.pedroPathing.localization; -// -//import com.acmerobotics.roadrunner.util.NanoClock; -//import com.qualcomm.robotcore.hardware.DcMotorEx; -//import com.qualcomm.robotcore.hardware.DcMotorSimple; -// -///** -// * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding -// * slot's motor direction -// */ -//public class RoadRunnerEncoder { -// private final static int CPS_STEP = 0x10000; -// -// private static double inverseOverflow(double input, double estimate) { -// // convert to uint16 -// int real = (int) input & 0xffff; -// // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits -// // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window -// real += ((real % 20) / 4) * CPS_STEP; -// // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by -// real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP; -// return real; -// } -// -// public enum Direction { -// FORWARD(1), -// REVERSE(-1); -// -// private int multiplier; -// -// Direction(int multiplier) { -// this.multiplier = multiplier; -// } -// -// public int getMultiplier() { -// return multiplier; -// } -// } -// -// private DcMotorEx motor; -// private NanoClock clock; -// -// private Direction direction; -// -// private int lastPosition; -// private int velocityEstimateIdx; -// private double[] velocityEstimates; -// private double lastUpdateTime; -// -// public RoadRunnerEncoder(DcMotorEx motor, NanoClock clock) { -// this.motor = motor; -// this.clock = clock; -// -// this.direction = Direction.FORWARD; -// -// this.lastPosition = 0; -// this.velocityEstimates = new double[3]; -// this.lastUpdateTime = clock.seconds(); -// } -// -// public RoadRunnerEncoder(DcMotorEx motor) { -// this(motor, NanoClock.system()); -// } -// -// public Direction getDirection() { -// return direction; -// } -// -// private int getMultiplier() { -// return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1); -// } -// -// /** -// * Allows you to set the direction of the counts and velocity without modifying the motor's direction state -// * @param direction either reverse or forward depending on if encoder counts should be negated -// */ -// public void setDirection(Direction direction) { -// this.direction = direction; -// } -// -// /** -// * Gets the position from the underlying motor and adjusts for the set direction. -// * Additionally, this method updates the velocity estimates used for compensated velocity -// * -// * @return encoder position -// */ -// public int getCurrentPosition() { -// int multiplier = getMultiplier(); -// int currentPosition = motor.getCurrentPosition() * multiplier; -// if (currentPosition != lastPosition) { -// double currentTime = clock.seconds(); -// double dt = currentTime - lastUpdateTime; -// velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt; -// velocityEstimateIdx = (velocityEstimateIdx + 1) % 3; -// lastPosition = currentPosition; -// lastUpdateTime = currentTime; -// } -// return currentPosition; -// } -// -// /** -// * Gets the velocity directly from the underlying motor and compensates for the direction -// * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore) -// * -// * @return raw velocity -// */ -// public double getRawVelocity() { -// int multiplier = getMultiplier(); -// return motor.getVelocity() * multiplier; -// } -// -// /** -// * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity -// * that are lost in overflow due to velocity being transmitted as 16 bits. -// * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly. -// * -// * @return corrected velocity -// */ -// public double getCorrectedVelocity() { -// double median = velocityEstimates[0] > velocityEstimates[1] -// ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2])) -// : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2])); -// return inverseOverflow(getRawVelocity(), median); -// } -//} +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +/** + * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding + * slot's motor direction + */ +public class RoadRunnerEncoder { + private final static int CPS_STEP = 0x10000; + + private static double inverseOverflow(double input, double estimate) { + // convert to uint16 + int real = (int) input & 0xffff; + // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits + // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window + real += ((real % 20) / 4) * CPS_STEP; + // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by + real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP; + return real; + } + + public enum Direction { + FORWARD(1), + REVERSE(-1); + + private int multiplier; + + Direction(int multiplier) { + this.multiplier = multiplier; + } + + public int getMultiplier() { + return multiplier; + } + } + + private DcMotorEx motor; + private NanoClock clock; + + private Direction direction; + + private int lastPosition; + private int velocityEstimateIdx; + private double[] velocityEstimates; + private double lastUpdateTime; + + public RoadRunnerEncoder(DcMotorEx motor, NanoClock clock) { + this.motor = motor; + this.clock = clock; + + this.direction = Direction.FORWARD; + + this.lastPosition = 0; + this.velocityEstimates = new double[3]; + this.lastUpdateTime = clock.seconds(); + } + + public RoadRunnerEncoder(DcMotorEx motor) { + this(motor, NanoClock.system()); + } + + public Direction getDirection() { + return direction; + } + + private int getMultiplier() { + return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1); + } + + /** + * Allows you to set the direction of the counts and velocity without modifying the motor's direction state + * @param direction either reverse or forward depending on if encoder counts should be negated + */ + public void setDirection(Direction direction) { + this.direction = direction; + } + + /** + * Gets the position from the underlying motor and adjusts for the set direction. + * Additionally, this method updates the velocity estimates used for compensated velocity + * + * @return encoder position + */ + public int getCurrentPosition() { + int multiplier = getMultiplier(); + int currentPosition = motor.getCurrentPosition() * multiplier; + if (currentPosition != lastPosition) { + double currentTime = clock.seconds(); + double dt = currentTime - lastUpdateTime; + velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt; + velocityEstimateIdx = (velocityEstimateIdx + 1) % 3; + lastPosition = currentPosition; + lastUpdateTime = currentTime; + } + return currentPosition; + } + + /** + * Gets the velocity directly from the underlying motor and compensates for the direction + * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore) + * + * @return raw velocity + */ + public double getRawVelocity() { + int multiplier = getMultiplier(); + return motor.getVelocity() * multiplier; + } + + /** + * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity + * that are lost in overflow due to velocity being transmitted as 16 bits. + * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly. + * + * @return corrected velocity + */ + public double getCorrectedVelocity() { + double median = velocityEstimates[0] > velocityEstimates[1] + ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2])) + : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2])); + return inverseOverflow(getRawVelocity(), median); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java index ce879cc..93d13bb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java @@ -1,117 +1,117 @@ -//package org.firstinspires.ftc.teamcode.pedroPathing.localization; -// -//import androidx.annotation.NonNull; -// -//import com.acmerobotics.dashboard.config.Config; -//import com.acmerobotics.roadrunner.geometry.Pose2d; -//import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; -//import com.qualcomm.robotcore.hardware.DcMotorEx; -//import com.qualcomm.robotcore.hardware.HardwareMap; -// -//import java.util.Arrays; -//import java.util.List; -// -///* -// * Sample tracking wheel localizer implementation assuming the standard configuration: -// * -// * left on robot is y pos -// * -// * front of robot is x pos -// * -// * /--------------\ -// * | ____ | -// * | ---- | -// * | || || | -// * | || || | -// * | | -// * | | -// * \--------------/ -// * -// */ -// -///** -// * This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will -// * be replaced with a custom localizer. -// */ -//@Config -//public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer { -// public static double TICKS_PER_REV = 8192; -// public static double WHEEL_RADIUS = 1.37795; // in -// public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed -// -// public static double X_MULTIPLIER = 0.5008239963; -// public static double Y_MULTIPLIER = 0.5018874659; -// -// public static double leftX = -18.5/25.4 - 0.1, leftY = 164.4/25.4, rightX = -18.4/25.4 - 0.1, rightY = -159.6/25.4, strafeX = -107.9/25.4+0.25, strafeY = -1.1/25.4-0.23; -// -// private RoadRunnerEncoder leftEncoder, rightEncoder, strafeEncoder; -// -// private List lastEncPositions, lastEncVels; -// -// public RoadRunnerThreeWheelLocalizer(HardwareMap hardwareMap, List lastTrackingEncPositions, List lastTrackingEncVels) { -// super(Arrays.asList( -// new Pose2d(leftX, leftY, 0), // left -// new Pose2d(rightX, rightY, 0), // right -// new Pose2d(strafeX, strafeY, Math.toRadians(90)) // strafe -// )); -// -// lastEncPositions = lastTrackingEncPositions; -// lastEncVels = lastTrackingEncVels; -// -// // TODO: redo the configs here -// leftEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "leftRear")); -// rightEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "rightFront")); -// strafeEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); -// -// // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) -// leftEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); -// rightEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); -// strafeEncoder.setDirection(RoadRunnerEncoder.Direction.FORWARD); -// } -// -// public void resetHeading(double heading) { -// setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading)); -// } -// -// public static double encoderTicksToInches(double ticks) { -// return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; -// } -// -// @NonNull -// @Override -// public List getWheelPositions() { -// int leftPos = leftEncoder.getCurrentPosition(); -// int rightPos = rightEncoder.getCurrentPosition(); -// int frontPos = strafeEncoder.getCurrentPosition(); -// -// lastEncPositions.clear(); -// lastEncPositions.add(leftPos); -// lastEncPositions.add(rightPos); -// lastEncPositions.add(frontPos); -// -// return Arrays.asList( -// encoderTicksToInches(leftPos) * X_MULTIPLIER, -// encoderTicksToInches(rightPos) * X_MULTIPLIER, -// encoderTicksToInches(frontPos) * Y_MULTIPLIER -// ); -// } -// -// @NonNull -// @Override -// public List getWheelVelocities() { -// int leftVel = (int) leftEncoder.getCorrectedVelocity(); -// int rightVel = (int) rightEncoder.getCorrectedVelocity(); -// int frontVel = (int) strafeEncoder.getCorrectedVelocity(); -// -// lastEncVels.clear(); -// lastEncVels.add(leftVel); -// lastEncVels.add(rightVel); -// lastEncVels.add(frontVel); -// -// return Arrays.asList( -// encoderTicksToInches(leftVel) * X_MULTIPLIER, -// encoderTicksToInches(rightVel) * X_MULTIPLIER, -// encoderTicksToInches(frontVel) * Y_MULTIPLIER -// ); -// } -//} +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import java.util.Arrays; +import java.util.List; + +/* + * Sample tracking wheel localizer implementation assuming the standard configuration: + * + * left on robot is y pos + * + * front of robot is x pos + * + * /--------------\ + * | ____ | + * | ---- | + * | || || | + * | || || | + * | | + * | | + * \--------------/ + * + */ + +/** + * This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will + * be replaced with a custom localizer. + */ +@Config +public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer { + public static double TICKS_PER_REV = 8192; + public static double WHEEL_RADIUS = 1.37795; // in + public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed + + public static double X_MULTIPLIER = 0.5008239963; + public static double Y_MULTIPLIER = 0.5018874659; + + public static double leftX = -18.5/25.4 - 0.1, leftY = 164.4/25.4, rightX = -18.4/25.4 - 0.1, rightY = -159.6/25.4, strafeX = -107.9/25.4+0.25, strafeY = -1.1/25.4-0.23; + + private RoadRunnerEncoder leftEncoder, rightEncoder, strafeEncoder; + + private List lastEncPositions, lastEncVels; + + public RoadRunnerThreeWheelLocalizer(HardwareMap hardwareMap, List lastTrackingEncPositions, List lastTrackingEncVels) { + super(Arrays.asList( + new Pose2d(leftX, leftY, 0), // left + new Pose2d(rightX, rightY, 0), // right + new Pose2d(strafeX, strafeY, Math.toRadians(90)) // strafe + )); + + lastEncPositions = lastTrackingEncPositions; + lastEncVels = lastTrackingEncVels; + + // TODO: redo the configs here + leftEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "leftRear")); + rightEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "rightFront")); + strafeEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); + + // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) + leftEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); + rightEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); + strafeEncoder.setDirection(RoadRunnerEncoder.Direction.FORWARD); + } + + public void resetHeading(double heading) { + setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading)); + } + + public static double encoderTicksToInches(double ticks) { + return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; + } + + @NonNull + @Override + public List getWheelPositions() { + int leftPos = leftEncoder.getCurrentPosition(); + int rightPos = rightEncoder.getCurrentPosition(); + int frontPos = strafeEncoder.getCurrentPosition(); + + lastEncPositions.clear(); + lastEncPositions.add(leftPos); + lastEncPositions.add(rightPos); + lastEncPositions.add(frontPos); + + return Arrays.asList( + encoderTicksToInches(leftPos) * X_MULTIPLIER, + encoderTicksToInches(rightPos) * X_MULTIPLIER, + encoderTicksToInches(frontPos) * Y_MULTIPLIER + ); + } + + @NonNull + @Override + public List getWheelVelocities() { + int leftVel = (int) leftEncoder.getCorrectedVelocity(); + int rightVel = (int) rightEncoder.getCorrectedVelocity(); + int frontVel = (int) strafeEncoder.getCorrectedVelocity(); + + lastEncVels.clear(); + lastEncVels.add(leftVel); + lastEncVels.add(rightVel); + lastEncVels.add(frontVel); + + return Arrays.asList( + encoderTicksToInches(leftVel) * X_MULTIPLIER, + encoderTicksToInches(rightVel) * X_MULTIPLIER, + encoderTicksToInches(frontVel) * Y_MULTIPLIER + ); + } +} From 168537cb2882f4b93471141577a1385615a01f63 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Mon, 13 May 2024 23:15:20 -0400 Subject: [PATCH 13/94] added comments to all new files and increased FTC Dashboard field drawing functionality, will add a readme for the localization section --- .../pedroPathing/follower/Follower.java | 51 ++++--- .../localization/DriveEncoderLocalizer.java | 86 ++++++++++++ .../pedroPathing/localization/Encoder.java | 35 ++++- .../pedroPathing/localization/Localizer.java | 55 ++++++++ .../pedroPathing/localization/Matrix.java | 121 +++++++++++++++++ .../pedroPathing/localization/Pose.java | 102 ++++++++++++++ .../localization/PoseUpdater.java | 18 ++- .../RRToPedroThreeWheelLocalizer.java | 66 +++++++++ .../localization/RoadRunnerEncoder.java | 11 +- .../RoadRunnerThreeWheelLocalizer.java | 8 +- .../localization/ThreeWheelLocalizer.java | 86 ++++++++++++ .../localization/TwoWheelLocalizer.java | 86 ++++++++++++ .../localization/tuning/FowardTuner.java | 23 ++++ .../localization/tuning/LateralTuner.java | 23 ++++ .../localization/tuning/LocalizationTest.java | 31 +++-- .../localization/tuning/TurnTuner.java | 22 +++ .../pathGeneration/BezierCurve.java | 43 +++++- .../pathGeneration/BezierLine.java | 1 + .../pathGeneration/BezierPoint.java | 1 + .../pedroPathing/pathGeneration/Path.java | 10 ++ .../pedroPathing/pathGeneration/Point.java | 10 ++ .../tuning/FollowerConstants.java | 6 +- .../util/DashboardPoseTracker.java | 76 +++++++++++ .../teamcode/pedroPathing/util/Drawing.java | 128 +++++++++++++++++- 24 files changed, 1042 insertions(+), 57 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index 3a7fa37..16a9819 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -12,10 +12,8 @@ import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstan import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.smallTranslationalPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFSwitch; -import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; @@ -34,6 +32,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; import org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants; +import org.firstinspires.ftc.teamcode.pedroPathing.util.DashboardPoseTracker; import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; import org.firstinspires.ftc.teamcode.pedroPathing.util.PIDFController; @@ -63,6 +62,7 @@ public class Follower { private DriveVectorScaler driveVectorScaler; private PoseUpdater poseUpdater; + private DashboardPoseTracker dashboardPoseTracker; private Pose closestPose; @@ -122,7 +122,7 @@ public class Follower { private PIDFController largeDrivePIDF = new PIDFController(FollowerConstants.largeDrivePIDFCoefficients); - public static boolean drawStuff = true; + public static boolean drawOnDashboard = true; public static boolean useTranslational = true; public static boolean useCentripetal = true; public static boolean useHeading = true; @@ -189,7 +189,8 @@ public class Follower { accelerations.add(new Vector()); } calculateAveragedVelocityAndAcceleration(); - draw(); + + dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); } /** @@ -427,6 +428,11 @@ public class Follower { */ public void update() { poseUpdater.update(); + + if (drawOnDashboard) { + dashboardPoseTracker.update(); + } + if (auto) { if (holdingPosition) { closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1); @@ -495,14 +501,6 @@ public class Follower { motors.get(i).setPower(drivePowers[i]); } } - draw(); - } - - public void draw() { - TelemetryPacket packet = new TelemetryPacket(); - packet.fieldOverlay().setStroke("#3F51B5"); - Drawing.drawRobot(packet.fieldOverlay(), getPose()); - FtcDashboard.getInstance().sendTelemetryPacket(packet); } /** @@ -893,14 +891,9 @@ public class Follower { telemetry.addData("velocity magnitude", getVelocity().getMagnitude()); telemetry.addData("velocity heading", getVelocity().getTheta()); telemetry.update(); - TelemetryPacket packet = new TelemetryPacket(); - //Draws the current target path that the robot would like to follow - packet.fieldOverlay().setStroke("#4CAF50"); - Drawing.drawRobot(packet.fieldOverlay(), new Pose(getPose().getX(), getPose().getY(), getPose().getHeading())); - //Draws the current path that the robot is following - packet.fieldOverlay().setStroke("#3F51B5"); - if (currentPath != null) Drawing.drawRobot(packet.fieldOverlay(), getPointFromPath(currentPath.getClosestPointTValue())); - FtcDashboard.getInstance().sendTelemetryPacket(packet); + if (drawOnDashboard) { + Drawing.drawDebug(this); + } } /** @@ -921,4 +914,22 @@ public class Follower { public double getTotalHeading() { return poseUpdater.getTotalHeading(); } + + /** + * This returns the current Path the Follower is following. This can be null. + * + * @return returns the current Path. + */ + public Path getCurrentPath() { + return currentPath; + } + + /** + * This returns the pose tracker for the robot to draw on the Dashboard. + * + * @return returns the pose tracker + */ + public DashboardPoseTracker getDashboardPoseTracker() { + return dashboardPoseTracker; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java index 3a10761..15501cc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java @@ -35,10 +35,23 @@ public class DriveEncoderLocalizer extends Localizer { // todo: make drive encod public static double ROBOT_WIDTH = 1; public static double ROBOT_LENGTH = 1; + /** + * This creates a new DriveEncoderLocalizer from a HardwareMap, with a starting Pose at (0,0) + * facing 0 heading. + * + * @param map the HardwareMap + */ public DriveEncoderLocalizer(HardwareMap map) { this(map, new Pose()); } + /** + * This creates a new DriveEncoderLocalizer from a HardwareMap and a Pose, with the Pose + * specifying the starting pose of the localizer. + * + * @param map the HardwareMap + * @param setStartPose the Pose to start from + */ public DriveEncoderLocalizer(HardwareMap map, Pose setStartPose) { hardwareMap = map; @@ -62,26 +75,52 @@ public class DriveEncoderLocalizer extends Localizer { // todo: make drive encod currentVelocity = new Pose(); } + /** + * This returns the current pose estimate. + * + * @return returns the current pose estimate as a Pose + */ @Override public Pose getPose() { return MathFunctions.addPoses(startPose, displacementPose); } + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Pose + */ @Override public Pose getVelocity() { return currentVelocity.copy(); } + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Vector + */ @Override public Vector getVelocityVector() { return currentVelocity.getVector(); } + /** + * This sets the start pose. Changing the start pose should move the robot as if all its + * previous movements were displacing it from its new start pose. + * + * @param setStart the new start pose + */ @Override public void setStartPose(Pose setStart) { startPose = setStart; } + /** + * This sets the Matrix that contains the previous pose's heading rotation. + * + * @param heading the rotation of the Matrix + */ public void setPrevRotationMatrix(double heading) { prevRotationMatrix = new Matrix(3,3); prevRotationMatrix.set(0, 0, Math.cos(heading)); @@ -91,12 +130,23 @@ public class DriveEncoderLocalizer extends Localizer { // todo: make drive encod prevRotationMatrix.set(2, 2, 1.0); } + /** + * This sets the current pose estimate. Changing this should just change the robot's current + * pose estimate, not anything to do with the start pose. + * + * @param setPose the new current pose estimate + */ @Override public void setPose(Pose setPose) { displacementPose = MathFunctions.subtractPoses(setPose, startPose); resetEncoders(); } + /** + * This updates the elapsed time timer that keeps track of time between updates, as well as the + * change position of the Encoders. Then, the robot's global change in position is calculated + * using the pose exponential method. + */ @Override public void update() { deltaTimeNano = timer.getElapsedTime(); @@ -130,6 +180,9 @@ public class DriveEncoderLocalizer extends Localizer { // todo: make drive encod totalHeading += globalDeltas.get(2, 0); } + /** + * This updates the Encoders. + */ public void updateEncoders() { leftFront.update(); rightFront.update(); @@ -137,6 +190,9 @@ public class DriveEncoderLocalizer extends Localizer { // todo: make drive encod rightRear.update(); } + /** + * This resets the Encoders. + */ public void resetEncoders() { leftFront.reset(); rightFront.reset(); @@ -144,6 +200,12 @@ public class DriveEncoderLocalizer extends Localizer { // todo: make drive encod rightRear.reset(); } + /** + * This calculates the change in position from the perspective of the robot using information + * from the Encoders. + * + * @return returns a Matrix containing the robot relative movement. + */ public Matrix getRobotDeltas() { Matrix returnMatrix = new Matrix(3,1); // x/forward movement @@ -155,18 +217,42 @@ public class DriveEncoderLocalizer extends Localizer { // todo: make drive encod return returnMatrix; } + /** + * This returns how far the robot has turned in radians, in a number not clamped between 0 and + * 2 * pi radians. This is used for some tuning things and nothing actually within the following. + * + * @return returns how far the robot has turned in total, in radians. + */ public double getTotalHeading() { return totalHeading; } + /** + * This returns the multiplier applied to forward movement measurement to convert from encoder + * ticks to inches. This is found empirically through a tuner. + * + * @return returns the forward ticks to inches multiplier + */ public double getForwardMultiplier() { return FORWARD_TICKS_TO_INCHES; } + /** + * This returns the multiplier applied to lateral/strafe movement measurement to convert from + * encoder ticks to inches. This is found empirically through a tuner. + * + * @return returns the lateral/strafe ticks to inches multiplier + */ public double getLateralMultiplier() { return STRAFE_TICKS_TO_INCHES; } + /** + * This returns the multiplier applied to turning movement measurement to convert from encoder + * ticks to radians. This is found empirically through a tuner. + * + * @return returns the turning ticks to radians multiplier + */ public double getTurningMultiplier() { return TURN_TICKS_TO_RADIANS; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java index 3f8f72f..573c4cf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java @@ -5,7 +5,8 @@ import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; /** - * This is the Encoder class. + * This is the Encoder class. This tracks the position of a motor of class DcMotorEx. The motor + * must have an encoder attached. It can also get changes in position. * * @author Anyi Lin - 10158 Scott's Bots * @version 1.0, 4/2/2024 @@ -18,16 +19,31 @@ public class Encoder { public final static double FORWARD = 1, REVERSE = -1; + /** + * This creates a new Encoder from a DcMotorEx. + * + * @param setMotor the motor this will be tracking + */ public Encoder(DcMotorEx setMotor) { motor = setMotor; multiplier = FORWARD; reset(); } + /** + * This sets the direction/multiplier of the Encoder. Setting 1 or -1 will make the Encoder track + * forward or in reverse, respectively. Any multiple of either one will scale the Encoder's output + * by that amount. + * + * @param setMultiplier the multiplier/direction to set + */ public void setDirection(double setMultiplier) { multiplier = setMultiplier; } + /** + * This resets the Encoder's position and the current and previous position in the code. + */ public void reset() { motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); previousPosition = motor.getCurrentPosition(); @@ -35,15 +51,32 @@ public class Encoder { motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); } + /** + * This updates the Encoder's tracked current position and previous position. + */ public void update() { previousPosition = currentPosition; currentPosition = motor.getCurrentPosition(); } + /** + * This returns the multiplier/direction of the Encoder. + * + * @return returns the multiplier + */ public double getMultiplier() { return multiplier * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1); } + /** + * This returns the change in position from the previous position to the current position. One + * important thing to note is that this encoder does not track velocity, only change in position. + * This is because I am using a pose exponential method of localization, which doesn't need the + * velocity of the encoders. Velocity of the robot is calculated in the localizer using an elapsed + * time timer there. + * + * @return returns the change in position of the Encoder + */ public double getDeltaPosition() { return getMultiplier() * (currentPosition - previousPosition); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java index 4aa7702..2fa2173 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java @@ -12,23 +12,78 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; */ public abstract class Localizer { + /** + * This returns the current pose estimate from the Localizer. + * + * @return returns the pose as a Pose object. + */ public abstract Pose getPose(); + /** + * This returns the current velocity estimate from the Localizer. + * + * @return returns the velocity as a Pose object. + */ public abstract Pose getVelocity(); + /** + * This returns the current velocity estimate from the Localizer as a Vector. + * + * @return returns the velocity as a Vector. + */ public abstract Vector getVelocityVector(); + /** + * This sets the start pose of the Localizer. Changing the start pose should move the robot as if + * all its previous movements were displacing it from its new start pose. + * + * @param setStart the new start pose + */ public abstract void setStartPose(Pose setStart); + /** + * This sets the current pose estimate of the Localizer. Changing this should just change the + * robot's current pose estimate, not anything to do with the start pose. + * + * @param setPose the new current pose estimate + */ public abstract void setPose(Pose setPose); + /** + * This calls an update to the Localizer, updating the current pose estimate and current velocity + * estimate. + */ public abstract void update(); + /** + * This returns how far the robot has turned in radians, in a number not clamped between 0 and + * 2 * pi radians. This is used for some tuning things and nothing actually within the following. + * + * @return returns how far the robot has turned in total, in radians. + */ public abstract double getTotalHeading(); + /** + * This returns the multiplier applied to forward movement measurement to convert from encoder + * ticks to inches. This is found empirically through a tuner. + * + * @return returns the forward ticks to inches multiplier + */ public abstract double getForwardMultiplier(); + /** + * This returns the multiplier applied to lateral/strafe movement measurement to convert from + * encoder ticks to inches. This is found empirically through a tuner. + * + * @return returns the lateral/strafe ticks to inches multiplier + */ public abstract double getLateralMultiplier(); + /** + * This returns the multiplier applied to turning movement measurement to convert from encoder + * ticks to radians. This is found empirically through a tuner. + * + * @return returns the turning ticks to radians multiplier + */ public abstract double getTurningMultiplier(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java index a16a3db..c93ab7e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java @@ -14,22 +14,49 @@ import java.util.Arrays; public class Matrix { private double[][] matrix; + /** + * This creates a new Matrix of width and height 0. + */ public Matrix() { matrix = new double[0][0]; } + /** + * This creates a new Matrix with a specified width and height. + * + * @param rows the number of rows, or height + * @param columns the number of columns, or width + */ public Matrix(int rows, int columns) { matrix = new double[rows][columns]; } + /** + * This creates a new Matrix from a 2D matrix of doubles. Please only enter rectangular 2D + * Arrays of doubles or else things mess up. + * + * @param setMatrix the 2D Array of doubles + */ public Matrix(double[][] setMatrix) { setMatrix(setMatrix); } + /** + * This creates a new Matrix from another Matrix. + * + * @param setMatrix the Matrix input. + */ public Matrix(Matrix setMatrix) { setMatrix(setMatrix); } + /** + * This creates a copy of a 2D Array of doubles that references entirely new memory locations + * from the original 2D Array of doubles, so no issues with mutability. + * + * @param copyMatrix the 2D Array of doubles to copy + * @return returns a deep copy of the input Array + */ public static double[][] deepCopy(double[][] copyMatrix) { double[][] returnMatrix = new double[copyMatrix.length][copyMatrix[0].length]; for (int i = 0; i < copyMatrix.length; i++) { @@ -38,30 +65,72 @@ public class Matrix { return returnMatrix; } + /** + * This returns a deep copy of the 2D Array that this Matrix is based on. + * + * @return returns the 2D Array of doubles this Matrix is built on + */ public double[][] getMatrix() { return deepCopy(matrix); } + /** + * This returns a specified row of the Matrix in the form of an Array of doubles. + * + * @param row the index of the row to return + * @return returns the row of the Matrix specified + */ public double[] get(int row) { return Arrays.copyOf(matrix[row], matrix[row].length); } + /** + * This returns a specified element of the Matrix as a double. + * + * @param row the index of the row of the element + * @param column the index of the column of the element + * @return returns the element of the Matrix specified + */ public double get(int row, int column) { return get(row)[column]; } + /** + * This returns the number of rows of the Matrix, as determined by the length of the 2D Array. + * If the Matrix/2D Array is not rectangular, issues arise. + * + * @return returns the number of rows in the Matrix + */ public int getRows() { return matrix.length; } + /** + * This returns the number of columns of the Matrix, as determined by the length of the first Array + * in the 2D Array. If the Matrix/2D Array is not rectangular, issues arise. + * + * @return returns the number of columns in the Matrix + */ public int getColumns() { return matrix[0].length; } + /** + * This sets the 2D Array of this Matrix to a copy of the 2D Array of another Matrix. + * + * @param setMatrix the Matrix to copy from + * @return returns if the operation was successful + */ public boolean setMatrix(Matrix setMatrix) { return setMatrix(setMatrix.getMatrix()); } + /** + * This sets the 2D Array of this Matrix to a copy of a specified 2D Array. + * + * @param setMatrix the 2D Array to copy from + * @return returns if the operation was successful + */ public boolean setMatrix(double[][] setMatrix) { int columns = setMatrix[0].length; for (int i = 0; i < setMatrix.length; i++) { @@ -73,6 +142,13 @@ public class Matrix { return true; } + /** + * This sets a row of the Matrix to a copy of a specified Array of doubles. + * + * @param row the row to be written over + * @param input the Array input + * @return returns if the operation was successful + */ public boolean set(int row, double[] input) { if (input.length != getColumns()) { return false; @@ -81,11 +157,25 @@ public class Matrix { return true; } + /** + * This sets a specified element of the Matrix to an input value. + * + * @param row the index of the row of the specified element + * @param column the index of the column of the specified element + * @param input the input value + * @return returns if the operation was successful + */ public boolean set(int row, int column, double input) { matrix[row][column] = input; return true; } + /** + * This adds a Matrix to this Matrix. + * + * @param input the Matrix to add to this. Nothing will change in this Matrix + * @return returns if the operation was successful + */ public boolean add(Matrix input) { if (input.getRows() == getRows() && input.getColumns() == getColumns()) { for (int i = 0; i < getRows(); i++) { @@ -98,6 +188,12 @@ public class Matrix { return false; } + /** + * This subtracts a Matrix from this Matrix. + * + * @param input the Matrix to subtract from this. Nothing will change in this Matrix + * @return returns if the operation was successful + */ public boolean subtract(Matrix input) { if (input.getRows() == getRows() && input.getColumns() == getColumns()) { for (int i = 0; i < getRows(); i++) { @@ -110,6 +206,12 @@ public class Matrix { return false; } + /** + * This multiplies this Matrix with a scalar. + * + * @param scalar the scalar number + * @return returns if the operation was successful + */ public boolean scalarMultiply(double scalar) { for (int i = 0; i < getRows(); i++) { for (int j = 0; j < getColumns(); j++) { @@ -119,10 +221,21 @@ public class Matrix { return true; } + /** + * This multiplies the Matrix by -1, flipping the signs of all the elements within. + * + * @return returns if the operation was successful + */ public boolean flipSigns() { return scalarMultiply(-1); } + /** + * This multiplies a Matrix to this Matrix. + * + * @param input the Matrix to multiply to this. Nothing will change in this Matrix + * @return returns if the operation was successful + */ public boolean multiply(Matrix input) { if (getColumns() == input.getRows()) { Matrix product = new Matrix(getRows(), input.getColumns()); @@ -141,6 +254,14 @@ public class Matrix { return false; } + /** + * This multiplies a Matrix to another Matrix. This will not change any data in the two input + * Matrices. + * + * @param one the first Matrix to multiply. + * @param two the second Matrix to multiply + * @return returns if the operation was successful + */ public static Matrix multiply(Matrix one, Matrix two) { Matrix returnMatrix = new Matrix(one); if (returnMatrix.multiply(two)) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java index 667196c..d5ac720 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java @@ -17,92 +17,194 @@ public class Pose { private double y; private double heading; + /** + * This creates a new Pose from a x, y, and heading inputs. + * + * @param setX the initial x value + * @param setY the initial y value + * @param setHeading the initial heading value + */ public Pose(double setX, double setY, double setHeading) { setX(setX); setY(setY); setHeading(setHeading); } + /** + * This creates a new Pose from x and y inputs. The heading is set to 0. + * + * @param setX the initial x value + * @param setY the initial y value + */ public Pose(double setX, double setY) { this(setX, setY, 0); } + /** + * This creates a new Pose with no inputs and 0 for all values. + */ public Pose() { this(0,0,0); } + /** + * This sets the x value. + * + * @param set the x value + */ public void setX(double set) { x = set; } + /** + * This sets the y value. + * + * @param set the y value + */ public void setY(double set) { y = set; } + /** + * This sets the heading value. + * + * @param set the heading value + */ public void setHeading(double set) { heading = MathFunctions.normalizeAngle(set); } + /** + * This returns the x value. + * + * @return returns the x value + */ public double getX() { return x; } + /** + * This returns the y value. + * + * @return returns the y value + */ public double getY() { return y; } + /** + * This returns the heading value. + * + * @return returns the heading value + */ public double getHeading() { return heading; } + /** + * This returns the Pose as a Vector. Naturally, the heading data in the Pose cannot be included + * in the Vector. + * + * @return returns the Pose as a Vector + */ public Vector getVector() { Vector returnVector = new Vector(); returnVector.setOrthogonalComponents(x, y); return returnVector; } + /** + * This returns a new Vector with magnitude 1 pointing in the direction of the heading. + * + * @return returns a unit Vector in the direction of the heading + */ public Vector getHeadingVector() { return new Vector(1, heading); } + /** + * This adds all the values of an input Pose to this Pose. The input Pose's data will not be + * changed. + * + * @param pose the input Pose + */ public void add(Pose pose) { setX(x + pose.getX()); setY(y + pose.getY()); setHeading(heading + pose.getHeading()); } + /** + * This subtracts all the values of an input Pose from this Pose. The input Pose's data will not + * be changed. + * + * @param pose the input Pose + */ public void subtract(Pose pose) { setX(x - pose.getX()); setY(y - pose.getY()); setHeading(heading - pose.getHeading()); } + /** + * This multiplies all the values of this Pose by a scalar. + * + * @param scalar the scalar + */ public void scalarMultiply(double scalar) { setX(x * scalar); setY(y * scalar); setHeading(heading * scalar); } + /** + * This divides all the values of this Pose by a scalar. + * + * @param scalar the scalar + */ public void scalarDivide(double scalar) { setX(x / scalar); setY(y / scalar); setHeading(heading / scalar); } + /** + * This flips the signs of all values in this Pose by multiplying them by -1. Heading values are + * still normalized to be between 0 and 2 * pi in value. + */ public void flipSigns() { setX(-x); setY(-y); setHeading(-heading); } + /** + * This returns if a Pose is within a specified accuracy of this Pose in terms of x position, + * y position, and heading. + * + * @param pose the input Pose to check + * @param accuracy the specified accuracy necessary to return true + * @return returns if the input Pose is within the specified accuracy of this Pose + */ public boolean roughlyEquals(Pose pose, double accuracy) { return MathFunctions.roughlyEquals(x, pose.getX(), accuracy) && MathFunctions.roughlyEquals(y, pose.getY(), accuracy) && MathFunctions.roughlyEquals(MathFunctions.getSmallestAngleDifference(heading, pose.getHeading()), 0, accuracy); } + /** + * This checks if the input Pose is within 0.0001 in all values to this Pose. + * + * @param pose the input Pose + * @return returns if the input Pose is within 0.0001 of this Pose + */ public boolean roughlyEquals(Pose pose) { return roughlyEquals(pose, 0.0001); } + /** + * This creates a copy of this Pose that points to a new memory location. + * + * @return returns a deep copy of this Pose + */ public Pose copy() { return new Pose(getX(), getY(), getHeading()); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java index a010f6a..d667f40 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java @@ -44,19 +44,29 @@ public class PoseUpdater { private long currentPoseTime; /** - * Creates a new PoseUpdater from a HardwareMap. + * Creates a new PoseUpdater from a HardwareMap and a Localizer. * * @param hardwareMap the HardwareMap + * @param localizer the Localizer */ - public PoseUpdater(HardwareMap hardwareMap) { + public PoseUpdater(HardwareMap hardwareMap, Localizer localizer) { this.hardwareMap = hardwareMap; for (LynxModule module : hardwareMap.getAll(LynxModule.class)) { module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); } - // TODO: change this to your preferred localizer - localizer = new ThreeWheelLocalizer(hardwareMap); + this.localizer = localizer; + } + + /** + * Creates a new PoseUpdater from a HardwareMap. + * + * @param hardwareMap the HardwareMap + */ + public PoseUpdater(HardwareMap hardwareMap) { + // TODO: replace the second argument with your preferred localizer + this(hardwareMap, new ThreeWheelLocalizer(hardwareMap)); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java index 5ad8cd1..19c6382 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java @@ -23,6 +23,12 @@ public class RRToPedroThreeWheelLocalizer extends Localizer { private Pose startPose; private Pose previousPose; + /** + * This creates a new RRToPedroThreeWheelLocalizer from a HardwareMap. This adapts the previously + * used Road Runner localization system to the new Pedro Pathing localization system. + * + * @param hardwareMap the HardwareMap + */ public RRToPedroThreeWheelLocalizer(HardwareMap hardwareMap) { List lastTrackingEncPositions = new ArrayList<>(); List lastTrackingEncVels = new ArrayList<>(); @@ -32,18 +38,34 @@ public class RRToPedroThreeWheelLocalizer extends Localizer { startPose = new Pose(); previousPose = new Pose(); } + + /** + * This returns the current pose estimate as a Pose. + * + * @return returns the current pose estimate + */ @Override public Pose getPose() { Pose2d pose = localizer.getPoseEstimate(); return new Pose(pose.getX(), pose.getY(), pose.getHeading()); } + /** + * This returns the current velocity estimate as a Pose. + * + * @return returns the current velocity estimate + */ @Override public Pose getVelocity() { Pose2d pose = localizer.getPoseVelocity(); return new Pose(pose.getX(), pose.getY(), pose.getHeading()); } + /** + * This returns the current velocity estimate as a Vector. + * + * @return returns the current velocity estimate + */ @Override public Vector getVelocityVector() { Pose2d pose = localizer.getPoseVelocity(); @@ -52,6 +74,12 @@ public class RRToPedroThreeWheelLocalizer extends Localizer { return returnVector; } + /** + * This sets the start pose. Any movement of the robot is treated as a displacement from the + * start pose, so moving the start pose will move the current pose estimate the same amount. + * + * @param setStart the new start pose + */ @Override public void setStartPose(Pose setStart) { Pose oldStart = startPose; @@ -60,32 +88,70 @@ public class RRToPedroThreeWheelLocalizer extends Localizer { localizer.setPoseEstimate(new Pose2d(getPose().getX() + startDiff.getX(), getPose().getY() + startDiff.getY(), getPose().getHeading() + startDiff.getHeading())); } + /** + * This sets the current pose estimate. This has no effect on the start pose. + * + * @param setPose the new current pose estimate + */ @Override public void setPose(Pose setPose) { localizer.setPoseEstimate(new Pose2d(setPose.getX(), setPose.getY(), setPose.getHeading())); } + /** + * This updates the total heading and previous pose estimate. Everything else is handled by the + * Road Runner localizer on its own, but updating this tells you how far the robot has really + * turned. + */ @Override public void update() { totalHeading += MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(previousPose.getHeading(), getPose().getHeading()); previousPose = getPose(); } + /** + * This returns how far the robot has actually turned. + * + * @return returns the total angle turned, in degrees. + */ @Override public double getTotalHeading() { return totalHeading; } + /** + * This returns the forward multiplier of the Road Runner localizer, which converts from ticks + * to inches. You can actually use the tuners in Pedro Pathing to find the value that everything + * multiplied together should be. If you do use that, then do be aware that the value returned is + * the product of the Road Runner ticks to inches and the x multiplier. + * + * @return returns the forward multiplier + */ @Override public double getForwardMultiplier() { return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.X_MULTIPLIER; } + /** + * This returns the lateral multiplier of the Road Runner localizer, which converts from ticks + * to inches. You can actually use the tuners in Pedro Pathing to find the value that everything + * multiplied together should be. If you do use that, then do be aware that the value returned is + * the product of the Road Runner ticks to inches and the y multiplier. + * + * @return returns the lateral multiplier + */ @Override public double getLateralMultiplier() { return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.Y_MULTIPLIER; } + /** + * This returns the turning multiplier of the Road Runner localizer, which doesn't actually exist. + * There really isn't a point in tuning the turning for the Road Runner localizer. This will + * actually just return the average of the two other multipliers. + * + * @return returns the turning multiplier + */ @Override public double getTurningMultiplier() { return (getForwardMultiplier() + getLateralMultiplier()) / 2; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java index edbe337..24ea188 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java @@ -5,8 +5,15 @@ import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; /** - * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding - * slot's motor direction + * This class is adapted from the Road Runner Encoder class. Later, this will be replaced with a + * custom encoder class. According to Road Runner, this wraps a motor instance to provide corrected + * velocity counts and allow reversing independently of the corresponding slot's motor direction. + * + * I'm fairly sure I didn't make any changes to this class, just copied it so I wouldn't have to have + * import statements, so I'm not crediting myself as an author for this. + * + * @author Road Runner dev team + * @version 1.0, 5/9/2024 */ public class RoadRunnerEncoder { private final static int CPS_STEP = 0x10000; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java index 93d13bb..c2e9b94 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java @@ -31,7 +31,13 @@ import java.util.List; /** * This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will - * be replaced with a custom localizer. + * be replaced with a custom localizer. I made some minor changes, so I'm crediting myself as an + * 'author' of sorts, but really this is pretty much Road Runner's code, just moved to be local to + * Pedro Pathing to avoid having imports. + * + * @author Road Runner dev team + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 5/9/2024 */ @Config public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java index cdbc35d..a7e6548 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java @@ -52,10 +52,23 @@ public class ThreeWheelLocalizer extends Localizer { public static double STRAFE_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659; public static double TURN_TICKS_TO_RADIANS = 0.00053717;//8192 * 1.37795 * 2 * Math.PI * 0.5; + /** + * This creates a new ThreeWheelLocalizer from a HardwareMap, with a starting Pose at (0,0) + * facing 0 heading. + * + * @param map the HardwareMap + */ public ThreeWheelLocalizer(HardwareMap map) { this(map, new Pose()); } + /** + * This creates a new ThreeWheelLocalizer from a HardwareMap and a Pose, with the Pose + * specifying the starting pose of the localizer. + * + * @param map the HardwareMap + * @param setStartPose the Pose to start from + */ public ThreeWheelLocalizer(HardwareMap map, Pose setStartPose) { // TODO: replace these with your encoder positions leftEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0); @@ -84,26 +97,52 @@ public class ThreeWheelLocalizer extends Localizer { resetEncoders(); } + /** + * This returns the current pose estimate. + * + * @return returns the current pose estimate as a Pose + */ @Override public Pose getPose() { return MathFunctions.addPoses(startPose, displacementPose); } + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Pose + */ @Override public Pose getVelocity() { return currentVelocity.copy(); } + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Vector + */ @Override public Vector getVelocityVector() { return currentVelocity.getVector(); } + /** + * This sets the start pose. Changing the start pose should move the robot as if all its + * previous movements were displacing it from its new start pose. + * + * @param setStart the new start pose + */ @Override public void setStartPose(Pose setStart) { startPose = setStart; } + /** + * This sets the Matrix that contains the previous pose's heading rotation. + * + * @param heading the rotation of the Matrix + */ public void setPrevRotationMatrix(double heading) { prevRotationMatrix = new Matrix(3,3); prevRotationMatrix.set(0, 0, Math.cos(heading)); @@ -113,12 +152,23 @@ public class ThreeWheelLocalizer extends Localizer { prevRotationMatrix.set(2, 2, 1.0); } + /** + * This sets the current pose estimate. Changing this should just change the robot's current + * pose estimate, not anything to do with the start pose. + * + * @param setPose the new current pose estimate + */ @Override public void setPose(Pose setPose) { displacementPose = MathFunctions.subtractPoses(setPose, startPose); resetEncoders(); } + /** + * This updates the elapsed time timer that keeps track of time between updates, as well as the + * change position of the Encoders. Then, the robot's global change in position is calculated + * using the pose exponential method. + */ @Override public void update() { deltaTimeNano = timer.getElapsedTime(); @@ -152,18 +202,30 @@ public class ThreeWheelLocalizer extends Localizer { totalHeading += globalDeltas.get(2, 0); } + /** + * This updates the Encoders. + */ public void updateEncoders() { leftEncoder.update(); rightEncoder.update(); strafeEncoder.update(); } + /** + * This resets the Encoders. + */ public void resetEncoders() { leftEncoder.reset(); rightEncoder.reset(); strafeEncoder.reset(); } + /** + * This calculates the change in position from the perspective of the robot using information + * from the Encoders. + * + * @return returns a Matrix containing the robot relative movement. + */ public Matrix getRobotDeltas() { Matrix returnMatrix = new Matrix(3,1); // x/forward movement @@ -175,18 +237,42 @@ public class ThreeWheelLocalizer extends Localizer { return returnMatrix; } + /** + * This returns how far the robot has turned in radians, in a number not clamped between 0 and + * 2 * pi radians. This is used for some tuning things and nothing actually within the following. + * + * @return returns how far the robot has turned in total, in radians. + */ public double getTotalHeading() { return totalHeading; } + /** + * This returns the multiplier applied to forward movement measurement to convert from encoder + * ticks to inches. This is found empirically through a tuner. + * + * @return returns the forward ticks to inches multiplier + */ public double getForwardMultiplier() { return FORWARD_TICKS_TO_INCHES; } + /** + * This returns the multiplier applied to lateral/strafe movement measurement to convert from + * encoder ticks to inches. This is found empirically through a tuner. + * + * @return returns the lateral/strafe ticks to inches multiplier + */ public double getLateralMultiplier() { return STRAFE_TICKS_TO_INCHES; } + /** + * This returns the multiplier applied to turning movement measurement to convert from encoder + * ticks to radians. This is found empirically through a tuner. + * + * @return returns the turning ticks to radians multiplier + */ public double getTurningMultiplier() { return TURN_TICKS_TO_RADIANS; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java index 26e46bc..5c4e932 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java @@ -55,10 +55,23 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w public static double FORWARD_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5008239963; public static double STRAFE_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5018874659; + /** + * This creates a new TwoWheelLocalizer from a HardwareMap, with a starting Pose at (0,0) + * facing 0 heading. + * + * @param map the HardwareMap + */ public TwoWheelLocalizer(HardwareMap map) { this(map, new Pose()); } + /** + * This creates a new TwoWheelLocalizer from a HardwareMap and a Pose, with the Pose + * specifying the starting pose of the localizer. + * + * @param map the HardwareMap + * @param setStartPose the Pose to start from + */ public TwoWheelLocalizer(HardwareMap map, Pose setStartPose) { // TODO: replace these with your encoder positions forwardEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0); @@ -87,26 +100,52 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w deltaRadians = 0; } + /** + * This returns the current pose estimate. + * + * @return returns the current pose estimate as a Pose + */ @Override public Pose getPose() { return MathFunctions.addPoses(startPose, displacementPose); } + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Pose + */ @Override public Pose getVelocity() { return currentVelocity.copy(); } + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Vector + */ @Override public Vector getVelocityVector() { return currentVelocity.getVector(); } + /** + * This sets the start pose. Changing the start pose should move the robot as if all its + * previous movements were displacing it from its new start pose. + * + * @param setStart the new start pose + */ @Override public void setStartPose(Pose setStart) { startPose = setStart; } + /** + * This sets the Matrix that contains the previous pose's heading rotation. + * + * @param heading the rotation of the Matrix + */ public void setPrevRotationMatrix(double heading) { prevRotationMatrix = new Matrix(3,3); prevRotationMatrix.set(0, 0, Math.cos(heading)); @@ -116,12 +155,23 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w prevRotationMatrix.set(2, 2, 1.0); } + /** + * This sets the current pose estimate. Changing this should just change the robot's current + * pose estimate, not anything to do with the start pose. + * + * @param setPose the new current pose estimate + */ @Override public void setPose(Pose setPose) { displacementPose = MathFunctions.subtractPoses(setPose, startPose); resetEncoders(); } + /** + * This updates the elapsed time timer that keeps track of time between updates, as well as the + * change position of the Encoders and the IMU readings. Then, the robot's global change in + * position is calculated using the pose exponential method. + */ @Override public void update() { deltaTimeNano = timer.getElapsedTime(); @@ -155,6 +205,9 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w totalHeading += globalDeltas.get(2, 0); } + /** + * This updates the Encoders as well as the IMU. + */ public void updateEncoders() { forwardEncoder.update(); strafeEncoder.update(); @@ -164,11 +217,20 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w previousIMUOrientation = currentIMUOrientation; } + /** + * This resets the Encoders. + */ public void resetEncoders() { forwardEncoder.reset(); strafeEncoder.reset(); } + /** + * This calculates the change in position from the perspective of the robot using information + * from the Encoders and IMU. + * + * @return returns a Matrix containing the robot relative movement. + */ public Matrix getRobotDeltas() { Matrix returnMatrix = new Matrix(3,1); // x/forward movement @@ -180,18 +242,42 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w return returnMatrix; } + /** + * This returns how far the robot has turned in radians, in a number not clamped between 0 and + * 2 * pi radians. This is used for some tuning things and nothing actually within the following. + * + * @return returns how far the robot has turned in total, in radians. + */ public double getTotalHeading() { return totalHeading; } + /** + * This returns the multiplier applied to forward movement measurement to convert from encoder + * ticks to inches. This is found empirically through a tuner. + * + * @return returns the forward ticks to inches multiplier + */ public double getForwardMultiplier() { return FORWARD_TICKS_TO_INCHES; } + /** + * This returns the multiplier applied to lateral/strafe movement measurement to convert from + * encoder ticks to inches. This is found empirically through a tuner. + * + * @return returns the lateral/strafe ticks to inches multiplier + */ public double getLateralMultiplier() { return STRAFE_TICKS_TO_INCHES; } + /** + * This returns the multiplier applied to turning movement measurement to convert from encoder + * ticks to radians. This is found empirically through a tuner. + * + * @return returns the turning ticks to radians multiplier + */ public double getTurningMultiplier() { return 1; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/FowardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/FowardTuner.java index 525d4ad..ff59830 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/FowardTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/FowardTuner.java @@ -3,11 +3,14 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.util.DashboardPoseTracker; +import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; /** * This is the ForwardTuner OpMode. This tracks the forward movement of the robot and displays the @@ -26,24 +29,44 @@ import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; @Autonomous(name = "Forward Localizer Tuner", group = "Autonomous Pathing Tuning") public class FowardTuner extends OpMode { private PoseUpdater poseUpdater; + private DashboardPoseTracker dashboardPoseTracker; private Telemetry telemetryA; public static double DISTANCE = 30; + /** + * This initializes the PoseUpdater as well as the FTC Dashboard telemetry. + */ @Override public void init() { poseUpdater = new PoseUpdater(hardwareMap); + + dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); telemetryA.addLine("Pull your robot forward " + DISTANCE + " inches. Your forward ticks to inches will be shown on the telemetry."); telemetryA.update(); + + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); } + /** + * This updates the robot's pose estimate, and updates the FTC Dashboard telemetry with the + * calculated multiplier and draws the robot. + */ @Override public void loop() { poseUpdater.update(); + telemetryA.addData("distance moved", poseUpdater.getPose().getX()); telemetryA.addLine("The multiplier will display what your forward ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); telemetryA.addData("multiplier", DISTANCE / (poseUpdater.getPose().getX() / poseUpdater.getLocalizer().getForwardMultiplier())); + telemetryA.update(); + + Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50"); + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java index 2995a2e..282577d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java @@ -3,11 +3,14 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.util.DashboardPoseTracker; +import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; /** * This is the LateralTuner OpMode. This tracks the strafe movement of the robot and displays the @@ -26,24 +29,44 @@ import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; @Autonomous(name = "Lateral Localizer Tuner", group = "Autonomous Pathing Tuning") public class LateralTuner extends OpMode { private PoseUpdater poseUpdater; + private DashboardPoseTracker dashboardPoseTracker; private Telemetry telemetryA; public static double DISTANCE = 30; + /** + * This initializes the PoseUpdater as well as the FTC Dashboard telemetry. + */ @Override public void init() { poseUpdater = new PoseUpdater(hardwareMap); + + dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); telemetryA.addLine("Pull your robot to the right " + DISTANCE + " inches. Your strafe ticks to inches will be shown on the telemetry."); telemetryA.update(); + + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); } + /** + * This updates the robot's pose estimate, and updates the FTC Dashboard telemetry with the + * calculated multiplier and draws the robot. + */ @Override public void loop() { poseUpdater.update(); + telemetryA.addData("distance moved", poseUpdater.getPose().getY()); telemetryA.addLine("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); telemetryA.addData("multiplier", DISTANCE / (poseUpdater.getPose().getX() / poseUpdater.getLocalizer().getLateralMultiplier())); + telemetryA.update(); + + Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50"); + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java index a6c19e3..103c0b2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java @@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; @@ -13,6 +12,7 @@ import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigu import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.util.DashboardPoseTracker; import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; import java.util.Arrays; @@ -30,6 +30,7 @@ import java.util.List; @TeleOp(group = "Pedro Pathing Tuning", name = "Localization Test") public class LocalizationTest extends OpMode { private PoseUpdater poseUpdater; + private DashboardPoseTracker dashboardPoseTracker; private Telemetry telemetryA; private DcMotorEx leftFront; @@ -38,10 +39,15 @@ public class LocalizationTest extends OpMode { private DcMotorEx rightRear; private List motors; + /** + * This initializes the PoseUpdater, the mecanum drive motors, and the FTC Dashboard telemetry. + */ @Override public void init() { poseUpdater = new PoseUpdater(hardwareMap); + dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); + leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); @@ -67,20 +73,18 @@ public class LocalizationTest extends OpMode { + "allowing robot control through a basic mecanum drive on gamepad 1."); telemetryA.update(); - TelemetryPacket packet = new TelemetryPacket(); - packet.fieldOverlay().setStroke("#3F51B5"); - Drawing.drawRobot(packet.fieldOverlay(), poseUpdater.getPose()); - FtcDashboard.getInstance().sendTelemetryPacket(packet); - } - - @Override - public void start() { - //poseUpdater.setPose(new Pose()); + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); } + /** + * This updates the robot's pose estimate, the simple mecanum drive, and updates the FTC + * Dashboard telemetry with the robot's position as well as draws the robot's position. + */ @Override public void loop() { poseUpdater.update(); + dashboardPoseTracker.update(); double y = -gamepad1.left_stick_y; // Remember, this is reversed! double x = gamepad1.left_stick_x; // this is strafing @@ -106,9 +110,8 @@ public class LocalizationTest extends OpMode { telemetryA.addData("total heading", poseUpdater.getTotalHeading()); telemetryA.update(); - TelemetryPacket packet = new TelemetryPacket(); - packet.fieldOverlay().setStroke("#3F51B5"); - Drawing.drawRobot(packet.fieldOverlay(), poseUpdater.getPose()); - FtcDashboard.getInstance().sendTelemetryPacket(packet); + Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50"); + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java index 4b2838d..1693990 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java @@ -3,11 +3,14 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.util.DashboardPoseTracker; +import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; /** * This is the TurnTuner OpMode. This tracks the turning movement of the robot and displays the @@ -26,24 +29,43 @@ import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; @Autonomous(name = "Turn Localizer Tuner", group = "Autonomous Pathing Tuning") public class TurnTuner extends OpMode { private PoseUpdater poseUpdater; + private DashboardPoseTracker dashboardPoseTracker; private Telemetry telemetryA; public static double ANGLE = 2 * Math.PI; + /** + * This initializes the PoseUpdater as well as the FTC Dashboard telemetry. + */ @Override public void init() { poseUpdater = new PoseUpdater(hardwareMap); + + dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); telemetryA.addLine("Turn your robot " + ANGLE + " radians. Your turn ticks to inches will be shown on the telemetry."); telemetryA.update(); + + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); } + /** + * This updates the robot's pose estimate, and updates the FTC Dashboard telemetry with the + * calculated multiplier and draws the robot. + */ @Override public void loop() { poseUpdater.update(); + telemetryA.addData("total angle", poseUpdater.getTotalHeading()); telemetryA.addLine("The multiplier will display what your turn ticks to inches should be to scale your current angle to " + ANGLE + " radians."); telemetryA.addData("multiplier", ANGLE / (poseUpdater.getTotalHeading() / poseUpdater.getLocalizer().getTurningMultiplier())); + + Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50"); + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java index 190bf8a..f5bb12a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java @@ -4,6 +4,7 @@ package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; import org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants; import java.util.ArrayList; +import java.util.Arrays; /** * This is the BezierCurve class. This class handles the creation of Bezier curves, which are used @@ -28,6 +29,10 @@ public class BezierCurve { private final int APPROXIMATION_STEPS = FollowerConstants.APPROXIMATION_STEPS; + private final int DASHBOARD_DRAWING_APPROXIMATION_STEPS = 100; + + private double[][] dashboardDrawingPoints; + private double UNIT_TO_TIME; private double length; @@ -55,11 +60,7 @@ public class BezierCurve { } } this.controlPoints = controlPoints; - generateBezierCurve(); - length = approximateLength(); - UNIT_TO_TIME = 1/length; - endTangent.setOrthogonalComponents(controlPoints.get(controlPoints.size()-1).getX()-controlPoints.get(controlPoints.size()-2).getX(), controlPoints.get(controlPoints.size()-1).getY()-controlPoints.get(controlPoints.size()-2).getY()); - endTangent = MathFunctions.normalizeVector(endTangent); + initialize(); } /** @@ -80,11 +81,41 @@ public class BezierCurve { e.printStackTrace(); } } + initialize(); + } + + /** + * This handles most of the initialization of the BezierCurve that is called from the constructor. + */ + public void initialize() { generateBezierCurve(); length = approximateLength(); UNIT_TO_TIME = 1/length; - endTangent.setOrthogonalComponents(this.controlPoints.get(this.controlPoints.size()-1).getX()-this.controlPoints.get(this.controlPoints.size()-2).getX(), this.controlPoints.get(this.controlPoints.size()-1).getY()-this.controlPoints.get(this.controlPoints.size()-2).getY()); + endTangent.setOrthogonalComponents(controlPoints.get(controlPoints.size()-1).getX()-controlPoints.get(controlPoints.size()-2).getX(), controlPoints.get(controlPoints.size()-1).getY()-controlPoints.get(controlPoints.size()-2).getY()); endTangent = MathFunctions.normalizeVector(endTangent); + initializeDashboardDrawingPoints(); + } + + /** + * This creates the Array that holds the Points to draw on the Dashboard. + */ + public void initializeDashboardDrawingPoints() { + dashboardDrawingPoints = new double[2][DASHBOARD_DRAWING_APPROXIMATION_STEPS + 1]; + for (int i = 0; i <= DASHBOARD_DRAWING_APPROXIMATION_STEPS; i++) { + Point currentPoint = getPoint(i/(double) (DASHBOARD_DRAWING_APPROXIMATION_STEPS)); + dashboardDrawingPoints[0][i] = currentPoint.getX(); + dashboardDrawingPoints[1][i] = currentPoint.getY(); + } + } + + /** + * This returns a 2D Array of doubles containing the x and y positions of points to draw on FTC + * Dashboard. + * + * @return returns the 2D Array to draw on FTC Dashboard + */ + public double[][] getDashboardDrawingPoints() { + return dashboardDrawingPoints; } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierLine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierLine.java index c2a2c52..04c7979 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierLine.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierLine.java @@ -37,6 +37,7 @@ public class BezierLine extends BezierCurve { length = approximateLength(); UNIT_TO_TIME = 1 / length; endTangent = MathFunctions.normalizeVector(getDerivative(1)); + super.initializeDashboardDrawingPoints(); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierPoint.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierPoint.java index b97700b..cd2382e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierPoint.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierPoint.java @@ -32,6 +32,7 @@ public class BezierPoint extends BezierCurve { super(); this.point = point; length = approximateLength(); + super.initializeDashboardDrawingPoints(); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java index 5a2f453..9d88d0a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java @@ -473,4 +473,14 @@ public class Path { public String pathType() { return curve.pathType(); } + + /** + * This returns a 2D Array of doubles containing the x and y positions of points to draw on FTC + * Dashboard. + * + * @return returns the 2D Array to draw on FTC Dashboard + */ + public double[][] getDashboardDrawingPoints() { + return curve.getDashboardDrawingPoints(); + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java index 7322447..0da8df8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java @@ -172,4 +172,14 @@ public class Point { public double getY() { return y; } + + /** + * This creates a new Point with the same information as this Point, just pointing to a different + * memory location. In other words, a deep copy. + * + * @return returns a copy of this Point. + */ + public Point copy() { + return new Point(getX(), getY(), CARTESIAN); + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java index bf0c302..08dc365 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java @@ -116,7 +116,7 @@ public class FollowerConstants { // Centripetal force to power scaling // todo: there are currently issues with the centripetal force correction, so just don't use it for now // i will fix these in another commit soon - public static double centripetalScaling = 0.001; + public static double centripetalScaling = 0.0005; // Acceleration of the drivetrain when power is cut in inches/second^2 (should be negative) // if not negative, then the robot thinks that its going to go faster under 0 power @@ -158,9 +158,9 @@ public class FollowerConstants { public static double pathEndTValueConstraint = 0.995; // When the Path is considered at its end parametrically, then the Follower has this many - // seconds to further correct by default. + // milliseconds to further correct by default. // This can be custom set for each Path. - public static double pathEndTimeoutConstraint = 1.5; + public static double pathEndTimeoutConstraint = 500; // This is how many steps the BezierCurve class uses to approximate the length of a BezierCurve. public static int APPROXIMATION_STEPS = 1000; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java new file mode 100644 index 0000000..250c7ce --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java @@ -0,0 +1,76 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; + +import java.util.ArrayList; + +/** + * This is the DashboardPoseTracker class. This tracks the pose history of the robot through a + * PoseUpdater, adding to the pose history at specified increments of time and storing the history + * for a specified length of time. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 5/13/2024 + */ +public class DashboardPoseTracker { + private double[] xPositions; + private double[] yPositions; + private PoseUpdater poseUpdater; + private long lastUpdateTime; + private final int TRACKING_LENGTH = 1000; + private final long UPDATE_TIME = 5; + private final int TRACKING_SIZE = TRACKING_LENGTH / (int) UPDATE_TIME; + + /** + * This creates a new DashboardPoseTracker from a PoseUpdater. + * + * @param poseUpdater the PoseUpdater + */ + public DashboardPoseTracker(PoseUpdater poseUpdater) { + this.poseUpdater = poseUpdater; + xPositions = new double[TRACKING_SIZE]; + yPositions = new double[TRACKING_SIZE]; + + for (int i = 0; i < TRACKING_SIZE; i++) { + xPositions[i] = poseUpdater.getPose().getX(); + yPositions[i] = poseUpdater.getPose().getY(); + } + + lastUpdateTime = System.currentTimeMillis() - UPDATE_TIME; + } + + /** + * This updates the DashboardPoseTracker. When the specified update time has passed from the last + * pose history log, another pose can be logged. The least recent log is also removed. + */ + public void update() { + if (System.currentTimeMillis() - lastUpdateTime > UPDATE_TIME) { + lastUpdateTime = System.currentTimeMillis(); + for (int i = TRACKING_SIZE - 1; i > 0; i--) { + xPositions[i] = xPositions[i - 1]; + yPositions[i] = yPositions[i - 1]; + } + xPositions[0] = poseUpdater.getPose().getX(); + yPositions[0] = poseUpdater.getPose().getY(); + } + } + + /** + * This returns the x positions of the pose history as an Array of doubles. + * + * @return returns the x positions of the pose history + */ + public double[] getXPositionsArray() { + return xPositions; + } + + /** + * This returns the y positions of the pose history as an Array of doubles. + * + * @return returns the y positions of the pose history + */ + public double[] getYPositionsArray() { + return yPositions; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java index 6d9af9c..9a2d956 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java @@ -1,20 +1,119 @@ package org.firstinspires.ftc.teamcode.pedroPathing.util; +import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; -/* -* Drawing class needed for putting your robot onto the dashboard -* Edit the radius needed to align to your robot the best possible simulation represented +/** + * This is the Drawing class. It handles the drawing of stuff on FTC Dashboard, like the robot. + * + * @author Logan Nash + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 4/22/2024 */ public class Drawing { - private Drawing() {} + private static TelemetryPacket packet; - public static void drawRobot(Canvas c, Point t) { + /** + * This draws everything that will be used in the Follower's telemetryDebug() method. This takes + * a Follower as an input, so an instance of the DashbaordDrawingHandler class is not needed. + * @param follower + */ + public static void drawDebug(Follower follower) { + if (follower.getCurrentPath() != null) { + drawPath(follower.getCurrentPath(), "#3F51B5"); + Point closestPoint = follower.getPointFromPath(follower.getCurrentPath().getClosestPointTValue()); + drawRobot(new Pose(closestPoint.getX(), closestPoint.getY(), follower.getCurrentPath().getHeadingGoal(follower.getCurrentPath().getClosestPointTValue())), "#3F51B5"); + } + drawPoseHistory(follower.getDashboardPoseTracker(), "#4CAF50"); + drawRobot(follower.getPose(), "#4CAF50"); + sendPacket(); + } + + /** + * This adds instructions to the current packet to draw a robot at a specified Pose with a specified + * color. If no packet exists, then a new one is created. + * + * @param pose the Pose to draw the robot at + * @param color the color to draw the robot with + */ + public static void drawRobot(Pose pose, String color) { + if (packet == null) packet = new TelemetryPacket(); + + packet.fieldOverlay().setStroke(color); + Drawing.drawRobotOnCanvas(packet.fieldOverlay(), pose.copy()); + } + + /** + * This adds instructions to the current packet to draw a Path with a specified color. If no + * packet exists, then a new one is created. + * + * @param path the Path to draw + * @param color the color to draw the Path with + */ + public static void drawPath(Path path, String color) { + if (packet == null) packet = new TelemetryPacket(); + + packet.fieldOverlay().setStroke(color); + Drawing.drawPath(packet.fieldOverlay(), path.getDashboardDrawingPoints()); + } + + /** + * This adds instructions to the current packet to draw all the Paths in a PathChain with a + * specified color. If no packet exists, then a new one is created. + * + * @param pathChain the PathChain to draw + * @param color the color to draw the PathChain with + */ + public static void drawPath(PathChain pathChain, String color) { + for (int i = 0; i < pathChain.size(); i++) { + drawPath(pathChain.getPath(i), color); + } + } + + /** + * This adds instructions to the current packet to draw the pose history of the robot. If no + * packet exists, then a new one is created. + * + * @param poseTracker the DashboardPoseTracker to get the pose history from + * @param color the color to draw the pose history with + */ + public static void drawPoseHistory(DashboardPoseTracker poseTracker, String color) { + if (packet == null) packet = new TelemetryPacket(); + + packet.fieldOverlay().setStroke(color); + packet.fieldOverlay().strokePolyline(poseTracker.getXPositionsArray(), poseTracker.getYPositionsArray()); + } + + /** + * This tries to send the current packet to FTC Dashboard. + * + * @return returns if the operation was successful. + */ + public static boolean sendPacket() { + if (packet != null) { + FtcDashboard.getInstance().sendTelemetryPacket(packet); + packet = null; + return true; + } + return false; + } + + /** + * This draws a robot on the Dashboard at a specified Point. + * + * @param c the Canvas on the Dashboard on which this will draw + * @param t the Point to draw at + */ + public static void drawRobotOnCanvas(Canvas c, Point t) { final double ROBOT_RADIUS = 9; c.setStrokeWidth(1); @@ -26,7 +125,14 @@ public class Drawing { c.strokeLine(p1.getXComponent(), p1.getYComponent(), p2.getXComponent(), p2.getYComponent()); } - public static void drawRobot(Canvas c, Pose t) { + /** + * This draws a robot on the Dashboard at a specified Pose. This is more useful for drawing the + * actual robot, since the Pose contains the direction the robot is facing as well as its position. + * + * @param c the Canvas on the Dashboard on which this will draw + * @param t the Pose to draw at + */ + public static void drawRobotOnCanvas(Canvas c, Pose t) { final double ROBOT_RADIUS = 9; c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS); @@ -36,4 +142,14 @@ public class Drawing { double x2 = t.getX() + v.getXComponent(), y2 = t.getY() + v.getYComponent(); c.strokeLine(x1, y1, x2, y2); } + + /** + * This draws a Path on the Dashboard from a specified Array of Points. + * + * @param c the Canvas on the Dashboard on which this will draw + * @param points the Points to draw + */ + public static void drawPath(Canvas c, double[][] points) { + c.strokePolyline(points[0], points[1]); + } } From 52f3cb91ae25cf2228992e10e341f536430f5633 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Tue, 14 May 2024 08:39:48 -0400 Subject: [PATCH 14/94] disabled road runner localizer adapters to reduce number of gradle dependencies --- TeamCode/build.gradle | 2 - .../ftc/teamcode/pedroPathing/README.md | 10 +- .../ftc/teamcode/pedroPathing/TUNING.md | 142 ++++---- .../RRToPedroThreeWheelLocalizer.java | 318 +++++++++--------- .../localization/RoadRunnerEncoder.java | 264 +++++++-------- .../RoadRunnerThreeWheelLocalizer.java | 246 +++++++------- .../util/DashboardPoseTracker.java | 4 +- 7 files changed, 491 insertions(+), 495 deletions(-) diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 442471b..c300cd0 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -27,7 +27,5 @@ dependencies { implementation project(':FtcRobotController') annotationProcessor files('lib/OpModeAnnotationProcessor.jar') - implementation 'org.apache.commons:commons-math3:3.6.1' implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7' - implementation 'com.acmerobotics.roadrunner:core:0.5.6' } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md index a8cf0cc..35ab001 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md @@ -1,6 +1,6 @@ ## Welcome! -This is the Pedro Pathing path following program developed by FTC team 10158 Scott's Bots in the -2023-2024 Centerstage season. +This is the Pedro Pathing path following program developed by FTC team 10158 Scott's Bots with Logan +Nash in the 2023-2024 Centerstage season. ## Installation The quickest way to get started is with the quickstart [here](https://github.com/brotherhobo/Pedro-Pathing-Quickstart). @@ -19,11 +19,9 @@ maven { url = 'https://maven.brott.dev/' } 3. Then, add the following code to the end of your `dependencies` block: ``` implementation 'com.acmerobotics.dashboard:dashboard:0.4.5' -``` +``` 4. Find the `build.gradle` file under the `teamcode` folder. -5. In this gradle file, add the following dependencies: +5. In this gradle file, add the following dependency: ``` -implementation 'org.apache.commons:commons-math3:3.6.1' implementation 'com.fasterxml/jackson.core:jacson-databind:2.12.7' -implementation 'com.acmerobotics.com.roadrunner:core:0.5.6' ``` \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md index a41a4b2..ddd1672 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md @@ -1,94 +1,94 @@ ## Prerequisites Obviously, you have to have a robot to use Pedro Pathing. Also, Pedro Pathing is only able to work with omnidirectional drives, like mecanum drive. There is currently no support for swerve drives. -You must also have a localizer of some sort. Pedro Pathing has both a three-wheel localizer and a -two-wheel localizer. You will need to have your localizer tuned before starting to tune PedroPathing. -Check out the tuning guide under the localization tab if you're planning on using one of the +You must also have a localizer of some sort. Pedro Pathing has a drive encoder, a two tracking wheel, +and a three tracking wheel localizer. You will need to have your localizer tuned before starting to +tune PedroPathing. Check out the tuning guide under the localization tab if you're planning on using one of the localizers available in Pedro Pathing. Additionally, using [FTC Dashboard](http://192.168.43.1:8080/dash) will help a lot in tuning, and we have a slightly scuffed Desmos path visualizer [here](https://www.desmos.com/calculator/3so1zx0hcd). One last thing to note is that Pedro Pathing operates in inches and radians. ## Tuning * To start with, we need the mass of the robot in kg. This is used for the centripetal force -correction, and the mass should be put on line `114` in the `FollowerConstants` class under the -`tuning` package. + correction, and the mass should be put on line `114` in the `FollowerConstants` class under the + `tuning` package. * Next, we need to find the preferred mecanum drive vectors. The rollers on mecanum wheels point at a -45 degree angle from the forward direction, but the actual direction the force is output is actually -closer to forward. To find the direction your wheels will go, you will need to run the -`Forward Velocity Tuner` and `Strafe Velocity Tuner` OpModes. These will run your robot at full -power for 40 inches forward and to the right, respectively. The distance can be changed through FTC -Dashboard under the dropdown for each respective class, but higher distances work better. After the -distance has finished running, the end velocity will be output to telemetry. The robot may continue -to drift a little bit after the robot has finished running the distance, so make sure you have -plenty of room. Once you're done, put the velocity for the `Forward Velocity Tuner` on line `25` in -the `FollowerConstants` class, and the velocity for the `Strafe Velocity Tuner` on line `26` in the -`FollowerConstants` class. + 45 degree angle from the forward direction, but the actual direction the force is output is actually + closer to forward. To find the direction your wheels will go, you will need to run the + `Forward Velocity Tuner` and `Strafe Velocity Tuner` OpModes. These will run your robot at full + power for 40 inches forward and to the right, respectively. The distance can be changed through FTC + Dashboard under the dropdown for each respective class, but higher distances work better. After the + distance has finished running, the end velocity will be output to telemetry. The robot may continue + to drift a little bit after the robot has finished running the distance, so make sure you have + plenty of room. Once you're done, put the velocity for the `Forward Velocity Tuner` on line `25` in + the `FollowerConstants` class, and the velocity for the `Strafe Velocity Tuner` on line `26` in the + `FollowerConstants` class. * The last set of automatic tuners you'll need to run are the zero power acceleration tuners. These -find the rate at which your robot decelerates when power is cut from the drivetrain. This is used to -get a more accurate estimation of the drive vector. To find this, you will need to run the -`Forward Zero Power Acceleration Tuner` and the `Lateral Zero Power Acceleration Tuner` OpModes. -These will run your robot until it hits a velocity of 10 inches/second forward and to the right, -respectively. The velocity can be changed through FTC Dashboard under the dropdown for each -respective class, but higher velocities work better. After the velocity has been reached, power will -be cut from the drivetrain and the robot's deceleration will be tracked until the robot stops, at -which point it will display the deceleration in telemetry. This robot will need to drift to a stop -to properly work, and the higher the velocity the greater the drift distance, so make sure you have -enough room. Once you're done, put the zero power acceleration for the -`Forward Zero Power Acceleration Tuner` on line `122` in the `FollowerConstants` class and the zero -power acceleration for the `Lateral Zero Power Acceleration Tuner` on line `127` in the -`FollowerConstants` class. + find the rate at which your robot decelerates when power is cut from the drivetrain. This is used to + get a more accurate estimation of the drive vector. To find this, you will need to run the + `Forward Zero Power Acceleration Tuner` and the `Lateral Zero Power Acceleration Tuner` OpModes. + These will run your robot until it hits a velocity of 10 inches/second forward and to the right, + respectively. The velocity can be changed through FTC Dashboard under the dropdown for each + respective class, but higher velocities work better. After the velocity has been reached, power will + be cut from the drivetrain and the robot's deceleration will be tracked until the robot stops, at + which point it will display the deceleration in telemetry. This robot will need to drift to a stop + to properly work, and the higher the velocity the greater the drift distance, so make sure you have + enough room. Once you're done, put the zero power acceleration for the + `Forward Zero Power Acceleration Tuner` on line `122` in the `FollowerConstants` class and the zero + power acceleration for the `Lateral Zero Power Acceleration Tuner` on line `127` in the + `FollowerConstants` class. * After this, we will want to tune the translational PIDs. Go to FTC Dashboard and disable all but -the `useTranslational` checkboxes under the `Follower` tab. Then, run `StraightBackAndForth`. Make -sure you disable the timer on autonomous OpModes. There are two different PIDs for translational -error, the `smallTranslationalPIDF` and `largeTranslationalPIDF`. If error is larger than a certain -amount, the `largeTranslationalPIDF` will be used, and if error is smaller than that amount the -`smallTranslationalPIDF` will be used. If you need to add a feedforward value, use the -`smallTranslationalPIDFFeedForward` and `largeTranslationalPIDFFeedForward` since those will add the -feedforward in the direction the robot is trying to move, rather than the feedforward in the PIDFs -themselves, since those will only add the feedforward one way. You can change the amount of error -required to switch PIDFs, as well as the PIDF constants and feedforward values, under the -`FollowerConstants` tab in FTC Dashboard. To tune the PIDs, push the robot off the path and see how -corrects. You will want to alternate sides you push to reduce field wear and tear as well as push -with varying power and distance. I would recommend tuning the large PID first, and tuning it so that -the PID is capable of correcting to the point where the PIDs switch with little momentum. Then, tune -the small PID to minimize oscillations while still achieving a satisfactory level of accuracy. -Overall, try to tune for fewer oscillations rather than higher speeds or perfect accuracy, since -this will make the robot run more smoothly under actual pathing conditions. + the `useTranslational` checkboxes under the `Follower` tab. Then, run `StraightBackAndForth`. Make + sure you disable the timer on autonomous OpModes. There are two different PIDs for translational + error, the `smallTranslationalPIDF` and `largeTranslationalPIDF`. If error is larger than a certain + amount, the `largeTranslationalPIDF` will be used, and if error is smaller than that amount the + `smallTranslationalPIDF` will be used. If you need to add a feedforward value, use the + `smallTranslationalPIDFFeedForward` and `largeTranslationalPIDFFeedForward` since those will add the + feedforward in the direction the robot is trying to move, rather than the feedforward in the PIDFs + themselves, since those will only add the feedforward one way. You can change the amount of error + required to switch PIDFs, as well as the PIDF constants and feedforward values, under the + `FollowerConstants` tab in FTC Dashboard. To tune the PIDs, push the robot off the path and see how + corrects. You will want to alternate sides you push to reduce field wear and tear as well as push + with varying power and distance. I would recommend tuning the large PID first, and tuning it so that + the PID is capable of correcting to the point where the PIDs switch with little momentum. Then, tune + the small PID to minimize oscillations while still achieving a satisfactory level of accuracy. + Overall, try to tune for fewer oscillations rather than higher speeds or perfect accuracy, since + this will make the robot run more smoothly under actual pathing conditions. * Next, we will tune the heading PIDs. The process is essentially the same as above, except you will -want to only enable `useHeading` under `Follower` on FTC Dashboard, as well as turn the robot from -opposing corners instead of pushing the robot. Naturally, instead of changing the stuff with -"translational" in the name, you will instead want to look for stuff with "heading" in the name. -Otherwise, these two PIDs are functionally very similar. The same tips from above will apply to this. + want to only enable `useHeading` under `Follower` on FTC Dashboard, as well as turn the robot from + opposing corners instead of pushing the robot. Naturally, instead of changing the stuff with + "translational" in the name, you will instead want to look for stuff with "heading" in the name. + Otherwise, these two PIDs are functionally very similar. The same tips from above will apply to this. * Afterwards, we will tune the drive PIDs. Before we continue, we will need to set the -`zeroPowerAccelerationMultiplier`. This determines how fast your robot will decelerate as a factor -of how fast your robot will coast to a stop. Honestly, this is up to you. I personally used 4, but -what works best for you is most important. Higher numbers will cause a faster brake, but increase -oscillations at the end. Lower numbers will do the opposite. This can be found on line `136` in -`FollowerConstants`. There are once again two PIDs for the drive vector, but these PIDs are much, -much more sensitive than the others. For reference, my P values were in the hundredths and -thousandths place values, and my D values were in the hundred thousandths and millionths place -values. To tune this, enable `useDrive`, `useHeading`, and `useTranslational` in the `Follower` -dropdown in FTC Dashboard. Next, run `StraightBackAndForth`and don't forget to turn off the timer on -the OpMode. Then, tune the large PID and then the small PID following the tips from earlier. For -this, it is very important to try to reduce oscillations. Additionally, I would absolutely not -recommend using the I, or integral, part of the PID for this. Using integral in drivetrain PIDs is -already not ideal, but it will continuously build up error in this PID, causing major issues when -it gets too strong. So, just don't use it. P and D are enough. + `zeroPowerAccelerationMultiplier`. This determines how fast your robot will decelerate as a factor + of how fast your robot will coast to a stop. Honestly, this is up to you. I personally used 4, but + what works best for you is most important. Higher numbers will cause a faster brake, but increase + oscillations at the end. Lower numbers will do the opposite. This can be found on line `136` in + `FollowerConstants`. There are once again two PIDs for the drive vector, but these PIDs are much, + much more sensitive than the others. For reference, my P values were in the hundredths and + thousandths place values, and my D values were in the hundred thousandths and millionths place + values. To tune this, enable `useDrive`, `useHeading`, and `useTranslational` in the `Follower` + dropdown in FTC Dashboard. Next, run `StraightBackAndForth`and don't forget to turn off the timer on + the OpMode. Then, tune the large PID and then the small PID following the tips from earlier. For + this, it is very important to try to reduce oscillations. Additionally, I would absolutely not + recommend using the I, or integral, part of the PID for this. Using integral in drivetrain PIDs is + already not ideal, but it will continuously build up error in this PID, causing major issues when + it gets too strong. So, just don't use it. P and D are enough. * Finally, we will want to tune the centripetal force correction. This is a pretty simple tune. Open -up FTC Dashboard and enable everything under the `Follower` tab. Then, run `CurvedBackAndForth` -and turn off its timer. If you notice the robot is correcting towards the inside of the curve -as/after running a path, then increase `centripetalScaling`, which can be found on line `117` of -`FollowerConstants`. If the robot is correcting towards the outside of the curve, then decrease -`centripetalScaling`. + up FTC Dashboard and enable everything under the `Follower` tab. Then, run `CurvedBackAndForth` + and turn off its timer. If you notice the robot is correcting towards the inside of the curve + as/after running a path, then increase `centripetalScaling`, which can be found on line `117` of + `FollowerConstants`. If the robot is correcting towards the outside of the curve, then decrease + `centripetalScaling`. * Once you've found satisfactory tunings for everything, run the robot around in -`StraightBackAndForth`, `CurvedBackAndForth`, or some paths of your own making. There's also -`Circle`, but that's more so for fun than anything else. If you notice something could be improved, -feel free to mess around more with your PIDs. That should be all! If you have any more questions, -refer to the OVERVIEW readme file or the README readme file. Best of luck to your team this season! :) \ No newline at end of file + `StraightBackAndForth`, `CurvedBackAndForth`, or some paths of your own making. There's also + `Circle`, but that's more so for fun than anything else. If you notice something could be improved, + feel free to mess around more with your PIDs. That should be all! If you have any more questions, + refer to the OVERVIEW readme file or the README readme file. Best of luck to your team this season! :) \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java index 19c6382..15275f0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java @@ -1,159 +1,159 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization; - -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; - -import java.util.ArrayList; -import java.util.List; - -/** - * This is the RRToPedroThreeWheelLocalizer class. This class extends the Localizer superclass and - * is intended to adapt the old Road Runner three wheel odometry localizer to the new Pedro Pathing - * localizer system. - * - * @author Anyi Lin - 10158 Scott's Bots - * @version 1.0, 5/9/2024 - */ -public class RRToPedroThreeWheelLocalizer extends Localizer { - private RoadRunnerThreeWheelLocalizer localizer; - private double totalHeading; - private Pose startPose; - private Pose previousPose; - - /** - * This creates a new RRToPedroThreeWheelLocalizer from a HardwareMap. This adapts the previously - * used Road Runner localization system to the new Pedro Pathing localization system. - * - * @param hardwareMap the HardwareMap - */ - public RRToPedroThreeWheelLocalizer(HardwareMap hardwareMap) { - List lastTrackingEncPositions = new ArrayList<>(); - List lastTrackingEncVels = new ArrayList<>(); - - localizer = new RoadRunnerThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels); - - startPose = new Pose(); - previousPose = new Pose(); - } - - /** - * This returns the current pose estimate as a Pose. - * - * @return returns the current pose estimate - */ - @Override - public Pose getPose() { - Pose2d pose = localizer.getPoseEstimate(); - return new Pose(pose.getX(), pose.getY(), pose.getHeading()); - } - - /** - * This returns the current velocity estimate as a Pose. - * - * @return returns the current velocity estimate - */ - @Override - public Pose getVelocity() { - Pose2d pose = localizer.getPoseVelocity(); - return new Pose(pose.getX(), pose.getY(), pose.getHeading()); - } - - /** - * This returns the current velocity estimate as a Vector. - * - * @return returns the current velocity estimate - */ - @Override - public Vector getVelocityVector() { - Pose2d pose = localizer.getPoseVelocity(); - Vector returnVector = new Vector(); - returnVector.setOrthogonalComponents(pose.getX(), pose.getY()); - return returnVector; - } - - /** - * This sets the start pose. Any movement of the robot is treated as a displacement from the - * start pose, so moving the start pose will move the current pose estimate the same amount. - * - * @param setStart the new start pose - */ - @Override - public void setStartPose(Pose setStart) { - Pose oldStart = startPose; - startPose = setStart; - Pose startDiff = MathFunctions.subtractPoses(startPose, oldStart); - localizer.setPoseEstimate(new Pose2d(getPose().getX() + startDiff.getX(), getPose().getY() + startDiff.getY(), getPose().getHeading() + startDiff.getHeading())); - } - - /** - * This sets the current pose estimate. This has no effect on the start pose. - * - * @param setPose the new current pose estimate - */ - @Override - public void setPose(Pose setPose) { - localizer.setPoseEstimate(new Pose2d(setPose.getX(), setPose.getY(), setPose.getHeading())); - } - - /** - * This updates the total heading and previous pose estimate. Everything else is handled by the - * Road Runner localizer on its own, but updating this tells you how far the robot has really - * turned. - */ - @Override - public void update() { - totalHeading += MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(previousPose.getHeading(), getPose().getHeading()); - previousPose = getPose(); - } - - /** - * This returns how far the robot has actually turned. - * - * @return returns the total angle turned, in degrees. - */ - @Override - public double getTotalHeading() { - return totalHeading; - } - - /** - * This returns the forward multiplier of the Road Runner localizer, which converts from ticks - * to inches. You can actually use the tuners in Pedro Pathing to find the value that everything - * multiplied together should be. If you do use that, then do be aware that the value returned is - * the product of the Road Runner ticks to inches and the x multiplier. - * - * @return returns the forward multiplier - */ - @Override - public double getForwardMultiplier() { - return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.X_MULTIPLIER; - } - - /** - * This returns the lateral multiplier of the Road Runner localizer, which converts from ticks - * to inches. You can actually use the tuners in Pedro Pathing to find the value that everything - * multiplied together should be. If you do use that, then do be aware that the value returned is - * the product of the Road Runner ticks to inches and the y multiplier. - * - * @return returns the lateral multiplier - */ - @Override - public double getLateralMultiplier() { - return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.Y_MULTIPLIER; - } - - /** - * This returns the turning multiplier of the Road Runner localizer, which doesn't actually exist. - * There really isn't a point in tuning the turning for the Road Runner localizer. This will - * actually just return the average of the two other multipliers. - * - * @return returns the turning multiplier - */ - @Override - public double getTurningMultiplier() { - return (getForwardMultiplier() + getLateralMultiplier()) / 2; - } -} +//package org.firstinspires.ftc.teamcode.pedroPathing.localization; +// +//import com.acmerobotics.roadrunner.geometry.Pose2d; +//import com.qualcomm.robotcore.hardware.HardwareMap; +// +//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +// +//import java.util.ArrayList; +//import java.util.List; +// +///** +// * This is the RRToPedroThreeWheelLocalizer class. This class extends the Localizer superclass and +// * is intended to adapt the old Road Runner three wheel odometry localizer to the new Pedro Pathing +// * localizer system. +// * +// * @author Anyi Lin - 10158 Scott's Bots +// * @version 1.0, 5/9/2024 +// */ +//public class RRToPedroThreeWheelLocalizer extends Localizer { +// private RoadRunnerThreeWheelLocalizer localizer; +// private double totalHeading; +// private Pose startPose; +// private Pose previousPose; +// +// /** +// * This creates a new RRToPedroThreeWheelLocalizer from a HardwareMap. This adapts the previously +// * used Road Runner localization system to the new Pedro Pathing localization system. +// * +// * @param hardwareMap the HardwareMap +// */ +// public RRToPedroThreeWheelLocalizer(HardwareMap hardwareMap) { +// List lastTrackingEncPositions = new ArrayList<>(); +// List lastTrackingEncVels = new ArrayList<>(); +// +// localizer = new RoadRunnerThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels); +// +// startPose = new Pose(); +// previousPose = new Pose(); +// } +// +// /** +// * This returns the current pose estimate as a Pose. +// * +// * @return returns the current pose estimate +// */ +// @Override +// public Pose getPose() { +// Pose2d pose = localizer.getPoseEstimate(); +// return new Pose(pose.getX(), pose.getY(), pose.getHeading()); +// } +// +// /** +// * This returns the current velocity estimate as a Pose. +// * +// * @return returns the current velocity estimate +// */ +// @Override +// public Pose getVelocity() { +// Pose2d pose = localizer.getPoseVelocity(); +// return new Pose(pose.getX(), pose.getY(), pose.getHeading()); +// } +// +// /** +// * This returns the current velocity estimate as a Vector. +// * +// * @return returns the current velocity estimate +// */ +// @Override +// public Vector getVelocityVector() { +// Pose2d pose = localizer.getPoseVelocity(); +// Vector returnVector = new Vector(); +// returnVector.setOrthogonalComponents(pose.getX(), pose.getY()); +// return returnVector; +// } +// +// /** +// * This sets the start pose. Any movement of the robot is treated as a displacement from the +// * start pose, so moving the start pose will move the current pose estimate the same amount. +// * +// * @param setStart the new start pose +// */ +// @Override +// public void setStartPose(Pose setStart) { +// Pose oldStart = startPose; +// startPose = setStart; +// Pose startDiff = MathFunctions.subtractPoses(startPose, oldStart); +// localizer.setPoseEstimate(new Pose2d(getPose().getX() + startDiff.getX(), getPose().getY() + startDiff.getY(), getPose().getHeading() + startDiff.getHeading())); +// } +// +// /** +// * This sets the current pose estimate. This has no effect on the start pose. +// * +// * @param setPose the new current pose estimate +// */ +// @Override +// public void setPose(Pose setPose) { +// localizer.setPoseEstimate(new Pose2d(setPose.getX(), setPose.getY(), setPose.getHeading())); +// } +// +// /** +// * This updates the total heading and previous pose estimate. Everything else is handled by the +// * Road Runner localizer on its own, but updating this tells you how far the robot has really +// * turned. +// */ +// @Override +// public void update() { +// totalHeading += MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(previousPose.getHeading(), getPose().getHeading()); +// previousPose = getPose(); +// } +// +// /** +// * This returns how far the robot has actually turned. +// * +// * @return returns the total angle turned, in degrees. +// */ +// @Override +// public double getTotalHeading() { +// return totalHeading; +// } +// +// /** +// * This returns the forward multiplier of the Road Runner localizer, which converts from ticks +// * to inches. You can actually use the tuners in Pedro Pathing to find the value that everything +// * multiplied together should be. If you do use that, then do be aware that the value returned is +// * the product of the Road Runner ticks to inches and the x multiplier. +// * +// * @return returns the forward multiplier +// */ +// @Override +// public double getForwardMultiplier() { +// return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.X_MULTIPLIER; +// } +// +// /** +// * This returns the lateral multiplier of the Road Runner localizer, which converts from ticks +// * to inches. You can actually use the tuners in Pedro Pathing to find the value that everything +// * multiplied together should be. If you do use that, then do be aware that the value returned is +// * the product of the Road Runner ticks to inches and the y multiplier. +// * +// * @return returns the lateral multiplier +// */ +// @Override +// public double getLateralMultiplier() { +// return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.Y_MULTIPLIER; +// } +// +// /** +// * This returns the turning multiplier of the Road Runner localizer, which doesn't actually exist. +// * There really isn't a point in tuning the turning for the Road Runner localizer. This will +// * actually just return the average of the two other multipliers. +// * +// * @return returns the turning multiplier +// */ +// @Override +// public double getTurningMultiplier() { +// return (getForwardMultiplier() + getLateralMultiplier()) / 2; +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java index 24ea188..d743b49 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java @@ -1,132 +1,132 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization; - -import com.acmerobotics.roadrunner.util.NanoClock; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; - -/** - * This class is adapted from the Road Runner Encoder class. Later, this will be replaced with a - * custom encoder class. According to Road Runner, this wraps a motor instance to provide corrected - * velocity counts and allow reversing independently of the corresponding slot's motor direction. - * - * I'm fairly sure I didn't make any changes to this class, just copied it so I wouldn't have to have - * import statements, so I'm not crediting myself as an author for this. - * - * @author Road Runner dev team - * @version 1.0, 5/9/2024 - */ -public class RoadRunnerEncoder { - private final static int CPS_STEP = 0x10000; - - private static double inverseOverflow(double input, double estimate) { - // convert to uint16 - int real = (int) input & 0xffff; - // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits - // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window - real += ((real % 20) / 4) * CPS_STEP; - // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by - real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP; - return real; - } - - public enum Direction { - FORWARD(1), - REVERSE(-1); - - private int multiplier; - - Direction(int multiplier) { - this.multiplier = multiplier; - } - - public int getMultiplier() { - return multiplier; - } - } - - private DcMotorEx motor; - private NanoClock clock; - - private Direction direction; - - private int lastPosition; - private int velocityEstimateIdx; - private double[] velocityEstimates; - private double lastUpdateTime; - - public RoadRunnerEncoder(DcMotorEx motor, NanoClock clock) { - this.motor = motor; - this.clock = clock; - - this.direction = Direction.FORWARD; - - this.lastPosition = 0; - this.velocityEstimates = new double[3]; - this.lastUpdateTime = clock.seconds(); - } - - public RoadRunnerEncoder(DcMotorEx motor) { - this(motor, NanoClock.system()); - } - - public Direction getDirection() { - return direction; - } - - private int getMultiplier() { - return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1); - } - - /** - * Allows you to set the direction of the counts and velocity without modifying the motor's direction state - * @param direction either reverse or forward depending on if encoder counts should be negated - */ - public void setDirection(Direction direction) { - this.direction = direction; - } - - /** - * Gets the position from the underlying motor and adjusts for the set direction. - * Additionally, this method updates the velocity estimates used for compensated velocity - * - * @return encoder position - */ - public int getCurrentPosition() { - int multiplier = getMultiplier(); - int currentPosition = motor.getCurrentPosition() * multiplier; - if (currentPosition != lastPosition) { - double currentTime = clock.seconds(); - double dt = currentTime - lastUpdateTime; - velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt; - velocityEstimateIdx = (velocityEstimateIdx + 1) % 3; - lastPosition = currentPosition; - lastUpdateTime = currentTime; - } - return currentPosition; - } - - /** - * Gets the velocity directly from the underlying motor and compensates for the direction - * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore) - * - * @return raw velocity - */ - public double getRawVelocity() { - int multiplier = getMultiplier(); - return motor.getVelocity() * multiplier; - } - - /** - * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity - * that are lost in overflow due to velocity being transmitted as 16 bits. - * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly. - * - * @return corrected velocity - */ - public double getCorrectedVelocity() { - double median = velocityEstimates[0] > velocityEstimates[1] - ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2])) - : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2])); - return inverseOverflow(getRawVelocity(), median); - } -} +//package org.firstinspires.ftc.teamcode.pedroPathing.localization; +// +//import com.acmerobotics.roadrunner.util.NanoClock; +//import com.qualcomm.robotcore.hardware.DcMotorEx; +//import com.qualcomm.robotcore.hardware.DcMotorSimple; +// +///** +// * This class is adapted from the Road Runner Encoder class. Later, this will be replaced with a +// * custom encoder class. According to Road Runner, this wraps a motor instance to provide corrected +// * velocity counts and allow reversing independently of the corresponding slot's motor direction. +// * +// * I'm fairly sure I didn't make any changes to this class, just copied it so I wouldn't have to have +// * import statements, so I'm not crediting myself as an author for this. +// * +// * @author Road Runner dev team +// * @version 1.0, 5/9/2024 +// */ +//public class RoadRunnerEncoder { +// private final static int CPS_STEP = 0x10000; +// +// private static double inverseOverflow(double input, double estimate) { +// // convert to uint16 +// int real = (int) input & 0xffff; +// // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits +// // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window +// real += ((real % 20) / 4) * CPS_STEP; +// // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by +// real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP; +// return real; +// } +// +// public enum Direction { +// FORWARD(1), +// REVERSE(-1); +// +// private int multiplier; +// +// Direction(int multiplier) { +// this.multiplier = multiplier; +// } +// +// public int getMultiplier() { +// return multiplier; +// } +// } +// +// private DcMotorEx motor; +// private NanoClock clock; +// +// private Direction direction; +// +// private int lastPosition; +// private int velocityEstimateIdx; +// private double[] velocityEstimates; +// private double lastUpdateTime; +// +// public RoadRunnerEncoder(DcMotorEx motor, NanoClock clock) { +// this.motor = motor; +// this.clock = clock; +// +// this.direction = Direction.FORWARD; +// +// this.lastPosition = 0; +// this.velocityEstimates = new double[3]; +// this.lastUpdateTime = clock.seconds(); +// } +// +// public RoadRunnerEncoder(DcMotorEx motor) { +// this(motor, NanoClock.system()); +// } +// +// public Direction getDirection() { +// return direction; +// } +// +// private int getMultiplier() { +// return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1); +// } +// +// /** +// * Allows you to set the direction of the counts and velocity without modifying the motor's direction state +// * @param direction either reverse or forward depending on if encoder counts should be negated +// */ +// public void setDirection(Direction direction) { +// this.direction = direction; +// } +// +// /** +// * Gets the position from the underlying motor and adjusts for the set direction. +// * Additionally, this method updates the velocity estimates used for compensated velocity +// * +// * @return encoder position +// */ +// public int getCurrentPosition() { +// int multiplier = getMultiplier(); +// int currentPosition = motor.getCurrentPosition() * multiplier; +// if (currentPosition != lastPosition) { +// double currentTime = clock.seconds(); +// double dt = currentTime - lastUpdateTime; +// velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt; +// velocityEstimateIdx = (velocityEstimateIdx + 1) % 3; +// lastPosition = currentPosition; +// lastUpdateTime = currentTime; +// } +// return currentPosition; +// } +// +// /** +// * Gets the velocity directly from the underlying motor and compensates for the direction +// * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore) +// * +// * @return raw velocity +// */ +// public double getRawVelocity() { +// int multiplier = getMultiplier(); +// return motor.getVelocity() * multiplier; +// } +// +// /** +// * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity +// * that are lost in overflow due to velocity being transmitted as 16 bits. +// * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly. +// * +// * @return corrected velocity +// */ +// public double getCorrectedVelocity() { +// double median = velocityEstimates[0] > velocityEstimates[1] +// ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2])) +// : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2])); +// return inverseOverflow(getRawVelocity(), median); +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java index c2e9b94..2ee75a9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java @@ -1,123 +1,123 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization; - -import androidx.annotation.NonNull; - -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.HardwareMap; - -import java.util.Arrays; -import java.util.List; - -/* - * Sample tracking wheel localizer implementation assuming the standard configuration: - * - * left on robot is y pos - * - * front of robot is x pos - * - * /--------------\ - * | ____ | - * | ---- | - * | || || | - * | || || | - * | | - * | | - * \--------------/ - * - */ - -/** - * This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will - * be replaced with a custom localizer. I made some minor changes, so I'm crediting myself as an - * 'author' of sorts, but really this is pretty much Road Runner's code, just moved to be local to - * Pedro Pathing to avoid having imports. - * - * @author Road Runner dev team - * @author Anyi Lin - 10158 Scott's Bots - * @version 1.0, 5/9/2024 - */ -@Config -public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer { - public static double TICKS_PER_REV = 8192; - public static double WHEEL_RADIUS = 1.37795; // in - public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed - - public static double X_MULTIPLIER = 0.5008239963; - public static double Y_MULTIPLIER = 0.5018874659; - - public static double leftX = -18.5/25.4 - 0.1, leftY = 164.4/25.4, rightX = -18.4/25.4 - 0.1, rightY = -159.6/25.4, strafeX = -107.9/25.4+0.25, strafeY = -1.1/25.4-0.23; - - private RoadRunnerEncoder leftEncoder, rightEncoder, strafeEncoder; - - private List lastEncPositions, lastEncVels; - - public RoadRunnerThreeWheelLocalizer(HardwareMap hardwareMap, List lastTrackingEncPositions, List lastTrackingEncVels) { - super(Arrays.asList( - new Pose2d(leftX, leftY, 0), // left - new Pose2d(rightX, rightY, 0), // right - new Pose2d(strafeX, strafeY, Math.toRadians(90)) // strafe - )); - - lastEncPositions = lastTrackingEncPositions; - lastEncVels = lastTrackingEncVels; - - // TODO: redo the configs here - leftEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "leftRear")); - rightEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "rightFront")); - strafeEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); - - // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) - leftEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); - rightEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); - strafeEncoder.setDirection(RoadRunnerEncoder.Direction.FORWARD); - } - - public void resetHeading(double heading) { - setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading)); - } - - public static double encoderTicksToInches(double ticks) { - return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; - } - - @NonNull - @Override - public List getWheelPositions() { - int leftPos = leftEncoder.getCurrentPosition(); - int rightPos = rightEncoder.getCurrentPosition(); - int frontPos = strafeEncoder.getCurrentPosition(); - - lastEncPositions.clear(); - lastEncPositions.add(leftPos); - lastEncPositions.add(rightPos); - lastEncPositions.add(frontPos); - - return Arrays.asList( - encoderTicksToInches(leftPos) * X_MULTIPLIER, - encoderTicksToInches(rightPos) * X_MULTIPLIER, - encoderTicksToInches(frontPos) * Y_MULTIPLIER - ); - } - - @NonNull - @Override - public List getWheelVelocities() { - int leftVel = (int) leftEncoder.getCorrectedVelocity(); - int rightVel = (int) rightEncoder.getCorrectedVelocity(); - int frontVel = (int) strafeEncoder.getCorrectedVelocity(); - - lastEncVels.clear(); - lastEncVels.add(leftVel); - lastEncVels.add(rightVel); - lastEncVels.add(frontVel); - - return Arrays.asList( - encoderTicksToInches(leftVel) * X_MULTIPLIER, - encoderTicksToInches(rightVel) * X_MULTIPLIER, - encoderTicksToInches(frontVel) * Y_MULTIPLIER - ); - } -} +//package org.firstinspires.ftc.teamcode.pedroPathing.localization; +// +//import androidx.annotation.NonNull; +// +//import com.acmerobotics.dashboard.config.Config; +//import com.acmerobotics.roadrunner.geometry.Pose2d; +//import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; +//import com.qualcomm.robotcore.hardware.DcMotorEx; +//import com.qualcomm.robotcore.hardware.HardwareMap; +// +//import java.util.Arrays; +//import java.util.List; +// +///* +// * Sample tracking wheel localizer implementation assuming the standard configuration: +// * +// * left on robot is y pos +// * +// * front of robot is x pos +// * +// * /--------------\ +// * | ____ | +// * | ---- | +// * | || || | +// * | || || | +// * | | +// * | | +// * \--------------/ +// * +// */ +// +///** +// * This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will +// * be replaced with a custom localizer. I made some minor changes, so I'm crediting myself as an +// * 'author' of sorts, but really this is pretty much Road Runner's code, just moved to be local to +// * Pedro Pathing to avoid having imports. +// * +// * @author Road Runner dev team +// * @author Anyi Lin - 10158 Scott's Bots +// * @version 1.0, 5/9/2024 +// */ +//@Config +//public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer { +// public static double TICKS_PER_REV = 8192; +// public static double WHEEL_RADIUS = 1.37795; // in +// public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed +// +// public static double X_MULTIPLIER = 0.5008239963; +// public static double Y_MULTIPLIER = 0.5018874659; +// +// public static double leftX = -18.5/25.4 - 0.1, leftY = 164.4/25.4, rightX = -18.4/25.4 - 0.1, rightY = -159.6/25.4, strafeX = -107.9/25.4+0.25, strafeY = -1.1/25.4-0.23; +// +// private RoadRunnerEncoder leftEncoder, rightEncoder, strafeEncoder; +// +// private List lastEncPositions, lastEncVels; +// +// public RoadRunnerThreeWheelLocalizer(HardwareMap hardwareMap, List lastTrackingEncPositions, List lastTrackingEncVels) { +// super(Arrays.asList( +// new Pose2d(leftX, leftY, 0), // left +// new Pose2d(rightX, rightY, 0), // right +// new Pose2d(strafeX, strafeY, Math.toRadians(90)) // strafe +// )); +// +// lastEncPositions = lastTrackingEncPositions; +// lastEncVels = lastTrackingEncVels; +// +// // TODO: redo the configs here +// leftEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "leftRear")); +// rightEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "rightFront")); +// strafeEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); +// +// // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) +// leftEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); +// rightEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); +// strafeEncoder.setDirection(RoadRunnerEncoder.Direction.FORWARD); +// } +// +// public void resetHeading(double heading) { +// setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading)); +// } +// +// public static double encoderTicksToInches(double ticks) { +// return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; +// } +// +// @NonNull +// @Override +// public List getWheelPositions() { +// int leftPos = leftEncoder.getCurrentPosition(); +// int rightPos = rightEncoder.getCurrentPosition(); +// int frontPos = strafeEncoder.getCurrentPosition(); +// +// lastEncPositions.clear(); +// lastEncPositions.add(leftPos); +// lastEncPositions.add(rightPos); +// lastEncPositions.add(frontPos); +// +// return Arrays.asList( +// encoderTicksToInches(leftPos) * X_MULTIPLIER, +// encoderTicksToInches(rightPos) * X_MULTIPLIER, +// encoderTicksToInches(frontPos) * Y_MULTIPLIER +// ); +// } +// +// @NonNull +// @Override +// public List getWheelVelocities() { +// int leftVel = (int) leftEncoder.getCorrectedVelocity(); +// int rightVel = (int) rightEncoder.getCorrectedVelocity(); +// int frontVel = (int) strafeEncoder.getCorrectedVelocity(); +// +// lastEncVels.clear(); +// lastEncVels.add(leftVel); +// lastEncVels.add(rightVel); +// lastEncVels.add(frontVel); +// +// return Arrays.asList( +// encoderTicksToInches(leftVel) * X_MULTIPLIER, +// encoderTicksToInches(rightVel) * X_MULTIPLIER, +// encoderTicksToInches(frontVel) * Y_MULTIPLIER +// ); +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java index 250c7ce..cab5daa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java @@ -18,8 +18,8 @@ public class DashboardPoseTracker { private double[] yPositions; private PoseUpdater poseUpdater; private long lastUpdateTime; - private final int TRACKING_LENGTH = 1000; - private final long UPDATE_TIME = 5; + private final int TRACKING_LENGTH = 2000; + private final long UPDATE_TIME = 50; private final int TRACKING_SIZE = TRACKING_LENGTH / (int) UPDATE_TIME; /** From 0f304e912d6b80681576b0da180fcd880372850f Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Tue, 14 May 2024 09:19:14 -0400 Subject: [PATCH 15/94] fixed gradle --- TeamCode/build.gradle | 1 + .../java/org/firstinspires/ftc/teamcode/pedroPathing/README.md | 1 + 2 files changed, 2 insertions(+) diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index c300cd0..d845fd6 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -28,4 +28,5 @@ dependencies { annotationProcessor files('lib/OpModeAnnotationProcessor.jar') implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7' + implementation 'org.jetbrains.kotlin:kotlin-stdlib:1.4.21' } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md index 35ab001..c1e8792 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md @@ -24,4 +24,5 @@ implementation 'com.acmerobotics.dashboard:dashboard:0.4.5' 5. In this gradle file, add the following dependency: ``` implementation 'com.fasterxml/jackson.core:jacson-databind:2.12.7' +implementation 'org.jetbrains.kotlin:kotlin-stdlib:1.4.21' ``` \ No newline at end of file From 252cc304adffc77465fd5c5c9c092ad858ad156e Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Tue, 14 May 2024 09:33:53 -0400 Subject: [PATCH 16/94] minor fixes and todo removed --- .../ftc/teamcode/pedroPathing/tuning/FollowerConstants.java | 2 -- .../ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java index 08dc365..a236b59 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java @@ -114,8 +114,6 @@ public class FollowerConstants { public static double mass = 10.65942; // Centripetal force to power scaling - // todo: there are currently issues with the centripetal force correction, so just don't use it for now - // i will fix these in another commit soon public static double centripetalScaling = 0.0005; // Acceleration of the drivetrain when power is cut in inches/second^2 (should be negative) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java index cab5daa..3c54156 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java @@ -18,7 +18,7 @@ public class DashboardPoseTracker { private double[] yPositions; private PoseUpdater poseUpdater; private long lastUpdateTime; - private final int TRACKING_LENGTH = 2000; + private final int TRACKING_LENGTH = 1500; private final long UPDATE_TIME = 50; private final int TRACKING_SIZE = TRACKING_LENGTH / (int) UPDATE_TIME; From bfab6dc7d08eb5ba7dfa86610a2b11e7e9c687b8 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Fri, 17 May 2024 10:04:28 -0400 Subject: [PATCH 17/94] more fixes --- .../localization/DriveEncoderLocalizer.java | 1 - .../localization/tuning/LateralTuner.java | 2 +- .../tuning/FollowerConstants.java | 48 +++++++++---------- .../LateralZeroPowerAccelerationTuner.java | 15 ++++++ 4 files changed, 39 insertions(+), 27 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java index 15501cc..44e04a5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java @@ -53,7 +53,6 @@ public class DriveEncoderLocalizer extends Localizer { // todo: make drive encod * @param setStartPose the Pose to start from */ public DriveEncoderLocalizer(HardwareMap map, Pose setStartPose) { - hardwareMap = map; // TODO: replace these with your encoder ports diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java index 282577d..f3d42f3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java @@ -62,7 +62,7 @@ public class LateralTuner extends OpMode { telemetryA.addData("distance moved", poseUpdater.getPose().getY()); telemetryA.addLine("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); - telemetryA.addData("multiplier", DISTANCE / (poseUpdater.getPose().getX() / poseUpdater.getLocalizer().getLateralMultiplier())); + telemetryA.addData("multiplier", DISTANCE / (poseUpdater.getPose().getY() / poseUpdater.getLocalizer().getLateralMultiplier())); telemetryA.update(); Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java index a236b59..b6aada6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java @@ -27,29 +27,6 @@ public class FollowerConstants { private static double[] convertToPolar = Point.cartesianToPolar(xMovement, -yMovement); public static Vector frontLeftVector = MathFunctions.normalizeVector(new Vector(convertToPolar[0],convertToPolar[1])); - // Large heading error PIDF coefficients - public static CustomPIDFCoefficients largeHeadingPIDFCoefficients = new CustomPIDFCoefficients( - 1, - 0, - 0, - 0); - - // Feed forward constant added on to the large heading PIDF - public static double largeHeadingPIDFFeedForward = 0.01; - - // the limit at which the heading PIDF switches between the large and small heading PIDFs - public static double headingPIDFSwitch = Math.PI/20; - - // Small heading error PIDF coefficients - public static CustomPIDFCoefficients smallHeadingPIDFCoefficients = new CustomPIDFCoefficients( - 5, - 0, - 0.08, - 0); - - // Feed forward constant added on to the small heading PIDF - public static double smallHeadingPIDFFeedForward = 0.01; - // Large translational PIDF coefficients public static CustomPIDFCoefficients largeTranslationalPIDFCoefficients = new CustomPIDFCoefficients( 0.1, @@ -87,6 +64,29 @@ public class FollowerConstants { // Feed forward constant added on to the small translational PIDF public static double smallTranslationalPIDFFeedForward = 0.015; + // Large heading error PIDF coefficients + public static CustomPIDFCoefficients largeHeadingPIDFCoefficients = new CustomPIDFCoefficients( + 1, + 0, + 0, + 0); + + // Feed forward constant added on to the large heading PIDF + public static double largeHeadingPIDFFeedForward = 0.01; + + // the limit at which the heading PIDF switches between the large and small heading PIDFs + public static double headingPIDFSwitch = Math.PI/20; + + // Small heading error PIDF coefficients + public static CustomPIDFCoefficients smallHeadingPIDFCoefficients = new CustomPIDFCoefficients( + 5, + 0, + 0.08, + 0); + + // Feed forward constant added on to the small heading PIDF + public static double smallHeadingPIDFFeedForward = 0.01; + // Large drive PIDF coefficients public static CustomPIDFCoefficients largeDrivePIDFCoefficients = new CustomPIDFCoefficients( 0.025, @@ -118,12 +118,10 @@ public class FollowerConstants { // Acceleration of the drivetrain when power is cut in inches/second^2 (should be negative) // if not negative, then the robot thinks that its going to go faster under 0 power - // this is for curves public static double forwardZeroPowerAcceleration = -34.62719; // Acceleration of the drivetrain when power is cut in inches/second^2 (should be negative) // if not negative, then the robot thinks that its going to go faster under 0 power - // this is for curves public static double lateralZeroPowerAcceleration = -78.15554; // A multiplier for the zero power acceleration to change the speed the robot decelerates at diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java index 8692e1b..0c22b9b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java @@ -19,6 +19,21 @@ import java.util.ArrayList; import java.util.Arrays; import java.util.List; +/** + * This is the LateralZeroPowerAccelerationTuner autonomous tuning OpMode. This runs the robot + * to the right until a specified velocity is achieved. Then, the robot cuts power to the motors, setting + * them to zero power. The deceleration, or negative acceleration, is then measured until the robot + * stops. The accelerations across the entire time the robot is slowing down is then averaged and + * that number is then printed. This is used to determine how the robot will decelerate in the + * forward direction when power is cut, making the estimations used in the calculations for the + * drive Vector more accurate and giving better braking at the end of Paths. + * You can adjust the max velocity the robot will hit on FTC Dashboard: 192/168/43/1:8080/dash + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/13/2024 + */ @Config @Autonomous (name = "Lateral Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") public class LateralZeroPowerAccelerationTuner extends OpMode { From 41c09e010314433b4d5ab07154d7b4e4320410cf Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Sat, 18 May 2024 16:36:17 -0400 Subject: [PATCH 18/94] localizer readme done --- .../pedroPathing/localization/README.md | 163 ++++++++++++++++++ 1 file changed, 163 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/README.md diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/README.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/README.md new file mode 100644 index 0000000..74267d3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/README.md @@ -0,0 +1,163 @@ +## Overview +This is the localization system developed for the Pedro Pathing path follower. These localizers use +the pose exponential method of localization. It's basically a way of turning movements from the +robot's coordinate frame to the global coordinate frame. If you're interested in reading more about +it, then check out pages 177 - 183 of [Controls Engineering in the FIRST Robotics Competition](https://file.tavsys.net/control/controls-engineering-in-frc.pdf) +by Tyler Veness. + +## Setting Your Localizer +Go to line `69` in the `PoseUpdater` class, and replace the `new ThreeWheelLocalizer(hardwareMap)` +with the localizer that applies to you: +* If you're using drive encoders, put `new DriveEncoderLocalizer(hardwareMap)` +* If you're using two wheel odometry, put `new TwoWheelLocalizer(hardwareMap)` +* If you're using three wheel odometry, put `new ThreeWheelLocalizer(hardwareMap)`, so basically + don't change it from the default + +## Tuning +To start, you'll want to select your localizer of choice. Below, I'll have instructions for the drive +encoder localizer, two tracking wheel localizer, and the three tracking wheel localizer offered in +Pedro Pathing. Scroll down to the section that applies to you and follow the directions there. + +# Drive Encoders +* First, you'll need all of your drive motors to have encoders attached. +* Then, go to `DriveEncoderLocalizer.java`. Go to where it tells you to replace the current statements with your encoder ports in the constructor. +Replace the `deviceName` parameter with the name of the port that the encoder for each motor is connected +to. The names of the variables is where on the robot the corresponding motor should be. +* Then, reverse the direction of any encoders so that all encoders tick up when the robot is moving forward. +* Now, you'll have to tune the multipliers. These convert your measurements from encoder ticks into + inches or radians, essentially scaling your localizer so that your numbers are accurate to the real + world. +* First, start with the `Turn Localizer Tuner`. You'll want to position your robot to be facing + in a direction you can easily find again, like lining up an edge of the robot against a field tile edge. + By default, you should spin the robot for one rotation going counterclockwise. Once you've spun + exactly that one rotation, or whatever you set that value to, then the turn multiplier will be shown + as the second number shown. The first number is how far the robot thinks you've spun, and the second + number is the multiplier you need to have to scale your current readings to your goal of one rotation, + or the custom set angle. Feel free to run a few more tests and average the results. Once you have + this multiplier, then replace `TURN_TICKS_TO_RADIANS` in the localizer with your multiplier. Make sure + you replace the number, not add on or multiply it to the previous number. The tuner takes into + account your current multiplier. +* Next, go on to `Forward Localizer Tuner`. You'll want to position a rule alongside your robot. + By default, you'll want to push the robot 30 inches forward. Once you've pushed that far, or whatever + you set that value to, then the forward multiplier will be shown as the second number shown. The + first number is how far the robot thinks you've gone, and the second number is the multiplier you + need to have to scale your current readings to your goal of 30 inches, or the custom set angle. + Feel free to run a few more tests and average the results. Once you have this multiplier, then + replace `FORWARD_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, + not add on or multiply it to the previous number. The tuner takes into account your current multiplier. +* Finally, go to `Lateral Localizer Tuner`. You'll want to position a rule alongside your robot. + By default, you'll want to push the robot 30 inches to the right. Once you've pushed that far, or whatever + you set that value to, then the lateral multiplier will be shown as the second number shown. The + first number is how far the robot thinks you've gone, and the second number is the multiplier you + need to have to scale your current readings to your goal of 30 inches, or the custom set angle. + Feel free to run a few more tests and average the results. Once you have this multiplier, then + replace `STRAFE_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, + not add on or multiply it to the previous number. The tuner takes into account your current multiplier. +* Once you're done with all this, your localizer should be tuned. To test it out, you can go to + `Localization Test` and push around or drive around your robot. Go to [FTC Dashboard](http://192.168.43.1:8080/dash) + and on the top right, switch the drop down from the default view to the field view. Then, on the bottom + left corner, you should see a field and the robot being drawn on the field. You can then move your + robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll + want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer! + +# Two Wheel Localizer +* First, you'll need a Control Hub with a working IMU, and two odometry wheels connected to motor + encoder ports on a hub. +* Then, go to `TwoWheelLocalizer.java`. First, in the constructor, enter in the positions of your + tracking wheels relative to the center of the wheels of the robot. The positions are in inches, so + convert measurements accordingly. Use the comment above the class declaration to help you with the + coordinates. +* Next, go to where it tells you to replace the current statements with your encoder ports in the constructor. + Replace the `deviceName` parameter with the name of the port that the encoder is connected to. The + variable names correspond to which tracking wheel should be connected. +* Then, reverse the direction of any encoders so that the forward encoder ticks up when the robot + is moving forward and the strafe encoder ticks up when the robot moves right. +* Now, you'll have to tune the multipliers. These convert your measurements from encoder ticks into + inches or radians, essentially scaling your localizer so that your numbers are accurate to the real + world. +* You actually won't need the turning tuner for this one, since the IMU in the Control Hub will take + care of the heading readings. +* First, start with the `Forward Localizer Tuner`. You'll want to position a rule alongside your robot. + By default, you'll want to push the robot 30 inches forward. Once you've pushed that far, or whatever + you set that value to, then the forward multiplier will be shown as the second number shown. The + first number is how far the robot thinks you've gone, and the second number is the multiplier you + need to have to scale your current readings to your goal of 30 inches, or the custom set angle. + Feel free to run a few more tests and average the results. Once you have this multiplier, then + replace `FORWARD_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, + not add on or multiply it to the previous number. The tuner takes into account your current multiplier. +* Finally, go to `Lateral Localizer Tuner`. You'll want to position a rule alongside your robot. + By default, you'll want to push the robot 30 inches to the right. Once you've pushed that far, or whatever + you set that value to, then the lateral multiplier will be shown as the second number shown. The + first number is how far the robot thinks you've gone, and the second number is the multiplier you + need to have to scale your current readings to your goal of 30 inches, or the custom set angle. + Feel free to run a few more tests and average the results. Once you have this multiplier, then + replace `STRAFE_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, + not add on or multiply it to the previous number. The tuner takes into account your current multiplier. +* Once you're done with all this, your localizer should be tuned. To test it out, you can go to + `Localization Test` and push around or drive around your robot. Go to [FTC Dashboard](http://192.168.43.1:8080/dash) + and on the top right, switch the drop down from the default view to the field view. Then, on the bottom + left corner, you should see a field and the robot being drawn on the field. You can then move your + robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll + want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer! + +# Three Wheel Localizer +* First, you'll need three odometry wheels connected to motor encoder ports on a hub. +* Then, go to `ThreeWheelLocalizer.java`. First, in the constructor, enter in the positions of your + tracking wheels relative to the center of the wheels of the robot. The positions are in inches, so + convert measurements accordingly. Use the comment above the class declaration to help you with the + coordinates. +* Next, go to where it tells you to replace the current statements with your encoder ports in the constructor. + Replace the `deviceName` parameter with the name of the port that the encoder is connected to. The + variable names correspond to which tracking wheel should be connected. +* Then, reverse the direction of any encoders so that the forward encoders tick up when the robot + is moving forward and the strafe encoder ticks up when the robot moves right. +* First, start with the `Turn Localizer Tuner`. You'll want to position your robot to be facing + in a direction you can easily find again, like lining up an edge of the robot against a field tile edge. + By default, you should spin the robot for one rotation going counterclockwise. Once you've spun + exactly that one rotation, or whatever you set that value to, then the turn multiplier will be shown + as the second number shown. The first number is how far the robot thinks you've spun, and the second + number is the multiplier you need to have to scale your current readings to your goal of one rotation, + or the custom set angle. Feel free to run a few more tests and average the results. Once you have + this multiplier, then replace `TURN_TICKS_TO_RADIANS` in the localizer with your multiplier. Make sure + you replace the number, not add on or multiply it to the previous number. The tuner takes into + account your current multiplier. +* Next, go on to `Forward Localizer Tuner`. You'll want to position a rule alongside your robot. + By default, you'll want to push the robot 30 inches forward. Once you've pushed that far, or whatever + you set that value to, then the forward multiplier will be shown as the second number shown. The + first number is how far the robot thinks you've gone, and the second number is the multiplier you + need to have to scale your current readings to your goal of 30 inches, or the custom set angle. + Feel free to run a few more tests and average the results. Once you have this multiplier, then + replace `FORWARD_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, + not add on or multiply it to the previous number. The tuner takes into account your current multiplier. +* Finally, go to `Lateral Localizer Tuner`. You'll want to position a rule alongside your robot. + By default, you'll want to push the robot 30 inches to the right. Once you've pushed that far, or whatever + you set that value to, then the lateral multiplier will be shown as the second number shown. The + first number is how far the robot thinks you've gone, and the second number is the multiplier you + need to have to scale your current readings to your goal of 30 inches, or the custom set angle. + Feel free to run a few more tests and average the results. Once you have this multiplier, then + replace `STRAFE_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, + not add on or multiply it to the previous number. The tuner takes into account your current multiplier. +* Once you're done with all this, your localizer should be tuned. To test it out, you can go to + `Localization Test` and push around or drive around your robot. Go to [FTC Dashboard](http://192.168.43.1:8080/dash) + and on the top right, switch the drop down from the default view to the field view. Then, on the bottom + left corner, you should see a field and the robot being drawn on the field. You can then move your + robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll + want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer! + +## Using Road Runner's Localizer +Of course, many teams have experience using Road Runner in the past and so have localizers from Road +Runner that are tuned. There is an adapter for the Road Runner three wheel localizer to the Pedro +Pathing localization system in Pedro Pathing, but it is commented out by default to reduce the number +of imports in gradle. + +To re-enable it, go to `RoadRunnerEncoder.java`, `RoadRunnerThreeWheelLocalizer.java`, and `RRToPedroThreeWheelLocalizer.java` +and hit `ctrl` + `a` to select everything within the files. Then, press `ctrl` + `/` to uncomment the code. + +Afterwards, go to `build.gradle` file under the `teamcode` folder and add the following dependencies: +``` +implementation 'org.apache.commons:commons-math3:3.6.1' +implementation 'com.acmerobotics.com.roadrunner:core:0.5.6' +``` + +After that, you should be good to go. If you want to use a different localizer from Road Runner, then +you can adapt it in the same process that's used for the Road Runner three wheel localizer. \ No newline at end of file From ce2009b8b44b753cddb54dd9ea62d0a0079bf374 Mon Sep 17 00:00:00 2001 From: brotherhobo <111587889+brotherhobo@users.noreply.github.com> Date: Sat, 8 Jun 2024 02:52:42 -0400 Subject: [PATCH 19/94] Create static.yml --- .github/workflows/static.yml | 43 ++++++++++++++++++++++++++++++++++++ 1 file changed, 43 insertions(+) create mode 100644 .github/workflows/static.yml diff --git a/.github/workflows/static.yml b/.github/workflows/static.yml new file mode 100644 index 0000000..0ba8230 --- /dev/null +++ b/.github/workflows/static.yml @@ -0,0 +1,43 @@ +# Simple workflow for deploying static content to GitHub Pages +name: Deploy static content to Pages + +on: + # Runs on pushes targeting the default branch + push: + branches: ["master"] + + # Allows you to run this workflow manually from the Actions tab + workflow_dispatch: + +# Sets permissions of the GITHUB_TOKEN to allow deployment to GitHub Pages +permissions: + contents: read + pages: write + id-token: write + +# Allow only one concurrent deployment, skipping runs queued between the run in-progress and latest queued. +# However, do NOT cancel in-progress runs as we want to allow these production deployments to complete. +concurrency: + group: "pages" + cancel-in-progress: false + +jobs: + # Single deploy job since we're just deploying + deploy: + environment: + name: github-pages + url: ${{ steps.deployment.outputs.page_url }} + runs-on: ubuntu-latest + steps: + - name: Checkout + uses: actions/checkout@v4 + - name: Setup Pages + uses: actions/configure-pages@v5 + - name: Upload artifact + uses: actions/upload-pages-artifact@v3 + with: + # Upload entire repository + path: '.' + - name: Deploy to GitHub Pages + id: deployment + uses: actions/deploy-pages@v4 From 14b5f267fb45b57297939f37ad38b63457a831fd Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Sat, 13 Jul 2024 23:09:55 -0400 Subject: [PATCH 20/94] Added new localizer that uses three tracking wheels, with the IMU for most heading tracking. Also fixed the math for the teleop centripetal force correction with help from @junkjunk123 on Discord --- .../pedroPathing/follower/Follower.java | 2 +- .../pedroPathing/localization/README.md | 57 +++- .../localization/ThreeWheelIMULocalizer.Java | 304 ++++++++++++++++++ .../localization/TwoWheelLocalizer.java | 1 + 4 files changed, 361 insertions(+), 3 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelIMULocalizer.Java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index 16a9819..577c67f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -789,7 +789,7 @@ public class Follower { } else { double yPrime = averageVelocity.getYComponent() / averageVelocity.getXComponent(); double yDoublePrime = averageAcceleration.getYComponent() / averageVelocity.getXComponent(); - curvature = (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)) / (yDoublePrime); + curvature = (yDoublePrime) / (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)); } if (Double.isNaN(curvature)) return new Vector(); centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -1, 1), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta())); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/README.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/README.md index 74267d3..9e4299a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/README.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/README.md @@ -12,11 +12,13 @@ with the localizer that applies to you: * If you're using two wheel odometry, put `new TwoWheelLocalizer(hardwareMap)` * If you're using three wheel odometry, put `new ThreeWheelLocalizer(hardwareMap)`, so basically don't change it from the default +* If you're using three wheel odometry with the IMU, put `new ThreeWheelIMULocalizer(hardwareMap)` ## Tuning To start, you'll want to select your localizer of choice. Below, I'll have instructions for the drive -encoder localizer, two tracking wheel localizer, and the three tracking wheel localizer offered in -Pedro Pathing. Scroll down to the section that applies to you and follow the directions there. +encoder localizer, two tracking wheel localizer, the three tracking wheel localizer, and the three +wheel with IMU localizer offered in Pedro Pathing. Scroll down to the section that applies to you +and follow the directions there. # Drive Encoders * First, you'll need all of your drive motors to have encoders attached. @@ -70,6 +72,8 @@ to. The names of the variables is where on the robot the corresponding motor sho * Next, go to where it tells you to replace the current statements with your encoder ports in the constructor. Replace the `deviceName` parameter with the name of the port that the encoder is connected to. The variable names correspond to which tracking wheel should be connected. +* After that, go to the instantiation of the IMU and change the orientation of the IMU to match that + of your robot's. * Then, reverse the direction of any encoders so that the forward encoder ticks up when the robot is moving forward and the strafe encoder ticks up when the robot moves right. * Now, you'll have to tune the multipliers. These convert your measurements from encoder ticks into @@ -144,6 +148,55 @@ to. The names of the variables is where on the robot the corresponding motor sho robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer! +# Three Wheel Localizer with IMU +* First, you'll need three odometry wheels connected to motor encoder ports on a hub. +* Then, go to `ThreeWheelIMULocalizer.java`. First, in the constructor, enter in the positions of your + tracking wheels relative to the center of the wheels of the robot. The positions are in inches, so + convert measurements accordingly. Use the comment above the class declaration to help you with the + coordinates. +* Next, go to where it tells you to replace the current statements with your encoder ports in the constructor. + Replace the `deviceName` parameter with the name of the port that the encoder is connected to. The + variable names correspond to which tracking wheel should be connected. +* After that, go to the instantiation of the IMU and change the orientation of the IMU to match that + of your robot's. +* Then, reverse the direction of any encoders so that the forward encoders tick up when the robot + is moving forward and the strafe encoder ticks up when the robot moves right. +* Although heading localization is done mostly through the IMU, the tracking wheels are still used for + small angle adjustments for better stability. So, you will still need to tune your turning multiplier. +* First, start with the `Turn Localizer Tuner`. Before doing any tuning, go to FTC Dashboard and find + the `ThreeWheelIMULocalizer` dropdown and deselect `useIMU`. You'll want to position your robot to be facing + in a direction you can easily find again, like lining up an edge of the robot against a field tile edge. + By default, you should spin the robot for one rotation going counterclockwise. Once you've spun + exactly that one rotation, or whatever you set that value to, then the turn multiplier will be shown + as the second number shown. The first number is how far the robot thinks you've spun, and the second + number is the multiplier you need to have to scale your current readings to your goal of one rotation, + or the custom set angle. Feel free to run a few more tests and average the results. Once you have + this multiplier, then replace `TURN_TICKS_TO_RADIANS` in the localizer with your multiplier. Make sure + you replace the number, not add on or multiply it to the previous number. The tuner takes into + account your current multiplier. +* Next, go on to `Forward Localizer Tuner`. You should re-enable `useIMU` at this time. You'll want to position a rule alongside your robot. + By default, you'll want to push the robot 30 inches forward. Once you've pushed that far, or whatever + you set that value to, then the forward multiplier will be shown as the second number shown. The + first number is how far the robot thinks you've gone, and the second number is the multiplier you + need to have to scale your current readings to your goal of 30 inches, or the custom set angle. + Feel free to run a few more tests and average the results. Once you have this multiplier, then + replace `FORWARD_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, + not add on or multiply it to the previous number. The tuner takes into account your current multiplier. +* Finally, go to `Lateral Localizer Tuner`. `useIMU` should be enabled for this step. You'll want to position a rule alongside your robot. + By default, you'll want to push the robot 30 inches to the right. Once you've pushed that far, or whatever + you set that value to, then the lateral multiplier will be shown as the second number shown. The + first number is how far the robot thinks you've gone, and the second number is the multiplier you + need to have to scale your current readings to your goal of 30 inches, or the custom set angle. + Feel free to run a few more tests and average the results. Once you have this multiplier, then + replace `STRAFE_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, + not add on or multiply it to the previous number. The tuner takes into account your current multiplier. +* Once you're done with all this, your localizer should be tuned. Make sure that `useIMU` is turned back on. To test it out, you can go to + `Localization Test` and push around or drive around your robot. Go to [FTC Dashboard](http://192.168.43.1:8080/dash) + and on the top right, switch the drop down from the default view to the field view. Then, on the bottom + left corner, you should see a field and the robot being drawn on the field. You can then move your + robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll + want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer! + ## Using Road Runner's Localizer Of course, many teams have experience using Road Runner in the past and so have localizers from Road Runner that are tuned. There is an adapter for the Road Runner three wheel localizer to the Pedro diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelIMULocalizer.Java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelIMULocalizer.Java new file mode 100644 index 0000000..69a1f03 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelIMULocalizer.Java @@ -0,0 +1,304 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; + +/** + * This is the ThreeWheelIMULocalizer class. This class extends the Localizer superclass and is a + * localizer that uses the three wheel odometry set up with the IMU to have more accurate heading + * readings. The diagram below, which is taken from Road Runner, shows a typical set up. + * + * The view is from the bottom of the robot looking upwards. + * + * left on robot is y pos + * + * front on robot is x pos + * + * /--------------\ + * | ____ | + * | ---- | + * | || || | + * | || || | left (y pos) + * | | + * | | + * \--------------/ + * front (x pos) + * + * @author Logan Nash + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 7/9/2024 + */ +@Config +public class ThreeWheelIMULocalizer extends Localizer { + private HardwareMap hardwareMap; + private Pose startPose; + private Pose displacementPose; + private Pose currentVelocity; + private Matrix prevRotationMatrix; + private NanoTimer timer; + private long deltaTimeNano; + private Encoder leftEncoder; + private Encoder rightEncoder; + private Encoder strafeEncoder; + private Pose leftEncoderPose; + private Pose rightEncoderPose; + private Pose strafeEncoderPose; + + public final IMU imu; + private double previousIMUOrientation; + private double deltaRadians; + private double totalHeading; + public static double FORWARD_TICKS_TO_INCHES = 0.002957;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963; + public static double STRAFE_TICKS_TO_INCHES = -0.003127403096038503;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659; + public static double TURN_TICKS_TO_RADIANS = 0.002995;//8192 * 1.37795 * 2 * Math.PI * 0.5; + + public static boolean useIMU = true; + + /** + * This creates a new ThreeWheelIMULocalizer from a HardwareMap, with a starting Pose at (0,0) + * facing 0 heading. + * + * @param map the HardwareMap + */ + public ThreeWheelIMULocalizer(HardwareMap map) { + this(map, new Pose()); + } + + /** + * This creates a new ThreeWheelIMULocalizer from a HardwareMap and a Pose, with the Pose + * specifying the starting pose of the localizer. + * + * @param map the HardwareMap + * @param setStartPose the Pose to start from + */ + public ThreeWheelIMULocalizer(HardwareMap map, Pose setStartPose) { + hardwareMap = map; + imu = hardwareMap.get(IMU.class, "imu"); + // TODO: replace this with your IMU's orientation + imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.LEFT, RevHubOrientationOnRobot.UsbFacingDirection.UP))); + + // TODO: replace these with your encoder positions + leftEncoderPose = new Pose(-3, 5.7, 0); + rightEncoderPose = new Pose(-3, -5.7, 0); + strafeEncoderPose = new Pose(6.9, 1, Math.toRadians(90)); + + + + // TODO: replace these with your encoder ports + leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "lb")); + rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "lf")); + strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rf")); + + // TODO: reverse any encoders necessary + leftEncoder.setDirection(Encoder.REVERSE); + rightEncoder.setDirection(Encoder.FORWARD); + strafeEncoder.setDirection(Encoder.FORWARD); + + setStartPose(setStartPose); + timer = new NanoTimer(); + deltaTimeNano = 1; + displacementPose = new Pose(); + currentVelocity = new Pose(); + totalHeading = 0; + + resetEncoders(); + } + + /** + * This returns the current pose estimate. + * + * @return returns the current pose estimate as a Pose + */ + @Override + public Pose getPose() { + return MathFunctions.addPoses(startPose, displacementPose); + } + + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Pose + */ + @Override + public Pose getVelocity() { + return currentVelocity.copy(); + } + + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Vector + */ + @Override + public Vector getVelocityVector() { + return currentVelocity.getVector(); + } + + /** + * This sets the start pose. Changing the start pose should move the robot as if all its + * previous movements were displacing it from its new start pose. + * + * @param setStart the new start pose + */ + @Override + public void setStartPose(Pose setStart) { + startPose = setStart; + } + + /** + * This sets the Matrix that contains the previous pose's heading rotation. + * + * @param heading the rotation of the Matrix + */ + public void setPrevRotationMatrix(double heading) { + prevRotationMatrix = new Matrix(3,3); + prevRotationMatrix.set(0, 0, Math.cos(heading)); + prevRotationMatrix.set(0, 1, -Math.sin(heading)); + prevRotationMatrix.set(1, 0, Math.sin(heading)); + prevRotationMatrix.set(1, 1, Math.cos(heading)); + prevRotationMatrix.set(2, 2, 1.0); + } + + /** + * This sets the current pose estimate. Changing this should just change the robot's current + * pose estimate, not anything to do with the start pose. + * + * @param setPose the new current pose estimate + */ + @Override + public void setPose(Pose setPose) { + displacementPose = MathFunctions.subtractPoses(setPose, startPose); + resetEncoders(); + } + + /** + * This updates the elapsed time timer that keeps track of time between updates, as well as the + * change position of the Encoders. Then, the robot's global change in position is calculated + * using the pose exponential method. + */ + @Override + public void update() { + deltaTimeNano = timer.getElapsedTime(); + timer.resetTimer(); + + updateEncoders(); + Matrix robotDeltas = getRobotDeltas(); + Matrix globalDeltas; + setPrevRotationMatrix(getPose().getHeading()); + + Matrix transformation = new Matrix(3,3); + if (Math.abs(robotDeltas.get(2, 0)) < 0.001) { + transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); + transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0); + transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0); + transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); + transformation.set(2, 2, 1.0); + } else { + transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); + transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0)); + transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0)); + transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); + transformation.set(2, 2, 1.0); + } + + globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas); + + displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0))); + currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9))); + + totalHeading += globalDeltas.get(2, 0); + } + + /** + * This updates the Encoders. + */ + public void updateEncoders() { + leftEncoder.update(); + rightEncoder.update(); + strafeEncoder.update(); + + double currentIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); + deltaRadians = MathFunctions.getTurnDirection(previousIMUOrientation, currentIMUOrientation) * MathFunctions.getSmallestAngleDifference(currentIMUOrientation, previousIMUOrientation); + previousIMUOrientation = currentIMUOrientation; + } + + /** + * This resets the Encoders. + */ + public void resetEncoders() { + leftEncoder.reset(); + rightEncoder.reset(); + strafeEncoder.reset(); + } + + /** + * This calculates the change in position from the perspective of the robot using information + * from the Encoders. + * + * @return returns a Matrix containing the robot relative movement. + */ + public Matrix getRobotDeltas() { + Matrix returnMatrix = new Matrix(3,1); + // x/forward movement + returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * ((rightEncoder.getDeltaPosition() * leftEncoderPose.getY() - leftEncoder.getDeltaPosition() * rightEncoderPose.getY()) / (leftEncoderPose.getY() - rightEncoderPose.getY()))); + //y/strafe movement + returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY())))); + // theta/turning + if (MathFunctions.getSmallestAngleDifference(0, deltaRadians) > 0.00005 && useIMU) { + returnMatrix.set(2, 0, deltaRadians); + } else { + returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY()))); + } + return returnMatrix; + } + + /** + * This returns how far the robot has turned in radians, in a number not clamped between 0 and + * 2 * pi radians. This is used for some tuning things and nothing actually within the following. + * + * @return returns how far the robot has turned in total, in radians. + */ + public double getTotalHeading() { + return totalHeading; + } + + /** + * This returns the multiplier applied to forward movement measurement to convert from encoder + * ticks to inches. This is found empirically through a tuner. + * + * @return returns the forward ticks to inches multiplier + */ + public double getForwardMultiplier() { + return FORWARD_TICKS_TO_INCHES; + } + + /** + * This returns the multiplier applied to lateral/strafe movement measurement to convert from + * encoder ticks to inches. This is found empirically through a tuner. + * + * @return returns the lateral/strafe ticks to inches multiplier + */ + public double getLateralMultiplier() { + return STRAFE_TICKS_TO_INCHES; + } + + /** + * This returns the multiplier applied to turning movement measurement to convert from encoder + * ticks to radians. This is found empirically through a tuner. + * + * @return returns the turning ticks to radians multiplier + */ + public double getTurningMultiplier() { + return TURN_TICKS_TO_RADIANS; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java index 5c4e932..c4284b9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java @@ -80,6 +80,7 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w hardwareMap = map; imu = hardwareMap.get(IMU.class, "imu"); + // TODO: replace this with your IMU's orientation imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.UP, RevHubOrientationOnRobot.UsbFacingDirection.LEFT))); // TODO: replace these with your encoder ports From 67f56a9931338683af1a681bddd9ff5b6f7553c8 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Thu, 18 Jul 2024 22:34:07 -0400 Subject: [PATCH 21/94] updated to the 9.2 FTC SDK and working on adding the kalman filter and filtered pidf to the drive pid --- .../src/main/AndroidManifest.xml | 4 +- .../samples/ConceptAprilTagMultiPortal.java | 104 ++ .../external/samples/ConceptDoubleVision.java | 202 --- .../ConceptTensorFlowObjectDetection.java | 199 --- .../ConceptTensorFlowObjectDetectionEasy.java | 142 -- ...rFlowObjectDetectionSwitchableCameras.java | 186 -- .../external/samples/SensorOctoQuad.java | 141 ++ .../external/samples/SensorOctoQuadAdv.java | 278 +++ .../external/samples/SensorSparkFunOTOS.java | 156 ++ .../samples/UtilityOctoQuadConfigMenu.java | 812 +++++++++ .../external/samples/sample_conventions.md | 5 + .../internal/FtcOpModeRegister.java | 4 +- .../pedroPathing/follower/Follower.java | 43 +- .../tuning/FollowerConstants.java | 17 +- .../util/CustomFilteredPIDFCoefficients.java | 70 + .../util/FilteredPIDFController.java | 245 +++ .../pedroPathing/util/KalmanFilter.java | 62 + .../util/KalmanFilterParameters.java | 25 + .../org/firstinspires/ftc/teamcode/readme.md | 1552 ++++++++++++++++- build.dependencies.gradle | 18 +- gradle.properties | 4 +- 21 files changed, 3430 insertions(+), 839 deletions(-) create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagMultiPortal.java delete mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptDoubleVision.java delete mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetection.java delete mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionEasy.java delete mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionSwitchableCameras.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorSparkFunOTOS.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityOctoQuadConfigMenu.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomFilteredPIDFCoefficients.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FilteredPIDFController.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilter.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilterParameters.java diff --git a/FtcRobotController/src/main/AndroidManifest.xml b/FtcRobotController/src/main/AndroidManifest.xml index 24ef7b2..eb87ce8 100644 --- a/FtcRobotController/src/main/AndroidManifest.xml +++ b/FtcRobotController/src/main/AndroidManifest.xml @@ -1,8 +1,8 @@ + android:versionCode="54" + android:versionName="9.2"> diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagMultiPortal.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagMultiPortal.java new file mode 100644 index 0000000..b364d7b --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagMultiPortal.java @@ -0,0 +1,104 @@ +/* Copyright (c) 2024 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +/** + * This OpMode demonstrates the basics of using multiple vision portals simultaneously + */ +@TeleOp(name = "Concept: AprilTagMultiPortal", group = "Concept") +@Disabled +public class ConceptAprilTagMultiPortal extends LinearOpMode +{ + VisionPortal portal1; + VisionPortal portal2; + + AprilTagProcessor aprilTagProcessor1; + AprilTagProcessor aprilTagProcessor2; + + @Override + public void runOpMode() throws InterruptedException + { + // Because we want to show two camera feeds simultaneously, we need to inform + // the SDK that we want it to split the camera monitor area into two smaller + // areas for us. It will then give us View IDs which we can pass to the individual + // vision portals to allow them to properly hook into the UI in tandem. + int[] viewIds = VisionPortal.makeMultiPortalView(2, VisionPortal.MultiPortalLayout.VERTICAL); + + // We extract the two view IDs from the array to make our lives a little easier later. + // NB: the array is 2 long because we asked for 2 portals up above. + int portal1ViewId = viewIds[0]; + int portal2ViewId = viewIds[1]; + + // If we want to run AprilTag detection on two portals simultaneously, + // we need to create two distinct instances of the AprilTag processor, + // one for each portal. If you want to see more detail about different + // options that you have when creating these processors, go check out + // the ConceptAprilTag OpMode. + aprilTagProcessor1 = AprilTagProcessor.easyCreateWithDefaults(); + aprilTagProcessor2 = AprilTagProcessor.easyCreateWithDefaults(); + + // Now we build both portals. The CRITICAL thing to notice here is the call to + // setLiveViewContainerId(), where we pass in the IDs we received earlier from + // makeMultiPortalView(). + portal1 = new VisionPortal.Builder() + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .setLiveViewContainerId(portal1ViewId) + .addProcessor(aprilTagProcessor1) + .build(); + portal2 = new VisionPortal.Builder() + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 2")) + .setLiveViewContainerId(portal2ViewId) + .addProcessor(aprilTagProcessor2) + .build(); + + waitForStart(); + + // Main Loop + while (opModeIsActive()) + { + // Just show some basic telemetry to demonstrate both processors are working in parallel + // on their respective cameras. If you want to see more detail about the information you + // can get back from the processor, you should look at ConceptAprilTag. + telemetry.addData("Number of tags in Camera 1", aprilTagProcessor1.getDetections().size()); + telemetry.addData("Number of tags in Camera 2", aprilTagProcessor2.getDetections().size()); + telemetry.update(); + sleep(20); + } + } +} \ No newline at end of file diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptDoubleVision.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptDoubleVision.java deleted file mode 100644 index 1d50049..0000000 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptDoubleVision.java +++ /dev/null @@ -1,202 +0,0 @@ -/* Copyright (c) 2023 FIRST. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted (subject to the limitations in the disclaimer below) provided that - * the following conditions are met: - * - * Redistributions of source code must retain the above copyright notice, this list - * of conditions and the following disclaimer. - * - * Redistributions in binary form must reproduce the above copyright notice, this - * list of conditions and the following disclaimer in the documentation and/or - * other materials provided with the distribution. - * - * Neither the name of FIRST nor the names of its contributors may be used to endorse or - * promote products derived from this software without specific prior written permission. - * - * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS - * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -package org.firstinspires.ftc.robotcontroller.external.samples; - -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.robotcore.external.tfod.Recognition; -import org.firstinspires.ftc.vision.VisionPortal; -import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; -import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; -import org.firstinspires.ftc.vision.tfod.TfodProcessor; - -import java.util.List; - -/* - * This OpMode illustrates the basics of using both AprilTag recognition and TensorFlow - * Object Detection. - * - * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. - * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. - */ -@TeleOp(name = "Concept: Double Vision", group = "Concept") -@Disabled -public class ConceptDoubleVision extends LinearOpMode { - private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera - - /** - * The variable to store our instance of the AprilTag processor. - */ - private AprilTagProcessor aprilTag; - - /** - * The variable to store our instance of the TensorFlow Object Detection processor. - */ - private TfodProcessor tfod; - - /** - * The variable to store our instance of the vision portal. - */ - private VisionPortal myVisionPortal; - - @Override - public void runOpMode() { - initDoubleVision(); - - // This OpMode loops continuously, allowing the user to switch between - // AprilTag and TensorFlow Object Detection (TFOD) image processors. - while (!isStopRequested()) { - - if (opModeInInit()) { - telemetry.addData("DS preview on/off","3 dots, Camera Stream"); - telemetry.addLine(); - telemetry.addLine("----------------------------------------"); - } - - if (myVisionPortal.getProcessorEnabled(aprilTag)) { - // User instructions: Dpad left or Dpad right. - telemetry.addLine("Dpad Left to disable AprilTag"); - telemetry.addLine(); - telemetryAprilTag(); - } else { - telemetry.addLine("Dpad Right to enable AprilTag"); - } - telemetry.addLine(); - telemetry.addLine("----------------------------------------"); - if (myVisionPortal.getProcessorEnabled(tfod)) { - telemetry.addLine("Dpad Down to disable TFOD"); - telemetry.addLine(); - telemetryTfod(); - } else { - telemetry.addLine("Dpad Up to enable TFOD"); - } - - // Push telemetry to the Driver Station. - telemetry.update(); - - if (gamepad1.dpad_left) { - myVisionPortal.setProcessorEnabled(aprilTag, false); - } else if (gamepad1.dpad_right) { - myVisionPortal.setProcessorEnabled(aprilTag, true); - } - if (gamepad1.dpad_down) { - myVisionPortal.setProcessorEnabled(tfod, false); - } else if (gamepad1.dpad_up) { - myVisionPortal.setProcessorEnabled(tfod, true); - } - - sleep(20); - - } // end while loop - - } // end method runOpMode() - - - /** - * Initialize AprilTag and TFOD. - */ - private void initDoubleVision() { - // ----------------------------------------------------------------------------------------- - // AprilTag Configuration - // ----------------------------------------------------------------------------------------- - - aprilTag = new AprilTagProcessor.Builder() - .build(); - - // ----------------------------------------------------------------------------------------- - // TFOD Configuration - // ----------------------------------------------------------------------------------------- - - tfod = new TfodProcessor.Builder() - .build(); - - // ----------------------------------------------------------------------------------------- - // Camera Configuration - // ----------------------------------------------------------------------------------------- - - if (USE_WEBCAM) { - myVisionPortal = new VisionPortal.Builder() - .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) - .addProcessors(tfod, aprilTag) - .build(); - } else { - myVisionPortal = new VisionPortal.Builder() - .setCamera(BuiltinCameraDirection.BACK) - .addProcessors(tfod, aprilTag) - .build(); - } - } // end initDoubleVision() - - /** - * Add telemetry about AprilTag detections. - */ - private void telemetryAprilTag() { - List currentDetections = aprilTag.getDetections(); - telemetry.addData("# AprilTags Detected", currentDetections.size()); - - // Step through the list of detections and display info for each one. - for (AprilTagDetection detection : currentDetections) { - if (detection.metadata != null) { - telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); - telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); - telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); - telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); - } else { - telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); - telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); - } - } // end for() loop - - } // end method telemetryAprilTag() - - /** - * Add telemetry about TensorFlow Object Detection (TFOD) recognitions. - */ - private void telemetryTfod() { - List currentRecognitions = tfod.getRecognitions(); - telemetry.addData("# Objects Detected", currentRecognitions.size()); - - // Step through the list of recognitions and display info for each one. - for (Recognition recognition : currentRecognitions) { - double x = (recognition.getLeft() + recognition.getRight()) / 2 ; - double y = (recognition.getTop() + recognition.getBottom()) / 2 ; - - telemetry.addData(""," "); - telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100); - telemetry.addData("- Position", "%.0f / %.0f", x, y); - telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight()); - } // end for() loop - - } // end method telemetryTfod() - -} // end class diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetection.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetection.java deleted file mode 100644 index f8e3688..0000000 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetection.java +++ /dev/null @@ -1,199 +0,0 @@ -/* Copyright (c) 2019 FIRST. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted (subject to the limitations in the disclaimer below) provided that - * the following conditions are met: - * - * Redistributions of source code must retain the above copyright notice, this list - * of conditions and the following disclaimer. - * - * Redistributions in binary form must reproduce the above copyright notice, this - * list of conditions and the following disclaimer in the documentation and/or - * other materials provided with the distribution. - * - * Neither the name of FIRST nor the names of its contributors may be used to endorse or - * promote products derived from this software without specific prior written permission. - * - * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS - * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -package org.firstinspires.ftc.robotcontroller.external.samples; - -import android.util.Size; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.robotcore.external.tfod.Recognition; -import org.firstinspires.ftc.vision.VisionPortal; -import org.firstinspires.ftc.vision.tfod.TfodProcessor; - -import java.util.List; - -/* - * This OpMode illustrates the basics of TensorFlow Object Detection, - * including Java Builder structures for specifying Vision parameters. - * - * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. - * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. - */ -@TeleOp(name = "Concept: TensorFlow Object Detection", group = "Concept") -@Disabled -public class ConceptTensorFlowObjectDetection extends LinearOpMode { - - private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera - - // TFOD_MODEL_ASSET points to a model file stored in the project Asset location, - // this is only used for Android Studio when using models in Assets. - private static final String TFOD_MODEL_ASSET = "MyModelStoredAsAsset.tflite"; - // TFOD_MODEL_FILE points to a model file stored onboard the Robot Controller's storage, - // this is used when uploading models directly to the RC using the model upload interface. - private static final String TFOD_MODEL_FILE = "/sdcard/FIRST/tflitemodels/myCustomModel.tflite"; - // Define the labels recognized in the model for TFOD (must be in training order!) - private static final String[] LABELS = { - "Pixel", - }; - - /** - * The variable to store our instance of the TensorFlow Object Detection processor. - */ - private TfodProcessor tfod; - - /** - * The variable to store our instance of the vision portal. - */ - private VisionPortal visionPortal; - - @Override - public void runOpMode() { - - initTfod(); - - // Wait for the DS start button to be touched. - telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); - telemetry.addData(">", "Touch Play to start OpMode"); - telemetry.update(); - waitForStart(); - - if (opModeIsActive()) { - while (opModeIsActive()) { - - telemetryTfod(); - - // Push telemetry to the Driver Station. - telemetry.update(); - - // Save CPU resources; can resume streaming when needed. - if (gamepad1.dpad_down) { - visionPortal.stopStreaming(); - } else if (gamepad1.dpad_up) { - visionPortal.resumeStreaming(); - } - - // Share the CPU. - sleep(20); - } - } - - // Save more CPU resources when camera is no longer needed. - visionPortal.close(); - - } // end runOpMode() - - /** - * Initialize the TensorFlow Object Detection processor. - */ - private void initTfod() { - - // Create the TensorFlow processor by using a builder. - tfod = new TfodProcessor.Builder() - - // With the following lines commented out, the default TfodProcessor Builder - // will load the default model for the season. To define a custom model to load, - // choose one of the following: - // Use setModelAssetName() if the custom TF Model is built in as an asset (AS only). - // Use setModelFileName() if you have downloaded a custom team model to the Robot Controller. - //.setModelAssetName(TFOD_MODEL_ASSET) - //.setModelFileName(TFOD_MODEL_FILE) - - // The following default settings are available to un-comment and edit as needed to - // set parameters for custom models. - //.setModelLabels(LABELS) - //.setIsModelTensorFlow2(true) - //.setIsModelQuantized(true) - //.setModelInputSize(300) - //.setModelAspectRatio(16.0 / 9.0) - - .build(); - - // Create the vision portal by using a builder. - VisionPortal.Builder builder = new VisionPortal.Builder(); - - // Set the camera (webcam vs. built-in RC phone camera). - if (USE_WEBCAM) { - builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")); - } else { - builder.setCamera(BuiltinCameraDirection.BACK); - } - - // Choose a camera resolution. Not all cameras support all resolutions. - //builder.setCameraResolution(new Size(640, 480)); - - // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. - //builder.enableLiveView(true); - - // Set the stream format; MJPEG uses less bandwidth than default YUY2. - //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); - - // Choose whether or not LiveView stops if no processors are enabled. - // If set "true", monitor shows solid orange screen if no processors enabled. - // If set "false", monitor shows camera view without annotations. - //builder.setAutoStopLiveView(false); - - // Set and enable the processor. - builder.addProcessor(tfod); - - // Build the Vision Portal, using the above settings. - visionPortal = builder.build(); - - // Set confidence threshold for TFOD recognitions, at any time. - //tfod.setMinResultConfidence(0.75f); - - // Disable or re-enable the TFOD processor at any time. - //visionPortal.setProcessorEnabled(tfod, true); - - } // end method initTfod() - - /** - * Add telemetry about TensorFlow Object Detection (TFOD) recognitions. - */ - private void telemetryTfod() { - - List currentRecognitions = tfod.getRecognitions(); - telemetry.addData("# Objects Detected", currentRecognitions.size()); - - // Step through the list of recognitions and display info for each one. - for (Recognition recognition : currentRecognitions) { - double x = (recognition.getLeft() + recognition.getRight()) / 2 ; - double y = (recognition.getTop() + recognition.getBottom()) / 2 ; - - telemetry.addData(""," "); - telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100); - telemetry.addData("- Position", "%.0f / %.0f", x, y); - telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight()); - } // end for() loop - - } // end method telemetryTfod() - -} // end class diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionEasy.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionEasy.java deleted file mode 100644 index 5c1b712..0000000 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionEasy.java +++ /dev/null @@ -1,142 +0,0 @@ -/* Copyright (c) 2019 FIRST. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted (subject to the limitations in the disclaimer below) provided that - * the following conditions are met: - * - * Redistributions of source code must retain the above copyright notice, this list - * of conditions and the following disclaimer. - * - * Redistributions in binary form must reproduce the above copyright notice, this - * list of conditions and the following disclaimer in the documentation and/or - * other materials provided with the distribution. - * - * Neither the name of FIRST nor the names of its contributors may be used to endorse or - * promote products derived from this software without specific prior written permission. - * - * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS - * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -package org.firstinspires.ftc.robotcontroller.external.samples; - -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.robotcore.external.tfod.Recognition; -import org.firstinspires.ftc.vision.VisionPortal; -import org.firstinspires.ftc.vision.tfod.TfodProcessor; - -import java.util.List; - -/* - * This OpMode illustrates the basics of TensorFlow Object Detection, using - * the easiest way. - * - * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. - * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. - */ -@TeleOp(name = "Concept: TensorFlow Object Detection Easy", group = "Concept") -@Disabled -public class ConceptTensorFlowObjectDetectionEasy extends LinearOpMode { - - private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera - - /** - * The variable to store our instance of the TensorFlow Object Detection processor. - */ - private TfodProcessor tfod; - - /** - * The variable to store our instance of the vision portal. - */ - private VisionPortal visionPortal; - - @Override - public void runOpMode() { - - initTfod(); - - // Wait for the DS start button to be touched. - telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); - telemetry.addData(">", "Touch Play to start OpMode"); - telemetry.update(); - waitForStart(); - - if (opModeIsActive()) { - while (opModeIsActive()) { - - telemetryTfod(); - - // Push telemetry to the Driver Station. - telemetry.update(); - - // Save CPU resources; can resume streaming when needed. - if (gamepad1.dpad_down) { - visionPortal.stopStreaming(); - } else if (gamepad1.dpad_up) { - visionPortal.resumeStreaming(); - } - - // Share the CPU. - sleep(20); - } - } - - // Save more CPU resources when camera is no longer needed. - visionPortal.close(); - - } // end runOpMode() - - /** - * Initialize the TensorFlow Object Detection processor. - */ - private void initTfod() { - - // Create the TensorFlow processor the easy way. - tfod = TfodProcessor.easyCreateWithDefaults(); - - // Create the vision portal the easy way. - if (USE_WEBCAM) { - visionPortal = VisionPortal.easyCreateWithDefaults( - hardwareMap.get(WebcamName.class, "Webcam 1"), tfod); - } else { - visionPortal = VisionPortal.easyCreateWithDefaults( - BuiltinCameraDirection.BACK, tfod); - } - - } // end method initTfod() - - /** - * Add telemetry about TensorFlow Object Detection (TFOD) recognitions. - */ - private void telemetryTfod() { - - List currentRecognitions = tfod.getRecognitions(); - telemetry.addData("# Objects Detected", currentRecognitions.size()); - - // Step through the list of recognitions and display info for each one. - for (Recognition recognition : currentRecognitions) { - double x = (recognition.getLeft() + recognition.getRight()) / 2 ; - double y = (recognition.getTop() + recognition.getBottom()) / 2 ; - - telemetry.addData(""," "); - telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100); - telemetry.addData("- Position", "%.0f / %.0f", x, y); - telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight()); - } // end for() loop - - } // end method telemetryTfod() - -} // end class diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionSwitchableCameras.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionSwitchableCameras.java deleted file mode 100644 index 1c0ed77..0000000 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionSwitchableCameras.java +++ /dev/null @@ -1,186 +0,0 @@ -/* Copyright (c) 2020 FIRST. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted (subject to the limitations in the disclaimer below) provided that - * the following conditions are met: - * - * Redistributions of source code must retain the above copyright notice, this list - * of conditions and the following disclaimer. - * - * Redistributions in binary form must reproduce the above copyright notice, this - * list of conditions and the following disclaimer in the documentation and/or - * other materials provided with the distribution. - * - * Neither the name of FIRST nor the names of its contributors may be used to endorse or - * promote products derived from this software without specific prior written permission. - * - * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS - * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -package org.firstinspires.ftc.robotcontroller.external.samples; - -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.firstinspires.ftc.robotcore.external.ClassFactory; -import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraName; -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.robotcore.external.tfod.Recognition; -import org.firstinspires.ftc.vision.VisionPortal; -import org.firstinspires.ftc.vision.VisionPortal.CameraState; -import org.firstinspires.ftc.vision.tfod.TfodProcessor; - -import java.util.List; - -/* - * This OpMode illustrates the basics of TensorFlow Object Detection, using - * two webcams. - * - * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. - * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. - */ -@TeleOp(name = "Concept: TensorFlow Object Detection Switchable Cameras", group = "Concept") -@Disabled -public class ConceptTensorFlowObjectDetectionSwitchableCameras extends LinearOpMode { - - /** - * Variables used for switching cameras. - */ - private WebcamName webcam1, webcam2; - private boolean oldLeftBumper; - private boolean oldRightBumper; - - /** - * The variable to store our instance of the TensorFlow Object Detection processor. - */ - private TfodProcessor tfod; - - /** - * The variable to store our instance of the vision portal. - */ - private VisionPortal visionPortal; - - @Override - public void runOpMode() { - - initTfod(); - - // Wait for the DS start button to be touched. - telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); - telemetry.addData(">", "Touch Play to start OpMode"); - telemetry.update(); - waitForStart(); - - if (opModeIsActive()) { - while (opModeIsActive()) { - - telemetryCameraSwitching(); - telemetryTfod(); - - // Push telemetry to the Driver Station. - telemetry.update(); - - // Save CPU resources; can resume streaming when needed. - if (gamepad1.dpad_down) { - visionPortal.stopStreaming(); - } else if (gamepad1.dpad_up) { - visionPortal.resumeStreaming(); - } - - doCameraSwitching(); - - // Share the CPU. - sleep(20); - } - } - - // Save more CPU resources when camera is no longer needed. - visionPortal.close(); - - } // end runOpMode() - - /** - * Initialize the TensorFlow Object Detection processor. - */ - private void initTfod() { - - // Create the TensorFlow processor by using a builder. - tfod = new TfodProcessor.Builder().build(); - - webcam1 = hardwareMap.get(WebcamName.class, "Webcam 1"); - webcam2 = hardwareMap.get(WebcamName.class, "Webcam 2"); - CameraName switchableCamera = ClassFactory.getInstance() - .getCameraManager().nameForSwitchableCamera(webcam1, webcam2); - - // Create the vision portal by using a builder. - visionPortal = new VisionPortal.Builder() - .setCamera(switchableCamera) - .addProcessor(tfod) - .build(); - - } // end method initTfod() - - /** - * Add telemetry about camera switching. - */ - private void telemetryCameraSwitching() { - if (visionPortal.getActiveCamera().equals(webcam1)) { - telemetry.addData("activeCamera", "Webcam 1"); - telemetry.addData("Press RightBumper", "to switch to Webcam 2"); - } else { - telemetry.addData("activeCamera", "Webcam 2"); - telemetry.addData("Press LeftBumper", "to switch to Webcam 1"); - } - } // end method telemetryCameraSwitching() - - /** - * Add telemetry about TensorFlow Object Detection (TFOD) recognitions. - */ - private void telemetryTfod() { - - List currentRecognitions = tfod.getRecognitions(); - telemetry.addData("# Objects Detected", currentRecognitions.size()); - - // Step through the list of recognitions and display info for each one. - for (Recognition recognition : currentRecognitions) { - double x = (recognition.getLeft() + recognition.getRight()) / 2 ; - double y = (recognition.getTop() + recognition.getBottom()) / 2 ; - - telemetry.addData(""," "); - telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100); - telemetry.addData("- Position", "%.0f / %.0f", x, y); - telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight()); - } // end for() loop - - } // end method telemetryTfod() - - /** - * Set the active camera according to input from the gamepad. - */ - private void doCameraSwitching() { - if (visionPortal.getCameraState() == CameraState.STREAMING) { - // If the left bumper is pressed, use Webcam 1. - // If the right bumper is pressed, use Webcam 2. - boolean newLeftBumper = gamepad1.left_bumper; - boolean newRightBumper = gamepad1.right_bumper; - if (newLeftBumper && !oldLeftBumper) { - visionPortal.setActiveCamera(webcam1); - } else if (newRightBumper && !oldRightBumper) { - visionPortal.setActiveCamera(webcam2); - } - oldLeftBumper = newLeftBumper; - oldRightBumper = newRightBumper; - } - } // end method doCameraSwitching() - -} // end class diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java new file mode 100644 index 0000000..71adee0 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java @@ -0,0 +1,141 @@ +/* + * Copyright (c) 2024 DigitalChickenLabs + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.digitalchickenlabs.OctoQuad; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import org.firstinspires.ftc.robotcore.external.Telemetry; + +/* + * This OpMode illustrates how to use the DigitalChickenLabs OctoQuad Quadrature Encoder & Pulse Width Interface Module + * + * The OctoQuad has 8 input channels that can used to read either Relative Quadrature Encoders or Pulse-Width Absolute Encoder inputs. + * Relative Quadrature encoders are found on most FTC motors, and some stand-alone position sensors like the REV Thru-Bore encoder. + * Pulse-Width encoders are less common. The REV Thru-Bore encoder can provide its absolute position via a variable pulse width, + * as can several sonar rangefinders such as the MaxBotix MB1000 series. + * + * This basic sample shows how an OctoQuad can be used to read the position three Odometry pods fitted + * with REV Thru-Bore encoders. For a more advanced example showing additional OctoQuad capabilities, see the SensorOctoQuadAdv sample. + * + * This OpMode assumes that the OctoQuad is attached to an I2C interface named "octoquad" in the robot configuration. + * + * The code assumes the first three OctoQuad inputs are connected as follows + * - Chan 0: for measuring forward motion on the left side of the robot. + * - Chan 1: for measuring forward motion on the right side of the robot. + * - Chan 2: for measuring Lateral (strafing) motion. + * + * The encoder values may be reset to zero by pressing the X (left most) button on Gamepad 1. + * + * This sample does not show how to interpret these readings, just how to obtain and display them. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * See the sensor's product page: https://www.tindie.com/products/digitalchickenlabs/octoquad-8ch-quadrature-pulse-width-decoder/ + */ +@TeleOp(name = "OctoQuad Basic", group="OctoQuad") +@Disabled +public class SensorOctoQuad extends LinearOpMode { + + // Identify which encoder OctoQuad inputs are connected to each odometry pod. + private final int ODO_LEFT = 0; // Facing forward direction on left side of robot (Axial motion) + private final int ODO_RIGHT = 1; // Facing forward direction on right side or robot (Axial motion ) + private final int ODO_PERP = 2; // Facing perpendicular direction at the center of the robot (Lateral motion) + + // Declare the OctoQuad object and members to store encoder positions and velocities + private OctoQuad octoquad; + + private int posLeft; + private int posRight; + private int posPerp; + + /** + * This function is executed when this OpMode is selected from the Driver Station. + */ + @Override + public void runOpMode() { + + // Connect to OctoQuad by referring to its name in the Robot Configuration. + octoquad = hardwareMap.get(OctoQuad.class, "octoquad"); + + // Read the Firmware Revision number from the OctoQuad and display it as telemetry. + telemetry.addData("OctoQuad Firmware Version ", octoquad.getFirmwareVersion()); + + // Reverse the count-direction of any encoder that is not what you require. + // Eg: if you push the robot forward and the left encoder counts down, then reverse it so it counts up. + octoquad.setSingleEncoderDirection(ODO_LEFT, OctoQuad.EncoderDirection.REVERSE); + octoquad.setSingleEncoderDirection(ODO_RIGHT, OctoQuad.EncoderDirection.FORWARD); + octoquad.setSingleEncoderDirection(ODO_PERP, OctoQuad.EncoderDirection.FORWARD); + + // Any changes that are made should be saved in FLASH just in case there is a sensor power glitch. + octoquad.saveParametersToFlash(); + + telemetry.addLine("\nPress Play to read encoder values"); + telemetry.update(); + + waitForStart(); + + // Configure the telemetry for optimal display of data. + telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); + telemetry.setMsTransmissionInterval(50); + + // Set all the encoder inputs to zero + octoquad.resetAllPositions(); + + // Loop while displaying the odometry pod positions. + while (opModeIsActive()) { + telemetry.addData(">", "Press X to Reset Encoders\n"); + + // Check for X button to reset encoders + if (gamepad1.x) { + // Reset the position of all encoders to zero. + octoquad.resetAllPositions(); + } + + // Read all the encoder data. Load into local members + readOdometryPods(); + + // Display the values + telemetry.addData("Left ", "%8d counts", posLeft); + telemetry.addData("Right", "%8d counts", posRight); + telemetry.addData("Perp ", "%8d counts", posPerp); + telemetry.update(); + } + } + + private void readOdometryPods() { + // For best performance, we should only perform ONE transaction with the OctoQuad each cycle. + // Since this example only needs to read positions from a few channels, we could use either + // readPositionRange(idxFirst, idxLast) to get a select number of sequential channels + // or + // readAllPositions() to get all 8 encoder readings + // + // Since both calls take almost the same amount of time, and the actual channels may not end up being sequential, + // we will read all of the encoder positions, and then pick out the ones we need. + int[] positions = octoquad.readAllPositions(); + posLeft = positions[ODO_LEFT]; + posRight = positions[ODO_RIGHT]; + posPerp = positions[ODO_PERP]; + } +} \ No newline at end of file diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java new file mode 100644 index 0000000..c87c05a --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java @@ -0,0 +1,278 @@ +/* + * Copyright (c) 2024 DigitalChickenLabs + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.digitalchickenlabs.OctoQuad; +import com.qualcomm.hardware.digitalchickenlabs.OctoQuadBase; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.MovingStatistics; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; + + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; + +import java.util.ArrayList; +import java.util.List; + +/* + * This OpMode illustrates how to use advanced features of the DigitalChickenLabs OctoQuad Quadrature Encoder & Pulse Width Interface Module + * + * The OctoQuad has 8 input channels that can used to read either Quadrature Encoder signals (like with most FTC motors) + * or Pulse-Width style Absolute Encoder inputs (eg: REV Through Bore encoder pulse width output). + * + * This OpMode illustrates several of the more advanced features of an OctoQuad, including Pulse Width measurement and velocity measurement. + * For a more basic OpMode just showing how to read encoder inputs, see the SensorOctoQuad sample. + * + * This OpMode assumes that the OctoQuad is attached to an I2C interface named "octoquad" in the robot configuration. + * + * One system that requires a lot of encoder inputs is a Swerve Drive system. + * Such a drive requires two encoders per drive module: one for position & velocity of the Drive motor/wheel, and one for position/angle of the Steering motor. + * The Drive motor usually requires a quadrature encoder, and the Steering motor requires an Absolute encoder. + * This quantity and combination of encoder inputs is a challenge, but a single OctoQuad could be used to read them all. + * + * This sample will configure an OctoQuad for a swerve drive, as follows + * - Configure 4 Relative Quadrature Encoder inputs and 4 Absolute Pulse-Width Encoder inputs + * - Configure a velocity sample interval of 25 mSec + * - Configure a pulse width input range of 1-1024 uSec for a REV Through Bore Encoder's Absolute Pulse output. + * + * An OctoSwerveDrive class will be created to initialize the OctoQuad, and manage the 4 swerve modules. + * An OctoSwerveModule class will be created to configure and read a single swerve module. + * + * Wiring: + * The OctoQuad will be configured to accept Quadrature encoders on the first four channels and Absolute (pulse width) encoders on the last four channels. + * + * The standard 4-pin to 4-pin cable can be used to connect each Driver Motor encoder to the OctoQuad. (channels 0-3) + * + * A modified version of the REV 6-4 pin cable (shipped with the encoder) connects the steering encoder to the OctoQuad. (channels 4-7) + * To connect the Absolute position signal from a REV Thru-Bore encoder to the OctoQuad, the Provided 6-pin to 4-pin cable will need to be modified. + * At the 6-pin connector end, move the yellow wire from its initial pin-3 position to the ABS pin-5 position. This can be done easily + * by using a small flathead screwdriver to lift the small white tab holding the metal wire crimp in place and gently pulling the wire out. + * See the OctoSwerveDrive() constructor below for the correct wheel/channel assignment. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * Note: If you prefer, you can move the two support classes from this file, and place them in their own files. + * But leaving them in place is simpler for this example. + * + * See the sensor's product page: https://www.tindie.com/products/digitalchickenlabs/octoquad-8ch-quadrature-pulse-width-decoder/ + */ +@TeleOp(name="OctoQuad Advanced", group="OctoQuad") +@Disabled +public class SensorOctoQuadAdv extends LinearOpMode { + + @Override + public void runOpMode() throws InterruptedException { + // Connect to the OctoQuad by looking up its name in the hardwareMap. + OctoQuad octoquad = hardwareMap.get(OctoQuad.class, "octoquad"); + + // Create the interface for the Swerve Drive Encoders + OctoSwerveDrive octoSwerveDrive = new OctoSwerveDrive(octoquad); + + // Display the OctoQuad firmware revision + telemetry.addLine("OctoQuad Firmware v" + octoquad.getFirmwareVersion()); + telemetry.addLine("\nPress Play to read encoder values"); + telemetry.update(); + + waitForStart(); + + // Configure the telemetry for optimal display of data. + telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); + telemetry.setMsTransmissionInterval(50); + + // Run stats to determine cycle times. + MovingStatistics avgTime = new MovingStatistics(100); + ElapsedTime elapsedTime = new ElapsedTime(); + + while (opModeIsActive()) { + telemetry.addData(">", "Press X to Reset Encoders\n"); + + if(gamepad1.x) { + octoquad.resetAllPositions(); + } + + // read all the swerve drive encoders and update positions and velocities. + octoSwerveDrive.updateModules(); + octoSwerveDrive.show(telemetry); + + // Update cycle time stats + avgTime.add(elapsedTime.nanoseconds()); + elapsedTime.reset(); + + telemetry.addData("Loop time", "%.1f mS", avgTime.getMean()/1000000); + telemetry.update(); + } + } +} + +// ============================ Internal (Inner) Classes ============================= + +/*** + * OctoSwerveDrive class manages 4 Swerve Modules + * - Performs general OctoQuad initialization + * - Creates 4 module classes, one for each swerve module + * - Updates swerve drive status by reading all data from OctoQuad and Updating each module + * - Displays all swerve drive data as telemetry + */ +class OctoSwerveDrive { + + private final OctoQuad octoquad; + private final List allModules = new ArrayList<>(); + + // members to hold encoder data for each module. + public final OctoSwerveModule LeftFront; + public final OctoSwerveModule RightFront; + public final OctoSwerveModule LeftBack; + public final OctoSwerveModule RightBack; + + // Prepare an object to hold an entire OctoQuad encoder readable register bank (pos and vel) + private final OctoQuad.EncoderDataBlock encoderDataBlock = new OctoQuad.EncoderDataBlock(); + + public OctoSwerveDrive(OctoQuad octoquad) { + this.octoquad = octoquad; + + // Clear out all prior settings and encoder data before setting up desired configuration + octoquad.resetEverything(); + + // Assume first 4 channels are relative encoders and the next 4 are absolute encoders + octoquad.setChannelBankConfig(OctoQuad.ChannelBankConfig.BANK1_QUADRATURE_BANK2_PULSE_WIDTH); + + // Create the four OctoSwerveModules, and add them to a 'list' for easier reference. + + // Note: The wheel/channel order is set here. Ensure your encoder cables match. + // + // Note: The angleOffset must be set for each module so a forward facing wheel shows a steer angle of 0 degrees. + // The process for determining the correct angleOffsets is as follows: + // 1) Set all the angleValues (below) to zero then build and deploy the code. + // 2) Physically rotate all the swerve drives so the wheels are facing forward in the desired 0 degree position + // 3) Run the OpMode, view the telemetry and record the "Degrees" value for each module. + // 4) Update the code by entering the recorded Degrees value for each module as the angleOffset (last) parameter in the lines below. + // + // Rebuild and deploy the new code. Verify that the telemetry now indicates 0 degrees when the wheels are facing forward. + // Also verify that the correct module values change appropriately when you manually spin (drive) and rotate (steer) a wheel. + + allModules.add(LeftFront = new OctoSwerveModule(octoquad, "LF ",0,0));// Drive = 0, Steer = 4 + allModules.add(RightFront = new OctoSwerveModule(octoquad, "RF ",1,0));// Drive = 1, Steer = 5 + allModules.add(LeftBack = new OctoSwerveModule(octoquad, "LB ",2,0));// Drive = 2, Steer = 6 + allModules.add(RightBack = new OctoSwerveModule(octoquad, "RB ",3,0));// Drive = 3, Steer = 7 + + // now make sure the settings persist through any power glitches. + octoquad.saveParametersToFlash(); + } + + /** + * Updates all 4 swerve modules + */ + public void updateModules() { + // Read full OctoQuad data block and then pass DataBlock to each swerve module to update themselves. + octoquad.readAllEncoderData(encoderDataBlock); + for (OctoSwerveModule module : allModules) { + module.updateModule(encoderDataBlock); + } + } + + /** + * Generate telemetry data for all modules. + * @param telemetry OpMode Telemetry object + */ + public void show(Telemetry telemetry) { + // create general header block and then have each module add its own telemetry + telemetry.addData("pos", " Count CPS Degree DPS"); + for (OctoSwerveModule module : allModules) { + module.show(telemetry); + } + } +} + +/*** + * The OctoSwerveModule class manages a single swerve module + */ +class OctoSwerveModule { + + public double driveCounts; + public double driveCountsPerSec; + public double steerDegrees; + public double steerDegreesPerSec; + + private final String name; + private final int channel; + private final double angleOffset; + private final double steerDirMult; + + private static final int VELOCITY_SAMPLE_INTERVAL_MS = 25; // To provide 40 updates per second. + private static final double DEGREES_PER_US = (360.0 / 1024.0); // based on REV Through Bore Encoder + private static final double VELOCITY_SAMPLES_PER_S = (1000.0 / VELOCITY_SAMPLE_INTERVAL_MS); + + // The correct drive and turn directions must be set for the Swerve Module based on the specific hardware geometry. + // Forward motion must generate an increasing drive count. + // Counter Clockwise steer rotation must generate an increasing Steer Angle (degrees) + private static final boolean INVERT_DRIVE_ENCODER = false; // Set true if forward motion decreases drive "Count" + private static final boolean INVERT_STEER_ENCODER = false; // Set true if counter clockwise steer action decreases steer "Degree" + + /*** + * @param octoquad provide access to configure OctoQuad + * @param name name used for telemetry display + * @param quadChannel Quadrature encoder channel. Pulse Width channel is this + 4 + * @param angleOffset Angle to subtract from absolute encoder to calibrate zero position. (see comments above) + */ + public OctoSwerveModule (OctoQuad octoquad, String name, int quadChannel, double angleOffset) { + this.name = name; + this.channel = quadChannel; + this.angleOffset = angleOffset; + this.steerDirMult = INVERT_STEER_ENCODER ? -1 : 1 ; // create a multiplier to flip the steer angle. + + // Set the drive encoder direction. Note the absolute encoder does not have built-in direction inversion. + octoquad.setSingleEncoderDirection(channel, INVERT_DRIVE_ENCODER ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD); + + // Set the velocity sample interval on both encoders + octoquad.setSingleVelocitySampleInterval(channel, VELOCITY_SAMPLE_INTERVAL_MS); + octoquad.setSingleVelocitySampleInterval(channel + 4, VELOCITY_SAMPLE_INTERVAL_MS); + + // Setup Absolute encoder pulse range to match REV Through Bore encoder. + octoquad.setSingleChannelPulseWidthParams (channel + 4, new OctoQuad.ChannelPulseWidthParams(1,1024)); + } + + /*** + * Calculate the Swerve module's position and velocity values + * @param encoderDataBlock most recent full data block read from OctoQuad. + */ + public void updateModule(OctoQuad.EncoderDataBlock encoderDataBlock) { + driveCounts = encoderDataBlock.positions[channel]; // get Counts. + driveCountsPerSec = encoderDataBlock.velocities[channel] * VELOCITY_SAMPLES_PER_S; // convert counts/interval to counts/sec + + // convert uS to degrees. Add in any possible direction flip. + steerDegrees = AngleUnit.normalizeDegrees((encoderDataBlock.positions[channel+ 4] * DEGREES_PER_US * steerDirMult) - angleOffset); + // convert uS/interval to deg per sec. Add in any possible direction flip. + steerDegreesPerSec = encoderDataBlock.velocities[channel + 4] * DEGREES_PER_US * steerDirMult * VELOCITY_SAMPLES_PER_S; + } + + /** + * Display the Swerve module's state as telemetry + * @param telemetry OpMode Telemetry object + */ + public void show(Telemetry telemetry) { + telemetry.addData(name, "%8.0f %7.0f %7.0f %6.0f", driveCounts, driveCountsPerSec, steerDegrees, steerDegreesPerSec); + } +} \ No newline at end of file diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorSparkFunOTOS.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorSparkFunOTOS.java new file mode 100644 index 0000000..34ad8f0 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorSparkFunOTOS.java @@ -0,0 +1,156 @@ +/* + SPDX-License-Identifier: MIT + + Copyright (c) 2024 SparkFun Electronics +*/ +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import com.qualcomm.hardware.sparkfun.SparkFunOTOS; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +/* + * This OpMode illustrates how to use the SparkFun Qwiic Optical Tracking Odometry Sensor (OTOS) + * + * The OpMode assumes that the sensor is configured with a name of "sensor_otos". + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * See the sensor's product page: https://www.sparkfun.com/products/24904 + */ +@TeleOp(name = "Sensor: SparkFun OTOS", group = "Sensor") +@Disabled +public class SensorSparkFunOTOS extends LinearOpMode { + // Create an instance of the sensor + SparkFunOTOS myOtos; + + @Override + public void runOpMode() throws InterruptedException { + // Get a reference to the sensor + myOtos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos"); + + // All the configuration for the OTOS is done in this helper method, check it out! + configureOtos(); + + // Wait for the start button to be pressed + waitForStart(); + + // Loop until the OpMode ends + while (opModeIsActive()) { + // Get the latest position, which includes the x and y coordinates, plus the + // heading angle + SparkFunOTOS.Pose2D pos = myOtos.getPosition(); + + // Reset the tracking if the user requests it + if (gamepad1.y) { + myOtos.resetTracking(); + } + + // Re-calibrate the IMU if the user requests it + if (gamepad1.x) { + myOtos.calibrateImu(); + } + + // Inform user of available controls + telemetry.addLine("Press Y (triangle) on Gamepad to reset tracking"); + telemetry.addLine("Press X (square) on Gamepad to calibrate the IMU"); + telemetry.addLine(); + + // Log the position to the telemetry + telemetry.addData("X coordinate", pos.x); + telemetry.addData("Y coordinate", pos.y); + telemetry.addData("Heading angle", pos.h); + + // Update the telemetry on the driver station + telemetry.update(); + } + } + + private void configureOtos() { + telemetry.addLine("Configuring OTOS..."); + telemetry.update(); + + // Set the desired units for linear and angular measurements. Can be either + // meters or inches for linear, and radians or degrees for angular. If not + // set, the default is inches and degrees. Note that this setting is not + // persisted in the sensor, so you need to set at the start of all your + // OpModes if using the non-default value. + // myOtos.setLinearUnit(DistanceUnit.METER); + myOtos.setLinearUnit(DistanceUnit.INCH); + // myOtos.setAngularUnit(AnguleUnit.RADIANS); + myOtos.setAngularUnit(AngleUnit.DEGREES); + + // Assuming you've mounted your sensor to a robot and it's not centered, + // you can specify the offset for the sensor relative to the center of the + // robot. The units default to inches and degrees, but if you want to use + // different units, specify them before setting the offset! Note that as of + // firmware version 1.0, these values will be lost after a power cycle, so + // you will need to set them each time you power up the sensor. For example, if + // the sensor is mounted 5 inches to the left (negative X) and 10 inches + // forward (positive Y) of the center of the robot, and mounted 90 degrees + // clockwise (negative rotation) from the robot's orientation, the offset + // would be {-5, 10, -90}. These can be any value, even the angle can be + // tweaked slightly to compensate for imperfect mounting (eg. 1.3 degrees). + SparkFunOTOS.Pose2D offset = new SparkFunOTOS.Pose2D(0, 0, 0); + myOtos.setOffset(offset); + + // Here we can set the linear and angular scalars, which can compensate for + // scaling issues with the sensor measurements. Note that as of firmware + // version 1.0, these values will be lost after a power cycle, so you will + // need to set them each time you power up the sensor. They can be any value + // from 0.872 to 1.127 in increments of 0.001 (0.1%). It is recommended to + // first set both scalars to 1.0, then calibrate the angular scalar, then + // the linear scalar. To calibrate the angular scalar, spin the robot by + // multiple rotations (eg. 10) to get a precise error, then set the scalar + // to the inverse of the error. Remember that the angle wraps from -180 to + // 180 degrees, so for example, if after 10 rotations counterclockwise + // (positive rotation), the sensor reports -15 degrees, the required scalar + // would be 3600/3585 = 1.004. To calibrate the linear scalar, move the + // robot a known distance and measure the error; do this multiple times at + // multiple speeds to get an average, then set the linear scalar to the + // inverse of the error. For example, if you move the robot 100 inches and + // the sensor reports 103 inches, set the linear scalar to 100/103 = 0.971 + myOtos.setLinearScalar(1.0); + myOtos.setAngularScalar(1.0); + + // The IMU on the OTOS includes a gyroscope and accelerometer, which could + // have an offset. Note that as of firmware version 1.0, the calibration + // will be lost after a power cycle; the OTOS performs a quick calibration + // when it powers up, but it is recommended to perform a more thorough + // calibration at the start of all your OpModes. Note that the sensor must + // be completely stationary and flat during calibration! When calling + // calibrateImu(), you can specify the number of samples to take and whether + // to wait until the calibration is complete. If no parameters are provided, + // it will take 255 samples and wait until done; each sample takes about + // 2.4ms, so about 612ms total + myOtos.calibrateImu(); + + // Reset the tracking algorithm - this resets the position to the origin, + // but can also be used to recover from some rare tracking errors + myOtos.resetTracking(); + + // After resetting the tracking, the OTOS will report that the robot is at + // the origin. If your robot does not start at the origin, or you have + // another source of location information (eg. vision odometry), you can set + // the OTOS location to match and it will continue to track from there. + SparkFunOTOS.Pose2D currentPosition = new SparkFunOTOS.Pose2D(0, 0, 0); + myOtos.setPosition(currentPosition); + + // Get the hardware and firmware version + SparkFunOTOS.Version hwVersion = new SparkFunOTOS.Version(); + SparkFunOTOS.Version fwVersion = new SparkFunOTOS.Version(); + myOtos.getVersionInfo(hwVersion, fwVersion); + + telemetry.addLine("OTOS configured! Press start to get position data!"); + telemetry.addLine(); + telemetry.addLine(String.format("OTOS Hardware Version: v%d.%d", hwVersion.major, hwVersion.minor)); + telemetry.addLine(String.format("OTOS Firmware Version: v%d.%d", fwVersion.major, fwVersion.minor)); + telemetry.update(); + } +} \ No newline at end of file diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityOctoQuadConfigMenu.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityOctoQuadConfigMenu.java new file mode 100644 index 0000000..45d4e24 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityOctoQuadConfigMenu.java @@ -0,0 +1,812 @@ +/* + * Copyright (c) 2024 DigitalChickenLabs + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.digitalchickenlabs.OctoQuad; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +import java.util.ArrayList; +import java.util.Stack; + +/* + * This OpMode illustrates how to use the DigitalChickenLabs OctoQuad Quadrature Encoder & Pulse Width Interface Module. + * + * The OctoQuad has 8 input channels that can used to read either Relative Quadrature Encoders or Pulse-Width Absolute Encoder inputs. + * Relative Quadrature encoders are found on most FTC motors, and some stand-alone position sensors like the REV Thru-Bore encoder. + * Pulse-Width encoders are less common. The REV Thru-Bore encoder can provide its absolute position via a variable pulse width, + * as can several sonar rangefinders such as the MaxBotix MB1000 series. + * + * This OpMode assumes that the OctoQuad is attached to an I2C interface named "octoquad" in the robot configuration. + * + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * Select, Init and run the "OctoQuad Configuration Tool" OpMode + * Read the blue User-Interface tips at the top of the telemetry screen. + * Use the UI buttons to navigate the menu and make any desired changes to the OctoQuad configuration. + * Use the Program Settings To FLASH option to make any changes permanent. + * + * See the sensor's product page: https://www.tindie.com/products/digitalchickenlabs/octoquad-8ch-quadrature-pulse-width-decoder/ + */ +@TeleOp(name = "OctoQuad Configuration Tool", group="OctoQuad") +@Disabled +public class UtilityOctoQuadConfigMenu extends LinearOpMode +{ + TelemetryMenu.MenuElement rootMenu = new TelemetryMenu.MenuElement("OctoQuad Config Menu", true); + TelemetryMenu.MenuElement menuHwInfo = new TelemetryMenu.MenuElement("Hardware Information", false); + TelemetryMenu.EnumOption optionI2cResetMode; + TelemetryMenu.EnumOption optionChannelBankConfig; + + TelemetryMenu.MenuElement menuEncoderDirections = new TelemetryMenu.MenuElement("Set Encoder Directions", false); + TelemetryMenu.BooleanOption[] optionsEncoderDirections = new TelemetryMenu.BooleanOption[OctoQuad.NUM_ENCODERS]; + + TelemetryMenu.MenuElement menuVelocityIntervals = new TelemetryMenu.MenuElement("Velocity Measurement Intervals", false); + TelemetryMenu.IntegerOption[] optionsVelocityIntervals = new TelemetryMenu.IntegerOption[OctoQuad.NUM_ENCODERS]; + + TelemetryMenu.MenuElement menuAbsParams = new TelemetryMenu.MenuElement("Abs. Encoder Pulse Width Params", false); + TelemetryMenu.IntegerOption[] optionsAbsParamsMax = new TelemetryMenu.IntegerOption[OctoQuad.NUM_ENCODERS]; + TelemetryMenu.IntegerOption[] optionsAbsParamsMin = new TelemetryMenu.IntegerOption[OctoQuad.NUM_ENCODERS]; + + TelemetryMenu.OptionElement optionProgramToFlash; + TelemetryMenu.OptionElement optionSendToRAM; + + TelemetryMenu.StaticClickableOption optionExit; + + OctoQuad octoquad; + + boolean error = false; + + @Override + public void runOpMode() + { + octoquad = hardwareMap.getAll(OctoQuad.class).get(0); + + if(octoquad.getChipId() != OctoQuad.OCTOQUAD_CHIP_ID) + { + telemetry.addLine("Error: cannot communicate with OctoQuad. Check your wiring and configuration and try again"); + telemetry.update(); + + error = true; + } + else + { + if(octoquad.getFirmwareVersion().maj != OctoQuad.SUPPORTED_FW_VERSION_MAJ) + { + telemetry.addLine("Error: The OctoQuad is running a different major firmware version than this driver was built for. Cannot run configuration tool"); + telemetry.update(); + + error = true; + } + } + + if(error) + { + waitForStart(); + return; + } + + telemetry.addLine("Retrieving current configuration from OctoQuad"); + telemetry.update(); + + optionExit = new TelemetryMenu.StaticClickableOption("Exit configuration menu") + { + @Override + void onClick() // called on OpMode thread + { + requestOpModeStop(); + } + }; + + optionI2cResetMode = new TelemetryMenu.EnumOption("I2C Reset Mode", OctoQuad.I2cRecoveryMode.values(), octoquad.getI2cRecoveryMode()); + optionChannelBankConfig = new TelemetryMenu.EnumOption("Channel Bank Modes", OctoQuad.ChannelBankConfig.values(), octoquad.getChannelBankConfig()); + + menuHwInfo.addChild(new TelemetryMenu.StaticItem("Board Firmware: v" + octoquad.getFirmwareVersion())); + //menuHwInfo.addChild(new TelemetryMenu.StaticItem("Board unique ID: FIXME")); + + for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++) + { + optionsEncoderDirections[i] = new TelemetryMenu.BooleanOption( + String.format("Encoder %d direction", i), + octoquad.getSingleEncoderDirection(i) == OctoQuad.EncoderDirection.REVERSE, + "-", + "+"); + } + menuEncoderDirections.addChildren(optionsEncoderDirections); + + for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++) + { + optionsVelocityIntervals[i] = new TelemetryMenu.IntegerOption( + String.format("Chan %d velocity intvl", i), + OctoQuad.MIN_VELOCITY_MEASUREMENT_INTERVAL_MS, + OctoQuad.MAX_VELOCITY_MEASUREMENT_INTERVAL_MS, + octoquad.getSingleVelocitySampleInterval(i)); + } + menuVelocityIntervals.addChildren(optionsVelocityIntervals); + + for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++) + { + OctoQuad.ChannelPulseWidthParams params = octoquad.getSingleChannelPulseWidthParams(i); + + optionsAbsParamsMax[i] = new TelemetryMenu.IntegerOption( + String.format("Chan %d max pulse length", i), + OctoQuad.MIN_PULSE_WIDTH_US, + OctoQuad.MAX_PULSE_WIDTH_US, + params.max_length_us); + + optionsAbsParamsMin[i] = new TelemetryMenu.IntegerOption( + String.format("Chan %d min pulse length", i), + OctoQuad.MIN_PULSE_WIDTH_US, + OctoQuad.MAX_PULSE_WIDTH_US, + params.min_length_us); + } + menuAbsParams.addChildren(optionsAbsParamsMin); + menuAbsParams.addChildren(optionsAbsParamsMax); + + optionProgramToFlash = new TelemetryMenu.OptionElement() + { + String name = "Program Settings to FLASH"; + long lastClickTime = 0; + + @Override + protected String getDisplayText() + { + if(lastClickTime == 0) + { + return name; + } + else + { + if(System.currentTimeMillis() - lastClickTime < 1000) + { + return name + " **OK**"; + } + else + { + lastClickTime = 0; + return name; + } + } + } + + @Override + void onClick() + { + sendSettingsToRam(); + octoquad.saveParametersToFlash(); + lastClickTime = System.currentTimeMillis(); + } + }; + + optionSendToRAM = new TelemetryMenu.OptionElement() + { + String name = "Send Settings to RAM"; + long lastClickTime = 0; + + @Override + protected String getDisplayText() + { + if(lastClickTime == 0) + { + return name; + } + else + { + if(System.currentTimeMillis() - lastClickTime < 1000) + { + return name + " **OK**"; + } + else + { + lastClickTime = 0; + return name; + } + } + } + + @Override + void onClick() + { + sendSettingsToRam(); + lastClickTime = System.currentTimeMillis(); + } + }; + + rootMenu.addChild(menuHwInfo); + rootMenu.addChild(optionI2cResetMode); + rootMenu.addChild(optionChannelBankConfig); + rootMenu.addChild(menuEncoderDirections); + rootMenu.addChild(menuVelocityIntervals); + rootMenu.addChild(menuAbsParams); + rootMenu.addChild(optionProgramToFlash); + rootMenu.addChild(optionSendToRAM); + rootMenu.addChild(optionExit); + + TelemetryMenu menu = new TelemetryMenu(telemetry, rootMenu); + + while (!isStopRequested()) + { + menu.loop(gamepad1); + telemetry.update(); + sleep(20); + } + } + + void sendSettingsToRam() + { + for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++) + { + octoquad.setSingleEncoderDirection(i, optionsEncoderDirections[i].getValue() ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD); + octoquad.setSingleVelocitySampleInterval(i, optionsVelocityIntervals[i].getValue()); + + OctoQuad.ChannelPulseWidthParams params = new OctoQuad.ChannelPulseWidthParams(); + params.max_length_us = optionsAbsParamsMax[i].getValue(); + params.min_length_us = optionsAbsParamsMin[i].getValue(); + + octoquad.setSingleChannelPulseWidthParams(i, params); + } + + octoquad.setI2cRecoveryMode((OctoQuad.I2cRecoveryMode) optionI2cResetMode.getValue()); + octoquad.setChannelBankConfig((OctoQuad.ChannelBankConfig) optionChannelBankConfig.getValue()); + } + + /* + * Copyright (c) 2023 OpenFTC Team + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + + public static class TelemetryMenu + { + private final MenuElement root; + private MenuElement currentLevel; + + private boolean dpadUpPrev; + private boolean dpadDnPrev; + private boolean dpadRightPrev; + private boolean dpadLeftPrev; + private boolean xPrev; + private boolean lbPrev; + + private int selectedIdx = 0; + private Stack selectedIdxStack = new Stack<>(); + + private final Telemetry telemetry; + + /** + * TelemetryMenu constructor + * @param telemetry pass in 'telemetry' from your OpMode + * @param root the root menu element + */ + public TelemetryMenu(Telemetry telemetry, MenuElement root) + { + this.root = root; + this.currentLevel = root; + this.telemetry = telemetry; + + telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); + telemetry.setMsTransmissionInterval(50); + } + + /** + * Call this from inside your loop to put the current menu state into + * telemetry, and process gamepad inputs for navigating the menu + * @param gamepad the gamepad you want to use to navigate the menu + */ + public void loop(Gamepad gamepad) + { + // Capture current state of the gamepad buttons we care about; + // We can only look once or we risk a race condition + boolean dpadUp = gamepad.dpad_up; + boolean dpadDn = gamepad.dpad_down; + boolean dpadRight = gamepad.dpad_right; + boolean dpadLeft = gamepad.dpad_left; + boolean x = gamepad.x; + boolean lb = gamepad.left_bumper; + + // Figure out who our children our at this level + // and figure out which item is currently highlighted + // with the selection pointer + ArrayList children = currentLevel.children(); + Element currentSelection = children.get(selectedIdx); + + // Left and right are inputs to the selected item (if it's an Option) + if (currentSelection instanceof OptionElement) + { + if (dpadRight && !dpadRightPrev) // rising edge + { + ((OptionElement) currentSelection).onRightInput(); + } + else if (dpadLeft && !dpadLeftPrev) // rising edge + { + ((OptionElement) currentSelection).onLeftInput(); + } + } + + // Up and down navigate the current selection pointer + if (dpadUp && !dpadUpPrev) // rising edge + { + selectedIdx--; // Move selection pointer up + } + else if (dpadDn && !dpadDnPrev) // rising edge + { + selectedIdx++; // Move selection pointer down + } + + // Make selected index sane (don't let it go out of bounds) :eyes: + if (selectedIdx >= children.size()) + { + selectedIdx = children.size()-1; + } + else if (selectedIdx < 0) + { + selectedIdx = 0; + } + + // Select: either enter submenu or input to option + else if (x && !xPrev) // rising edge + { + // Select up element + if (currentSelection instanceof SpecialUpElement) + { + // We can only go up if we're not at the root level + if (currentLevel != root) + { + // Restore selection pointer to where it was before + selectedIdx = selectedIdxStack.pop(); + + // Change to the parent level + currentLevel = currentLevel.parent(); + } + } + // Input to option + else if (currentSelection instanceof OptionElement) + { + ((OptionElement) currentSelection).onClick(); + } + // Enter submenu + else if (currentSelection instanceof MenuElement) + { + // Save our current selection pointer so we can restore it + // later if the user navigates back up a level + selectedIdxStack.push(selectedIdx); + + // We have no idea what's in the submenu :monkey: so best to + // just set the selection pointer to the first element + selectedIdx = 0; + + // Now the current level becomes the submenu that the selection + // pointer was on + currentLevel = (MenuElement) currentSelection; + } + } + + // Go up a level + else if (lb && !lbPrev) + { + // We can only go up if we're not at the root level + if (currentLevel != root) + { + // Restore selection pointer to where it was before + selectedIdx = selectedIdxStack.pop(); + + // Change to the parent level + currentLevel = currentLevel.parent(); + } + } + + // Save the current button states so that we can look for + // the rising edge the next time around the loop :) + dpadUpPrev = dpadUp; + dpadDnPrev = dpadDn; + dpadRightPrev = dpadRight; + dpadLeftPrev = dpadLeft; + xPrev = x; + lbPrev = lb; + + // Start building the text display. + // First, we add the static directions for gamepad operation + StringBuilder builder = new StringBuilder(); + builder.append(""); + builder.append("Navigate items.....dpad up/down\n") + .append("Select.............X\n") + .append("Edit option........dpad left/right\n") + .append("Up one level.......left bumper\n"); + builder.append(""); + builder.append("\n"); + + // Now actually add the menu options. We start by adding the name of the current menu level. + builder.append(""); + builder.append("Current Menu: ").append(currentLevel.name).append("\n"); + + // Now we loop through all the child elements of this level and add them + for (int i = 0; i < children.size(); i++) + { + // If the selection pointer is at this index, put a green dot in the box :) + if (selectedIdx == i) + { + builder.append("[•] "); + } + // Otherwise, just put an empty box + else + { + builder.append("[ ] "); + } + + // Figure out who the selection pointer is pointing at :eyes: + Element e = children.get(i); + + // If it's pointing at a submenu, indicate that it's a submenu to the user + // by prefixing "> " to the name. + if (e instanceof MenuElement) + { + builder.append("> "); + } + + // Finally, add the element's name + builder.append(e.getDisplayText()); + + // We musn't forget the newline + builder.append("\n"); + } + + // Don't forget to close the font tag either + builder.append(""); + + // Build the string!!!! :nerd: + String menu = builder.toString(); + + // Add it to telemetry + telemetry.addLine(menu); + } + + public static class MenuElement extends Element + { + private String name; + private ArrayList children = new ArrayList<>(); + + /** + * Create a new MenuElement; may either be the root menu, or a submenu (set isRoot accordingly) + * @param name the name for this menu + * @param isRoot whether this is a root menu, or a submenu + */ + public MenuElement(String name, boolean isRoot) + { + this.name = name; + + // If it's not the root menu, we add the up one level option as the first element + if (!isRoot) + { + children.add(new SpecialUpElement()); + } + } + + /** + * Add a child element to this menu (may either be an Option or another menu) + * @param child the child element to add + */ + public void addChild(Element child) + { + child.setParent(this); + children.add(child); + } + + /** + * Add multiple child elements to this menu (may either be option, or another menu) + * @param children the children to add + */ + public void addChildren(Element[] children) + { + for (Element e : children) + { + e.setParent(this); + this.children.add(e); + } + } + + @Override + protected String getDisplayText() + { + return name; + } + + private ArrayList children() + { + return children; + } + } + + public static abstract class OptionElement extends Element + { + /** + * Override this to get notified when the element is clicked + */ + void onClick() {} + + /** + * Override this to get notified when the element gets a "left edit" input + */ + protected void onLeftInput() {} + + /** + * Override this to get notified when the element gets a "right edit" input + */ + protected void onRightInput() {} + } + + public static class EnumOption extends OptionElement + { + protected int idx = 0; + protected Enum[] e; + protected String name; + + public EnumOption(String name, Enum[] e) + { + this.e = e; + this.name = name; + } + + public EnumOption(String name, Enum[] e, Enum def) + { + this(name, e); + idx = def.ordinal(); + } + + @Override + public void onLeftInput() + { + idx++; + + if(idx > e.length-1) + { + idx = 0; + } + } + + @Override + public void onRightInput() + { + idx--; + + if(idx < 0) + { + idx = e.length-1; + } + } + + @Override + public void onClick() + { + onRightInput(); + } + + @Override + protected String getDisplayText() + { + return String.format("%s: %s", name, e[idx].name()); + } + + public Enum getValue() + { + return e[idx]; + } + } + + public static class IntegerOption extends OptionElement + { + protected int i; + protected int min; + protected int max; + protected String name; + + public IntegerOption(String name, int min, int max, int def) + { + this.name = name; + this.min = min; + this.max = max; + this.i = def; + } + + @Override + public void onLeftInput() + { + i--; + + if(i < min) + { + i = max; + } + } + + @Override + public void onRightInput() + { + i++; + + if(i > max) + { + i = min; + } + } + + @Override + public void onClick() + { + onRightInput(); + } + + @Override + protected String getDisplayText() + { + return String.format("%s: %d", name, i); + } + + public int getValue() + { + return i; + } + } + + static class BooleanOption extends OptionElement + { + private String name; + private boolean val = true; + + private String customTrue; + private String customFalse; + + BooleanOption(String name, boolean def) + { + this.name = name; + this.val = def; + } + + BooleanOption(String name, boolean def, String customTrue, String customFalse) + { + this(name, def); + this.customTrue = customTrue; + this.customFalse = customFalse; + } + + @Override + public void onLeftInput() + { + val = !val; + } + + @Override + public void onRightInput() + { + val = !val; + } + + @Override + public void onClick() + { + val = !val; + } + + @Override + protected String getDisplayText() + { + String valStr; + + if(customTrue != null && customFalse != null) + { + valStr = val ? customTrue : customFalse; + } + else + { + valStr = val ? "true" : "false"; + } + + return String.format("%s: %s", name, valStr); + } + + public boolean getValue() + { + return val; + } + } + + /** + * + */ + public static class StaticItem extends OptionElement + { + private String name; + + public StaticItem(String name) + { + this.name = name; + } + + @Override + protected String getDisplayText() + { + return name; + } + } + + public static abstract class StaticClickableOption extends OptionElement + { + private String name; + + public StaticClickableOption(String name) + { + this.name = name; + } + + abstract void onClick(); + + @Override + protected String getDisplayText() + { + return name; + } + } + + private static abstract class Element + { + private MenuElement parent; + + protected void setParent(MenuElement parent) + { + this.parent = parent; + } + + protected MenuElement parent() + { + return parent; + } + + protected abstract String getDisplayText(); + } + + private static class SpecialUpElement extends Element + { + @Override + protected String getDisplayText() + { + return ".. ↰ Up One Level"; + } + } + } +} \ No newline at end of file diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/sample_conventions.md b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/sample_conventions.md index 45968ef..e85e625 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/sample_conventions.md +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/sample_conventions.md @@ -29,6 +29,11 @@ Concept: This is a sample OpMode that illustrates performing a specific function Each OpMode should try to only demonstrate a single concept so they are easy to locate based on their name. These OpModes may not produce a drivable robot. +Utility: This sample type is provided as a useful tool, or aide, to perform some specific development task. + It is not expected to be something you would include in your own robot code. + To use the tool, comment out the @Disabled annotation and build the App. + Read the comments found in the sample for an explanation of its intended use. + After the prefix, other conventions will apply: * Sensor class names should constructed as: Sensor - Company - Type diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcOpModeRegister.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcOpModeRegister.java index 8c6ea59..ceab67d 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcOpModeRegister.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcOpModeRegister.java @@ -34,8 +34,6 @@ package org.firstinspires.ftc.robotcontroller.internal; import com.qualcomm.robotcore.eventloop.opmode.OpModeManager; import com.qualcomm.robotcore.eventloop.opmode.OpModeRegister; -import org.firstinspires.ftc.robotcontroller.external.samples.ConceptNullOp; - /** * {@link FtcOpModeRegister} is responsible for registering OpModes for use in an FTC game. * @see #register(OpModeManager) @@ -49,7 +47,7 @@ public class FtcOpModeRegister implements OpModeRegister { * There are two mechanisms by which an OpMode may be registered. * * 1) The preferred method is by means of class annotations in the OpMode itself. - * See, for example the class annotations in {@link ConceptNullOp}. + * See, for example the class annotations in {@link org.firstinspires.ftc.robotcontroller.external.samples.ConceptNullOp}. * * 2) The other, retired, method is to modify this {@link #register(OpModeManager)} * method to include explicit calls to OpModeManager.register(). diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index 577c67f..03eee3c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -11,6 +11,8 @@ import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstan import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.smallHeadingPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.smallTranslationalPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFSwitch; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.tunedDriveErrorKalmanGain; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.tunedDriveErrorVariance; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -34,6 +36,8 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; import org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants; import org.firstinspires.ftc.teamcode.pedroPathing.util.DashboardPoseTracker; import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; +import org.firstinspires.ftc.teamcode.pedroPathing.util.FilteredPIDFController; +import org.firstinspires.ftc.teamcode.pedroPathing.util.KalmanFilter; import org.firstinspires.ftc.teamcode.pedroPathing.util.PIDFController; import java.util.ArrayList; @@ -118,9 +122,14 @@ public class Follower { private PIDFController largeTranslationalIntegral = new PIDFController(FollowerConstants.largeTranslationalIntegral); private PIDFController smallHeadingPIDF = new PIDFController(FollowerConstants.smallHeadingPIDFCoefficients); private PIDFController largeHeadingPIDF = new PIDFController(FollowerConstants.largeHeadingPIDFCoefficients); - private PIDFController smallDrivePIDF = new PIDFController(FollowerConstants.smallDrivePIDFCoefficients); - private PIDFController largeDrivePIDF = new PIDFController(FollowerConstants.largeDrivePIDFCoefficients); + private FilteredPIDFController smallDrivePIDF = new FilteredPIDFController(FollowerConstants.smallDrivePIDFCoefficients); + private FilteredPIDFController largeDrivePIDF = new FilteredPIDFController(FollowerConstants.largeDrivePIDFCoefficients); + private KalmanFilter driveKalmanFilter = new KalmanFilter(FollowerConstants.driveKalmanFilterParameters); + private long[] driveErrorTimes; + private double[] driveErrors; + private double rawDriverError; + private double previousRawDriverError; public static boolean drawOnDashboard = true; public static boolean useTranslational = true; @@ -580,6 +589,14 @@ public class Follower { correctiveVector = new Vector(); driveError = 0; headingError = 0; + rawDriverError = 0; + previousRawDriverError = 0; + driveErrors = new double[3]; + driveErrorTimes = new long[3]; + for (int i = 0; i < driveErrorTimes.length; i++) { + driveErrorTimes[i] = System.currentTimeMillis(); + } + driveKalmanFilter.reset(0, tunedDriveErrorVariance, tunedDriveErrorKalmanGain); for (int i = 0; i < motors.size(); i++) { motors.get(i).setPower(0); @@ -665,7 +682,26 @@ public class Follower { Vector lateralVelocityError = new Vector(lateralVelocityGoal - lateralVelocityZeroPowerDecay - lateralVelocity, lateralHeadingVector.getTheta()); Vector velocityErrorVector = MathFunctions.addVectors(forwardVelocityError, lateralVelocityError); - return velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector())); + previousRawDriverError = rawDriverError; + rawDriverError = velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector())); + + double previousErrorVelocity = (driveErrors[1] - driveErrors[0]) / ((driveErrorTimes[1] - driveErrorTimes[0]) / 1000.0); + double errorVelocity = (driveErrors[2] - driveErrors[1]) / ((driveErrorTimes[2] - driveErrorTimes[1]) / 1000.0); + double errorAcceleration = ((errorVelocity - previousErrorVelocity) / ((((driveErrorTimes[2] - driveErrorTimes[1]) / 1000.0) / 2.0) - (((driveErrorTimes[1] - driveErrorTimes[0]) / 1000.0) / 2.0))); + double time = (((driveErrorTimes[2] - driveErrorTimes[1]) / 1000.0) + ((driveErrorTimes[1] - driveErrorTimes[0]) / 1000.0)) / 2.0; + + double projection = errorVelocity * time + 0.5 * errorAcceleration * Math.pow(time, 2); + + driveKalmanFilter.update(rawDriverError - previousRawDriverError, projection); + + for (int i = 0; i < driveErrors.length - 1; i++) { + driveErrors[i] = driveErrors[i + 1]; + driveErrorTimes[i] = driveErrorTimes[i + 1]; + } + driveErrors[2] = driveKalmanFilter.getState(); + driveErrorTimes[2] = System.currentTimeMillis(); + + return driveKalmanFilter.getState(); } /** @@ -890,6 +926,7 @@ public class Follower { telemetry.addData("total heading", poseUpdater.getTotalHeading()); telemetry.addData("velocity magnitude", getVelocity().getMagnitude()); telemetry.addData("velocity heading", getVelocity().getTheta()); + driveKalmanFilter.debug(telemetry); telemetry.update(); if (drawOnDashboard) { Drawing.drawDebug(this); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java index b6aada6..60dcd9a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java @@ -2,10 +2,12 @@ package org.firstinspires.ftc.teamcode.pedroPathing.tuning; import com.acmerobotics.dashboard.config.Config; +import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomFilteredPIDFCoefficients; import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomPIDFCoefficients; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.util.KalmanFilterParameters; /** * This is the FollowerConstants class. It holds many constants and parameters for various parts of @@ -88,10 +90,11 @@ public class FollowerConstants { public static double smallHeadingPIDFFeedForward = 0.01; // Large drive PIDF coefficients - public static CustomPIDFCoefficients largeDrivePIDFCoefficients = new CustomPIDFCoefficients( + public static CustomFilteredPIDFCoefficients largeDrivePIDFCoefficients = new CustomFilteredPIDFCoefficients( 0.025, 0, 0.00001, + 0.01, 0); // Feed forward constant added on to the large drive PIDF @@ -101,15 +104,25 @@ public class FollowerConstants { public static double drivePIDFSwitch = 20; // Small drive PIDF coefficients - public static CustomPIDFCoefficients smallDrivePIDFCoefficients = new CustomPIDFCoefficients( + public static CustomFilteredPIDFCoefficients smallDrivePIDFCoefficients = new CustomFilteredPIDFCoefficients( 0.02, 0, 0.000005, + 0.01, 0); // Feed forward constant added on to the small drive PIDF public static double smallDrivePIDFFeedForward = 0.01; + // Kalman filter parameters for the drive error Kalman filter + public static KalmanFilterParameters driveKalmanFilterParameters = new KalmanFilterParameters( + 0.4, + 0.1); + + // These are the empirically tuned parameters for the drive error Kalman filter so it works faster. + public static double tunedDriveErrorVariance = 1; + public static double tunedDriveErrorKalmanGain = 1; + // Mass of robot in kilograms public static double mass = 10.65942; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomFilteredPIDFCoefficients.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomFilteredPIDFCoefficients.java new file mode 100644 index 0000000..0b02174 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomFilteredPIDFCoefficients.java @@ -0,0 +1,70 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +import kotlin.jvm.JvmField; + +/** + * This is the CustomFilteredPIDFCoefficients class. This class handles holding coefficients for filtered PIDF + * controllers. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 7/15/2024 + */ +public class CustomFilteredPIDFCoefficients { + @JvmField public double P; + @JvmField public double I; + @JvmField public double D; + @JvmField public double T; + @JvmField public double F; + + public FeedForwardConstant feedForwardConstantEquation; + + private boolean usingEquation; + + /** + * This creates a new CustomFilteredPIDFCoefficients with constant coefficients. + * + * @param p the coefficient for the proportional factor. + * @param i the coefficient for the integral factor. + * @param d the coefficient for the derivative factor. + * @param t the time constant for the filter + * @param f the coefficient for the feedforward factor. + */ + public CustomFilteredPIDFCoefficients(double p, double i, double d, double t, double f) { + P = p; + I = i; + D = d; + T = t; + F = f; + } + + /** + * This creates a new CustomFilteredPIDFCoefficients with constant PID coefficients and a variable + * feedforward equation using a FeedForwardConstant. + * + * @param p the coefficient for the proportional factor. + * @param i the coefficient for the integral factor. + * @param d the coefficient for the derivative factor. + * @param t the time constant for the filter + * @param f the equation for the feedforward factor. + */ + public CustomFilteredPIDFCoefficients(double p, double i, double d, double t, FeedForwardConstant f) { + usingEquation = true; + P = p; + I = i; + D = d; + T = t; + feedForwardConstantEquation = f; + } + + /** + * This returns the coefficient for the feedforward factor. + * + * @param input this is inputted into the feedforward equation, if applicable. If there's no + * equation, then any input can be used. + * @return This returns the coefficient for the feedforward factor. + */ + public double getCoefficient(double input) { + if (!usingEquation) return F; + return feedForwardConstantEquation.getConstant(input); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FilteredPIDFController.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FilteredPIDFController.java new file mode 100644 index 0000000..07747fe --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FilteredPIDFController.java @@ -0,0 +1,245 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +/** + * This is the FilteredPIDFController class. This class handles the running of filtered filtered PIDFs. This + * behaves very similarly to a regular filtered PIDF controller, but the derivative portion is filtered with + * a low pass filter to reduce high frequency noise that could affect results. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 7/15/2024 + */ +public class FilteredPIDFController { + private CustomFilteredPIDFCoefficients coefficients; + + private double previousError; + private double error; + private double position; + private double targetPosition; + private double errorIntegral; + private double errorDerivative; + private double previousDerivative; + private double filteredDerivative; + private double feedForwardInput; + + private long previousUpdateTimeNano; + private long deltaTimeNano; + + /** + * This creates a new filtered PIDFController from a CustomPIDFCoefficients. + * + * @param set the coefficients to use. + */ + public FilteredPIDFController(CustomFilteredPIDFCoefficients set) { + setCoefficients(set); + reset(); + } + + /** + * This takes the current error and runs the filtered PIDF on it. + * + * @return this returns the value of the filtered PIDF from the current error. + */ + public double runPIDF() { + return error * P() + filteredDerivative * D() + errorIntegral * I() + F(); + } + + /** + * This can be used to update the filtered PIDF's current position when inputting a current position and + * a target position to calculate error. This will update the error from the current position to + * the target position specified. + * + * @param update This is the current position. + */ + public void updatePosition(double update) { + position = update; + previousError = error; + error = targetPosition - position; + + deltaTimeNano = System.nanoTime() - previousUpdateTimeNano; + previousUpdateTimeNano = System.nanoTime(); + + errorIntegral += error * (deltaTimeNano / Math.pow(10.0, 9)); + previousDerivative = filteredDerivative; + errorDerivative = (error - previousError) / (deltaTimeNano / Math.pow(10.0, 9)); + filteredDerivative = T() * previousDerivative + (1 - T()) * errorDerivative; + } + + /** + * As opposed to updating position against a target position, this just sets the error to some + * specified value. + * + * @param error The error specified. + */ + public void updateError(double error) { + previousError = this.error; + this.error = error; + + deltaTimeNano = System.nanoTime() - previousUpdateTimeNano; + previousUpdateTimeNano = System.nanoTime(); + + errorIntegral += error * (deltaTimeNano / Math.pow(10.0, 9)); + previousDerivative = errorDerivative; + errorDerivative = (error - previousError) / (deltaTimeNano / Math.pow(10.0, 9)); + filteredDerivative = T() * previousDerivative + (1 - T()) * errorDerivative; + } + + /** + * This can be used to update the feedforward equation's input, if applicable. + * + * @param input the input into the feedforward equation. + */ + public void updateFeedForwardInput(double input) { + feedForwardInput = input; + } + + /** + * This resets all the filtered PIDF's error and position values, as well as the time stamps. + */ + public void reset() { + previousError = 0; + error = 0; + position = 0; + targetPosition = 0; + errorIntegral = 0; + errorDerivative = 0; + previousDerivative = 0; + filteredDerivative = 0; + previousUpdateTimeNano = System.nanoTime(); + } + + /** + * This is used to set the target position if the filtered PIDF is being run with current position and + * target position inputs rather than error inputs. + * + * @param set this sets the target position. + */ + public void setTargetPosition(double set) { + targetPosition = set; + } + + /** + * This returns the target position of the filtered PIDF. + * + * @return this returns the target position. + */ + public double getTargetPosition() { + return targetPosition; + } + + /** + * This is used to set the coefficients of the filtered PIDF. + * + * @param set the coefficients that the filtered PIDF will use. + */ + public void setCoefficients(CustomFilteredPIDFCoefficients set) { + coefficients = set; + } + + /** + * This returns the filtered PIDF's current coefficients. + * + * @return this returns the current coefficients. + */ + public CustomFilteredPIDFCoefficients getCoefficients() { + return coefficients; + } + + /** + * This sets the proportional (P) coefficient of the filtered PIDF only. + * + * @param set this sets the P coefficient. + */ + public void setP(double set) { + coefficients.P = set; + } + + /** + * This returns the proportional (P) coefficient of the filtered PIDF. + * + * @return this returns the P coefficient. + */ + public double P() { + return coefficients.P; + } + + /** + * This sets the integral (I) coefficient of the filtered PIDF only. + * + * @param set this sets the I coefficient. + */ + public void setI(double set) { + coefficients.I = set; + } + + /** + * This returns the integral (I) coefficient of the filtered PIDF. + * + * @return this returns the I coefficient. + */ + public double I() { + return coefficients.I; + } + + /** + * This sets the derivative (D) coefficient of the filtered PIDF only. + * + * @param set this sets the D coefficient. + */ + public void setD(double set) { + coefficients.D = set; + } + + /** + * This returns the derivative (D) coefficient of the filtered PIDF. + * + * @return this returns the D coefficient. + */ + public double D() { + return coefficients.D; + } + + /** + * This sets the time constant (T) of the filtered PIDF only. + * + * @param set this sets the time constant. + */ + public void setT(double set) { + coefficients.T = set; + } + + /** + * This returns the time constant (T) of the filtered PIDF. + * + * @return this returns the time constant. + */ + public double T() { + return coefficients.T; + } + + /** + * This sets the feedforward (F) constant of the filtered PIDF only. + * + * @param set this sets the F constant. + */ + public void setF(double set) { + coefficients.F = set; + } + + /** + * This returns the feedforward (F) constant of the filtered PIDF. + * + * @return this returns the F constant. + */ + public double F() { + return coefficients.getCoefficient(feedForwardInput); + } + + /** + * This returns the current error of the filtered PIDF. + * + * @return this returns the error. + */ + public double getError() { + return error; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilter.java new file mode 100644 index 0000000..41aa630 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilter.java @@ -0,0 +1,62 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +import com.acmerobotics.dashboard.config.Config; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +/** + * This is the KalmanFilter class. This creates a Kalman filter that is used to smooth out data. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 7/17/2024 + */ +public class KalmanFilter { + private KalmanFilterParameters parameters; + private double state; + private double variance; + private double kalmanGain; + private double previousState; + private double previousVariance; + + public KalmanFilter(KalmanFilterParameters parameters) { + this.parameters = parameters; + reset(); + } + + public KalmanFilter(KalmanFilterParameters parameters, double startState, double startVariance, double startGain) { + this.parameters = parameters; + reset(startState, startVariance, startGain); + } + + public void reset(double startState, double startVariance, double startGain) { + state = startState; + previousState = startState; + variance = startVariance; + previousVariance = startVariance; + kalmanGain = startGain; + } + + public void reset() { + reset(0, 1, 1); + } + + public void update(double updateData, double updateProjection) { + state = previousState + updateData; + variance = previousVariance + parameters.modelCovariance; + kalmanGain = variance / (variance + parameters.dataCovariance); + state += kalmanGain * (updateProjection - state); + variance *= (1.0 - kalmanGain); + previousState = state; + previousVariance = variance; + } + + public double getState() { + return state; + } + + public void debug(Telemetry telemetry) { + telemetry.addData("state", state); + telemetry.addData("variance", variance); + telemetry.addData("Kalman gain", kalmanGain); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilterParameters.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilterParameters.java new file mode 100644 index 0000000..e31260b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilterParameters.java @@ -0,0 +1,25 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +import kotlin.jvm.JvmField; + +/** + * This is the KalmanFilterParameters class. This class handles holding parameters Kalman filters. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 7/17/2024 + */ +public class KalmanFilterParameters { + @JvmField public double modelCovariance; + @JvmField public double dataCovariance; + + /** + * This creates a new KalmanFilterParameters with a specified model and data covariance. + * + * @param modelCovariance the covariance of the model. + * @param dataCovariance the covariance of the data. + */ + public KalmanFilterParameters(double modelCovariance, double dataCovariance) { + this.modelCovariance = modelCovariance; + this.dataCovariance = dataCovariance; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md index 4d1da42..da4d5b1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md @@ -1,131 +1,1505 @@ -## TeamCode Module +## NOTICE -Welcome! +This repository contains the public FTC SDK for the CENTERSTAGE (2023-2024) competition season. -This module, TeamCode, is the place where you will write/paste the code for your team's -robot controller App. This module is currently empty (a clean slate) but the -process for adding OpModes is straightforward. +## Welcome! +This GitHub repository contains the source code that is used to build an Android app to control a *FIRST* Tech Challenge competition robot. To use this SDK, download/clone the entire project to your local computer. -## Creating your own OpModes +## Requirements +To use this Android Studio project, you will need Android Studio 2021.2 (codename Chipmunk) or later. -The easiest way to create your own OpMode is to copy a Sample OpMode and make it your own. +To program your robot in Blocks or OnBot Java, you do not need Android Studio. -Sample opmodes exist in the FtcRobotController module. -To locate these samples, find the FtcRobotController module in the "Project/Android" tab. +## Getting Started +If you are new to robotics or new to the *FIRST* Tech Challenge, then you should consider reviewing the [FTC Blocks Tutorial](https://ftc-docs.firstinspires.org/programming_resources/blocks/Blocks-Tutorial.html) to get familiar with how to use the control system: -Expand the following tree elements: - FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples +      [FTC Blocks Online Tutorial](https://ftc-docs.firstinspires.org/programming_resources/blocks/Blocks-Tutorial.html) -### Naming of Samples +Even if you are an advanced Java programmer, it is helpful to start with the [FTC Blocks tutorial](https://ftc-docs.firstinspires.org/programming_resources/blocks/Blocks-Tutorial.html), and then migrate to the [OnBot Java Tool](https://ftc-docs.firstinspires.org/programming_resources/onbot_java/OnBot-Java-Tutorial.html) or to [Android Studio](https://ftc-docs.firstinspires.org/programming_resources/android_studio_java/Android-Studio-Tutorial.html) afterwards. -To gain a better understanding of how the samples are organized, and how to interpret the -naming system, it will help to understand the conventions that were used during their creation. +## Downloading the Project +If you are an Android Studio programmer, there are several ways to download this repo. Note that if you use the Blocks or OnBot Java Tool to program your robot, then you do not need to download this repository. -These conventions are described (in detail) in the sample_conventions.md file in this folder. +* If you are a git user, you can clone the most current version of the repository: -To summarize: A range of different samples classes will reside in the java/external/samples. -The class names will follow a naming convention which indicates the purpose of each class. -The prefix of the name will be one of the following: +

            git clone https://github.com/FIRST-Tech-Challenge/FtcRobotController.git

-Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure - of a particular style of OpMode. These are bare bones examples. +* Or, if you prefer, you can use the "Download Zip" button available through the main repository page. Downloading the project as a .ZIP file will keep the size of the download manageable. -Sensor: This is a Sample OpMode that shows how to use a specific sensor. - It is not intended to drive a functioning robot, it is simply showing the minimal code - required to read and display the sensor values. +* You can also download the project folder (as a .zip or .tar.gz archive file) from the Downloads subsection of the [Releases](https://github.com/FIRST-Tech-Challenge/FtcRobotController/releases) page for this repository. -Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base. - It may be used to provide a common baseline driving OpMode, or - to demonstrate how a particular sensor or concept can be used to navigate. +* The Releases page also contains prebuilt APKs. -Concept: This is a sample OpMode that illustrates performing a specific function or concept. - These may be complex, but their operation should be explained clearly in the comments, - or the comments should reference an external doc, guide or tutorial. - Each OpMode should try to only demonstrate a single concept so they are easy to - locate based on their name. These OpModes may not produce a drivable robot. +Once you have downloaded and uncompressed (if needed) your folder, you can use Android Studio to import the folder ("Import project (Eclipse ADT, Gradle, etc.)"). -After the prefix, other conventions will apply: +## Getting Help +### User Documentation and Tutorials +*FIRST* maintains online documentation with information and tutorials on how to use the *FIRST* Tech Challenge software and robot control system. You can access this documentation using the following link: -* Sensor class names are constructed as: Sensor - Company - Type -* Robot class names are constructed as: Robot - Mode - Action - OpModetype -* Concept class names are constructed as: Concept - Topic - OpModetype +      [FIRST Tech Challenge Documentation](https://ftc-docs.firstinspires.org/index.html) -Once you are familiar with the range of samples available, you can choose one to be the -basis for your own robot. In all cases, the desired sample(s) needs to be copied into -your TeamCode module to be used. +Note that the online documentation is an "evergreen" document that is constantly being updated and edited. It contains the most current information about the *FIRST* Tech Challenge software and control system. -This is done inside Android Studio directly, using the following steps: +### Javadoc Reference Material +The Javadoc reference documentation for the FTC SDK is now available online. Click on the following link to view the FTC SDK Javadoc documentation as a live website: - 1) Locate the desired sample class in the Project/Android tree. +      [FTC Javadoc Documentation](https://javadoc.io/doc/org.firstinspires.ftc) - 2) Right click on the sample class and select "Copy" +### Online User Forum +For technical questions regarding the Control System or the FTC SDK, please visit the FIRST Tech Challenge Community site: - 3) Expand the TeamCode/java folder +      [FIRST Tech Challenge Community](https://ftc-community.firstinspires.org/) - 4) Right click on the org.firstinspires.ftc.teamcode folder and select "Paste" +### Sample OpModes +This project contains a large selection of Sample OpModes (robot code examples) which can be cut and pasted into your /teamcode folder to be used as-is, or modified to suit your team's needs. - 5) You will be prompted for a class name for the copy. - Choose something meaningful based on the purpose of this class. - Start with a capital letter, and remember that there may be more similar classes later. +Samples Folder:    [/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples](FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples) -Once your copy has been created, you should prepare it for use on your robot. -This is done by adjusting the OpMode's name, and enabling it to be displayed on the -Driver Station's OpMode list. +The readme.md file located in the [/TeamCode/src/main/java/org/firstinspires/ftc/teamcode](TeamCode/src/main/java/org/firstinspires/ftc/teamcode) folder contains an explanation of the sample naming convention, and instructions on how to copy them to your own project space. -Each OpMode sample class begins with several lines of code like the ones shown below: +# Release Information -``` - @TeleOp(name="Template: Linear OpMode", group="Linear Opmode") - @Disabled -``` +## Version 9.2 (20240701-085519) -The name that will appear on the driver station's "opmode list" is defined by the code: - ``name="Template: Linear OpMode"`` -You can change what appears between the quotes to better describe your opmode. -The "group=" portion of the code can be used to help organize your list of OpModes. +### Important Notes +* Java classes and Blocks for TensorFlow Object Detection have been deprecated and will be removed in Version 10.0. +* The samples that use TensorFlow Object Detection have been removed. -As shown, the current OpMode will NOT appear on the driver station's OpMode list because of the - ``@Disabled`` annotation which has been included. -This line can simply be deleted , or commented out, to make the OpMode visible. +### Enhancements +* Adds explanatory text to failed items on the inspection activities. To view the explanatory text tap the red warning icon for a failed item. +* In the Blocks editor: added a new kind of variable set block that sets the variable and also returns the new value. +* Changes the way that camera controls behave for a SwitchableCamera. Now, each method (such as getExposure, getMinExposure, getMaxExposure, setExposure for ExposureControl) acts on the currently active camera. +* Adds support for the REV USB PS4 Compatible Gamepad (REV-31-2983) +* Adds ConceptAprilTagMultiPortal OpMode +* Adds support for OctoQuad Quadrature Encoder & Pulse Width Interface Module +* Adds the ExportAprilTagLibraryToBlocks annotation that indicates that a static method that returns an AprilTagLibrary is exported to the Blocks programming environment. The corresponding block will appear in the Blocks toolbox along with the built-in tag libraries. +* Adds Blocks OpMode ConceptAprilTagOptimizeExposure. +* Adds support for the SparkFun Optical Tracking Odometry sensor. + +### Bug Fixes +* Fixes https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/942 where visionPortal.close() can cause an IndexOutOfBoundsError. +* Fixes a bug in the blocks editor where collapsed function blocks show a warning "Collapsed blocks contain warnings." when the Blocks OpMode is reopened. +* Fixes a bug where the blocks editor wouldn't warn you that you have unsaved changes when you try to leave. This bug was introduced due to a behavior change in Chrome 119. +* [Issue #764](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/764) - Get gain control returns a null pointer for a switchable camera +* Fixes a bug where the correct deadzone for certain gamepads was not applied when Advanced Gamepad Features was enabled + +## Version 9.1 (20240215-115542) + +### Enhancements +* Fixes a problem with Blocks: if the user closes a Block's warning balloon, it will still be closed next time the project is opened in the Blocks editor. +* In the Blocks editor, an alert concerning missing hardware devices is not shown if all the Blocks that use the missing hardware devices are disabled. +* Adds Blocks to support comparing property values CRServo.Direction, DCMotor.Direction, DCMotor.Mode, DCMotor.ZeroPowerBehavior, DigitalChannel.Mode, GyroSensor.HeadingMode, IrSeekerSensor.Mode, and Servo.Direction, to the corresponding enum Block. +* Improves OnBotJava auto-import to correctly import classes when used in certain situations. +* Improves OnBotJava autocomplete to provide better completion options in most cases. + * This fixes an issue where autocomplete would fail if a method with two or more formal parameters was defined. +* In OnBotJava, code folding support was added to expand and collapse code sections +* In OnBotJava, the copyright header is now automatically collapsed loading new files +* For all Blocks OpMode samples, intro comments have been moved to the RunOpMode comment balloon. +* The Clean up Blocks command in the Blocks editor now positions function Blocks so their comment balloons don't overlap other function Blocks. +* Added Blocks OpMode sample SensorTouch. +* Added Java OpMode sample SensorDigitalTouch. +* Several improvements to VisionPortal + * Adds option to control whether the stream is automatically started following a `.build()` call on a VisionPortal Builder + * Adds option to control whether the vision processing statistics overlay is rendered or not + * VisionPortals now implement the `CameraStreamSource` interface, allowing multiportal users to select which portal is routed to the DS in INIT by calling CameraStreamServer.getInstance().setSource(visionPortal). Can be selected via gamepad, between Camera Stream sessions. + * Add option to `AprilTagProcessor` to suppress calibration warnings + * Improves camera calibration warnings + * If a calibration is scaled, the resolution it was scaled from will be listed + * If calibrations exist with the wrong aspect ratio, the calibrated resolutions will be listed + * Fixes race condition which caused app crash when calling `stopStreaming()` immediately followed by `close()` on a VisionPortal + * Fixes IllegalStateException when calling `stopStreaming()` immediately after building a VisionPortal + * Added FTC Blocks counterparts to new Java methods: + * VisionPortal.Builder.setAutoStartStreamOnBuild + * VisionPortal.Builder.setShowStatsOverlay + * AprilTagProcessor.Builder.setSuppressCalibrationWarnings + * CameraStreamServer.setSource​ + +### Bug Fixes +* Fixes a problem where OnBotJava does not apply font size settings to the editor. +* Updates EasyOpenCV dependency to v1.7.1 + * Fixes inability to use EasyOpenCV CameraFactory in OnBotJava + * Fixes entire RC app crash when user pipeline throws an exception + * Fixes entire RC app crash when user user canvas annotator throws an exception + * Use the modern stacktrace display when handling user exceptions instead of the legacy ESTOP telemetry message + +## Version 9.0.1 (20230929-083754) + +### Enhancements +* Updates AprilTag samples to include Decimation and additional Comments. Also corrects misleading tag ID warnings +* Increases maximum size of Blocks inline comments to 140 characters +* Adds Blocks sample BasicOmniOpMode. +* Updated CENTERSTAGE library AprilTag orientation quaternions + * Thanks [@FromenActual](https://github.com/FromenActual) +* Updated Java Sample ConceptTensorFlowObjectDetection.java to include missing elements needed for custom model support. + +### Bug Fixes +* Fixes a problem where after October 1 the Driver Station will report as obsolete on v9.0 and prompt the user to update. + +## Version 9.0 (20230830-154348) + +### Breaking Changes +* Removes Vuforia +* Fields in `AprilTagDetection` and `AprilTagPose(ftc/raw)` objects are now `final` +* VisionPortal builder method `setCameraMonitorViewId()` has been renamed to `setLiveViewContainerId()` and `enableCameraMonitoring()` has been renamed to `enableLiveView()` + +### Enhancements +* Adds support for the DFRobot HuskyLens Vision Sensor. +* Blocks teams can now perform webcam calibration. + * Added a Block for System.currentTimeMillis (under Utilities/Time) + * Added a Block for VisionPortal.saveNextFrameRaw (under Vision/VisionPortal) + * Added a new sample Blocks OpMode called UtilityCameraFrameCapture. +* The RobotDriveByGyro sample has been updated to use the new universal IMU interface. It now supports both IMU types. +* Removed some error-prone ElapsedTime Blocks from the Blocks editor's toolbox. This is not a + breaking change: old Blocks OpModes that use these Blocks will still function, both in the + Blocks editor and at runtime. +* Standardizes on the form "OpMode" for the term OpMode. + * The preferred way to refer to OpModes that specifically extend `LinearOpMode` (including Blocks OpModes) is "linear OpMode". + * The preferred way to refer to OpModes that specifically extend `OpMode` directly is "iterative OpMode". +* Overhauls `OpMode` and `LinearOpMode` Javadoc comments to be easier to read and include more detail. +* Makes minor enhancements to Java samples + * Javadoc comments in samples that could be rendered badly in Android Studio have been converted to standard multi-line comments + * Consistency between samples has been improved + * The SensorDigitalTouch sample has been replaced with a new SensorTouch sample that uses the `TouchSensor` interface instead of `DigitalChannel`. + * The ConceptCompassCalibration, SensorMRCompass, and SensorMRIRSeeker samples have been deleted, as they are not useful for modern FTC competitions. + +### Bug Fixes +* Fixes a bug which prevented PlayStation gamepads from being used in bluetooth mode. Bluetooth is NOT legal for competition but may be useful to allow a DS device to be used while charging, or at an outreach event. +* Fixes a bug where a Blocks OpMode's Date Modified value can change to December 31, 1969, if the Control Hub is rebooted while the Blocks OpMode is being edited. +* Fixes the automatic TeleOp preselection feature (was broken in 8.2) +* Fixes a bug where passing an integer number such as 123 to the Telemetry.addData block that takes a number shows up as 123.0 in the telemetry. +* Fixes OnBotJava autocomplete issues: + * Autocomplete would incorrectly provide values for the current class when autocompleting a local variable + * `hardwareMap` autocomplete would incorrectly include lambda class entries +* Fixes OnBotJava not automatically importing classes. +* Fixes OnBotJava tabs failing to close when their file is deleted. +* Fixes a project view refresh not happening when a file is renamed in OnBotJava. +* Fixes the "Download" context menu item for external libraries in the OnBotJava interface. +* Fixes issue where Driver Station telemetry would intermittently freeze when set to Monospace mode. +* Fixes performance regression for certain REV Hub operations that was introduced in version 8.2. +* Fixes TagID comparison logic in DriveToTag samples. + +## Version 8.2 (20230707-131020) + +### Breaking Changes +* Non-linear (iterative) OpModes are no longer allowed to manipulate actuators in their `stop()` method. Attempts to do so will be ignored and logged. + * When an OpMode attempts to illegally manipulate an actuator, the Robot Controller will print a log message + including the text `CANCELLED_FOR_SAFETY`. + * Additionally, LinearOpModes are no longer able to regain the ability to manipulate actuators by removing their + thread's interrupt or using another thread. +* Removes support for Android version 6.0 (Marshmallow). The minSdkVersion is now 24. +* Increases the Robocol version. + * This means an 8.2 or later Robot Controller or Driver Station will not be able to communicate with an 8.1 or earlier Driver Station or Robot Controller. + * If you forget to update both apps at the same time, an error message will be shown explaining which app is older and should be updated. +* FTC_FieldCoordinateSystemDefinition.pdf has been moved. It is still in the git history, but has been removed from the git snapshot corresponding with the 8.2 tag. The official version now lives at [Field Coordinate System](https://ftc-docs.firstinspires.org/field-coordinate-system). +* `LynxUsbDevice.addConfiguredModule()` and `LynxUsbDevice.getConfiguredModule()` have been replaced with `LynxUsbDevice.getOrAddModule()`. +* Old Blocks for Vuforia and TensorFlow Object Detection are obsolete and have been removed from the + Blocks editor's toolbox. Existing Blocks OpModes that contain the old Blocks for Vuforia or + TensorFlow Object Detection can be opened in the Blocks editor, but running them will not work. + +### New features +* Adds new `VisionPortal` API for computer vision + * **This API may be subject to change for final kickoff release!** + * Several new samples added. + * Adds support for detecting AprilTags. + * `VisionPortal` is the new entry point for both AprilTag and TFOD processing. + * Vuforia will be removed in a future release. + * Updated TensorFlow dependencies. + * Added support for webcam camera controls to blocks. + * The Blocks editor's toolbox now has a Vision category, directly above the Utilities category. +* Related documentation for associated technologies can be found at + * [AprilTag Introduction](https://ftc-docs.firstinspires.org/apriltag-intro) + * [AprilTag SDK Guide](https://ftc-docs.firstinspires.org/apriltag-sdk) + * [AprilTag Detection Values](https://ftc-docs.firstinspires.org/apriltag-detection-values) + * [AprilTag Test Images](https://ftc-docs.firstinspires.org/apriltag-test-images) + * [Camera Calibration](https://ftc-docs.firstinspires.org/camera-calibration) +* Adds Driver Station support for Logitech Dual Action and Sony PS5 DualSense gamepads. + * This **does not** include support for the Sony PS5 DualSense Edge gamepad. + * Always refer to Game Manual 1 to determine gamepad legality in competition. +* Adds support for MJPEG payload streaming to UVC driver (external JPEG decompression routine required for use). +* Shows a hint on the Driver Station UI about how to bind a gamepad when buttons are pressed or the sticks are moved on an unbound gamepad. +* Adds option for fullscreening "Camera Stream" on Driver Station. +* OnBotJava source code is automatically saved as a ZIP file on every build with a rolling window of the last 30 builds kept; allows recovering source code from previous builds if code is accidentally deleted or corrupted. +* Adds support for changing the addresses of Expansion Hubs that are not connected directly via USB. + * The Expansion Hub Address Change screen now has an Apply button that changes the addresses without leaving the screen. + * Addresses that are assigned to other hubs connected to the same USB connection or Control Hub are no longer able to be selected. +* Increases maximum size of Blocks inline comments to 100 characters +* Saves position of open Blocks comment balloons +* Adds new AprilTag Driving samples: RobotDriveToAprilTagTank & RobotDriveToAprilTagOmni +* Adds Sample to illustrate optimizing camera exposure for AprilTags: ConceptAprilTagOptimizeExposure + +### Bug Fixes +* Corrects inspection screen to report app version using the SDK version defined in the libraries instead of the version specified in `AndroidManifest.xml`. This corrects the case where the app could show matching versions numbers to the user but still state that the versions did not match. + * If the version specified in `AndroidManifest.xml` does not match the SDK version, an SDK version entry will be displayed on the Manage webpage. +* Fixes no error being displayed when saving a configuration file with duplicate names from the Driver Station. +* Fixes a deadlock in the UVC driver which manifested in https://github.com/OpenFTC/EasyOpenCV/issues/57. +* Fixes a deadlock in the UVC driver that could occur when hot-plugging cameras. +* Fixes UVC driver compatibility with Arducam OV9281 global shutter camera. +* Fixes Emergency Stop condition when an OnBotJava build with duplicate OpMode names occurs. +* Fixes known causes of "Attempted use of a closed LynxModule instance" logspam. +* Fixes the visual identification LED pattern when configuring Expansion Hubs connected via RS-485. + +## Version 8.1.1 (20221201-150726) + +This is a bug fix only release to address the following four issues. + +* [Issue #492](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/492) - Can't create new blocks opmodes. +* [Issue #495](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/495) - Remove the final modifier from the OpMode's Telemetry object. +* [Issue #500](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/500) - Some devices cannot be configured when the Driver Station app has been updated to 8.1 + * Updating either the Robot Controller app or the Driver Station app to 8.1.1 or later will fix this issue. +* The Modern Robotics touch sensor was configurable as a Digital Device. It can only be used as an Analog Device. + +## Version 8.1 (20221121-115119) + +### Breaking Changes +* Deprecates the `OpMode` fields `msStuckDetectInit`, `msStuckDetectInitLoop`, `msStuckDetectStart`, `msStuckDetectLoop`, and `msStuckDetectStop`. + * OpModes no longer have a time limit for `init()`, `init_loop()`, `start()` or `loop()`, so the fields corresponding to those methods are no longer used. + * `stop()` still has a time limit, but it is now hardcoded to be 1 second, and cannot be changed using `msStuckDetectStop`. +* Deprecates the `OpMode` methods `internalPreInit()`, `internalPostInitLoop()`, and `internalPostLoop()`. + * Iterative `OpMode`s will continue to call these methods in case they were overridden. + * These methods will not be called at all for `LinearOpMode`s. +* Deprecates (and stops respecting) `DeviceProperties.xmlTagAliases`. + +### Enhancements +* Adds a new `IMU` interface to Blocks and Java that can be used with both the original BNO055 IMU + included in all older Control Hubs and Expansion Hubs, and the new alternative BHI260AP IMU. + * You can determine which type of IMU is in your Control Hub by navigating to the Manage page of the web interface. + * To learn how to use the new `IMU` interface, see https://ftc-docs.firstinspires.org/programming_resources/imu/imu.html. The `SensorIMU` Blocks sample was also updated to use the new `IMU` interface, and the following Java samples were added: + * `SensorIMUOrthogonal` + * Use this sample if your REV Hub is mounted so that it is parallel or perpendicular to the + bottom of your robot. + * `SensorIMUNonOrthogonal` + * Use this sample if your REV Hub is mounted to your robot in any other orientation + * `ConceptExploringIMUOrientations` + * This OpMode is a tool to help you understand how the orthogonal orientations work, and + which one applies to your robot. + * The BHI260AP IMU can only be accessed via the new `IMU` interface. The BNO055 IMU can be + programmed using the new `IMU` interface, or you can continue to program it using the old `BNO055IMU` + interface. If you want to be able to quickly switch to a new Control Hub that may contain the + BHI260AP IMU, you should migrate your code to use the new `IMU` interface. + * Unlike the old `BNO055IMU` interface, which only worked correctly when the REV Hub was mounted flat + on your robot, the `IMU` interface allows you to specify the orientation of the REV Hub on your + robot. It will account for this, and give you your orientation in a Robot Coordinate System, + instead of a special coordinate system for the REV Hub. As a result, your pitch and yaw will be + 0 when your *robot* is level, instead of when the REV Hub is level, which will result in much + more reliable orientation angle values for most mounting orientations. + * Because of the new robot-centric coordinate system, the pitch and roll angles returned by the + `IMU` interface will be different from the ones returned by the `BNO055IMU` interface. When you are + migrating your code, pay careful attention to the documentation. + * If you have calibrated your BNO055, you can provide that calibration data to the new `IMU` + interface by passing a `BNO055IMUNew.Parameters` instance to `IMU.initialize()`. + * The `IMU` interface is also suitable for implementation by third-party vendors for IMUs that + support providing the orientation in the form of a quaternion. +* Iterative `OpMode`s (as opposed to `LinearOpMode`s) now run on a dedicated thread. + * Cycle times should not be as impacted by everything else going on in the system. + * Slow `OpMode`s can no longer increase the amount of time it takes to process network commands, and vice versa. + * The `init()`, `init_loop()`, `start()` and `loop()` methods no longer need to return within a certain time frame. +* BNO055 IMU legacy driver: restores the ability to initialize in one OpMode, and then have another OpMode re-use that + initialization. This allows you to maintain the 0-yaw position between OpModes, if desired. +* Allows customized versions of device drivers in the FTC SDK to use the same XML tag. + * Before, if you wanted to customize a device driver, you had to copy it to a new class _and_ give + it a new XML tag. Giving it a new XML tag meant that to switch which driver was being used, you + had to modify your configuration file. + * Now, to use your custom driver, all you have to do is specify your custom driver's class when + calling `hardwareMap.get()`. To go back to the original driver, specify the original driver + class. If you specify an interface that is implemented by both the original driver and the + custom driver, there is no guarantee about which implementation will be returned. + +### Bug Fixes +* Fixes accessing the "Manage TensorFlow Lite Models" and "Manage Sounds" links and performing + Blocks and OnBotJava OpMode downloads from the REV Hardware Client. +* Fixes issue where an I2C device driver would be auto-initialized using the parameters assigned in + a previous OpMode run. +* Improves Driver Station popup menu placement in the landscape layout. +* Fixes NullPointerException when attempting to get a non-configured BNO055 IMU in a Blocks OpMode on an RC phone. +* Fixes problem with Blocks if a variable is named `orientation`. + +## Version 8.0 (20220907-131644) + +### Breaking Changes +* Increases the Robocol version. + * This means an 8.0 or later Robot Controller or Driver Station will not be able to communicate with a 7.2 or earlier Driver Station or Robot Controller. + * If you forget to update both apps at the same time, an error message will be shown explaining which app is older and should be updated. +* Initializing I2C devices now happens when you retrieve them from the `HardwareMap` for the first time. + * Previously, all I2C devices would be initialized before the OpMode even began executing, + whether you were actually going to use them or not. This could result in reduced performance and + unnecessary warnings. + * With this change, it is very important for Java users to retrieve all needed devices from the + `HardwareMap` **during the Init phase of the OpMode**. Namely, declare a variable for each hardware + device the OpMode will use, and assign a value to each. Do not do this during the Run phase, or your + OpMode may briefly hang while the devices you are retrieving get initialized. + * OpModes that do not use all of the I2C devices specified in the configuration file should take + less time to initialize. OpModes that do use all of the specified I2C devices should take the + same amount of time as previously. +* Fixes [issue #251](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/251) by changing the order in which axis rotation rates are read from the angular velocity vector in the BNO055 IMU driver. +* Deprecates `pitchMode` in `BNO055IMU.Parameters`. + * Setting `pitchMode` to `PitchMode.WINDOWS` would break the coordinate conventions used by the driver. +* Moves `OpModeManagerImpl` to the `com.qualcomm.robotcore.eventloop.opmode` package. + * This breaks third party libraries EasyOpenCV (version 1.5.1 and earlier) and FTC Dashboard (version 0.4.4 and earlier). +* Deletes the deprecated `OpMode` method `resetStartTime()` (use `resetRuntime()` instead). +* Deletes the protected `LinearOpMode.LinearOpModeHelper` class (which was not meant for use by OpModes). +* Removes I2C Device (Synchronous) config type (deprecated since 2018) + +### Enhancements +* Uncaught exceptions in OpModes no longer require a Restart Robot + * A blue screen popping up with a stacktrace is not an SDK error; this replaces the red text in the telemetry area. + * Since the very first SDK release, OpMode crashes have put the robot into "EMERGENCY STOP" state, only showing the first line of the exception, and requiring the user to press "Restart Robot" to continue + * Exceptions during an OpMode now open a popup window with the same color scheme as the log viewer, containing 15 lines of the exception stacktrace to allow easily tracing down the offending line without needing to connect to view logs over ADB or scroll through large amounts of logs in the log viewer. + * The exception text in the popup window is both zoomable and scrollable just like a webpage. + * Pressing the "OK" button in the popup window will return to the main screen of the Driver Station and allow an OpMode to be run again immediately, without the need to perform a "Restart Robot" +* Adds new Java sample to demonstrate using a hardware class to abstract robot actuators, and share them across multiple OpModes. + * Sample OpMode is [ConceptExternalHardwareClass.java](FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExternalHardwareClass.java) + * Abstracted hardware class is [RobotHardware.java](FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotHardware.java) +* Updates RobotAutoDriveByGyro_Linear Java sample to use REV Control/Expansion hub IMU. +* Updates Vuforia samples to reference PowerPlay assets and have correct names and field locations of image targets. +* Updates TensorFlow samples to reference PowerPlay assets. +* Adds opt-in support for Java 8 language features to the OnBotJava editor. + * To opt in, open the OnBotJava Settings, and check `Enable beta Java 8 support`. + * Note that Java 8 code will only compile when the Robot Controller runs Android 7.0 Nougat or later. + * Please report issues [here](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues). +* In OnBotJava, clicking on build errors now correctly jumps to the correct location. +* Improves OnBotJava autocomplete behavior, to provide better completion options in most cases. +* Adds a QR code to the Robot Controller Inspection Report when viewed from the Driver Station for scanning by inspectors at competition. +* Improves I2C performance and reliability in some scenarios. + +## Version 7.2 (20220723-130006) + +### Breaking Changes +* Updates the build tooling. For Android Studio users, this change requires Android Studio Chipmunk 2021.2.1. +* Removes support for devices that are not competition legal, including Modern Robotics Core Control Modules, the Matrix Controller, and HiTechnic/NXT controllers and sensors. Support remains for Modern Robotics I2C sensors. + +### Enhancements +* Increases the height of the 3-dots Landscape menu touch area on the Driver Station, making it much easier to select. +* Adds `terminateOpModeNow()` method to allow OpModes to cleanly self-exit immediately. +* Adds `opModeInInit()` method to `LinearOpMode` to facilitate init-loops. Similar to `opModeIsActive()` but for the init phase. +* Warns user if they have a Logitech F310 gamepad connected that is set to DirectInput mode. +* Allows SPARKmini motor controllers to react more quickly to speed changes. +* Hides the version number of incorrectly installed sister app (i.e. DS installed on RC device or vice-versa) on inspection screen. +* Adds support for allowing the user to edit the comment for the runOpMode block. +* Adds parameterDefaultValues field to @ExportToBlocks. This provides the ability for a java method with an @ExportToBlocks annotation to specify default values for method parameters when it is shown in the block editor. +* Make LinearOpMode blocks more readable. The opmode name is displayed on the runOpMode block, but not on the other LinearOpMode blocks. +* Added support to TensorFlow Object Detection for using a different frame generator, instead of Vuforia. + Using Vuforia to pass the camera frame to TFOD is still supported. +* Removes usage of Renderscript. +* Fixes logspam on app startup of repeated stacktraces relating to `"Failed resolution of: Landroid/net/wifi/p2p/WifiP2pManager$DeviceInfoListener"` +* Allows disabling bluetooth radio from inspection screen +* Improves warning messages when I2C devices are not responding +* Adds support for controlling the RGB LED present on PS4/Etpark gamepads from OpModes +* Removes legacy Pushbot references from OpMode samples. Renames "Pushbot" samples to "Robot". Motor directions reversed to be compatible with "direct Drive" drive train. + + +### Bug fixes +* Fixes [issue #316](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/316) (MatrixF.inverted() returned an incorrectly-sized matrix for 1x1 and 2x2 matrixes). +* Self inspect now allows for Driver Station and Robot Controller compatibility between point releases. +* Fixes bug where if the same `RumbleEffect` object instance was queued for multiple gamepads, it + could happen that both rumble commands would be sent to just one gamepad. +* Fixes bug in Driver Station where on the Driver Hub, if Advanced Gamepad Features was disabled and + an officially supported gamepad was connected, then opening the Advanced Gamepad Features or + Gamepad Type Overrides screens would cause the gamepad to be rebound by the custom USB driver even + though advanced gamepad features was disabled. +* Protects against (unlikely) null pointer exception in Vuforia Localizer. +* Harden OnBotJava and Blocks saves to protect against save issues when disconnecting from Program and Manage +* Fixes issue where the RC app would hang if a REV Hub I2C write failed because the previous I2C + operation was still in progress. This hang most commonly occurred during REV 2M Distance Sensor initialization +* Removes ConceptWebcam.java sample program. This sample is not compatible with OnBotJava. +* Fixes bug where using html tags in an @ExportToBlocks comment field prevented the blocks editor from loading. +* Fixes blocks editor so it doesn't ask you to save when you haven't modified anything. +* Fixes uploading a very large blocks project to offline blocks editor. +* Fixes bug that caused blocks for DcMotorEx to be omitted from the blocks editor toolbox. +* Fixes [Blocks Programs Stripped of Blocks (due to using TensorFlow Label block)](https://ftcforum.firstinspires.org/forum/ftc-technology/blocks-programming/87035-blocks-programs-stripped-of-blocks) + +## Version 7.1 (20211223-120805) + +* Fixes crash when calling `isPwmEnabled()` ([issue #223](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/233)). +* Fixes lint error ([issue #4](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/4)). +* Fixes Driver Station crash when attempting to use DualShock4 v1 gamepad with Advanced Gamepad Features enabled ([issue #173](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/173)). +* Fixes possible (but unlikely) Driver Station crash when connecting gamepads of any type. +* Fixes bug where Driver Station would use generic 20% deadzone for Xbox360 and Logitech F310 gamepads when Advanced Gamepad Features was disabled. +* Added SimpleOmniDrive sample OpMode. +* Adds UVC white balance control API. +* Fixes [issue #259](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/259) Most blocks samples for TensorFlow can't be used for a different model. + * The blocks previously labeled TensorFlowObjectDetectionFreightFrenzy (from the subcategory named "Optimized for Freight Frenzy") and TensorFlowObjectDetectionCustomModel (from the subcategory named "Custom Model") have been replaced with blocks labeled TensorFlowObjectDetection. Blocks in existing opmodes will be automatically updated to the new blocks when opened in the blocks editor. +* Fixes [issue #260](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/260) Blocks can't call java method that has a VuforiaLocalizer parameter. + * Blocks now has a block labeled VuforiaFreightFrenzy.getVuforiaLocalizer for this. +* Added a page to manage the TensorFlow Lite models in /sdcard/FIRST/tflitemodels. To get to the TFLite Models page: + * You can click on the link at the bottom of the the Manage page. + * You can click on the link at the upper-right the Blocks project page. +* Fixes logspam when `isBusy()` is called on a motor not in RTP mode. +* Hides the "RC Password" item on the inspection screen for phone-based Robot Controllers. (It is only applicable for Control Hubs). +* Adds channel 165 to Wi-Fi Direct channel selection menu in the settings screen. (165 was previously available through the web UI, but not locally in the app). + +## Version 7.0 (20210915-141025) + +### Enhancements and New Features +* Adds support for external libraries to OnBotJava and Blocks. + * Upload .jar and .aar files in OnBotJava. + * Known limitation - RobotController device must be running Android 7.0 or greater. + * Known limitation - .aar files with assets are not supported. + * External libraries can provide support for hardware devices by using the annotation in the + com.qualcomm.robotcore.hardware.configuration.annotations package. + * External libraries can include .so files for native code. + * External libraries can be used from OnBotJava OpModes. + * External libraries that use the following annotations can be used from Blocks OpModes. + * org.firstinspires.ftc.robotcore.external.ExportClassToBlocks + * org.firstinspires.ftc.robotcore.external.ExportToBlocks + * External libraries that use the following annotations can add new hardware devices: + * com.qualcomm.robotcore.hardware.configuration.annotations.AnalogSensorType + * com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties + * com.qualcomm.robotcore.hardware.configuration.annotations.DigitalIoDeviceType + * com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType + * com.qualcomm.robotcore.hardware.configuration.annotations.MotorType + * com.qualcomm.robotcore.hardware.configuration.annotations.ServoType + * External libraries that use the following annotations can add new functionality to the Robot Controller: + * org.firstinspires.ftc.ftccommon.external.OnCreate + * org.firstinspires.ftc.ftccommon.external.OnCreateEventLoop + * org.firstinspires.ftc.ftccommon.external.OnCreateMenu + * org.firstinspires.ftc.ftccommon.external.OnDestroy + * org.firstinspires.ftc.ftccommon.external.WebHandlerRegistrar +* Adds support for REV Robotics Driver Hub. +* Adds fully custom userspace USB gamepad driver to Driver Station (see "Advanced Gamepad Features" menu in DS settings). + * Allows gamepads to work on devices without native Linux kernel support (e.g. some Romanian Motorola devices). + * Allows the DS to read the unique serial number of each gamepad, enabling auto-recovery of dropped gamepads even if two gamepads of the same model drop. *(NOTE: unfortunately this does not apply to Etpark gamepads, because they do not have a unique serial)*. + * Reading the unique serial number also provides the ability to configure the DS to assign gamepads to a certain position by default (so no need to do start+a/b at all). + * The LED ring on the Xbox360 gamepad and the RGB LED bar on the PS4 gamepad is used to indicate the driver position the gamepad is bound to. + * The rumble motors on the Xbox360, PS4, and Etpark gamepads can be controlled from OpModes. + * The 2-point touchpad on the PS4 gamepad can be read from OpModes. + * The "back" and "guide" buttons on the gamepad can now be safely bound to robot controls (Previously, on many devices, Android would intercept these buttons as home button presses and close the app). + * Advanced Gamepad features are enabled by default, but may be disabled through the settings menu in order to revert to gamepad support provided natively by Android. +* Improves accuracy of ping measurement. + * Fixes issue where the ping time showed as being higher than reality when initially connecting to or restarting the robot. + * To see the full improvement, you must update both the Robot Controller and Driver Station apps. +* Updates samples located at [/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples](FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples). + * Added ConceptGamepadRumble and ConceptGamepadTouchpad samples to illustrate the use of these new gampad capabilities. + * Condensed existing Vuforia samples into just 2 samples (ConceptVuforiaFieldNavigation & ConceptVuforiaFieldNavigationWebcam) showing how to determine the robot's location on the field using Vuforia. These both use the current season's Target images. + * Added ConceptVuforiaDriveToTargetWebcam to illustrate an easy way to drive directly to any visible Vuforia target. +* Makes many improvements to the warning system and individual warnings. + * Warnings are now much more spaced out, so that they are easier to read. + * New warnings were added for conditions that should be resolved before competing. + * The mismatched apps warning now uses the major and minor app versions, not the version code. + * The warnings are automatically re-enabled when a Robot Controller app from a new FTC season is installed. +* Adds support for I2C transactions on the Expansion Hub / Control Hub without specifying a register address. + * See section 3 of the [TI I2C spec](https://www.ti.com/lit/an/slva704/slva704.pdf). + * Calling these new methods when using Modern Robotics hardware will result in an UnsupportedOperationException. +* Changes VuforiaLocalizer `close()` method to be public. +* Adds support for TensorFlow v2 object detection models. +* Reduces ambiguity of the Self Inspect language and graphics. +* OnBotJava now warns about potentially unintended file overwrites. +* Improves behavior of the Wi-Fi band and channel selector on the Manage webpage. + +### Bug fixes +* Fixes Robot Controller app crash on Android 9+ when a Driver Station connects. +* Fixes issue where an OpMode was responsible for calling shutdown on the + TensorFlow TFObjectDetector. Now this is done automatically. +* Fixes Vuforia initialization blocks to allow user to chose AxesOrder. Updated + relevant blocks sample opmodes. +* Fixes [FtcRobotController issue #114](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/114) + LED blocks and Java class do not work. +* Fixes match logging for OpModes that contain special characters in their names. +* Fixes Driver Station OpMode controls becoming unresponsive if the Driver Station was set to the landscape layout and an OnBotJava build was triggered while an OpMode was running. +* Fixes the Driver Station app closing itself when it is switched away from, or the screen is turned off. +* Fixes "black swirl of doom" (Infinite "configuring Wi-Fi Direct" message) on older devices. +* Updates the wiki comment on the OnBotJava intro page. + +## Version 6.2 (20210218-074821) + +### Enhancements +* Attempts to automatically fix the condition where a Control Hub's internal Expansion Hub is not + working by re-flashing its firmware +* Makes various improvements to the Wi-Fi Direct pairing screen, especially in landscape mode +* Makes the Robot Controller service no longer be categorically restarted when the main activity is brought to foreground + * (e.g. the service is no longer restarted simply by viewing the Self Inspect screen and pressing the back button) + * It is still restarted if the Settings menu or Configure Robot menu is opened + + +### Bug fixes +* Fixes [FtcRobotController issue #71](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/71) + Cannot open OpModes in v6.1 Blocks offline editor +* Fixes [FtcRobotController issue #79](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/79) + 6.1 causes a soft reboot on the Motorola E5 Play +* Fixes issue where the Control Hub OS's watchdog would restart the Robot Controller app if + the Control Hub was not able to communicate with its internal Expansion Hub +* Fixes certain I2C devices not showing up in the appropriate `HardwareMap` fields (such as `hardwareMap.colorSensor`) +* Fixes issue where performing a Wi-Fi factory reset on the Control Hub would not set the Wi-Fi band to 2.4 GHz +* Fixes issue where OnBotJava might fail to create a new file if the option to "Setup Code for Configured Hardware" was selected +* Fixes issue where performing certain operations after an OpMode crashes would temporarily break Control/Expansion Hub communication +* Fixes issue where a Control Hub with a configured USB-connected Expansion Hub would not work if the Expansion Hub was missing at startup +* Fixes potential issues caused by having mismatched Control/Expansion Hub firmware versions +* Fixes [ftc_app issue 673](https://github.com/ftctechnh/ftc_app/issues/673) Latest matchlog is being deleted instead of old ones by RobotLog +* Fixes ConceptVuforiaUltimateGoalNavigationWebcam sample opmode by correctly orienting camera on robot. +* Fixes issue where logcat would be spammed with InterruptedExceptions when stop is requested from the Driver Station (this behavior was accidentally introduced in v5.3). This change has no impact on functionality. +* Fixes issue where the blocks editor fails to load if the name of any TeleOp opmode contains an apostrophe. + +## Version 6.1 (20201209-113742) +* Makes the scan button on the configuration screen update the list of Expansion Hubs connected via RS-485 + * Fixes [SkyStone issue #143](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/143) +* Improves web interface compatibility with older browser and Android System WebView versions. +* Fixes issue in UVC driver where some cameras (e.g. certain MS Lifecams) which reported frame intervals as rounded rather than truncated values (e.g. `666667*100ns` instead of `666666*100ns` for 15FPS) would fail to start streaming. +* Adds support in UVC driver for virtual PTZ control +* Adds support in UVC driver for gain (ISO) control +* Adds support in UVC driver for enabling/disable AE priority. This setting provides a means to tell the camera firmware either + * A) It can undershoot the requested frame rate in order to provide a theoretically better image (i.e. with a longer exposure than the inter-frame period of the selected frame rate allows) + * B) It *must* meet the inter-frame deadline for the selected frame rate, even if the image may be underexposed as a result +* Adds support for the Control Hub OS 1.1.2 Robot Controller watchdog + * The Robot Controller app will be restarted if it stops responding for more than 10 seconds +* Adds support for using the Driver Station app on Android 10+ +* Introduces an automatic TeleOp preselection feature + * For details and usage guide, please see [this wiki entry](https://github.com/FIRST-Tech-Challenge/FtcRobotController/wiki/Automatically-Loading-a-Driver-Controlled-Op-Mode) +* Shows icon next to OpMode name in the OpMode list dropdown on the Driver Station to indicate the source of the OpMode (i.e. the programming tool used to create it) +* Fixes issue where the Driver Station app would exit after displaying the Configuring Wi-Fi Direct screen +* Fixes Blocks and OnBotJava prompts when accessed via the REV Hardware Client + +## Version 6.0 (20200921-085816) + +### Important Notes +* Version 6.0 is the version for the Ultimate Goal season. +* Requires Android Studio 4.0. +* Android Studio users need to be connected to the Internet the first time they build the app (in order to download needed packages for the build). +* Version 5.5 was a moderately large off-season, August 2020, drop. It's worth reviewing those release notes below also. +* Version 5.5 and greater will not work on older Android 4.x and 5.x phones. Users must upgrade to an approved Android 6.x device or newer. +* The default PIDF values for REV motors have been reverted to the default PID values that were used in the 2018-2019 season + * This change was made because the 2018-2019 values turned out to work better for many mechanisms + * This brings the behavior of the REV motors in line with the behavior of all other motors + * If you prefer the 2019-2020 season's behavior for REV motors, here are the PIDF values that were in place, so that you can manually set them in your OpModes: +
+ **HD Hex motors (all gearboxes):** + Velocity PIDF values: `P = 1.17`, `I = 0.117`, `F = 11.7` + Position PIDF values: `P = 5.0` + **Core Hex motor:** + Velocity PIDF values: `P = 4.96`, `I = 0.496`, `F = 49.6` + Position PIDF values: `P = 5.0` + +### New features +* Includes TensorFlow inference model and sample OpModes to detect Ultimate Goal Starter Stacks (four rings vs single ring stack). +* Includes Vuforia Ultimate Goal vision targets and sample OpModes. +* Introduces a digital zoom feature for TensorFlow object detection (to detect objects more accurately at greater distances). +* Adds configuration entry for the REV UltraPlanetary HD Hex motor + +### Enhancements +* Adds setGain() and getGain() methods to the NormalizedColorSensor interface + * By setting the gain of a color sensor, you can adjust for different lighting conditions. + For example, if you detect lower color values than expected, you can increase the gain. + * The gain value is only applied to the argb() and getNormalizedColors() methods, not to the raw color methods. + The getNormalizedColors() method is recommended for ease-of-use and clarity, since argb() has to be converted. + * Updates SensorColor Java sample to demonstrate gain usage +* Merges SensorREVColorDistance Java sample into SensorColor Java sample, which showcases best practices for all color sensors +* Improves retrieving values from the REV Color Sensor V3 + * Updates the normalization calculation of the RGB channels + * Improves the calculation of the alpha channel (can be used as an overall brightness indicator) + * Fixes the default sensor resolution, which caused issues with bright environments + * Adds support for changing the resolution and measuring rate of the Broadcom sensor chip + * Removes IR readings and calculations not meant for the Broadcom sensor chip + +### Bug fixes +* Improves reliability of BNO055IMU IMU initialization to prevent random initialization failures (which manifested as `Problem with 'imu'`). + +## Version 5.5 (20200824-090813) + +Version 5.5 requires Android Studio 4.0 or later. + +### New features +* Adds support for calling custom Java classes from Blocks OpModes (fixes [SkyStone issue #161](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/161)). + * Classes must be in the org.firstinspires.ftc.teamcode package. + * To have easy access to the opMode, hardwareMap, telemetry, gamepad1, and gamepad2, classes can + extends org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion. + * Methods must be public static and have no more than 21 parameters. + * Methods must be annotated with org.firstinspires.ftc.robotcore.external.ExportToBlocks. + * Parameters declared as OpMode, LinearOpMode, Telemetry, and HardwareMap are supported and the + argument is provided automatically, regardless of the order of the parameters. On the block, + the sockets for those parameters are automatically filled in. + * Parameters declared as char or java.lang.Character will accept any block that returns text + and will only use the first character in the text. + * Parameters declared as boolean or java.lang.Boolean will accept any block that returns boolean. + * Parameters declared as byte, java.lang.Byte, short, java.lang.Short, int, java.lang.Integer, + long, or java.lang.Long, will accept any block that returns a number and will round that + value to the nearest whole number. + * Parameters declared as float, java.lang.Float, double, java.lang.Double will accept any + block that returns a number. +* Adds telemetry API method for setting display format + * Classic + * Monospace + * HTML (certain tags only) +* Adds blocks support for switching cameras. +* Adds Blocks support for TensorFlow Object Detection with a custom model. +* Adds support for uploading a custom TensorFlow Object Detection model in the Manage page, which + is especially useful for Blocks and OnBotJava users. +* Shows new Control Hub blink codes when the Wi-Fi band is switched using the Control Hub's button (only possible on Control Hub OS 1.1.2) +* Adds new warnings which can be disabled in the Advanced RC Settings + * Mismatched app versions warning + * Unnecessary 2.4 GHz Wi-Fi usage warning + * REV Hub is running outdated firmware (older than version 1.8.2) +* Adds support for Sony PS4 gamepad, and reworks how gamepads work on the Driver Station + * Removes preference which sets gamepad type based on driver position. Replaced with menu which allows specifying type for gamepads with unknown VID and PID + * Attempts to auto-detect gamepad type based on USB VID and PID + * If gamepad VID and PID is not known, use type specified by user for that VID and PID + * If gamepad VID and PID is not known AND the user has not specified a type for that VID and PID, an educated guess is made about how to map the gamepad +* Driver Station will now attempt to automatically recover from a gamepad disconnecting, and re-assign it to the position it was assigned to when it dropped + * If only one gamepad is assigned and it drops: it can be recovered + * If two gamepads are assigned, and have **different** VID/PID signatures, and only one drops: it will be recovered + * If two gamepads are assigned, and have **different** VID/PID signatures, and BOTH drop: both will be recovered + * If two gamepads are assigned, and have **the same** VID/PID signatures, and only one drops: it will be recovered + * If two gamepads are assigned, and have **the same** VID/PID signatures, and BOTH drop: **neither** will be recovered, because of the ambiguity of the gamepads when they re-appear on the USB bus. + * There is currently one known edge case: if there are **two** gamepads with **the same** VID/PID signature plugged in, **but only one is assigned**, and they BOTH drop, it's a 50-50 chance of which one will be chosen for automatic recovery to the assigned position: it is determined by whichever one is re-enumerated first by the USB bus controller. +* Adds landscape user interface to Driver Station + * New feature: practice timer with audio cues + * New feature (Control Hub only): wireless network connection strength indicator (0-5 bars) + * New feature (Control Hub only): tapping on the ping/channel display will switch to an alternate display showing radio RX dBm and link speed (tap again to switch back) + * The layout will NOT autorotate. You can switch the layout from the Driver Station's settings menu. +### Breaking changes +* Removes support for Android versions 4.4 through 5.1 (KitKat and Lollipop). The minSdkVersion is now 23. +* Removes the deprecated `LinearOpMode` methods `waitOneFullHardwareCycle()` and `waitForNextHardwareCycle()` +### Enhancements +* Handles RS485 address of Control Hub automatically + * The Control Hub is automatically given a reserved address + * Existing configuration files will continue to work + * All addresses in the range of 1-10 are still available for Expansion Hubs + * The Control Hub light will now normally be solid green, without blinking to indicate the address + * The Control Hub will not be shown on the Expansion Hub Address Change settings page +* Improves REV Hub firmware updater + * The user can now choose between all available firmware update files + * Version 1.8.2 of the REV Hub firmware is bundled into the Robot Controller app. + * Text was added to clarify that Expansion Hubs can only be updated via USB. + * Firmware update speed was reduced to improve reliability + * Allows REV Hub firmware to be updated directly from the Manage webpage +* Improves log viewer on Robot Controller + * Horizontal scrolling support (no longer word wrapped) + * Supports pinch-to-zoom + * Uses a monospaced font + * Error messages are highlighted + * New color scheme +* Attempts to force-stop a runaway/stuck OpMode without restarting the entire app + * Not all types of runaway conditions are stoppable, but if the user code attempts to talk to hardware during the runaway, the system should be able to capture it. +* Makes various tweaks to the Self Inspect screen + * Renames "OS version" entry to "Android version" + * Renames "Wi-Fi Direct Name" to "Wi-Fi Name" + * Adds Control Hub OS version, when viewing the report of a Control Hub + * Hides the airplane mode entry, when viewing the report of a Control Hub + * Removes check for ZTE Speed Channel Changer + * Shows firmware version for **all** Expansion and Control Hubs +* Reworks network settings portion of Manage page + * All network settings are now applied with a single click + * The Wi-Fi Direct channel of phone-based Robot Controllers can now be changed from the Manage page + * Wi-Fi channels are filtered by band (2.4 vs 5 GHz) and whether they overlap with other channels + * The current Wi-Fi channel is pre-selected on phone-based Robot Controllers, and Control Hubs running OS 1.1.2 or later. + * On Control Hubs running OS 1.1.2 or later, you can choose to have the system automatically select a channel on the 5 GHz band +* Improves OnBotJava + * New light and dark themes replace the old themes (chaos, github, chrome,...) + * the new default theme is `light` and will be used when you first update to this version + * OnBotJava now has a tabbed editor + * Read-only offline mode +* Improves function of "exit" menu item on Robot Controller and Driver Station + * Now guaranteed to be fully stopped and unloaded from memory +* Shows a warning message if a LinearOpMode exists prematurely due to failure to monitor for the start condition +* Improves error message shown when the Driver Station and Robot Controller are incompatible with each other +* Driver Station OpMode Control Panel now disabled while a Restart Robot is in progress +* Disables advanced settings related to Wi-Fi Direct when the Robot Controller is a Control Hub. +* Tint phone battery icons on Driver Station when low/critical. +* Uses names "Control Hub Portal" and "Control Hub" (when appropriate) in new configuration files +* Improve I2C read performance + * Very large improvement on Control Hub; up to ~2x faster with small (e.g. 6 byte) reads + * Not as apparent on Expansion Hubs connected to a phone +* Update/refresh build infrastructure + * Update to 'androidx' support library from 'com.android.support:appcompat', which is end-of-life + * Update targetSdkVersion and compileSdkVersion to 28 + * Update Android Studio's Android plugin to latest + * Fix reported build timestamp in 'About' screen +* Add sample illustrating manual webcam use: ConceptWebcam + + +### Bug fixes +* Fixes [SkyStone issue #248](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/248) +* Fixes [SkyStone issue #232](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/232) and + modifies bulk caching semantics to allow for cache-preserving MANUAL/AUTO transitions. +* Improves performance when REV 2M distance sensor is unplugged +* Improves readability of Toast messages on certain devices +* Allows a Driver Station to connect to a Robot Controller after another has disconnected +* Improves generation of fake serial numbers for UVC cameras which do not provide a real serial number + * Previously some devices would assign such cameras a serial of `0:0` and fail to open and start streaming + * Fixes [ftc_app issue #638](https://github.com/ftctechnh/ftc_app/issues/638). +* Fixes a slew of bugs with the Vuforia camera monitor including: + * Fixes bug where preview could be displayed with a wonky aspect ratio + * Fixes bug where preview could be cut off in landscape + * Fixes bug where preview got totally messed up when rotating phone + * Fixes bug where crosshair could drift off target when using webcams +* Fixes issue in UVC driver on some devices ([ftc_app 681](https://github.com/ftctechnh/ftc_app/issues/681)) if streaming was started/stopped multiple times in a row + * Issue manifested as kernel panic on devices which do not have [this kernel patch](https://lore.kernel.org/patchwork/patch/352400/). + * On affected devices which **do** have the patch, the issue was manifest as simply a failure to start streaming. + * The Tech Team believes that the root cause of the issue is a bug in the Linux kernel XHCI driver. A workaround was implemented in the SDK UVC driver. +* Fixes bug in UVC driver where often half the frames from the camera would be dropped (e.g. only 15FPS delivered during a streaming session configured for 30FPS). +* Fixes issue where TensorFlow Object Detection would show results whose confidence was lower than + the minimum confidence parameter. +* Fixes a potential exploitation issue of [CVE-2019-11358](https://www.cvedetails.com/cve/CVE-2019-11358/) in OnBotJava +* Fixes changing the address of an Expansion Hub with additional Expansion Hubs connected to it +* Preserves the Control Hub's network connection when "Restart Robot" is selected +* Fixes issue where device scans would fail while the Robot was restarting +* Fix RenderScript usage + * Use androidx.renderscript variant: increased compatibility + * Use RenderScript in Java mode, not native: simplifies build +* Fixes webcam-frame-to-bitmap conversion problem: alpha channel wasn't being initialized, only R, G, & B +* Fixes possible arithmetic overflow in Deadline +* Fixes deadlock in Vuforia webcam support which could cause 5-second delays when stopping OpMode + +## Version 5.4 (20200108-101156) +* Fixes [SkyStone issue #88](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/88) +* Adds an inspection item that notes when a robot controller (Control Hub) is using the factory default password. +* Fixes [SkyStone issue #61](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/61) +* Fixes [SkyStone issue #142](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/142) +* Fixes [ftc_app issue #417](https://github.com/ftctechnh/ftc_app/issues/417) by adding more current and voltage monitoring capabilities for REV Hubs. +* Fixes [a crash sometimes caused by OnBotJava activity](https://ftcforum.firstinspires.org/forum/ftc-technology/76217-onbotjava-crashes-robot-controller) +* Improves OnBotJava autosave functionality [ftc_app #738](https://github.com/ftctechnh/ftc_app/issues/738) +* Fixes system responsiveness issue when an Expansion Hub is disconnected +* Fixes issue where IMU initialization could prevent OpModes from stopping +* Fixes issue where AndroidTextToSpeech.speak() would fail if it was called too early +* Adds telemetry.speak() methods and blocks, which cause the Driver Station (if also updated) to speak text +* Adds and improves Expansion Hub-related warnings + * Improves Expansion Hub low battery warning + * Displays the warning immediately after the hub reports it + * Specifies whether the condition is current or occurred temporarily during an OpMode run + * Displays which hubs reported low battery + * Displays warning when hub loses and regains power during an OpMode run + * Fixes the hub's LED pattern after this condition + * Displays warning when Expansion Hub is not responding to commands + * Specifies whether the condition is current or occurred temporarily during an OpMode run + * Clarifies warning when Expansion Hub is not present at startup + * Specifies that this condition requires a Robot Restart before the hub can be used. + * The hub light will now accurately reflect this state + * Improves logging and reduces log spam during these conditions +* Syncs the Control Hub time and timezone to a connected web browser programming the robot, if a Driver Station is not available. +* Adds bulk read functionality for REV Hubs + * A bulk caching mode must be set at the Hub level with `LynxModule#setBulkCachingMode()`. This applies to all relevant SDK hardware classes that reference that Hub. + * The following following Hub bulk caching modes are available: + * `BulkCachingMode.OFF` (default): All hardware calls operate as usual. Bulk data can read through `LynxModule#getBulkData()` and processed manually. + * `BulkCachingMode.AUTO`: Applicable hardware calls are served from a bulk read cache that is cleared/refreshed automatically to ensure identical commands don't hit the same cache. The cache can also be cleared manually with `LynxModule#clearBulkCache()`, although this is not recommended. + * (advanced users) `BulkCachingMode.MANUAL`: Same as `BulkCachingMode.AUTO` except the cache is never cleared automatically. To avoid getting stale data, the cache must be manually cleared at the beginning of each loop body or as the user deems appropriate. +* Removes PIDF Annotation values added in Rev 5.3 (to AndyMark, goBILDA and TETRIX motor configurations). + * The new motor types will still be available but their Default control behavior will revert back to Rev 5.2 +* Adds new `ConceptMotorBulkRead` sample Opmode to demonstrate and compare Motor Bulk-Read modes for reducing I/O latencies. + +## Version 5.3 (20191004-112306) +* Fixes external USB/UVC webcam support +* Makes various bugfixes and improvements to Blocks page, including but not limited to: + * Many visual tweaks + * Browser zoom and window resize behave better + * Resizing the Java preview pane works better and more consistently across browsers + * The Java preview pane consistently gets scrollbars when needed + * The Java preview pane is hidden by default on phones + * Internet Explorer 11 should work + * Large dropdown lists display properly on lower res screens + * Disabled buttons are now visually identifiable as disabled + * A warning is shown if a user selects a TFOD sample, but their device is not compatible + * Warning messages in a Blocks OpMode are now visible by default. +* Adds goBILDA 5201 and 5202 motors to Robot Configurator +* Adds PIDF Annotation values to AndyMark, goBILDA and TETRIX motor configurations. + This has the effect of causing the RUN_USING_ENCODERS and RUN_TO_POSITION modes to use + PIDF vs PID closed loop control on these motors. This should provide more responsive, yet stable, speed control. + PIDF adds Feedforward control to the basic PID control loop. + Feedforward is useful when controlling a motor's speed because it "anticipates" how much the control voltage + must change to achieve a new speed set-point, rather than requiring the integrated error to change sufficiently. + The PIDF values were chosen to provide responsive, yet stable, speed control on a lightly loaded motor. + The more heavily a motor is loaded (drag or friction), the more noticable the PIDF improvement will be. +* Fixes startup crash on Android 10 +* Fixes [ftc_app issue #712](https://github.com/ftctechnh/ftc_app/issues/712) (thanks to FROGbots-4634) +* Fixes [ftc_app issue #542](https://github.com/ftctechnh/ftc_app/issues/542) +* Allows "A" and lowercase letters when naming device through RC and DS apps. + +## Version 5.2 (20190905-083277) +* Fixes extra-wide margins on settings activities, and placement of the new configuration button +* Adds Skystone Vuforia image target data. + * Includes sample Skystone Vuforia Navigation OpModes (Java). + * Includes sample Skystone Vuforia Navigation OpModes (Blocks). +* Adds TensorFlow inference model (.tflite) for Skystone game elements. + * Includes sample Skystone TensorFlow OpModes (Java). + * Includes sample Skystone TensorFlow OpModes (Blocks). +* Removes older (season-specific) sample OpModes. +* Includes 64-bit support (to comply with [Google Play requirements](https://android-developers.googleblog.com/2019/01/get-your-apps-ready-for-64-bit.html)). +* Protects against Stuck OpModes when a Restart Robot is requested. (Thanks to FROGbots-4634) ([ftc_app issue #709](https://github.com/ftctechnh/ftc_app/issues/709)) +* Blocks related changes: + * Fixes bug with blocks generated code when hardware device name is a java or javascript reserved word. + * Shows generated java code for blocks, even when hardware items are missing from the active configuration. + * Displays warning icon when outdated Vuforia and TensorFlow blocks are used ([SkyStone issue #27](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/27)) + +## Version 5.1 (20190820-222104) +* Defines default PIDF parameters for the following motors: + * REV Core Hex Motor + * REV 20:1 HD Hex Motor + * REV 40:1 HD Hex Motor +* Adds back button when running on a device without a system back button (such as a Control Hub) +* Allows a REV Control Hub to update the firmware on a REV Expansion Hub via USB +* Fixes [SkyStone issue #9](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/9) +* Fixes [ftc_app issue #715](https://github.com/ftctechnh/ftc_app/issues/715) +* Prevents extra DS User clicks by filtering based on current state. +* Prevents incorrect DS UI state changes when receiving new OpMode list from RC +* Adds support for REV Color Sensor V3 +* Adds a manual-refresh DS Camera Stream for remotely viewing RC camera frames. + * To show the stream on the DS, initialize **but do not run** a stream-enabled opmode, select the Camera Stream option in the DS menu, and tap the image to refresh. This feature is automatically enabled when using Vuforia or TFOD—no additional RC configuration is required for typical use cases. To hide the stream, select the same menu item again. + * Note that gamepads are disabled and the selected opmode cannot be started while the stream is open as a safety precaution. + * To use custom streams, consult the API docs for `CameraStreamServer#setSource` and `CameraStreamSource`. +* Adds many Star Wars sounds to RobotController resources. +* Added Skystone Sounds Chooser Sample Program. +* Switches out startup, connect chimes, and error/warning sounds for Star Wars sounds +* Updates OnBot Java to use a WebSocket for communication with the robot + * The OnBot Java page no longer has to do a full refresh when a user switches from editing one file to another + +Known issues: +* Camera Stream + * The Vuforia camera stream inherits the issues present in the phone preview (namely [ftc_app issue #574](https://github.com/ftctechnh/ftc_app/issues/574)). This problem does not affect the TFOD camera stream even though it receives frames from Vuforia. + * The orientation of the stream frames may not always match the phone preview. For now, these frames may be rotated manually via a custom `CameraStreamSource` if desired. +* OnBotJava + * Browser back button may not always work correctly + * It's possible for a build to be queued, but not started. The OnBot Java build console will display a warning if this occurs. + * A user might not realize they are editing a different file if the user inadvertently switches from one file to another since this switch is now seamless. The name of the currently open file is displayed in the browser tab. + +## Version 5.0 (built on 19.06.14) +* Support for the REV Robotics Control Hub. +* Adds a Java preview pane to the Blocks editor. +* Adds a new offline export feature to the Blocks editor. +* Display Wi-Fi channel in Network circle on Driver Station. +* Adds calibration for Logitech C270 +* Updates build tooling and target SDK. +* Compliance with Google's permissions infrastructure (Required after build tooling update). +* Keep Alives to mitigate the Motorola Wi-Fi scanning problem. Telemetry substitute no longer necessary. +* Improves Vuforia error reporting. +* Fixes ftctechnh/ftc_app issues 621, 713. +* Miscellaneous bug fixes and improvements. + +## Version 4.3 (built on 18.10.31) +* Includes missing TensorFlow-related libraries and files. + +## Version 4.2 (built on 18.10.30) +* Includes fix to avoid deadlock situation with WatchdogMonitor which could result in USB communication errors. + - Comm error appeared to require that user disconnect USB cable and restart the Robot Controller app to recover. + - robotControllerLog.txt would have error messages that included the words "E RobotCore: lynx xmit lock: #### abandoning lock:" +* Includes fix to correctly list the parent module address for a REV Robotics Expansion Hub in a configuration (.xml) file. + - Bug in versions 4.0 and 4.1 would incorrect list the address module for a parent REV Robotics device as "1". + - If the parent module had a higher address value than the daisy-chained module, then this bug would prevent the Robot Controller from communicating with the downstream Expansion Hub. +* Added requirement for ACCESS_COARSE_LOCATION to allow a Driver Station running Android Oreo to scan for Wi-Fi Direct devices. +* Added google() repo to build.gradle because aapt2 must be downloaded from the google() repository beginning with version 3.2 of the Android Gradle Plugin. + - Important Note: Android Studio users will need to be connected to the Internet the first time build the ftc_app project. + - Internet connectivity is required for the first build so the appropriate files can be downloaded from the Google repository. + - Users should not need to be connected to the Internet for subsequent builds. + - This should also fix buid issue where Android Studio would complain that it "Could not find com.android.tools.lint:lint-gradle:26.1.4" (or similar). +* Added support for REV Spark Mini motor controller as part of the configuration menu for a servo/PWM port on the REV Expansion Hub. +* Provide examples for playing audio files in an OpMode. +* Block Development Tool Changes + - Includes a fix for a problem with the Velocity blocks that were reported in the FTC Technology forum (Blocks Programming subforum). + - Change the "Save completed successfully." message to a white color so it will contrast with a green background. + - Fixed the "Download image" feature so it will work if there are text blocks in the OpMode. +* Introduce support for Google's TensorFlow Lite technology for object detetion for 2018-2019 game. + - TensorFlow lite can recognize Gold Mineral and Silver Mineral from 2018-2019 game. + - Example Java and Block OpModes are included to show how to determine the relative position of the gold block (left, center, right). + +## Version 4.1 (released on 18.09.24) + +Changes include: +* Fix to prevent crash when deprecated configuration annotations are used. +* Change to allow FTC Robot Controller APK to be auto-updated using FIRST Global Control Hub update scripts. +* Removed samples for non supported / non legal hardware. +* Improvements to Telemetry.addData block with "text" socket. +* Updated Blocks sample OpMode list to include Rover Ruckus Vuforia example. +* Update SDK library version number. + +## Version 4.0 (released on 18.09.12) + +Changes include: +* Initial support for UVC compatible cameras + - If UVC camera has a unique serial number, RC will detect and enumerate by serial number. + - If UVC camera lacks a unique serial number, RC will only support one camera of that type connected. + - Calibration settings for a few cameras are included (see TeamCode/src/main/res/xml/teamwebcamcalibrations.xml for details). + - User can upload calibration files from Program and Manage web interface. + - UVC cameras seem to draw a fair amount of electrical current from the USB bus. + + This does not appear to present any problems for the REV Robotics Control Hub. + + This does seem to create stability problems when using some cameras with an Android phone-based Robot Controller. + + FTC Tech Team is investigating options to mitigate this issue with the phone-based Robot Controllers. + - Updated sample Vuforia Navigation and VuMark OpModes to demonstrate how to use an internal phone-based camera and an external UVC webcam. + +* Support for improved motor control. + - REV Robotics Expansion Hub firmware 1.8 and greater will support a feed forward mechanism for closed loop motor control. + - FTC SDK has been modified to support PIDF coefficients (proportional, integral, derivative, and feed forward). + - FTC Blocks development tool modified to include PIDF programming blocks. + - Deprecated older PID-related methods and variables. + - REV's 1.8.x PIDF-related changes provide a more linear and accurate way to control a motor. + +* Wireless + - Added 5GHz support for wireless channel changing for those devices that support it. + + Tested with Moto G5 and E4 phones. + + Also tested with other (currently non-approved) phones such as Samsung Galaxy S8. + +* Improved Expansion Hub firmware update support in Robot Controller app + - Changes to make the system more robust during the firmware update process (when performed through Robot Controller app). + - User no longer has to disconnect a downstream daisy-chained Expansion Hub when updating an Expansion Hub's firmware. + + If user is updating an Expansion Hub's firmware through a USB connection, he/she does not have to disconnect RS485 connection to other Expansion Hubs. + + The user still must use a USB connection to update an Expansion Hub's firmware. + + The user cannot update the Expansion Hub firmware for a downstream device that is daisy chained through an RS485 connection. + - If an Expansion Hub accidentally gets "bricked" the Robot Controller app is now more likely to recognize the Hub when it scans the USB bus. + + Robot Controller app should be able to detect an Expansion Hub, even if it accidentally was bricked in a previous update attempt. + + Robot Controller app should be able to install the firmware onto the Hub, even if if accidentally was bricked in a previous update attempt. + +* Resiliency + - FTC software can detect and enable an FTDI reset feature that is available with REV Robotics v1.8 Expansion Hub firmware and greater. + + When enabled, the Expansion Hub can detect if it hasn't communicated with the Robot Controller over the FTDI (USB) connection. + + If the Hub hasn't heard from the Robot Controller in a while, it will reset the FTDI connection. + + This action helps system recover from some ESD-induced disruptions. + - Various fixes to improve reliability of FTC software. + +* Blocks + - Fixed errors with string and list indices in blocks export to java. + - Support for USB connected UVC webcams. + - Refactored optimized Blocks Vuforia code to support Rover Ruckus image targets. + - Added programming blocks to support PIDF (proportional, integral, derivative and feed forward) motor control. + - Added formatting options (under Telemetry and Miscellaneous categories) so user can set how many decimal places to display a numerical value. + - Support to play audio files (which are uploaded through Blocks web interface) on Driver Station in addition to the Robot Controller. + - Fixed bug with Download Image of Blocks feature. + - Support for REV Robotics Blinkin LED Controller. + - Support for REV Robotics 2m Distance Sensor. + - Added support for a REV Touch Sensor (no longer have to configure as a generic digital device). + - Added blocks for DcMotorEx methods. + + These are enhanced methods that you can use when supported by the motor controller hardware. + + The REV Robotics Expansion Hub supports these enhanced methods. + + Enhanced methods include methods to get/set motor velocity (in encoder pulses per second), get/set PIDF coefficients, etc.. + +* Modest Improvements in Logging + - Decrease frequency of battery checker voltage statements. + - Removed non-FTC related log statements (wherever possible). + - Introduced a "Match Logging" feature. + + Under "Settings" a user can enable/disable this feature (it's disabled by default). + + If enabled, user provides a "Match Number" through the Driver Station user interface (top of the screen). + * The Match Number is used to create a log file specifically with log statements from that particular OpMode run. + * Match log files are stored in /sdcard/FIRST/matlogs on the Robot Controller. + * Once an OpMode run is complete, the Match Number is cleared. + * This is a convenient way to create a separate match log with statements only related to a specific OpMode run. + +* New Devices + - Support for REV Robotics Blinkin LED Controller. + - Support for REV Robotics 2m Distance Sensor. + - Added configuration option for REV 20:1 HD Hex Motor. + - Added support for a REV Touch Sensor (no longer have to configure as a generic digital device). + +* Miscellaneous + - Fixed some errors in the definitions for acceleration and velocity in our javadoc documentation. + - Added ability to play audio files on Driver Station + - When user is configuring an Expansion Hub, the LED on the Expansion Hub will change blink pattern (purple-cyan) to indicate which Hub is currently being configured. + - Renamed I2cSensorType to I2cDeviceType. + - Added an external sample OpMode that demonstrates localization using 2018-2019 (Rover Ruckus presented by QualComm) Vuforia targets. + - Added an external sample OpMode that demonstrates how to use the REV Robotics 2m Laser Distance Sensor. + - Added an external sample OpMode that demonstrates how to use the REV Robotics Blinkin LED Controller. + - Re-categorized external Java sample OpModes to "TeleOp" instead of "Autonomous". + +Known issues: +* Initial support for UVC compatible cameras + - UVC cameras seem to draw significant amount of current from the USB bus. + + This does not appear to present any problems for the REV Robotics Control Hub. + + This does seem to create stability problems when using some cameras with an Android phone-based Robot Controller. + + FTC Tech Team is investigating options to mitigate this issue with the phone-based Robot Controllers. + - There might be a possible deadlock which causes the RC to become unresponsive when using a UVC webcam with a Nougat Android Robot Controller. + +* Wireless + - When user selects a wireless channel, this channel does not necessarily persist if the phone is power cycled. + + Tech Team is hoping to eventually address this issue in a future release. + + Issue has been present since apps were introduced (i.e., it is not new with the v4.0 release). + - Wireless channel is not currently displayed for Wi-Fi Direct connections. + +* Miscellaneous + - The blink indication feature that shows which Expansion Hub is currently being configured does not work for a newly created configuration file. + + User has to first save a newly created configuration file and then close and re-edit the file in order for blink indicator to work. + +## Version 3.6 (built on 17.12.18) + +Changes include: +* Blocks Changes + - Uses updated Google Blockly software to allow users to edit their OpModes on Apple iOS devices (including iPad and iPhone). + - Improvement in Blocks tool to handle corrupt OpMode files. + - Autonomous OpModes should no longer get switched back to tele-op after re-opening them to be edited. + - The system can now detect type mismatches during runtime and alert the user with a message on the Driver Station. +* Updated javadoc documentation for setPower() method to reflect correct range of values (-1 to +1). +* Modified VuforiaLocalizerImpl to allow for user rendering of frames + - Added a user-overrideable onRenderFrame() method which gets called by the class's renderFrame() method. + +## Version 3.5 (built on 17.10.30) + +Changes with version 3.5 include: +* Introduced a fix to prevent random OpMode stops, which can occur after the Robot Controller app has been paused and then resumed (for example, when a user temporarily turns off the display of the Robot Controller phone, and then turns the screen back on). +* Introduced a fix to prevent random OpMode stops, which were previously caused by random peer disconnect events on the Driver Station. +* Fixes issue where log files would be closed on pause of the RC or DS, but not re-opened upon resume. +* Fixes issue with battery handler (voltage) start/stop race. +* Fixes issue where Android Studio generated OpModes would disappear from available list in certain situations. +* Fixes problem where OnBot Java would not build on REV Robotics Control Hub. +* Fixes problem where OnBot Java would not build if the date and time on the Robot Controller device was "rewound" (set to an earlier date/time). +* Improved error message on OnBot Java that occurs when renaming a file fails. +* Removed unneeded resources from android.jar binaries used by OnBot Java to reduce final size of Robot Controller app. +* Added MR_ANALOG_TOUCH_SENSOR block to Blocks Programming Tool. + +## Version 3.4 (built on 17.09.06) + +Changes with version 3.4 include: +* Added telemetry.update() statement for BlankLinearOpMode template. +* Renamed sample Block OpModes to be more consistent with Java samples. +* Added some additional sample Block OpModes. +* Reworded OnBot Java readme slightly. + +## Version 3.3 (built on 17.09.04) + +This version of the software includes improves for the FTC Blocks Programming Tool and the OnBot Java Programming Tool. + +Changes with verion 3.3 include: +* Android Studio ftc_app project has been updated to use Gradle Plugin 2.3.3. +* Android Studio ftc_app project is already using gradle 3.5 distribution. +* Robot Controller log has been renamed to /sdcard/RobotControllerLog.txt (note that this change was actually introduced w/ v3.2). +* Improvements in I2C reliability. +* Optimized I2C read for REV Expansion Hub, with v1.7 firmware or greater. +* Updated all external/samples (available through OnBot and in Android project folder). +* Vuforia + - Added support for VuMarks that will be used for the 2017-2018 season game. +* Blocks + - Update to latest Google Blockly release. + - Sample OpModes can be selected as a template when creating new OpMode. + - Fixed bug where the blocks would disappear temporarily when mouse button is held down. + - Added blocks for Range.clip and Range.scale. + - User can now disable/enable Block OpModes. + - Fix to prevent occasional Blocks deadlock. +* OnBot Java + - Significant improvements with autocomplete function for OnBot Java editor. + - Sample OpModes can be selected as a template when creating new OpMode. + - Fixes and changes to complete hardware setup feature. + - Updated (and more useful) onBot welcome message. + +Known issues: +* Android Studio + - After updating to the new v3.3 Android Studio project folder, if you get error messages indicating "InvalidVirtualFileAccessException" then you might need to do a File->Invalidate Caches / Restart to clear the error. +* OnBot Java + - Sometimes when you push the build button to build all OpModes, the RC returns an error message that the build failed. If you press the build button a second time, the build typically suceeds. + +## Version 3.2 (built on 17.08.02) + +This version of the software introduces the "OnBot Java" Development Tool. Similar to the FTC Blocks Development Tool, the FTC OnBot Java Development Tool allows a user to create, edit and build OpModes dynamically using only a Javascript-enabled web browser. + +The OnBot Java Development Tool is an integrated development environment (IDE) that is served up by the Robot Controller. OpModes are created and edited using a Javascript-enabled browser (Google Chromse is recommended). OpModes are saved on the Robot Controller Android device directly. + +The OnBot Java Development Tool provides a Java programming environment that does NOT need Android Studio. -## ADVANCED Multi-Team App management: Cloning the TeamCode Module +Changes with version 3.2 include: +* Enhanced web-based development tools + - Introduction of OnBot Java Development Tool. + - Web-based programming and management features are "always on" (user no longer needs to put Robot Controller into programming mode). + - Web-based management interface (where user can change Robot Controller name and also easily download Robot Controller log file). + - OnBot Java, Blocks and Management features available from web based interface. -In some situations, you have multiple teams in your club and you want them to all share -a common code organization, with each being able to *see* the others code but each having -their own team module with their own code that they maintain themselves. +* Blocks Programming Development Tool: + - Changed "LynxI2cColorRangeSensor" block to "REV Color/range sensor" block. + - Fixed tooltip for ColorSensor.isLightOn block. + Added blocks for ColorSensor.getNormalizedColors and LynxI2cColorRangeSensor.getNormalizedColors. -In this situation, you might wish to clone the TeamCode module, once for each of these teams. -Each of the clones would then appear along side each other in the Android Studio module list, -together with the FtcRobotController module (and the original TeamCode module). +* Added example OpModes for digital touch sensor and REV Robotics Color Distance sensor. +* User selectable color themes. +* Includes many minor enhancements and fixes (too numerous to list). -Selective Team phones can then be programmed by selecting the desired Module from the pulldown list -prior to clicking to the green Run arrow. +Known issues: +* Auto complete function is incomplete and does not support the following (for now): + - Access via *this* keyword + - Access via *super* keyword + - Members of the super cloass, not overridden by the class + - Any methods provided in the current class + - Inner classes + - Can't handle casted objects + - Any objects coming from an parenthetically enclosed expression -Warning: This is not for the inexperienced Software developer. -You will need to be comfortable with File manipulations and managing Android Studio Modules. -These changes are performed OUTSIDE of Android Studios, so close Android Studios before you do this. - -Also.. Make a full project backup before you start this :) +## Version 3.10 (built on 17.05.09) -To clone TeamCode, do the following: +This version of the software provides support for the REV Robotics Expansion Hub. This version also includes improvements in the USB communication layer in an effort to enhance system resiliency. If you were using a 2.x version of the software previously, updating to version 3.1 requires that you also update your Driver Station software in addition to updating the Robot Controller software. -Note: Some names start with "Team" and others start with "team". This is intentional. +Also note that in version 3.10 software, the setMaxSpeed and getMaxSpeed methods are no longer available (not deprecated, they have been removed from the SDK). Also note that the the new 3.x software incorporates motor profiles that a user can select as he/she configures the robot. -1) Using your operating system file management tools, copy the whole "TeamCode" - folder to a sibling folder with a corresponding new name, eg: "Team0417". +Changes include: +* Blocks changes + - Added VuforiaTrackableDefaultListener.getPose and Vuforia.trackPose blocks. + - Added optimized blocks support for Vuforia extended tracking. + - Added atan2 block to the math category. + - Added useCompetitionFieldTargetLocations parameter to Vuforia.initialize block. If set to false, the target locations are placed at (0,0,0) with target orientation as specified in https://github.com/gearsincorg/FTCVuforiaDemo/blob/master/Robot_Navigation.java tutorial OpMode. +* Incorporates additional improvements to USB comm layer to improve system resiliency (to recover from a greater number of communication disruptions). -2) In the new Team0417 folder, delete the TeamCode.iml file. +************************************************************************************** -3) the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder - to a matching name with a lowercase 'team' eg: "team0417". +Additional Notes Regarding Version 3.00 (built on 17.04.13) -4) In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that contains - package="org.firstinspires.ftc.teamcode" - to be - package="org.firstinspires.ftc.team0417" +In addition to the release changes listed below (see section labeled "Version 3.00 (built on 17.04.013)"), version 3.00 has the following important changes: -5) Add: include ':Team0417' to the "/settings.gradle" file. - -6) Open up Android Studios and clean out any old files by using the menu to "Build/Clean Project"" \ No newline at end of file +1. Version 3.00 software uses a new version of the FTC Robocol (robot protocol). If you upgrade to v3.0 on the Robot Controller and/or Android Studio side, you must also upgrade the Driver Station software to match the new Robocol. +2. Version 3.00 software removes the setMaxSpeed and getMaxSpeed methods from the DcMotor class. If you have an OpMode that formerly used these methods, you will need to remove the references/calls to these methods. Instead, v3.0 provides the max speed information through the use of motor profiles that are selected by the user during robot configuration. +3. Version 3.00 software currently does not have a mechanism to disable extra i2c sensors. We hope to re-introduce this function with a release in the near future. + +************************************************************************************** + +## Version 3.00 (built on 17.04.13) + +*** Use this version of the software at YOUR OWN RISK!!! *** + +This software is being released as an "alpha" version. Use this version at your own risk! + +This pre-release software contains SIGNIFICANT changes, including changes to the Wi-Fi Direct pairing mechanism, rewrites of the I2C sensor classes, changes to the USB/FTDI layer, and the introduction of support for the REV Robotics Expansion Hub and the REV Robotics color-range-light sensor. These changes were implemented to improve the reliability and resiliency of the FTC control system. + +Please note, however, that version 3.00 is considered "alpha" code. This code is being released so that the FIRST community will have an opportunity to test the new REV Expansion Hub electronics module when it becomes available in May. The developers do not recommend using this code for critical applications (i.e., competition use). + +*** Use this version of the software at YOUR OWN RISK!!! *** + +Changes include: +* Major rework of sensor-related infrastructure. Includes rewriting sensor classes to implement synchronous I2C communication. +* Fix to reset Autonomous timer back to 30 seconds. +* Implementation of specific motor profiles for approved 12V motors (includes Tetrix, AndyMark, Matrix and REV models). +* Modest improvements to enhance Wi-Fi P2P pairing. +* Fixes telemetry log addition race. +* Publishes all the sources (not just a select few). +* Includes Block programming improvements + - Addition of optimized Vuforia blocks. + - Auto scrollbar to projects and sounds pages. + - Fixed blocks paste bug. + - Blocks execute after while-opModeIsActive loop (to allow for cleanup before exiting OpMode). + - Added gyro integratedZValue block. + - Fixes bug with projects page for Firefox browser. + - Added IsSpeaking block to AndroidTextToSpeech. +* Implements support for the REV Robotics Expansion Hub + - Implements support for integral REV IMU (physically installed on I2C bus 0, uses same Bosch BNO055 9 axis absolute orientation sensor as Adafruit 9DOF abs orientation sensor). - Implements support for REV color/range/light sensor. + - Provides support to update Expansion Hub firmware through FTC SDK. + - Detects REV firmware version and records in log file. + - Includes support for REV Control Hub (note that the REV Control Hub is not yet approved for FTC use). + - Implements FTC Blocks programming support for REV Expansion Hub and sensor hardware. + - Detects and alerts when I2C device disconnect. + +## Version 2.62 (built on 17.01.07) +* Added null pointer check before calling modeToByte() in finishModeSwitchIfNecessary method for ModernRoboticsUsbDcMotorController class. +* Changes to enhance Modern Robotics USB protocol robustness. + +## Version 2.61 (released on 16.12.19) +* Blocks Programming mode changes: + - Fix to correct issue when an exception was thrown because an OpticalDistanceSensor object appears twice in the hardware map (the second time as a LightSensor). + +## Version 2.6 (released on 16.12.16) +* Fixes for Gyro class: + - Improve (decrease) sensor refresh latency. + - fix isCalibrating issues. +* Blocks Programming mode changes: + - Blocks now ignores a device in the configuration xml if the name is empty. Other devices work in configuration work fine. + +## Version 2.5 (internal release on released on 16.12.13) +* Blocks Programming mode changes: + - Added blocks support for AdafruitBNO055IMU. + - Added Download OpMode button to FtcBocks.html. + - Added support for copying blocks in one OpMode and pasting them in an other OpMode. The clipboard content is stored on the phone, so the programming mode server must be running. + - Modified Utilities section of the toolbox. + - In Programming Mode, display information about the active connections. + - Fixed paste location when workspace has been scrolled. + - Added blocks support for the android Accelerometer. + - Fixed issue where Blocks Upload OpMode truncated name at first dot. + - Added blocks support for Android SoundPool. + - Added type safety to blocks for Acceleration. + - Added type safety to blocks for AdafruitBNO055IMU.Parameters. + - Added type safety to blocks for AnalogInput. + - Added type safety to blocks for AngularVelocity. + - Added type safety to blocks for Color. + - Added type safety to blocks for ColorSensor. + - Added type safety to blocks for CompassSensor. + - Added type safety to blocks for CRServo. + - Added type safety to blocks for DigitalChannel. + - Added type safety to blocks for ElapsedTime. + - Added type safety to blocks for Gamepad. + - Added type safety to blocks for GyroSensor. + - Added type safety to blocks for IrSeekerSensor. + - Added type safety to blocks for LED. + - Added type safety to blocks for LightSensor. + - Added type safety to blocks for LinearOpMode. + - Added type safety to blocks for MagneticFlux. + - Added type safety to blocks for MatrixF. + - Added type safety to blocks for MrI2cCompassSensor. + - Added type safety to blocks for MrI2cRangeSensor. + - Added type safety to blocks for OpticalDistanceSensor. + - Added type safety to blocks for Orientation. + - Added type safety to blocks for Position. + - Added type safety to blocks for Quaternion. + - Added type safety to blocks for Servo. + - Added type safety to blocks for ServoController. + - Added type safety to blocks for Telemetry. + - Added type safety to blocks for Temperature. + - Added type safety to blocks for TouchSensor. + - Added type safety to blocks for UltrasonicSensor. + - Added type safety to blocks for VectorF. + - Added type safety to blocks for Velocity. + - Added type safety to blocks for VoltageSensor. + - Added type safety to blocks for VuforiaLocalizer.Parameters. + - Added type safety to blocks for VuforiaTrackable. + - Added type safety to blocks for VuforiaTrackables. + - Added type safety to blocks for enums in AdafruitBNO055IMU.Parameters. + - Added type safety to blocks for AndroidAccelerometer, AndroidGyroscope, AndroidOrientation, and AndroidTextToSpeech. + +## Version 2.4 (released on 16.11.13) +* Fix to avoid crashing for nonexistent resources. +* Blocks Programming mode changes: + - Added blocks to support OpenGLMatrix, MatrixF, and VectorF. + - Added blocks to support AngleUnit, AxesOrder, AxesReference, CameraDirection, CameraMonitorFeedback, DistanceUnit, and TempUnit. + - Added blocks to support Acceleration. + - Added blocks to support LinearOpMode.getRuntime. + - Added blocks to support MagneticFlux and Position. + - Fixed typos. + - Made blocks for ElapsedTime more consistent with other objects. + - Added blocks to support Quaternion, Velocity, Orientation, AngularVelocity. + - Added blocks to support VuforiaTrackables, VuforiaTrackable, VuforiaLocalizer, VuforiaTrackableDefaultListener. + - Fixed a few blocks. + - Added type checking to new blocks. + - Updated to latest blockly. + - Added default variable blocks to navigation and matrix blocks. + - Fixed toolbox entry for openGLMatrix_rotation_withAxesArgs. + - When user downloads Blocks-generated OpMode, only the .blk file is downloaded. + - When user uploads Blocks-generated OpMode (.blk file), Javascript code is auto generated. + - Added DbgLog support. + - Added logging when a blocks file is read/written. + - Fixed bug to properly render blocks even if missing devices from configuration file. + - Added support for additional characters (not just alphanumeric) for the block file names (for download and upload). + - Added support for OpMode flavor (“Autonomous” or “TeleOp”) and group. +* Changes to Samples to prevent tutorial issues. +* Incorporated suggested changes from public pull 216 (“Replace .. paths”). +* Remove Servo Glitches when robot stopped. +* if user hits “Cancels” when editing a configuration file, clears the unsaved changes and reverts to original unmodified configuration. +* Added log info to help diagnose why the Robot Controller app was terminated (for example, by watch dog function). +* Added ability to transfer log from the controller. +* Fixed inconsistency for AngularVelocity +* Limit unbounded growth of data for telemetry. If user does not call telemetry.update() for LinearOpMode in a timely manner, data added for telemetry might get lost if size limit is exceeded. + +## Version 2.35 (released on 16.10.06) +* Blockly programming mode - Removed unnecesary idle() call from blocks for new project. + +## Version 2.30 (released on 16.10.05) +* Blockly programming mode: + - Mechanism added to save Blockly OpModes from Programming Mode Server onto local device + - To avoid clutter, blocks are displayed in categorized folders + - Added support for DigitalChannel + - Added support for ModernRoboticsI2cCompassSensor + - Added support for ModernRoboticsI2cRangeSensor + - Added support for VoltageSensor + - Added support for AnalogInput + - Added support for AnalogOutput + - Fix for CompassSensor setMode block +* Vuforia + - Fix deadlock / make camera data available while Vuforia is running. + - Update to Vuforia 6.0.117 (recommended by Vuforia and Google to close security loophole). +* Fix for autonomous 30 second timer bug (where timer was in effect, even though it appeared to have timed out). +* opModeIsActive changes to allow cleanup after OpMode is stopped (with enforced 2 second safety timeout). +* Fix to avoid reading i2c twice. +* Updated sample OpModes. +* Improved logging and fixed intermittent freezing. +* Added digital I/O sample. +* Cleaned up device names in sample OpModes to be consistent with Pushbot guide. +* Fix to allow use of IrSeekerSensorV3. + +## Version 2.20 (released on 16.09.08) +* Support for Modern Robotics Compass Sensor. +* Support for Modern Robotics Range Sensor. +* Revise device names for Pushbot templates to match the names used in Pushbot guide. +* Fixed bug so that IrSeekerSensorV3 device is accessible as IrSeekerSensor in hardwareMap. +* Modified computer vision code to require an individual Vuforia license (per legal requirement from PTC). +* Minor fixes. +* Blockly enhancements: + - Support for Voltage Sensor. + - Support for Analog Input. + - Support for Analog Output. + - Support for Light Sensor. + - Support for Servo Controller. + +## Version 2.10 (released on 16.09.03) +* Support for Adafruit IMU. +* Improvements to ModernRoboticsI2cGyro class + - Block on reset of z axis. + - isCalibrating() returns true while gyro is calibration. +* Updated sample gyro program. +* Blockly enhancements + - support for android.graphics.Color. + - added support for ElapsedTime. + - improved look and legibility of blocks. + - support for compass sensor. + - support for ultrasonic sensor. + - support for IrSeeker. + - support for LED. + - support for color sensor. + - support for CRServo + - prompt user to configure robot before using programming mode. +* Provides ability to disable audio cues. +* various bug fixes and improvements. + +## Version 2.00 (released on 16.08.19) +* This is the new release for the upcoming 2016-2017 FIRST Tech Challenge Season. +* Channel change is enabled in the FTC Robot Controller app for Moto G 2nd and 3rd Gen phones. +* Users can now use annotations to register/disable their OpModes. +* Changes in the Android SDK, JDK and build tool requirements (minsdk=19, java 1.7, build tools 23.0.3). +* Standardized units in analog input. +* Cleaned up code for existing analog sensor classes. +* setChannelMode and getChannelMode were REMOVED from the DcMotorController class. This is important - we no longer set the motor modes through the motor controller. +* setMode and getMode were added to the DcMotor class. +* ContinuousRotationServo class has been added to the FTC SDK. +* Range.clip() method has been overloaded so it can support this operation for int, short and byte integers. +* Some changes have been made (new methods added) on how a user can access items from the hardware map. +* Users can now set the zero power behavior for a DC motor so that the motor will brake or float when power is zero. +* Prototype Blockly Programming Mode has been added to FTC Robot Controller. Users can place the Robot Controller into this mode, and then use a device (such as a laptop) that has a Javascript enabled browser to write Blockly-based OpModes directly onto the Robot Controller. +* Users can now configure the robot remotely through the FTC Driver Station app. +* Android Studio project supports Android Studio 2.1.x and compile SDK Version 23 (Marshmallow). +* Vuforia Computer Vision SDK integrated into FTC SDK. Users can use sample vision targets to get localization information on a standard FTC field. +* Project structure has been reorganized so that there is now a TeamCode package that users can use to place their local/custom OpModes into this package. +* Inspection function has been integrated into the FTC Robot Controller and Driver Station Apps (Thanks Team HazMat… 9277 & 10650!). +* Audio cues have been incorporated into FTC SDK. +* Swap mechanism added to FTC Robot Controller configuration activity. For example, if you have two motor controllers on a robot, and you misidentified them in your configuration file, you can use the Swap button to swap the devices within the configuration file (so you do not have to manually re-enter in the configuration info for the two devices). +* Fix mechanism added to all user to replace an electronic module easily. For example, suppose a servo controller dies on your robot. You replace the broken module with a new module, which has a different serial number from the original servo controller. You can use the Fix button to automatically reconfigure your configuration file to use the serial number of the new module. +* Improvements made to fix resiliency and responsiveness of the system. +* For LinearOpMode the user now must for a telemetry.update() to update the telemetry data on the driver station. This update() mechanism ensures that the driver station gets the updated data properly and at the same time. +* The Auto Configure function of the Robot Controller is now template based. If there is a commonly used robot configuration, a template can be created so that the Auto Configure mechanism can be used to quickly configure a robot of this type. +* The logic to detect a runaway OpMode (both in the LinearOpMode and OpMode types) and to abort the run, then auto recover has been improved/implemented. +* Fix has been incorporated so that Logitech F310 gamepad mappings will be correct for Marshmallow users. + +## Release 16.07.08 + +* For the ftc_app project, the gradle files have been modified to support Android Studio 2.1.x. + +## Release 16.03.30 + +* For the MIT App Inventor, the design blocks have new icons that better represent the function of each design component. +* Some changes were made to the shutdown logic to ensure the robust shutdown of some of our USB services. +* A change was made to LinearOpMode so as to allow a given instance to be executed more than once, which is required for the App Inventor. +* Javadoc improved/updated. + +## Release 16.03.09 + +* Changes made to make the FTC SDK synchronous (significant change!) + - waitOneFullHardwareCycle() and waitForNextHardwareCycle() are no longer needed and have been deprecated. + - runOpMode() (for a LinearOpMode) is now decoupled from the system's hardware read/write thread. + - loop() (for an OpMode) is now decoupled from the system's hardware read/write thread. + - Methods are synchronous. + - For example, if you call setMode(DcMotorController.RunMode.RESET_ENCODERS) for a motor, the encoder is guaranteed to be reset when the method call is complete. + - For legacy module (NXT compatible), user no longer has to toggle between read and write modes when reading from or writing to a legacy device. +* Changes made to enhance reliability/robustness during ESD event. +* Changes made to make code thread safe. +* Debug keystore added so that user-generated robot controller APKs will all use the same signed key (to avoid conflicts if a team has multiple developer laptops for example). +* Firmware version information for Modern Robotics modules are now logged. +* Changes made to improve USB comm reliability and robustness. +* Added support for voltage indicator for legacy (NXT-compatible) motor controllers. +* Changes made to provide auto stop capabilities for OpModes. + - A LinearOpMode class will stop when the statements in runOpMode() are complete. User does not have to push the stop button on the driver station. + - If an OpMode is stopped by the driver station, but there is a run away/uninterruptible thread persisting, the app will log an error message then force itself to crash to stop the runaway thread. +* Driver Station UI modified to display lowest measured voltage below current voltage (12V battery). +* Driver Station UI modified to have color background for current voltage (green=good, yellow=caution, red=danger, extremely low voltage). +* javadoc improved (edits and additional classes). +* Added app build time to About activity for driver station and robot controller apps. +* Display local IP addresses on Driver Station About activity. +* Added I2cDeviceSynchImpl. +* Added I2cDeviceSync interface. +* Added seconds() and milliseconds() to ElapsedTime for clarity. +* Added getCallbackCount() to I2cDevice. +* Added missing clearI2cPortActionFlag. +* Added code to create log messages while waiting for LinearOpMode shutdown. +* Fix so Wi-Fi Direct Config activity will no longer launch multiple times. +* Added the ability to specify an alternate i2c address in software for the Modern Robotics gyro. + +## Release 16.02.09 + +* Improved battery checker feature so that voltage values get refreshed regularly (every 250 msec) on Driver Station (DS) user interface. +* Improved software so that Robot Controller (RC) is much more resilient and “self-healing” to USB disconnects: + - If user attempts to start/restart RC with one or more module missing, it will display a warning but still start up. + - When running an OpMode, if one or more modules gets disconnected, the RC & DS will display warnings,and robot will keep on working in spite of the missing module(s). + - If a disconnected module gets physically reconnected the RC will auto detect the module and the user will regain control of the recently connected module. + - Warning messages are more helpful (identifies the type of module that’s missing plus its USB serial number). +* Code changes to fix the null gamepad reference when users try to reference the gamepads in the init() portion of their OpMode. +* NXT light sensor output is now properly scaled. Note that teams might have to readjust their light threshold values in their OpModes. +* On DS user interface, gamepad icon for a driver will disappear if the matching gamepad is disconnected or if that gamepad gets designated as a different driver. +* Robot Protocol (ROBOCOL) version number info is displayed in About screen on RC and DS apps. +* Incorporated a display filter on pairing screen to filter out devices that don’t use the “-“ format. This filter can be turned off to show all Wi-Fi Direct devices. +* Updated text in License file. +* Fixed formatting error in OpticalDistanceSensor.toString(). +* Fixed issue on with a blank (“”) device name that would disrupt Wi-Fi Direct Pairing. +* Made a change so that the Wi-Fi info and battery info can be displayed more quickly on the DS upon connecting to RC. +* Improved javadoc generation. +* Modified code to make it easier to support language localization in the future. + +## Release 16.01.04 + +* Updated compileSdkVersion for apps +* Prevent Wi-Fi from entering power saving mode +* removed unused import from driver station +* Corrrected "Dead zone" joystick code. +* LED.getDeviceName and .getConnectionInfo() return null +* apps check for ROBOCOL_VERSION mismatch +* Fix for Telemetry also has off-by-one errors in its data string sizing / short size limitations error +* User telemetry output is sorted. +* added formatting variants to DbgLog and RobotLog APIs +* code modified to allow for a long list of OpMode names. +* changes to improve thread safety of RobocolDatagramSocket +* Fix for "missing hardware leaves robot controller disconnected from driver station" error +* fix for "fast tapping of Init/Start causes problems" (toast is now only instantiated on UI thread). +* added some log statements for thread life cycle. +* moved gamepad reset logic inside of initActiveOpMode() for robustness +* changes made to mitigate risk of race conditions on public methods. +* changes to try and flag when Wi-Fi Direct name contains non-printable characters. +* fix to correct race condition between .run() and .close() in ReadWriteRunnableStandard. +* updated FTDI driver +* made ReadWriteRunnableStanard interface public. +* fixed off-by-one errors in Command constructor +* moved specific hardware implmentations into their own package. +* moved specific gamepad implemnatations to the hardware library. +* changed LICENSE file to new BSD version. +* fixed race condition when shutting down Modern Robotics USB devices. +* methods in the ColorSensor classes have been synchronized. +* corrected isBusy() status to reflect end of motion. +* corrected "back" button keycode. +* the notSupported() method of the GyroSensor class was changed to protected (it should not be public). + +## Release 15.11.04.001 + +* Added Support for Modern Robotics Gyro. +- The GyroSensor class now supports the MR Gyro Sensor. +- Users can access heading data (about Z axis) +- Users can also access raw gyro data (X, Y, & Z axes). +- Example MRGyroTest.java OpMode included. +* Improved error messages +- More descriptive error messages for exceptions in user code. +* Updated DcMotor API +* Enable read mode on new address in setI2cAddress +* Fix so that driver station app resets the gamepads when switching OpModes. +* USB-related code changes to make USB comm more responsive and to display more explicit error messages. +- Fix so that USB will recover properly if the USB bus returns garbage data. +- Fix USB initializtion race condition. +- Better error reporting during FTDI open. +- More explicit messages during USB failures. +- Fixed bug so that USB device is closed if event loop teardown method was not called. +* Fixed timer UI issue +* Fixed duplicate name UI bug (Legacy Module configuration). +* Fixed race condition in EventLoopManager. +* Fix to keep references stable when updating gamepad. +* For legacy Matrix motor/servo controllers removed necessity of appending "Motor" and "Servo" to controller names. +* Updated HT color sensor driver to use constants from ModernRoboticsUsbLegacyModule class. +* Updated MR color sensor driver to use constants from ModernRoboticsUsbDeviceInterfaceModule class. +* Correctly handle I2C Address change in all color sensors +* Updated/cleaned up OpModes. +- Updated comments in LinearI2cAddressChange.java example OpMode. +- Replaced the calls to "setChannelMode" with "setMode" (to match the new of the DcMotor method). +- Removed K9AutoTime.java OpMode. +- Added MRGyroTest.java OpMode (demonstrates how to use MR Gyro Sensor). +- Added MRRGBExample.java OpMode (demonstrates how to use MR Color Sensor). +- Added HTRGBExample.java OpMode (demonstrates how to use HT legacy color sensor). +- Added MatrixControllerDemo.java (demonstrates how to use legacy Matrix controller). +* Updated javadoc documentation. +* Updated release .apk files for Robot Controller and Driver Station apps. + +## Release 15.10.06.002 + +* Added support for Legacy Matrix 9.6V motor/servo controller. +* Cleaned up build.gradle file. +* Minor UI and bug fixes for driver station and robot controller apps. +* Throws error if Ultrasonic sensor (NXT) is not configured for legacy module port 4 or 5. + + +## Release 15.08.03.001 + +* New user interfaces for FTC Driver Station and FTC Robot Controller apps. +* An init() method is added to the OpMode class. + - For this release, init() is triggered right before the start() method. + - Eventually, the init() method will be triggered when the user presses an "INIT" button on driver station. + - The init() and loop() methods are now required (i.e., need to be overridden in the user's OpMode). + - The start() and stop() methods are optional. +* A new LinearOpMode class is introduced. + - Teams can use the LinearOpMode mode to create a linear (not event driven) program model. + - Teams can use blocking statements like Thread.sleep() within a linear OpMode. +* The API for the Legacy Module and Core Device Interface Module have been updated. + - Support for encoders with the Legacy Module is now working. +* The hardware loop has been updated for better performance. \ No newline at end of file diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 9d26ae5..0176858 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -6,15 +6,15 @@ repositories { } dependencies { - implementation 'org.firstinspires.ftc:Inspection:9.1.0' - implementation 'org.firstinspires.ftc:Blocks:9.1.0' - implementation 'org.firstinspires.ftc:Tfod:9.1.0' - implementation 'org.firstinspires.ftc:RobotCore:9.1.0' - implementation 'org.firstinspires.ftc:RobotServer:9.1.0' - implementation 'org.firstinspires.ftc:OnBotJava:9.1.0' - implementation 'org.firstinspires.ftc:Hardware:9.1.0' - implementation 'org.firstinspires.ftc:FtcCommon:9.1.0' - implementation 'org.firstinspires.ftc:Vision:9.1.0' + implementation 'org.firstinspires.ftc:Inspection:9.2.0' + implementation 'org.firstinspires.ftc:Blocks:9.2.0' + implementation 'org.firstinspires.ftc:Tfod:9.2.0' + implementation 'org.firstinspires.ftc:RobotCore:9.2.0' + implementation 'org.firstinspires.ftc:RobotServer:9.2.0' + implementation 'org.firstinspires.ftc:OnBotJava:9.2.0' + implementation 'org.firstinspires.ftc:Hardware:9.2.0' + implementation 'org.firstinspires.ftc:FtcCommon:9.2.0' + implementation 'org.firstinspires.ftc:Vision:9.2.0' implementation 'org.firstinspires.ftc:gameAssets-CenterStage:1.0.0' implementation 'org.tensorflow:tensorflow-lite-task-vision:0.4.3' runtimeOnly 'org.tensorflow:tensorflow-lite:2.12.0' diff --git a/gradle.properties b/gradle.properties index 104e43c..7a370c5 100644 --- a/gradle.properties +++ b/gradle.properties @@ -3,8 +3,8 @@ # https://developer.android.com/topic/libraries/support-library/androidx-rn android.useAndroidX=true -# Automatically convert third-party libraries to use AndroidX -android.enableJetifier=true +# We no longer need to auto-convert third-party libraries to use AndroidX, which slowed down the build +android.enableJetifier=false # Allow Gradle to use up to 1 GB of RAM org.gradle.jvmargs=-Xmx1024M From 7b8adf25bfd574995201e94abca886edfbe9625e Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Sat, 20 Jul 2024 00:56:24 -0400 Subject: [PATCH 22/94] work mostly done on kalman filter --- .../pedroPathing/follower/Follower.java | 35 +++++++------------ .../tuning/FollowerConstants.java | 12 +++---- 2 files changed, 16 insertions(+), 31 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index 03eee3c..383b7aa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -11,8 +11,6 @@ import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstan import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.smallHeadingPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.smallTranslationalPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFSwitch; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.tunedDriveErrorKalmanGain; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.tunedDriveErrorVariance; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -126,10 +124,9 @@ public class Follower { private FilteredPIDFController largeDrivePIDF = new FilteredPIDFController(FollowerConstants.largeDrivePIDFCoefficients); private KalmanFilter driveKalmanFilter = new KalmanFilter(FollowerConstants.driveKalmanFilterParameters); - private long[] driveErrorTimes; private double[] driveErrors; - private double rawDriverError; - private double previousRawDriverError; + private double rawDriveError; + private double previousRawDriveError; public static boolean drawOnDashboard = true; public static boolean useTranslational = true; @@ -589,14 +586,13 @@ public class Follower { correctiveVector = new Vector(); driveError = 0; headingError = 0; - rawDriverError = 0; - previousRawDriverError = 0; - driveErrors = new double[3]; - driveErrorTimes = new long[3]; - for (int i = 0; i < driveErrorTimes.length; i++) { - driveErrorTimes[i] = System.currentTimeMillis(); + rawDriveError = 0; + previousRawDriveError = 0; + driveErrors = new double[2]; + for (int i = 0; i < driveErrors.length; i++) { + driveErrors[i] = 0; } - driveKalmanFilter.reset(0, tunedDriveErrorVariance, tunedDriveErrorKalmanGain); + driveKalmanFilter.reset(); for (int i = 0; i < motors.size(); i++) { motors.get(i).setPower(0); @@ -682,24 +678,17 @@ public class Follower { Vector lateralVelocityError = new Vector(lateralVelocityGoal - lateralVelocityZeroPowerDecay - lateralVelocity, lateralHeadingVector.getTheta()); Vector velocityErrorVector = MathFunctions.addVectors(forwardVelocityError, lateralVelocityError); - previousRawDriverError = rawDriverError; - rawDriverError = velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector())); + previousRawDriveError = rawDriveError; + rawDriveError = velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector())); - double previousErrorVelocity = (driveErrors[1] - driveErrors[0]) / ((driveErrorTimes[1] - driveErrorTimes[0]) / 1000.0); - double errorVelocity = (driveErrors[2] - driveErrors[1]) / ((driveErrorTimes[2] - driveErrorTimes[1]) / 1000.0); - double errorAcceleration = ((errorVelocity - previousErrorVelocity) / ((((driveErrorTimes[2] - driveErrorTimes[1]) / 1000.0) / 2.0) - (((driveErrorTimes[1] - driveErrorTimes[0]) / 1000.0) / 2.0))); - double time = (((driveErrorTimes[2] - driveErrorTimes[1]) / 1000.0) + ((driveErrorTimes[1] - driveErrorTimes[0]) / 1000.0)) / 2.0; + double projection = 2 * driveErrors[2] - driveErrors[1]; - double projection = errorVelocity * time + 0.5 * errorAcceleration * Math.pow(time, 2); - - driveKalmanFilter.update(rawDriverError - previousRawDriverError, projection); + driveKalmanFilter.update(rawDriveError - previousRawDriveError, projection); for (int i = 0; i < driveErrors.length - 1; i++) { driveErrors[i] = driveErrors[i + 1]; - driveErrorTimes[i] = driveErrorTimes[i + 1]; } driveErrors[2] = driveKalmanFilter.getState(); - driveErrorTimes[2] = System.currentTimeMillis(); return driveKalmanFilter.getState(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java index 60dcd9a..4569d77 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java @@ -94,7 +94,7 @@ public class FollowerConstants { 0.025, 0, 0.00001, - 0.01, + 0.6, 0); // Feed forward constant added on to the large drive PIDF @@ -108,7 +108,7 @@ public class FollowerConstants { 0.02, 0, 0.000005, - 0.01, + 0.6, 0); // Feed forward constant added on to the small drive PIDF @@ -116,12 +116,8 @@ public class FollowerConstants { // Kalman filter parameters for the drive error Kalman filter public static KalmanFilterParameters driveKalmanFilterParameters = new KalmanFilterParameters( - 0.4, - 0.1); - - // These are the empirically tuned parameters for the drive error Kalman filter so it works faster. - public static double tunedDriveErrorVariance = 1; - public static double tunedDriveErrorKalmanGain = 1; + 6, + 1); // Mass of robot in kilograms public static double mass = 10.65942; From 09ae88b27eb4b910aeaaea22c11c77937e7019f8 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Sat, 20 Jul 2024 23:03:05 -0400 Subject: [PATCH 23/94] refactored the localizer files and added an OTOS localizer --- .../localization/PoseUpdater.java | 1 + .../DriveEncoderLocalizer.java | 6 +- .../localizers/OTOSLocalizer.java | 182 ++++++++++++++++++ .../RRToPedroThreeWheelLocalizer.java | 0 .../{ => localizers}/RoadRunnerEncoder.java | 0 .../RoadRunnerThreeWheelLocalizer.java | 0 .../ThreeWheelIMULocalizer.Java | 8 +- .../{ => localizers}/ThreeWheelLocalizer.java | 6 +- .../{ => localizers}/TwoWheelLocalizer.java | 6 +- 9 files changed, 203 insertions(+), 6 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/{ => localizers}/DriveEncoderLocalizer.java (96%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/{ => localizers}/RRToPedroThreeWheelLocalizer.java (100%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/{ => localizers}/RoadRunnerEncoder.java (100%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/{ => localizers}/RoadRunnerThreeWheelLocalizer.java (100%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/{ => localizers}/ThreeWheelIMULocalizer.Java (97%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/{ => localizers}/ThreeWheelLocalizer.java (97%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/{ => localizers}/TwoWheelLocalizer.java (97%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java index d667f40..415b5a0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java @@ -5,6 +5,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.ThreeWheelLocalizer; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java similarity index 96% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java index 44e04a5..0d06142 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java @@ -1,9 +1,13 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization; +package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java new file mode 100644 index 0000000..3bca78e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java @@ -0,0 +1,182 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; + +import com.qualcomm.hardware.sparkfun.SparkFunOTOS; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +/** + * This is the OTOSLocalizer class. This class extends the Localizer superclass and is a + * localizer that uses the SparkFun OTOS. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 7/20/2024 + */ +public class OTOSLocalizer extends Localizer { + private HardwareMap hardwareMap; + private Pose startPose; + private SparkFunOTOS otos; + private double previousHeading; + private double totalHeading; + + /** + * This creates a new OTOSLocalizer from a HardwareMap, with a starting Pose at (0,0) + * facing 0 heading. + * + * @param map the HardwareMap + */ + public OTOSLocalizer(HardwareMap map) { + this(map, new Pose()); + } + + /** + * This creates a new OTOSLocalizer from a HardwareMap and a Pose, with the Pose + * specifying the starting pose of the localizer. + * + * @param map the HardwareMap + * @param setStartPose the Pose to start from + */ + public OTOSLocalizer(HardwareMap map, Pose setStartPose) { + hardwareMap = map; + + // TODO: replace this with your OTOS port + otos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos"); + + otos.setLinearUnit(DistanceUnit.INCH); + otos.setAngularUnit(AngleUnit.RADIANS); + + // TODO: replace this with your OTOS offset from the center of the robot + // For the OTOS, left/right is the x axis and forward/backward is the y axis, with right being + // positive x and forward being positive y. 0 radians is facing forward, and clockwise + // rotation is negative rotation. + otos.setOffset(new SparkFunOTOS.Pose2D(0,0,0)); + + otos.calibrateImu(); + otos.resetTracking(); + + setStartPose(setStartPose); + totalHeading = 0; + previousHeading = startPose.getHeading(); + + resetOTOS(); + } + + /** + * This returns the current pose estimate. + * + * @return returns the current pose estimate as a Pose + */ + @Override + public Pose getPose() { + SparkFunOTOS.Pose2D pose = otos.getPosition(); + return MathFunctions.addPoses(startPose, new Pose(pose.x, pose.y, pose.h)); + } + + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Pose + */ + @Override + public Pose getVelocity() { + SparkFunOTOS.Pose2D OTOSVelocity = otos.getVelocity(); + return new Pose(OTOSVelocity.x, OTOSVelocity.y, OTOSVelocity.h); + } + + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Vector + */ + @Override + public Vector getVelocityVector() { + return getVelocity().getVector(); + } + + /** + * This sets the start pose. Changing the start pose should move the robot as if all its + * previous movements were displacing it from its new start pose. + * + * @param setStart the new start pose + */ + @Override + public void setStartPose(Pose setStart) { + startPose = setStart; + } + + /** + * This sets the current pose estimate. Changing this should just change the robot's current + * pose estimate, not anything to do with the start pose. + * + * @param setPose the new current pose estimate + */ + @Override + public void setPose(Pose setPose) { + resetOTOS(); + Pose setOTOSPose = MathFunctions.subtractPoses(setPose, startPose); + otos.setPosition(new SparkFunOTOS.Pose2D(setOTOSPose.getX(), setOTOSPose.getY(), setOTOSPose.getHeading())); + } + + /** + * This updates the total heading of the robot. The OTOS handles all other updates itself. + */ + @Override + public void update() { + totalHeading += MathFunctions.getSmallestAngleDifference(otos.getPosition().h, previousHeading); + previousHeading = otos.getPosition().h; + } + + /** + * This resets the OTOS. + */ + public void resetOTOS() { + otos.resetTracking(); + } + + /** + * This returns how far the robot has turned in radians, in a number not clamped between 0 and + * 2 * pi radians. This is used for some tuning things and nothing actually within the following. + * + * @return returns how far the robot has turned in total, in radians. + */ + public double getTotalHeading() { + return totalHeading; + } + + /** + * This returns the multiplier applied to forward movement measurement to convert from OTOS + * ticks to inches. For the OTOS, this value is the same as the lateral multiplier. + * This is found empirically through a tuner. + * + * @return returns the forward ticks to inches multiplier + */ + public double getForwardMultiplier() { + return otos.getLinearScalar(); + } + + /** + * This returns the multiplier applied to lateral/strafe movement measurement to convert from + * OTOS ticks to inches. For the OTOS, this value is the same as the forward multiplier. + * This is found empirically through a tuner. + * + * @return returns the lateral/strafe ticks to inches multiplier + */ + public double getLateralMultiplier() { + return otos.getLinearScalar(); + } + + /** + * This returns the multiplier applied to turning movement measurement to convert from OTOS ticks + * to radians. This is found empirically through a tuner. + * + * @return returns the turning ticks to radians multiplier + */ + public double getTurningMultiplier() { + return otos.getAngularScalar(); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RRToPedroThreeWheelLocalizer.java similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RRToPedroThreeWheelLocalizer.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerEncoder.java similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerEncoder.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerThreeWheelLocalizer.java similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerThreeWheelLocalizer.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelIMULocalizer.Java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.Java similarity index 97% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelIMULocalizer.Java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.Java index 69a1f03..675f46b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelIMULocalizer.Java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.Java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization; +package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; import com.acmerobotics.dashboard.config.Config; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; @@ -7,8 +7,10 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; -import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java similarity index 97% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java index a7e6548..494d07d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java @@ -1,9 +1,13 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization; +package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java similarity index 97% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java index c4284b9..e93b496 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization; +package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; import com.acmerobotics.dashboard.config.Config; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; @@ -7,6 +7,10 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; From 70cfd3be44497fcbf261ccd638932058df45278a Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Sun, 21 Jul 2024 12:54:47 -0400 Subject: [PATCH 24/94] updated readmes for the drive pid changes and the OTOS localizer --- .../ftc/teamcode/pedroPathing/TUNING.md | 28 ++++-- .../pedroPathing/localization/README.md | 85 ++++++++++++++----- .../localizers/OTOSLocalizer.java | 29 ++++++- 3 files changed, 110 insertions(+), 32 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md index ddd1672..f098c3e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md @@ -10,7 +10,7 @@ One last thing to note is that Pedro Pathing operates in inches and radians. ## Tuning * To start with, we need the mass of the robot in kg. This is used for the centripetal force - correction, and the mass should be put on line `114` in the `FollowerConstants` class under the + correction, and the mass should be put on line `123` in the `FollowerConstants` class under the `tuning` package. * Next, we need to find the preferred mecanum drive vectors. The rollers on mecanum wheels point at a @@ -21,8 +21,8 @@ One last thing to note is that Pedro Pathing operates in inches and radians. Dashboard under the dropdown for each respective class, but higher distances work better. After the distance has finished running, the end velocity will be output to telemetry. The robot may continue to drift a little bit after the robot has finished running the distance, so make sure you have - plenty of room. Once you're done, put the velocity for the `Forward Velocity Tuner` on line `25` in - the `FollowerConstants` class, and the velocity for the `Strafe Velocity Tuner` on line `26` in the + plenty of room. Once you're done, put the velocity for the `Forward Velocity Tuner` on line `27` in + the `FollowerConstants` class, and the velocity for the `Strafe Velocity Tuner` on line `28` in the `FollowerConstants` class. * The last set of automatic tuners you'll need to run are the zero power acceleration tuners. These @@ -36,8 +36,8 @@ One last thing to note is that Pedro Pathing operates in inches and radians. which point it will display the deceleration in telemetry. This robot will need to drift to a stop to properly work, and the higher the velocity the greater the drift distance, so make sure you have enough room. Once you're done, put the zero power acceleration for the - `Forward Zero Power Acceleration Tuner` on line `122` in the `FollowerConstants` class and the zero - power acceleration for the `Lateral Zero Power Acceleration Tuner` on line `127` in the + `Forward Zero Power Acceleration Tuner` on line `130` in the `FollowerConstants` class and the zero + power acceleration for the `Lateral Zero Power Acceleration Tuner` on line `134` in the `FollowerConstants` class. * After this, we will want to tune the translational PIDs. Go to FTC Dashboard and disable all but @@ -68,7 +68,7 @@ One last thing to note is that Pedro Pathing operates in inches and radians. `zeroPowerAccelerationMultiplier`. This determines how fast your robot will decelerate as a factor of how fast your robot will coast to a stop. Honestly, this is up to you. I personally used 4, but what works best for you is most important. Higher numbers will cause a faster brake, but increase - oscillations at the end. Lower numbers will do the opposite. This can be found on line `136` in + oscillations at the end. Lower numbers will do the opposite. This can be found on line `143` in `FollowerConstants`. There are once again two PIDs for the drive vector, but these PIDs are much, much more sensitive than the others. For reference, my P values were in the hundredths and thousandths place values, and my D values were in the hundred thousandths and millionths place @@ -78,12 +78,24 @@ One last thing to note is that Pedro Pathing operates in inches and radians. this, it is very important to try to reduce oscillations. Additionally, I would absolutely not recommend using the I, or integral, part of the PID for this. Using integral in drivetrain PIDs is already not ideal, but it will continuously build up error in this PID, causing major issues when - it gets too strong. So, just don't use it. P and D are enough. + it gets too strong. So, just don't use it. P and D are enough. In the versions of Pedro Pathing + from after late July 2024, there is a Kalman filter on the drive error and the drive PIDs have a + filter as well. These smooth out the drive error and PID response so that there is not as much + oscillation during the braking portion of each path. The Kalman filter is tuned to have 6 for the + model covariance and 1 for the data covariance. These values should work, but if you feel inclined + to tune the Kalman filter yourself, a higher ratio of model covariance to data covariance means that + the filter will rely more on its previous output rather than the data, and the opposite ratio will + mean that the filter will rely more so on the data input (the raw drive error) rather than the model. + The filtered PIDs function like normal PIDs, except the derivative term is a weighted average of the + current derivative and the previous derivative. Currently, the weighting, or time constant for the + drive filtered PIDs is 0.6, so the derivative output is 0.6 times the previous derivative plus 0.4 + times the current derivative. Feel free to mess around with these values and find what works best + for your robot! * Finally, we will want to tune the centripetal force correction. This is a pretty simple tune. Open up FTC Dashboard and enable everything under the `Follower` tab. Then, run `CurvedBackAndForth` and turn off its timer. If you notice the robot is correcting towards the inside of the curve - as/after running a path, then increase `centripetalScaling`, which can be found on line `117` of + as/after running a path, then increase `centripetalScaling`, which can be found on line `126` of `FollowerConstants`. If the robot is correcting towards the outside of the curve, then decrease `centripetalScaling`. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/README.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/README.md index 9e4299a..0a1f6d2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/README.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/README.md @@ -3,7 +3,8 @@ This is the localization system developed for the Pedro Pathing path follower. T the pose exponential method of localization. It's basically a way of turning movements from the robot's coordinate frame to the global coordinate frame. If you're interested in reading more about it, then check out pages 177 - 183 of [Controls Engineering in the FIRST Robotics Competition](https://file.tavsys.net/control/controls-engineering-in-frc.pdf) -by Tyler Veness. +by Tyler Veness. However, the OTOS localizer uses its own onboard system for calculating localization, +which I do not know about. ## Setting Your Localizer Go to line `69` in the `PoseUpdater` class, and replace the `new ThreeWheelLocalizer(hardwareMap)` @@ -13,12 +14,13 @@ with the localizer that applies to you: * If you're using three wheel odometry, put `new ThreeWheelLocalizer(hardwareMap)`, so basically don't change it from the default * If you're using three wheel odometry with the IMU, put `new ThreeWheelIMULocalizer(hardwareMap)` +* If you're using OTOS, put `new OTOSLocalizer(hardwareMap)` ## Tuning To start, you'll want to select your localizer of choice. Below, I'll have instructions for the drive -encoder localizer, two tracking wheel localizer, the three tracking wheel localizer, and the three -wheel with IMU localizer offered in Pedro Pathing. Scroll down to the section that applies to you -and follow the directions there. +encoder localizer, two tracking wheel localizer, the three tracking wheel localizer, the three +wheel with IMU localizer, and the OTOS localizer offered in Pedro Pathing. Scroll down to the section +that applies to you and follow the directions there. # Drive Encoders * First, you'll need all of your drive motors to have encoders attached. @@ -39,19 +41,19 @@ to. The names of the variables is where on the robot the corresponding motor sho this multiplier, then replace `TURN_TICKS_TO_RADIANS` in the localizer with your multiplier. Make sure you replace the number, not add on or multiply it to the previous number. The tuner takes into account your current multiplier. -* Next, go on to `Forward Localizer Tuner`. You'll want to position a rule alongside your robot. +* Next, go on to `Forward Localizer Tuner`. You'll want to position a ruler alongside your robot. By default, you'll want to push the robot 30 inches forward. Once you've pushed that far, or whatever you set that value to, then the forward multiplier will be shown as the second number shown. The first number is how far the robot thinks you've gone, and the second number is the multiplier you - need to have to scale your current readings to your goal of 30 inches, or the custom set angle. + need to have to scale your current readings to your goal of 30 inches, or the custom set distance. Feel free to run a few more tests and average the results. Once you have this multiplier, then replace `FORWARD_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, not add on or multiply it to the previous number. The tuner takes into account your current multiplier. -* Finally, go to `Lateral Localizer Tuner`. You'll want to position a rule alongside your robot. +* Finally, go to `Lateral Localizer Tuner`. You'll want to position a ruler alongside your robot. By default, you'll want to push the robot 30 inches to the right. Once you've pushed that far, or whatever you set that value to, then the lateral multiplier will be shown as the second number shown. The first number is how far the robot thinks you've gone, and the second number is the multiplier you - need to have to scale your current readings to your goal of 30 inches, or the custom set angle. + need to have to scale your current readings to your goal of 30 inches, or the custom set distance. Feel free to run a few more tests and average the results. Once you have this multiplier, then replace `STRAFE_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, not add on or multiply it to the previous number. The tuner takes into account your current multiplier. @@ -81,19 +83,19 @@ to. The names of the variables is where on the robot the corresponding motor sho world. * You actually won't need the turning tuner for this one, since the IMU in the Control Hub will take care of the heading readings. -* First, start with the `Forward Localizer Tuner`. You'll want to position a rule alongside your robot. +* First, start with the `Forward Localizer Tuner`. You'll want to position a ruler alongside your robot. By default, you'll want to push the robot 30 inches forward. Once you've pushed that far, or whatever you set that value to, then the forward multiplier will be shown as the second number shown. The first number is how far the robot thinks you've gone, and the second number is the multiplier you - need to have to scale your current readings to your goal of 30 inches, or the custom set angle. + need to have to scale your current readings to your goal of 30 inches, or the custom set distance. Feel free to run a few more tests and average the results. Once you have this multiplier, then replace `FORWARD_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, not add on or multiply it to the previous number. The tuner takes into account your current multiplier. -* Finally, go to `Lateral Localizer Tuner`. You'll want to position a rule alongside your robot. +* Finally, go to `Lateral Localizer Tuner`. You'll want to position a ruler alongside your robot. By default, you'll want to push the robot 30 inches to the right. Once you've pushed that far, or whatever you set that value to, then the lateral multiplier will be shown as the second number shown. The first number is how far the robot thinks you've gone, and the second number is the multiplier you - need to have to scale your current readings to your goal of 30 inches, or the custom set angle. + need to have to scale your current readings to your goal of 30 inches, or the custom set distance. Feel free to run a few more tests and average the results. Once you have this multiplier, then replace `STRAFE_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, not add on or multiply it to the previous number. The tuner takes into account your current multiplier. @@ -125,19 +127,19 @@ to. The names of the variables is where on the robot the corresponding motor sho this multiplier, then replace `TURN_TICKS_TO_RADIANS` in the localizer with your multiplier. Make sure you replace the number, not add on or multiply it to the previous number. The tuner takes into account your current multiplier. -* Next, go on to `Forward Localizer Tuner`. You'll want to position a rule alongside your robot. +* Next, go on to `Forward Localizer Tuner`. You'll want to position a ruler alongside your robot. By default, you'll want to push the robot 30 inches forward. Once you've pushed that far, or whatever you set that value to, then the forward multiplier will be shown as the second number shown. The first number is how far the robot thinks you've gone, and the second number is the multiplier you - need to have to scale your current readings to your goal of 30 inches, or the custom set angle. + need to have to scale your current readings to your goal of 30 inches, or the custom set distance. Feel free to run a few more tests and average the results. Once you have this multiplier, then replace `FORWARD_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, not add on or multiply it to the previous number. The tuner takes into account your current multiplier. -* Finally, go to `Lateral Localizer Tuner`. You'll want to position a rule alongside your robot. +* Finally, go to `Lateral Localizer Tuner`. You'll want to position a ruler alongside your robot. By default, you'll want to push the robot 30 inches to the right. Once you've pushed that far, or whatever you set that value to, then the lateral multiplier will be shown as the second number shown. The first number is how far the robot thinks you've gone, and the second number is the multiplier you - need to have to scale your current readings to your goal of 30 inches, or the custom set angle. + need to have to scale your current readings to your goal of 30 inches, or the custom set distance. Feel free to run a few more tests and average the results. Once you have this multiplier, then replace `STRAFE_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, not add on or multiply it to the previous number. The tuner takes into account your current multiplier. @@ -174,19 +176,19 @@ to. The names of the variables is where on the robot the corresponding motor sho this multiplier, then replace `TURN_TICKS_TO_RADIANS` in the localizer with your multiplier. Make sure you replace the number, not add on or multiply it to the previous number. The tuner takes into account your current multiplier. -* Next, go on to `Forward Localizer Tuner`. You should re-enable `useIMU` at this time. You'll want to position a rule alongside your robot. +* Next, go on to `Forward Localizer Tuner`. You should re-enable `useIMU` at this time. You'll want to position a ruler alongside your robot. By default, you'll want to push the robot 30 inches forward. Once you've pushed that far, or whatever you set that value to, then the forward multiplier will be shown as the second number shown. The first number is how far the robot thinks you've gone, and the second number is the multiplier you - need to have to scale your current readings to your goal of 30 inches, or the custom set angle. + need to have to scale your current readings to your goal of 30 inches, or the custom set distance. Feel free to run a few more tests and average the results. Once you have this multiplier, then replace `FORWARD_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, not add on or multiply it to the previous number. The tuner takes into account your current multiplier. -* Finally, go to `Lateral Localizer Tuner`. `useIMU` should be enabled for this step. You'll want to position a rule alongside your robot. +* Finally, go to `Lateral Localizer Tuner`. `useIMU` should be enabled for this step. You'll want to position a ruler alongside your robot. By default, you'll want to push the robot 30 inches to the right. Once you've pushed that far, or whatever you set that value to, then the lateral multiplier will be shown as the second number shown. The first number is how far the robot thinks you've gone, and the second number is the multiplier you - need to have to scale your current readings to your goal of 30 inches, or the custom set angle. + need to have to scale your current readings to your goal of 30 inches, or the custom set distance. Feel free to run a few more tests and average the results. Once you have this multiplier, then replace `STRAFE_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, not add on or multiply it to the previous number. The tuner takes into account your current multiplier. @@ -197,6 +199,49 @@ to. The names of the variables is where on the robot the corresponding motor sho robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer! +# OTOS Localizer +* First, you'll need the OTOS connected to an I2C port on a hub. Make sure the film on the sensor is removed. +* Then, go to `OTOSLocalizer.java`. First, in the constructor, go to where it tells you to replace + the current statement with your OTOS port in the constructor. Replace the `deviceName` parameter + with the name of the port that the OTOS is connected to. +* Next, enter in the position of your OTOS relative to the center of the wheels of the robot. The + positions are in inches, so convert measurements accordingly. Use the comment above the class + declaration as well as to help you with the coordinates. +* First, start with the `Turn Localizer Tuner`. You'll want to position your robot to be facing + in a direction you can easily find again, like lining up an edge of the robot against a field tile edge. + By default, you should spin the robot for one rotation going counterclockwise. Once you've spun + exactly that one rotation, or whatever you set that value to, then the angular scalar will be shown + as the second number shown. The first number is how far the robot thinks you've spun, and the second + number is the scalar you need to have to scale your current readings to your goal of one rotation, + or the custom set angle. Feel free to run a few more tests and average the results. Once you have + this scalar, then replace the angular scalar on line `78` in the localizer with your scalar. + Make sure you replace the number, not add on or multiply it to the previous number. The tuner takes into + account your current angular scalar. +* For this next step, since OTOS only has one linear scalar, you can run either the forward or lateral + localizer tuner and the result should be the same. So, you choose which one you want to run. +* Option 1: go on to `Forward Localizer Tuner`. You'll want to position a ruler alongside your robot. + By default, you'll want to push the robot 30 inches forward. Once you've pushed that far, or whatever + you set that value to, then the linear scalar will be shown as the second number shown. The + first number is how far the robot thinks you've gone, and the second number is the scalar you + need to have to scale your current readings to your goal of 30 inches, or the custom set distance. + Feel free to run a few more tests and average the results. Once you have this scalar, then + replace the linear scalar on line `77` in the localizer with your scalar. Make sure you replace the number, + not add on or multiply it to the previous number. The tuner takes into account your current scalar. +* Option 2: go to `Lateral Localizer Tuner`. You'll want to position a ruler alongside your robot. + By default, you'll want to push the robot 30 inches to the right. Once you've pushed that far, or whatever + you set that value to, then the linear scalar will be shown as the second number shown. The + first number is how far the robot thinks you've gone, and the second number is the scalar you + need to have to scale your current readings to your goal of 30 inches, or the custom set distance. + Feel free to run a few more tests and average the results. Once you have this scalar, then + replace the linear scalar on line `77` in the localizer with your scalar. Make sure you replace the number, + not add on or multiply it to the previous number. The tuner takes into account your current scalar. +* Once you're done with all this, your localizer should be tuned. To test it out, you can go to + `Localization Test` and push around or drive around your robot. Go to [FTC Dashboard](http://192.168.43.1:8080/dash) + and on the top right, switch the drop down from the default view to the field view. Then, on the bottom + left corner, you should see a field and the robot being drawn on the field. You can then move your + robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll + want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer! + ## Using Road Runner's Localizer Of course, many teams have experience using Road Runner in the past and so have localizers from Road Runner that are tuned. There is an adapter for the Road Runner three wheel localizer to the Pedro diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java index 3bca78e..13f63a9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java @@ -12,7 +12,24 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; /** * This is the OTOSLocalizer class. This class extends the Localizer superclass and is a - * localizer that uses the SparkFun OTOS. + * localizer that uses the SparkFun OTOS. The diagram below, which is taken from + * Road Runner, shows a typical set up. + * + * The view is from the bottom of the robot looking upwards. + * + * left on robot is y pos + * + * front on robot is x pos + * + * /--------------\ + * | ____ | + * | ---- | + * | || || | + * | || || | left (y pos) + * | | + * | | + * \--------------/ + * front (x pos) * * @author Anyi Lin - 10158 Scott's Bots * @version 1.0, 7/20/2024 @@ -51,10 +68,14 @@ public class OTOSLocalizer extends Localizer { otos.setAngularUnit(AngleUnit.RADIANS); // TODO: replace this with your OTOS offset from the center of the robot - // For the OTOS, left/right is the x axis and forward/backward is the y axis, with right being - // positive x and forward being positive y. 0 radians is facing forward, and clockwise + // For the OTOS, left/right is the y axis and forward/backward is the x axis, with left being + // positive y and forward being positive x. PI/2 radians is facing forward, and clockwise // rotation is negative rotation. - otos.setOffset(new SparkFunOTOS.Pose2D(0,0,0)); + otos.setOffset(new SparkFunOTOS.Pose2D(0,0,Math.PI / 2)); + + // TODO: replace these with your tuned multipliers + otos.setLinearScalar(1.0); + otos.setAngularScalar(1.0); otos.calibrateImu(); otos.resetTracking(); From a823aa7af79beda2c359a0c2184b55f942802346 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Sun, 21 Jul 2024 22:52:25 -0400 Subject: [PATCH 25/94] small misc changes --- .../pedroPathing/{localization/README.md => LOCALIZATION.md} | 2 +- .../ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/{localization/README.md => LOCALIZATION.md} (99%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/README.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/LOCALIZATION.md similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/README.md rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/LOCALIZATION.md index 0a1f6d2..28ef56d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/README.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/LOCALIZATION.md @@ -7,7 +7,7 @@ by Tyler Veness. However, the OTOS localizer uses its own onboard system for cal which I do not know about. ## Setting Your Localizer -Go to line `69` in the `PoseUpdater` class, and replace the `new ThreeWheelLocalizer(hardwareMap)` +Go to line `70` in the `PoseUpdater` class, and replace the `new ThreeWheelLocalizer(hardwareMap)` with the localizer that applies to you: * If you're using drive encoders, put `new DriveEncoderLocalizer(hardwareMap)` * If you're using two wheel odometry, put `new TwoWheelLocalizer(hardwareMap)` diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java index 79a804f..1f0c2fc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java @@ -59,6 +59,8 @@ public class TeleOpEnhancements extends OpMode { public void loop() { driveVector.setOrthogonalComponents(-gamepad1.left_stick_y, -gamepad1.left_stick_x); driveVector.setMagnitude(MathFunctions.clamp(driveVector.getMagnitude(), 0, 1)); + + // TODO: if you want to make this field centric, then just remove this line driveVector.rotateVector(follower.getPose().getHeading()); headingVector.setComponents(-gamepad1.left_stick_x, follower.getPose().getHeading()); From bee9f025777ecd523572184ed5e8178decd91e83 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Fri, 26 Jul 2024 22:12:02 -0400 Subject: [PATCH 26/94] teleop enhancements fix --- .../ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java index 1f0c2fc..dfcd231 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java @@ -63,7 +63,7 @@ public class TeleOpEnhancements extends OpMode { // TODO: if you want to make this field centric, then just remove this line driveVector.rotateVector(follower.getPose().getHeading()); - headingVector.setComponents(-gamepad1.left_stick_x, follower.getPose().getHeading()); + headingVector.setComponents(-gamepad1.right_stick_x, follower.getPose().getHeading()); follower.setMovementVectors(follower.getCentripetalForceCorrection(), headingVector, driveVector); follower.update(); From b7ddb3465accab94086af149bc09efc323ac7b54 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Mon, 29 Jul 2024 22:15:07 -0400 Subject: [PATCH 27/94] teleop enhancements fix --- .../examples/TeleOpEnhancements.java | 20 +--- .../pedroPathing/follower/Follower.java | 100 ++++++++++-------- 2 files changed, 60 insertions(+), 60 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java index dfcd231..e5872e5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java @@ -6,8 +6,6 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; /** * This is the TeleOpEnhancements OpMode. It is an example usage of the TeleOp enhancements that @@ -27,15 +25,12 @@ public class TeleOpEnhancements extends OpMode { private DcMotorEx rightFront; private DcMotorEx rightRear; - private Vector driveVector; - private Vector headingVector; - /** * This initializes the drive motors as well as the Follower and motion Vectors. */ @Override public void init() { - follower = new Follower(hardwareMap, false); + follower = new Follower(hardwareMap); leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); @@ -47,8 +42,7 @@ public class TeleOpEnhancements extends OpMode { rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - driveVector = new Vector(); - headingVector = new Vector(); + follower.startTeleopDrive(); } /** @@ -57,15 +51,7 @@ public class TeleOpEnhancements extends OpMode { */ @Override public void loop() { - driveVector.setOrthogonalComponents(-gamepad1.left_stick_y, -gamepad1.left_stick_x); - driveVector.setMagnitude(MathFunctions.clamp(driveVector.getMagnitude(), 0, 1)); - - // TODO: if you want to make this field centric, then just remove this line - driveVector.rotateVector(follower.getPose().getHeading()); - - headingVector.setComponents(-gamepad1.right_stick_x, follower.getPose().getHeading()); - - follower.setMovementVectors(follower.getCentripetalForceCorrection(), headingVector, driveVector); + follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x); follower.update(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index 383b7aa..3d61d7a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -82,9 +82,10 @@ public class Follower { private boolean followingPathChain; private boolean holdingPosition; private boolean isBusy; - private boolean auto = true; private boolean reachedParametricPathEnd; private boolean holdPositionAtEnd; + private boolean teleopDrive; + private boolean fieldCentric; private double maxPower = 1; private double previousSmallTranslationalIntegral; @@ -97,6 +98,7 @@ public class Follower { private long reachedParametricPathEndTime; private double[] drivePowers; + private double[] teleopDriveValues; private Vector[] teleOpMovementVectors = new Vector[]{new Vector(), new Vector(), new Vector()}; @@ -144,19 +146,6 @@ public class Follower { initialize(); } - /** - * This creates a new Follower given a HardwareMap and sets whether the Follower is being used - * in autonomous or teleop. - * - * @param hardwareMap HardwareMap required - * @param setAuto sets whether or not the Follower is being used in autonomous or teleop - */ - public Follower(HardwareMap hardwareMap, boolean setAuto) { - this.hardwareMap = hardwareMap; - setAuto(setAuto); - initialize(); - } - /** * This initializes the follower. * In this, the DriveVectorScaler and PoseUpdater is instantiated, the drive motors are @@ -188,15 +177,12 @@ public class Follower { motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); } - for (int i = 0; i < AVERAGED_VELOCITY_SAMPLE_NUMBER; i++) { - velocities.add(new Vector()); - } - for (int i = 0; i < AVERAGED_VELOCITY_SAMPLE_NUMBER / 2; i++) { - accelerations.add(new Vector()); - } - calculateAveragedVelocityAndAcceleration(); + // TODO: Set this to true if you want to use field centric teleop drive. + fieldCentric = false; dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); + + breakFollowing(); } /** @@ -428,6 +414,14 @@ public class Follower { followPath(pathChain, false); } + /** + * This starts teleop drive control. + */ + public void startTeleopDrive() { + breakFollowing(); + teleopDrive = true; + } + /** * This calls an update to the PoseUpdater, which updates the robot's current position estimate. * This also updates all the Follower's PIDFs, which updates the motor powers. @@ -439,7 +433,7 @@ public class Follower { dashboardPoseTracker.update(); } - if (auto) { + if (!teleopDrive) { if (holdingPosition) { closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1); @@ -499,7 +493,19 @@ public class Follower { calculateAveragedVelocityAndAcceleration(); - drivePowers = driveVectorScaler.getDrivePowers(teleOpMovementVectors[0], teleOpMovementVectors[1], teleOpMovementVectors[2], poseUpdater.getPose().getHeading()); + Vector teleopDriveVector = new Vector(); + Vector teleopHeadingVector = new Vector(); + + teleopDriveVector.setOrthogonalComponents(teleopDriveValues[0], teleopDriveValues[1]); + teleopDriveVector.setMagnitude(MathFunctions.clamp(driveVector.getMagnitude(), 0, 1)); + + if (!fieldCentric) { + teleopDriveVector.rotateVector(getPose().getHeading()); + } + + teleopHeadingVector.setComponents(teleopDriveValues[2], getPose().getHeading()); + + drivePowers = driveVectorScaler.getDrivePowers(getCentripetalForceCorrection(), teleopHeadingVector, teleopDriveVector, poseUpdater.getPose().getHeading()); limitDrivePowers(); @@ -509,6 +515,21 @@ public class Follower { } } + /** + * This sets the teleop drive vectors. + * + * @param forwardDrive determines the forward drive vector for the robot in teleop. In field centric + * movement, this is the x-axis. + * @param lateralDrive determines the lateral drive vector for the robot in teleop. In field centric + * movement, this is the y-axis. + * @param heading determines the heading vector for the robot in teleop. + */ + public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, double heading) { + teleopDriveValues[0] = MathFunctions.clamp(forwardDrive, -1, 1); + teleopDriveValues[1] = MathFunctions.clamp(lateralDrive, -1, 1); + teleopDriveValues[2] = MathFunctions.clamp(heading, -1, 1); + } + /** * This calculates an averaged approximate velocity and acceleration. This is used for a * real-time correction of centripetal force, which is used in teleop. @@ -564,6 +585,7 @@ public class Follower { * This resets the PIDFs and stops following the current Path. */ public void breakFollowing() { + teleopDrive = false; holdingPosition = false; isBusy = false; reachedParametricPathEnd = false; @@ -594,6 +616,15 @@ public class Follower { } driveKalmanFilter.reset(); + for (int i = 0; i < AVERAGED_VELOCITY_SAMPLE_NUMBER; i++) { + velocities.add(new Vector()); + } + for (int i = 0; i < AVERAGED_VELOCITY_SAMPLE_NUMBER / 2; i++) { + accelerations.add(new Vector()); + } + calculateAveragedVelocityAndAcceleration(); + teleopDriveValues = new double[3]; + for (int i = 0; i < motors.size(); i++) { motors.get(i).setPower(0); } @@ -608,14 +639,6 @@ public class Follower { return isBusy; } - /** - * Sets the correctional, heading, and drive movement vectors for teleop enhancements. - * The correctional Vector only accounts for an approximated centripetal correction. - */ - public void setMovementVectors(Vector correctional, Vector heading, Vector drive) { - teleOpMovementVectors = new Vector[]{correctional, heading, drive}; - } - /** * This returns a Vector in the direction the robot must go to move along the path. This Vector * takes into account the projected position of the robot to calculate how much power is needed. @@ -681,14 +704,14 @@ public class Follower { previousRawDriveError = rawDriveError; rawDriveError = velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector())); - double projection = 2 * driveErrors[2] - driveErrors[1]; + double projection = 2 * driveErrors[1] - driveErrors[0]; driveKalmanFilter.update(rawDriveError - previousRawDriveError, projection); for (int i = 0; i < driveErrors.length - 1; i++) { driveErrors[i] = driveErrors[i + 1]; } - driveErrors[2] = driveKalmanFilter.getState(); + driveErrors[1] = driveKalmanFilter.getState(); return driveKalmanFilter.getState(); } @@ -809,7 +832,7 @@ public class Follower { public Vector getCentripetalForceCorrection() { if (!useCentripetal) return new Vector(); double curvature; - if (auto) { + if (!teleopDrive) { curvature = currentPath.getClosestPointCurvature(); } else { double yPrime = averageVelocity.getYComponent() / averageVelocity.getXComponent(); @@ -832,15 +855,6 @@ public class Follower { return closestPose; } - /** - * This sets whether or not the Follower is being used in auto or teleop. - * - * @param set sets auto or not - */ - public void setAuto(boolean set) { - auto = set; - } - /** * This returns whether the follower is at the parametric end of its current Path. * The parametric end is determined by if the closest Point t-value is greater than some specified From 1703f88159b39e35c16fd93789ad417ed04d8943 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Tue, 30 Jul 2024 00:33:55 -0400 Subject: [PATCH 28/94] better teleop drive robot/field centric functionality --- .../pedroPathing/follower/Follower.java | 44 ++++++++++++------- 1 file changed, 27 insertions(+), 17 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index 3d61d7a..d6b028b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -85,7 +85,6 @@ public class Follower { private boolean reachedParametricPathEnd; private boolean holdPositionAtEnd; private boolean teleopDrive; - private boolean fieldCentric; private double maxPower = 1; private double previousSmallTranslationalIntegral; @@ -110,6 +109,8 @@ public class Follower { private Vector averageAcceleration; private Vector smallTranslationalIntegralVector; private Vector largeTranslationalIntegralVector; + private Vector teleopDriveVector; + private Vector teleopHeadingVector; public Vector driveVector; public Vector headingVector; public Vector translationalVector; @@ -177,9 +178,6 @@ public class Follower { motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); } - // TODO: Set this to true if you want to use field centric teleop drive. - fieldCentric = false; - dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); breakFollowing(); @@ -493,18 +491,6 @@ public class Follower { calculateAveragedVelocityAndAcceleration(); - Vector teleopDriveVector = new Vector(); - Vector teleopHeadingVector = new Vector(); - - teleopDriveVector.setOrthogonalComponents(teleopDriveValues[0], teleopDriveValues[1]); - teleopDriveVector.setMagnitude(MathFunctions.clamp(driveVector.getMagnitude(), 0, 1)); - - if (!fieldCentric) { - teleopDriveVector.rotateVector(getPose().getHeading()); - } - - teleopHeadingVector.setComponents(teleopDriveValues[2], getPose().getHeading()); - drivePowers = driveVectorScaler.getDrivePowers(getCentripetalForceCorrection(), teleopHeadingVector, teleopDriveVector, poseUpdater.getPose().getHeading()); limitDrivePowers(); @@ -516,7 +502,7 @@ public class Follower { } /** - * This sets the teleop drive vectors. + * This sets the teleop drive vectors. This defaults to robot centric. * * @param forwardDrive determines the forward drive vector for the robot in teleop. In field centric * movement, this is the x-axis. @@ -525,9 +511,31 @@ public class Follower { * @param heading determines the heading vector for the robot in teleop. */ public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, double heading) { + setTeleOpMovementVectors(forwardDrive, lateralDrive, heading, true); + } + + /** + * This sets the teleop drive vectors. + * + * @param forwardDrive determines the forward drive vector for the robot in teleop. In field centric + * movement, this is the x-axis. + * @param lateralDrive determines the lateral drive vector for the robot in teleop. In field centric + * movement, this is the y-axis. + * @param heading determines the heading vector for the robot in teleop. + * @param robotCentric sets if the movement will be field or robot centric + */ + public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, double heading, boolean robotCentric) { teleopDriveValues[0] = MathFunctions.clamp(forwardDrive, -1, 1); teleopDriveValues[1] = MathFunctions.clamp(lateralDrive, -1, 1); teleopDriveValues[2] = MathFunctions.clamp(heading, -1, 1); + teleopDriveVector.setOrthogonalComponents(teleopDriveValues[0], teleopDriveValues[1]); + teleopDriveVector.setMagnitude(MathFunctions.clamp(teleopDriveVector.getMagnitude(), 0, 1)); + + if (robotCentric) { + teleopDriveVector.rotateVector(getPose().getHeading()); + } + + teleopHeadingVector.setComponents(teleopDriveValues[2], getPose().getHeading()); } /** @@ -624,6 +632,8 @@ public class Follower { } calculateAveragedVelocityAndAcceleration(); teleopDriveValues = new double[3]; + teleopDriveVector = new Vector(); + teleopHeadingVector = new Vector(); for (int i = 0; i < motors.size(); i++) { motors.get(i).setPower(0); From 22d38b128965b17b2c4c84261d765988cbc8928f Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Wed, 31 Jul 2024 15:23:57 -0400 Subject: [PATCH 29/94] hopefully fixes any issues with the class --- ...{ThreeWheelIMULocalizer.Java => ThreeWheelIMULocalizer.java} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/{ThreeWheelIMULocalizer.Java => ThreeWheelIMULocalizer.java} (100%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.Java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.Java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java index 675f46b..4760e1c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.Java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java @@ -86,6 +86,7 @@ public class ThreeWheelIMULocalizer extends Localizer { public ThreeWheelIMULocalizer(HardwareMap map, Pose setStartPose) { hardwareMap = map; imu = hardwareMap.get(IMU.class, "imu"); + // TODO: replace this with your IMU's orientation imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.LEFT, RevHubOrientationOnRobot.UsbFacingDirection.UP))); @@ -95,7 +96,6 @@ public class ThreeWheelIMULocalizer extends Localizer { strafeEncoderPose = new Pose(6.9, 1, Math.toRadians(90)); - // TODO: replace these with your encoder ports leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "lb")); rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "lf")); From 4a669f770f99919b4927e01224cf201ae0a7432c Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Tue, 6 Aug 2024 23:00:02 -0400 Subject: [PATCH 30/94] simplifying the PID system --- .../ftc/teamcode/pedroPathing/OVERVIEW.md | 19 +-- .../ftc/teamcode/pedroPathing/TUNING.md | 36 +++-- .../pedroPathing/follower/Follower.java | 115 +++++++-------- .../tuning/FollowerConstants.java | 135 ++++++++++-------- 4 files changed, 155 insertions(+), 150 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md index 71ce35a..a0dbca1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md @@ -68,27 +68,18 @@ calculate the force necessary to keep the robot on the path, and then tune a sca that force into a corresponding power for the robot. ### Translational Correction -This is as simple as it sounds: this corrects error in the robot's position only. However, the -robot's translational error is actually used within two different PIDs to move the robot. A large -PID and a small PID are used. The large PID is used when error exceeds a certain limit. and the small -PID is used when the error is within that limit. - -When the robot encounters error, the large PID, if applicable, is used to bring the robot within the -small PID error range without much overshoot to avoid oscillations. Then, the small PID is used to -bring the robot to within acceptable error ranges. The reason for this double PID is to allow for -aggressive correction to bring the robot within tight tolerances, while not correcting too -aggressively at larger ranges, which would've caused oscillations. +This is as simple as it sounds: this corrects error in the robot's position only. The robot's translational +error is corrected with a PID control. The translational correction does not act along the path the +robot takes, but instead moves the robot back to the closest point on the path. ### Heading Correction The heading correction operates very similarly to the translational correction, except this corrects -the direction the robot is facing. It also uses a small and large PID rather than a single PID, like -the translational correction. The heading correction will turn in the closest direction from the +the direction the robot is facing. The heading correction will turn in the closest direction from the robot's current heading to the target heading. ### Drive Vector The drive vector points in the direction of the tangent of the path and it is responsible for moving -the robot along the path. Similar to the translational and heading corrections, the drive vector -uses a large and small PID. Using basic kinematics equations, we can use the velocity of the robot +the robot along the path. Using basic kinematics equations, we can use the velocity of the robot along the path, the length of path left, and a specified target rate of deceleration to calculate the velocity we should be moving at. Additionally, after finding out the rate of deceleration of the robot under 0 power, we can compensate for that with another kinematics equation. Combining these diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md index f098c3e..42d3819 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md @@ -10,7 +10,7 @@ One last thing to note is that Pedro Pathing operates in inches and radians. ## Tuning * To start with, we need the mass of the robot in kg. This is used for the centripetal force - correction, and the mass should be put on line `123` in the `FollowerConstants` class under the + correction, and the mass should be put on line `80` in the `FollowerConstants` class under the `tuning` package. * Next, we need to find the preferred mecanum drive vectors. The rollers on mecanum wheels point at a @@ -36,27 +36,23 @@ One last thing to note is that Pedro Pathing operates in inches and radians. which point it will display the deceleration in telemetry. This robot will need to drift to a stop to properly work, and the higher the velocity the greater the drift distance, so make sure you have enough room. Once you're done, put the zero power acceleration for the - `Forward Zero Power Acceleration Tuner` on line `130` in the `FollowerConstants` class and the zero - power acceleration for the `Lateral Zero Power Acceleration Tuner` on line `134` in the + `Forward Zero Power Acceleration Tuner` on line `88` in the `FollowerConstants` class and the zero + power acceleration for the `Lateral Zero Power Acceleration Tuner` on line `92` in the `FollowerConstants` class. * After this, we will want to tune the translational PIDs. Go to FTC Dashboard and disable all but the `useTranslational` checkboxes under the `Follower` tab. Then, run `StraightBackAndForth`. Make - sure you disable the timer on autonomous OpModes. There are two different PIDs for translational - error, the `smallTranslationalPIDF` and `largeTranslationalPIDF`. If error is larger than a certain - amount, the `largeTranslationalPIDF` will be used, and if error is smaller than that amount the - `smallTranslationalPIDF` will be used. If you need to add a feedforward value, use the - `smallTranslationalPIDFFeedForward` and `largeTranslationalPIDFFeedForward` since those will add the - feedforward in the direction the robot is trying to move, rather than the feedforward in the PIDFs - themselves, since those will only add the feedforward one way. You can change the amount of error - required to switch PIDFs, as well as the PIDF constants and feedforward values, under the - `FollowerConstants` tab in FTC Dashboard. To tune the PIDs, push the robot off the path and see how - corrects. You will want to alternate sides you push to reduce field wear and tear as well as push - with varying power and distance. I would recommend tuning the large PID first, and tuning it so that - the PID is capable of correcting to the point where the PIDs switch with little momentum. Then, tune - the small PID to minimize oscillations while still achieving a satisfactory level of accuracy. - Overall, try to tune for fewer oscillations rather than higher speeds or perfect accuracy, since - this will make the robot run more smoothly under actual pathing conditions. + sure you disable the timer on autonomous OpModes. The PID for the translational error is called + `translationalPIDF`. If you need to add a feedforward value, use the `translationalPIDFFeedForward` + since that will add the feedforward in the direction the robot is trying to move, rather than the + feedforward in the PIDF itself, since those will only add the feedforward one way. You can change + the PIDF constants and feedforward values, under the `FollowerConstants` tab in FTC Dashboard. + To tune the PID, push the robot off the path and see how corrects. You will want to alternate sides + you push to reduce field wear and tear as well as push with varying power and distance. I would + recommend tuning the PID so that it is capable of correcting while minimizing oscillations and still + achieving a satisfactory level of accuracy. Overall, try to tune for fewer oscillations rather than + higher speeds or perfect accuracy, since this will make the robot run more smoothly under actual + pathing conditions. * Next, we will tune the heading PIDs. The process is essentially the same as above, except you will want to only enable `useHeading` under `Follower` on FTC Dashboard, as well as turn the robot from @@ -68,7 +64,7 @@ One last thing to note is that Pedro Pathing operates in inches and radians. `zeroPowerAccelerationMultiplier`. This determines how fast your robot will decelerate as a factor of how fast your robot will coast to a stop. Honestly, this is up to you. I personally used 4, but what works best for you is most important. Higher numbers will cause a faster brake, but increase - oscillations at the end. Lower numbers will do the opposite. This can be found on line `143` in + oscillations at the end. Lower numbers will do the opposite. This can be found on line `101` in `FollowerConstants`. There are once again two PIDs for the drive vector, but these PIDs are much, much more sensitive than the others. For reference, my P values were in the hundredths and thousandths place values, and my D values were in the hundred thousandths and millionths place @@ -95,7 +91,7 @@ One last thing to note is that Pedro Pathing operates in inches and radians. * Finally, we will want to tune the centripetal force correction. This is a pretty simple tune. Open up FTC Dashboard and enable everything under the `Follower` tab. Then, run `CurvedBackAndForth` and turn off its timer. If you notice the robot is correcting towards the inside of the curve - as/after running a path, then increase `centripetalScaling`, which can be found on line `126` of + as/after running a path, then increase `centripetalScaling`, which can be found on line `83` of `FollowerConstants`. If the robot is correcting towards the outside of the curve, then decrease `centripetalScaling`. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index d6b028b..66728fd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -3,14 +3,17 @@ package org.firstinspires.ftc.teamcode.pedroPathing.follower; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFSwitch; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.forwardZeroPowerAcceleration; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.largeDrivePIDFFeedForward; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.largeHeadingPIDFFeedForward; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.largeTranslationalPIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.lateralZeroPowerAcceleration; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.smallDrivePIDFFeedForward; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.smallHeadingPIDFFeedForward; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.smallTranslationalPIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryDrivePIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryHeadingPIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryTranslationalPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFSwitch; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryDrivePID; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryHeadingPID; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryTranslationalPID; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -87,8 +90,8 @@ public class Follower { private boolean teleopDrive; private double maxPower = 1; - private double previousSmallTranslationalIntegral; - private double previousLargeTranslationalIntegral; + private double previousSecondaryTranslationalIntegral; + private double previousTranslationalIntegral; private double holdPointTranslationalScaling = FollowerConstants.holdPointTranslationalScaling; private double holdPointHeadingScaling = FollowerConstants.holdPointHeadingScaling; public double driveError; @@ -99,16 +102,14 @@ public class Follower { private double[] drivePowers; private double[] teleopDriveValues; - private Vector[] teleOpMovementVectors = new Vector[]{new Vector(), new Vector(), new Vector()}; - private ArrayList velocities = new ArrayList<>(); private ArrayList accelerations = new ArrayList<>(); private Vector averageVelocity; private Vector averagePreviousVelocity; private Vector averageAcceleration; - private Vector smallTranslationalIntegralVector; - private Vector largeTranslationalIntegralVector; + private Vector secondaryTranslationalIntegralVector; + private Vector translationalIntegralVector; private Vector teleopDriveVector; private Vector teleopHeadingVector; public Vector driveVector; @@ -117,14 +118,14 @@ public class Follower { public Vector centripetalVector; public Vector correctiveVector; - private PIDFController smallTranslationalPIDF = new PIDFController(FollowerConstants.smallTranslationalPIDFCoefficients); - private PIDFController smallTranslationalIntegral = new PIDFController(FollowerConstants.smallTranslationalIntegral); - private PIDFController largeTranslationalPIDF = new PIDFController(FollowerConstants.largeTranslationalPIDFCoefficients); - private PIDFController largeTranslationalIntegral = new PIDFController(FollowerConstants.largeTranslationalIntegral); - private PIDFController smallHeadingPIDF = new PIDFController(FollowerConstants.smallHeadingPIDFCoefficients); - private PIDFController largeHeadingPIDF = new PIDFController(FollowerConstants.largeHeadingPIDFCoefficients); - private FilteredPIDFController smallDrivePIDF = new FilteredPIDFController(FollowerConstants.smallDrivePIDFCoefficients); - private FilteredPIDFController largeDrivePIDF = new FilteredPIDFController(FollowerConstants.largeDrivePIDFCoefficients); + private PIDFController secondaryTranslationalPIDF = new PIDFController(FollowerConstants.secondaryTranslationalPIDFCoefficients); + private PIDFController secondaryTranslationalIntegral = new PIDFController(FollowerConstants.secondaryTranslationalIntegral); + private PIDFController translationalPIDF = new PIDFController(FollowerConstants.translationalPIDFCoefficients); + private PIDFController translationalIntegral = new PIDFController(FollowerConstants.translationalIntegral); + private PIDFController secondaryHeadingPIDF = new PIDFController(FollowerConstants.secondaryHeadingPIDFCoefficients); + private PIDFController headingPIDF = new PIDFController(FollowerConstants.headingPIDFCoefficients); + private FilteredPIDFController secondaryDrivePIDF = new FilteredPIDFController(FollowerConstants.secondaryDrivePIDFCoefficients); + private FilteredPIDFController drivePIDF = new FilteredPIDFController(FollowerConstants.drivePIDFCoefficients); private KalmanFilter driveKalmanFilter = new KalmanFilter(FollowerConstants.driveKalmanFilterParameters); private double[] driveErrors; @@ -597,18 +598,18 @@ public class Follower { holdingPosition = false; isBusy = false; reachedParametricPathEnd = false; - smallDrivePIDF.reset(); - largeDrivePIDF.reset(); - smallHeadingPIDF.reset(); - largeHeadingPIDF.reset(); - smallTranslationalPIDF.reset(); - smallTranslationalIntegral.reset(); - smallTranslationalIntegralVector = new Vector(); - previousSmallTranslationalIntegral = 0; - largeTranslationalPIDF.reset(); - largeTranslationalIntegral.reset(); - largeTranslationalIntegralVector = new Vector(); - previousLargeTranslationalIntegral = 0; + secondaryDrivePIDF.reset(); + drivePIDF.reset(); + secondaryHeadingPIDF.reset(); + headingPIDF.reset(); + secondaryTranslationalPIDF.reset(); + secondaryTranslationalIntegral.reset(); + secondaryTranslationalIntegralVector = new Vector(); + previousSecondaryTranslationalIntegral = 0; + translationalPIDF.reset(); + translationalIntegral.reset(); + translationalIntegralVector = new Vector(); + previousTranslationalIntegral = 0; driveVector = new Vector(); headingVector = new Vector(); translationalVector = new Vector(); @@ -665,14 +666,14 @@ public class Follower { driveError = getDriveVelocityError(); - if (Math.abs(driveError) < drivePIDFSwitch) { - smallDrivePIDF.updateError(driveError); - driveVector = new Vector(MathFunctions.clamp(smallDrivePIDF.runPIDF() + smallDrivePIDFFeedForward * MathFunctions.getSign(driveError), -1, 1), currentPath.getClosestPointTangentVector().getTheta()); + if (Math.abs(driveError) < drivePIDFSwitch && useSecondaryDrivePID) { + secondaryDrivePIDF.updateError(driveError); + driveVector = new Vector(MathFunctions.clamp(secondaryDrivePIDF.runPIDF() + secondaryDrivePIDFFeedForward * MathFunctions.getSign(driveError), -1, 1), currentPath.getClosestPointTangentVector().getTheta()); return MathFunctions.copyVector(driveVector); } - largeDrivePIDF.updateError(driveError); - driveVector = new Vector(MathFunctions.clamp(largeDrivePIDF.runPIDF() + largeDrivePIDFFeedForward * MathFunctions.getSign(driveError), -1, 1), currentPath.getClosestPointTangentVector().getTheta()); + drivePIDF.updateError(driveError); + driveVector = new Vector(MathFunctions.clamp(drivePIDF.runPIDF() + drivePIDFFeedForward * MathFunctions.getSign(driveError), -1, 1), currentPath.getClosestPointTangentVector().getTheta()); return MathFunctions.copyVector(driveVector); } @@ -739,13 +740,13 @@ public class Follower { public Vector getHeadingVector() { if (!useHeading) return new Vector(); headingError = MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()) * MathFunctions.getSmallestAngleDifference(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()); - if (Math.abs(headingError) < headingPIDFSwitch) { - smallHeadingPIDF.updateError(headingError); - headingVector = new Vector(MathFunctions.clamp(smallHeadingPIDF.runPIDF() + smallHeadingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -1, 1), poseUpdater.getPose().getHeading()); + if (Math.abs(headingError) < headingPIDFSwitch && useSecondaryHeadingPID) { + secondaryHeadingPIDF.updateError(headingError); + headingVector = new Vector(MathFunctions.clamp(secondaryHeadingPIDF.runPIDF() + secondaryHeadingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -1, 1), poseUpdater.getPose().getHeading()); return MathFunctions.copyVector(headingVector); } - largeHeadingPIDF.updateError(headingError); - headingVector = new Vector(MathFunctions.clamp(largeHeadingPIDF.runPIDF() + largeHeadingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -1, 1), poseUpdater.getPose().getHeading()); + headingPIDF.updateError(headingError); + headingVector = new Vector(MathFunctions.clamp(headingPIDF.runPIDF() + headingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -1, 1), poseUpdater.getPose().getHeading()); return MathFunctions.copyVector(headingVector); } @@ -789,26 +790,26 @@ public class Follower { if (!(currentPath.isAtParametricEnd() || currentPath.isAtParametricStart())) { translationalVector = MathFunctions.subtractVectors(translationalVector, new Vector(MathFunctions.dotProduct(translationalVector, MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta())); - smallTranslationalIntegralVector = MathFunctions.subtractVectors(smallTranslationalIntegralVector, new Vector(MathFunctions.dotProduct(smallTranslationalIntegralVector, MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta())); - largeTranslationalIntegralVector = MathFunctions.subtractVectors(largeTranslationalIntegralVector, new Vector(MathFunctions.dotProduct(largeTranslationalIntegralVector, MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta())); + secondaryTranslationalIntegralVector = MathFunctions.subtractVectors(secondaryTranslationalIntegralVector, new Vector(MathFunctions.dotProduct(secondaryTranslationalIntegralVector, MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta())); + translationalIntegralVector = MathFunctions.subtractVectors(translationalIntegralVector, new Vector(MathFunctions.dotProduct(translationalIntegralVector, MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta())); } - if (MathFunctions.distance(poseUpdater.getPose(), closestPose) < translationalPIDFSwitch) { - smallTranslationalIntegral.updateError(translationalVector.getMagnitude()); - smallTranslationalIntegralVector = MathFunctions.addVectors(smallTranslationalIntegralVector, new Vector(smallTranslationalIntegral.runPIDF() - previousSmallTranslationalIntegral, translationalVector.getTheta())); - previousSmallTranslationalIntegral = smallTranslationalIntegral.runPIDF(); + if (MathFunctions.distance(poseUpdater.getPose(), closestPose) < translationalPIDFSwitch && useSecondaryTranslationalPID) { + secondaryTranslationalIntegral.updateError(translationalVector.getMagnitude()); + secondaryTranslationalIntegralVector = MathFunctions.addVectors(secondaryTranslationalIntegralVector, new Vector(secondaryTranslationalIntegral.runPIDF() - previousSecondaryTranslationalIntegral, translationalVector.getTheta())); + previousSecondaryTranslationalIntegral = secondaryTranslationalIntegral.runPIDF(); - smallTranslationalPIDF.updateError(translationalVector.getMagnitude()); - translationalVector.setMagnitude(smallTranslationalPIDF.runPIDF() + smallTranslationalPIDFFeedForward); - translationalVector = MathFunctions.addVectors(translationalVector, smallTranslationalIntegralVector); + secondaryTranslationalPIDF.updateError(translationalVector.getMagnitude()); + translationalVector.setMagnitude(secondaryTranslationalPIDF.runPIDF() + secondaryTranslationalPIDFFeedForward); + translationalVector = MathFunctions.addVectors(translationalVector, secondaryTranslationalIntegralVector); } else { - largeTranslationalIntegral.updateError(translationalVector.getMagnitude()); - largeTranslationalIntegralVector = MathFunctions.addVectors(largeTranslationalIntegralVector, new Vector(largeTranslationalIntegral.runPIDF() - previousLargeTranslationalIntegral, translationalVector.getTheta())); - previousLargeTranslationalIntegral = largeTranslationalIntegral.runPIDF(); + translationalIntegral.updateError(translationalVector.getMagnitude()); + translationalIntegralVector = MathFunctions.addVectors(translationalIntegralVector, new Vector(translationalIntegral.runPIDF() - previousTranslationalIntegral, translationalVector.getTheta())); + previousTranslationalIntegral = translationalIntegral.runPIDF(); - largeTranslationalPIDF.updateError(translationalVector.getMagnitude()); - translationalVector.setMagnitude(largeTranslationalPIDF.runPIDF() + largeTranslationalPIDFFeedForward); - translationalVector = MathFunctions.addVectors(translationalVector, largeTranslationalIntegralVector); + translationalPIDF.updateError(translationalVector.getMagnitude()); + translationalVector.setMagnitude(translationalPIDF.runPIDF() + translationalPIDFFeedForward); + translationalVector = MathFunctions.addVectors(translationalVector, translationalIntegralVector); } translationalVector.setMagnitude(MathFunctions.clamp(translationalVector.getMagnitude(), 0, 1)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java index 4569d77..df04c33 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java @@ -29,102 +29,60 @@ public class FollowerConstants { private static double[] convertToPolar = Point.cartesianToPolar(xMovement, -yMovement); public static Vector frontLeftVector = MathFunctions.normalizeVector(new Vector(convertToPolar[0],convertToPolar[1])); - // Large translational PIDF coefficients - public static CustomPIDFCoefficients largeTranslationalPIDFCoefficients = new CustomPIDFCoefficients( + + // Translational PIDF coefficients (don't use integral) + public static CustomPIDFCoefficients translationalPIDFCoefficients = new CustomPIDFCoefficients( 0.1, 0, 0, 0); - // Feed forward constant added on to the large translational PIDF - public static double largeTranslationalPIDFFeedForward = 0.015; - - // Large translational Integral - public static CustomPIDFCoefficients largeTranslationalIntegral = new CustomPIDFCoefficients( + // Translational Integral + public static CustomPIDFCoefficients translationalIntegral = new CustomPIDFCoefficients( 0, 0, 0, 0); - // the limit at which the heading PIDF switches between the large and small translational PIDFs - public static double translationalPIDFSwitch = 3; + // Feed forward constant added on to the translational PIDF + public static double translationalPIDFFeedForward = 0.015; - // Small translational PIDF coefficients - public static CustomPIDFCoefficients smallTranslationalPIDFCoefficients = new CustomPIDFCoefficients( - 0.3, - 0, - 0.01, - 0); - // Small translational Integral value - public static CustomPIDFCoefficients smallTranslationalIntegral = new CustomPIDFCoefficients( - 0, - 0, - 0, - 0); - - // Feed forward constant added on to the small translational PIDF - public static double smallTranslationalPIDFFeedForward = 0.015; - - // Large heading error PIDF coefficients - public static CustomPIDFCoefficients largeHeadingPIDFCoefficients = new CustomPIDFCoefficients( + // Heading error PIDF coefficients + public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients( 1, 0, 0, 0); - // Feed forward constant added on to the large heading PIDF - public static double largeHeadingPIDFFeedForward = 0.01; + // Feed forward constant added on to the heading PIDF + public static double headingPIDFFeedForward = 0.01; - // the limit at which the heading PIDF switches between the large and small heading PIDFs - public static double headingPIDFSwitch = Math.PI/20; - // Small heading error PIDF coefficients - public static CustomPIDFCoefficients smallHeadingPIDFCoefficients = new CustomPIDFCoefficients( - 5, - 0, - 0.08, - 0); - - // Feed forward constant added on to the small heading PIDF - public static double smallHeadingPIDFFeedForward = 0.01; - - // Large drive PIDF coefficients - public static CustomFilteredPIDFCoefficients largeDrivePIDFCoefficients = new CustomFilteredPIDFCoefficients( + // Drive PIDF coefficients + public static CustomFilteredPIDFCoefficients drivePIDFCoefficients = new CustomFilteredPIDFCoefficients( 0.025, 0, 0.00001, 0.6, 0); - // Feed forward constant added on to the large drive PIDF - public static double largeDrivePIDFFeedForward = 0.01; - - // the limit at which the heading PIDF switches between the large and small drive PIDFs - public static double drivePIDFSwitch = 20; - - // Small drive PIDF coefficients - public static CustomFilteredPIDFCoefficients smallDrivePIDFCoefficients = new CustomFilteredPIDFCoefficients( - 0.02, - 0, - 0.000005, - 0.6, - 0); - - // Feed forward constant added on to the small drive PIDF - public static double smallDrivePIDFFeedForward = 0.01; + // Feed forward constant added on to the drive PIDF + public static double drivePIDFFeedForward = 0.01; // Kalman filter parameters for the drive error Kalman filter public static KalmanFilterParameters driveKalmanFilterParameters = new KalmanFilterParameters( 6, 1); + // Mass of robot in kilograms public static double mass = 10.65942; // Centripetal force to power scaling public static double centripetalScaling = 0.0005; + // Acceleration of the drivetrain when power is cut in inches/second^2 (should be negative) // if not negative, then the robot thinks that its going to go faster under 0 power public static double forwardZeroPowerAcceleration = -34.62719; @@ -142,6 +100,7 @@ public class FollowerConstants { // This can be set individually for each Path, but this is the default. public static double zeroPowerAccelerationMultiplier = 4; + // When the robot is at the end of its current Path or PathChain and the velocity goes below // this value, then end the Path. This is in inches/second. // This can be custom set for each Path. @@ -185,4 +144,62 @@ public class FollowerConstants { // accuracy, and this increases at an exponential rate. However, more steps also does take more // time. public static int BEZIER_CURVE_BINARY_STEP_LIMIT = 10; + + + // These activate / deactivate the secondary PIDs. These take over at errors under a set limit for + // the translational, heading, and drive PIDs. + public static boolean useSecondaryTranslationalPID = false; + public static boolean useSecondaryHeadingPID = false; + public static boolean useSecondaryDrivePID = false; + + + // the limit at which the translational PIDF switches between the main and secondary translational PIDFs, + // if the secondary PID is active + public static double translationalPIDFSwitch = 3; + + // Secondary translational PIDF coefficients (don't use integral) + public static CustomPIDFCoefficients secondaryTranslationalPIDFCoefficients = new CustomPIDFCoefficients( + 0.3, + 0, + 0.01, + 0); + + // Secondary translational Integral value + public static CustomPIDFCoefficients secondaryTranslationalIntegral = new CustomPIDFCoefficients( + 0, + 0, + 0, + 0); + + // Feed forward constant added on to the small translational PIDF + public static double secondaryTranslationalPIDFFeedForward = 0.015; + + + // the limit at which the heading PIDF switches between the main and secondary heading PIDFs + public static double headingPIDFSwitch = Math.PI/20; + + // Secondary heading error PIDF coefficients + public static CustomPIDFCoefficients secondaryHeadingPIDFCoefficients = new CustomPIDFCoefficients( + 5, + 0, + 0.08, + 0); + + // Feed forward constant added on to the secondary heading PIDF + public static double secondaryHeadingPIDFFeedForward = 0.01; + + + // the limit at which the heading PIDF switches between the main and secondary drive PIDFs + public static double drivePIDFSwitch = 20; + + // Secondary drive PIDF coefficients + public static CustomFilteredPIDFCoefficients secondaryDrivePIDFCoefficients = new CustomFilteredPIDFCoefficients( + 0.02, + 0, + 0.000005, + 0.6, + 0); + + // Feed forward constant added on to the secondary drive PIDF + public static double secondaryDrivePIDFFeedForward = 0.01; } From e849fd8e1e49765a4b928439f503dcdbf68a377d Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Wed, 7 Aug 2024 21:39:27 -0400 Subject: [PATCH 31/94] simplifying the PID system --- .../ftc/teamcode/pedroPathing/TUNING.md | 48 +++++++++++-------- 1 file changed, 29 insertions(+), 19 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md index 42d3819..175a050 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md @@ -9,8 +9,8 @@ will help a lot in tuning, and we have a slightly scuffed Desmos path visualizer One last thing to note is that Pedro Pathing operates in inches and radians. ## Tuning -* To start with, we need the mass of the robot in kg. This is used for the centripetal force - correction, and the mass should be put on line `80` in the `FollowerConstants` class under the +* To start with, we need the mass of the robot in kg. This is used for the centripetal force correction, + and the mass should be put on line `80` in the `FollowerConstants` class under the `tuning` package. * Next, we need to find the preferred mecanum drive vectors. The rollers on mecanum wheels point at a @@ -40,7 +40,7 @@ One last thing to note is that Pedro Pathing operates in inches and radians. power acceleration for the `Lateral Zero Power Acceleration Tuner` on line `92` in the `FollowerConstants` class. -* After this, we will want to tune the translational PIDs. Go to FTC Dashboard and disable all but +* After this, we will want to tune the translational PID. Go to FTC Dashboard and disable all but the `useTranslational` checkboxes under the `Follower` tab. Then, run `StraightBackAndForth`. Make sure you disable the timer on autonomous OpModes. The PID for the translational error is called `translationalPIDF`. If you need to add a feedforward value, use the `translationalPIDFFeedForward` @@ -54,37 +54,36 @@ One last thing to note is that Pedro Pathing operates in inches and radians. higher speeds or perfect accuracy, since this will make the robot run more smoothly under actual pathing conditions. -* Next, we will tune the heading PIDs. The process is essentially the same as above, except you will +* Next, we will tune the heading PID. The process is essentially the same as above, except you will want to only enable `useHeading` under `Follower` on FTC Dashboard, as well as turn the robot from opposing corners instead of pushing the robot. Naturally, instead of changing the stuff with "translational" in the name, you will instead want to look for stuff with "heading" in the name. Otherwise, these two PIDs are functionally very similar. The same tips from above will apply to this. -* Afterwards, we will tune the drive PIDs. Before we continue, we will need to set the +* Afterwards, we will tune the drive PID. Before we continue, we will need to set the `zeroPowerAccelerationMultiplier`. This determines how fast your robot will decelerate as a factor of how fast your robot will coast to a stop. Honestly, this is up to you. I personally used 4, but what works best for you is most important. Higher numbers will cause a faster brake, but increase oscillations at the end. Lower numbers will do the opposite. This can be found on line `101` in - `FollowerConstants`. There are once again two PIDs for the drive vector, but these PIDs are much, - much more sensitive than the others. For reference, my P values were in the hundredths and - thousandths place values, and my D values were in the hundred thousandths and millionths place - values. To tune this, enable `useDrive`, `useHeading`, and `useTranslational` in the `Follower` - dropdown in FTC Dashboard. Next, run `StraightBackAndForth`and don't forget to turn off the timer on - the OpMode. Then, tune the large PID and then the small PID following the tips from earlier. For - this, it is very important to try to reduce oscillations. Additionally, I would absolutely not - recommend using the I, or integral, part of the PID for this. Using integral in drivetrain PIDs is - already not ideal, but it will continuously build up error in this PID, causing major issues when - it gets too strong. So, just don't use it. P and D are enough. In the versions of Pedro Pathing - from after late July 2024, there is a Kalman filter on the drive error and the drive PIDs have a + `FollowerConstants`. The drive PID is much, much more sensitive than the others. For reference, + my P values were in the hundredths and thousandths place values, and my D values were in the hundred + thousandths and millionths place values. To tune this, enable `useDrive`, `useHeading`, and + `useTranslational` in the `Follower` dropdown in FTC Dashboard. Next, run `StraightBackAndForth` + and don't forget to turn off the timer on the OpMode. Then, tune the PID following the tips from + earlier. For this, it is very important to try to reduce oscillations. Additionally, I would + absolutely not recommend using the I, or integral, part of the PID for this. Using integral in + drivetrain PIDs is already not ideal, but it will continuously build up error in this PID, causing + major issues when it gets too strong. Don't use I; P and D are enough. In the versions of Pedro Pathing + from after late July 2024, there is a Kalman filter on the drive error and the drive PID has a filter as well. These smooth out the drive error and PID response so that there is not as much oscillation during the braking portion of each path. The Kalman filter is tuned to have 6 for the model covariance and 1 for the data covariance. These values should work, but if you feel inclined to tune the Kalman filter yourself, a higher ratio of model covariance to data covariance means that the filter will rely more on its previous output rather than the data, and the opposite ratio will mean that the filter will rely more so on the data input (the raw drive error) rather than the model. - The filtered PIDs function like normal PIDs, except the derivative term is a weighted average of the + The filtered PID functions like a normal PID, except the derivative term is a weighted average of the current derivative and the previous derivative. Currently, the weighting, or time constant for the - drive filtered PIDs is 0.6, so the derivative output is 0.6 times the previous derivative plus 0.4 + drive filtered PID is 0.6, so the derivative output is 0.6 times the previous derivative plus 0.4 times the current derivative. Feel free to mess around with these values and find what works best for your robot! @@ -99,4 +98,15 @@ One last thing to note is that Pedro Pathing operates in inches and radians. `StraightBackAndForth`, `CurvedBackAndForth`, or some paths of your own making. There's also `Circle`, but that's more so for fun than anything else. If you notice something could be improved, feel free to mess around more with your PIDs. That should be all! If you have any more questions, - refer to the OVERVIEW readme file or the README readme file. Best of luck to your team this season! :) \ No newline at end of file + refer to the OVERVIEW readme file or the README readme file. Best of luck to your team this season! :) + +## Note About the PIDs +In versions of Pedro Pathing before early August 2024, there were 2 PIDs used in the translational, +heading, and drive control. However, now there is only one main PID. The old system can still be used. +Scroll down to the bottom of `FollowerConstants` and set all the booleans from lines `151` to `153` +to true. This will enable the two PID system that Pedro Pathing originally used. From there, scroll +down and all the values pertaining to the secondary PIDs will be there. The two PID system works with +a PID that handles larger errors (the main PID) and a second PID to handle smaller errors (the +secondary PID). The main PID should be tuned to move the error within the secondary PID's range +without providing too much momentum that could cause an overshoot. The secondary PID should be tuned +to correct within its range quickly and accurately while minimizing oscillations. \ No newline at end of file From 2c28cda12177ca8749c26e6a30ad5a218e2620c6 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Fri, 9 Aug 2024 17:59:13 -0400 Subject: [PATCH 32/94] new path visualizer by team 16166 watt's up! --- .../firstinspires/ftc/teamcode/pedroPathing/TUNING.md | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md index 175a050..6b508f4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md @@ -5,8 +5,13 @@ You must also have a localizer of some sort. Pedro Pathing has a drive encoder, and a three tracking wheel localizer. You will need to have your localizer tuned before starting to tune PedroPathing. Check out the tuning guide under the localization tab if you're planning on using one of the localizers available in Pedro Pathing. Additionally, using [FTC Dashboard](http://192.168.43.1:8080/dash) -will help a lot in tuning, and we have a slightly scuffed Desmos path visualizer [here](https://www.desmos.com/calculator/3so1zx0hcd). -One last thing to note is that Pedro Pathing operates in inches and radians. +will help a lot in tuning. Team 16166 Watt'S Up made a path visualizer linked [here](https://pedro-path-generator.vercel.app). +The old Desmos visualizer is [here](https://www.desmos.com/calculator/3so1zx0hcd), but the one by +Watt'S Up is honestly a lot better. +One last thing to note is that Pedro Pathing operates in inches and radians. You can use centimeters +instead of inches, but you'll have to input all your measurement in centimeters, and any distances +that the tuners require you to push the robot or the tuners output will say "inches" when the actual +measurements will be in centimeters. ## Tuning * To start with, we need the mass of the robot in kg. This is used for the centripetal force correction, From 87f42407f7448e58a3122c934b51212718f3ffbe Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Sun, 11 Aug 2024 15:50:08 -0400 Subject: [PATCH 33/94] centripetal correction --- .../pedroPathing/follower/Follower.java | 82 ++++++++++--------- 1 file changed, 42 insertions(+), 40 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index 66728fd..f618418 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -433,54 +433,56 @@ public class Follower { } if (!teleopDrive) { - if (holdingPosition) { - closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1); + if (currentPath != null) { + if (holdingPosition) { + closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1); - drivePowers = driveVectorScaler.getDrivePowers(MathFunctions.scalarMultiplyVector(getTranslationalCorrection(), holdPointTranslationalScaling), MathFunctions.scalarMultiplyVector(getHeadingVector(), holdPointHeadingScaling), new Vector(), poseUpdater.getPose().getHeading()); - - limitDrivePowers(); - - for (int i = 0; i < motors.size(); i++) { - motors.get(i).setPower(drivePowers[i]); - } - } else { - if (isBusy) { - closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_BINARY_STEP_LIMIT); - - if (followingPathChain) updateCallbacks(); - - drivePowers = driveVectorScaler.getDrivePowers(getCorrectiveVector(), getHeadingVector(), getDriveVector(), poseUpdater.getPose().getHeading()); + drivePowers = driveVectorScaler.getDrivePowers(MathFunctions.scalarMultiplyVector(getTranslationalCorrection(), holdPointTranslationalScaling), MathFunctions.scalarMultiplyVector(getHeadingVector(), holdPointHeadingScaling), new Vector(), poseUpdater.getPose().getHeading()); limitDrivePowers(); for (int i = 0; i < motors.size(); i++) { motors.get(i).setPower(drivePowers[i]); } - } - if (currentPath.isAtParametricEnd()) { - if (followingPathChain && chainIndex < currentPathChain.size() - 1) { - // Not at last path, keep going - breakFollowing(); - pathStartTimes[chainIndex] = System.currentTimeMillis(); - isBusy = true; - followingPathChain = true; - chainIndex++; - currentPath = currentPathChain.getPath(chainIndex); + } else { + if (isBusy) { closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_BINARY_STEP_LIMIT); - } else { - // At last path, run some end detection stuff - // set isBusy to false if at end - if (!reachedParametricPathEnd) { - reachedParametricPathEnd = true; - reachedParametricPathEndTime = System.currentTimeMillis(); - } - if ((System.currentTimeMillis() - reachedParametricPathEndTime > currentPath.getPathEndTimeoutConstraint()) || (poseUpdater.getVelocity().getMagnitude() < currentPath.getPathEndVelocityConstraint() && MathFunctions.distance(poseUpdater.getPose(), closestPose) < currentPath.getPathEndTranslationalConstraint() && MathFunctions.getSmallestAngleDifference(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()) < currentPath.getPathEndHeadingConstraint())) { - if (holdPositionAtEnd) { - holdPositionAtEnd = false; - holdPoint(new BezierPoint(currentPath.getLastControlPoint()), currentPath.getHeadingGoal(1)); - } else { - breakFollowing(); + if (followingPathChain) updateCallbacks(); + + drivePowers = driveVectorScaler.getDrivePowers(getCorrectiveVector(), getHeadingVector(), getDriveVector(), poseUpdater.getPose().getHeading()); + + limitDrivePowers(); + + for (int i = 0; i < motors.size(); i++) { + motors.get(i).setPower(drivePowers[i]); + } + } + if (currentPath.isAtParametricEnd()) { + if (followingPathChain && chainIndex < currentPathChain.size() - 1) { + // Not at last path, keep going + breakFollowing(); + pathStartTimes[chainIndex] = System.currentTimeMillis(); + isBusy = true; + followingPathChain = true; + chainIndex++; + currentPath = currentPathChain.getPath(chainIndex); + closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_BINARY_STEP_LIMIT); + } else { + // At last path, run some end detection stuff + // set isBusy to false if at end + if (!reachedParametricPathEnd) { + reachedParametricPathEnd = true; + reachedParametricPathEndTime = System.currentTimeMillis(); + } + + if ((System.currentTimeMillis() - reachedParametricPathEndTime > currentPath.getPathEndTimeoutConstraint()) || (poseUpdater.getVelocity().getMagnitude() < currentPath.getPathEndVelocityConstraint() && MathFunctions.distance(poseUpdater.getPose(), closestPose) < currentPath.getPathEndTranslationalConstraint() && MathFunctions.getSmallestAngleDifference(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()) < currentPath.getPathEndHeadingConstraint())) { + if (holdPositionAtEnd) { + holdPositionAtEnd = false; + holdPoint(new BezierPoint(currentPath.getLastControlPoint()), currentPath.getHeadingGoal(1)); + } else { + breakFollowing(); + } } } } @@ -851,7 +853,7 @@ public class Follower { curvature = (yDoublePrime) / (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)); } if (Double.isNaN(curvature)) return new Vector(); - centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -1, 1), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta())); + centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * Math.abs(curvature), -1, 1), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta())); return centripetalVector; } From 13e503c730b31026405227c0c2a84edf3a42fb67 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Tue, 13 Aug 2024 01:04:21 -0400 Subject: [PATCH 34/94] centripetal correction oops --- .../ftc/teamcode/pedroPathing/follower/Follower.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index f618418..e345cba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -853,7 +853,7 @@ public class Follower { curvature = (yDoublePrime) / (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)); } if (Double.isNaN(curvature)) return new Vector(); - centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * Math.abs(curvature), -1, 1), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta())); + centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -1, 1), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta())); return centripetalVector; } From 93db7dcdfa381fd30f8cd527258ed789b999d480 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Tue, 27 Aug 2024 01:32:09 -0400 Subject: [PATCH 35/94] changing motor configuration is easier and readme updates --- .../ftc/teamcode/pedroPathing/LOCALIZATION.md | 5 ++-- .../ftc/teamcode/pedroPathing/README.md | 2 +- .../ftc/teamcode/pedroPathing/TUNING.md | 28 ++++++++++--------- .../examples/TeleOpEnhancements.java | 13 ++++++--- .../pedroPathing/follower/Follower.java | 12 +++++--- .../localizers/DriveEncoderLocalizer.java | 16 +++++++---- .../localization/tuning/LocalizationTest.java | 13 ++++++--- .../tuning/FollowerConstants.java | 6 ++++ .../tuning/ForwardVelocityTuner.java | 13 ++++++--- .../ForwardZeroPowerAccelerationTuner.java | 13 ++++++--- .../LateralZeroPowerAccelerationTuner.java | 13 ++++++--- .../tuning/StrafeVelocityTuner.java | 13 ++++++--- 12 files changed, 96 insertions(+), 51 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/LOCALIZATION.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/LOCALIZATION.md index 28ef56d..18d1206 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/LOCALIZATION.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/LOCALIZATION.md @@ -24,9 +24,8 @@ that applies to you and follow the directions there. # Drive Encoders * First, you'll need all of your drive motors to have encoders attached. -* Then, go to `DriveEncoderLocalizer.java`. Go to where it tells you to replace the current statements with your encoder ports in the constructor. -Replace the `deviceName` parameter with the name of the port that the encoder for each motor is connected -to. The names of the variables is where on the robot the corresponding motor should be. +* Then, go to `DriveEncoderLocalizer.java`. The motor names are already set, so you don't have to do + anything to change the encoder names there. * Then, reverse the direction of any encoders so that all encoders tick up when the robot is moving forward. * Now, you'll have to tune the multipliers. These convert your measurements from encoder ticks into inches or radians, essentially scaling your localizer so that your numbers are accurate to the real diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md index c1e8792..c7eea31 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md @@ -23,6 +23,6 @@ implementation 'com.acmerobotics.dashboard:dashboard:0.4.5' 4. Find the `build.gradle` file under the `teamcode` folder. 5. In this gradle file, add the following dependency: ``` -implementation 'com.fasterxml/jackson.core:jacson-databind:2.12.7' +implementation 'com.fasterxml/jackson.core:jackson-databind:2.12.7' implementation 'org.jetbrains.kotlin:kotlin-stdlib:1.4.21' ``` \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md index 6b508f4..bd22f46 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md @@ -15,8 +15,8 @@ measurements will be in centimeters. ## Tuning * To start with, we need the mass of the robot in kg. This is used for the centripetal force correction, - and the mass should be put on line `80` in the `FollowerConstants` class under the - `tuning` package. + and the mass, with the variable name `mass`, should be put on line `86` in the `FollowerConstants` + class under the `tuning` package. * Next, we need to find the preferred mecanum drive vectors. The rollers on mecanum wheels point at a 45 degree angle from the forward direction, but the actual direction the force is output is actually @@ -26,9 +26,9 @@ measurements will be in centimeters. Dashboard under the dropdown for each respective class, but higher distances work better. After the distance has finished running, the end velocity will be output to telemetry. The robot may continue to drift a little bit after the robot has finished running the distance, so make sure you have - plenty of room. Once you're done, put the velocity for the `Forward Velocity Tuner` on line `27` in - the `FollowerConstants` class, and the velocity for the `Strafe Velocity Tuner` on line `28` in the - `FollowerConstants` class. + plenty of room. Once you're done, put the velocity for the `Forward Velocity Tuner` on line `33` in + the `FollowerConstants` class, and the velocity for the `Strafe Velocity Tuner` on line `34` in the + `FollowerConstants` class. The variable names should be `xMovement` and `yMovement`, respectively. * The last set of automatic tuners you'll need to run are the zero power acceleration tuners. These find the rate at which your robot decelerates when power is cut from the drivetrain. This is used to @@ -41,9 +41,10 @@ measurements will be in centimeters. which point it will display the deceleration in telemetry. This robot will need to drift to a stop to properly work, and the higher the velocity the greater the drift distance, so make sure you have enough room. Once you're done, put the zero power acceleration for the - `Forward Zero Power Acceleration Tuner` on line `88` in the `FollowerConstants` class and the zero - power acceleration for the `Lateral Zero Power Acceleration Tuner` on line `92` in the - `FollowerConstants` class. + `Forward Zero Power Acceleration Tuner` on line `94` in the `FollowerConstants` class and the zero + power acceleration for the `Lateral Zero Power Acceleration Tuner` on line `98` in the + `FollowerConstants` class. The variable names should be `forwardZeroPowerAcceleration` and + `lateralZeroPowerAcceleration`, respectively. * After this, we will want to tune the translational PID. Go to FTC Dashboard and disable all but the `useTranslational` checkboxes under the `Follower` tab. Then, run `StraightBackAndForth`. Make @@ -69,8 +70,8 @@ measurements will be in centimeters. `zeroPowerAccelerationMultiplier`. This determines how fast your robot will decelerate as a factor of how fast your robot will coast to a stop. Honestly, this is up to you. I personally used 4, but what works best for you is most important. Higher numbers will cause a faster brake, but increase - oscillations at the end. Lower numbers will do the opposite. This can be found on line `101` in - `FollowerConstants`. The drive PID is much, much more sensitive than the others. For reference, + oscillations at the end. Lower numbers will do the opposite. This can be found on line `107` in + `FollowerConstants`, named `zeroPowerAccelerationMultiplier`. The drive PID is much, much more sensitive than the others. For reference, my P values were in the hundredths and thousandths place values, and my D values were in the hundred thousandths and millionths place values. To tune this, enable `useDrive`, `useHeading`, and `useTranslational` in the `Follower` dropdown in FTC Dashboard. Next, run `StraightBackAndForth` @@ -95,7 +96,7 @@ measurements will be in centimeters. * Finally, we will want to tune the centripetal force correction. This is a pretty simple tune. Open up FTC Dashboard and enable everything under the `Follower` tab. Then, run `CurvedBackAndForth` and turn off its timer. If you notice the robot is correcting towards the inside of the curve - as/after running a path, then increase `centripetalScaling`, which can be found on line `83` of + as/after running a path, then increase `centripetalScaling`, which can be found on line `89` of `FollowerConstants`. If the robot is correcting towards the outside of the curve, then decrease `centripetalScaling`. @@ -108,8 +109,9 @@ measurements will be in centimeters. ## Note About the PIDs In versions of Pedro Pathing before early August 2024, there were 2 PIDs used in the translational, heading, and drive control. However, now there is only one main PID. The old system can still be used. -Scroll down to the bottom of `FollowerConstants` and set all the booleans from lines `151` to `153` -to true. This will enable the two PID system that Pedro Pathing originally used. From there, scroll +Scroll down to the bottom of `FollowerConstants` and set all the booleans from lines `157` to `159` +to true. They should be named `useSecondaryTranslationalPID`, `useSecondaryHeadingPID`, and `useSecondaryDrivePID`. +This will enable the two PID system that Pedro Pathing originally used. From there, scroll down and all the values pertaining to the secondary PIDs will be there. The two PID system works with a PID that handles larger errors (the main PID) and a second PID to handle smaller errors (the secondary PID). The main PID should be tuned to move the error within the secondary PID's range diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java index e5872e5..9f35ef4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java @@ -1,5 +1,10 @@ package org.firstinspires.ftc.teamcode.pedroPathing.examples; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; + import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; @@ -32,10 +37,10 @@ public class TeleOpEnhancements extends OpMode { public void init() { follower = new Follower(hardwareMap); - leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); - leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); - rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); - rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName); + leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); + rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); + rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); leftRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index e345cba..8407732 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -5,6 +5,10 @@ import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstan import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.lateralZeroPowerAcceleration; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryDrivePIDFFeedForward; @@ -158,10 +162,10 @@ public class Follower { driveVectorScaler = new DriveVectorScaler(FollowerConstants.frontLeftVector); poseUpdater = new PoseUpdater(hardwareMap); - leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); - leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); - rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); - rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName); + leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); + rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); + rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); // TODO: Make sure that this is the direction your motors need to be reversed in. leftFront.setDirection(DcMotorSimple.Direction.REVERSE); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java index 0d06142..c414057 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java @@ -1,5 +1,10 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; + import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -20,7 +25,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; * @version 1.0, 4/2/2024 */ @Config -public class DriveEncoderLocalizer extends Localizer { // todo: make drive encoders work +public class DriveEncoderLocalizer extends Localizer { private HardwareMap hardwareMap; private Pose startPose; private Pose displacementPose; @@ -59,11 +64,10 @@ public class DriveEncoderLocalizer extends Localizer { // todo: make drive encod public DriveEncoderLocalizer(HardwareMap map, Pose setStartPose) { hardwareMap = map; - // TODO: replace these with your encoder ports - leftFront = new Encoder(hardwareMap.get(DcMotorEx.class, "leftFront")); - rightFront = new Encoder(hardwareMap.get(DcMotorEx.class, "rightFront")); - leftRear = new Encoder(hardwareMap.get(DcMotorEx.class, "leftRear")); - rightRear = new Encoder(hardwareMap.get(DcMotorEx.class, "rightRear")); + leftFront = new Encoder(hardwareMap.get(DcMotorEx.class, leftFrontMotorName)); + leftRear = new Encoder(hardwareMap.get(DcMotorEx.class, leftRearMotorName)); + rightRear = new Encoder(hardwareMap.get(DcMotorEx.class, rightRearMotorName)); + rightFront = new Encoder(hardwareMap.get(DcMotorEx.class, rightFrontMotorName)); // TODO: reverse any encoders necessary leftFront.setDirection(Encoder.REVERSE); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java index 103c0b2..32e0452 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java @@ -1,5 +1,10 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -48,10 +53,10 @@ public class LocalizationTest extends OpMode { dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); - leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); - leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); - rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); - rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); + leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName); + leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); + rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); + rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); leftFront.setDirection(DcMotorSimple.Direction.REVERSE); leftRear.setDirection(DcMotorSimple.Direction.REVERSE); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java index df04c33..af72609 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java @@ -22,6 +22,12 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.KalmanFilterParameters; @Config public class FollowerConstants { + // This section is for configuring your motors + public static String leftFrontMotorName = "leftFront"; + public static String leftRearMotorName = "leftRear"; + public static String rightFrontMotorName = "rightFront"; + public static String rightRearMotorName = "rightRear"; + // This section is for setting the actual drive vector for the front left wheel, if the robot // is facing a heading of 0 radians with the wheel centered at (0,0) private static double xMovement = 81.34056; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java index c2ce2ac..e97f78a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java @@ -1,5 +1,10 @@ package org.firstinspires.ftc.teamcode.pedroPathing.tuning; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -62,10 +67,10 @@ public class ForwardVelocityTuner extends OpMode { public void init() { poseUpdater = new PoseUpdater(hardwareMap); - leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); - leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); - rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); - rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName); + leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); + rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); + rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); // TODO: Make sure that this is the direction your motors need to be reversed in. leftFront.setDirection(DcMotorSimple.Direction.REVERSE); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java index c6d212e..9454e13 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java @@ -1,5 +1,10 @@ package org.firstinspires.ftc.teamcode.pedroPathing.tuning; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -65,10 +70,10 @@ public class ForwardZeroPowerAccelerationTuner extends OpMode { public void init() { poseUpdater = new PoseUpdater(hardwareMap); - leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); - leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); - rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); - rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName); + leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); + rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); + rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); // TODO: Make sure that this is the direction your motors need to be reversed in. leftFront.setDirection(DcMotorSimple.Direction.REVERSE); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java index 0c22b9b..505245d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java @@ -1,5 +1,10 @@ package org.firstinspires.ftc.teamcode.pedroPathing.tuning; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -65,10 +70,10 @@ public class LateralZeroPowerAccelerationTuner extends OpMode { public void init() { poseUpdater = new PoseUpdater(hardwareMap); - leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); - leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); - rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); - rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName); + leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); + rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); + rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); // TODO: Make sure that this is the direction your motors need to be reversed in. leftFront.setDirection(DcMotorSimple.Direction.REVERSE); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java index aedccbd..acd90c0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java @@ -1,5 +1,10 @@ package org.firstinspires.ftc.teamcode.pedroPathing.tuning; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -62,10 +67,10 @@ public class StrafeVelocityTuner extends OpMode { public void init() { poseUpdater = new PoseUpdater(hardwareMap); - leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); - leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); - rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); - rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName); + leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); + rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); + rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); // TODO: Make sure that this is the direction your motors need to be reversed in. leftFront.setDirection(DcMotorSimple.Direction.REVERSE); From 94bad19674087dd89c6807b1ce70f5ee99127e34 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Tue, 27 Aug 2024 10:05:53 -0400 Subject: [PATCH 36/94] made the robot odo pod diagram more readable --- .../localization/localizers/OTOSLocalizer.java | 15 +++++++++------ .../localizers/ThreeWheelIMULocalizer.java | 15 +++++++++------ .../localizers/ThreeWheelLocalizer.java | 15 +++++++++------ .../localizers/TwoWheelLocalizer.java | 17 ++++++++++------- 4 files changed, 37 insertions(+), 25 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java index 13f63a9..6b11ce2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java @@ -12,24 +12,27 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; /** * This is the OTOSLocalizer class. This class extends the Localizer superclass and is a - * localizer that uses the SparkFun OTOS. The diagram below, which is taken from + * localizer that uses the SparkFun OTOS. The diagram below, which is modified from * Road Runner, shows a typical set up. * - * The view is from the bottom of the robot looking upwards. + * The view is from the top of the robot looking downwards. * - * left on robot is y pos + * left on robot is the y positive direction * - * front on robot is x pos + * forward on robot is the x positive direction * * /--------------\ * | ____ | * | ---- | * | || || | - * | || || | left (y pos) + * | || || | ----> left (y positive) * | | * | | * \--------------/ - * front (x pos) + * | + * | + * V + * forward (x positive) * * @author Anyi Lin - 10158 Scott's Bots * @version 1.0, 7/20/2024 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java index 4760e1c..44a03c6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java @@ -18,23 +18,26 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; /** * This is the ThreeWheelIMULocalizer class. This class extends the Localizer superclass and is a * localizer that uses the three wheel odometry set up with the IMU to have more accurate heading - * readings. The diagram below, which is taken from Road Runner, shows a typical set up. + * readings. The diagram below, which is modified from Road Runner, shows a typical set up. * - * The view is from the bottom of the robot looking upwards. + * The view is from the top of the robot looking downwards. * - * left on robot is y pos + * left on robot is the y positive direction * - * front on robot is x pos + * forward on robot is the x positive direction * * /--------------\ * | ____ | * | ---- | * | || || | - * | || || | left (y pos) + * | || || | ----> left (y positive) * | | * | | * \--------------/ - * front (x pos) + * | + * | + * V + * forward (x positive) * * @author Logan Nash * @author Anyi Lin - 10158 Scott's Bots diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java index 494d07d..a955946 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java @@ -14,24 +14,27 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; /** * This is the ThreeWheelLocalizer class. This class extends the Localizer superclass and is a - * localizer that uses the three wheel odometry set up. The diagram below, which is taken from + * localizer that uses the three wheel odometry set up. The diagram below, which is modified from * Road Runner, shows a typical set up. * - * The view is from the bottom of the robot looking upwards. + * The view is from the top of the robot looking downwards. * - * left on robot is y pos + * left on robot is the y positive direction * - * front on robot is x pos + * forward on robot is the x positive direction * * /--------------\ * | ____ | * | ---- | * | || || | - * | || || | left (y pos) + * | || || | ----> left (y positive) * | | * | | * \--------------/ - * front (x pos) + * | + * | + * V + * forward (x positive) * * @author Anyi Lin - 10158 Scott's Bots * @version 1.0, 4/2/2024 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java index e93b496..aa40a8d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java @@ -17,24 +17,27 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; /** * This is the TwoWheelLocalizer class. This class extends the Localizer superclass and is a - * localizer that uses the two wheel odometry with IMU set up. The diagram below, which is taken from + * localizer that uses the two wheel odometry with IMU set up. The diagram below, which is modified from * Road Runner, shows a typical set up. * - * The view is from the bottom of the robot looking upwards. + * The view is from the top of the robot looking downwards. * - * left on robot is y pos + * left on robot is the y positive direction * - * front on robot is x pos + * forward on robot is the x positive direction * * /--------------\ * | ____ | * | ---- | - * | || | - * | || | left (y pos) + * | || || | + * | || || | ----> left (y positive) * | | * | | * \--------------/ - * front (x pos) + * | + * | + * V + * forward (x positive) * * @author Anyi Lin - 10158 Scott's Bots * @version 1.0, 4/2/2024 From 74422baed43cb923063e53406d2269a4a72c8636 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Tue, 27 Aug 2024 20:06:12 -0400 Subject: [PATCH 37/94] oopsie --- .../java/org/firstinspires/ftc/teamcode/pedroPathing/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md index c7eea31..a398ea4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md @@ -23,6 +23,6 @@ implementation 'com.acmerobotics.dashboard:dashboard:0.4.5' 4. Find the `build.gradle` file under the `teamcode` folder. 5. In this gradle file, add the following dependency: ``` -implementation 'com.fasterxml/jackson.core:jackson-databind:2.12.7' +implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7' implementation 'org.jetbrains.kotlin:kotlin-stdlib:1.4.21' ``` \ No newline at end of file From 425c001a1b8d3036a4c26b592760c76d57af8721 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Tue, 27 Aug 2024 20:24:23 -0400 Subject: [PATCH 38/94] github name update --- .../org/firstinspires/ftc/teamcode/pedroPathing/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md index a398ea4..0b23ad5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md @@ -3,11 +3,11 @@ This is the Pedro Pathing path following program developed by FTC team 10158 Sco Nash in the 2023-2024 Centerstage season. ## Installation -The quickest way to get started is with the quickstart [here](https://github.com/brotherhobo/Pedro-Pathing-Quickstart). +The quickest way to get started is with the quickstart [here](https://github.com/AnyiLin/Pedro-Pathing-Quickstart). Otherwise, take the `pedroPathing` folder and put it under the `teamcode` folder in your project. You can do this from either downloading the project from the above quickstart link or the 10158 -CENTERSTAGE repository [here](https://github.com/brotherhobo/10158-Centerstage). +CENTERSTAGE repository [here](https://github.com/AnyiLin/10158-Centerstage). For this version of Pedro Pathing, the localizer used is the Road Runner localizer. To install its dependencies: From 7158b61d94c5ac0a7e2cf6596967c67cdd22add0 Mon Sep 17 00:00:00 2001 From: Cal Kestis Date: Wed, 28 Aug 2024 12:49:37 -0700 Subject: [PATCH 39/94] FtcRobotController v10.0 --- .../src/main/AndroidManifest.xml | 4 +- .../samples/BasicOmniOpMode_Linear.java | 2 +- .../samples/BasicOpMode_Iterative.java | 6 +- .../external/samples/BasicOpMode_Linear.java | 2 +- .../external/samples/ConceptAprilTag.java | 2 +- .../external/samples/ConceptAprilTagEasy.java | 2 +- .../samples/ConceptAprilTagLocalization.java | 244 ++++++++++++++++++ .../ConceptAprilTagOptimizeExposure.java | 2 +- .../ConceptAprilTagSwitchableCameras.java | 2 +- .../samples/ConceptExternalHardwareClass.java | 2 +- .../external/samples/ConceptLEDStick.java | 123 +++++++++ .../samples/ConceptMotorBulkRead.java | 2 +- .../external/samples/ConceptNullOp.java | 6 +- .../external/samples/ConceptRevLED.java | 78 ++++++ .../external/samples/ConceptRevSPARKMini.java | 10 +- .../external/samples/ConceptSoundsASJava.java | 2 +- .../RobotAutoDriveByEncoder_Linear.java | 2 +- .../samples/RobotAutoDriveByGyro_Linear.java | 12 +- .../samples/RobotAutoDriveByTime_Linear.java | 4 +- .../samples/RobotAutoDriveToAprilTagOmni.java | 14 +- .../samples/RobotAutoDriveToAprilTagTank.java | 10 +- .../samples/RobotAutoDriveToLine_Linear.java | 2 +- .../samples/RobotTeleopPOV_Linear.java | 4 +- .../samples/RobotTeleopTank_Iterative.java | 8 +- .../external/samples/SensorHuskyLens.java | 17 +- .../samples/SensorIMUNonOrthogonal.java | 7 +- .../external/samples/SensorIMUOrthogonal.java | 5 +- .../external/samples/SensorLimelight3A.java | 159 ++++++++++++ .../external/samples/SensorOctoQuad.java | 22 +- .../external/samples/SensorOctoQuadAdv.java | 6 +- .../samples/UtilityOctoQuadConfigMenu.java | 6 +- README.md | 48 ++++ TeamCode/build.gradle | 1 - build.common.gradle | 4 - build.dependencies.gradle | 20 +- 35 files changed, 750 insertions(+), 90 deletions(-) create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptLEDStick.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevLED.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java diff --git a/FtcRobotController/src/main/AndroidManifest.xml b/FtcRobotController/src/main/AndroidManifest.xml index b5a63b1..46149bd 100644 --- a/FtcRobotController/src/main/AndroidManifest.xml +++ b/FtcRobotController/src/main/AndroidManifest.xml @@ -1,8 +1,8 @@ + android:versionCode="55" + android:versionName="10.0"> diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java index 940b527..1d14dfb 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java @@ -99,7 +99,7 @@ public class BasicOmniOpMode_Linear extends LinearOpMode { rightFrontDrive.setDirection(DcMotor.Direction.FORWARD); rightBackDrive.setDirection(DcMotor.Direction.FORWARD); - // Wait for the game to start (driver presses PLAY) + // Wait for the game to start (driver presses START) telemetry.addData("Status", "Initialized"); telemetry.update(); diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Iterative.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Iterative.java index 7c10408..6504e58 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Iterative.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Iterative.java @@ -83,14 +83,14 @@ public class BasicOpMode_Iterative extends OpMode } /* - * Code to run REPEATEDLY after the driver hits INIT, but before they hit PLAY + * Code to run REPEATEDLY after the driver hits INIT, but before they hit START */ @Override public void init_loop() { } /* - * Code to run ONCE when the driver hits PLAY + * Code to run ONCE when the driver hits START */ @Override public void start() { @@ -98,7 +98,7 @@ public class BasicOpMode_Iterative extends OpMode } /* - * Code to run REPEATEDLY after the driver hits PLAY but before they hit STOP + * Code to run REPEATEDLY after the driver hits START but before they hit STOP */ @Override public void loop() { diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Linear.java index 9fe0bb6..ab0bb25 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Linear.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Linear.java @@ -76,7 +76,7 @@ public class BasicOpMode_Linear extends LinearOpMode { leftDrive.setDirection(DcMotor.Direction.REVERSE); rightDrive.setDirection(DcMotor.Direction.FORWARD); - // Wait for the game to start (driver presses PLAY) + // Wait for the game to start (driver presses START) waitForStart(); runtime.reset(); diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java index b6aa4e5..8ec77dd 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java @@ -88,7 +88,7 @@ public class ConceptAprilTag extends LinearOpMode { // Wait for the DS start button to be touched. telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); - telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.addData(">", "Touch START to start OpMode"); telemetry.update(); waitForStart(); diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java index d305d55..12dcf6e 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java @@ -84,7 +84,7 @@ public class ConceptAprilTagEasy extends LinearOpMode { // Wait for the DS start button to be touched. telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); - telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.addData(">", "Touch START to start OpMode"); telemetry.update(); waitForStart(); diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java new file mode 100644 index 0000000..05318ba --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java @@ -0,0 +1,244 @@ +/* Copyright (c) 2024 Dryw Wade. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; + +/* + * This OpMode illustrates the basics of AprilTag based localization. + * + * For an introduction to AprilTags, see the FTC-DOCS link below: + * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html + * + * In this sample, any visible tag ID will be detected and displayed, but only tags that are included in the default + * "TagLibrary" will be used to compute the robot's location and orientation. This default TagLibrary contains + * the current Season's AprilTags and a small set of "test Tags" in the high number range. + * + * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the robot, relative to the field origin. + * This information is provided in the "robotPose" member of the returned "detection". + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Concept: AprilTag Localization", group = "Concept") +@Disabled +public class ConceptAprilTagLocalization extends LinearOpMode { + + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + /** + * Variables to store the position and orientation of the camera on the robot. Setting these + * values requires a definition of the axes of the camera and robot: + * + * Camera axes: + * Origin location: Center of the lens + * Axes orientation: +x right, +y down, +z forward (from camera's perspective) + * + * Robot axes (this is typical, but you can define this however you want): + * Origin location: Center of the robot at field height + * Axes orientation: +x right, +y forward, +z upward + * + * Position: + * If all values are zero (no translation), that implies the camera is at the center of the + * robot. Suppose your camera is positioned 5 inches to the left, 7 inches forward, and 12 + * inches above the ground - you would need to set the position to (-5, 7, 12). + * + * Orientation: + * If all values are zero (no rotation), that implies the camera is pointing straight up. In + * most cases, you'll need to set the pitch to -90 degrees (rotation about the x-axis), meaning + * the camera is horizontal. Use a yaw of 0 if the camera is pointing forwards, +90 degrees if + * it's pointing straight left, -90 degrees for straight right, etc. You can also set the roll + * to +/-90 degrees if it's vertical, or 180 degrees if it's upside-down. + */ + private Position cameraPosition = new Position(DistanceUnit.INCH, + 0, 0, 0, 0); + private YawPitchRollAngles cameraOrientation = new YawPitchRollAngles(AngleUnit.DEGREES, + 0, -90, 0, 0); + + /** + * The variable to store our instance of the AprilTag processor. + */ + private AprilTagProcessor aprilTag; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + @Override + public void runOpMode() { + + initAprilTag(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch START to start OpMode"); + telemetry.update(); + waitForStart(); + + while (opModeIsActive()) { + + telemetryAprilTag(); + + // Push telemetry to the Driver Station. + telemetry.update(); + + // Save CPU resources; can resume streaming when needed. + if (gamepad1.dpad_down) { + visionPortal.stopStreaming(); + } else if (gamepad1.dpad_up) { + visionPortal.resumeStreaming(); + } + + // Share the CPU. + sleep(20); + } + + // Save more CPU resources when camera is no longer needed. + visionPortal.close(); + + } // end method runOpMode() + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + + // Create the AprilTag processor. + aprilTag = new AprilTagProcessor.Builder() + + // The following default settings are available to un-comment and edit as needed. + //.setDrawAxes(false) + //.setDrawCubeProjection(false) + //.setDrawTagOutline(true) + //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) + //.setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary()) + //.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) + .setCameraPose(cameraPosition, cameraOrientation) + + // == CAMERA CALIBRATION == + // If you do not manually specify calibration parameters, the SDK will attempt + // to load a predefined calibration for your camera. + //.setLensIntrinsics(578.272, 578.272, 402.145, 221.506) + // ... these parameters are fx, fy, cx, cy. + + .build(); + + // Adjust Image Decimation to trade-off detection-range for detection-rate. + // eg: Some typical detection data using a Logitech C920 WebCam + // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second + // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second + // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second (default) + // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second (default) + // Note: Decimation can be changed on-the-fly to adapt during a match. + //aprilTag.setDecimation(3); + + // Create the vision portal by using a builder. + VisionPortal.Builder builder = new VisionPortal.Builder(); + + // Set the camera (webcam vs. built-in RC phone camera). + if (USE_WEBCAM) { + builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")); + } else { + builder.setCamera(BuiltinCameraDirection.BACK); + } + + // Choose a camera resolution. Not all cameras support all resolutions. + //builder.setCameraResolution(new Size(640, 480)); + + // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. + //builder.enableLiveView(true); + + // Set the stream format; MJPEG uses less bandwidth than default YUY2. + //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); + + // Choose whether or not LiveView stops if no processors are enabled. + // If set "true", monitor shows solid orange screen if no processors enabled. + // If set "false", monitor shows camera view without annotations. + //builder.setAutoStopLiveView(false); + + // Set and enable the processor. + builder.addProcessor(aprilTag); + + // Build the Vision Portal, using the above settings. + visionPortal = builder.build(); + + // Disable or re-enable the aprilTag processor at any time. + //visionPortal.setProcessorEnabled(aprilTag, true); + + } // end method initAprilTag() + + /** + * Add telemetry about AprilTag detections. + */ + private void telemetryAprilTag() { + + List currentDetections = aprilTag.getDetections(); + telemetry.addData("# AprilTags Detected", currentDetections.size()); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", + detection.robotPose.getPosition().x, + detection.robotPose.getPosition().y, + detection.robotPose.getPosition().z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", + detection.robotPose.getOrientation().getPitch(AngleUnit.DEGREES), + detection.robotPose.getOrientation().getRoll(AngleUnit.DEGREES), + detection.robotPose.getOrientation().getYaw(AngleUnit.DEGREES))); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + } // end for() loop + + // Add "key" information to telemetry + telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); + telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); + + } // end method telemetryAprilTag() + +} // end class diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagOptimizeExposure.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagOptimizeExposure.java index 3792992..adee952 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagOptimizeExposure.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagOptimizeExposure.java @@ -98,7 +98,7 @@ public class ConceptAprilTagOptimizeExposure extends LinearOpMode // Wait for the match to begin. telemetry.addData("Camera preview on/off", "3 dots, Camera Stream"); - telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.addData(">", "Touch START to start OpMode"); telemetry.update(); waitForStart(); diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java index 9285224..7ee1064 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java @@ -77,7 +77,7 @@ public class ConceptAprilTagSwitchableCameras extends LinearOpMode { // Wait for the DS start button to be touched. telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); - telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.addData(">", "Touch START to start OpMode"); telemetry.update(); waitForStart(); diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExternalHardwareClass.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExternalHardwareClass.java index 2fe4154..7a721fe 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExternalHardwareClass.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExternalHardwareClass.java @@ -83,7 +83,7 @@ public class ConceptExternalHardwareClass extends LinearOpMode { robot.init(); // Send telemetry message to signify robot waiting; - // Wait for the game to start (driver presses PLAY) + // Wait for the game to start (driver presses START) waitForStart(); // run until the end of the match (driver presses STOP) diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptLEDStick.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptLEDStick.java new file mode 100644 index 0000000..01729bb --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptLEDStick.java @@ -0,0 +1,123 @@ +package org.firstinspires.ftc.robotcontroller.external.samples; +/* + Copyright (c) 2021-24 Alan Smith + + All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, + are permitted (subject to the limitations in the disclaimer below) provided that + the following conditions are met: + + Redistributions of source code must retain the above copyright notice, this list + of conditions and the following disclaimer. + + Redistributions in binary form must reproduce the above copyright notice, this + list of conditions and the following disclaimer in the documentation and/or + other materials provided with the distribution. + + Neither the name of Alan Smith nor the names of its contributors may be used to + endorse or promote products derived from this software without specific prior + written permission. + + NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESSFOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR + TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +import android.graphics.Color; + +import com.qualcomm.hardware.sparkfun.SparkFunLEDStick; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.Range; + +/* + * This OpMode illustrates how to use the SparkFun QWIIC LED Strip + * + * This is a simple way to add a strip of 10 LEDs to your robot where you can set the color of each + * LED or the whole strip. This allows for driver feedback or even just fun ways to show your team + * colors. + * + * Why? + * Because more LEDs == more fun!! + * + * This OpMode assumes that the QWIIC LED Stick is attached to an I2C interface named "back_leds" in the robot configuration. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * You can buy this product here: https://www.sparkfun.com/products/18354 + * Don't forget to also buy this to make it easy to connect to your Control or Expansion Hub: + * https://www.sparkfun.com/products/25596 + */ +@TeleOp(name = "Concept: LED Stick", group = "Concept") +@Disabled +public class ConceptLEDStick extends OpMode { + private boolean wasUp; + private boolean wasDown; + private int brightness = 5; // this needs to be between 0 and 31 + private final static double END_GAME_TIME = 120 - 30; + + private SparkFunLEDStick ledStick; + + @Override + public void init() { + ledStick = hardwareMap.get(SparkFunLEDStick.class, "back_leds"); + ledStick.setBrightness(brightness); + ledStick.setColor(Color.GREEN); + } + + @Override + public void start() { + resetRuntime(); + } + + @Override + public void loop() { + telemetry.addLine("Hold the A button to turn blue"); + telemetry.addLine("Hold the B button to turn red"); + telemetry.addLine("Hold the left bumper to turn off"); + telemetry.addLine("Use DPAD Up/Down to change brightness"); + + if (getRuntime() > END_GAME_TIME) { + int[] ledColors = {Color.RED, Color.YELLOW, Color.RED, Color.YELLOW, Color.RED, + Color.YELLOW, Color.RED, Color.YELLOW, Color.RED, Color.YELLOW}; + ledStick.setColors(ledColors); + } else if (gamepad1.a) { + ledStick.setColor(Color.BLUE); + } else if (gamepad1.b) { + ledStick.setColor(Color.RED); + } else if (gamepad1.left_bumper) { + ledStick.turnAllOff(); + } else { + ledStick.setColor(Color.GREEN); + } + + /* + * Use DPAD up and down to change brightness + */ + int newBrightness = brightness; + if (gamepad1.dpad_up && !wasUp) { + newBrightness = brightness + 1; + } else if (gamepad1.dpad_down && !wasDown) { + newBrightness = brightness - 1; + } + if (newBrightness != brightness) { + brightness = Range.clip(newBrightness, 0, 31); + ledStick.setBrightness(brightness); + } + telemetry.addData("Brightness", brightness); + + wasDown = gamepad1.dpad_down; + wasUp = gamepad1.dpad_up; + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java index 3bade8b..5439f78 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java @@ -110,7 +110,7 @@ public class ConceptMotorBulkRead extends LinearOpMode { ElapsedTime timer = new ElapsedTime(); - telemetry.addData(">", "Press play to start tests"); + telemetry.addData(">", "Press START to start tests"); telemetry.addData(">", "Test results will update for each access method."); telemetry.update(); waitForStart(); diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java index 7ea4683..4a887bd 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java @@ -53,7 +53,7 @@ public class ConceptNullOp extends OpMode { /** * This method will be called repeatedly during the period between when - * the init button is pressed and when the play button is pressed (or the + * the INIT button is pressed and when the START button is pressed (or the * OpMode is stopped). */ @Override @@ -61,7 +61,7 @@ public class ConceptNullOp extends OpMode { } /** - * This method will be called once, when the play button is pressed. + * This method will be called once, when the START button is pressed. */ @Override public void start() { @@ -70,7 +70,7 @@ public class ConceptNullOp extends OpMode { /** * This method will be called repeatedly during the period between when - * the play button is pressed and when the OpMode is stopped. + * the START button is pressed and when the OpMode is stopped. */ @Override public void loop() { diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevLED.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevLED.java new file mode 100644 index 0000000..9c168d5 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevLED.java @@ -0,0 +1,78 @@ +/* + Copyright (c) 2024 Alan Smith + All rights reserved. + Redistribution and use in source and binary forms, with or without modification, + are permitted (subject to the limitations in the disclaimer below) provided that + the following conditions are met: + Redistributions of source code must retain the above copyright notice, this list + of conditions and the following disclaimer. + Redistributions in binary form must reproduce the above copyright notice, this + list of conditions and the following disclaimer in the documentation and/or + other materials provided with the distribution. + Neither the name of Alan Smith nor the names of its contributors may be used to + endorse or promote products derived from this software without specific prior + written permission. + NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESSFOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR + TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcontroller.external.samples; + +/* + * This OpMode illustrates how to use the REV Digital Indicator + * + * This is a simple way to add the REV Digital Indicator which has a red and green LED. + * (and as you may remember, red + green = yellow so when they are both on you can get yellow) + * + * Why? + * You can use this to show information to the driver. For example, green might be that you can + * pick up more game elements and red would be that you already have the possession limit. + * + * This OpMode assumes that the REV Digital Indicator is setup as 2 Digital Channels named + * front_led_green and front_led_red. (the green should be the lower of the 2 channels it is plugged + * into and the red should be the higher) + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * You can buy this product here: https://www.revrobotics.com/rev-31-2010/ + */ +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.LED; + +@TeleOp(name = "Concept: RevLED", group = "Concept") +@Disabled +public class ConceptRevLED extends OpMode { + LED frontLED_red; + LED frontLED_green; + + @Override + public void init() { + frontLED_green = hardwareMap.get(LED.class, "front_led_green"); + frontLED_red = hardwareMap.get(LED.class, "front_led_red"); + } + + @Override + public void loop() { + if (gamepad1.a) { + frontLED_red.on(); + } else { + frontLED_red.off(); + } + if (gamepad1.b) { + frontLED_green.on(); + } else { + frontLED_green.off(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevSPARKMini.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevSPARKMini.java index e710662..798d689 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevSPARKMini.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevSPARKMini.java @@ -38,8 +38,9 @@ import com.qualcomm.robotcore.util.Range; /* - * This OpMode executes a basic Tank Drive Teleop for a two wheeled robot using two REV SPARKminis. - * To use this example, connect two REV SPARKminis into servo ports on the Expansion Hub. On the + * This OpMode demonstrates a POV Drive system, with commented-out code for a Tank Drive system, + * for a two wheeled robot using two REV SPARKminis. + * To use this example, connect two REV SPARKminis into servo ports on the Control Hub. On the * robot configuration, use the drop down list under 'Servos' to select 'REV SPARKmini Controller' * and name them 'left_drive' and 'right_drive'. * @@ -62,8 +63,7 @@ public class ConceptRevSPARKMini extends LinearOpMode { telemetry.update(); // Initialize the hardware variables. Note that the strings used here as parameters - // to 'get' must correspond to the names assigned during the robot configuration - // step (using the FTC Robot Controller app on the phone). + // to 'get' must correspond to the names assigned during robot configuration. leftDrive = hardwareMap.get(DcMotorSimple.class, "left_drive"); rightDrive = hardwareMap.get(DcMotorSimple.class, "right_drive"); @@ -72,7 +72,7 @@ public class ConceptRevSPARKMini extends LinearOpMode { leftDrive.setDirection(DcMotorSimple.Direction.FORWARD); rightDrive.setDirection(DcMotorSimple.Direction.REVERSE); - // Wait for the game to start (driver presses PLAY) + // Wait for the game to start (driver presses START) waitForStart(); runtime.reset(); diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsASJava.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsASJava.java index f0a855f..1ceaa17 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsASJava.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsASJava.java @@ -100,7 +100,7 @@ public class ConceptSoundsASJava extends LinearOpMode { telemetry.addData("gold resource", goldFound ? "Found" : "NOT found\n Add gold.wav to /src/main/res/raw" ); telemetry.addData("silver resource", silverFound ? "Found" : "Not found\n Add silver.wav to /src/main/res/raw" ); - // Wait for the game to start (driver presses PLAY) + // Wait for the game to start (driver presses START) telemetry.addData(">", "Press Start to continue"); telemetry.update(); waitForStart(); diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByEncoder_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByEncoder_Linear.java index be2e218..63293d0 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByEncoder_Linear.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByEncoder_Linear.java @@ -110,7 +110,7 @@ public class RobotAutoDriveByEncoder_Linear extends LinearOpMode { rightDrive.getCurrentPosition()); telemetry.update(); - // Wait for the game to start (driver presses PLAY) + // Wait for the game to start (driver presses START) waitForStart(); // Step through each leg of the path, diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByGyro_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByGyro_Linear.java index 7885fe4..ab70934 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByGyro_Linear.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByGyro_Linear.java @@ -65,7 +65,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; * Notes: * * All angles are referenced to the coordinate-frame that is set whenever resetHeading() is called. - * In this sample, the heading is reset when the Start button is touched on the Driver station. + * In this sample, the heading is reset when the Start button is touched on the Driver Station. * Note: It would be possible to reset the heading after each move, but this would accumulate steering errors. * * The angle of movement/rotation is assumed to be a standardized rotation around the robot Z axis, @@ -124,15 +124,15 @@ public class RobotAutoDriveByGyro_Linear extends LinearOpMode { // These constants define the desired driving/control characteristics // They can/should be tweaked to suit the specific robot drive train. static final double DRIVE_SPEED = 0.4; // Max driving speed for better distance accuracy. - static final double TURN_SPEED = 0.2; // Max Turn speed to limit turn rate + static final double TURN_SPEED = 0.2; // Max turn speed to limit turn rate. static final double HEADING_THRESHOLD = 1.0 ; // How close must the heading get to the target before moving to next step. // Requiring more accuracy (a smaller number) will often make the turn take longer to get into the final position. // Define the Proportional control coefficient (or GAIN) for "heading control". // We define one value when Turning (larger errors), and the other is used when Driving straight (smaller errors). - // Increase these numbers if the heading does not corrects strongly enough (eg: a heavy robot or using tracks) + // Increase these numbers if the heading does not correct strongly enough (eg: a heavy robot or using tracks) // Decrease these numbers if the heading does not settle on the correct value (eg: very agile robot with omni wheels) - static final double P_TURN_GAIN = 0.02; // Larger is more responsive, but also less stable - static final double P_DRIVE_GAIN = 0.03; // Larger is more responsive, but also less stable + static final double P_TURN_GAIN = 0.02; // Larger is more responsive, but also less stable. + static final double P_DRIVE_GAIN = 0.03; // Larger is more responsive, but also less stable. @Override @@ -317,7 +317,7 @@ public class RobotAutoDriveByGyro_Linear extends LinearOpMode { *

* Move will stop once the requested time has elapsed *

- * This function is useful for giving the robot a moment to stabilize it's heading between movements. + * This function is useful for giving the robot a moment to stabilize its heading between movements. * * @param maxTurnSpeed Maximum differential turn speed (range 0 to +1.0) * @param heading Absolute Heading Angle (in Degrees) relative to last gyro reset. diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByTime_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByTime_Linear.java index b449258..a714748 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByTime_Linear.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByTime_Linear.java @@ -85,10 +85,10 @@ public class RobotAutoDriveByTime_Linear extends LinearOpMode { telemetry.addData("Status", "Ready to run"); // telemetry.update(); - // Wait for the game to start (driver presses PLAY) + // Wait for the game to start (driver presses START) waitForStart(); - // Step through each leg of the path, ensuring that the Auto mode has not been stopped along the way + // Step through each leg of the path, ensuring that the OpMode has not been stopped along the way. // Step 1: Drive forward for 3 seconds leftDrive.setPower(FORWARD_SPEED); diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java index 21def8a..9bac006 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java @@ -64,7 +64,7 @@ import java.util.concurrent.TimeUnit; * The code assumes a Robot Configuration with motors named: leftfront_drive and rightfront_drive, leftback_drive and rightback_drive. * The motor directions must be set so a positive power goes forward on all wheels. * This sample assumes that the current game AprilTag Library (usually for the current season) is being loaded by default, - * so you should choose to approach a valid tag ID (usually starting at 0) + * so you should choose to approach a valid tag ID. * * Under manual control, the left stick will move forward/back & left/right. The right stick will rotate the robot. * Manually drive the robot until it displays Target data on the Driver Station. @@ -95,12 +95,12 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode // Set the GAIN constants to control the relationship between the measured position error, and how much power is // applied to the drive motors to correct the error. // Drive = Error * Gain Make these values smaller for smoother control, or larger for a more aggressive response. - final double SPEED_GAIN = 0.02 ; // Forward Speed Control "Gain". eg: Ramp up to 50% power at a 25 inch error. (0.50 / 25.0) - final double STRAFE_GAIN = 0.015 ; // Strafe Speed Control "Gain". eg: Ramp up to 25% power at a 25 degree Yaw error. (0.25 / 25.0) - final double TURN_GAIN = 0.01 ; // Turn Control "Gain". eg: Ramp up to 25% power at a 25 degree error. (0.25 / 25.0) + final double SPEED_GAIN = 0.02 ; // Forward Speed Control "Gain". e.g. Ramp up to 50% power at a 25 inch error. (0.50 / 25.0) + final double STRAFE_GAIN = 0.015 ; // Strafe Speed Control "Gain". e.g. Ramp up to 37% power at a 25 degree Yaw error. (0.375 / 25.0) + final double TURN_GAIN = 0.01 ; // Turn Control "Gain". e.g. Ramp up to 25% power at a 25 degree error. (0.25 / 25.0) final double MAX_AUTO_SPEED = 0.5; // Clip the approach speed to this max value (adjust for your robot) - final double MAX_AUTO_STRAFE= 0.5; // Clip the approach speed to this max value (adjust for your robot) + final double MAX_AUTO_STRAFE= 0.5; // Clip the strafing speed to this max value (adjust for your robot) final double MAX_AUTO_TURN = 0.3; // Clip the turn speed to this max value (adjust for your robot) private DcMotor leftFrontDrive = null; // Used to control the left front drive wheel @@ -145,7 +145,7 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode // Wait for driver to press start telemetry.addData("Camera preview on/off", "3 dots, Camera Stream"); - telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.addData(">", "Touch START to start OpMode"); telemetry.update(); waitForStart(); @@ -259,7 +259,7 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode aprilTag = new AprilTagProcessor.Builder().build(); // Adjust Image Decimation to trade-off detection-range for detection-rate. - // eg: Some typical detection data using a Logitech C920 WebCam + // e.g. Some typical detection data using a Logitech C920 WebCam // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java index 58bbaa6..ba3eb4f 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java @@ -63,7 +63,7 @@ import java.util.concurrent.TimeUnit; * The code assumes a Robot Configuration with motors named left_drive and right_drive. * The motor directions must be set so a positive power goes forward on both wheels; * This sample assumes that the default AprilTag Library (usually for the current season) is being loaded by default - * so you should choose to approach a valid tag ID (usually starting at 0) + * so you should choose to approach a valid tag ID. * * Under manual control, the left stick will move forward/back, and the right stick will rotate the robot. * This is called POV Joystick mode, different than Tank Drive (where each joystick controls a wheel). @@ -94,8 +94,8 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode // Set the GAIN constants to control the relationship between the measured position error, and how much power is // applied to the drive motors to correct the error. // Drive = Error * Gain Make these values smaller for smoother control, or larger for a more aggressive response. - final double SPEED_GAIN = 0.02 ; // Speed Control "Gain". eg: Ramp up to 50% power at a 25 inch error. (0.50 / 25.0) - final double TURN_GAIN = 0.01 ; // Turn Control "Gain". eg: Ramp up to 25% power at a 25 degree error. (0.25 / 25.0) + final double SPEED_GAIN = 0.02 ; // Speed Control "Gain". e.g. Ramp up to 50% power at a 25 inch error. (0.50 / 25.0) + final double TURN_GAIN = 0.01 ; // Turn Control "Gain". e.g. Ramp up to 25% power at a 25 degree error. (0.25 / 25.0) final double MAX_AUTO_SPEED = 0.5; // Clip the approach speed to this max value (adjust for your robot) final double MAX_AUTO_TURN = 0.25; // Clip the turn speed to this max value (adjust for your robot) @@ -135,7 +135,7 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode // Wait for the driver to press Start telemetry.addData("Camera preview on/off", "3 dots, Camera Stream"); - telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.addData(">", "Touch START to start OpMode"); telemetry.update(); waitForStart(); @@ -234,7 +234,7 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode aprilTag = new AprilTagProcessor.Builder().build(); // Adjust Image Decimation to trade-off detection-range for detection-rate. - // eg: Some typical detection data using a Logitech C920 WebCam + // e.g. Some typical detection data using a Logitech C920 WebCam // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToLine_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToLine_Linear.java index e5076c0..780f260 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToLine_Linear.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToLine_Linear.java @@ -106,7 +106,7 @@ public class RobotAutoDriveToLine_Linear extends LinearOpMode { // A gain of 15 causes a Rev Color Sensor V2 to produce an Alpha value of 1.0 at about 1.5" above the floor. colorSensor.setGain(15); - // Wait for driver to press PLAY) + // Wait for driver to press START) // Abort this loop is started or stopped. while (opModeInInit()) { diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopPOV_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopPOV_Linear.java index 454d5a7..af3840d 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopPOV_Linear.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopPOV_Linear.java @@ -96,10 +96,10 @@ public class RobotTeleopPOV_Linear extends LinearOpMode { rightClaw.setPosition(MID_SERVO); // Send telemetry message to signify robot waiting; - telemetry.addData(">", "Robot Ready. Press Play."); // + telemetry.addData(">", "Robot Ready. Press START."); // telemetry.update(); - // Wait for the game to start (driver presses PLAY) + // Wait for the game to start (driver presses START) waitForStart(); // run until the end of the match (driver presses STOP) diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopTank_Iterative.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopTank_Iterative.java index b9f4fcf..a622f27 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopTank_Iterative.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopTank_Iterative.java @@ -94,25 +94,25 @@ public class RobotTeleopTank_Iterative extends OpMode{ rightClaw.setPosition(MID_SERVO); // Send telemetry message to signify robot waiting; - telemetry.addData(">", "Robot Ready. Press Play."); // + telemetry.addData(">", "Robot Ready. Press START."); // } /* - * Code to run REPEATEDLY after the driver hits INIT, but before they hit PLAY + * Code to run REPEATEDLY after the driver hits INIT, but before they hit START */ @Override public void init_loop() { } /* - * Code to run ONCE when the driver hits PLAY + * Code to run ONCE when the driver hits START */ @Override public void start() { } /* - * Code to run REPEATEDLY after the driver hits PLAY but before they hit STOP + * Code to run REPEATEDLY after the driver hits START but before they hit STOP */ @Override public void loop() { diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHuskyLens.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHuskyLens.java index 1ec1f55..af7ca55 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHuskyLens.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHuskyLens.java @@ -33,13 +33,10 @@ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. package org.firstinspires.ftc.robotcontroller.external.samples; import com.qualcomm.hardware.dfrobot.HuskyLens; -import com.qualcomm.hardware.rev.Rev2mDistanceSensor; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DistanceSensor; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.internal.system.Deadline; import java.util.concurrent.TimeUnit; @@ -51,6 +48,9 @@ import java.util.concurrent.TimeUnit; * detect a number of predefined objects and AprilTags in the 36h11 family, can * recognize colors, and can be trained to detect custom objects. See this website for * documentation: https://wiki.dfrobot.com/HUSKYLENS_V1.0_SKU_SEN0305_SEN0336 + * + * For detailed instructions on how a HuskyLens is used in FTC, please see this tutorial: + * https://ftc-docs.firstinspires.org/en/latest/devices/huskylens/huskylens.html * * This sample illustrates how to detect AprilTags, but can be used to detect other types * of objects by changing the algorithm. It assumes that the HuskyLens is configured with @@ -110,6 +110,8 @@ public class SensorHuskyLens extends LinearOpMode { * Users, should, in general, explicitly choose the algorithm they want to use * within the OpMode by calling selectAlgorithm() and passing it one of the values * found in the enumeration HuskyLens.Algorithm. + * + * Other algorithm choices for FTC might be: OBJECT_RECOGNITION, COLOR_RECOGNITION or OBJECT_CLASSIFICATION. */ huskyLens.selectAlgorithm(HuskyLens.Algorithm.TAG_RECOGNITION); @@ -141,6 +143,15 @@ public class SensorHuskyLens extends LinearOpMode { telemetry.addData("Block count", blocks.length); for (int i = 0; i < blocks.length; i++) { telemetry.addData("Block", blocks[i].toString()); + /* + * Here inside the FOR loop, you could save or evaluate specific info for the currently recognized Bounding Box: + * - blocks[i].width and blocks[i].height (size of box, in pixels) + * - blocks[i].left and blocks[i].top (edges of box) + * - blocks[i].x and blocks[i].y (center location) + * - blocks[i].id (Color ID) + * + * These values have Java type int (integer). + */ } telemetry.update(); diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUNonOrthogonal.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUNonOrthogonal.java index eb1c7ca..70bc8d4 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUNonOrthogonal.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUNonOrthogonal.java @@ -58,7 +58,7 @@ import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.xyzOrientation; * planes (X/Y, X/Z or Y/Z) OR that the Hub has only been rotated in a range of 90 degree increments. * * Note: if your Hub is mounted Orthogonally (on a orthogonal surface, angled at some multiple of - * 90 Degrees) then you should use the simpler SensorImuOrthogonal sample in this folder. + * 90 Degrees) then you should use the simpler SensorIMUOrthogonal sample in this folder. * * But... If your Hub is mounted Non-Orthogonally, you must specify one or more rotational angles * that transform a "Default" Hub orientation into your desired orientation. That is what is @@ -94,6 +94,9 @@ public class SensorIMUNonOrthogonal extends LinearOpMode * 1) Hub laying flat on a horizontal surface, with the Printed Logo facing UP * 2) Rotated such that the USB ports are facing forward on the robot. * + * If you are using a REV External IMU, the "Default" orientation is the same as for a REV Hub, but instead of + * the USB ports facing forward, the I2C port faces forward. + * * The order that the rotations are performed matters, so this sample shows doing them in the order X, Y, then Z. * For specifying non-orthogonal hub mounting orientations, we must temporarily use axes * defined relative to the Hub itself, instead of the usual Robot Coordinate System axes @@ -124,7 +127,7 @@ public class SensorIMUNonOrthogonal extends LinearOpMode * * To get the "Default" hub into this configuration you would just need a single rotation, but around a different axis. * 1) No rotation around the X or Y axes. - * 1) Rotate the Hub -30 degrees (Clockwise) around the Z axis, since a positive angle would be Counter Clockwise. + * 2) Rotate the Hub -30 degrees (Clockwise) around the Z axis, since a positive angle would be Counter Clockwise. * * So the X,Y,Z rotations would be 0,0,-30 * diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUOrthogonal.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUOrthogonal.java index d92ce3e..af4c202 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUOrthogonal.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUOrthogonal.java @@ -55,7 +55,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; * (X/Y, X/Z or Y/Z) and that the Hub has only been rotated in a range of 90 degree increments. * * Note: if your Hub is mounted on a surface angled at some non-90 Degree multiple (like 30) look at - * the alternative SensorImuNonOrthogonal sample in this folder. + * the alternative SensorIMUNonOrthogonal sample in this folder. * * This "Orthogonal" requirement means that: * @@ -98,6 +98,9 @@ public class SensorIMUOrthogonal extends LinearOpMode * The first parameter specifies the direction the printed logo on the Hub is pointing. * The second parameter specifies the direction the USB connector on the Hub is pointing. * All directions are relative to the robot, and left/right is as-viewed from behind the robot. + * + * If you are using a REV 9-Axis IMU, you can use the Rev9AxisImuOrientationOnRobot class instead of the + * RevHubOrientationOnRobot class, which has an I2cPortFacingDirection instead of a UsbFacingDirection. */ /* The next two lines define Hub orientation. diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java new file mode 100644 index 0000000..6c1f702 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java @@ -0,0 +1,159 @@ +/* +Copyright (c) 2024 Limelight Vision + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of FIRST nor the names of its contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; + +import java.util.List; + +/* + * This OpMode illustrates how to use the Limelight3A Vision Sensor. + * + * @see Limelight + * + * Notes on configuration: + * + * The device presents itself, when plugged into a USB port on a Control Hub as an ethernet + * interface. A DHCP server running on the Limelight automatically assigns the Control Hub an + * ip address for the new ethernet interface. + * + * Since the Limelight is plugged into a USB port, it will be listed on the top level configuration + * activity along with the Control Hub Portal and other USB devices such as webcams. Typically + * serial numbers are displayed below the device's names. In the case of the Limelight device, the + * Control Hub's assigned ip address for that ethernet interface is used as the "serial number". + * + * Tapping the Limelight's name, transitions to a new screen where the user can rename the Limelight + * and specify the Limelight's ip address. Users should take care not to confuse the ip address of + * the Limelight itself, which can be configured through the Limelight settings page via a web browser, + * and the ip address the Limelight device assigned the Control Hub and which is displayed in small text + * below the name of the Limelight on the top level configuration screen. + */ +@TeleOp(name = "Sensor: Limelight3A", group = "Sensor") +@Disabled +public class SensorLimelight3A extends LinearOpMode { + + private Limelight3A limelight; + + @Override + public void runOpMode() throws InterruptedException + { + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + + telemetry.setMsTransmissionInterval(11); + + limelight.pipelineSwitch(0); + + /* + * Starts polling for data. If you neglect to call start(), getLatestResult() will return null. + */ + limelight.start(); + + telemetry.addData(">", "Robot Ready. Press Play."); + telemetry.update(); + waitForStart(); + + while (opModeIsActive()) { + LLStatus status = limelight.getStatus(); + telemetry.addData("Name", "%s", + status.getName()); + telemetry.addData("LL", "Temp: %.1fC, CPU: %.1f%%, FPS: %d", + status.getTemp(), status.getCpu(),(int)status.getFps()); + telemetry.addData("Pipeline", "Index: %d, Type: %s", + status.getPipelineIndex(), status.getPipelineType()); + + LLResult result = limelight.getLatestResult(); + if (result != null) { + // Access general information + Pose3D botpose = result.getBotpose(); + double captureLatency = result.getCaptureLatency(); + double targetingLatency = result.getTargetingLatency(); + double parseLatency = result.getParseLatency(); + telemetry.addData("LL Latency", captureLatency + targetingLatency); + telemetry.addData("Parse Latency", parseLatency); + telemetry.addData("PythonOutput", java.util.Arrays.toString(result.getPythonOutput())); + + if (result.isValid()) { + telemetry.addData("tx", result.getTx()); + telemetry.addData("txnc", result.getTxNC()); + telemetry.addData("ty", result.getTy()); + telemetry.addData("tync", result.getTyNC()); + + telemetry.addData("Botpose", botpose.toString()); + + // Access barcode results + List barcodeResults = result.getBarcodeResults(); + for (LLResultTypes.BarcodeResult br : barcodeResults) { + telemetry.addData("Barcode", "Data: %s", br.getData()); + } + + // Access classifier results + List classifierResults = result.getClassifierResults(); + for (LLResultTypes.ClassifierResult cr : classifierResults) { + telemetry.addData("Classifier", "Class: %s, Confidence: %.2f", cr.getClassName(), cr.getConfidence()); + } + + // Access detector results + List detectorResults = result.getDetectorResults(); + for (LLResultTypes.DetectorResult dr : detectorResults) { + telemetry.addData("Detector", "Class: %s, Area: %.2f", dr.getClassName(), dr.getTargetArea()); + } + + // Access fiducial results + List fiducialResults = result.getFiducialResults(); + for (LLResultTypes.FiducialResult fr : fiducialResults) { + telemetry.addData("Fiducial", "ID: %d, Family: %s, X: %.2f, Y: %.2f", fr.getFiducialId(), fr.getFamily(),fr.getTargetXDegrees(), fr.getTargetYDegrees()); + } + + // Access color results + List colorResults = result.getColorResults(); + for (LLResultTypes.ColorResult cr : colorResults) { + telemetry.addData("Color", "X: %.2f, Y: %.2f", cr.getTargetXDegrees(), cr.getTargetYDegrees()); + } + } + } else { + telemetry.addData("Limelight", "No data available"); + } + + telemetry.update(); + } + limelight.stop(); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java index 71adee0..f797c6b 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java @@ -52,7 +52,7 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list * - * See the sensor's product page: https://www.tindie.com/products/digitalchickenlabs/octoquad-8ch-quadrature-pulse-width-decoder/ + * See the sensor's product page: https://www.tindie.com/products/35114/ */ @TeleOp(name = "OctoQuad Basic", group="OctoQuad") @Disabled @@ -60,7 +60,7 @@ public class SensorOctoQuad extends LinearOpMode { // Identify which encoder OctoQuad inputs are connected to each odometry pod. private final int ODO_LEFT = 0; // Facing forward direction on left side of robot (Axial motion) - private final int ODO_RIGHT = 1; // Facing forward direction on right side or robot (Axial motion ) + private final int ODO_RIGHT = 1; // Facing forward direction on right side or robot (Axial motion) private final int ODO_PERP = 2; // Facing perpendicular direction at the center of the robot (Lateral motion) // Declare the OctoQuad object and members to store encoder positions and velocities @@ -83,7 +83,7 @@ public class SensorOctoQuad extends LinearOpMode { telemetry.addData("OctoQuad Firmware Version ", octoquad.getFirmwareVersion()); // Reverse the count-direction of any encoder that is not what you require. - // Eg: if you push the robot forward and the left encoder counts down, then reverse it so it counts up. + // e.g. if you push the robot forward and the left encoder counts down, then reverse it so it counts up. octoquad.setSingleEncoderDirection(ODO_LEFT, OctoQuad.EncoderDirection.REVERSE); octoquad.setSingleEncoderDirection(ODO_RIGHT, OctoQuad.EncoderDirection.FORWARD); octoquad.setSingleEncoderDirection(ODO_PERP, OctoQuad.EncoderDirection.FORWARD); @@ -91,7 +91,7 @@ public class SensorOctoQuad extends LinearOpMode { // Any changes that are made should be saved in FLASH just in case there is a sensor power glitch. octoquad.saveParametersToFlash(); - telemetry.addLine("\nPress Play to read encoder values"); + telemetry.addLine("\nPress START to read encoder values"); telemetry.update(); waitForStart(); @@ -100,23 +100,23 @@ public class SensorOctoQuad extends LinearOpMode { telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); telemetry.setMsTransmissionInterval(50); - // Set all the encoder inputs to zero + // Set all the encoder inputs to zero. octoquad.resetAllPositions(); // Loop while displaying the odometry pod positions. while (opModeIsActive()) { telemetry.addData(">", "Press X to Reset Encoders\n"); - // Check for X button to reset encoders + // Check for X button to reset encoders. if (gamepad1.x) { // Reset the position of all encoders to zero. octoquad.resetAllPositions(); } - // Read all the encoder data. Load into local members + // Read all the encoder data. Load into local members. readOdometryPods(); - // Display the values + // Display the values. telemetry.addData("Left ", "%8d counts", posLeft); telemetry.addData("Right", "%8d counts", posRight); telemetry.addData("Perp ", "%8d counts", posPerp); @@ -131,11 +131,11 @@ public class SensorOctoQuad extends LinearOpMode { // or // readAllPositions() to get all 8 encoder readings // - // Since both calls take almost the same amount of time, and the actual channels may not end up being sequential, - // we will read all of the encoder positions, and then pick out the ones we need. + // Since both calls take almost the same amount of time, and the actual channels may not end up + // being sequential, we will read all of the encoder positions, and then pick out the ones we need. int[] positions = octoquad.readAllPositions(); posLeft = positions[ODO_LEFT]; posRight = positions[ODO_RIGHT]; posPerp = positions[ODO_PERP]; } -} \ No newline at end of file +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java index c87c05a..e763b9a 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java @@ -77,7 +77,7 @@ import java.util.List; * Note: If you prefer, you can move the two support classes from this file, and place them in their own files. * But leaving them in place is simpler for this example. * - * See the sensor's product page: https://www.tindie.com/products/digitalchickenlabs/octoquad-8ch-quadrature-pulse-width-decoder/ + * See the sensor's product page: https://www.tindie.com/products/35114/ */ @TeleOp(name="OctoQuad Advanced", group="OctoQuad") @Disabled @@ -93,7 +93,7 @@ public class SensorOctoQuadAdv extends LinearOpMode { // Display the OctoQuad firmware revision telemetry.addLine("OctoQuad Firmware v" + octoquad.getFirmwareVersion()); - telemetry.addLine("\nPress Play to read encoder values"); + telemetry.addLine("\nPress START to read encoder values"); telemetry.update(); waitForStart(); @@ -275,4 +275,4 @@ class OctoSwerveModule { public void show(Telemetry telemetry) { telemetry.addData(name, "%8.0f %7.0f %7.0f %6.0f", driveCounts, driveCountsPerSec, steerDegrees, steerDegreesPerSec); } -} \ No newline at end of file +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityOctoQuadConfigMenu.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityOctoQuadConfigMenu.java index 2fd47fb..a962919 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityOctoQuadConfigMenu.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityOctoQuadConfigMenu.java @@ -446,7 +446,7 @@ public class UtilityOctoQuadConfigMenu extends LinearOpMode StringBuilder builder = new StringBuilder(); builder.append(""); builder.append("Navigate items.....dpad up/down\n") - .append("Select.............X\n") + .append("Select.............X or Square\n") .append("Edit option........dpad left/right\n") .append("Up one level.......left bumper\n"); builder.append(""); @@ -614,7 +614,7 @@ public class UtilityOctoQuadConfigMenu extends LinearOpMode @Override public void onClick() { - onRightInput(); + //onRightInput(); } @Override @@ -669,7 +669,7 @@ public class UtilityOctoQuadConfigMenu extends LinearOpMode @Override public void onClick() { - onRightInput(); + //onRightInput(); } @Override diff --git a/README.md b/README.md index bcfc5ad..bc78e7f 100644 --- a/README.md +++ b/README.md @@ -59,6 +59,54 @@ The readme.md file located in the [/TeamCode/src/main/java/org/firstinspires/ftc # Release Information +## Version 10.0 (20240828-111152) + +### Breaking Changes +* Java classes and Blocks for TensorFlow Object Detection have been removed. +* `AngularVelocity.unit` which was of type `AngleUnit` has been renamed `AngularVelocity.angleUnit` of type `UnnormalizedAngleUnit` + +### Enhancements +* Sample for REV Digital Indicator has been added - ConceptRevLED +* Adds support for the [Sparkfun QWIIC LED Stick](https://www.sparkfun.com/products/18354) + * To connect it directly, you need this [cable](https://www.sparkfun.com/products/25596) +* Adds ConceptLEDStick OpMode +* Adds Blocks for colors black, blue, cyan, dkgray, gray, green, ltgray, magenta, red, white, and yellow. +* Adds an "evaluate but ignore result" Block that executes the connected block and ignores the result. Allows you to call a function and ignore the return value. +* Adds I2C driver for Maxbotix Maxsonar I2CXL sonar rangefinder +* Adds Blocks for setPwmEnable, setPwmDisable, and isPwmEnabled for servos and CR servos. +* In the Blocks editor: a \n in the ExportToBlocks annotation's comment field is displayed as a line break. +* Telemetry has new method setNumDecimalPlaces +* Telemetry now formats doubles and floats (not inside objects, just by themselves) +* Adds support for the Limelight 3A. +* Adds initial support for the REV Servo Hub + * Both the Robot Controller and Driver Station need to be updated to version 10.0 in order for Servo Hubs to be + configurable as Servo Hubs. If the app on either device is outdated, the Servo Hub will show up as an Expansion Hub, + and some functionality will not work as expected. You should wait to create a configuration that includes a Servo Hub + until both the Driver Station and Robot Controller apps have been updated to version 10.0. + * Updating the Servo Hub's firmware and changing its address can only be done using the REV Hardware Client at this time +* Adds support for the REV 9-Axis IMU (REV-31-3332) + * The REV 9-Axis IMU is only supported by the [Universal IMU interface](https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html) + * Adds `Rev9AxisImuOrientationOnRobot` Java class. + * If you mentally substitute this IMU's I2C port for the Control Hub's USB ports, `RevHubOrientationOnRobot` is also compatible with this sensor + * Adds Blocks for Rev9AxisImuOrientationOnRobot, including RevHubImuOrientationOnRobot.xyzOrientation and RevHubImuOrientationOnRobot.zyxOrientation. + * Adds Blocks samples SensorRev9AxisIMUOrthogonal and SensorRev9AxisIMUNonOrthogonal. +* Improves Blocks support for RevHubImuOrientationOnRobot. + * Adds Blocks for RevHubImuOrientationOnRobot.xyzOrientation and RevHubImuOrientationOnRobot.zyxOrientation. + * Adds Blocks samples SensorHubIMUOrthogonal (replaces SensorIMU) and SensorHubIMUNonOrthogonal. +* Updates EasyOpenCV, AprilTag, OpenCV, and `libjpeg-turbo` versions +* Adds Blocks for max and min that take two numbers. +* Adds Blocks OpModes ConceptRevSPARKMini, RobotAutoDriveByEncoder, RobotAutoDriveByGyro, RobotAutoDriveByTime, RobotAutoDriveToAprilTagOmni, and RobotAutoDriveToAprilTagTank. +* Two OpModes with the same name now automatically get renamed with the name followed by a "-" and the class name allowing them to both be on the device. +* Shows the name of the active configuration on the Manage page of the Robot Controller Console +* Updated AprilTag Library for INTO THE DEEP. Notably, `getCurrentGameTagLibrary()` now returns INTO THE DEEP tags. +* Adds Blocks for Telemetry.setMsTransmissionInterval and Telemetry.getMsTransmissionInterval. +* Adds Blocks sample SensorOctoQuad. + +### Bug Fixes +* Fixes a bug where the RevBlinkinLedDriver Blocks were under Actuators in the Blocks editor toolbox. They are now Other Devices. +* Fixes a bug where `Exception`s thrown in user code after a stop was requested by the Driver Station would be silently eaten +* Fixed a bug where if you asked for `AngularVelocity` in a unit different than the device reported it in, it would normalize it between -PI and PI for radians, and -180 and 180 for degrees. + ## Version 9.2 (20240701-085519) ### Important Notes diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 436ce67..878287a 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -25,5 +25,4 @@ android { dependencies { implementation project(':FtcRobotController') - annotationProcessor files('lib/OpModeAnnotationProcessor.jar') } diff --git a/build.common.gradle b/build.common.gradle index 12e6acb..046a4a1 100644 --- a/build.common.gradle +++ b/build.common.gradle @@ -47,10 +47,6 @@ android { } } - aaptOptions { - noCompress "tflite" - } - defaultConfig { signingConfig signingConfigs.debug applicationId 'com.qualcomm.ftcrobotcontroller' diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 09d4b8a..c82423d 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -4,18 +4,14 @@ repositories { } dependencies { - implementation 'org.firstinspires.ftc:Inspection:9.2.0' - implementation 'org.firstinspires.ftc:Blocks:9.2.0' - implementation 'org.firstinspires.ftc:Tfod:9.2.0' - implementation 'org.firstinspires.ftc:RobotCore:9.2.0' - implementation 'org.firstinspires.ftc:RobotServer:9.2.0' - implementation 'org.firstinspires.ftc:OnBotJava:9.2.0' - implementation 'org.firstinspires.ftc:Hardware:9.2.0' - implementation 'org.firstinspires.ftc:FtcCommon:9.2.0' - implementation 'org.firstinspires.ftc:Vision:9.2.0' - implementation 'org.firstinspires.ftc:gameAssets-CenterStage:1.0.0' - implementation 'org.tensorflow:tensorflow-lite-task-vision:0.4.3' - runtimeOnly 'org.tensorflow:tensorflow-lite:2.12.0' + implementation 'org.firstinspires.ftc:Inspection:10.0.0' + implementation 'org.firstinspires.ftc:Blocks:10.0.0' + implementation 'org.firstinspires.ftc:RobotCore:10.0.0' + implementation 'org.firstinspires.ftc:RobotServer:10.0.0' + implementation 'org.firstinspires.ftc:OnBotJava:10.0.0' + implementation 'org.firstinspires.ftc:Hardware:10.0.0' + implementation 'org.firstinspires.ftc:FtcCommon:10.0.0' + implementation 'org.firstinspires.ftc:Vision:10.0.0' implementation 'androidx.appcompat:appcompat:1.2.0' } From cfc696479994ecef7a914d2646d98b4e719276a2 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Wed, 4 Sep 2024 20:57:29 -0400 Subject: [PATCH 40/94] can now use SparkFunOTOSCorrected --- .../localization/SparkFunOTOSCorrected.kt | 46 +++++++++++++++++++ .../localizers/OTOSLocalizer.java | 6 +++ .../localizers/TwoWheelLocalizer.java | 2 +- 3 files changed, 53 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/SparkFunOTOSCorrected.kt diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/SparkFunOTOSCorrected.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/SparkFunOTOSCorrected.kt new file mode 100644 index 0000000..76d825c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/SparkFunOTOSCorrected.kt @@ -0,0 +1,46 @@ +package com.acmerobotics.roadrunner.ftc + +import com.qualcomm.hardware.sparkfun.SparkFunOTOS +import com.qualcomm.robotcore.hardware.I2cDeviceSynch +import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties +import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType +import java.util.* +@I2cDeviceType +@DeviceProperties( + name = "SparkFun OTOS Corrected", + xmlTag = "SparkFunOTOS2", + description = "SparkFun Qwiic Optical Tracking Odometry Sensor Corrected" +) +class SparkFunOTOSCorrected(deviceClient: I2cDeviceSynch) : SparkFunOTOS(deviceClient) { + /** + * Gets only the position and velocity measured by the + * OTOS in a single burst read + * @param pos Position measured by the OTOS + * @param vel Velocity measured by the OTOS + */ + fun getPosVel(pos: Pose2D, vel: Pose2D) { + // Read all pose registers + val rawData = deviceClient.read(REG_POS_XL.toInt(), 12) + + // Convert raw data to pose units + pos.set(regsToPose(Arrays.copyOfRange(rawData, 0, 6), INT16_TO_METER, INT16_TO_RAD)) + vel.set(regsToPose(Arrays.copyOfRange(rawData, 6, 12), INT16_TO_MPS, INT16_TO_RPS)) + } + + // Modified version of poseToRegs to fix pose setting issue + // see https://discord.com/channels/225450307654647808/1246977443030368349/1271702497659977760 + override fun poseToRegs(rawData: ByteArray, pose: Pose2D, xyToRaw: Double, hToRaw: Double) { + // Convert pose units to raw data + val rawX = (_distanceUnit.toMeters(pose.x) * xyToRaw).toInt().toShort() + val rawY = (_distanceUnit.toMeters(pose.y) * xyToRaw).toInt().toShort() + val rawH = (_angularUnit.toRadians(pose.h) * hToRaw).toInt().toShort() + + // Store raw data in buffer + rawData[0] = (rawX.toInt() and 0xFF).toByte() + rawData[1] = ((rawX.toInt() shr 8) and 0xFF).toByte() + rawData[2] = (rawY.toInt() and 0xFF).toByte() + rawData[3] = ((rawY.toInt() shr 8) and 0xFF).toByte() + rawData[4] = (rawH.toInt() and 0xFF).toByte() + rawData[5] = ((rawH.toInt() shr 8) and 0xFF).toByte() + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java index 6b11ce2..afe1954 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java @@ -65,6 +65,12 @@ public class OTOSLocalizer extends Localizer { hardwareMap = map; // TODO: replace this with your OTOS port + /* + TODO: If you want to use the "SparkFunOTOSCorrected" version of OTOS, then replace the + 'SparkFunOTOS.class' below with 'SparkFunOTOSCorrected.class' and set the OTOS as a + "SparkFunOTOS Corrected" in your robot confg + */ + SparkFunOTOS otos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos"); otos.setLinearUnit(DistanceUnit.INCH); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java index aa40a8d..bef838e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java @@ -220,7 +220,7 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w forwardEncoder.update(); strafeEncoder.update(); - double currentIMUOrientation =MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); + double currentIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); deltaRadians = MathFunctions.getTurnDirection(previousIMUOrientation, currentIMUOrientation) * MathFunctions.getSmallestAngleDifference(currentIMUOrientation, previousIMUOrientation); previousIMUOrientation = currentIMUOrientation; } From 2d8df47c59ef4d608fdc0f736d8f86cf5bde2028 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Fri, 6 Sep 2024 01:44:14 -0400 Subject: [PATCH 41/94] pathEndTimeoutConstraint is in milliseconds, not seconds --- .../ftc/teamcode/pedroPathing/pathGeneration/Path.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java index 9d88d0a..1539a49 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java @@ -60,7 +60,7 @@ public class Path { private double pathEndTValueConstraint = FollowerConstants.pathEndTValueConstraint; // When the Path is considered at its end parametrically, then the Follower has this many - // seconds to further correct by default. + // milliseconds to further correct by default. // This can be custom set for each Path. private double pathEndTimeoutConstraint = FollowerConstants.pathEndTimeoutConstraint; @@ -402,7 +402,7 @@ public class Path { /** * This sets the Path end timeout. If the Path is at its end parametrically, then the Follower - * has this many seconds to correct before the Path gets ended anyways. + * has this many milliseconds to correct before the Path gets ended anyways. * * @param set This sets the Path end timeout. */ From 442c867b93c7da9215826752dd124951fe2a060a Mon Sep 17 00:00:00 2001 From: Cal Kestis <51927529+CalKestis@users.noreply.github.com> Date: Sat, 7 Sep 2024 09:38:48 -0700 Subject: [PATCH 42/94] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index bc78e7f..5353afb 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ ## NOTICE -This repository contains the public FTC SDK for the CENTERSTAGE (2023-2024) competition season. +This repository contains the public FTC SDK for the INTO THE DEEP (2024-2025) competition season. ## Welcome! This GitHub repository contains the source code that is used to build an Android app to control a *FIRST* Tech Challenge competition robot. To use this SDK, download/clone the entire project to your local computer. From f91cf1a8bf719c08e4db9fb6fd66336e17617656 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Sat, 7 Sep 2024 17:36:52 -0400 Subject: [PATCH 43/94] you can now reset the IMU! --- .../ftc/teamcode/pedroPathing/follower/Follower.java | 7 +++++++ .../teamcode/pedroPathing/localization/Localizer.java | 5 +++++ .../teamcode/pedroPathing/localization/PoseUpdater.java | 9 +++++++++ .../localization/localizers/DriveEncoderLocalizer.java | 6 ++++++ .../localization/localizers/OTOSLocalizer.java | 6 ++++++ .../localization/localizers/ThreeWheelIMULocalizer.java | 7 +++++++ .../localization/localizers/ThreeWheelLocalizer.java | 6 ++++++ .../localization/localizers/TwoWheelLocalizer.java | 7 +++++++ 8 files changed, 53 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index 8407732..fe9d153 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -989,4 +989,11 @@ public class Follower { public DashboardPoseTracker getDashboardPoseTracker() { return dashboardPoseTracker; } + + /** + * This resets the IMU, if applicable. + */ + public void resetIMU() { + poseUpdater.resetIMU(); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java index 2fa2173..8f9687f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java @@ -86,4 +86,9 @@ public abstract class Localizer { * @return returns the turning ticks to radians multiplier */ public abstract double getTurningMultiplier(); + + /** + * This resets the IMU of the localizer, if applicable. + */ + public abstract void resetIMU(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java index 415b5a0..ce2a8f2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java @@ -5,7 +5,9 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.ThreeWheelIMULocalizer; import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.ThreeWheelLocalizer; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.TwoWheelLocalizer; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; @@ -336,4 +338,11 @@ public class PoseUpdater { public Localizer getLocalizer() { return localizer; } + + /** + * + */ + public void resetIMU() { + localizer.resetIMU(); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java index c414057..a8e3902 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java @@ -263,4 +263,10 @@ public class DriveEncoderLocalizer extends Localizer { public double getTurningMultiplier() { return TURN_TICKS_TO_RADIANS; } + + /** + * This does nothing since this localizer does not use the IMU. + */ + public void resetIMU() { + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java index afe1954..cdacd52 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java @@ -209,4 +209,10 @@ public class OTOSLocalizer extends Localizer { public double getTurningMultiplier() { return otos.getAngularScalar(); } + + /** + * This does nothing since this localizer does not use the IMU. + */ + public void resetIMU() { + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java index 44a03c6..1e9fd7b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java @@ -306,4 +306,11 @@ public class ThreeWheelIMULocalizer extends Localizer { public double getTurningMultiplier() { return TURN_TICKS_TO_RADIANS; } + + /** + * This resets the IMU. + */ + public void resetIMU() { + imu.resetYaw(); + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java index a955946..6814b86 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java @@ -283,4 +283,10 @@ public class ThreeWheelLocalizer extends Localizer { public double getTurningMultiplier() { return TURN_TICKS_TO_RADIANS; } + + /** + * This does nothing since this localizer does not use the IMU. + */ + public void resetIMU() { + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java index bef838e..62415d6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java @@ -289,4 +289,11 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w public double getTurningMultiplier() { return 1; } + + /** + * This resets the IMU. + */ + public void resetIMU() { + imu.resetYaw(); + } } From 151ead49d2633a7d69ce33fe4b8f0c002cd6d616 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Tue, 10 Sep 2024 14:25:57 -0400 Subject: [PATCH 44/94] v10.0 sdk --- FtcRobotController/src/main/AndroidManifest.xml | 4 ++-- .../external/samples/BasicOmniOpMode_Linear.java | 2 +- .../external/samples/BasicOpMode_Iterative.java | 6 +++--- .../external/samples/BasicOpMode_Linear.java | 2 +- .../robotcontroller/external/samples/ConceptAprilTag.java | 2 +- .../external/samples/ConceptAprilTagEasy.java | 2 +- .../external/samples/ConceptAprilTagOptimizeExposure.java | 2 +- .../external/samples/ConceptAprilTagSwitchableCameras.java | 2 +- .../external/samples/ConceptExternalHardwareClass.java | 2 +- .../external/samples/ConceptMotorBulkRead.java | 2 +- .../ftc/robotcontroller/external/samples/ConceptNullOp.java | 6 +++--- 11 files changed, 16 insertions(+), 16 deletions(-) diff --git a/FtcRobotController/src/main/AndroidManifest.xml b/FtcRobotController/src/main/AndroidManifest.xml index eb87ce8..3b87d9d 100644 --- a/FtcRobotController/src/main/AndroidManifest.xml +++ b/FtcRobotController/src/main/AndroidManifest.xml @@ -1,8 +1,8 @@ + android:versionCode="55" + android:versionName="10.0"> diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java index 940b527..1d14dfb 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java @@ -99,7 +99,7 @@ public class BasicOmniOpMode_Linear extends LinearOpMode { rightFrontDrive.setDirection(DcMotor.Direction.FORWARD); rightBackDrive.setDirection(DcMotor.Direction.FORWARD); - // Wait for the game to start (driver presses PLAY) + // Wait for the game to start (driver presses START) telemetry.addData("Status", "Initialized"); telemetry.update(); diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Iterative.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Iterative.java index 7c10408..6504e58 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Iterative.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Iterative.java @@ -83,14 +83,14 @@ public class BasicOpMode_Iterative extends OpMode } /* - * Code to run REPEATEDLY after the driver hits INIT, but before they hit PLAY + * Code to run REPEATEDLY after the driver hits INIT, but before they hit START */ @Override public void init_loop() { } /* - * Code to run ONCE when the driver hits PLAY + * Code to run ONCE when the driver hits START */ @Override public void start() { @@ -98,7 +98,7 @@ public class BasicOpMode_Iterative extends OpMode } /* - * Code to run REPEATEDLY after the driver hits PLAY but before they hit STOP + * Code to run REPEATEDLY after the driver hits START but before they hit STOP */ @Override public void loop() { diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Linear.java index 9fe0bb6..ab0bb25 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Linear.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Linear.java @@ -76,7 +76,7 @@ public class BasicOpMode_Linear extends LinearOpMode { leftDrive.setDirection(DcMotor.Direction.REVERSE); rightDrive.setDirection(DcMotor.Direction.FORWARD); - // Wait for the game to start (driver presses PLAY) + // Wait for the game to start (driver presses START) waitForStart(); runtime.reset(); diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java index b6aa4e5..8ec77dd 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java @@ -88,7 +88,7 @@ public class ConceptAprilTag extends LinearOpMode { // Wait for the DS start button to be touched. telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); - telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.addData(">", "Touch START to start OpMode"); telemetry.update(); waitForStart(); diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java index d305d55..12dcf6e 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java @@ -84,7 +84,7 @@ public class ConceptAprilTagEasy extends LinearOpMode { // Wait for the DS start button to be touched. telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); - telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.addData(">", "Touch START to start OpMode"); telemetry.update(); waitForStart(); diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagOptimizeExposure.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagOptimizeExposure.java index 3792992..adee952 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagOptimizeExposure.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagOptimizeExposure.java @@ -98,7 +98,7 @@ public class ConceptAprilTagOptimizeExposure extends LinearOpMode // Wait for the match to begin. telemetry.addData("Camera preview on/off", "3 dots, Camera Stream"); - telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.addData(">", "Touch START to start OpMode"); telemetry.update(); waitForStart(); diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java index 9285224..7ee1064 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java @@ -77,7 +77,7 @@ public class ConceptAprilTagSwitchableCameras extends LinearOpMode { // Wait for the DS start button to be touched. telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); - telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.addData(">", "Touch START to start OpMode"); telemetry.update(); waitForStart(); diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExternalHardwareClass.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExternalHardwareClass.java index 2fe4154..7a721fe 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExternalHardwareClass.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExternalHardwareClass.java @@ -83,7 +83,7 @@ public class ConceptExternalHardwareClass extends LinearOpMode { robot.init(); // Send telemetry message to signify robot waiting; - // Wait for the game to start (driver presses PLAY) + // Wait for the game to start (driver presses START) waitForStart(); // run until the end of the match (driver presses STOP) diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java index 3bade8b..5439f78 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java @@ -110,7 +110,7 @@ public class ConceptMotorBulkRead extends LinearOpMode { ElapsedTime timer = new ElapsedTime(); - telemetry.addData(">", "Press play to start tests"); + telemetry.addData(">", "Press START to start tests"); telemetry.addData(">", "Test results will update for each access method."); telemetry.update(); waitForStart(); diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java index 7ea4683..4a887bd 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java @@ -53,7 +53,7 @@ public class ConceptNullOp extends OpMode { /** * This method will be called repeatedly during the period between when - * the init button is pressed and when the play button is pressed (or the + * the INIT button is pressed and when the START button is pressed (or the * OpMode is stopped). */ @Override @@ -61,7 +61,7 @@ public class ConceptNullOp extends OpMode { } /** - * This method will be called once, when the play button is pressed. + * This method will be called once, when the START button is pressed. */ @Override public void start() { @@ -70,7 +70,7 @@ public class ConceptNullOp extends OpMode { /** * This method will be called repeatedly during the period between when - * the play button is pressed and when the OpMode is stopped. + * the START button is pressed and when the OpMode is stopped. */ @Override public void loop() { From e72326506f149b07639c44d64c50cee52d72b69b Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Sat, 14 Sep 2024 03:14:52 -0400 Subject: [PATCH 45/94] small typo --- .../tuning/{FowardTuner.java => ForwardTuner.java} | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/{FowardTuner.java => ForwardTuner.java} (96%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/FowardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/ForwardTuner.java similarity index 96% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/FowardTuner.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/ForwardTuner.java index ff59830..eed2210 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/FowardTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/ForwardTuner.java @@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; @@ -27,7 +26,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; */ @Config @Autonomous(name = "Forward Localizer Tuner", group = "Autonomous Pathing Tuning") -public class FowardTuner extends OpMode { +public class ForwardTuner extends OpMode { private PoseUpdater poseUpdater; private DashboardPoseTracker dashboardPoseTracker; From 054017df61156a822a85893c0b7e4514e2e48429 Mon Sep 17 00:00:00 2001 From: Cal Kestis Date: Thu, 19 Sep 2024 13:38:50 -0700 Subject: [PATCH 46/94] FtcRobotController v10.1 --- .../src/main/AndroidManifest.xml | 4 +- .../samples/ConceptAprilTagLocalization.java | 3 + .../samples/ConceptVisionColorLocator.java | 191 ++++++++++++++++++ .../samples/ConceptVisionColorSensor.java | 136 +++++++++++++ README.md | 22 +- build.dependencies.gradle | 16 +- 6 files changed, 360 insertions(+), 12 deletions(-) create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java diff --git a/FtcRobotController/src/main/AndroidManifest.xml b/FtcRobotController/src/main/AndroidManifest.xml index 46149bd..787878b 100644 --- a/FtcRobotController/src/main/AndroidManifest.xml +++ b/FtcRobotController/src/main/AndroidManifest.xml @@ -1,8 +1,8 @@ + android:versionCode="56" + android:versionName="10.1"> diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java index 05318ba..d90261e 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java @@ -58,6 +58,9 @@ import java.util.List; * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the robot, relative to the field origin. * This information is provided in the "robotPose" member of the returned "detection". * + * To learn about the Field Coordinate System that is defined for FTC (and used by this OpMode), see the FTC-DOCS link below: + * https://ftc-docs.firstinspires.org/en/latest/game_specific_resources/field_coordinate_system/field-coordinate-system.html + * * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. */ diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator.java new file mode 100644 index 0000000..987694d --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator.java @@ -0,0 +1,191 @@ +/* + * Copyright (c) 2024 Phil Malone + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import android.util.Size; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.SortOrder; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor; +import org.firstinspires.ftc.vision.opencv.ColorRange; +import org.firstinspires.ftc.vision.opencv.ImageRegion; +import org.opencv.core.RotatedRect; + +import java.util.List; + +/* + * This OpMode illustrates how to use a video source (camera) to locate specifically colored regions + * + * Unlike a "color sensor" which determines the color of an object in the field of view, this "color locator" + * will search the Region Of Interest (ROI) in a camera image, and find any "blobs" of color that match the requested color range. + * These blobs can be further filtered and sorted to find the one most likely to be the item the user is looking for. + * + * To perform this function, a VisionPortal runs a ColorBlobLocatorProcessor process. + * The ColorBlobLocatorProcessor process is created first, and then the VisionPortal is built to use this process. + * The ColorBlobLocatorProcessor analyses the ROI and locates pixels that match the ColorRange to form a "mask". + * The matching pixels are then collected into contiguous "blobs" of pixels. The outer boundaries of these blobs are called its "contour". + * For each blob, the process then creates the smallest possible rectangle "boxFit" that will fully encase the contour. + * The user can then call getBlobs() to retrieve the list of Blobs, where each Blob contains the contour and the boxFit data. + * Note: The default sort order for Blobs is ContourArea, in descending order, so the biggest contours are listed first. + * + * To aid the user, a colored boxFit rectangle is drawn on the camera preview to show the location of each Blob + * The original Blob contour can also be added to the preview. This is helpful when configuring the ColorBlobLocatorProcessor parameters. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@Disabled +@TeleOp(name = "Concept: Vision Color-Locator", group = "Concept") +public class ConceptVisionColorLocator extends LinearOpMode +{ + @Override + public void runOpMode() + { + /* Build a "Color Locator" vision processor based on the ColorBlobLocatorProcessor class. + * - Specify the color range you are looking for. You can use a predefined color, or create you own color range + * .setTargetColorRange(ColorRange.BLUE) // use a predefined color match + * Available predefined colors are: RED, BLUE YELLOW GREEN + * .setTargetColorRange(new ColorRange(ColorSpace.YCrCb, // or define your own color match + * new Scalar( 32, 176, 0), + * new Scalar(255, 255, 132))) + * + * - Focus the color locator by defining a RegionOfInterest (ROI) which you want to search. + * This can be the entire frame, or a sub-region defined using: + * 1) standard image coordinates or 2) a normalized +/- 1.0 coordinate system. + * Use one form of the ImageRegion class to define the ROI. + * ImageRegion.entireFrame() + * ImageRegion.asImageCoordinates(50, 50, 150, 150) 100x100 pixel square near the upper left corner + * ImageRegion.asUnityCenterCoordinates(-0.5, 0.5, 0.5, -0.5) 50% width/height square centered on screen + * + * - Define which contours are included. + * You can get ALL the contours, or you can skip any contours that are completely inside another contour. + * .setContourMode(ColorBlobLocatorProcessor.ContourMode.ALL_FLATTENED_HIERARCHY) // return all contours + * .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) // exclude contours inside other contours + * note: EXTERNAL_ONLY helps to avoid bright reflection spots from breaking up areas of solid color. + * + * - turn the display of contours ON or OFF. Turning this on helps debugging but takes up valuable CPU time. + * .setDrawContours(true) + * + * - include any pre-processing of the image or mask before looking for Blobs. + * There are some extra processing you can include to improve the formation of blobs. Using these features requires + * an understanding of how they may effect the final blobs. The "pixels" argument sets the NxN kernel size. + * .setBlurSize(int pixels) Blurring an image helps to provide a smooth color transition between objects, and smoother contours. + * The higher the number of pixels, the more blurred the image becomes. + * Note: Even "pixels" values will be incremented to satisfy the "odd number" requirement. + * Blurring too much may hide smaller features. A "pixels" size of 5 is good for a 320x240 image. + * .setErodeSize(int pixels) Erosion removes floating pixels and thin lines so that only substantive objects remain. + * Erosion can grow holes inside regions, and also shrink objects. + * "pixels" in the range of 2-4 are suitable for low res images. + * .setDilateSize(int pixels) Dilation makes objects more visible by filling in small holes, making lines appear thicker, + * and making filled shapes appear larger. Dilation is useful for joining broken parts of an + * object, such as when removing noise from an image. + * "pixels" in the range of 2-4 are suitable for low res images. + */ + ColorBlobLocatorProcessor colorLocator = new ColorBlobLocatorProcessor.Builder() + .setTargetColorRange(ColorRange.BLUE) // use a predefined color match + .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) // exclude blobs inside blobs + .setRoi(ImageRegion.asUnityCenterCoordinates(-0.5, 0.5, 0.5, -0.5)) // search central 1/4 of camera view + .setDrawContours(true) // Show contours on the Stream Preview + .setBlurSize(5) // Smooth the transitions between different colors in image + .build(); + + /* + * Build a vision portal to run the Color Locator process. + * + * - Add the colorLocator process created above. + * - Set the desired video resolution. + * Since a high resolution will not improve this process, choose a lower resolution that is + * supported by your camera. This will improve overall performance and reduce latency. + * - Choose your video source. This may be + * .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) ..... for a webcam + * or + * .setCamera(BuiltinCameraDirection.BACK) ... for a Phone Camera + */ + VisionPortal portal = new VisionPortal.Builder() + .addProcessor(colorLocator) + .setCameraResolution(new Size(320, 240)) + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .build(); + + telemetry.setMsTransmissionInterval(50); // Speed up telemetry updates, Just use for debugging. + telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); + + // WARNING: To be able to view the stream preview on the Driver Station, this code runs in INIT mode. + while (opModeIsActive() || opModeInInit()) + { + telemetry.addData("preview on/off", "... Camera Stream\n"); + + // Read the current list + List blobs = colorLocator.getBlobs(); + + /* + * The list of Blobs can be filtered to remove unwanted Blobs. + * Note: All contours will be still displayed on the Stream Preview, but only those that satisfy the filter + * conditions will remain in the current list of "blobs". Multiple filters may be used. + * + * Use any of the following filters. + * + * ColorBlobLocatorProcessor.Util.filterByArea(minArea, maxArea, blobs); + * A Blob's area is the number of pixels contained within the Contour. Filter out any that are too big or small. + * Start with a large range and then refine the range based on the likely size of the desired object in the viewfinder. + * + * ColorBlobLocatorProcessor.Util.filterByDensity(minDensity, maxDensity, blobs); + * A blob's density is an indication of how "full" the contour is. + * If you put a rubber band around the contour you would get the "Convex Hull" of the contour. + * The density is the ratio of Contour-area to Convex Hull-area. + * + * ColorBlobLocatorProcessor.Util.filterByAspectRatio(minAspect, maxAspect, blobs); + * A blob's Aspect ratio is the ratio of boxFit long side to short side. + * A perfect Square has an aspect ratio of 1. All others are > 1 + */ + ColorBlobLocatorProcessor.Util.filterByArea(50, 20000, blobs); // filter out very small blobs. + + /* + * The list of Blobs can be sorted using the same Blob attributes as listed above. + * No more than one sort call should be made. Sorting can use ascending or descending order. + * ColorBlobLocatorProcessor.Util.sortByArea(SortOrder.DESCENDING, blobs); // Default + * ColorBlobLocatorProcessor.Util.sortByDensity(SortOrder.DESCENDING, blobs); + * ColorBlobLocatorProcessor.Util.sortByAspectRatio(SortOrder.DESCENDING, blobs); + */ + + telemetry.addLine(" Area Density Aspect Center"); + + // Display the size (area) and center location for each Blob. + for(ColorBlobLocatorProcessor.Blob b : blobs) + { + RotatedRect boxFit = b.getBoxFit(); + telemetry.addLine(String.format("%5d %4.2f %5.2f (%3d,%3d)", + b.getContourArea(), b.getDensity(), b.getAspectRatio(), (int) boxFit.center.x, (int) boxFit.center.y)); + } + + telemetry.update(); + sleep(50); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java new file mode 100644 index 0000000..6be2bc4 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java @@ -0,0 +1,136 @@ +/* + * Copyright (c) 2024 Phil Malone + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import android.graphics.Color; +import android.util.Size; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.opencv.ImageRegion; +import org.firstinspires.ftc.vision.opencv.PredominantColorProcessor; + +/* + * This OpMode illustrates how to use a video source (camera) as a color sensor + * + * A "color sensor" will typically determine the color of the object that it is pointed at. + * + * This sample performs the same function, except it uses a video camera to inspect an object or scene. + * The user may choose to inspect all, or just a Region of Interest (ROI), of the active camera view. + * The user must also provide a list of "acceptable colors" (Swatches) from which the closest matching color will be selected. + * + * To perform this function, a VisionPortal runs a PredominantColorProcessor process. + * The PredominantColorProcessor process is created first, and then the VisionPortal is built to use this process. + * The PredominantColorProcessor analyses the ROI and splits the colored pixels into several color-clusters. + * The largest of these clusters is then considered to be the "Predominant Color" + * The process then matches the Predominant Color with the closest Swatch and returns that match. + * + * To aid the user, a colored rectangle is drawn on the camera preview to show the RegionOfInterest, + * The Predominant Color is used to paint the rectangle border, so the user can verify that the color is reasonable. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@Disabled +@TeleOp(name = "Concept: Vision Color-Sensor", group = "Concept") +public class ConceptVisionColorSensor extends LinearOpMode +{ + @Override + public void runOpMode() + { + /* Build a "Color Sensor" vision processor based on the PredominantColorProcessor class. + * + * - Focus the color sensor by defining a RegionOfInterest (ROI) which you want to inspect. + * This can be the entire frame, or a sub-region defined using: + * 1) standard image coordinates or 2) a normalized +/- 1.0 coordinate system. + * Use one form of the ImageRegion class to define the ROI. + * ImageRegion.entireFrame() + * ImageRegion.asImageCoordinates(50, 50, 150, 150) 100x100 pixel square near the upper left corner + * ImageRegion.asUnityCenterCoordinates(-0.1, 0.1, 0.1, -0.1) 10% width/height square centered on screen + * + * - Set the list of "acceptable" color swatches (matches). + * Only colors that you assign here will be returned. + * If you know the sensor will be pointing to one of a few specific colors, enter them here. + * Or, if the sensor may be pointed randomly, provide some additional colors that may match the surrounding. + * Possible choices are: + * RED, ORANGE, YELLOW, GREEN, CYAN, BLUE, PURPLE, MAGENTA, BLACK, WHITE; + * + * Note that in the example shown below, only some of the available colors are included. + * This will force any other colored region into one of these colors. + * eg: Green may be reported as YELLOW, as this may be the "closest" match. + */ + PredominantColorProcessor colorSensor = new PredominantColorProcessor.Builder() + .setRoi(ImageRegion.asUnityCenterCoordinates(-0.1, 0.1, 0.1, -0.1)) + .setSwatches( + PredominantColorProcessor.Swatch.RED, + PredominantColorProcessor.Swatch.BLUE, + PredominantColorProcessor.Swatch.YELLOW, + PredominantColorProcessor.Swatch.BLACK, + PredominantColorProcessor.Swatch.WHITE) + .build(); + + /* + * Build a vision portal to run the Color Sensor process. + * + * - Add the colorSensor process created above. + * - Set the desired video resolution. + * Since a high resolution will not improve this process, choose a lower resolution that is + * supported by your camera. This will improve overall performance and reduce latency. + * - Choose your video source. This may be + * .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) ..... for a webcam + * or + * .setCamera(BuiltinCameraDirection.BACK) ... for a Phone Camera + */ + VisionPortal portal = new VisionPortal.Builder() + .addProcessor(colorSensor) + .setCameraResolution(new Size(320, 240)) + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .build(); + + telemetry.setMsTransmissionInterval(50); // Speed up telemetry updates, Just use for debugging. + + // WARNING: To be able to view the stream preview on the Driver Station, this code runs in INIT mode. + while (opModeIsActive() || opModeInInit()) + { + telemetry.addData("DS preview on/off", "3 dots, Camera Stream\n"); + + // Request the most recent color analysis. + // This will return the closest matching colorSwatch and the predominant RGB color. + // Note: to take actions based on the detected color, simply use the colorSwatch in a comparison or switch. + // eg: + // if (result.closestSwatch == PredominantColorProcessor.Swatch.RED) {... some code ...} + PredominantColorProcessor.Result result = colorSensor.getAnalysis(); + + // Display the Color Sensor result. + telemetry.addData("Best Match:", result.closestSwatch); + telemetry.addLine(String.format("R %3d, G %3d, B %3d", Color.red(result.rgb), Color.green(result.rgb), Color.blue(result.rgb))); + telemetry.update(); + + sleep(20); + } + } +} diff --git a/README.md b/README.md index 5353afb..f7a02c9 100644 --- a/README.md +++ b/README.md @@ -59,6 +59,24 @@ The readme.md file located in the [/TeamCode/src/main/java/org/firstinspires/ftc # Release Information +## Version 10.1 (20240919-122750) + +### Enhancements +* Adds new OpenCV-based `VisionProcessor`s (which may be attached to a VisionPortal in either Java or Blocks) to help teams implement color processing via computer vision in the INTO THE DEEP game + * `ColorBlobLocatorProcessor` implements OpenCV color "blob" detection. A new sample program `ConceptVisionColorLocator` demonstrates its use. + * A choice is offered between pre-defined color ranges, or creating a custom one in RGB, HSV, or YCrCb color space + * The ability is provided to restrict detection to a specified Region of Interest on the screen + * Functions for applying erosion / dilation morphing to the threshold mask are provided + * Functions for sorting and filtering the returned data are provided + * `PredominantColorProcessor` allows using a region of the camera as a "long range color sensor" to determine the predominant color of that region. A new sample program `ConceptVisionColorSensor` demonstrates its use. + * The determined predominant color is selected from a discrete set of color "swatches", similar to the MINDSTORMS NXT color sensor + * Documentation on this Color Processing feature can be found here: https://ftc-docs.firstinspires.org/color-processing +* Added Blocks sample programs for color sensors: RobotAutoDriveToLine and SensorColor. +* Updated Self-Inspect to identify mismatched RC/DS software versions as a "caution" rather than a "failure." + +### Bug Fixes +* Fixes [AngularVelocity conversion regression](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/1070) + ## Version 10.0 (20240828-111152) ### Breaking Changes @@ -469,7 +487,7 @@ This is a bug fix only release to address the following four issues. * Fixes [issue #260](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/260) Blocks can't call java method that has a VuforiaLocalizer parameter. * Blocks now has a block labeled VuforiaFreightFrenzy.getVuforiaLocalizer for this. * Added a page to manage the TensorFlow Lite models in /sdcard/FIRST/tflitemodels. To get to the TFLite Models page: - * You can click on the link at the bottom of the the Manage page. + * You can click on the link at the bottom of the Manage page. * You can click on the link at the upper-right the Blocks project page. * Fixes logspam when `isBusy()` is called on a motor not in RTP mode. * Hides the "RC Password" item on the inspection screen for phone-based Robot Controllers. (It is only applicable for Control Hubs). @@ -1154,7 +1172,7 @@ Known issues: This version of the software provides support for the REV Robotics Expansion Hub. This version also includes improvements in the USB communication layer in an effort to enhance system resiliency. If you were using a 2.x version of the software previously, updating to version 3.1 requires that you also update your Driver Station software in addition to updating the Robot Controller software. -Also note that in version 3.10 software, the setMaxSpeed and getMaxSpeed methods are no longer available (not deprecated, they have been removed from the SDK). Also note that the the new 3.x software incorporates motor profiles that a user can select as he/she configures the robot. +Also note that in version 3.10 software, the setMaxSpeed and getMaxSpeed methods are no longer available (not deprecated, they have been removed from the SDK). Also note that the new 3.x software incorporates motor profiles that a user can select as he/she configures the robot. Changes include: * Blocks changes diff --git a/build.dependencies.gradle b/build.dependencies.gradle index c82423d..a2b4ea1 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -4,14 +4,14 @@ repositories { } dependencies { - implementation 'org.firstinspires.ftc:Inspection:10.0.0' - implementation 'org.firstinspires.ftc:Blocks:10.0.0' - implementation 'org.firstinspires.ftc:RobotCore:10.0.0' - implementation 'org.firstinspires.ftc:RobotServer:10.0.0' - implementation 'org.firstinspires.ftc:OnBotJava:10.0.0' - implementation 'org.firstinspires.ftc:Hardware:10.0.0' - implementation 'org.firstinspires.ftc:FtcCommon:10.0.0' - implementation 'org.firstinspires.ftc:Vision:10.0.0' + implementation 'org.firstinspires.ftc:Inspection:10.1.0' + implementation 'org.firstinspires.ftc:Blocks:10.1.0' + implementation 'org.firstinspires.ftc:RobotCore:10.1.0' + implementation 'org.firstinspires.ftc:RobotServer:10.1.0' + implementation 'org.firstinspires.ftc:OnBotJava:10.1.0' + implementation 'org.firstinspires.ftc:Hardware:10.1.0' + implementation 'org.firstinspires.ftc:FtcCommon:10.1.0' + implementation 'org.firstinspires.ftc:Vision:10.1.0' implementation 'androidx.appcompat:appcompat:1.2.0' } From 91d5984af720b50e6fac22285ec62bdc9705976f Mon Sep 17 00:00:00 2001 From: Cal Kestis Date: Thu, 19 Sep 2024 13:38:50 -0700 Subject: [PATCH 47/94] FtcRobotController v10.1 --- .../samples/ConceptAprilTagLocalization.java | 3 + .../samples/ConceptVisionColorLocator.java | 191 ++++++++++++++++++ .../samples/ConceptVisionColorSensor.java | 136 +++++++++++++ README.md | 22 +- build.dependencies.gradle | 16 +- 5 files changed, 358 insertions(+), 10 deletions(-) create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java index 05318ba..d90261e 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java @@ -58,6 +58,9 @@ import java.util.List; * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the robot, relative to the field origin. * This information is provided in the "robotPose" member of the returned "detection". * + * To learn about the Field Coordinate System that is defined for FTC (and used by this OpMode), see the FTC-DOCS link below: + * https://ftc-docs.firstinspires.org/en/latest/game_specific_resources/field_coordinate_system/field-coordinate-system.html + * * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. */ diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator.java new file mode 100644 index 0000000..987694d --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator.java @@ -0,0 +1,191 @@ +/* + * Copyright (c) 2024 Phil Malone + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import android.util.Size; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.SortOrder; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor; +import org.firstinspires.ftc.vision.opencv.ColorRange; +import org.firstinspires.ftc.vision.opencv.ImageRegion; +import org.opencv.core.RotatedRect; + +import java.util.List; + +/* + * This OpMode illustrates how to use a video source (camera) to locate specifically colored regions + * + * Unlike a "color sensor" which determines the color of an object in the field of view, this "color locator" + * will search the Region Of Interest (ROI) in a camera image, and find any "blobs" of color that match the requested color range. + * These blobs can be further filtered and sorted to find the one most likely to be the item the user is looking for. + * + * To perform this function, a VisionPortal runs a ColorBlobLocatorProcessor process. + * The ColorBlobLocatorProcessor process is created first, and then the VisionPortal is built to use this process. + * The ColorBlobLocatorProcessor analyses the ROI and locates pixels that match the ColorRange to form a "mask". + * The matching pixels are then collected into contiguous "blobs" of pixels. The outer boundaries of these blobs are called its "contour". + * For each blob, the process then creates the smallest possible rectangle "boxFit" that will fully encase the contour. + * The user can then call getBlobs() to retrieve the list of Blobs, where each Blob contains the contour and the boxFit data. + * Note: The default sort order for Blobs is ContourArea, in descending order, so the biggest contours are listed first. + * + * To aid the user, a colored boxFit rectangle is drawn on the camera preview to show the location of each Blob + * The original Blob contour can also be added to the preview. This is helpful when configuring the ColorBlobLocatorProcessor parameters. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@Disabled +@TeleOp(name = "Concept: Vision Color-Locator", group = "Concept") +public class ConceptVisionColorLocator extends LinearOpMode +{ + @Override + public void runOpMode() + { + /* Build a "Color Locator" vision processor based on the ColorBlobLocatorProcessor class. + * - Specify the color range you are looking for. You can use a predefined color, or create you own color range + * .setTargetColorRange(ColorRange.BLUE) // use a predefined color match + * Available predefined colors are: RED, BLUE YELLOW GREEN + * .setTargetColorRange(new ColorRange(ColorSpace.YCrCb, // or define your own color match + * new Scalar( 32, 176, 0), + * new Scalar(255, 255, 132))) + * + * - Focus the color locator by defining a RegionOfInterest (ROI) which you want to search. + * This can be the entire frame, or a sub-region defined using: + * 1) standard image coordinates or 2) a normalized +/- 1.0 coordinate system. + * Use one form of the ImageRegion class to define the ROI. + * ImageRegion.entireFrame() + * ImageRegion.asImageCoordinates(50, 50, 150, 150) 100x100 pixel square near the upper left corner + * ImageRegion.asUnityCenterCoordinates(-0.5, 0.5, 0.5, -0.5) 50% width/height square centered on screen + * + * - Define which contours are included. + * You can get ALL the contours, or you can skip any contours that are completely inside another contour. + * .setContourMode(ColorBlobLocatorProcessor.ContourMode.ALL_FLATTENED_HIERARCHY) // return all contours + * .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) // exclude contours inside other contours + * note: EXTERNAL_ONLY helps to avoid bright reflection spots from breaking up areas of solid color. + * + * - turn the display of contours ON or OFF. Turning this on helps debugging but takes up valuable CPU time. + * .setDrawContours(true) + * + * - include any pre-processing of the image or mask before looking for Blobs. + * There are some extra processing you can include to improve the formation of blobs. Using these features requires + * an understanding of how they may effect the final blobs. The "pixels" argument sets the NxN kernel size. + * .setBlurSize(int pixels) Blurring an image helps to provide a smooth color transition between objects, and smoother contours. + * The higher the number of pixels, the more blurred the image becomes. + * Note: Even "pixels" values will be incremented to satisfy the "odd number" requirement. + * Blurring too much may hide smaller features. A "pixels" size of 5 is good for a 320x240 image. + * .setErodeSize(int pixels) Erosion removes floating pixels and thin lines so that only substantive objects remain. + * Erosion can grow holes inside regions, and also shrink objects. + * "pixels" in the range of 2-4 are suitable for low res images. + * .setDilateSize(int pixels) Dilation makes objects more visible by filling in small holes, making lines appear thicker, + * and making filled shapes appear larger. Dilation is useful for joining broken parts of an + * object, such as when removing noise from an image. + * "pixels" in the range of 2-4 are suitable for low res images. + */ + ColorBlobLocatorProcessor colorLocator = new ColorBlobLocatorProcessor.Builder() + .setTargetColorRange(ColorRange.BLUE) // use a predefined color match + .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) // exclude blobs inside blobs + .setRoi(ImageRegion.asUnityCenterCoordinates(-0.5, 0.5, 0.5, -0.5)) // search central 1/4 of camera view + .setDrawContours(true) // Show contours on the Stream Preview + .setBlurSize(5) // Smooth the transitions between different colors in image + .build(); + + /* + * Build a vision portal to run the Color Locator process. + * + * - Add the colorLocator process created above. + * - Set the desired video resolution. + * Since a high resolution will not improve this process, choose a lower resolution that is + * supported by your camera. This will improve overall performance and reduce latency. + * - Choose your video source. This may be + * .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) ..... for a webcam + * or + * .setCamera(BuiltinCameraDirection.BACK) ... for a Phone Camera + */ + VisionPortal portal = new VisionPortal.Builder() + .addProcessor(colorLocator) + .setCameraResolution(new Size(320, 240)) + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .build(); + + telemetry.setMsTransmissionInterval(50); // Speed up telemetry updates, Just use for debugging. + telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); + + // WARNING: To be able to view the stream preview on the Driver Station, this code runs in INIT mode. + while (opModeIsActive() || opModeInInit()) + { + telemetry.addData("preview on/off", "... Camera Stream\n"); + + // Read the current list + List blobs = colorLocator.getBlobs(); + + /* + * The list of Blobs can be filtered to remove unwanted Blobs. + * Note: All contours will be still displayed on the Stream Preview, but only those that satisfy the filter + * conditions will remain in the current list of "blobs". Multiple filters may be used. + * + * Use any of the following filters. + * + * ColorBlobLocatorProcessor.Util.filterByArea(minArea, maxArea, blobs); + * A Blob's area is the number of pixels contained within the Contour. Filter out any that are too big or small. + * Start with a large range and then refine the range based on the likely size of the desired object in the viewfinder. + * + * ColorBlobLocatorProcessor.Util.filterByDensity(minDensity, maxDensity, blobs); + * A blob's density is an indication of how "full" the contour is. + * If you put a rubber band around the contour you would get the "Convex Hull" of the contour. + * The density is the ratio of Contour-area to Convex Hull-area. + * + * ColorBlobLocatorProcessor.Util.filterByAspectRatio(minAspect, maxAspect, blobs); + * A blob's Aspect ratio is the ratio of boxFit long side to short side. + * A perfect Square has an aspect ratio of 1. All others are > 1 + */ + ColorBlobLocatorProcessor.Util.filterByArea(50, 20000, blobs); // filter out very small blobs. + + /* + * The list of Blobs can be sorted using the same Blob attributes as listed above. + * No more than one sort call should be made. Sorting can use ascending or descending order. + * ColorBlobLocatorProcessor.Util.sortByArea(SortOrder.DESCENDING, blobs); // Default + * ColorBlobLocatorProcessor.Util.sortByDensity(SortOrder.DESCENDING, blobs); + * ColorBlobLocatorProcessor.Util.sortByAspectRatio(SortOrder.DESCENDING, blobs); + */ + + telemetry.addLine(" Area Density Aspect Center"); + + // Display the size (area) and center location for each Blob. + for(ColorBlobLocatorProcessor.Blob b : blobs) + { + RotatedRect boxFit = b.getBoxFit(); + telemetry.addLine(String.format("%5d %4.2f %5.2f (%3d,%3d)", + b.getContourArea(), b.getDensity(), b.getAspectRatio(), (int) boxFit.center.x, (int) boxFit.center.y)); + } + + telemetry.update(); + sleep(50); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java new file mode 100644 index 0000000..6be2bc4 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java @@ -0,0 +1,136 @@ +/* + * Copyright (c) 2024 Phil Malone + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import android.graphics.Color; +import android.util.Size; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.opencv.ImageRegion; +import org.firstinspires.ftc.vision.opencv.PredominantColorProcessor; + +/* + * This OpMode illustrates how to use a video source (camera) as a color sensor + * + * A "color sensor" will typically determine the color of the object that it is pointed at. + * + * This sample performs the same function, except it uses a video camera to inspect an object or scene. + * The user may choose to inspect all, or just a Region of Interest (ROI), of the active camera view. + * The user must also provide a list of "acceptable colors" (Swatches) from which the closest matching color will be selected. + * + * To perform this function, a VisionPortal runs a PredominantColorProcessor process. + * The PredominantColorProcessor process is created first, and then the VisionPortal is built to use this process. + * The PredominantColorProcessor analyses the ROI and splits the colored pixels into several color-clusters. + * The largest of these clusters is then considered to be the "Predominant Color" + * The process then matches the Predominant Color with the closest Swatch and returns that match. + * + * To aid the user, a colored rectangle is drawn on the camera preview to show the RegionOfInterest, + * The Predominant Color is used to paint the rectangle border, so the user can verify that the color is reasonable. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@Disabled +@TeleOp(name = "Concept: Vision Color-Sensor", group = "Concept") +public class ConceptVisionColorSensor extends LinearOpMode +{ + @Override + public void runOpMode() + { + /* Build a "Color Sensor" vision processor based on the PredominantColorProcessor class. + * + * - Focus the color sensor by defining a RegionOfInterest (ROI) which you want to inspect. + * This can be the entire frame, or a sub-region defined using: + * 1) standard image coordinates or 2) a normalized +/- 1.0 coordinate system. + * Use one form of the ImageRegion class to define the ROI. + * ImageRegion.entireFrame() + * ImageRegion.asImageCoordinates(50, 50, 150, 150) 100x100 pixel square near the upper left corner + * ImageRegion.asUnityCenterCoordinates(-0.1, 0.1, 0.1, -0.1) 10% width/height square centered on screen + * + * - Set the list of "acceptable" color swatches (matches). + * Only colors that you assign here will be returned. + * If you know the sensor will be pointing to one of a few specific colors, enter them here. + * Or, if the sensor may be pointed randomly, provide some additional colors that may match the surrounding. + * Possible choices are: + * RED, ORANGE, YELLOW, GREEN, CYAN, BLUE, PURPLE, MAGENTA, BLACK, WHITE; + * + * Note that in the example shown below, only some of the available colors are included. + * This will force any other colored region into one of these colors. + * eg: Green may be reported as YELLOW, as this may be the "closest" match. + */ + PredominantColorProcessor colorSensor = new PredominantColorProcessor.Builder() + .setRoi(ImageRegion.asUnityCenterCoordinates(-0.1, 0.1, 0.1, -0.1)) + .setSwatches( + PredominantColorProcessor.Swatch.RED, + PredominantColorProcessor.Swatch.BLUE, + PredominantColorProcessor.Swatch.YELLOW, + PredominantColorProcessor.Swatch.BLACK, + PredominantColorProcessor.Swatch.WHITE) + .build(); + + /* + * Build a vision portal to run the Color Sensor process. + * + * - Add the colorSensor process created above. + * - Set the desired video resolution. + * Since a high resolution will not improve this process, choose a lower resolution that is + * supported by your camera. This will improve overall performance and reduce latency. + * - Choose your video source. This may be + * .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) ..... for a webcam + * or + * .setCamera(BuiltinCameraDirection.BACK) ... for a Phone Camera + */ + VisionPortal portal = new VisionPortal.Builder() + .addProcessor(colorSensor) + .setCameraResolution(new Size(320, 240)) + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .build(); + + telemetry.setMsTransmissionInterval(50); // Speed up telemetry updates, Just use for debugging. + + // WARNING: To be able to view the stream preview on the Driver Station, this code runs in INIT mode. + while (opModeIsActive() || opModeInInit()) + { + telemetry.addData("DS preview on/off", "3 dots, Camera Stream\n"); + + // Request the most recent color analysis. + // This will return the closest matching colorSwatch and the predominant RGB color. + // Note: to take actions based on the detected color, simply use the colorSwatch in a comparison or switch. + // eg: + // if (result.closestSwatch == PredominantColorProcessor.Swatch.RED) {... some code ...} + PredominantColorProcessor.Result result = colorSensor.getAnalysis(); + + // Display the Color Sensor result. + telemetry.addData("Best Match:", result.closestSwatch); + telemetry.addLine(String.format("R %3d, G %3d, B %3d", Color.red(result.rgb), Color.green(result.rgb), Color.blue(result.rgb))); + telemetry.update(); + + sleep(20); + } + } +} diff --git a/README.md b/README.md index 5353afb..f7a02c9 100644 --- a/README.md +++ b/README.md @@ -59,6 +59,24 @@ The readme.md file located in the [/TeamCode/src/main/java/org/firstinspires/ftc # Release Information +## Version 10.1 (20240919-122750) + +### Enhancements +* Adds new OpenCV-based `VisionProcessor`s (which may be attached to a VisionPortal in either Java or Blocks) to help teams implement color processing via computer vision in the INTO THE DEEP game + * `ColorBlobLocatorProcessor` implements OpenCV color "blob" detection. A new sample program `ConceptVisionColorLocator` demonstrates its use. + * A choice is offered between pre-defined color ranges, or creating a custom one in RGB, HSV, or YCrCb color space + * The ability is provided to restrict detection to a specified Region of Interest on the screen + * Functions for applying erosion / dilation morphing to the threshold mask are provided + * Functions for sorting and filtering the returned data are provided + * `PredominantColorProcessor` allows using a region of the camera as a "long range color sensor" to determine the predominant color of that region. A new sample program `ConceptVisionColorSensor` demonstrates its use. + * The determined predominant color is selected from a discrete set of color "swatches", similar to the MINDSTORMS NXT color sensor + * Documentation on this Color Processing feature can be found here: https://ftc-docs.firstinspires.org/color-processing +* Added Blocks sample programs for color sensors: RobotAutoDriveToLine and SensorColor. +* Updated Self-Inspect to identify mismatched RC/DS software versions as a "caution" rather than a "failure." + +### Bug Fixes +* Fixes [AngularVelocity conversion regression](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/1070) + ## Version 10.0 (20240828-111152) ### Breaking Changes @@ -469,7 +487,7 @@ This is a bug fix only release to address the following four issues. * Fixes [issue #260](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/260) Blocks can't call java method that has a VuforiaLocalizer parameter. * Blocks now has a block labeled VuforiaFreightFrenzy.getVuforiaLocalizer for this. * Added a page to manage the TensorFlow Lite models in /sdcard/FIRST/tflitemodels. To get to the TFLite Models page: - * You can click on the link at the bottom of the the Manage page. + * You can click on the link at the bottom of the Manage page. * You can click on the link at the upper-right the Blocks project page. * Fixes logspam when `isBusy()` is called on a motor not in RTP mode. * Hides the "RC Password" item on the inspection screen for phone-based Robot Controllers. (It is only applicable for Control Hubs). @@ -1154,7 +1172,7 @@ Known issues: This version of the software provides support for the REV Robotics Expansion Hub. This version also includes improvements in the USB communication layer in an effort to enhance system resiliency. If you were using a 2.x version of the software previously, updating to version 3.1 requires that you also update your Driver Station software in addition to updating the Robot Controller software. -Also note that in version 3.10 software, the setMaxSpeed and getMaxSpeed methods are no longer available (not deprecated, they have been removed from the SDK). Also note that the the new 3.x software incorporates motor profiles that a user can select as he/she configures the robot. +Also note that in version 3.10 software, the setMaxSpeed and getMaxSpeed methods are no longer available (not deprecated, they have been removed from the SDK). Also note that the new 3.x software incorporates motor profiles that a user can select as he/she configures the robot. Changes include: * Blocks changes diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 90ef1b5..4061db7 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -6,14 +6,14 @@ repositories { } dependencies { - implementation 'org.firstinspires.ftc:Inspection:10.0.0' - implementation 'org.firstinspires.ftc:Blocks:10.0.0' - implementation 'org.firstinspires.ftc:RobotCore:10.0.0' - implementation 'org.firstinspires.ftc:RobotServer:10.0.0' - implementation 'org.firstinspires.ftc:OnBotJava:10.0.0' - implementation 'org.firstinspires.ftc:Hardware:10.0.0' - implementation 'org.firstinspires.ftc:FtcCommon:10.0.0' - implementation 'org.firstinspires.ftc:Vision:10.0.0' + implementation 'org.firstinspires.ftc:Inspection:10.1.0' + implementation 'org.firstinspires.ftc:Blocks:10.1.0' + implementation 'org.firstinspires.ftc:RobotCore:10.1.0' + implementation 'org.firstinspires.ftc:RobotServer:10.1.0' + implementation 'org.firstinspires.ftc:OnBotJava:10.1.0' + implementation 'org.firstinspires.ftc:Hardware:10.1.0' + implementation 'org.firstinspires.ftc:FtcCommon:10.1.0' + implementation 'org.firstinspires.ftc:Vision:10.1.0' implementation 'androidx.appcompat:appcompat:1.2.0' implementation 'com.acmerobotics.dashboard:dashboard:0.4.5' From 6e82dc98df2fef0209c19000192e1ebd460fe7e4 Mon Sep 17 00:00:00 2001 From: Anyi Lin Date: Sat, 21 Sep 2024 02:52:11 -0400 Subject: [PATCH 48/94] version 10.1 sdk --- FtcRobotController/src/main/AndroidManifest.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/FtcRobotController/src/main/AndroidManifest.xml b/FtcRobotController/src/main/AndroidManifest.xml index 3b87d9d..fc3aaf5 100644 --- a/FtcRobotController/src/main/AndroidManifest.xml +++ b/FtcRobotController/src/main/AndroidManifest.xml @@ -1,8 +1,8 @@ + android:versionCode="56" + android:versionName="10.1"> From ce3ae6c03b47a96e3419834dfe9685a7ff913ba0 Mon Sep 17 00:00:00 2001 From: Anyi Lin Date: Sun, 29 Sep 2024 00:08:57 -0400 Subject: [PATCH 49/94] OTOSLocalizer fix --- .../pedroPathing/localization/localizers/OTOSLocalizer.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java index cdacd52..6a0bb38 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java @@ -64,13 +64,12 @@ public class OTOSLocalizer extends Localizer { public OTOSLocalizer(HardwareMap map, Pose setStartPose) { hardwareMap = map; - // TODO: replace this with your OTOS port /* TODO: If you want to use the "SparkFunOTOSCorrected" version of OTOS, then replace the 'SparkFunOTOS.class' below with 'SparkFunOTOSCorrected.class' and set the OTOS as a - "SparkFunOTOS Corrected" in your robot confg + "SparkFunOTOS Corrected" in your robot config */ - SparkFunOTOS + // TODO: replace this with your OTOS port otos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos"); otos.setLinearUnit(DistanceUnit.INCH); From e0a96df549b0cae6f97536532b06172d8061df9c Mon Sep 17 00:00:00 2001 From: Anyi Lin Date: Thu, 10 Oct 2024 03:46:24 -0400 Subject: [PATCH 50/94] optimized OTOSLocalizer thanks to j5155 --- .../localization/localizers/OTOSLocalizer.java | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java index 6a0bb38..e4e1f34 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java @@ -41,6 +41,9 @@ public class OTOSLocalizer extends Localizer { private HardwareMap hardwareMap; private Pose startPose; private SparkFunOTOS otos; + private SparkFunOTOS.Pose2D otosPose; + private SparkFunOTOS.Pose2D otosVel; + private SparkFunOTOS.Pose2D otosAcc; private double previousHeading; private double totalHeading; @@ -89,6 +92,9 @@ public class OTOSLocalizer extends Localizer { otos.resetTracking(); setStartPose(setStartPose); + otosPose = new SparkFunOTOS.Pose2D(); + otosVel = new SparkFunOTOS.Pose2D(); + otosAcc = new SparkFunOTOS.Pose2D(); totalHeading = 0; previousHeading = startPose.getHeading(); @@ -102,8 +108,7 @@ public class OTOSLocalizer extends Localizer { */ @Override public Pose getPose() { - SparkFunOTOS.Pose2D pose = otos.getPosition(); - return MathFunctions.addPoses(startPose, new Pose(pose.x, pose.y, pose.h)); + return MathFunctions.addPoses(startPose, new Pose(otosPose.x, otosPose.y, otosPose.h)); } /** @@ -113,8 +118,7 @@ public class OTOSLocalizer extends Localizer { */ @Override public Pose getVelocity() { - SparkFunOTOS.Pose2D OTOSVelocity = otos.getVelocity(); - return new Pose(OTOSVelocity.x, OTOSVelocity.y, OTOSVelocity.h); + return new Pose(otosVel.x, otosVel.y, otosVel.h); } /** @@ -156,8 +160,9 @@ public class OTOSLocalizer extends Localizer { */ @Override public void update() { - totalHeading += MathFunctions.getSmallestAngleDifference(otos.getPosition().h, previousHeading); - previousHeading = otos.getPosition().h; + otos.getPosVelAcc(otosPose,otosVel,otosAcc); + totalHeading += MathFunctions.getSmallestAngleDifference(otosPose.h, previousHeading); + previousHeading = otosPose.h; } /** From f5ac5ea8e961945412546400353199b4eb8d47de Mon Sep 17 00:00:00 2001 From: Logan-Nash Date: Thu, 17 Oct 2024 14:09:35 -0400 Subject: [PATCH 51/94] Added Pinpoint Localization! --- .../ftc/teamcode/pedroPathing/LOCALIZATION.md | 20 +- .../ftc/teamcode/pedroPathing/OVERVIEW.md | 3 +- .../localization/GoBildaPinpointDriver.java | 513 ++++++++++++++++++ .../localizers/PinpointLocalizer.java | 211 +++++++ .../tuning/SensorGoBildaPinpointExample.java | 200 +++++++ 5 files changed, 945 insertions(+), 2 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/SensorGoBildaPinpointExample.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/LOCALIZATION.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/LOCALIZATION.md index 18d1206..8343ee9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/LOCALIZATION.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/LOCALIZATION.md @@ -4,7 +4,7 @@ the pose exponential method of localization. It's basically a way of turning mov robot's coordinate frame to the global coordinate frame. If you're interested in reading more about it, then check out pages 177 - 183 of [Controls Engineering in the FIRST Robotics Competition](https://file.tavsys.net/control/controls-engineering-in-frc.pdf) by Tyler Veness. However, the OTOS localizer uses its own onboard system for calculating localization, -which I do not know about. +which we do not know about. ## Setting Your Localizer Go to line `70` in the `PoseUpdater` class, and replace the `new ThreeWheelLocalizer(hardwareMap)` @@ -15,6 +15,7 @@ with the localizer that applies to you: don't change it from the default * If you're using three wheel odometry with the IMU, put `new ThreeWheelIMULocalizer(hardwareMap)` * If you're using OTOS, put `new OTOSLocalizer(hardwareMap)` +* If you're using Pinpoint, put `new PinpointLocalizer(hardwareMap)` ## Tuning To start, you'll want to select your localizer of choice. Below, I'll have instructions for the drive @@ -241,6 +242,23 @@ that applies to you and follow the directions there. robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer! +## Pinpoint Localizer +* First you will need to plug in the pinpoint to the i2c port. Ensure that the dead wheel encoder wires are + plugged into the proper ports on the pinpoint to ensure no error within the tuning steps. +* Then, go to the `PinpointLocalier.java` file and go to where it tells you to replace + the current statement with your pinpoint port in the constructor. Replace the `deviceName` parameter + with the name of the port that the pinpoint is connected to. +* Next, follow the instructions left by the TODO: comment and enter in the odometry measurements either in + mms or inches (We have the conversion rates listed). +* First, to ensure that your pinpoint is properly connected, please run the `SensorGoBildaPinpointExample.java` + file left in the `tuning` folder located within `localization`. +* Once completed, the localizer should be properly tuned. To test it out, you can go to + `Localization Test` and push around or drive around your robot. Go to [FTC Dashboard](http://192.168.43.1:8080/dash) + and on the top right, switch the drop down from the default view to the field view. Then, on the bottom + left corner, you should see a field and the robot being drawn on the field. You can then move your + robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll + want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer! + ## Using Road Runner's Localizer Of course, many teams have experience using Road Runner in the past and so have localizers from Road Runner that are tuned. There is an adapter for the Road Runner three wheel localizer to the Pedro diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md index a0dbca1..f8c8868 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md @@ -105,4 +105,5 @@ of the curvature formula, we can estimate a centripetal force correction and app control. ## Questions? -If you still have more questions, feel free to contact us at `scottsbots10158@gmail.com` \ No newline at end of file +If you still have more questions, feel free to contact us at `scottsbots10158@gmail.com` or +within our discord linked here(https://discord.gg/2GfC4qBP5s) \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java new file mode 100644 index 0000000..a2bc9d8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java @@ -0,0 +1,513 @@ + +/* MIT License + * Copyright (c) [2024] [Base 10 Assets, LLC] + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import static com.qualcomm.robotcore.util.TypeConversion.byteArrayToInt; + +import com.qualcomm.hardware.lynx.LynxI2cDeviceSynch; +import com.qualcomm.robotcore.hardware.I2cAddr; +import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice; +import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple; +import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties; +import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType; +import com.qualcomm.robotcore.util.TypeConversion; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; + +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import java.util.Arrays; + + +@I2cDeviceType +@DeviceProperties( + name = "goBILDA® Pinpoint Odometry Computer", + xmlTag = "goBILDAPinpoint", + description ="goBILDA® Pinpoint Odometry Computer (IMU Sensor Fusion for 2 Wheel Odometry)" +) + +public class GoBildaPinpointDriver extends I2cDeviceSynchDevice { + + private int deviceStatus = 0; + private int loopTime = 0; + private int xEncoderValue = 0; + private int yEncoderValue = 0; + private float xPosition = 0; + private float yPosition = 0; + private float hOrientation = 0; + private float xVelocity = 0; + private float yVelocity = 0; + private float hVelocity = 0; + + private static final float goBILDA_SWINGARM_POD = 13.26291192f; //ticks-per-mm for the goBILDA Swingarm Pod + private static final float goBILDA_4_BAR_POD = 19.89436789f; //ticks-per-mm for the goBILDA 4-Bar Pod + + //i2c address of the device + public static final byte DEFAULT_ADDRESS = 0x31; + + public GoBildaPinpointDriver(I2cDeviceSynchSimple deviceClient, boolean deviceClientIsOwned) { + super(deviceClient, deviceClientIsOwned); + + this.deviceClient.setI2cAddress(I2cAddr.create7bit(DEFAULT_ADDRESS)); + super.registerArmingStateCallback(false); + } + + + @Override + public Manufacturer getManufacturer() { + return Manufacturer.Other; + } + + @Override + protected synchronized boolean doInitialize() { + ((LynxI2cDeviceSynch)(deviceClient)).setBusSpeed(LynxI2cDeviceSynch.BusSpeed.FAST_400K); + return true; + } + + @Override + public String getDeviceName() { + return "goBILDA® Pinpoint Odometry Computer"; + } + + + //Register map of the i2c device + private enum Register { + DEVICE_ID (1), + DEVICE_VERSION (2), + DEVICE_STATUS (3), + DEVICE_CONTROL (4), + LOOP_TIME (5), + X_ENCODER_VALUE (6), + Y_ENCODER_VALUE (7), + X_POSITION (8), + Y_POSITION (9), + H_ORIENTATION (10), + X_VELOCITY (11), + Y_VELOCITY (12), + H_VELOCITY (13), + MM_PER_TICK (14), + X_POD_OFFSET (15), + Y_POD_OFFSET (16), + YAW_SCALAR (17), + BULK_READ (18); + + private final int bVal; + + Register(int bVal){ + this.bVal = bVal; + } + } + + //Device Status enum that captures the current fault condition of the device + public enum DeviceStatus{ + NOT_READY (0), + READY (1), + CALIBRATING (1 << 1), + FAULT_X_POD_NOT_DETECTED (1 << 2), + FAULT_Y_POD_NOT_DETECTED (1 << 3), + FAULT_NO_PODS_DETECTED (1 << 2 | 1 << 3), + FAULT_IMU_RUNAWAY (1 << 4); + + private final int status; + + DeviceStatus(int status){ + this.status = status; + } + } + + //enum that captures the direction the encoders are set to + public enum EncoderDirection{ + FORWARD, + REVERSED; + } + + //enum that captures the kind of goBILDA odometry pods, if goBILDA pods are used + public enum GoBildaOdometryPods { + goBILDA_SWINGARM_POD, + goBILDA_4_BAR_POD; + } + //enum that captures a limited scope of read data. More options may be added in future update + public enum readData { + ONLY_UPDATE_HEADING, + } + + + /** Writes an int to the i2c device + @param reg the register to write the int to + @param i the integer to write to the register + */ + private void writeInt(final Register reg, int i){ + deviceClient.write(reg.bVal, TypeConversion.intToByteArray(i,ByteOrder.LITTLE_ENDIAN)); + } + + /** + * Reads an int from a register of the i2c device + * @param reg the register to read from + * @return returns an int that contains the value stored in the read register + */ + private int readInt(Register reg){ + return byteArrayToInt(deviceClient.read(reg.bVal,4), ByteOrder.LITTLE_ENDIAN); + } + + /** + * Converts a byte array to a float value + * @param byteArray byte array to transform + * @param byteOrder order of byte array to convert + * @return the float value stored by the byte array + */ + private float byteArrayToFloat(byte[] byteArray, ByteOrder byteOrder){ + return ByteBuffer.wrap(byteArray).order(byteOrder).getFloat(); + } + /** + * Reads a float from a register + * @param reg the register to read + * @return the float value stored in that register + */ + + private float readFloat(Register reg){ + return byteArrayToFloat(deviceClient.read(reg.bVal,4),ByteOrder.LITTLE_ENDIAN); + } + + + /** + * Converts a float to a byte array + * @param value the float array to convert + * @return the byte array converted from the float + */ + private byte [] floatToByteArray (float value, ByteOrder byteOrder) { + return ByteBuffer.allocate(4).order(byteOrder).putFloat(value).array(); + } + + /** + * Writes a byte array to a register on the i2c device + * @param reg the register to write to + * @param bytes the byte array to write + */ + private void writeByteArray (Register reg, byte[] bytes){ + deviceClient.write(reg.bVal,bytes); + } + + /** + * Writes a float to a register on the i2c device + * @param reg the register to write to + * @param f the float to write + */ + private void writeFloat (Register reg, float f){ + byte[] bytes = ByteBuffer.allocate(4).order(ByteOrder.LITTLE_ENDIAN).putFloat(f).array(); + deviceClient.write(reg.bVal,bytes); + } + + /** + * Looks up the DeviceStatus enum corresponding with an int value + * @param s int to lookup + * @return the Odometry Computer state + */ + private DeviceStatus lookupStatus (int s){ + if ((s & DeviceStatus.CALIBRATING.status) != 0){ + return DeviceStatus.CALIBRATING; + } + boolean xPodDetected = (s & DeviceStatus.FAULT_X_POD_NOT_DETECTED.status) == 0; + boolean yPodDetected = (s & DeviceStatus.FAULT_Y_POD_NOT_DETECTED.status) == 0; + + if(!xPodDetected && !yPodDetected){ + return DeviceStatus.FAULT_NO_PODS_DETECTED; + } + if (!xPodDetected){ + return DeviceStatus.FAULT_X_POD_NOT_DETECTED; + } + if (!yPodDetected){ + return DeviceStatus.FAULT_Y_POD_NOT_DETECTED; + } + if ((s & DeviceStatus.FAULT_IMU_RUNAWAY.status) != 0){ + return DeviceStatus.FAULT_IMU_RUNAWAY; + } + if ((s & DeviceStatus.READY.status) != 0){ + return DeviceStatus.READY; + } + else { + return DeviceStatus.NOT_READY; + } + } + + /** + * Call this once per loop to read new data from the Odometry Computer. Data will only update once this is called. + */ + public void update(){ + byte[] bArr = deviceClient.read(Register.BULK_READ.bVal, 40); + deviceStatus = byteArrayToInt(Arrays.copyOfRange (bArr, 0, 4), ByteOrder.LITTLE_ENDIAN); + loopTime = byteArrayToInt(Arrays.copyOfRange (bArr, 4, 8), ByteOrder.LITTLE_ENDIAN); + xEncoderValue = byteArrayToInt(Arrays.copyOfRange (bArr, 8, 12), ByteOrder.LITTLE_ENDIAN); + yEncoderValue = byteArrayToInt(Arrays.copyOfRange (bArr, 12,16), ByteOrder.LITTLE_ENDIAN); + xPosition = byteArrayToFloat(Arrays.copyOfRange(bArr, 16,20), ByteOrder.LITTLE_ENDIAN); + yPosition = byteArrayToFloat(Arrays.copyOfRange(bArr, 20,24), ByteOrder.LITTLE_ENDIAN); + hOrientation = byteArrayToFloat(Arrays.copyOfRange(bArr, 24,28), ByteOrder.LITTLE_ENDIAN); + xVelocity = byteArrayToFloat(Arrays.copyOfRange(bArr, 28,32), ByteOrder.LITTLE_ENDIAN); + yVelocity = byteArrayToFloat(Arrays.copyOfRange(bArr, 32,36), ByteOrder.LITTLE_ENDIAN); + hVelocity = byteArrayToFloat(Arrays.copyOfRange(bArr, 36,40), ByteOrder.LITTLE_ENDIAN); + } + + /** + * Call this once per loop to read new data from the Odometry Computer. This is an override of the update() function + * which allows a narrower range of data to be read from the device for faster read times. Currently ONLY_UPDATE_HEADING + * is supported. + * @param data GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING + */ + public void update(readData data) { + if (data == readData.ONLY_UPDATE_HEADING) { + hOrientation = byteArrayToFloat(deviceClient.read(Register.H_ORIENTATION.bVal, 4), ByteOrder.LITTLE_ENDIAN); + } + } + + /** + * Sets the odometry pod positions relative to the point that the odometry computer tracks around.

+ * The most common tracking position is the center of the robot.

+ * The X pod offset refers to how far sideways (in mm) from the tracking point the X (forward) odometry pod is. Left of the center is a positive number, right of center is a negative number.
+ * the Y pod offset refers to how far forwards (in mm) from the tracking point the Y (strafe) odometry pod is. forward of center is a positive number, backwards is a negative number.
+ * @param xOffset how sideways from the center of the robot is the X (forward) pod? Left increases + * @param yOffset how far forward from the center of the robot is the Y (Strafe) pod? forward increases + */ + public void setOffsets(double xOffset, double yOffset){ + writeFloat(Register.X_POD_OFFSET, (float) xOffset); + writeFloat(Register.Y_POD_OFFSET, (float) yOffset); + } + + /** + * Recalibrates the Odometry Computer's internal IMU.

+ * Robot MUST be stationary

+ * Device takes a large number of samples, and uses those as the gyroscope zero-offset. This takes approximately 0.25 seconds. + */ + public void recalibrateIMU(){writeInt(Register.DEVICE_CONTROL,1<<0);} + + /** + * Resets the current position to 0,0,0 and recalibrates the Odometry Computer's internal IMU.

+ * Robot MUST be stationary

+ * Device takes a large number of samples, and uses those as the gyroscope zero-offset. This takes approximately 0.25 seconds. + */ + public void resetPosAndIMU(){writeInt(Register.DEVICE_CONTROL,1<<1);} + + /** + * Can reverse the direction of each encoder. + * @param xEncoder FORWARD or REVERSED, X (forward) pod should increase when the robot is moving forward + * @param yEncoder FORWARD or REVERSED, Y (strafe) pod should increase when the robot is moving left + */ + public void setEncoderDirections(EncoderDirection xEncoder, EncoderDirection yEncoder){ + if (xEncoder == EncoderDirection.REVERSED) { + writeInt(Register.DEVICE_CONTROL,1<<4); + } + if (yEncoder == EncoderDirection.REVERSED){ + writeInt(Register.DEVICE_CONTROL,1<<2); + } + } + + /** + * If you're using goBILDA odometry pods, the ticks-per-mm values are stored here for easy access.

+ * @param pods goBILDA_SWINGARM_POD or goBILDA_4_BAR_POD + */ + public void setEncoderResolution(GoBildaOdometryPods pods){ + if (pods == GoBildaOdometryPods.goBILDA_SWINGARM_POD) { + writeByteArray(Register.MM_PER_TICK, (floatToByteArray(goBILDA_SWINGARM_POD, ByteOrder.LITTLE_ENDIAN))); + } + if (pods == GoBildaOdometryPods.goBILDA_4_BAR_POD){ + writeByteArray(Register.MM_PER_TICK,(floatToByteArray(goBILDA_4_BAR_POD, ByteOrder.LITTLE_ENDIAN))); + } + } + + /** + * Sets the encoder resolution in ticks per mm of the odometry pods.
+ * You can find this number by dividing the counts-per-revolution of your encoder by the circumference of the wheel. + * @param ticks_per_mm should be somewhere between 10 ticks/mm and 100 ticks/mm a goBILDA Swingarm pod is ~13.26291192 + */ + public void setEncoderResolution(double ticks_per_mm){ + writeByteArray(Register.MM_PER_TICK,(floatToByteArray((float) ticks_per_mm,ByteOrder.LITTLE_ENDIAN))); + } + + /** + * Tuning this value should be unnecessary.
+ * The goBILDA Odometry Computer has a per-device tuned yaw offset already applied when you receive it.

+ * This is a scalar that is applied to the gyro's yaw value. Increasing it will mean it will report more than one degree for every degree the sensor fusion algorithm measures.

+ * You can tune this variable by rotating the robot a large amount (10 full turns is a good starting place) and comparing the amount that the robot rotated to the amount measured. + * Rotating the robot exactly 10 times should measure 3600°. If it measures more or less, divide moved amount by the measured amount and apply that value to the Yaw Offset.

+ * If you find that to get an accurate heading number you need to apply a scalar of more than 1.05, or less than 0.95, your device may be bad. Please reach out to tech@gobilda.com + * @param yawOffset A scalar for the robot's heading. + */ + public void setYawScalar(double yawOffset){ + writeByteArray(Register.YAW_SCALAR,(floatToByteArray((float) yawOffset, ByteOrder.LITTLE_ENDIAN))); + } + + /** + * Send a position that the Pinpoint should use to track your robot relative to. You can use this to + * update the estimated position of your robot with new external sensor data, or to run a robot + * in field coordinates.

+ * This overrides the current position.

+ * Using this feature to track your robot's position in field coordinates:
+ * When you start your code, send a Pose2D that describes the starting position on the field of your robot.
+ * Say you're on the red alliance, your robot is against the wall and closer to the audience side, + * and the front of your robot is pointing towards the center of the field. + * You can send a setPosition with something like -600mm x, -1200mm Y, and 90 degrees. The pinpoint would then always + * keep track of how far away from the center of the field you are.

+ * Using this feature to update your position with additional sensors:
+ * Some robots have a secondary way to locate their robot on the field. This is commonly + * Apriltag localization in FTC, but it can also be something like a distance sensor. + * Often these external sensors are absolute (meaning they measure something about the field) + * so their data is very accurate. But they can be slower to read, or you may need to be in a very specific + * position on the field to use them. In that case, spend most of your time relying on the Pinpoint + * to determine your location. Then when you pull a new position from your secondary sensor, + * send a setPosition command with the new position. The Pinpoint will then track your movement + * relative to that new, more accurate position. + * @param pos a Pose2D describing the robot's new position. + */ + public Pose2D setPosition(Pose2D pos){ + writeByteArray(Register.X_POSITION,(floatToByteArray((float) pos.getX(DistanceUnit.MM), ByteOrder.LITTLE_ENDIAN))); + writeByteArray(Register.Y_POSITION,(floatToByteArray((float) pos.getY(DistanceUnit.MM),ByteOrder.LITTLE_ENDIAN))); + writeByteArray(Register.H_ORIENTATION,(floatToByteArray((float) pos.getHeading(AngleUnit.RADIANS),ByteOrder.LITTLE_ENDIAN))); + return pos; + } + + /** + * Checks the deviceID of the Odometry Computer. Should return 1. + * @return 1 if device is functional. + */ + public int getDeviceID(){return readInt(Register.DEVICE_ID);} + + /** + * @return the firmware version of the Odometry Computer + */ + public int getDeviceVersion(){return readInt(Register.DEVICE_VERSION); } + + public float getYawScalar(){return readFloat(Register.YAW_SCALAR); } + + /** + * Device Status stores any faults the Odometry Computer may be experiencing. These faults include: + * @return one of the following states:
+ * NOT_READY - The device is currently powering up. And has not initialized yet. RED LED
+ * READY - The device is currently functioning as normal. GREEN LED
+ * CALIBRATING - The device is currently recalibrating the gyro. RED LED
+ * FAULT_NO_PODS_DETECTED - the device does not detect any pods plugged in. PURPLE LED
+ * FAULT_X_POD_NOT_DETECTED - The device does not detect an X pod plugged in. BLUE LED
+ * FAULT_Y_POD_NOT_DETECTED - The device does not detect a Y pod plugged in. ORANGE LED
+ */ + public DeviceStatus getDeviceStatus(){return lookupStatus(deviceStatus); } + + /** + * Checks the Odometry Computer's most recent loop time.

+ * If values less than 500, or more than 1100 are commonly seen here, there may be something wrong with your device. Please reach out to tech@gobilda.com + * @return loop time in microseconds (1/1,000,000 seconds) + */ + public int getLoopTime(){return loopTime; } + + /** + * Checks the Odometry Computer's most recent loop frequency.

+ * If values less than 900, or more than 2000 are commonly seen here, there may be something wrong with your device. Please reach out to tech@gobilda.com + * @return Pinpoint Frequency in Hz (loops per second), + */ + public double getFrequency(){ + if (loopTime != 0){ + return 1000000.0/loopTime; + } + else { + return 0; + } + } + + /** + * @return the raw value of the X (forward) encoder in ticks + */ + public int getEncoderX(){return xEncoderValue; } + + /** + * @return the raw value of the Y (strafe) encoder in ticks + */ + public int getEncoderY(){return yEncoderValue; } + + /** + * @return the estimated X (forward) position of the robot in mm + */ + public double getPosX(){return xPosition; } + + /** + * @return the estimated Y (Strafe) position of the robot in mm + */ + public double getPosY(){return yPosition; } + + /** + * @return the estimated H (heading) position of the robot in Radians + */ + public double getHeading(){return hOrientation;} + + /** + * @return the estimated X (forward) velocity of the robot in mm/sec + */ + public double getVelX(){return xVelocity; } + + /** + * @return the estimated Y (strafe) velocity of the robot in mm/sec + */ + public double getVelY(){return yVelocity; } + + /** + * @return the estimated H (heading) velocity of the robot in radians/sec + */ + public double getHeadingVelocity(){return hVelocity; } + + /** + * This uses its own I2C read, avoid calling this every loop. + * @return the user-set offset for the X (forward) pod + */ + public float getXOffset(){return readFloat(Register.X_POD_OFFSET);} + + /** + * This uses its own I2C read, avoid calling this every loop. + * @return the user-set offset for the Y (strafe) pod + */ + public float getYOffset(){return readFloat(Register.Y_POD_OFFSET);} + + /** + * @return a Pose2D containing the estimated position of the robot + */ + public Pose2D getPosition(){ + return new Pose2D(DistanceUnit.MM, + xPosition, + yPosition, + AngleUnit.RADIANS, + hOrientation); + } + + + + /** + * @return a Pose2D containing the estimated velocity of the robot, velocity is unit per second + */ + public Pose2D getVelocity(){ + return new Pose2D(DistanceUnit.MM, + xVelocity, + yVelocity, + AngleUnit.RADIANS, + hVelocity); + } + + + +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java new file mode 100644 index 0000000..3e564a8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java @@ -0,0 +1,211 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; + + +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.GoBildaPinpointDriver; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +/** + * This is the Pinpoint class. This class extends the Localizer superclass and is a + * localizer that uses the two wheel odometry set up with the IMU to have more accurate heading + * readings. The diagram below, which is modified from Road Runner, shows a typical set up. + * + * The view is from the top of the robot looking downwards. + * + * left on robot is the y positive direction + * + * forward on robot is the x positive direction + * + * /--------------\ + * | ____ | + * | ---- | + * | || | + * | || | ----> left (y positive) + * | | + * | | + * \--------------/ + * | + * | + * V + * forward (x positive) + * With the pinpoint your readings will be used in mm + * to use inches ensure to divide your mm value by 25.4 + * @author Logan Nash + * @author Havish Sripada 12808 - RevAmped Robotics + * @author Ethan Doak - Gobilda + * @version 1.0, 10/2/2024 + */ +public class PinpointLocalizer extends Localizer { + private HardwareMap hardwareMap; + private Pose startPose; + private GoBildaPinpointDriver odo; + private double previousHeading; + private double totalHeading; + + /** + * This creates a new PinpointLocalizer from a HardwareMap, with a starting Pose at (0,0) + * facing 0 heading. + * + * @param map the HardwareMap + */ + public PinpointLocalizer(HardwareMap map){ this(map, new Pose());} + + /** + * This creates a new PinpointLocalizer from a HardwareMap and a Pose, with the Pose + * specifying the starting pose of the localizer. + * + * @param map the HardwareMap + * @param setStartPose the Pose to start from + */ + public PinpointLocalizer(HardwareMap map, Pose setStartPose){ + hardwareMap = map; + // TODO: replace this with your Pinpoint port + odo = hardwareMap.get(GoBildaPinpointDriver.class,"odo"); + + //This uses mm, to use inches divide these numbers by 25.4 + odo.setOffsets(-84.0, -168.0); //these are tuned for 3110-0002-0001 Product Insight #1 + //TODO: If you find that the gobilda Yaw Scaling is incorrect you can edit this here + // odo.setYawScalar(1.0); + //TODO: Set your encoder resolution here, I have the Gobilda Odometry products already included. + //TODO: If you would like to use your own odometry pods input the ticks per mm in the commented part below + odo.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); + //odo.setEncoderResolution(13.26291192); + //TODO: Set encoder directions + odo.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD); + + odo.resetPosAndIMU(); + + setStartPose(setStartPose); + totalHeading = 0; + previousHeading = startPose.getHeading(); + + resetPinpoint(); + } + + /** + * This returns the current pose estimate. + * + * @return returns the current pose estimate as a Pose + */ + @Override + public Pose getPose() { + Pose2D pose = odo.getPosition(); + return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), pose.getHeading(AngleUnit.RADIANS)); + } + + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Pose + */ + @Override + public Pose getVelocity() { + Pose2D pose = odo.getVelocity(); + return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), pose.getHeading(AngleUnit.RADIANS)); + } + + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Vector + */ + @Override + public Vector getVelocityVector() { + Pose2D pose = odo.getVelocity(); + Vector returnVector = new Vector(); + returnVector.setOrthogonalComponents(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH)); + return returnVector; + } + + /** + * This sets the start pose. Changing the start pose should move the robot as if all its + * previous movements were displacing it from its new start pose. + * + * @param setStart the new start pose + */ + @Override + public void setStartPose(Pose setStart) {startPose = setStart;} + + /** + * This sets the current pose estimate. Changing this should just change the robot's current + * pose estimate, not anything to do with the start pose. + * + * @param setPose the new current pose estimate + */ + @Override + public void setPose(Pose setPose) { + resetPinpoint(); + Pose setPinpointPose = MathFunctions.subtractPoses(setPose, startPose); + odo.setPosition(new Pose2D(DistanceUnit.INCH, setPinpointPose.getY(), setPinpointPose.getX(), AngleUnit.RADIANS, setPinpointPose.getHeading())); + } + + /** + * This updates the total heading of the robot. The Pinpoint handles all other updates itself. + */ + @Override + public void update() { + odo.update(); + totalHeading += MathFunctions.getSmallestAngleDifference(odo.getHeading(),previousHeading); + previousHeading = odo.getHeading(); + } + + /** + * This returns how far the robot has turned in radians, in a number not clamped between 0 and + * 2 * pi radians. This is used for some tuning things and nothing actually within the following. + * + * @return returns how far the robot has turned in total, in radians. + */ + @Override + public double getTotalHeading() { + return totalHeading; + } + + /** + * This returns the Y encoder value as none of the odometry tuners are required for this localizer + * @return returns the Y encoder value + */ + @Override + public double getForwardMultiplier() { + return odo.getEncoderY(); + } + + /** + * This returns the X encoder value as none of the odometry tuners are required for this localizer + * @return returns the X encoder value + */ + @Override + public double getLateralMultiplier() { + return odo.getEncoderX(); + } + + /** + * This returns either the factory tuned yaw scalar or the yaw scalar tuned by yourself. + * @return returns the yaw scalar + */ + @Override + public double getTurningMultiplier() { + return odo.getYawScalar(); + } + + /** + * This resets the IMU. + */ + @Override + public void resetIMU() { + odo.recalibrateIMU(); + } + + /** + * This resets the OTOS. + */ + public void resetPinpoint(){ + odo.resetPosAndIMU(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/SensorGoBildaPinpointExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/SensorGoBildaPinpointExample.java new file mode 100644 index 0000000..9fbac95 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/SensorGoBildaPinpointExample.java @@ -0,0 +1,200 @@ +/* MIT License + * Copyright (c) [2024] [Base 10 Assets, LLC] + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.GoBildaPinpointDriver; + +import java.util.Locale; + +/* +This opmode shows how to use the goBILDA® Pinpoint Odometry Computer. +The goBILDA Odometry Computer is a device designed to solve the Pose Exponential calculation +commonly associated with Dead Wheel Odometry systems. It reads two encoders, and an integrated +system of senors to determine the robot's current heading, X position, and Y position. + +it uses an ESP32-S3 as a main cpu, with an STM LSM6DSV16X IMU. +It is validated with goBILDA "Dead Wheel" Odometry pods, but should be compatible with any +quadrature rotary encoder. The ESP32 PCNT peripheral is speced to decode quadrature encoder signals +at a maximum of 40mhz per channel. Though the maximum in-application tested number is 130khz. + +The device expects two perpendicularly mounted Dead Wheel pods. The encoder pulses are translated +into mm and their readings are transformed by an "offset", this offset describes how far away +the pods are from the "tracking point", usually the center of rotation of the robot. + +Dead Wheel pods should both increase in count when moved forwards and to the left. +The gyro will report an increase in heading when rotated counterclockwise. + +The Pose Exponential algorithm used is described on pg 181 of this book: +https://github.com/calcmogul/controls-engineering-in-frc + +For support, contact tech@gobilda.com + +-Ethan Doak + */ + +//TODO: If tuning comment out the @Disabled +@TeleOp(name="goBILDA® PinPoint Odometry Example", group="Linear OpMode") +@Disabled + +public class SensorGoBildaPinpointExample extends LinearOpMode { + + GoBildaPinpointDriver odo; // Declare OpMode member for the Odometry Computer + + double oldTime = 0; + + + @Override + public void runOpMode() { + + // Initialize the hardware variables. Note that the strings used here must correspond + // to the names assigned during the robot configuration step on the DS or RC devices. + + odo = hardwareMap.get(GoBildaPinpointDriver.class,"odo"); + + /* + Set the odometry pod positions relative to the point that the odometry computer tracks around. + The X pod offset refers to how far sideways from the tracking point the + X (forward) odometry pod is. Left of the center is a positive number, + right of center is a negative number. the Y pod offset refers to how far forwards from + the tracking point the Y (strafe) odometry pod is. forward of center is a positive number, + backwards is a negative number. + */ + odo.setOffsets(-84.0, -168.0); //these are tuned for 3110-0002-0001 Product Insight #1 + + /* + Set the kind of pods used by your robot. If you're using goBILDA odometry pods, select either + the goBILDA_SWINGARM_POD, or the goBILDA_4_BAR_POD. + If you're using another kind of odometry pod, uncomment setEncoderResolution and input the + number of ticks per mm of your odometry pod. + */ + odo.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); + //odo.setEncoderResolution(13.26291192); + + + /* + Set the direction that each of the two odometry pods count. The X (forward) pod should + increase when you move the robot forward. And the Y (strafe) pod should increase when + you move the robot to the left. + */ + odo.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD); + + + /* + Before running the robot, recalibrate the IMU. This needs to happen when the robot is stationary + The IMU will automatically calibrate when first powered on, but recalibrating before running + the robot is a good idea to ensure that the calibration is "good". + resetPosAndIMU will reset the position to 0,0,0 and also recalibrate the IMU. + This is recommended before you run your autonomous, as a bad initial calibration can cause + an incorrect starting value for x, y, and heading. + */ + //odo.recalibrateIMU(); + odo.resetPosAndIMU(); + + telemetry.addData("Status", "Initialized"); + telemetry.addData("X offset", odo.getXOffset()); + telemetry.addData("Y offset", odo.getYOffset()); + telemetry.addData("Device Version Number:", odo.getDeviceVersion()); + telemetry.addData("Device Scalar", odo.getYawScalar()); + telemetry.update(); + + // Wait for the game to start (driver presses START) + waitForStart(); + resetRuntime(); + + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + /* + Request an update from the Pinpoint odometry computer. This checks almost all outputs + from the device in a single I2C read. + */ + odo.update(); + + /* + Optionally, you can update only the heading of the device. This takes less time to read, but will not + pull any other data. Only the heading (which you can pull with getHeading() or in getPosition(). + */ + //odo.update(GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING); + + + if (gamepad1.a){ + odo.resetPosAndIMU(); //resets the position to 0 and recalibrates the IMU + } + + if (gamepad1.b){ + odo.recalibrateIMU(); //recalibrates the IMU without resetting position + } + + /* + This code prints the loop frequency of the REV Control Hub. This frequency is effected + by I²C reads/writes. So it's good to keep an eye on. This code calculates the amount + of time each cycle takes and finds the frequency (number of updates per second) from + that cycle time. + */ + double newTime = getRuntime(); + double loopTime = newTime-oldTime; + double frequency = 1/loopTime; + oldTime = newTime; + + + /* + gets the current Position (x & y in mm, and heading in degrees) of the robot, and prints it. + */ + Pose2D pos = odo.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + + /* + gets the current Velocity (x & y in mm/sec and heading in degrees/sec) and prints it. + */ + Pose2D vel = odo.getVelocity(); + String velocity = String.format(Locale.US,"{XVel: %.3f, YVel: %.3f, HVel: %.3f}", vel.getX(DistanceUnit.MM), vel.getY(DistanceUnit.MM), vel.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Velocity", velocity); + + + /* + Gets the Pinpoint device status. Pinpoint can reflect a few states. But we'll primarily see + READY: the device is working as normal + CALIBRATING: the device is calibrating and outputs are put on hold + NOT_READY: the device is resetting from scratch. This should only happen after a power-cycle + FAULT_NO_PODS_DETECTED - the device does not detect any pods plugged in + FAULT_X_POD_NOT_DETECTED - The device does not detect an X pod plugged in + FAULT_Y_POD_NOT_DETECTED - The device does not detect a Y pod plugged in + */ + telemetry.addData("Status", odo.getDeviceStatus()); + + telemetry.addData("Pinpoint Frequency", odo.getFrequency()); //prints/gets the current refresh rate of the Pinpoint + + telemetry.addData("REV Hub Frequency: ", frequency); //prints the control system refresh rate + telemetry.update(); + + } + }} \ No newline at end of file From c5f33af419733ae97abbbe75ee26c8ec646ddeb3 Mon Sep 17 00:00:00 2001 From: Anyi Lin Date: Thu, 17 Oct 2024 21:56:08 -0400 Subject: [PATCH 52/94] small fix to PinpointLocalizer --- .../pedroPathing/localization/localizers/PinpointLocalizer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java index 3e564a8..151311b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java @@ -143,7 +143,7 @@ public class PinpointLocalizer extends Localizer { public void setPose(Pose setPose) { resetPinpoint(); Pose setPinpointPose = MathFunctions.subtractPoses(setPose, startPose); - odo.setPosition(new Pose2D(DistanceUnit.INCH, setPinpointPose.getY(), setPinpointPose.getX(), AngleUnit.RADIANS, setPinpointPose.getHeading())); + odo.setPosition(new Pose2D(DistanceUnit.INCH, setPinpointPose.getX(), setPinpointPose.getY(), AngleUnit.RADIANS, setPinpointPose.getHeading())); } /** From 6830a8240318d27ba140610ef0e7f55a0e359c4e Mon Sep 17 00:00:00 2001 From: Logan-Nash Date: Fri, 25 Oct 2024 11:54:29 -0400 Subject: [PATCH 53/94] Added fixes to external odometry processors. Signed-off-by: Logan-Nash --- .../localization/localizers/OTOSLocalizer.java | 8 +++++++- .../localization/localizers/PinpointLocalizer.java | 9 +++++++-- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java index e4e1f34..2d38fd1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java @@ -108,7 +108,13 @@ public class OTOSLocalizer extends Localizer { */ @Override public Pose getPose() { - return MathFunctions.addPoses(startPose, new Pose(otosPose.x, otosPose.y, otosPose.h)); + SparkFunOTOS.Pose2D rawPose = otos.getPosition(); + Pose pose = new Pose(rawPose.x, rawPose.y, rawPose.h); + + Vector vec = pose.getVector(); + vec.rotateVector(startPose.getHeading()); + + return MathFunctions.addPoses(startPose, new Pose(vec.getXComponent(), vec.getYComponent(), pose.getHeading())); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java index 151311b..c36a0f7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java @@ -96,8 +96,13 @@ public class PinpointLocalizer extends Localizer { */ @Override public Pose getPose() { - Pose2D pose = odo.getPosition(); - return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), pose.getHeading(AngleUnit.RADIANS)); + Pose2D rawPose = odo.getPosition(); + Pose pose = new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS)); + + Vector vec = pose.getVector(); + vec.rotateVector(startPose.getHeading()); + + return MathFunctions.addPoses(startPose, new Pose(vec.getXComponent(), vec.getYComponent(), pose.getHeading())); } /** From 4e18dae336ce79df1290463dbefc4cf65abab1ca Mon Sep 17 00:00:00 2001 From: TBHGodPro Date: Sat, 26 Oct 2024 10:57:10 -0500 Subject: [PATCH 54/94] Improve OTOSLocalizer and PinpointLocalizer performance --- .../localization/localizers/OTOSLocalizer.java | 5 +---- .../localizers/PinpointLocalizer.java | 5 +---- .../pathGeneration/MathFunctions.java | 16 ++++++++++++++++ 3 files changed, 18 insertions(+), 8 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java index 2d38fd1..463caa0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java @@ -111,10 +111,7 @@ public class OTOSLocalizer extends Localizer { SparkFunOTOS.Pose2D rawPose = otos.getPosition(); Pose pose = new Pose(rawPose.x, rawPose.y, rawPose.h); - Vector vec = pose.getVector(); - vec.rotateVector(startPose.getHeading()); - - return MathFunctions.addPoses(startPose, new Pose(vec.getXComponent(), vec.getYComponent(), pose.getHeading())); + return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false)); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java index c36a0f7..ef0b133 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java @@ -99,10 +99,7 @@ public class PinpointLocalizer extends Localizer { Pose2D rawPose = odo.getPosition(); Pose pose = new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS)); - Vector vec = pose.getVector(); - vec.rotateVector(startPose.getHeading()); - - return MathFunctions.addPoses(startPose, new Pose(vec.getXComponent(), vec.getYComponent(), pose.getHeading())); + return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false)); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java index 0edfbfd..b43c6ff 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java @@ -172,6 +172,22 @@ public class MathFunctions { return new Pose(one.getX() - two.getX(), one.getY() - two.getY(), one.getHeading() - two.getHeading()); } + /** + * This rotates the given pose by the given theta, + * + * @param pose the Pose to rotate. + * @param theta the angle to rotate by. + * @param rotateHeading whether to adjust the Pose heading too. + * @return the rotated Pose. + */ + public static Pose rotatePose(Pose pose, double theta, boolean rotateHeading) { + double x = pose.getX() * Math.cos(theta) - pose.getY() * Math.sin(theta); + double y = pose.getX() * Math.sin(theta) + pose.getY() * Math.cos(theta); + double heading = rotateHeading ? normalizeAngle(pose.getHeading() + theta) : pose.getHeading(); + + return new Pose(x, y, heading); + } + /** * This multiplies a Point by a scalar and returns the result as a Point * From 2530445d0dae5eec54d77f35a63a98e88b404519 Mon Sep 17 00:00:00 2001 From: TBHGodPro Date: Mon, 28 Oct 2024 17:39:03 -0500 Subject: [PATCH 55/94] Multiple usability patches --- .../pedroPathing/follower/Follower.java | 46 ++++++++++--- .../pedroPathing/localization/Pose.java | 10 ++- .../pathGeneration/MathFunctions.java | 40 +++++------ .../pathGeneration/PathBuilder.java | 68 ++++++++++++++----- .../pedroPathing/pathGeneration/Point.java | 60 ++++++++++------ .../tuning/FollowerConstants.java | 8 +-- .../tuning/ForwardVelocityTuner.java | 6 +- .../ForwardZeroPowerAccelerationTuner.java | 8 ++- .../LateralZeroPowerAccelerationTuner.java | 10 ++- .../tuning/StrafeVelocityTuner.java | 8 ++- .../teamcode/pedroPathing/util/Drawing.java | 10 +-- 11 files changed, 187 insertions(+), 87 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index fe9d153..5089eb4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -1,19 +1,19 @@ package org.firstinspires.ftc.teamcode.pedroPathing.follower; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFSwitch; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.forwardZeroPowerAcceleration; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.lateralZeroPowerAcceleration; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFFeedForward; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.lateralZeroPowerAcceleration; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryDrivePIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryHeadingPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryTranslationalPIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFSwitch; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryDrivePID; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryHeadingPID; @@ -365,6 +365,25 @@ public class Follower { closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1); } + /** + * This holds a Point. + * + * @param point the Point to stay at. + * @param heading the heading to face. + */ + public void holdPoint(Point point, double heading) { + holdPoint(new BezierPoint(point), heading); + } + + /** + * This holds a Point. + * + * @param pose the Point (as a Pose) to stay at. + */ + public void holdPoint(Pose pose) { + holdPoint(new Point(pose), pose.getHeading()); + } + /** * This follows a Path. * This also makes the Follower hold the last Point on the Path. @@ -426,15 +445,22 @@ public class Follower { } /** - * This calls an update to the PoseUpdater, which updates the robot's current position estimate. - * This also updates all the Follower's PIDFs, which updates the motor powers. + * Calls an update to the PoseUpdater, which updates the robot's current position estimate. */ - public void update() { + public void updatePose() { poseUpdater.update(); if (drawOnDashboard) { dashboardPoseTracker.update(); } + } + + /** + * This calls an update to the PoseUpdater, which updates the robot's current position estimate. + * This also updates all the Follower's PIDFs, which updates the motor powers. + */ + public void update() { + updatePose(); if (!teleopDrive) { if (currentPath != null) { @@ -515,7 +541,7 @@ public class Follower { * movement, this is the x-axis. * @param lateralDrive determines the lateral drive vector for the robot in teleop. In field centric * movement, this is the y-axis. - * @param heading determines the heading vector for the robot in teleop. + * @param heading determines the heading vector for the robot in teleop. */ public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, double heading) { setTeleOpMovementVectors(forwardDrive, lateralDrive, heading, true); @@ -528,7 +554,7 @@ public class Follower { * movement, this is the x-axis. * @param lateralDrive determines the lateral drive vector for the robot in teleop. In field centric * movement, this is the y-axis. - * @param heading determines the heading vector for the robot in teleop. + * @param heading determines the heading vector for the robot in teleop. * @param robotCentric sets if the movement will be field or robot centric */ public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, double heading, boolean robotCentric) { @@ -719,7 +745,7 @@ public class Follower { Vector velocityErrorVector = MathFunctions.addVectors(forwardVelocityError, lateralVelocityError); previousRawDriveError = rawDriveError; - rawDriveError = velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector())); + rawDriveError = velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector())); double projection = 2 * driveErrors[1] - driveErrors[0]; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java index d5ac720..d784c09 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization; +import androidx.annotation.NonNull; + import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; @@ -44,7 +46,7 @@ public class Pose { * This creates a new Pose with no inputs and 0 for all values. */ public Pose() { - this(0,0,0); + this(0, 0, 0); } /** @@ -208,4 +210,10 @@ public class Pose { public Pose copy() { return new Pose(getX(), getY(), getHeading()); } + + @NonNull + @Override + public String toString() { + return "(" + getX() + ", " + getY() + ", " + Math.toDegrees(getHeading()) + ")"; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java index b43c6ff..bd4e393 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java @@ -27,16 +27,16 @@ public class MathFunctions { double denom = 1; // this multiplies up the numerator of the nCr function - for (int i = n; i > n-r; i--) { + for (int i = n; i > n - r; i--) { num *= i; } // this multiplies up the denominator of the nCr function - for (int i = 1; i <=r; i++) { + for (int i = 1; i <= r; i++) { denom *= i; } - return num/denom; + return num / denom; } /** @@ -67,7 +67,7 @@ public class MathFunctions { /** * This normalizes an angle to be between 0 and 2 pi radians, inclusive. - * + *

* IMPORTANT NOTE: This method operates in radians. * * @param angleRadians the angle to be normalized. @@ -75,8 +75,8 @@ public class MathFunctions { */ public static double normalizeAngle(double angleRadians) { double angle = angleRadians; - while (angle<0) angle += 2*Math.PI; - while (angle>2*Math.PI) angle -= 2*Math.PI; + while (angle < 0) angle += 2 * Math.PI; + while (angle > 2 * Math.PI) angle -= 2 * Math.PI; return angle; } @@ -88,7 +88,7 @@ public class MathFunctions { * @return returns the smallest angle. */ public static double getSmallestAngleDifference(double one, double two) { - return Math.min(MathFunctions.normalizeAngle(one-two), MathFunctions.normalizeAngle(two-one)); + return Math.min(MathFunctions.normalizeAngle(one - two), MathFunctions.normalizeAngle(two - one)); } /** @@ -98,7 +98,7 @@ public class MathFunctions { * @return returns the turn direction. */ public static double getTurnDirection(double startHeading, double endHeading) { - if (MathFunctions.normalizeAngle(endHeading-startHeading) >= 0 && MathFunctions.normalizeAngle(endHeading-startHeading) <= Math.PI) { + if (MathFunctions.normalizeAngle(endHeading - startHeading) >= 0 && MathFunctions.normalizeAngle(endHeading - startHeading) <= Math.PI) { return 1; // counter clock wise } return -1; // clock wise @@ -112,7 +112,7 @@ public class MathFunctions { * @return returns the distance between the two. */ public static double distance(Pose pose, Point point) { - return Math.sqrt(Math.pow(pose.getX()-point.getX(), 2) + Math.pow(pose.getY()-point.getY(), 2)); + return Math.sqrt(Math.pow(pose.getX() - point.getX(), 2) + Math.pow(pose.getY() - point.getY(), 2)); } /** @@ -123,7 +123,7 @@ public class MathFunctions { * @return returns the distance between the two. */ public static double distance(Pose one, Pose two) { - return Math.sqrt(Math.pow(one.getX()-two.getX(), 2) + Math.pow(one.getY()-two.getY(), 2)); + return Math.sqrt(Math.pow(one.getX() - two.getX(), 2) + Math.pow(one.getY() - two.getY(), 2)); } /** @@ -175,8 +175,8 @@ public class MathFunctions { /** * This rotates the given pose by the given theta, * - * @param pose the Pose to rotate. - * @param theta the angle to rotate by. + * @param pose the Pose to rotate. + * @param theta the angle to rotate by. * @param rotateHeading whether to adjust the Pose heading too. * @return the rotated Pose. */ @@ -196,7 +196,7 @@ public class MathFunctions { * @return returns the scaled Point. */ public static Point scalarMultiplyPoint(Point point, double scalar) { - return new Point(point.getX()*scalar, point.getY()*scalar, Point.CARTESIAN); + return new Point(point.getX() * scalar, point.getY() * scalar, Point.CARTESIAN); } /** @@ -229,7 +229,7 @@ public class MathFunctions { * @return returns the scaled Vector. */ public static Vector scalarMultiplyVector(Vector vector, double scalar) { - return new Vector(vector.getMagnitude()*scalar, vector.getTheta()); + return new Vector(vector.getMagnitude() * scalar, vector.getTheta()); } /** @@ -243,7 +243,7 @@ public class MathFunctions { if (vector.getMagnitude() == 0) { return new Vector(0.0, vector.getTheta()); } else { - return new Vector(vector.getMagnitude()/Math.abs(vector.getMagnitude()), vector.getTheta()); + return new Vector(vector.getMagnitude() / Math.abs(vector.getMagnitude()), vector.getTheta()); } } @@ -256,7 +256,7 @@ public class MathFunctions { */ public static Vector addVectors(Vector one, Vector two) { Vector returnVector = new Vector(); - returnVector.setOrthogonalComponents(one.getXComponent()+two.getXComponent(), one.getYComponent()+two.getYComponent()); + returnVector.setOrthogonalComponents(one.getXComponent() + two.getXComponent(), one.getYComponent() + two.getYComponent()); return returnVector; } @@ -270,7 +270,7 @@ public class MathFunctions { */ public static Vector subtractVectors(Vector one, Vector two) { Vector returnVector = new Vector(); - returnVector.setOrthogonalComponents(one.getXComponent()-two.getXComponent(), one.getYComponent()-two.getYComponent()); + returnVector.setOrthogonalComponents(one.getXComponent() - two.getXComponent(), one.getYComponent() - two.getYComponent()); return returnVector; } @@ -282,7 +282,7 @@ public class MathFunctions { * @return returns the dot product of the two Vectors. */ public static double dotProduct(Vector one, Vector two) { - return one.getXComponent()*two.getXComponent() + one.getYComponent()*two.getYComponent(); + return one.getXComponent() * two.getXComponent() + one.getYComponent() * two.getYComponent(); } /** @@ -294,7 +294,7 @@ public class MathFunctions { * @return returns the cross product of the two Vectors. */ public static double crossProduct(Vector one, Vector two) { - return one.getXComponent()*two.getYComponent() - one.getYComponent()*two.getXComponent(); + return one.getXComponent() * two.getYComponent() - one.getYComponent() * two.getXComponent(); } /** @@ -302,7 +302,7 @@ public class MathFunctions { * specified accuracy amount. * * @param one first number specified. - * @param two second number specified. + * @param two Second number specified. * @param accuracy the level of accuracy specified. * @return returns if the two numbers are within the specified accuracy of each other. */ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java index 5bfff7d..f044ce7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java @@ -51,16 +51,47 @@ public class PathBuilder { return this; } + /** + * This adds a default Path defined by a specified BezierCurve to the PathBuilder. + * + * @param controlPoints This is the specified control points that define the BezierCurve. + * @return This returns itself with the updated data. + */ + public PathBuilder addBezierCurve(Point... controlPoints) { + return addPath(new BezierCurve(controlPoints)); + } + + /** + * This adds a default Path defined by a specified BezierCurve to the PathBuilder. + * + * @param controlPoints This is the specified control points that define the BezierCurve. + * @return This returns itself with the updated data. + */ + public PathBuilder addBezierCurve(ArrayList controlPoints) { + return addPath(new BezierCurve(controlPoints)); + } + + /** + * This adds a default Path defined by a specified BezierLine to the PathBuilder. + * + * @param startPoint start point of the line. + * @param endPoint end point of the line. + * @return This returns itself with the updated data. + */ + public PathBuilder addBezierLine(Point startPoint, Point endPoint) { + return addPath(new BezierLine(startPoint, endPoint)); + } + /** * This sets a linear heading interpolation on the last Path added to the PathBuilder. * * @param startHeading The start of the linear heading interpolation. * @param endHeading The end of the linear heading interpolation. - * This will be reached at the end of the Path if no end time is specified. + * This will be reached at the end of the Path if no end time is specified. * @return This returns itself with the updated data. */ public PathBuilder setLinearHeadingInterpolation(double startHeading, double endHeading) { - this.paths.get(paths.size()-1).setLinearHeadingInterpolation(startHeading, endHeading); + this.paths.get(paths.size() - 1).setLinearHeadingInterpolation(startHeading, endHeading); return this; } @@ -69,13 +100,13 @@ public class PathBuilder { * * @param startHeading The start of the linear heading interpolation. * @param endHeading The end of the linear heading interpolation. - * This will be reached at the end of the Path if no end time is specified. + * This will be reached at the end of the Path if no end time is specified. * @param endTime The end time on the Path that the linear heading interpolation will end. - * This value goes from [0, 1] since Bezier curves are parametric functions. + * This value goes from [0, 1] since Bezier curves are parametric functions. * @return This returns itself with the updated data. */ public PathBuilder setLinearHeadingInterpolation(double startHeading, double endHeading, double endTime) { - this.paths.get(paths.size()-1).setLinearHeadingInterpolation(startHeading, endHeading, endTime); + this.paths.get(paths.size() - 1).setLinearHeadingInterpolation(startHeading, endHeading, endTime); return this; } @@ -86,7 +117,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setConstantHeadingInterpolation(double setHeading) { - this.paths.get(paths.size()-1).setConstantHeadingInterpolation(setHeading); + this.paths.get(paths.size() - 1).setConstantHeadingInterpolation(setHeading); return this; } @@ -94,20 +125,21 @@ public class PathBuilder { * This sets a reversed or tangent heading interpolation on the last Path added to the PathBuilder. * * @param set This sets the heading to reversed tangent following if this parameter is true. - * Conversely, this sets a tangent following if this parameter is false. + * Conversely, this sets a tangent following if this parameter is false. * @return This returns itself with the updated data. */ public PathBuilder setReversed(boolean set) { - this.paths.get(paths.size()-1).setReversed(set); + this.paths.get(paths.size() - 1).setReversed(set); return this; } + /** * This sets the heading interpolation to tangential on the last Path added to the PathBuilder. * There really shouldn't be a reason to use this since the default heading interpolation is * tangential but it's here. */ public PathBuilder setTangentHeadingInterpolation() { - this.paths.get(paths.size()-1).setTangentHeadingInterpolation(); + this.paths.get(paths.size() - 1).setTangentHeadingInterpolation(); return this; } @@ -118,7 +150,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setZeroPowerAccelerationMultiplier(double set) { - this.paths.get(paths.size()-1).setZeroPowerAccelerationMultiplier(set); + this.paths.get(paths.size() - 1).setZeroPowerAccelerationMultiplier(set); return this; } @@ -129,7 +161,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setPathEndVelocityConstraint(double set) { - this.paths.get(paths.size()-1).setPathEndVelocityConstraint(set); + this.paths.get(paths.size() - 1).setPathEndVelocityConstraint(set); return this; } @@ -140,7 +172,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setPathEndTranslationalConstraint(double set) { - this.paths.get(paths.size()-1).setPathEndTranslationalConstraint(set); + this.paths.get(paths.size() - 1).setPathEndTranslationalConstraint(set); return this; } @@ -151,7 +183,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setPathEndHeadingConstraint(double set) { - this.paths.get(paths.size()-1).setPathEndHeadingConstraint(set); + this.paths.get(paths.size() - 1).setPathEndHeadingConstraint(set); return this; } @@ -162,7 +194,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setPathEndTValueConstraint(double set) { - this.paths.get(paths.size()-1).setPathEndTValueConstraint(set); + this.paths.get(paths.size() - 1).setPathEndTValueConstraint(set); return this; } @@ -173,7 +205,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setPathEndTimeoutConstraint(double set) { - this.paths.get(paths.size()-1).setPathEndTimeoutConstraint(set); + this.paths.get(paths.size() - 1).setPathEndTimeoutConstraint(set); return this; } @@ -182,12 +214,12 @@ public class PathBuilder { * This callback is set to run at a specified number of milliseconds after the start of the path. * * @param time This sets the number of milliseconds of wait between the start of the Path and - * the calling of the callback. + * the calling of the callback. * @param runnable This sets the code for the callback to run. Use lambda statements for this. * @return This returns itself with the updated data. */ public PathBuilder addTemporalCallback(double time, Runnable runnable) { - this.callbacks.add(new PathCallback(time, runnable, PathCallback.TIME, paths.size()-1)); + this.callbacks.add(new PathCallback(time, runnable, PathCallback.TIME, paths.size() - 1)); return this; } @@ -200,7 +232,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder addParametricCallback(double t, Runnable runnable) { - this.callbacks.add(new PathCallback(t, runnable, PathCallback.PARAMETRIC, paths.size()-1)); + this.callbacks.add(new PathCallback(t, runnable, PathCallback.PARAMETRIC, paths.size() - 1)); return this; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java index 0da8df8..95a5f05 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; +import androidx.annotation.NonNull; + import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; /** @@ -31,13 +33,13 @@ public class Point { * This creates a new Point with coordinate inputs and a specified coordinate system. * * @param rOrX Depending on the coordinate system specified, this is either the r or x value. - * In polar coordinates, the r value is the distance from the origin. - * In Cartesian coordinates, the x value is the distance left/right from the origin. + * In polar coordinates, the r value is the distance from the origin. + * In Cartesian coordinates, the x value is the distance left/right from the origin. * @param thetaOrY Depending on the coordinate system specified, this is either the theta or - * y value. - * In polar coordinates, the theta value is the angle from the positive x-axis. - * Increasing theta moves in the counter-clockwise direction. - * In Cartesian coordinates, the y value is the distance up/down from the origin. + * y value. + * In polar coordinates, the theta value is the angle from the positive x-axis. + * Increasing theta moves in the counter-clockwise direction. + * In Cartesian coordinates, the y value is the distance up/down from the origin. * @param identifier this specifies what coordinate system the coordinate inputs are in. */ public Point(double rOrX, double thetaOrY, int identifier) { @@ -53,17 +55,27 @@ public class Point { setCoordinates(pose.getX(), pose.getY(), CARTESIAN); } + /** + * This creates a new Point from a X and Y value. + * + * @param setX the X value. + * @param setY the Y value. + */ + public Point(double setX, double setY) { + setCoordinates(setX, setY, CARTESIAN); + } + /** * This sets the coordinates of the Point using the specified coordinate system. * * @param rOrX Depending on the coordinate system specified, this is either the r or x value. - * In polar coordinates, the r value is the distance from the origin. - * In Cartesian coordinates, the x value is the distance left/right from the origin. + * In polar coordinates, the r value is the distance from the origin. + * In Cartesian coordinates, the x value is the distance left/right from the origin. * @param thetaOrY Depending on the coordinate system specified, this is either the theta or - * y value. - * In polar coordinates, the theta value is the angle from the positive x-axis. - * Increasing theta moves in the counter-clockwise direction. - * In Cartesian coordinates, the y value is the distance up/down from the origin. + * y value. + * In polar coordinates, the theta value is the angle from the positive x-axis. + * Increasing theta moves in the counter-clockwise direction. + * In Cartesian coordinates, the y value is the distance up/down from the origin. * @param identifier this specifies what coordinate system to use when setting values. */ public void setCoordinates(double rOrX, double thetaOrY, int identifier) { @@ -78,9 +90,9 @@ public class Point { theta = setOtherCoordinates[1]; break; default: - if (rOrX<0) { + if (rOrX < 0) { r = -rOrX; - theta = MathFunctions.normalizeAngle(thetaOrY+Math.PI); + theta = MathFunctions.normalizeAngle(thetaOrY + Math.PI); } else { r = rOrX; theta = MathFunctions.normalizeAngle(thetaOrY); @@ -99,7 +111,7 @@ public class Point { * @return returns the distance between the two Points. */ public double distanceFrom(Point otherPoint) { - return Math.sqrt(Math.pow(otherPoint.getX()-x, 2) + Math.pow(otherPoint.getY()-y, 2)); + return Math.sqrt(Math.pow(otherPoint.getX() - x, 2) + Math.pow(otherPoint.getY() - y, 2)); } /** @@ -110,7 +122,7 @@ public class Point { * @return this returns the x and y values, in that order, in an Array of doubles. */ public static double[] polarToCartesian(double r, double theta) { - return new double[] {r * Math.cos(theta), r * Math.sin(theta)}; + return new double[]{r * Math.cos(theta), r * Math.sin(theta)}; } /** @@ -123,17 +135,17 @@ public class Point { public static double[] cartesianToPolar(double x, double y) { if (x == 0) { if (y > 0) { - return new double[] {Math.abs(y), Math.PI/2}; + return new double[]{Math.abs(y), Math.PI / 2}; } else { - return new double[] {Math.abs(y), (3 * Math.PI) / 2}; + return new double[]{Math.abs(y), (3 * Math.PI) / 2}; } } - double r = Math.sqrt(x*x+y*y); - if (x < 0) return new double[] {r, Math.PI+Math.atan(y/x)}; + double r = Math.sqrt(x * x + y * y); + if (x < 0) return new double[]{r, Math.PI + Math.atan(y / x)}; if (y > 0) { return new double[]{r, Math.atan(y / x)}; } else { - return new double[]{r, (2*Math.PI) + Math.atan(y / x)}; + return new double[]{r, (2 * Math.PI) + Math.atan(y / x)}; } } @@ -182,4 +194,10 @@ public class Point { public Point copy() { return new Point(getX(), getY(), CARTESIAN); } + + @NonNull + @Override + public String toString() { + return "(" + getX() + ", " + getY() + ")"; + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java index af72609..bd932cd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java @@ -2,11 +2,11 @@ package org.firstinspires.ftc.teamcode.pedroPathing.tuning; import com.acmerobotics.dashboard.config.Config; -import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomFilteredPIDFCoefficients; -import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomPIDFCoefficients; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomFilteredPIDFCoefficients; +import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomPIDFCoefficients; import org.firstinspires.ftc.teamcode.pedroPathing.util.KalmanFilterParameters; /** @@ -33,7 +33,7 @@ public class FollowerConstants { private static double xMovement = 81.34056; private static double yMovement = 65.43028; private static double[] convertToPolar = Point.cartesianToPolar(xMovement, -yMovement); - public static Vector frontLeftVector = MathFunctions.normalizeVector(new Vector(convertToPolar[0],convertToPolar[1])); + public static Vector frontLeftVector = MathFunctions.normalizeVector(new Vector(convertToPolar[0], convertToPolar[1])); // Translational PIDF coefficients (don't use integral) @@ -182,7 +182,7 @@ public class FollowerConstants { // the limit at which the heading PIDF switches between the main and secondary heading PIDFs - public static double headingPIDFSwitch = Math.PI/20; + public static double headingPIDFSwitch = Math.PI / 20; // Secondary heading error PIDF coefficients public static CustomPIDFCoefficients secondaryHeadingPIDFCoefficients = new CustomPIDFCoefficients( diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java index e97f78a..ad07d88 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java @@ -40,7 +40,7 @@ import java.util.List; * @version 1.0, 3/13/2024 */ @Config -@Autonomous (name = "Forward Velocity Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Forward Velocity Tuner", group = "Autonomous Pathing Tuning") public class ForwardVelocityTuner extends OpMode { private ArrayList velocities = new ArrayList<>(); @@ -122,6 +122,10 @@ public class ForwardVelocityTuner extends OpMode { @Override public void loop() { if (gamepad1.cross || gamepad1.a) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } requestOpModeStop(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java index 9454e13..f7393bd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java @@ -40,7 +40,7 @@ import java.util.List; * @version 1.0, 3/13/2024 */ @Config -@Autonomous (name = "Forward Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Forward Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") public class ForwardZeroPowerAccelerationTuner extends OpMode { private ArrayList accelerations = new ArrayList<>(); @@ -120,6 +120,10 @@ public class ForwardZeroPowerAccelerationTuner extends OpMode { @Override public void loop() { if (gamepad1.cross || gamepad1.a) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } requestOpModeStop(); } @@ -149,7 +153,7 @@ public class ForwardZeroPowerAccelerationTuner extends OpMode { for (Double acceleration : accelerations) { average += acceleration; } - average /= (double)accelerations.size(); + average /= (double) accelerations.size(); telemetryA.addData("forward zero power acceleration (deceleration):", average); telemetryA.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java index 505245d..87b1978 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java @@ -40,7 +40,7 @@ import java.util.List; * @version 1.0, 3/13/2024 */ @Config -@Autonomous (name = "Lateral Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Lateral Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") public class LateralZeroPowerAccelerationTuner extends OpMode { private ArrayList accelerations = new ArrayList<>(); @@ -120,11 +120,15 @@ public class LateralZeroPowerAccelerationTuner extends OpMode { @Override public void loop() { if (gamepad1.cross || gamepad1.a) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } requestOpModeStop(); } poseUpdater.update(); - Vector heading = new Vector(1.0, poseUpdater.getPose().getHeading() - Math.PI/2); + Vector heading = new Vector(1.0, poseUpdater.getPose().getHeading() - Math.PI / 2); if (!end) { if (!stopping) { if (MathFunctions.dotProduct(poseUpdater.getVelocity(), heading) > VELOCITY) { @@ -149,7 +153,7 @@ public class LateralZeroPowerAccelerationTuner extends OpMode { for (Double acceleration : accelerations) { average += acceleration; } - average /= (double)accelerations.size(); + average /= (double) accelerations.size(); telemetryA.addData("lateral zero power acceleration (deceleration):", average); telemetryA.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java index acd90c0..ad6d22b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java @@ -40,7 +40,7 @@ import java.util.List; * @version 1.0, 3/13/2024 */ @Config -@Autonomous (name = "Strafe Velocity Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Strafe Velocity Tuner", group = "Autonomous Pathing Tuning") public class StrafeVelocityTuner extends OpMode { private ArrayList velocities = new ArrayList<>(); @@ -120,6 +120,10 @@ public class StrafeVelocityTuner extends OpMode { @Override public void loop() { if (gamepad1.cross || gamepad1.a) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } requestOpModeStop(); } @@ -132,7 +136,7 @@ public class StrafeVelocityTuner extends OpMode { motor.setPower(0); } } else { - double currentVelocity = Math.abs(MathFunctions.dotProduct(poseUpdater.getVelocity(), new Vector(1, Math.PI/2))); + double currentVelocity = Math.abs(MathFunctions.dotProduct(poseUpdater.getVelocity(), new Vector(1, Math.PI / 2))); velocities.add(currentVelocity); velocities.remove(0); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java index 9a2d956..9af9662 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java @@ -20,11 +20,14 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; * @version 1.0, 4/22/2024 */ public class Drawing { + public static final double ROBOT_RADIUS = 9; + private static TelemetryPacket packet; /** * This draws everything that will be used in the Follower's telemetryDebug() method. This takes * a Follower as an input, so an instance of the DashbaordDrawingHandler class is not needed. + * * @param follower */ public static void drawDebug(Follower follower) { @@ -35,6 +38,7 @@ public class Drawing { } drawPoseHistory(follower.getDashboardPoseTracker(), "#4CAF50"); drawRobot(follower.getPose(), "#4CAF50"); + sendPacket(); } @@ -114,12 +118,10 @@ public class Drawing { * @param t the Point to draw at */ public static void drawRobotOnCanvas(Canvas c, Point t) { - final double ROBOT_RADIUS = 9; - c.setStrokeWidth(1); c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS); - Vector halfv = new Vector(0.5*ROBOT_RADIUS, t.getTheta()); + Vector halfv = new Vector(0.5 * ROBOT_RADIUS, t.getTheta()); Vector p1 = MathFunctions.addVectors(halfv, new Vector(t.getR(), t.getTheta())); Vector p2 = MathFunctions.addVectors(p1, halfv); c.strokeLine(p1.getXComponent(), p1.getYComponent(), p2.getXComponent(), p2.getYComponent()); @@ -133,8 +135,6 @@ public class Drawing { * @param t the Pose to draw at */ public static void drawRobotOnCanvas(Canvas c, Pose t) { - final double ROBOT_RADIUS = 9; - c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS); Vector v = t.getHeadingVector(); v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS); From 9f11128c611cda46f92c0cbb4cb929322aa1ad70 Mon Sep 17 00:00:00 2001 From: Cal Kestis Date: Sat, 2 Nov 2024 10:16:56 +0900 Subject: [PATCH 56/94] FtcRobotController v10.1 --- FtcRobotController/build.gradle | 6 +++++- .../src/main/AndroidManifest.xml | 4 ++-- README.md | 21 ++++++++++++++++++- build.common.gradle | 6 +----- build.dependencies.gradle | 16 +++++++------- build.gradle | 2 +- gradle.properties | 2 ++ gradle/wrapper/gradle-wrapper.properties | 2 +- 8 files changed, 40 insertions(+), 19 deletions(-) diff --git a/FtcRobotController/build.gradle b/FtcRobotController/build.gradle index e25651f..9fa2169 100644 --- a/FtcRobotController/build.gradle +++ b/FtcRobotController/build.gradle @@ -14,7 +14,11 @@ android { buildConfigField "String", "APP_BUILD_TIME", '"' + (new SimpleDateFormat("yyyy-MM-dd'T'HH:mm:ss.SSSZ", Locale.ROOT).format(new Date())) + '"' } - compileSdkVersion 29 + buildFeatures { + buildConfig = true + } + + compileSdkVersion 30 compileOptions { sourceCompatibility JavaVersion.VERSION_1_8 diff --git a/FtcRobotController/src/main/AndroidManifest.xml b/FtcRobotController/src/main/AndroidManifest.xml index 787878b..1ce6a3e 100644 --- a/FtcRobotController/src/main/AndroidManifest.xml +++ b/FtcRobotController/src/main/AndroidManifest.xml @@ -1,8 +1,8 @@ + android:versionCode="57" + android:versionName="10.1.1"> diff --git a/README.md b/README.md index f7a02c9..d5856cc 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,7 @@ This repository contains the public FTC SDK for the INTO THE DEEP (2024-2025) co This GitHub repository contains the source code that is used to build an Android app to control a *FIRST* Tech Challenge competition robot. To use this SDK, download/clone the entire project to your local computer. ## Requirements -To use this Android Studio project, you will need Android Studio 2021.2 (codename Chipmunk) or later. +To use this Android Studio project, you will need Android Studio Ladybug (2024.2) or later. To program your robot in Blocks or OnBot Java, you do not need Android Studio. @@ -59,6 +59,25 @@ The readme.md file located in the [/TeamCode/src/main/java/org/firstinspires/ftc # Release Information +## Version 10.1.1 (20241102-092223) + +### Breaking Changes + +* Support for Android Studio Ladybug. Requires Android Studio Ladybug. + +### Known Issues + +* Android Studio Ladybug's bundled JDK is version 21. JDK 21 has deprecated support for Java 1.8, and Ladybug will warn on this deprecation. + OnBotJava only supports Java 1.8, therefore, in order to ensure that software developed using Android Studio will + run within the OnBotJava environment, the targetCompatibility and sourceCompatibility versions for the SDK have been left at VERSION_1_8. + FIRST has decided that until it can devote the resources to migrating OnBotJava to a newer version of Java, the deprecation is the + lesser of two non-optimal situations. + +### Enhancements + +* Added `toString()` method to Pose2D +* Added `toString()` method to SparkFunOTOS.Pose2D + ## Version 10.1 (20240919-122750) ### Enhancements diff --git a/build.common.gradle b/build.common.gradle index 046a4a1..a1d2fa4 100644 --- a/build.common.gradle +++ b/build.common.gradle @@ -21,7 +21,7 @@ apply plugin: 'com.android.application' android { - compileSdkVersion 29 + compileSdkVersion 30 signingConfigs { release { @@ -109,10 +109,6 @@ android { packagingOptions { pickFirst '**/*.so' } - sourceSets.main { - jni.srcDirs = [] - jniLibs.srcDir rootProject.file('libs') - } ndkVersion '21.3.6528147' } diff --git a/build.dependencies.gradle b/build.dependencies.gradle index a2b4ea1..593b31a 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -4,14 +4,14 @@ repositories { } dependencies { - implementation 'org.firstinspires.ftc:Inspection:10.1.0' - implementation 'org.firstinspires.ftc:Blocks:10.1.0' - implementation 'org.firstinspires.ftc:RobotCore:10.1.0' - implementation 'org.firstinspires.ftc:RobotServer:10.1.0' - implementation 'org.firstinspires.ftc:OnBotJava:10.1.0' - implementation 'org.firstinspires.ftc:Hardware:10.1.0' - implementation 'org.firstinspires.ftc:FtcCommon:10.1.0' - implementation 'org.firstinspires.ftc:Vision:10.1.0' + implementation 'org.firstinspires.ftc:Inspection:10.1.1' + implementation 'org.firstinspires.ftc:Blocks:10.1.1' + implementation 'org.firstinspires.ftc:RobotCore:10.1.1' + implementation 'org.firstinspires.ftc:RobotServer:10.1.1' + implementation 'org.firstinspires.ftc:OnBotJava:10.1.1' + implementation 'org.firstinspires.ftc:Hardware:10.1.1' + implementation 'org.firstinspires.ftc:FtcCommon:10.1.1' + implementation 'org.firstinspires.ftc:Vision:10.1.1' implementation 'androidx.appcompat:appcompat:1.2.0' } diff --git a/build.gradle b/build.gradle index 8969a41..e70f209 100644 --- a/build.gradle +++ b/build.gradle @@ -11,7 +11,7 @@ buildscript { } dependencies { // Note for FTC Teams: Do not modify this yourself. - classpath 'com.android.tools.build:gradle:7.2.0' + classpath 'com.android.tools.build:gradle:8.7.0' } } diff --git a/gradle.properties b/gradle.properties index 7a370c5..f5935e9 100644 --- a/gradle.properties +++ b/gradle.properties @@ -8,3 +8,5 @@ android.enableJetifier=false # Allow Gradle to use up to 1 GB of RAM org.gradle.jvmargs=-Xmx1024M + +android.nonTransitiveRClass=false \ No newline at end of file diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index aa991fc..19cfad9 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,5 @@ distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-7.4.2-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.9-bin.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists From 1056fe0fc66b16aaf17d0ec778dd6244056e0e4a Mon Sep 17 00:00:00 2001 From: Marley Reeves Date: Sun, 17 Nov 2024 13:36:33 +1000 Subject: [PATCH 57/94] Rotate robot diagrams in localizer files to reduce confusion --- .../pedroPathing/follower/Follower.java | 46 ++++++++++--- .../pedroPathing/localization/Pose.java | 10 ++- .../localizers/OTOSLocalizer.java | 31 +++++---- .../localizers/PinpointLocalizer.java | 30 ++++---- .../localizers/ThreeWheelIMULocalizer.java | 26 +++---- .../localizers/ThreeWheelLocalizer.java | 24 +++---- .../localizers/TwoWheelLocalizer.java | 24 +++---- .../pathGeneration/MathFunctions.java | 52 +++++++++----- .../pathGeneration/PathBuilder.java | 68 ++++++++++++++----- .../pedroPathing/pathGeneration/Point.java | 60 ++++++++++------ .../tuning/FollowerConstants.java | 8 +-- .../tuning/ForwardVelocityTuner.java | 6 +- .../ForwardZeroPowerAccelerationTuner.java | 8 ++- .../LateralZeroPowerAccelerationTuner.java | 10 ++- .../tuning/StrafeVelocityTuner.java | 8 ++- .../teamcode/pedroPathing/util/Drawing.java | 10 +-- 16 files changed, 271 insertions(+), 150 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index fe9d153..5089eb4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -1,19 +1,19 @@ package org.firstinspires.ftc.teamcode.pedroPathing.follower; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFSwitch; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.forwardZeroPowerAcceleration; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.lateralZeroPowerAcceleration; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFFeedForward; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.lateralZeroPowerAcceleration; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryDrivePIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryHeadingPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryTranslationalPIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFSwitch; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryDrivePID; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryHeadingPID; @@ -365,6 +365,25 @@ public class Follower { closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1); } + /** + * This holds a Point. + * + * @param point the Point to stay at. + * @param heading the heading to face. + */ + public void holdPoint(Point point, double heading) { + holdPoint(new BezierPoint(point), heading); + } + + /** + * This holds a Point. + * + * @param pose the Point (as a Pose) to stay at. + */ + public void holdPoint(Pose pose) { + holdPoint(new Point(pose), pose.getHeading()); + } + /** * This follows a Path. * This also makes the Follower hold the last Point on the Path. @@ -426,15 +445,22 @@ public class Follower { } /** - * This calls an update to the PoseUpdater, which updates the robot's current position estimate. - * This also updates all the Follower's PIDFs, which updates the motor powers. + * Calls an update to the PoseUpdater, which updates the robot's current position estimate. */ - public void update() { + public void updatePose() { poseUpdater.update(); if (drawOnDashboard) { dashboardPoseTracker.update(); } + } + + /** + * This calls an update to the PoseUpdater, which updates the robot's current position estimate. + * This also updates all the Follower's PIDFs, which updates the motor powers. + */ + public void update() { + updatePose(); if (!teleopDrive) { if (currentPath != null) { @@ -515,7 +541,7 @@ public class Follower { * movement, this is the x-axis. * @param lateralDrive determines the lateral drive vector for the robot in teleop. In field centric * movement, this is the y-axis. - * @param heading determines the heading vector for the robot in teleop. + * @param heading determines the heading vector for the robot in teleop. */ public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, double heading) { setTeleOpMovementVectors(forwardDrive, lateralDrive, heading, true); @@ -528,7 +554,7 @@ public class Follower { * movement, this is the x-axis. * @param lateralDrive determines the lateral drive vector for the robot in teleop. In field centric * movement, this is the y-axis. - * @param heading determines the heading vector for the robot in teleop. + * @param heading determines the heading vector for the robot in teleop. * @param robotCentric sets if the movement will be field or robot centric */ public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, double heading, boolean robotCentric) { @@ -719,7 +745,7 @@ public class Follower { Vector velocityErrorVector = MathFunctions.addVectors(forwardVelocityError, lateralVelocityError); previousRawDriveError = rawDriveError; - rawDriveError = velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector())); + rawDriveError = velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector())); double projection = 2 * driveErrors[1] - driveErrors[0]; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java index d5ac720..d784c09 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization; +import androidx.annotation.NonNull; + import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; @@ -44,7 +46,7 @@ public class Pose { * This creates a new Pose with no inputs and 0 for all values. */ public Pose() { - this(0,0,0); + this(0, 0, 0); } /** @@ -208,4 +210,10 @@ public class Pose { public Pose copy() { return new Pose(getX(), getY(), getHeading()); } + + @NonNull + @Override + public String toString() { + return "(" + getX() + ", " + getY() + ", " + Math.toDegrees(getHeading()) + ")"; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java index e4e1f34..a700e3b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java @@ -21,18 +21,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; * * forward on robot is the x positive direction * - * /--------------\ - * | ____ | - * | ---- | - * | || || | - * | || || | ----> left (y positive) - * | | - * | | - * \--------------/ - * | - * | - * V - * forward (x positive) + * forward (x positive) + * △ + * | + * | + * /--------------\ + * | | + * | | + * | || || | + * left (y positive) <--- | || || | + * | ____ | + * | ---- | + * \--------------/ * * @author Anyi Lin - 10158 Scott's Bots * @version 1.0, 7/20/2024 @@ -108,7 +108,10 @@ public class OTOSLocalizer extends Localizer { */ @Override public Pose getPose() { - return MathFunctions.addPoses(startPose, new Pose(otosPose.x, otosPose.y, otosPose.h)); + SparkFunOTOS.Pose2D rawPose = otos.getPosition(); + Pose pose = new Pose(rawPose.x, rawPose.y, rawPose.h); + + return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false)); } /** @@ -219,4 +222,4 @@ public class OTOSLocalizer extends Localizer { */ public void resetIMU() { } -} \ No newline at end of file +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java index 151311b..232df9a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java @@ -23,18 +23,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; * * forward on robot is the x positive direction * - * /--------------\ - * | ____ | - * | ---- | - * | || | - * | || | ----> left (y positive) - * | | - * | | - * \--------------/ - * | - * | - * V - * forward (x positive) + * forward (x positive) + * △ + * | + * | + * /--------------\ + * | | + * | | + * | || | + * left (y positive) <--- | || | + * | ____ | + * | ---- | + * \--------------/ * With the pinpoint your readings will be used in mm * to use inches ensure to divide your mm value by 25.4 * @author Logan Nash @@ -96,8 +96,10 @@ public class PinpointLocalizer extends Localizer { */ @Override public Pose getPose() { - Pose2D pose = odo.getPosition(); - return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), pose.getHeading(AngleUnit.RADIANS)); + Pose2D rawPose = odo.getPosition(); + Pose pose = new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS)); + + return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false)); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java index 1e9fd7b..bd3c598 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java @@ -26,18 +26,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; * * forward on robot is the x positive direction * - * /--------------\ - * | ____ | - * | ---- | - * | || || | - * | || || | ----> left (y positive) - * | | - * | | - * \--------------/ - * | - * | - * V - * forward (x positive) + * forward (x positive) + * △ + * | + * | + * /--------------\ + * | | + * | | + * | || || | + * left (y positive) <--- | || || | + * | ____ | + * | ---- | + * \--------------/ * * @author Logan Nash * @author Anyi Lin - 10158 Scott's Bots @@ -313,4 +313,4 @@ public class ThreeWheelIMULocalizer extends Localizer { public void resetIMU() { imu.resetYaw(); } -} \ No newline at end of file +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java index 6814b86..4368dc0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java @@ -23,18 +23,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; * * forward on robot is the x positive direction * - * /--------------\ - * | ____ | - * | ---- | - * | || || | - * | || || | ----> left (y positive) - * | | - * | | - * \--------------/ - * | - * | - * V - * forward (x positive) + * forward (x positive) + * △ + * | + * | + * /--------------\ + * | | + * | | + * | || || | + * left (y positive) <--- | || || | + * | ____ | + * | ---- | + * \--------------/ * * @author Anyi Lin - 10158 Scott's Bots * @version 1.0, 4/2/2024 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java index 62415d6..50d7ec2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java @@ -26,18 +26,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; * * forward on robot is the x positive direction * - * /--------------\ - * | ____ | - * | ---- | - * | || || | - * | || || | ----> left (y positive) - * | | - * | | - * \--------------/ - * | - * | - * V - * forward (x positive) +* forward (x positive) + * △ + * | + * | + * /--------------\ + * | | + * | | + * | || | + * left (y positive) <--- | || | + * | ____ | + * | ---- | + * \--------------/ * * @author Anyi Lin - 10158 Scott's Bots * @version 1.0, 4/2/2024 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java index 0edfbfd..bd4e393 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java @@ -27,16 +27,16 @@ public class MathFunctions { double denom = 1; // this multiplies up the numerator of the nCr function - for (int i = n; i > n-r; i--) { + for (int i = n; i > n - r; i--) { num *= i; } // this multiplies up the denominator of the nCr function - for (int i = 1; i <=r; i++) { + for (int i = 1; i <= r; i++) { denom *= i; } - return num/denom; + return num / denom; } /** @@ -67,7 +67,7 @@ public class MathFunctions { /** * This normalizes an angle to be between 0 and 2 pi radians, inclusive. - * + *

* IMPORTANT NOTE: This method operates in radians. * * @param angleRadians the angle to be normalized. @@ -75,8 +75,8 @@ public class MathFunctions { */ public static double normalizeAngle(double angleRadians) { double angle = angleRadians; - while (angle<0) angle += 2*Math.PI; - while (angle>2*Math.PI) angle -= 2*Math.PI; + while (angle < 0) angle += 2 * Math.PI; + while (angle > 2 * Math.PI) angle -= 2 * Math.PI; return angle; } @@ -88,7 +88,7 @@ public class MathFunctions { * @return returns the smallest angle. */ public static double getSmallestAngleDifference(double one, double two) { - return Math.min(MathFunctions.normalizeAngle(one-two), MathFunctions.normalizeAngle(two-one)); + return Math.min(MathFunctions.normalizeAngle(one - two), MathFunctions.normalizeAngle(two - one)); } /** @@ -98,7 +98,7 @@ public class MathFunctions { * @return returns the turn direction. */ public static double getTurnDirection(double startHeading, double endHeading) { - if (MathFunctions.normalizeAngle(endHeading-startHeading) >= 0 && MathFunctions.normalizeAngle(endHeading-startHeading) <= Math.PI) { + if (MathFunctions.normalizeAngle(endHeading - startHeading) >= 0 && MathFunctions.normalizeAngle(endHeading - startHeading) <= Math.PI) { return 1; // counter clock wise } return -1; // clock wise @@ -112,7 +112,7 @@ public class MathFunctions { * @return returns the distance between the two. */ public static double distance(Pose pose, Point point) { - return Math.sqrt(Math.pow(pose.getX()-point.getX(), 2) + Math.pow(pose.getY()-point.getY(), 2)); + return Math.sqrt(Math.pow(pose.getX() - point.getX(), 2) + Math.pow(pose.getY() - point.getY(), 2)); } /** @@ -123,7 +123,7 @@ public class MathFunctions { * @return returns the distance between the two. */ public static double distance(Pose one, Pose two) { - return Math.sqrt(Math.pow(one.getX()-two.getX(), 2) + Math.pow(one.getY()-two.getY(), 2)); + return Math.sqrt(Math.pow(one.getX() - two.getX(), 2) + Math.pow(one.getY() - two.getY(), 2)); } /** @@ -172,6 +172,22 @@ public class MathFunctions { return new Pose(one.getX() - two.getX(), one.getY() - two.getY(), one.getHeading() - two.getHeading()); } + /** + * This rotates the given pose by the given theta, + * + * @param pose the Pose to rotate. + * @param theta the angle to rotate by. + * @param rotateHeading whether to adjust the Pose heading too. + * @return the rotated Pose. + */ + public static Pose rotatePose(Pose pose, double theta, boolean rotateHeading) { + double x = pose.getX() * Math.cos(theta) - pose.getY() * Math.sin(theta); + double y = pose.getX() * Math.sin(theta) + pose.getY() * Math.cos(theta); + double heading = rotateHeading ? normalizeAngle(pose.getHeading() + theta) : pose.getHeading(); + + return new Pose(x, y, heading); + } + /** * This multiplies a Point by a scalar and returns the result as a Point * @@ -180,7 +196,7 @@ public class MathFunctions { * @return returns the scaled Point. */ public static Point scalarMultiplyPoint(Point point, double scalar) { - return new Point(point.getX()*scalar, point.getY()*scalar, Point.CARTESIAN); + return new Point(point.getX() * scalar, point.getY() * scalar, Point.CARTESIAN); } /** @@ -213,7 +229,7 @@ public class MathFunctions { * @return returns the scaled Vector. */ public static Vector scalarMultiplyVector(Vector vector, double scalar) { - return new Vector(vector.getMagnitude()*scalar, vector.getTheta()); + return new Vector(vector.getMagnitude() * scalar, vector.getTheta()); } /** @@ -227,7 +243,7 @@ public class MathFunctions { if (vector.getMagnitude() == 0) { return new Vector(0.0, vector.getTheta()); } else { - return new Vector(vector.getMagnitude()/Math.abs(vector.getMagnitude()), vector.getTheta()); + return new Vector(vector.getMagnitude() / Math.abs(vector.getMagnitude()), vector.getTheta()); } } @@ -240,7 +256,7 @@ public class MathFunctions { */ public static Vector addVectors(Vector one, Vector two) { Vector returnVector = new Vector(); - returnVector.setOrthogonalComponents(one.getXComponent()+two.getXComponent(), one.getYComponent()+two.getYComponent()); + returnVector.setOrthogonalComponents(one.getXComponent() + two.getXComponent(), one.getYComponent() + two.getYComponent()); return returnVector; } @@ -254,7 +270,7 @@ public class MathFunctions { */ public static Vector subtractVectors(Vector one, Vector two) { Vector returnVector = new Vector(); - returnVector.setOrthogonalComponents(one.getXComponent()-two.getXComponent(), one.getYComponent()-two.getYComponent()); + returnVector.setOrthogonalComponents(one.getXComponent() - two.getXComponent(), one.getYComponent() - two.getYComponent()); return returnVector; } @@ -266,7 +282,7 @@ public class MathFunctions { * @return returns the dot product of the two Vectors. */ public static double dotProduct(Vector one, Vector two) { - return one.getXComponent()*two.getXComponent() + one.getYComponent()*two.getYComponent(); + return one.getXComponent() * two.getXComponent() + one.getYComponent() * two.getYComponent(); } /** @@ -278,7 +294,7 @@ public class MathFunctions { * @return returns the cross product of the two Vectors. */ public static double crossProduct(Vector one, Vector two) { - return one.getXComponent()*two.getYComponent() - one.getYComponent()*two.getXComponent(); + return one.getXComponent() * two.getYComponent() - one.getYComponent() * two.getXComponent(); } /** @@ -286,7 +302,7 @@ public class MathFunctions { * specified accuracy amount. * * @param one first number specified. - * @param two second number specified. + * @param two Second number specified. * @param accuracy the level of accuracy specified. * @return returns if the two numbers are within the specified accuracy of each other. */ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java index 5bfff7d..f044ce7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java @@ -51,16 +51,47 @@ public class PathBuilder { return this; } + /** + * This adds a default Path defined by a specified BezierCurve to the PathBuilder. + * + * @param controlPoints This is the specified control points that define the BezierCurve. + * @return This returns itself with the updated data. + */ + public PathBuilder addBezierCurve(Point... controlPoints) { + return addPath(new BezierCurve(controlPoints)); + } + + /** + * This adds a default Path defined by a specified BezierCurve to the PathBuilder. + * + * @param controlPoints This is the specified control points that define the BezierCurve. + * @return This returns itself with the updated data. + */ + public PathBuilder addBezierCurve(ArrayList controlPoints) { + return addPath(new BezierCurve(controlPoints)); + } + + /** + * This adds a default Path defined by a specified BezierLine to the PathBuilder. + * + * @param startPoint start point of the line. + * @param endPoint end point of the line. + * @return This returns itself with the updated data. + */ + public PathBuilder addBezierLine(Point startPoint, Point endPoint) { + return addPath(new BezierLine(startPoint, endPoint)); + } + /** * This sets a linear heading interpolation on the last Path added to the PathBuilder. * * @param startHeading The start of the linear heading interpolation. * @param endHeading The end of the linear heading interpolation. - * This will be reached at the end of the Path if no end time is specified. + * This will be reached at the end of the Path if no end time is specified. * @return This returns itself with the updated data. */ public PathBuilder setLinearHeadingInterpolation(double startHeading, double endHeading) { - this.paths.get(paths.size()-1).setLinearHeadingInterpolation(startHeading, endHeading); + this.paths.get(paths.size() - 1).setLinearHeadingInterpolation(startHeading, endHeading); return this; } @@ -69,13 +100,13 @@ public class PathBuilder { * * @param startHeading The start of the linear heading interpolation. * @param endHeading The end of the linear heading interpolation. - * This will be reached at the end of the Path if no end time is specified. + * This will be reached at the end of the Path if no end time is specified. * @param endTime The end time on the Path that the linear heading interpolation will end. - * This value goes from [0, 1] since Bezier curves are parametric functions. + * This value goes from [0, 1] since Bezier curves are parametric functions. * @return This returns itself with the updated data. */ public PathBuilder setLinearHeadingInterpolation(double startHeading, double endHeading, double endTime) { - this.paths.get(paths.size()-1).setLinearHeadingInterpolation(startHeading, endHeading, endTime); + this.paths.get(paths.size() - 1).setLinearHeadingInterpolation(startHeading, endHeading, endTime); return this; } @@ -86,7 +117,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setConstantHeadingInterpolation(double setHeading) { - this.paths.get(paths.size()-1).setConstantHeadingInterpolation(setHeading); + this.paths.get(paths.size() - 1).setConstantHeadingInterpolation(setHeading); return this; } @@ -94,20 +125,21 @@ public class PathBuilder { * This sets a reversed or tangent heading interpolation on the last Path added to the PathBuilder. * * @param set This sets the heading to reversed tangent following if this parameter is true. - * Conversely, this sets a tangent following if this parameter is false. + * Conversely, this sets a tangent following if this parameter is false. * @return This returns itself with the updated data. */ public PathBuilder setReversed(boolean set) { - this.paths.get(paths.size()-1).setReversed(set); + this.paths.get(paths.size() - 1).setReversed(set); return this; } + /** * This sets the heading interpolation to tangential on the last Path added to the PathBuilder. * There really shouldn't be a reason to use this since the default heading interpolation is * tangential but it's here. */ public PathBuilder setTangentHeadingInterpolation() { - this.paths.get(paths.size()-1).setTangentHeadingInterpolation(); + this.paths.get(paths.size() - 1).setTangentHeadingInterpolation(); return this; } @@ -118,7 +150,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setZeroPowerAccelerationMultiplier(double set) { - this.paths.get(paths.size()-1).setZeroPowerAccelerationMultiplier(set); + this.paths.get(paths.size() - 1).setZeroPowerAccelerationMultiplier(set); return this; } @@ -129,7 +161,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setPathEndVelocityConstraint(double set) { - this.paths.get(paths.size()-1).setPathEndVelocityConstraint(set); + this.paths.get(paths.size() - 1).setPathEndVelocityConstraint(set); return this; } @@ -140,7 +172,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setPathEndTranslationalConstraint(double set) { - this.paths.get(paths.size()-1).setPathEndTranslationalConstraint(set); + this.paths.get(paths.size() - 1).setPathEndTranslationalConstraint(set); return this; } @@ -151,7 +183,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setPathEndHeadingConstraint(double set) { - this.paths.get(paths.size()-1).setPathEndHeadingConstraint(set); + this.paths.get(paths.size() - 1).setPathEndHeadingConstraint(set); return this; } @@ -162,7 +194,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setPathEndTValueConstraint(double set) { - this.paths.get(paths.size()-1).setPathEndTValueConstraint(set); + this.paths.get(paths.size() - 1).setPathEndTValueConstraint(set); return this; } @@ -173,7 +205,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setPathEndTimeoutConstraint(double set) { - this.paths.get(paths.size()-1).setPathEndTimeoutConstraint(set); + this.paths.get(paths.size() - 1).setPathEndTimeoutConstraint(set); return this; } @@ -182,12 +214,12 @@ public class PathBuilder { * This callback is set to run at a specified number of milliseconds after the start of the path. * * @param time This sets the number of milliseconds of wait between the start of the Path and - * the calling of the callback. + * the calling of the callback. * @param runnable This sets the code for the callback to run. Use lambda statements for this. * @return This returns itself with the updated data. */ public PathBuilder addTemporalCallback(double time, Runnable runnable) { - this.callbacks.add(new PathCallback(time, runnable, PathCallback.TIME, paths.size()-1)); + this.callbacks.add(new PathCallback(time, runnable, PathCallback.TIME, paths.size() - 1)); return this; } @@ -200,7 +232,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder addParametricCallback(double t, Runnable runnable) { - this.callbacks.add(new PathCallback(t, runnable, PathCallback.PARAMETRIC, paths.size()-1)); + this.callbacks.add(new PathCallback(t, runnable, PathCallback.PARAMETRIC, paths.size() - 1)); return this; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java index 0da8df8..95a5f05 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; +import androidx.annotation.NonNull; + import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; /** @@ -31,13 +33,13 @@ public class Point { * This creates a new Point with coordinate inputs and a specified coordinate system. * * @param rOrX Depending on the coordinate system specified, this is either the r or x value. - * In polar coordinates, the r value is the distance from the origin. - * In Cartesian coordinates, the x value is the distance left/right from the origin. + * In polar coordinates, the r value is the distance from the origin. + * In Cartesian coordinates, the x value is the distance left/right from the origin. * @param thetaOrY Depending on the coordinate system specified, this is either the theta or - * y value. - * In polar coordinates, the theta value is the angle from the positive x-axis. - * Increasing theta moves in the counter-clockwise direction. - * In Cartesian coordinates, the y value is the distance up/down from the origin. + * y value. + * In polar coordinates, the theta value is the angle from the positive x-axis. + * Increasing theta moves in the counter-clockwise direction. + * In Cartesian coordinates, the y value is the distance up/down from the origin. * @param identifier this specifies what coordinate system the coordinate inputs are in. */ public Point(double rOrX, double thetaOrY, int identifier) { @@ -53,17 +55,27 @@ public class Point { setCoordinates(pose.getX(), pose.getY(), CARTESIAN); } + /** + * This creates a new Point from a X and Y value. + * + * @param setX the X value. + * @param setY the Y value. + */ + public Point(double setX, double setY) { + setCoordinates(setX, setY, CARTESIAN); + } + /** * This sets the coordinates of the Point using the specified coordinate system. * * @param rOrX Depending on the coordinate system specified, this is either the r or x value. - * In polar coordinates, the r value is the distance from the origin. - * In Cartesian coordinates, the x value is the distance left/right from the origin. + * In polar coordinates, the r value is the distance from the origin. + * In Cartesian coordinates, the x value is the distance left/right from the origin. * @param thetaOrY Depending on the coordinate system specified, this is either the theta or - * y value. - * In polar coordinates, the theta value is the angle from the positive x-axis. - * Increasing theta moves in the counter-clockwise direction. - * In Cartesian coordinates, the y value is the distance up/down from the origin. + * y value. + * In polar coordinates, the theta value is the angle from the positive x-axis. + * Increasing theta moves in the counter-clockwise direction. + * In Cartesian coordinates, the y value is the distance up/down from the origin. * @param identifier this specifies what coordinate system to use when setting values. */ public void setCoordinates(double rOrX, double thetaOrY, int identifier) { @@ -78,9 +90,9 @@ public class Point { theta = setOtherCoordinates[1]; break; default: - if (rOrX<0) { + if (rOrX < 0) { r = -rOrX; - theta = MathFunctions.normalizeAngle(thetaOrY+Math.PI); + theta = MathFunctions.normalizeAngle(thetaOrY + Math.PI); } else { r = rOrX; theta = MathFunctions.normalizeAngle(thetaOrY); @@ -99,7 +111,7 @@ public class Point { * @return returns the distance between the two Points. */ public double distanceFrom(Point otherPoint) { - return Math.sqrt(Math.pow(otherPoint.getX()-x, 2) + Math.pow(otherPoint.getY()-y, 2)); + return Math.sqrt(Math.pow(otherPoint.getX() - x, 2) + Math.pow(otherPoint.getY() - y, 2)); } /** @@ -110,7 +122,7 @@ public class Point { * @return this returns the x and y values, in that order, in an Array of doubles. */ public static double[] polarToCartesian(double r, double theta) { - return new double[] {r * Math.cos(theta), r * Math.sin(theta)}; + return new double[]{r * Math.cos(theta), r * Math.sin(theta)}; } /** @@ -123,17 +135,17 @@ public class Point { public static double[] cartesianToPolar(double x, double y) { if (x == 0) { if (y > 0) { - return new double[] {Math.abs(y), Math.PI/2}; + return new double[]{Math.abs(y), Math.PI / 2}; } else { - return new double[] {Math.abs(y), (3 * Math.PI) / 2}; + return new double[]{Math.abs(y), (3 * Math.PI) / 2}; } } - double r = Math.sqrt(x*x+y*y); - if (x < 0) return new double[] {r, Math.PI+Math.atan(y/x)}; + double r = Math.sqrt(x * x + y * y); + if (x < 0) return new double[]{r, Math.PI + Math.atan(y / x)}; if (y > 0) { return new double[]{r, Math.atan(y / x)}; } else { - return new double[]{r, (2*Math.PI) + Math.atan(y / x)}; + return new double[]{r, (2 * Math.PI) + Math.atan(y / x)}; } } @@ -182,4 +194,10 @@ public class Point { public Point copy() { return new Point(getX(), getY(), CARTESIAN); } + + @NonNull + @Override + public String toString() { + return "(" + getX() + ", " + getY() + ")"; + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java index af72609..bd932cd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java @@ -2,11 +2,11 @@ package org.firstinspires.ftc.teamcode.pedroPathing.tuning; import com.acmerobotics.dashboard.config.Config; -import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomFilteredPIDFCoefficients; -import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomPIDFCoefficients; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomFilteredPIDFCoefficients; +import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomPIDFCoefficients; import org.firstinspires.ftc.teamcode.pedroPathing.util.KalmanFilterParameters; /** @@ -33,7 +33,7 @@ public class FollowerConstants { private static double xMovement = 81.34056; private static double yMovement = 65.43028; private static double[] convertToPolar = Point.cartesianToPolar(xMovement, -yMovement); - public static Vector frontLeftVector = MathFunctions.normalizeVector(new Vector(convertToPolar[0],convertToPolar[1])); + public static Vector frontLeftVector = MathFunctions.normalizeVector(new Vector(convertToPolar[0], convertToPolar[1])); // Translational PIDF coefficients (don't use integral) @@ -182,7 +182,7 @@ public class FollowerConstants { // the limit at which the heading PIDF switches between the main and secondary heading PIDFs - public static double headingPIDFSwitch = Math.PI/20; + public static double headingPIDFSwitch = Math.PI / 20; // Secondary heading error PIDF coefficients public static CustomPIDFCoefficients secondaryHeadingPIDFCoefficients = new CustomPIDFCoefficients( diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java index e97f78a..ad07d88 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java @@ -40,7 +40,7 @@ import java.util.List; * @version 1.0, 3/13/2024 */ @Config -@Autonomous (name = "Forward Velocity Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Forward Velocity Tuner", group = "Autonomous Pathing Tuning") public class ForwardVelocityTuner extends OpMode { private ArrayList velocities = new ArrayList<>(); @@ -122,6 +122,10 @@ public class ForwardVelocityTuner extends OpMode { @Override public void loop() { if (gamepad1.cross || gamepad1.a) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } requestOpModeStop(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java index 9454e13..f7393bd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java @@ -40,7 +40,7 @@ import java.util.List; * @version 1.0, 3/13/2024 */ @Config -@Autonomous (name = "Forward Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Forward Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") public class ForwardZeroPowerAccelerationTuner extends OpMode { private ArrayList accelerations = new ArrayList<>(); @@ -120,6 +120,10 @@ public class ForwardZeroPowerAccelerationTuner extends OpMode { @Override public void loop() { if (gamepad1.cross || gamepad1.a) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } requestOpModeStop(); } @@ -149,7 +153,7 @@ public class ForwardZeroPowerAccelerationTuner extends OpMode { for (Double acceleration : accelerations) { average += acceleration; } - average /= (double)accelerations.size(); + average /= (double) accelerations.size(); telemetryA.addData("forward zero power acceleration (deceleration):", average); telemetryA.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java index 505245d..87b1978 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java @@ -40,7 +40,7 @@ import java.util.List; * @version 1.0, 3/13/2024 */ @Config -@Autonomous (name = "Lateral Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Lateral Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") public class LateralZeroPowerAccelerationTuner extends OpMode { private ArrayList accelerations = new ArrayList<>(); @@ -120,11 +120,15 @@ public class LateralZeroPowerAccelerationTuner extends OpMode { @Override public void loop() { if (gamepad1.cross || gamepad1.a) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } requestOpModeStop(); } poseUpdater.update(); - Vector heading = new Vector(1.0, poseUpdater.getPose().getHeading() - Math.PI/2); + Vector heading = new Vector(1.0, poseUpdater.getPose().getHeading() - Math.PI / 2); if (!end) { if (!stopping) { if (MathFunctions.dotProduct(poseUpdater.getVelocity(), heading) > VELOCITY) { @@ -149,7 +153,7 @@ public class LateralZeroPowerAccelerationTuner extends OpMode { for (Double acceleration : accelerations) { average += acceleration; } - average /= (double)accelerations.size(); + average /= (double) accelerations.size(); telemetryA.addData("lateral zero power acceleration (deceleration):", average); telemetryA.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java index acd90c0..ad6d22b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java @@ -40,7 +40,7 @@ import java.util.List; * @version 1.0, 3/13/2024 */ @Config -@Autonomous (name = "Strafe Velocity Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Strafe Velocity Tuner", group = "Autonomous Pathing Tuning") public class StrafeVelocityTuner extends OpMode { private ArrayList velocities = new ArrayList<>(); @@ -120,6 +120,10 @@ public class StrafeVelocityTuner extends OpMode { @Override public void loop() { if (gamepad1.cross || gamepad1.a) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } requestOpModeStop(); } @@ -132,7 +136,7 @@ public class StrafeVelocityTuner extends OpMode { motor.setPower(0); } } else { - double currentVelocity = Math.abs(MathFunctions.dotProduct(poseUpdater.getVelocity(), new Vector(1, Math.PI/2))); + double currentVelocity = Math.abs(MathFunctions.dotProduct(poseUpdater.getVelocity(), new Vector(1, Math.PI / 2))); velocities.add(currentVelocity); velocities.remove(0); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java index 9a2d956..9af9662 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java @@ -20,11 +20,14 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; * @version 1.0, 4/22/2024 */ public class Drawing { + public static final double ROBOT_RADIUS = 9; + private static TelemetryPacket packet; /** * This draws everything that will be used in the Follower's telemetryDebug() method. This takes * a Follower as an input, so an instance of the DashbaordDrawingHandler class is not needed. + * * @param follower */ public static void drawDebug(Follower follower) { @@ -35,6 +38,7 @@ public class Drawing { } drawPoseHistory(follower.getDashboardPoseTracker(), "#4CAF50"); drawRobot(follower.getPose(), "#4CAF50"); + sendPacket(); } @@ -114,12 +118,10 @@ public class Drawing { * @param t the Point to draw at */ public static void drawRobotOnCanvas(Canvas c, Point t) { - final double ROBOT_RADIUS = 9; - c.setStrokeWidth(1); c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS); - Vector halfv = new Vector(0.5*ROBOT_RADIUS, t.getTheta()); + Vector halfv = new Vector(0.5 * ROBOT_RADIUS, t.getTheta()); Vector p1 = MathFunctions.addVectors(halfv, new Vector(t.getR(), t.getTheta())); Vector p2 = MathFunctions.addVectors(p1, halfv); c.strokeLine(p1.getXComponent(), p1.getYComponent(), p2.getXComponent(), p2.getYComponent()); @@ -133,8 +135,6 @@ public class Drawing { * @param t the Pose to draw at */ public static void drawRobotOnCanvas(Canvas c, Pose t) { - final double ROBOT_RADIUS = 9; - c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS); Vector v = t.getHeadingVector(); v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS); From 775e7ffbc8ececc68755292c0a80a23e2d12395d Mon Sep 17 00:00:00 2001 From: Marley Reeves <79887575+Marlstar@users.noreply.github.com> Date: Sun, 17 Nov 2024 14:42:31 +1100 Subject: [PATCH 58/94] Clarify translational PIDF tuning process Some quirks in both the process of tuning the translational PIDF and the documentation of it led to confusion for myself and others. - Noted that the telemetry message that the robot will move forward and backward can be ignored for the moment - Noted that the robot should only move when pushed - Noted that the robot will only correct to a line in the original forward direction, not to the starting point --- .../ftc/teamcode/pedroPathing/TUNING.md | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md index bd22f46..e76196f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md @@ -47,12 +47,19 @@ measurements will be in centimeters. `lateralZeroPowerAcceleration`, respectively. * After this, we will want to tune the translational PID. Go to FTC Dashboard and disable all but - the `useTranslational` checkboxes under the `Follower` tab. Then, run `StraightBackAndForth`. Make - sure you disable the timer on autonomous OpModes. The PID for the translational error is called - `translationalPIDF`. If you need to add a feedforward value, use the `translationalPIDFFeedForward` - since that will add the feedforward in the direction the robot is trying to move, rather than the - feedforward in the PIDF itself, since those will only add the feedforward one way. You can change - the PIDF constants and feedforward values, under the `FollowerConstants` tab in FTC Dashboard. + the `useTranslational` checkboxes under the `Follower` tab. Then, run `StraightBackAndForth`. + Make sure you disable the timer on autonomous OpModes. You will notice in telemetry a message saying + that the robot will travel a distance forward and backward, this will not happen until later, so for + now you can ignore this message. The robot should not move when you run the opmode initally. Instead, + it should correct when you push it away from its starting position. Note that this correction should + happen regardless of the robot's rotation, and that the robot should not rotate itself (if it does, + disable `useHeading` as mentioned prior). Also note that the robot will only correct to an imaginary line + that runs straight forward from the robot's starting position, meaning that it will not correct in the + (original) forward direction. The PID for the translational error is called `translationalPIDF`. + If you need to add a feedforward value, use the `translationalPIDFFeedForward` since that will add + the feedforward in the direction the robot is trying to move, rather than the feedforward in the + PIDF itself, since those will only add the feedforward one way. You can change the PIDF constants + and feedforward values, under the `FollowerConstants` tab in FTC Dashboard. To tune the PID, push the robot off the path and see how corrects. You will want to alternate sides you push to reduce field wear and tear as well as push with varying power and distance. I would recommend tuning the PID so that it is capable of correcting while minimizing oscillations and still @@ -116,4 +123,4 @@ down and all the values pertaining to the secondary PIDs will be there. The two a PID that handles larger errors (the main PID) and a second PID to handle smaller errors (the secondary PID). The main PID should be tuned to move the error within the secondary PID's range without providing too much momentum that could cause an overshoot. The secondary PID should be tuned -to correct within its range quickly and accurately while minimizing oscillations. \ No newline at end of file +to correct within its range quickly and accurately while minimizing oscillations. From ea381fa92c0e038d94ae43008b886cbfaf81f1bf Mon Sep 17 00:00:00 2001 From: Anyi Lin Date: Thu, 21 Nov 2024 02:06:59 -0500 Subject: [PATCH 59/94] realigning class comment on PinpointLocalizer --- .../localizers/PinpointLocalizer.java | 50 +++++++++---------- 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java index 232df9a..0d6ff4b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java @@ -16,31 +16,31 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; * This is the Pinpoint class. This class extends the Localizer superclass and is a * localizer that uses the two wheel odometry set up with the IMU to have more accurate heading * readings. The diagram below, which is modified from Road Runner, shows a typical set up. - * - * The view is from the top of the robot looking downwards. - * - * left on robot is the y positive direction - * - * forward on robot is the x positive direction - * - * forward (x positive) - * △ - * | - * | - * /--------------\ - * | | - * | | - * | || | - * left (y positive) <--- | || | - * | ____ | - * | ---- | - * \--------------/ - * With the pinpoint your readings will be used in mm - * to use inches ensure to divide your mm value by 25.4 - * @author Logan Nash - * @author Havish Sripada 12808 - RevAmped Robotics - * @author Ethan Doak - Gobilda - * @version 1.0, 10/2/2024 + * + * The view is from the top of the robot looking downwards. + * + * left on robot is the y positive direction + * + * forward on robot is the x positive direction + * + * forward (x positive) + * △ + * | + * | + * /--------------\ + * | | + * | | + * | || | + * left (y positive) <--- | || | + * | ____ | + * | ---- | + * \--------------/ + * With the pinpoint your readings will be used in mm + * to use inches ensure to divide your mm value by 25.4 + * @author Logan Nash + * @author Havish Sripada 12808 - RevAmped Robotics + * @author Ethan Doak - Gobilda + * @version 1.0, 10/2/2024 */ public class PinpointLocalizer extends Localizer { private HardwareMap hardwareMap; From 437635838b25d970f60cf55166fdf8e747f83988 Mon Sep 17 00:00:00 2001 From: Anyi Lin Date: Thu, 21 Nov 2024 15:35:42 -0500 Subject: [PATCH 60/94] added functionality to IMU methods in PoseUpdater --- .../pedroPathing/localization/Localizer.java | 11 +++++++++++ .../pedroPathing/localization/PoseUpdater.java | 14 +++++++++++--- .../localizers/ThreeWheelIMULocalizer.java | 10 ++++++++++ .../localization/localizers/TwoWheelLocalizer.java | 10 ++++++++++ 4 files changed, 42 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java index 8f9687f..059ee31 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization; +import com.qualcomm.robotcore.hardware.IMU; + import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; /** @@ -91,4 +93,13 @@ public abstract class Localizer { * This resets the IMU of the localizer, if applicable. */ public abstract void resetIMU(); + + /** + * This is overridden to return the IMU, if there is one. + * + * @return returns the IMU if it exists + */ + public IMU getIMU() { + return null; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java index ce2a8f2..eff461f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java @@ -60,6 +60,7 @@ public class PoseUpdater { } this.localizer = localizer; + imu = localizer.getIMU(); } /** @@ -300,7 +301,9 @@ public class PoseUpdater { * This resets the heading of the robot to the IMU's heading, using Road Runner's pose reset. */ public void resetHeadingToIMU() { - localizer.setPose(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading())); + if (imu != null) { + localizer.setPose(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading())); + } } /** @@ -309,7 +312,9 @@ public class PoseUpdater { * method. */ public void resetHeadingToIMUWithOffsets() { - setCurrentPoseWithOffset(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading())); + if (imu != null) { + setCurrentPoseWithOffset(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading())); + } } /** @@ -318,7 +323,10 @@ public class PoseUpdater { * @return returns the normalized IMU heading. */ public double getNormalizedIMUHeading() { - return MathFunctions.normalizeAngle(-imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); + if (imu != null) { + return MathFunctions.normalizeAngle(-imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); + } + return 0; } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java index bd3c598..5f0724c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java @@ -313,4 +313,14 @@ public class ThreeWheelIMULocalizer extends Localizer { public void resetIMU() { imu.resetYaw(); } + + /** + * This is returns the IMU. + * + * @return returns the IMU + */ + @Override + public IMU getIMU() { + return imu; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java index 50d7ec2..685cebe 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java @@ -296,4 +296,14 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w public void resetIMU() { imu.resetYaw(); } + + /** + * This is returns the IMU. + * + * @return returns the IMU + */ + @Override + public IMU getIMU() { + return imu; + } } From 9711a268e5b798bf3be4598aea55f0ad91604414 Mon Sep 17 00:00:00 2001 From: Anyi Lin Date: Thu, 21 Nov 2024 17:09:44 -0500 Subject: [PATCH 61/94] hopefully fixed the PinpointLocalizer --- .../localizers/PinpointLocalizer.java | 24 +++++++++---------- 1 file changed, 11 insertions(+), 13 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java index 0d6ff4b..58c4746 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java @@ -44,7 +44,6 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; */ public class PinpointLocalizer extends Localizer { private HardwareMap hardwareMap; - private Pose startPose; private GoBildaPinpointDriver odo; private double previousHeading; private double totalHeading; @@ -84,7 +83,7 @@ public class PinpointLocalizer extends Localizer { setStartPose(setStartPose); totalHeading = 0; - previousHeading = startPose.getHeading(); + previousHeading = setStartPose.getHeading(); resetPinpoint(); } @@ -97,9 +96,7 @@ public class PinpointLocalizer extends Localizer { @Override public Pose getPose() { Pose2D rawPose = odo.getPosition(); - Pose pose = new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS)); - - return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false)); + return new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS)); } /** @@ -127,13 +124,15 @@ public class PinpointLocalizer extends Localizer { } /** - * This sets the start pose. Changing the start pose should move the robot as if all its - * previous movements were displacing it from its new start pose. + * This sets the start pose. Since nobody should be using this after the robot has begun moving, + * and due to issues with the PinpointLocalizer, this is functionally the same as setPose(Pose). * * @param setStart the new start pose */ @Override - public void setStartPose(Pose setStart) {startPose = setStart;} + public void setStartPose(Pose setStart) { + odo.setPosition(new Pose2D(DistanceUnit.INCH, setStart.getX(), setStart.getY(), AngleUnit.RADIANS, setStart.getHeading())); + } /** * This sets the current pose estimate. Changing this should just change the robot's current @@ -143,9 +142,8 @@ public class PinpointLocalizer extends Localizer { */ @Override public void setPose(Pose setPose) { - resetPinpoint(); - Pose setPinpointPose = MathFunctions.subtractPoses(setPose, startPose); - odo.setPosition(new Pose2D(DistanceUnit.INCH, setPinpointPose.getX(), setPinpointPose.getY(), AngleUnit.RADIANS, setPinpointPose.getHeading())); + resetPinpoint(); + odo.setPosition(new Pose2D(DistanceUnit.INCH, setPose.getX(), setPose.getY(), AngleUnit.RADIANS, setPose.getHeading())); } /** @@ -154,8 +152,8 @@ public class PinpointLocalizer extends Localizer { @Override public void update() { odo.update(); - totalHeading += MathFunctions.getSmallestAngleDifference(odo.getHeading(),previousHeading); - previousHeading = odo.getHeading(); + totalHeading += MathFunctions.getSmallestAngleDifference(odo.getHeading(), previousHeading); + previousHeading = odo.getHeading(); } /** From 089fc3ae6878b5fe2cbf8764f0df0e4dd1b0c89e Mon Sep 17 00:00:00 2001 From: Anyi Lin Date: Thu, 21 Nov 2024 17:13:30 -0500 Subject: [PATCH 62/94] moved velocity calculations to the localizers --- .../teamcode/pedroPathing/localization/PoseUpdater.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java index eff461f..3daf54a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java @@ -262,9 +262,10 @@ public class PoseUpdater { */ public Vector getVelocity() { if (currentVelocity == null) { - currentVelocity = new Vector(); - currentVelocity.setOrthogonalComponents(getPose().getX() - previousPose.getX(), getPose().getY() - previousPose.getY()); - currentVelocity.setMagnitude(MathFunctions.distance(getPose(), previousPose) / ((currentPoseTime - previousPoseTime) / Math.pow(10.0, 9))); +// currentVelocity = new Vector(); +// currentVelocity.setOrthogonalComponents(getPose().getX() - previousPose.getX(), getPose().getY() - previousPose.getY()); +// currentVelocity.setMagnitude(MathFunctions.distance(getPose(), previousPose) / ((currentPoseTime - previousPoseTime) / Math.pow(10.0, 9))); + currentVelocity = localizer.getVelocityVector(); return MathFunctions.copyVector(currentVelocity); } else { return MathFunctions.copyVector(currentVelocity); From 40f7b3391f8c2c6b1463c34508eabb588fd2a7bb Mon Sep 17 00:00:00 2001 From: Anyi Lin Date: Fri, 22 Nov 2024 03:13:25 -0500 Subject: [PATCH 63/94] added a reminder to reverse motors properly in TUNING.MD --- .../java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md index e76196f..4704e42 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md @@ -20,7 +20,8 @@ measurements will be in centimeters. * Next, we need to find the preferred mecanum drive vectors. The rollers on mecanum wheels point at a 45 degree angle from the forward direction, but the actual direction the force is output is actually - closer to forward. To find the direction your wheels will go, you will need to run the + closer to forward. Before running any OpModes, make sure your motors are reversed properly in the + `Follower` class constructor. To find the direction your wheels will go, you will need to run the `Forward Velocity Tuner` and `Strafe Velocity Tuner` OpModes. These will run your robot at full power for 40 inches forward and to the right, respectively. The distance can be changed through FTC Dashboard under the dropdown for each respective class, but higher distances work better. After the From fb056103d9064019ba9133c7ca97c1f1e3cc16f9 Mon Sep 17 00:00:00 2001 From: Anyi Lin Date: Fri, 22 Nov 2024 14:24:38 -0500 Subject: [PATCH 64/94] fix velocity inaccuracy in TUNING.md --- .../java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md index 4704e42..4ae8b4b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md @@ -35,7 +35,7 @@ measurements will be in centimeters. find the rate at which your robot decelerates when power is cut from the drivetrain. This is used to get a more accurate estimation of the drive vector. To find this, you will need to run the `Forward Zero Power Acceleration Tuner` and the `Lateral Zero Power Acceleration Tuner` OpModes. - These will run your robot until it hits a velocity of 10 inches/second forward and to the right, + These will run your robot until it hits a velocity of 30 inches/second forward and to the right, respectively. The velocity can be changed through FTC Dashboard under the dropdown for each respective class, but higher velocities work better. After the velocity has been reached, power will be cut from the drivetrain and the robot's deceleration will be tracked until the robot stops, at From 17f0ebaea5eed345defef806bd9787b8b71becf9 Mon Sep 17 00:00:00 2001 From: Anyi Lin Date: Fri, 22 Nov 2024 21:58:40 -0500 Subject: [PATCH 65/94] fixed velocity calculations in localizers --- .../localization/localizers/DriveEncoderLocalizer.java | 2 +- .../localization/localizers/ThreeWheelIMULocalizer.java | 2 +- .../localization/localizers/ThreeWheelLocalizer.java | 2 +- .../pedroPathing/localization/localizers/TwoWheelLocalizer.java | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java index a8e3902..aa295e4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java @@ -182,7 +182,7 @@ public class DriveEncoderLocalizer extends Localizer { globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas); displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0))); - currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9))); + currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano / Math.pow(10.0, 9))); totalHeading += globalDeltas.get(2, 0); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java index 5f0724c..83c172e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java @@ -219,7 +219,7 @@ public class ThreeWheelIMULocalizer extends Localizer { globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas); displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0))); - currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9))); + currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano / Math.pow(10.0, 9))); totalHeading += globalDeltas.get(2, 0); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java index 4368dc0..960a366 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java @@ -204,7 +204,7 @@ public class ThreeWheelLocalizer extends Localizer { globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas); displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0))); - currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9))); + currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano / Math.pow(10.0, 9))); totalHeading += globalDeltas.get(2, 0); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java index 685cebe..72cc63b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java @@ -208,7 +208,7 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas); displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0))); - currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9))); + currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano / Math.pow(10.0, 9))); totalHeading += globalDeltas.get(2, 0); } From 6441222924d98e6beb70204db068e2d078b716f8 Mon Sep 17 00:00:00 2001 From: Iris <87090296+Iris-TheRainbow@users.noreply.github.com> Date: Mon, 25 Nov 2024 19:13:16 -0600 Subject: [PATCH 66/94] Correct mistakes in roadrunner description --- .../org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md index f8c8868..ac88be4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md @@ -19,7 +19,7 @@ Why use Pedro Pathing? Why not something else like Road Runner or Pure Pursuit? * Pure Pursuit searches for the farthest point on the path that's within a certain radius from the robot. Pure Pursuit will then go in a straight line to that point. This poses several problems, as a small search radius will cause some oscillations on corners, and a large search radius will cut corners on paths, which makes the paths inaccurate to real life. * Pedro Pathing instead corrects to the closest point on the path while still following the path. This ensures that the follower will stay on the path while still being able to move forward along the path without cutting corners or encountering oscillation issues. * Why not Road Runner? - * Road Runner is a motion profile based follower, which means that a set of instructions for motor powers are calculated for each path beforehand and then run. After reaching the end of this motion profile, Road Runner corrects. This can be sufficient for most situations, but if the robot encounters an obstacle or wheel slippage, it may be unable to correct in time. + * Road Runner is a motion profile based follower, which means that a set of instructions for motor powers are calculated for each path beforehand and then run. During this motion profile, Road Runner can struggle to correct. This can be sufficient for many situations, but if the robot encounters an obstacle or wheel slippage, it may be unable to correct in time. * Pedro Pathing instead dynamically corrects throughout the path. The movement vectors are calculated at every point along the path, and because of this, the path can even be changed midway through and Pedro Pathing will still be able to correct. Since correction occurs throughout the path, the error correction isn't concentrated on the end of the path and therefore the robot is able to better minimize error. ## How Does Pedro Path? @@ -106,4 +106,4 @@ control. ## Questions? If you still have more questions, feel free to contact us at `scottsbots10158@gmail.com` or -within our discord linked here(https://discord.gg/2GfC4qBP5s) \ No newline at end of file +within our discord linked here(https://discord.gg/2GfC4qBP5s) From 97bb1faf3b867c89801f1cb15aa1005494c0ab6a Mon Sep 17 00:00:00 2001 From: Anyi Lin Date: Tue, 26 Nov 2024 00:18:34 -0500 Subject: [PATCH 67/94] drive encoder direction fix --- .../localization/localizers/DriveEncoderLocalizer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java index aa295e4..0dd7a56 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java @@ -71,7 +71,7 @@ public class DriveEncoderLocalizer extends Localizer { // TODO: reverse any encoders necessary leftFront.setDirection(Encoder.REVERSE); - rightRear.setDirection(Encoder.REVERSE); + rightFront.setDirection(Encoder.REVERSE); leftRear.setDirection(Encoder.FORWARD); rightRear.setDirection(Encoder.FORWARD); From 354c822b9229ac63f2192891cc97602880b2c438 Mon Sep 17 00:00:00 2001 From: Anyi Lin Date: Tue, 26 Nov 2024 00:18:34 -0500 Subject: [PATCH 68/94] drive encoder direction fix --- .../localization/localizers/DriveEncoderLocalizer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java index aa295e4..5f3290a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java @@ -71,8 +71,8 @@ public class DriveEncoderLocalizer extends Localizer { // TODO: reverse any encoders necessary leftFront.setDirection(Encoder.REVERSE); - rightRear.setDirection(Encoder.REVERSE); - leftRear.setDirection(Encoder.FORWARD); + leftRear.setDirection(Encoder.REVERSE); + rightFront.setDirection(Encoder.FORWARD); rightRear.setDirection(Encoder.FORWARD); setStartPose(setStartPose); From 659a22b33ede833fcc46dba9bb174568ad1a5473 Mon Sep 17 00:00:00 2001 From: Anyi Lin Date: Tue, 26 Nov 2024 04:54:23 -0500 Subject: [PATCH 69/94] fix total heading issue in PinpointLocalizer --- .../localization/localizers/PinpointLocalizer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java index 58c4746..e35875f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java @@ -152,8 +152,8 @@ public class PinpointLocalizer extends Localizer { @Override public void update() { odo.update(); - totalHeading += MathFunctions.getSmallestAngleDifference(odo.getHeading(), previousHeading); - previousHeading = odo.getHeading(); + totalHeading += MathFunctions.getSmallestAngleDifference(MathFunctions.normalizeAngle(odo.getHeading()), previousHeading); + previousHeading = MathFunctions.normalizeAngle(odo.getHeading()); } /** From f14c3f0873f53d30ed6a5cff14cf13043884d237 Mon Sep 17 00:00:00 2001 From: BaronClaps <126834072+BaronClaps@users.noreply.github.com> Date: Wed, 27 Nov 2024 12:34:57 -0600 Subject: [PATCH 70/94] Make distance to be 48 inches on push tests and velocity --- .../teamcode/pedroPathing/localization/tuning/ForwardTuner.java | 2 +- .../teamcode/pedroPathing/localization/tuning/LateralTuner.java | 2 +- .../ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java | 2 +- .../ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/ForwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/ForwardTuner.java index eed2210..d876746 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/ForwardTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/ForwardTuner.java @@ -32,7 +32,7 @@ public class ForwardTuner extends OpMode { private Telemetry telemetryA; - public static double DISTANCE = 30; + public static double DISTANCE = 48; /** * This initializes the PoseUpdater as well as the FTC Dashboard telemetry. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java index f3d42f3..dfa39dc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java @@ -33,7 +33,7 @@ public class LateralTuner extends OpMode { private Telemetry telemetryA; - public static double DISTANCE = 30; + public static double DISTANCE = 48; /** * This initializes the PoseUpdater as well as the FTC Dashboard telemetry. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java index ad07d88..f08eca3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java @@ -52,7 +52,7 @@ public class ForwardVelocityTuner extends OpMode { private PoseUpdater poseUpdater; - public static double DISTANCE = 40; + public static double DISTANCE = 48; public static double RECORD_NUMBER = 10; private Telemetry telemetryA; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java index ad6d22b..36da360 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java @@ -52,7 +52,7 @@ public class StrafeVelocityTuner extends OpMode { private PoseUpdater poseUpdater; - public static double DISTANCE = 40; + public static double DISTANCE = 48; public static double RECORD_NUMBER = 10; private Telemetry telemetryA; From fdc7a9f9792800242ee08ead305cdff9fad36a8c Mon Sep 17 00:00:00 2001 From: Anyi Lin Date: Thu, 28 Nov 2024 12:38:43 -0500 Subject: [PATCH 71/94] makes it brake after ending the velocity tuner --- .../teamcode/pedroPathing/tuning/ForwardVelocityTuner.java | 7 +++++++ .../teamcode/pedroPathing/tuning/StrafeVelocityTuner.java | 7 +++++++ 2 files changed, 14 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java index f08eca3..3045a34 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java @@ -143,6 +143,13 @@ public class ForwardVelocityTuner extends OpMode { velocities.remove(0); } } else { + leftFront.setPower(0); + leftRear.setPower(0); + rightFront.setPower(0); + rightRear.setPower(0); + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } double average = 0; for (Double velocity : velocities) { average += velocity; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java index 36da360..a15010a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java @@ -141,6 +141,13 @@ public class StrafeVelocityTuner extends OpMode { velocities.remove(0); } } else { + leftFront.setPower(0); + leftRear.setPower(0); + rightFront.setPower(0); + rightRear.setPower(0); + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } double average = 0; for (Double velocity : velocities) { average += velocity; From 58ab21d86d88e8893d90537c00053988d1bf2940 Mon Sep 17 00:00:00 2001 From: BaronClaps <126834072+BaronClaps@users.noreply.github.com> Date: Thu, 28 Nov 2024 20:48:25 -0600 Subject: [PATCH 72/94] TwoWheel + PinpointIMU Localizer --- .../TwoWheelPinpointIMULocalizer.java | 300 ++++++++++++++++++ 1 file changed, 300 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java new file mode 100644 index 0000000..bb2c7c7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java @@ -0,0 +1,300 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.GoBildaPinpointDriver; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; + + +/** + * This is the TwoWheelLocalizer class. This class extends the Localizer superclass and is a + * localizer that uses the two wheel odometry with IMU set up. The diagram below, which is modified from + * Road Runner, shows a typical set up. + * + * The view is from the top of the robot looking downwards. + * + * left on robot is the y positive direction + * + * forward on robot is the x positive direction + * + * /--------------\ + * | ____ | + * | ---- | + * | || || | + * | || || | ----> left (y positive) + * | | + * | | + * \--------------/ + * | + * | + * V + * forward (x positive) + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Havish Sripada - 12808 RevAmped Robotics + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 11/28/2024 + */ +@Config +public class TwoWheelPinpointIMULocalizer extends Localizer { + private HardwareMap hardwareMap; + private GoBildaPinpointDriver pinpoint; + private Pose startPose; + private Pose displacementPose; + private Pose currentVelocity; + private Matrix prevRotationMatrix; + private NanoTimer timer; + private long deltaTimeNano; + private Encoder forwardEncoder; + private Encoder strafeEncoder; + private Pose forwardEncoderPose; + private Pose strafeEncoderPose; + private double previousHeading; + private double deltaRadians; + private double totalHeading; + public static double FORWARD_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5008239963; + public static double STRAFE_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5018874659; + + /** + * This creates a new TwoWheelLocalizer from a HardwareMap, with a starting Pose at (0,0) + * facing 0 heading. + * + * @param map the HardwareMap + */ + public TwoWheelPinpointIMULocalizer(HardwareMap map) { + this(map, new Pose()); + } + + /** + * This creates a new TwoWheelLocalizer from a HardwareMap and a Pose, with the Pose + * specifying the starting pose of the localizer. + * + * @param map the HardwareMap + * @param setStartPose the Pose to start from + */ + public TwoWheelPinpointIMULocalizer(HardwareMap map, Pose setStartPose) { + // TODO: replace these with your encoder positions + forwardEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0); + strafeEncoderPose = new Pose(-107.9/25.4+0.25, -1.1/25.4-0.23, Math.toRadians(90)); + + hardwareMap = map; + + pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint"); + pinpoint.resetPosAndIMU(); + + // TODO: replace these with your encoder ports + forwardEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftRear")); + strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); + + // TODO: reverse any encoders necessary + forwardEncoder.setDirection(Encoder.REVERSE); + strafeEncoder.setDirection(Encoder.FORWARD); + + setStartPose(setStartPose); + timer = new NanoTimer(); + deltaTimeNano = 1; + displacementPose = new Pose(); + currentVelocity = new Pose(); + deltaRadians = 0; + } + + /** + * This returns the current pose estimate. + * + * @return returns the current pose estimate as a Pose + */ + @Override + public Pose getPose() { + return MathFunctions.addPoses(startPose, displacementPose); + } + + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Pose + */ + @Override + public Pose getVelocity() { + return currentVelocity.copy(); + } + + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Vector + */ + @Override + public Vector getVelocityVector() { + return currentVelocity.getVector(); + } + + /** + * This sets the start pose. Changing the start pose should move the robot as if all its + * previous movements were displacing it from its new start pose. + * + * @param setStart the new start pose + */ + @Override + public void setStartPose(Pose setStart) { + startPose = setStart; + } + + /** + * This sets the Matrix that contains the previous pose's heading rotation. + * + * @param heading the rotation of the Matrix + */ + public void setPrevRotationMatrix(double heading) { + prevRotationMatrix = new Matrix(3, 3); + prevRotationMatrix.set(0, 0, Math.cos(heading)); + prevRotationMatrix.set(0, 1, -Math.sin(heading)); + prevRotationMatrix.set(1, 0, Math.sin(heading)); + prevRotationMatrix.set(1, 1, Math.cos(heading)); + prevRotationMatrix.set(2, 2, 1.0); + } + + /** + * This sets the current pose estimate. Changing this should just change the robot's current + * pose estimate, not anything to do with the start pose. + * + * @param setPose the new current pose estimate + */ + @Override + public void setPose(Pose setPose) { + displacementPose = MathFunctions.subtractPoses(setPose, startPose); + resetEncoders(); + } + + /** + * This updates the elapsed time timer that keeps track of time between updates, as well as the + * change position of the Encoders and the IMU readings. Then, the robot's global change in + * position is calculated using the pose exponential method. + */ + @Override + public void update() { + deltaTimeNano = timer.getElapsedTime(); + timer.resetTimer(); + + updateEncoders(); + Matrix robotDeltas = getRobotDeltas(); + Matrix globalDeltas; + setPrevRotationMatrix(getPose().getHeading()); + + Matrix transformation = new Matrix(3, 3); + if (Math.abs(robotDeltas.get(2, 0)) < 0.001) { + transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); + transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0); + transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0); + transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); + transformation.set(2, 2, 1.0); + } else { + transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); + transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0)); + transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0)); + transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); + transformation.set(2, 2, 1.0); + } + + globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas); + + displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0))); + currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano / Math.pow(10.0, 9))); + + totalHeading += globalDeltas.get(2, 0); + } + + /** + * This updates the Encoders as well as the IMU. + */ + public void updateEncoders() { + forwardEncoder.update(); + strafeEncoder.update(); + + pinpoint.update(GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING); + double currentHeading = startPose.getHeading() + MathFunctions.normalizeAngle(pinpoint.getHeading()); + deltaRadians = MathFunctions.getTurnDirection(previousHeading, currentHeading) * MathFunctions.getSmallestAngleDifference(currentHeading, previousHeading); + previousHeading = currentHeading; + } + + /** + * This resets the Encoders. + */ + public void resetEncoders() { + forwardEncoder.reset(); + strafeEncoder.reset(); + } + + /** + * This calculates the change in position from the perspective of the robot using information + * from the Encoders and IMU. + * + * @return returns a Matrix containing the robot relative movement. + */ + public Matrix getRobotDeltas() { + Matrix returnMatrix = new Matrix(3, 1); + // x/forward movement + returnMatrix.set(0, 0, FORWARD_TICKS_TO_INCHES * (forwardEncoder.getDeltaPosition() - forwardEncoderPose.getY() * deltaRadians)); + //y/strafe movement + returnMatrix.set(1, 0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * deltaRadians)); + // theta/turning + returnMatrix.set(2, 0, deltaRadians); + return returnMatrix; + } + + /** + * This returns how far the robot has turned in radians, in a number not clamped between 0 and + * 2 * pi radians. This is used for some tuning things and nothing actually within the following. + * + * @return returns how far the robot has turned in total, in radians. + */ + public double getTotalHeading() { + return totalHeading; + } + + /** + * This returns the multiplier applied to forward movement measurement to convert from encoder + * ticks to inches. This is found empirically through a tuner. + * + * @return returns the forward ticks to inches multiplier + */ + public double getForwardMultiplier() { + return FORWARD_TICKS_TO_INCHES; + } + + /** + * This returns the multiplier applied to lateral/strafe movement measurement to convert from + * encoder ticks to inches. This is found empirically through a tuner. + * + * @return returns the lateral/strafe ticks to inches multiplier + */ + public double getLateralMultiplier() { + return STRAFE_TICKS_TO_INCHES; + } + + /** + * This returns the multiplier applied to turning movement measurement to convert from encoder + * ticks to radians. This is found empirically through a tuner. + * + * @return returns the turning ticks to radians multiplier + */ + public double getTurningMultiplier() { + return 1; + } + + /** + * This resets the IMU. + */ + + public void resetIMU() { + pinpoint.resetPosAndIMU(); + } +} + From 3b47dc42694c1395f2ac9f714844a02857af48aa Mon Sep 17 00:00:00 2001 From: BaronClaps <126834072+BaronClaps@users.noreply.github.com> Date: Thu, 28 Nov 2024 23:15:08 -0600 Subject: [PATCH 73/94] Update TwoWheelPinpointIMULocalizer.java --- .../TwoWheelPinpointIMULocalizer.java | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java index bb2c7c7..77e715a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java @@ -15,8 +15,8 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; /** - * This is the TwoWheelLocalizer class. This class extends the Localizer superclass and is a - * localizer that uses the two wheel odometry with IMU set up. The diagram below, which is modified from + * This is the TwoWheelPinpointIMULocalizer class. This class extends the Localizer superclass and is a + * localizer that uses the two wheel odometry set up. The diagram below, which is modified from * Road Runner, shows a typical set up. * * The view is from the top of the robot looking downwards. @@ -25,18 +25,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; * * forward on robot is the x positive direction * - * /--------------\ - * | ____ | - * | ---- | - * | || || | - * | || || | ----> left (y positive) - * | | - * | | - * \--------------/ - * | - * | - * V - * forward (x positive) + * forward (x positive) + * △ + * | + * | + * /--------------\ + * | | + * | | + * | || || | + * left (y positive) <--- | || || | + * | ____ | + * | ---- | + * \--------------/ * * @author Anyi Lin - 10158 Scott's Bots * @author Havish Sripada - 12808 RevAmped Robotics From 22dba85a5ff2fe7c509887ca44e466dcadff4f90 Mon Sep 17 00:00:00 2001 From: BaronClaps <126834072+BaronClaps@users.noreply.github.com> Date: Thu, 28 Nov 2024 23:16:24 -0600 Subject: [PATCH 74/94] Update TwoWheelPinpointIMULocalizer.java --- .../localizers/TwoWheelPinpointIMULocalizer.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java index 77e715a..d2e03f3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java @@ -16,7 +16,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; /** * This is the TwoWheelPinpointIMULocalizer class. This class extends the Localizer superclass and is a - * localizer that uses the two wheel odometry set up. The diagram below, which is modified from + * localizer that uses the two wheel odometry with the Pinpoint IMU set up. The diagram below, which is modified from * Road Runner, shows a typical set up. * * The view is from the top of the robot looking downwards. @@ -25,15 +25,15 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; * * forward on robot is the x positive direction * - * forward (x positive) +* forward (x positive) * △ * | * | * /--------------\ * | | * | | - * | || || | - * left (y positive) <--- | || || | + * | || | + * left (y positive) <--- | || | * | ____ | * | ---- | * \--------------/ From a0b3b7621083d6e627bffa89d428c3f79596d427 Mon Sep 17 00:00:00 2001 From: Julien Vanier Date: Fri, 29 Nov 2024 10:00:54 -0500 Subject: [PATCH 75/94] Extract motor direction to constants so they are set in one place --- .../teamcode/pedroPathing/follower/Follower.java | 13 ++++++++----- .../localization/tuning/LocalizationTest.java | 12 ++++++++---- .../pedroPathing/tuning/FollowerConstants.java | 6 ++++++ .../pedroPathing/tuning/ForwardVelocityTuner.java | 13 ++++++++----- .../tuning/ForwardZeroPowerAccelerationTuner.java | 13 ++++++++----- .../tuning/LateralZeroPowerAccelerationTuner.java | 13 ++++++++----- .../pedroPathing/tuning/StrafeVelocityTuner.java | 13 ++++++++----- 7 files changed, 54 insertions(+), 29 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index 5089eb4..cf7aaec 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -10,6 +10,10 @@ import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstan import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorDirection; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryDrivePIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryHeadingPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryTranslationalPIDFFeedForward; @@ -23,7 +27,6 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; @@ -166,10 +169,10 @@ public class Follower { leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); - - // TODO: Make sure that this is the direction your motors need to be reversed in. - leftFront.setDirection(DcMotorSimple.Direction.REVERSE); - leftRear.setDirection(DcMotorSimple.Direction.REVERSE); + leftFront.setDirection(leftFrontMotorDirection); + leftRear.setDirection(leftRearMotorDirection); + rightFront.setDirection(rightFrontMotorDirection); + rightRear.setDirection(rightRearMotorDirection); motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java index 32e0452..8da3c3e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java @@ -4,6 +4,10 @@ import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstan import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorDirection; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -12,7 +16,6 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -57,9 +60,10 @@ public class LocalizationTest extends OpMode { leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); - - leftFront.setDirection(DcMotorSimple.Direction.REVERSE); - leftRear.setDirection(DcMotorSimple.Direction.REVERSE); + leftFront.setDirection(leftFrontMotorDirection); + leftRear.setDirection(leftRearMotorDirection); + rightFront.setDirection(rightFrontMotorDirection); + rightRear.setDirection(rightRearMotorDirection); motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java index bd932cd..e3f25b4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.pedroPathing.tuning; import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; @@ -28,6 +29,11 @@ public class FollowerConstants { public static String rightFrontMotorName = "rightFront"; public static String rightRearMotorName = "rightRear"; + public static DcMotorSimple.Direction leftFrontMotorDirection = DcMotorSimple.Direction.REVERSE; + public static DcMotorSimple.Direction rightFrontMotorDirection = DcMotorSimple.Direction.REVERSE; + public static DcMotorSimple.Direction leftRearMotorDirection = DcMotorSimple.Direction.FORWARD; + public static DcMotorSimple.Direction rightRearMotorDirection = DcMotorSimple.Direction.FORWARD; + // This section is for setting the actual drive vector for the front left wheel, if the robot // is facing a heading of 0 radians with the wheel centered at (0,0) private static double xMovement = 81.34056; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java index ad07d88..8ff2f14 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java @@ -4,6 +4,10 @@ import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstan import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorDirection; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -12,7 +16,6 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -71,10 +74,10 @@ public class ForwardVelocityTuner extends OpMode { leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); - - // TODO: Make sure that this is the direction your motors need to be reversed in. - leftFront.setDirection(DcMotorSimple.Direction.REVERSE); - leftRear.setDirection(DcMotorSimple.Direction.REVERSE); + leftFront.setDirection(leftFrontMotorDirection); + leftRear.setDirection(leftRearMotorDirection); + rightFront.setDirection(rightFrontMotorDirection); + rightRear.setDirection(rightRearMotorDirection); motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java index f7393bd..e774504 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java @@ -4,6 +4,10 @@ import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstan import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorDirection; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -12,7 +16,6 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -74,10 +77,10 @@ public class ForwardZeroPowerAccelerationTuner extends OpMode { leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); - - // TODO: Make sure that this is the direction your motors need to be reversed in. - leftFront.setDirection(DcMotorSimple.Direction.REVERSE); - leftRear.setDirection(DcMotorSimple.Direction.REVERSE); + leftFront.setDirection(leftFrontMotorDirection); + leftRear.setDirection(leftRearMotorDirection); + rightFront.setDirection(rightFrontMotorDirection); + rightRear.setDirection(rightRearMotorDirection); motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java index 87b1978..db4e903 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java @@ -4,6 +4,10 @@ import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstan import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorDirection; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -12,7 +16,6 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -74,10 +77,10 @@ public class LateralZeroPowerAccelerationTuner extends OpMode { leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); - - // TODO: Make sure that this is the direction your motors need to be reversed in. - leftFront.setDirection(DcMotorSimple.Direction.REVERSE); - leftRear.setDirection(DcMotorSimple.Direction.REVERSE); + leftFront.setDirection(leftFrontMotorDirection); + leftRear.setDirection(leftRearMotorDirection); + rightFront.setDirection(rightFrontMotorDirection); + rightRear.setDirection(rightRearMotorDirection); motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java index ad6d22b..ca82022 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java @@ -4,6 +4,10 @@ import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstan import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorDirection; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorDirection; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -12,7 +16,6 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -71,10 +74,10 @@ public class StrafeVelocityTuner extends OpMode { leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); - - // TODO: Make sure that this is the direction your motors need to be reversed in. - leftFront.setDirection(DcMotorSimple.Direction.REVERSE); - leftRear.setDirection(DcMotorSimple.Direction.REVERSE); + leftFront.setDirection(leftFrontMotorDirection); + leftRear.setDirection(leftRearMotorDirection); + rightFront.setDirection(rightFrontMotorDirection); + rightRear.setDirection(rightRearMotorDirection); motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear); From 6b601b9dd44ad82abc009525b30ad196dd98920e Mon Sep 17 00:00:00 2001 From: junkjunk123 <139065303+junkjunk123@users.noreply.github.com> Date: Thu, 5 Dec 2024 18:08:59 -0800 Subject: [PATCH 76/94] Start position heading fix on TwoWheelPinpointIMU --- .../localization/localizers/TwoWheelPinpointIMULocalizer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java index d2e03f3..bdf418e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java @@ -113,7 +113,7 @@ public class TwoWheelPinpointIMULocalizer extends Localizer { */ @Override public Pose getPose() { - return MathFunctions.addPoses(startPose, displacementPose); + return new Pose(startPose.getX()+displacementPose.getX(), startPose.getY()+displacementPose.getY(),displacementPose.getHeading()); } /** From 460639127bfdba92b05c7447f82b029609dbad00 Mon Sep 17 00:00:00 2001 From: Logan-Nash Date: Fri, 6 Dec 2024 10:41:02 -0500 Subject: [PATCH 77/94] Commented out PinpointLocalizer.java , Too many issues for now. will uncomment when proven test is complete --- .../localizers/PinpointLocalizer.java | 422 +++++++++--------- 1 file changed, 211 insertions(+), 211 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java index e35875f..b0ef6bc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java @@ -1,211 +1,211 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; - - -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.GoBildaPinpointDriver; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; - -/** - * This is the Pinpoint class. This class extends the Localizer superclass and is a - * localizer that uses the two wheel odometry set up with the IMU to have more accurate heading - * readings. The diagram below, which is modified from Road Runner, shows a typical set up. - * - * The view is from the top of the robot looking downwards. - * - * left on robot is the y positive direction - * - * forward on robot is the x positive direction - * - * forward (x positive) - * △ - * | - * | - * /--------------\ - * | | - * | | - * | || | - * left (y positive) <--- | || | - * | ____ | - * | ---- | - * \--------------/ - * With the pinpoint your readings will be used in mm - * to use inches ensure to divide your mm value by 25.4 - * @author Logan Nash - * @author Havish Sripada 12808 - RevAmped Robotics - * @author Ethan Doak - Gobilda - * @version 1.0, 10/2/2024 - */ -public class PinpointLocalizer extends Localizer { - private HardwareMap hardwareMap; - private GoBildaPinpointDriver odo; - private double previousHeading; - private double totalHeading; - - /** - * This creates a new PinpointLocalizer from a HardwareMap, with a starting Pose at (0,0) - * facing 0 heading. - * - * @param map the HardwareMap - */ - public PinpointLocalizer(HardwareMap map){ this(map, new Pose());} - - /** - * This creates a new PinpointLocalizer from a HardwareMap and a Pose, with the Pose - * specifying the starting pose of the localizer. - * - * @param map the HardwareMap - * @param setStartPose the Pose to start from - */ - public PinpointLocalizer(HardwareMap map, Pose setStartPose){ - hardwareMap = map; - // TODO: replace this with your Pinpoint port - odo = hardwareMap.get(GoBildaPinpointDriver.class,"odo"); - - //This uses mm, to use inches divide these numbers by 25.4 - odo.setOffsets(-84.0, -168.0); //these are tuned for 3110-0002-0001 Product Insight #1 - //TODO: If you find that the gobilda Yaw Scaling is incorrect you can edit this here - // odo.setYawScalar(1.0); - //TODO: Set your encoder resolution here, I have the Gobilda Odometry products already included. - //TODO: If you would like to use your own odometry pods input the ticks per mm in the commented part below - odo.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); - //odo.setEncoderResolution(13.26291192); - //TODO: Set encoder directions - odo.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD); - - odo.resetPosAndIMU(); - - setStartPose(setStartPose); - totalHeading = 0; - previousHeading = setStartPose.getHeading(); - - resetPinpoint(); - } - - /** - * This returns the current pose estimate. - * - * @return returns the current pose estimate as a Pose - */ - @Override - public Pose getPose() { - Pose2D rawPose = odo.getPosition(); - return new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS)); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Pose - */ - @Override - public Pose getVelocity() { - Pose2D pose = odo.getVelocity(); - return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), pose.getHeading(AngleUnit.RADIANS)); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Vector - */ - @Override - public Vector getVelocityVector() { - Pose2D pose = odo.getVelocity(); - Vector returnVector = new Vector(); - returnVector.setOrthogonalComponents(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH)); - return returnVector; - } - - /** - * This sets the start pose. Since nobody should be using this after the robot has begun moving, - * and due to issues with the PinpointLocalizer, this is functionally the same as setPose(Pose). - * - * @param setStart the new start pose - */ - @Override - public void setStartPose(Pose setStart) { - odo.setPosition(new Pose2D(DistanceUnit.INCH, setStart.getX(), setStart.getY(), AngleUnit.RADIANS, setStart.getHeading())); - } - - /** - * This sets the current pose estimate. Changing this should just change the robot's current - * pose estimate, not anything to do with the start pose. - * - * @param setPose the new current pose estimate - */ - @Override - public void setPose(Pose setPose) { - resetPinpoint(); - odo.setPosition(new Pose2D(DistanceUnit.INCH, setPose.getX(), setPose.getY(), AngleUnit.RADIANS, setPose.getHeading())); - } - - /** - * This updates the total heading of the robot. The Pinpoint handles all other updates itself. - */ - @Override - public void update() { - odo.update(); - totalHeading += MathFunctions.getSmallestAngleDifference(MathFunctions.normalizeAngle(odo.getHeading()), previousHeading); - previousHeading = MathFunctions.normalizeAngle(odo.getHeading()); - } - - /** - * This returns how far the robot has turned in radians, in a number not clamped between 0 and - * 2 * pi radians. This is used for some tuning things and nothing actually within the following. - * - * @return returns how far the robot has turned in total, in radians. - */ - @Override - public double getTotalHeading() { - return totalHeading; - } - - /** - * This returns the Y encoder value as none of the odometry tuners are required for this localizer - * @return returns the Y encoder value - */ - @Override - public double getForwardMultiplier() { - return odo.getEncoderY(); - } - - /** - * This returns the X encoder value as none of the odometry tuners are required for this localizer - * @return returns the X encoder value - */ - @Override - public double getLateralMultiplier() { - return odo.getEncoderX(); - } - - /** - * This returns either the factory tuned yaw scalar or the yaw scalar tuned by yourself. - * @return returns the yaw scalar - */ - @Override - public double getTurningMultiplier() { - return odo.getYawScalar(); - } - - /** - * This resets the IMU. - */ - @Override - public void resetIMU() { - odo.recalibrateIMU(); - } - - /** - * This resets the OTOS. - */ - public void resetPinpoint(){ - odo.resetPosAndIMU(); - } -} +//package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; +// +// +//import com.qualcomm.robotcore.hardware.HardwareMap; +// +//import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +//import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +//import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +//import org.firstinspires.ftc.teamcode.pedroPathing.localization.GoBildaPinpointDriver; +//import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; +//import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +// +///** +// * This is the Pinpoint class. This class extends the Localizer superclass and is a +// * localizer that uses the two wheel odometry set up with the IMU to have more accurate heading +// * readings. The diagram below, which is modified from Road Runner, shows a typical set up. +// * +// * The view is from the top of the robot looking downwards. +// * +// * left on robot is the y positive direction +// * +// * forward on robot is the x positive direction +// * +// * forward (x positive) +// * △ +// * | +// * | +// * /--------------\ +// * | | +// * | | +// * | || | +// * left (y positive) <--- | || | +// * | ____ | +// * | ---- | +// * \--------------/ +// * With the pinpoint your readings will be used in mm +// * to use inches ensure to divide your mm value by 25.4 +// * @author Logan Nash +// * @author Havish Sripada 12808 - RevAmped Robotics +// * @author Ethan Doak - Gobilda +// * @version 1.0, 10/2/2024 +// */ +//public class PinpointLocalizer extends Localizer { +// private HardwareMap hardwareMap; +// private GoBildaPinpointDriver odo; +// private double previousHeading; +// private double totalHeading; +// +// /** +// * This creates a new PinpointLocalizer from a HardwareMap, with a starting Pose at (0,0) +// * facing 0 heading. +// * +// * @param map the HardwareMap +// */ +// public PinpointLocalizer(HardwareMap map){ this(map, new Pose());} +// +// /** +// * This creates a new PinpointLocalizer from a HardwareMap and a Pose, with the Pose +// * specifying the starting pose of the localizer. +// * +// * @param map the HardwareMap +// * @param setStartPose the Pose to start from +// */ +// public PinpointLocalizer(HardwareMap map, Pose setStartPose){ +// hardwareMap = map; +// // TODO: replace this with your Pinpoint port +// odo = hardwareMap.get(GoBildaPinpointDriver.class,"odo"); +// +// //This uses mm, to use inches divide these numbers by 25.4 +// odo.setOffsets(-84.0, -168.0); //these are tuned for 3110-0002-0001 Product Insight #1 +// //TODO: If you find that the gobilda Yaw Scaling is incorrect you can edit this here +// // odo.setYawScalar(1.0); +// //TODO: Set your encoder resolution here, I have the Gobilda Odometry products already included. +// //TODO: If you would like to use your own odometry pods input the ticks per mm in the commented part below +// odo.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); +// //odo.setEncoderResolution(13.26291192); +// //TODO: Set encoder directions +// odo.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD); +// +// odo.resetPosAndIMU(); +// +// setStartPose(setStartPose); +// totalHeading = 0; +// previousHeading = setStartPose.getHeading(); +// +// resetPinpoint(); +// } +// +// /** +// * This returns the current pose estimate. +// * +// * @return returns the current pose estimate as a Pose +// */ +// @Override +// public Pose getPose() { +// Pose2D rawPose = odo.getPosition(); +// return new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS)); +// } +// +// /** +// * This returns the current velocity estimate. +// * +// * @return returns the current velocity estimate as a Pose +// */ +// @Override +// public Pose getVelocity() { +// Pose2D pose = odo.getVelocity(); +// return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), pose.getHeading(AngleUnit.RADIANS)); +// } +// +// /** +// * This returns the current velocity estimate. +// * +// * @return returns the current velocity estimate as a Vector +// */ +// @Override +// public Vector getVelocityVector() { +// Pose2D pose = odo.getVelocity(); +// Vector returnVector = new Vector(); +// returnVector.setOrthogonalComponents(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH)); +// return returnVector; +// } +// +// /** +// * This sets the start pose. Since nobody should be using this after the robot has begun moving, +// * and due to issues with the PinpointLocalizer, this is functionally the same as setPose(Pose). +// * +// * @param setStart the new start pose +// */ +// @Override +// public void setStartPose(Pose setStart) { +// odo.setPosition(new Pose2D(DistanceUnit.INCH, setStart.getX(), setStart.getY(), AngleUnit.RADIANS, setStart.getHeading())); +// } +// +// /** +// * This sets the current pose estimate. Changing this should just change the robot's current +// * pose estimate, not anything to do with the start pose. +// * +// * @param setPose the new current pose estimate +// */ +// @Override +// public void setPose(Pose setPose) { +// resetPinpoint(); +// odo.setPosition(new Pose2D(DistanceUnit.INCH, setPose.getX(), setPose.getY(), AngleUnit.RADIANS, setPose.getHeading())); +// } +// +// /** +// * This updates the total heading of the robot. The Pinpoint handles all other updates itself. +// */ +// @Override +// public void update() { +// odo.update(); +// totalHeading += MathFunctions.getSmallestAngleDifference(MathFunctions.normalizeAngle(odo.getHeading()), previousHeading); +// previousHeading = MathFunctions.normalizeAngle(odo.getHeading()); +// } +// +// /** +// * This returns how far the robot has turned in radians, in a number not clamped between 0 and +// * 2 * pi radians. This is used for some tuning things and nothing actually within the following. +// * +// * @return returns how far the robot has turned in total, in radians. +// */ +// @Override +// public double getTotalHeading() { +// return totalHeading; +// } +// +// /** +// * This returns the Y encoder value as none of the odometry tuners are required for this localizer +// * @return returns the Y encoder value +// */ +// @Override +// public double getForwardMultiplier() { +// return odo.getEncoderY(); +// } +// +// /** +// * This returns the X encoder value as none of the odometry tuners are required for this localizer +// * @return returns the X encoder value +// */ +// @Override +// public double getLateralMultiplier() { +// return odo.getEncoderX(); +// } +// +// /** +// * This returns either the factory tuned yaw scalar or the yaw scalar tuned by yourself. +// * @return returns the yaw scalar +// */ +// @Override +// public double getTurningMultiplier() { +// return odo.getYawScalar(); +// } +// +// /** +// * This resets the IMU. +// */ +// @Override +// public void resetIMU() { +// odo.recalibrateIMU(); +// } +// +// /** +// * This resets the OTOS. +// */ +// public void resetPinpoint(){ +// odo.resetPosAndIMU(); +// } +//} From fd5d6b716b3cc643e93370990ecfc836d15d2c5e Mon Sep 17 00:00:00 2001 From: junkjunk123 <139065303+junkjunk123@users.noreply.github.com> Date: Fri, 6 Dec 2024 09:17:29 -0800 Subject: [PATCH 78/94] Update PinpointLocalizer.java --- .../localizers/PinpointLocalizer.java | 29 ++++++++++++------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java index b0ef6bc..f6ee733 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java @@ -79,13 +79,11 @@ // //TODO: Set encoder directions // odo.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD); // -// odo.resetPosAndIMU(); -// +// resetPinpoint();; + // setStartPose(setStartPose); // totalHeading = 0; // previousHeading = setStartPose.getHeading(); -// -// resetPinpoint(); // } // // /** @@ -142,7 +140,6 @@ // */ // @Override // public void setPose(Pose setPose) { -// resetPinpoint(); // odo.setPosition(new Pose2D(DistanceUnit.INCH, setPose.getX(), setPose.getY(), AngleUnit.RADIANS, setPose.getHeading())); // } // @@ -195,17 +192,29 @@ // } // // /** -// * This resets the IMU. +// * This resets the IMU. Note: This does not change the estimated heading orientation. // */ // @Override -// public void resetIMU() { -// odo.recalibrateIMU(); +// public void resetIMU() throws InterruptedException { +// odo.recalibrateIMU(); +// +// try { +// Thread.sleep(300); +// } catch (InterruptedException e) { +// throw new RuntimeException(e); +// } // } // // /** -// * This resets the OTOS. +// * This resets the pinpoint. // */ -// public void resetPinpoint(){ +// private void resetPinpoint() { // odo.resetPosAndIMU(); +// +// try { +// Thread.sleep(300); +// } catch (InterruptedException e) { +// throw new RuntimeException(e); +// } // } //} From 7bc4dbbd341078e25f0499ff31641a00367e540a Mon Sep 17 00:00:00 2001 From: junkjunk123 <139065303+junkjunk123@users.noreply.github.com> Date: Fri, 6 Dec 2024 09:18:37 -0800 Subject: [PATCH 79/94] Update PoseUpdater.java --- .../ftc/teamcode/pedroPathing/localization/PoseUpdater.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java index 3daf54a..b718320 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java @@ -351,7 +351,7 @@ public class PoseUpdater { /** * */ - public void resetIMU() { + public void resetIMU() throws InterruptedException { localizer.resetIMU(); } } From 8a55b00c42fe7b89aff23aef4efc52e21b42cb15 Mon Sep 17 00:00:00 2001 From: junkjunk123 <139065303+junkjunk123@users.noreply.github.com> Date: Fri, 6 Dec 2024 09:19:48 -0800 Subject: [PATCH 80/94] Update Localizer.java --- .../ftc/teamcode/pedroPathing/localization/Localizer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java index 059ee31..64710ac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java @@ -92,7 +92,7 @@ public abstract class Localizer { /** * This resets the IMU of the localizer, if applicable. */ - public abstract void resetIMU(); + public abstract void resetIMU() throws InterruptedException; /** * This is overridden to return the IMU, if there is one. From 7471de24d27fbe418080127e4169b2271edb817e Mon Sep 17 00:00:00 2001 From: junkjunk123 <139065303+junkjunk123@users.noreply.github.com> Date: Fri, 6 Dec 2024 09:20:27 -0800 Subject: [PATCH 81/94] Update Follower.java --- .../ftc/teamcode/pedroPathing/follower/Follower.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index 5089eb4..23885ca 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -1019,7 +1019,7 @@ public class Follower { /** * This resets the IMU, if applicable. */ - public void resetIMU() { + private void resetIMU() throws InterruptedException { poseUpdater.resetIMU(); } } From 9330c2221315f845986f7df4fcfd5daec072c7bb Mon Sep 17 00:00:00 2001 From: junkjunk123 <139065303+junkjunk123@users.noreply.github.com> Date: Fri, 6 Dec 2024 09:36:06 -0800 Subject: [PATCH 82/94] Update PinpointLocalizer.java --- .../pedroPathing/localization/localizers/PinpointLocalizer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java index f6ee733..73272b8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java @@ -105,7 +105,7 @@ // @Override // public Pose getVelocity() { // Pose2D pose = odo.getVelocity(); -// return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), pose.getHeading(AngleUnit.RADIANS)); +// return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), odo.getHeadingVelocity()); // } // // /** From f4942715d984666bd8d1c0fb57fdaba1b0cec02b Mon Sep 17 00:00:00 2001 From: Logan-Nash Date: Tue, 10 Dec 2024 10:40:22 -0500 Subject: [PATCH 83/94] Fixed loop times on OTOS localizer. Updated the pinpoint to the latest driver --- .../pedroPathing/localization/GoBildaPinpointDriver.java | 7 +++++++ .../localization/localizers/OTOSLocalizer.java | 8 +++++--- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java index a2bc9d8..882ed56 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java @@ -314,9 +314,16 @@ public class GoBildaPinpointDriver extends I2cDeviceSynchDevice Date: Sun, 15 Dec 2024 18:14:56 -0500 Subject: [PATCH 84/94] makes limiting the max power better --- .../follower/DriveVectorScaler.java | 46 +++++++++++++------ .../pedroPathing/follower/Follower.java | 30 ++++-------- 2 files changed, 42 insertions(+), 34 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java index f2cfcfd..a6ed238 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java @@ -15,6 +15,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; public class DriveVectorScaler { // This is ordered left front, left back, right front, right back. These are also normalized. private Vector[] mecanumVectors; + private double maxPowerScaling = 1; /** * This creates a new DriveVectorScaler, which takes in various movement vectors and outputs @@ -52,9 +53,9 @@ public class DriveVectorScaler { */ public double[] getDrivePowers(Vector correctivePower, Vector headingPower, Vector pathingPower, double robotHeading) { // clamps down the magnitudes of the input vectors - if (correctivePower.getMagnitude() > 1) correctivePower.setMagnitude(1); - if (headingPower.getMagnitude() > 1) headingPower.setMagnitude(1); - if (pathingPower.getMagnitude() > 1) pathingPower.setMagnitude(1); + if (correctivePower.getMagnitude() > maxPowerScaling) correctivePower.setMagnitude(maxPowerScaling); + if (headingPower.getMagnitude() > maxPowerScaling) headingPower.setMagnitude(maxPowerScaling); + if (pathingPower.getMagnitude() > maxPowerScaling) pathingPower.setMagnitude(maxPowerScaling); // the powers for the wheel vectors double [] wheelPowers = new double[4]; @@ -65,8 +66,8 @@ public class DriveVectorScaler { // this contains the pathing vectors, one for each side (heading control requires 2) Vector[] truePathingVectors = new Vector[2]; - if (correctivePower.getMagnitude() == 1) { - // checks for corrective power equal to 1 in magnitude. if equal to one, then set pathing power to that + if (correctivePower.getMagnitude() == maxPowerScaling) { + // checks for corrective power equal to max power scaling in magnitude. if equal, then set pathing power to that truePathingVectors[0] = MathFunctions.copyVector(correctivePower); truePathingVectors[1] = MathFunctions.copyVector(correctivePower); } else { @@ -74,7 +75,7 @@ public class DriveVectorScaler { Vector leftSideVector = MathFunctions.subtractVectors(correctivePower, headingPower); Vector rightSideVector = MathFunctions.addVectors(correctivePower, headingPower); - if (leftSideVector.getMagnitude() > 1 || rightSideVector.getMagnitude() > 1) { + if (leftSideVector.getMagnitude() > maxPowerScaling || rightSideVector.getMagnitude() > maxPowerScaling) { //if the combined corrective and heading power is greater than 1, then scale down heading power double headingScalingFactor = Math.min(findNormalizingScaling(correctivePower, headingPower), findNormalizingScaling(correctivePower, MathFunctions.scalarMultiplyVector(headingPower, -1))); truePathingVectors[0] = MathFunctions.subtractVectors(correctivePower, MathFunctions.scalarMultiplyVector(headingPower, headingScalingFactor)); @@ -84,7 +85,7 @@ public class DriveVectorScaler { Vector leftSideVectorWithPathing = MathFunctions.addVectors(leftSideVector, pathingPower); Vector rightSideVectorWithPathing = MathFunctions.addVectors(rightSideVector, pathingPower); - if (leftSideVectorWithPathing.getMagnitude() > 1 || rightSideVectorWithPathing.getMagnitude() > 1) { + if (leftSideVectorWithPathing.getMagnitude() > maxPowerScaling || rightSideVectorWithPathing.getMagnitude() > maxPowerScaling) { // too much power now, so we scale down the pathing vector double pathingScalingFactor = Math.min(findNormalizingScaling(leftSideVector, pathingPower), findNormalizingScaling(rightSideVector, pathingPower)); truePathingVectors[0] = MathFunctions.addVectors(leftSideVector, MathFunctions.scalarMultiplyVector(pathingPower, pathingScalingFactor)); @@ -113,7 +114,7 @@ public class DriveVectorScaler { wheelPowers[3] = (mecanumVectorsCopy[2].getXComponent()*truePathingVectors[1].getYComponent() - truePathingVectors[1].getXComponent()*mecanumVectorsCopy[2].getYComponent()) / (mecanumVectorsCopy[2].getXComponent()*mecanumVectorsCopy[3].getYComponent() - mecanumVectorsCopy[3].getXComponent()*mecanumVectorsCopy[2].getYComponent()); double wheelPowerMax = Math.max(Math.max(Math.abs(wheelPowers[0]), Math.abs(wheelPowers[1])), Math.max(Math.abs(wheelPowers[2]), Math.abs(wheelPowers[3]))); - if (wheelPowerMax > 1) { + if (wheelPowerMax > maxPowerScaling) { wheelPowers[0] /= wheelPowerMax; wheelPowers[1] /= wheelPowerMax; wheelPowers[2] /= wheelPowerMax; @@ -126,12 +127,12 @@ public class DriveVectorScaler { /** * This takes in two Vectors, one static and one variable, and returns the scaling factor that, * when multiplied to the variable Vector, results in magnitude of the sum of the static Vector - * and the scaled variable Vector being 1. + * and the scaled variable Vector being the max power scaling. * * IMPORTANT NOTE: I did not intend for this to be used for anything other than the method above - * this one in this class, so there will be errors if you input Vectors of length greater than 1, + * this one in this class, so there will be errors if you input Vectors of length greater than maxPowerScaling, * and it will scale up the variable Vector if the magnitude of the sum of the two input Vectors - * isn't greater than 1. So, just don't use this elsewhere. There's gotta be a better way to do + * isn't greater than maxPowerScaling. So, just don't use this elsewhere. There's gotta be a better way to do * whatever you're trying to do. * * I know that this is used outside of this class, however, I created this method so I get to @@ -140,13 +141,32 @@ public class DriveVectorScaler { * * @param staticVector the Vector that is held constant. * @param variableVector the Vector getting scaled to make the sum of the input Vectors have a - * magnitude of 1. + * magnitude of maxPowerScaling. * @return returns the scaling factor for the variable Vector. */ public double findNormalizingScaling(Vector staticVector, Vector variableVector) { double a = Math.pow(variableVector.getXComponent(), 2) + Math.pow(variableVector.getYComponent(), 2); double b = staticVector.getXComponent() * variableVector.getXComponent() + staticVector.getYComponent() * variableVector.getYComponent(); - double c = Math.pow(staticVector.getXComponent(), 2) + Math.pow(staticVector.getYComponent(), 2) - 1.0; + double c = Math.pow(staticVector.getXComponent(), 2) + Math.pow(staticVector.getYComponent(), 2) - Math.pow(maxPowerScaling, 2); return (-b + Math.sqrt(Math.pow(b, 2) - a*c))/(a); + + } + + /** + * Sets the maximum power that can be used by the drive vector scaler. Clamped between 0 and 1. + * + * @param maxPowerScaling setting the max power scaling + */ + public void setMaxPowerScaling(double maxPowerScaling) { + this.maxPowerScaling = MathFunctions.clamp(maxPowerScaling, 0, 1); + } + + /** + * Gets the maximum power that can be used by the drive vector scaler. Ranges between 0 and 1. + * + * @return returns the max power scaling + */ + public double getMaxPowerScaling() { + return maxPowerScaling; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index 23885ca..8753f59 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -93,7 +93,6 @@ public class Follower { private boolean holdPositionAtEnd; private boolean teleopDrive; - private double maxPower = 1; private double previousSecondaryTranslationalIntegral; private double previousTranslationalIntegral; private double holdPointTranslationalScaling = FollowerConstants.holdPointTranslationalScaling; @@ -194,18 +193,7 @@ public class Follower { * @param set This caps the motor power from [0, 1]. */ public void setMaxPower(double set) { - maxPower = MathFunctions.clamp(set, 0, 1); - } - - /** - * This handles the limiting of the drive powers array to the max power. - */ - public void limitDrivePowers() { - for (int i = 0; i < drivePowers.length; i++) { - if (Math.abs(drivePowers[i]) > maxPower) { - drivePowers[i] = maxPower * MathFunctions.getSign(drivePowers[i]); - } - } + driveVectorScaler.setMaxPowerScaling(set); } /** @@ -693,19 +681,19 @@ public class Follower { public Vector getDriveVector() { if (!useDrive) return new Vector(); if (followingPathChain && chainIndex < currentPathChain.size() - 1) { - return new Vector(1, currentPath.getClosestPointTangentVector().getTheta()); + return new Vector(driveVectorScaler.getMaxPowerScaling(), currentPath.getClosestPointTangentVector().getTheta()); } driveError = getDriveVelocityError(); if (Math.abs(driveError) < drivePIDFSwitch && useSecondaryDrivePID) { secondaryDrivePIDF.updateError(driveError); - driveVector = new Vector(MathFunctions.clamp(secondaryDrivePIDF.runPIDF() + secondaryDrivePIDFFeedForward * MathFunctions.getSign(driveError), -1, 1), currentPath.getClosestPointTangentVector().getTheta()); + driveVector = new Vector(MathFunctions.clamp(secondaryDrivePIDF.runPIDF() + secondaryDrivePIDFFeedForward * MathFunctions.getSign(driveError), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), currentPath.getClosestPointTangentVector().getTheta()); return MathFunctions.copyVector(driveVector); } drivePIDF.updateError(driveError); - driveVector = new Vector(MathFunctions.clamp(drivePIDF.runPIDF() + drivePIDFFeedForward * MathFunctions.getSign(driveError), -1, 1), currentPath.getClosestPointTangentVector().getTheta()); + driveVector = new Vector(MathFunctions.clamp(drivePIDF.runPIDF() + drivePIDFFeedForward * MathFunctions.getSign(driveError), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), currentPath.getClosestPointTangentVector().getTheta()); return MathFunctions.copyVector(driveVector); } @@ -774,11 +762,11 @@ public class Follower { headingError = MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()) * MathFunctions.getSmallestAngleDifference(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()); if (Math.abs(headingError) < headingPIDFSwitch && useSecondaryHeadingPID) { secondaryHeadingPIDF.updateError(headingError); - headingVector = new Vector(MathFunctions.clamp(secondaryHeadingPIDF.runPIDF() + secondaryHeadingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -1, 1), poseUpdater.getPose().getHeading()); + headingVector = new Vector(MathFunctions.clamp(secondaryHeadingPIDF.runPIDF() + secondaryHeadingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), poseUpdater.getPose().getHeading()); return MathFunctions.copyVector(headingVector); } headingPIDF.updateError(headingError); - headingVector = new Vector(MathFunctions.clamp(headingPIDF.runPIDF() + headingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -1, 1), poseUpdater.getPose().getHeading()); + headingVector = new Vector(MathFunctions.clamp(headingPIDF.runPIDF() + headingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), poseUpdater.getPose().getHeading()); return MathFunctions.copyVector(headingVector); } @@ -795,7 +783,7 @@ public class Follower { Vector translational = getTranslationalCorrection(); Vector corrective = MathFunctions.addVectors(centripetal, translational); - if (corrective.getMagnitude() > 1) { + if (corrective.getMagnitude() > driveVectorScaler.getMaxPowerScaling()) { return MathFunctions.addVectors(centripetal, MathFunctions.scalarMultiplyVector(translational, driveVectorScaler.findNormalizingScaling(centripetal, translational))); } @@ -844,7 +832,7 @@ public class Follower { translationalVector = MathFunctions.addVectors(translationalVector, translationalIntegralVector); } - translationalVector.setMagnitude(MathFunctions.clamp(translationalVector.getMagnitude(), 0, 1)); + translationalVector.setMagnitude(MathFunctions.clamp(translationalVector.getMagnitude(), 0, driveVectorScaler.getMaxPowerScaling())); this.translationalVector = MathFunctions.copyVector(translationalVector); @@ -883,7 +871,7 @@ public class Follower { curvature = (yDoublePrime) / (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)); } if (Double.isNaN(curvature)) return new Vector(); - centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -1, 1), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta())); + centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta())); return centripetalVector; } From 45da3275da37a649b50f1bf4dd3a41ef349c6355 Mon Sep 17 00:00:00 2001 From: Anyi Lin Date: Sun, 15 Dec 2024 18:14:56 -0500 Subject: [PATCH 85/94] makes limiting the max power better --- .../follower/DriveVectorScaler.java | 46 +++++++++++++------ .../pedroPathing/follower/Follower.java | 36 ++++----------- 2 files changed, 42 insertions(+), 40 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java index f2cfcfd..a6ed238 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java @@ -15,6 +15,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; public class DriveVectorScaler { // This is ordered left front, left back, right front, right back. These are also normalized. private Vector[] mecanumVectors; + private double maxPowerScaling = 1; /** * This creates a new DriveVectorScaler, which takes in various movement vectors and outputs @@ -52,9 +53,9 @@ public class DriveVectorScaler { */ public double[] getDrivePowers(Vector correctivePower, Vector headingPower, Vector pathingPower, double robotHeading) { // clamps down the magnitudes of the input vectors - if (correctivePower.getMagnitude() > 1) correctivePower.setMagnitude(1); - if (headingPower.getMagnitude() > 1) headingPower.setMagnitude(1); - if (pathingPower.getMagnitude() > 1) pathingPower.setMagnitude(1); + if (correctivePower.getMagnitude() > maxPowerScaling) correctivePower.setMagnitude(maxPowerScaling); + if (headingPower.getMagnitude() > maxPowerScaling) headingPower.setMagnitude(maxPowerScaling); + if (pathingPower.getMagnitude() > maxPowerScaling) pathingPower.setMagnitude(maxPowerScaling); // the powers for the wheel vectors double [] wheelPowers = new double[4]; @@ -65,8 +66,8 @@ public class DriveVectorScaler { // this contains the pathing vectors, one for each side (heading control requires 2) Vector[] truePathingVectors = new Vector[2]; - if (correctivePower.getMagnitude() == 1) { - // checks for corrective power equal to 1 in magnitude. if equal to one, then set pathing power to that + if (correctivePower.getMagnitude() == maxPowerScaling) { + // checks for corrective power equal to max power scaling in magnitude. if equal, then set pathing power to that truePathingVectors[0] = MathFunctions.copyVector(correctivePower); truePathingVectors[1] = MathFunctions.copyVector(correctivePower); } else { @@ -74,7 +75,7 @@ public class DriveVectorScaler { Vector leftSideVector = MathFunctions.subtractVectors(correctivePower, headingPower); Vector rightSideVector = MathFunctions.addVectors(correctivePower, headingPower); - if (leftSideVector.getMagnitude() > 1 || rightSideVector.getMagnitude() > 1) { + if (leftSideVector.getMagnitude() > maxPowerScaling || rightSideVector.getMagnitude() > maxPowerScaling) { //if the combined corrective and heading power is greater than 1, then scale down heading power double headingScalingFactor = Math.min(findNormalizingScaling(correctivePower, headingPower), findNormalizingScaling(correctivePower, MathFunctions.scalarMultiplyVector(headingPower, -1))); truePathingVectors[0] = MathFunctions.subtractVectors(correctivePower, MathFunctions.scalarMultiplyVector(headingPower, headingScalingFactor)); @@ -84,7 +85,7 @@ public class DriveVectorScaler { Vector leftSideVectorWithPathing = MathFunctions.addVectors(leftSideVector, pathingPower); Vector rightSideVectorWithPathing = MathFunctions.addVectors(rightSideVector, pathingPower); - if (leftSideVectorWithPathing.getMagnitude() > 1 || rightSideVectorWithPathing.getMagnitude() > 1) { + if (leftSideVectorWithPathing.getMagnitude() > maxPowerScaling || rightSideVectorWithPathing.getMagnitude() > maxPowerScaling) { // too much power now, so we scale down the pathing vector double pathingScalingFactor = Math.min(findNormalizingScaling(leftSideVector, pathingPower), findNormalizingScaling(rightSideVector, pathingPower)); truePathingVectors[0] = MathFunctions.addVectors(leftSideVector, MathFunctions.scalarMultiplyVector(pathingPower, pathingScalingFactor)); @@ -113,7 +114,7 @@ public class DriveVectorScaler { wheelPowers[3] = (mecanumVectorsCopy[2].getXComponent()*truePathingVectors[1].getYComponent() - truePathingVectors[1].getXComponent()*mecanumVectorsCopy[2].getYComponent()) / (mecanumVectorsCopy[2].getXComponent()*mecanumVectorsCopy[3].getYComponent() - mecanumVectorsCopy[3].getXComponent()*mecanumVectorsCopy[2].getYComponent()); double wheelPowerMax = Math.max(Math.max(Math.abs(wheelPowers[0]), Math.abs(wheelPowers[1])), Math.max(Math.abs(wheelPowers[2]), Math.abs(wheelPowers[3]))); - if (wheelPowerMax > 1) { + if (wheelPowerMax > maxPowerScaling) { wheelPowers[0] /= wheelPowerMax; wheelPowers[1] /= wheelPowerMax; wheelPowers[2] /= wheelPowerMax; @@ -126,12 +127,12 @@ public class DriveVectorScaler { /** * This takes in two Vectors, one static and one variable, and returns the scaling factor that, * when multiplied to the variable Vector, results in magnitude of the sum of the static Vector - * and the scaled variable Vector being 1. + * and the scaled variable Vector being the max power scaling. * * IMPORTANT NOTE: I did not intend for this to be used for anything other than the method above - * this one in this class, so there will be errors if you input Vectors of length greater than 1, + * this one in this class, so there will be errors if you input Vectors of length greater than maxPowerScaling, * and it will scale up the variable Vector if the magnitude of the sum of the two input Vectors - * isn't greater than 1. So, just don't use this elsewhere. There's gotta be a better way to do + * isn't greater than maxPowerScaling. So, just don't use this elsewhere. There's gotta be a better way to do * whatever you're trying to do. * * I know that this is used outside of this class, however, I created this method so I get to @@ -140,13 +141,32 @@ public class DriveVectorScaler { * * @param staticVector the Vector that is held constant. * @param variableVector the Vector getting scaled to make the sum of the input Vectors have a - * magnitude of 1. + * magnitude of maxPowerScaling. * @return returns the scaling factor for the variable Vector. */ public double findNormalizingScaling(Vector staticVector, Vector variableVector) { double a = Math.pow(variableVector.getXComponent(), 2) + Math.pow(variableVector.getYComponent(), 2); double b = staticVector.getXComponent() * variableVector.getXComponent() + staticVector.getYComponent() * variableVector.getYComponent(); - double c = Math.pow(staticVector.getXComponent(), 2) + Math.pow(staticVector.getYComponent(), 2) - 1.0; + double c = Math.pow(staticVector.getXComponent(), 2) + Math.pow(staticVector.getYComponent(), 2) - Math.pow(maxPowerScaling, 2); return (-b + Math.sqrt(Math.pow(b, 2) - a*c))/(a); + + } + + /** + * Sets the maximum power that can be used by the drive vector scaler. Clamped between 0 and 1. + * + * @param maxPowerScaling setting the max power scaling + */ + public void setMaxPowerScaling(double maxPowerScaling) { + this.maxPowerScaling = MathFunctions.clamp(maxPowerScaling, 0, 1); + } + + /** + * Gets the maximum power that can be used by the drive vector scaler. Ranges between 0 and 1. + * + * @return returns the max power scaling + */ + public double getMaxPowerScaling() { + return maxPowerScaling; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index 23885ca..fbac650 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -93,7 +93,6 @@ public class Follower { private boolean holdPositionAtEnd; private boolean teleopDrive; - private double maxPower = 1; private double previousSecondaryTranslationalIntegral; private double previousTranslationalIntegral; private double holdPointTranslationalScaling = FollowerConstants.holdPointTranslationalScaling; @@ -194,18 +193,7 @@ public class Follower { * @param set This caps the motor power from [0, 1]. */ public void setMaxPower(double set) { - maxPower = MathFunctions.clamp(set, 0, 1); - } - - /** - * This handles the limiting of the drive powers array to the max power. - */ - public void limitDrivePowers() { - for (int i = 0; i < drivePowers.length; i++) { - if (Math.abs(drivePowers[i]) > maxPower) { - drivePowers[i] = maxPower * MathFunctions.getSign(drivePowers[i]); - } - } + driveVectorScaler.setMaxPowerScaling(set); } /** @@ -469,8 +457,6 @@ public class Follower { drivePowers = driveVectorScaler.getDrivePowers(MathFunctions.scalarMultiplyVector(getTranslationalCorrection(), holdPointTranslationalScaling), MathFunctions.scalarMultiplyVector(getHeadingVector(), holdPointHeadingScaling), new Vector(), poseUpdater.getPose().getHeading()); - limitDrivePowers(); - for (int i = 0; i < motors.size(); i++) { motors.get(i).setPower(drivePowers[i]); } @@ -482,8 +468,6 @@ public class Follower { drivePowers = driveVectorScaler.getDrivePowers(getCorrectiveVector(), getHeadingVector(), getDriveVector(), poseUpdater.getPose().getHeading()); - limitDrivePowers(); - for (int i = 0; i < motors.size(); i++) { motors.get(i).setPower(drivePowers[i]); } @@ -526,8 +510,6 @@ public class Follower { drivePowers = driveVectorScaler.getDrivePowers(getCentripetalForceCorrection(), teleopHeadingVector, teleopDriveVector, poseUpdater.getPose().getHeading()); - limitDrivePowers(); - for (int i = 0; i < motors.size(); i++) { motors.get(i).setPower(drivePowers[i]); } @@ -693,19 +675,19 @@ public class Follower { public Vector getDriveVector() { if (!useDrive) return new Vector(); if (followingPathChain && chainIndex < currentPathChain.size() - 1) { - return new Vector(1, currentPath.getClosestPointTangentVector().getTheta()); + return new Vector(driveVectorScaler.getMaxPowerScaling(), currentPath.getClosestPointTangentVector().getTheta()); } driveError = getDriveVelocityError(); if (Math.abs(driveError) < drivePIDFSwitch && useSecondaryDrivePID) { secondaryDrivePIDF.updateError(driveError); - driveVector = new Vector(MathFunctions.clamp(secondaryDrivePIDF.runPIDF() + secondaryDrivePIDFFeedForward * MathFunctions.getSign(driveError), -1, 1), currentPath.getClosestPointTangentVector().getTheta()); + driveVector = new Vector(MathFunctions.clamp(secondaryDrivePIDF.runPIDF() + secondaryDrivePIDFFeedForward * MathFunctions.getSign(driveError), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), currentPath.getClosestPointTangentVector().getTheta()); return MathFunctions.copyVector(driveVector); } drivePIDF.updateError(driveError); - driveVector = new Vector(MathFunctions.clamp(drivePIDF.runPIDF() + drivePIDFFeedForward * MathFunctions.getSign(driveError), -1, 1), currentPath.getClosestPointTangentVector().getTheta()); + driveVector = new Vector(MathFunctions.clamp(drivePIDF.runPIDF() + drivePIDFFeedForward * MathFunctions.getSign(driveError), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), currentPath.getClosestPointTangentVector().getTheta()); return MathFunctions.copyVector(driveVector); } @@ -774,11 +756,11 @@ public class Follower { headingError = MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()) * MathFunctions.getSmallestAngleDifference(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()); if (Math.abs(headingError) < headingPIDFSwitch && useSecondaryHeadingPID) { secondaryHeadingPIDF.updateError(headingError); - headingVector = new Vector(MathFunctions.clamp(secondaryHeadingPIDF.runPIDF() + secondaryHeadingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -1, 1), poseUpdater.getPose().getHeading()); + headingVector = new Vector(MathFunctions.clamp(secondaryHeadingPIDF.runPIDF() + secondaryHeadingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), poseUpdater.getPose().getHeading()); return MathFunctions.copyVector(headingVector); } headingPIDF.updateError(headingError); - headingVector = new Vector(MathFunctions.clamp(headingPIDF.runPIDF() + headingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -1, 1), poseUpdater.getPose().getHeading()); + headingVector = new Vector(MathFunctions.clamp(headingPIDF.runPIDF() + headingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), poseUpdater.getPose().getHeading()); return MathFunctions.copyVector(headingVector); } @@ -795,7 +777,7 @@ public class Follower { Vector translational = getTranslationalCorrection(); Vector corrective = MathFunctions.addVectors(centripetal, translational); - if (corrective.getMagnitude() > 1) { + if (corrective.getMagnitude() > driveVectorScaler.getMaxPowerScaling()) { return MathFunctions.addVectors(centripetal, MathFunctions.scalarMultiplyVector(translational, driveVectorScaler.findNormalizingScaling(centripetal, translational))); } @@ -844,7 +826,7 @@ public class Follower { translationalVector = MathFunctions.addVectors(translationalVector, translationalIntegralVector); } - translationalVector.setMagnitude(MathFunctions.clamp(translationalVector.getMagnitude(), 0, 1)); + translationalVector.setMagnitude(MathFunctions.clamp(translationalVector.getMagnitude(), 0, driveVectorScaler.getMaxPowerScaling())); this.translationalVector = MathFunctions.copyVector(translationalVector); @@ -883,7 +865,7 @@ public class Follower { curvature = (yDoublePrime) / (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)); } if (Double.isNaN(curvature)) return new Vector(); - centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -1, 1), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta())); + centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta())); return centripetalVector; } From e4f0447312ff9bd3b5e2885168a61c56254582ce Mon Sep 17 00:00:00 2001 From: Anyi Lin Date: Sun, 15 Dec 2024 18:35:23 -0500 Subject: [PATCH 86/94] changing documentation to match FollowerConstants motor name/direction update --- .../ftc/teamcode/pedroPathing/LOCALIZATION.md | 2 +- .../ftc/teamcode/pedroPathing/TUNING.md | 21 +++++++++++-------- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/LOCALIZATION.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/LOCALIZATION.md index 8343ee9..1d431e3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/LOCALIZATION.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/LOCALIZATION.md @@ -7,7 +7,7 @@ by Tyler Veness. However, the OTOS localizer uses its own onboard system for cal which we do not know about. ## Setting Your Localizer -Go to line `70` in the `PoseUpdater` class, and replace the `new ThreeWheelLocalizer(hardwareMap)` +Go to line `73` in the `PoseUpdater` class, and replace the `new ThreeWheelLocalizer(hardwareMap)` with the localizer that applies to you: * If you're using drive encoders, put `new DriveEncoderLocalizer(hardwareMap)` * If you're using two wheel odometry, put `new TwoWheelLocalizer(hardwareMap)` diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md index 4ae8b4b..cf39512 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md @@ -14,8 +14,11 @@ that the tuners require you to push the robot or the tuners output will say "inc measurements will be in centimeters. ## Tuning -* To start with, we need the mass of the robot in kg. This is used for the centripetal force correction, - and the mass, with the variable name `mass`, should be put on line `86` in the `FollowerConstants` +* First, make sure that your motor names and directions, located at the top of `FollowerConstants`, + are correct. + +* After that, we need the mass of the robot in kg. This is used for the centripetal force correction, + and the mass, with the variable name `mass`, should be put on line `92` in the `FollowerConstants` class under the `tuning` package. * Next, we need to find the preferred mecanum drive vectors. The rollers on mecanum wheels point at a @@ -27,8 +30,8 @@ measurements will be in centimeters. Dashboard under the dropdown for each respective class, but higher distances work better. After the distance has finished running, the end velocity will be output to telemetry. The robot may continue to drift a little bit after the robot has finished running the distance, so make sure you have - plenty of room. Once you're done, put the velocity for the `Forward Velocity Tuner` on line `33` in - the `FollowerConstants` class, and the velocity for the `Strafe Velocity Tuner` on line `34` in the + plenty of room. Once you're done, put the velocity for the `Forward Velocity Tuner` on line `39` in + the `FollowerConstants` class, and the velocity for the `Strafe Velocity Tuner` on line `40` in the `FollowerConstants` class. The variable names should be `xMovement` and `yMovement`, respectively. * The last set of automatic tuners you'll need to run are the zero power acceleration tuners. These @@ -42,8 +45,8 @@ measurements will be in centimeters. which point it will display the deceleration in telemetry. This robot will need to drift to a stop to properly work, and the higher the velocity the greater the drift distance, so make sure you have enough room. Once you're done, put the zero power acceleration for the - `Forward Zero Power Acceleration Tuner` on line `94` in the `FollowerConstants` class and the zero - power acceleration for the `Lateral Zero Power Acceleration Tuner` on line `98` in the + `Forward Zero Power Acceleration Tuner` on line `100` in the `FollowerConstants` class and the zero + power acceleration for the `Lateral Zero Power Acceleration Tuner` on line `104` in the `FollowerConstants` class. The variable names should be `forwardZeroPowerAcceleration` and `lateralZeroPowerAcceleration`, respectively. @@ -78,7 +81,7 @@ measurements will be in centimeters. `zeroPowerAccelerationMultiplier`. This determines how fast your robot will decelerate as a factor of how fast your robot will coast to a stop. Honestly, this is up to you. I personally used 4, but what works best for you is most important. Higher numbers will cause a faster brake, but increase - oscillations at the end. Lower numbers will do the opposite. This can be found on line `107` in + oscillations at the end. Lower numbers will do the opposite. This can be found on line `113` in `FollowerConstants`, named `zeroPowerAccelerationMultiplier`. The drive PID is much, much more sensitive than the others. For reference, my P values were in the hundredths and thousandths place values, and my D values were in the hundred thousandths and millionths place values. To tune this, enable `useDrive`, `useHeading`, and @@ -104,7 +107,7 @@ measurements will be in centimeters. * Finally, we will want to tune the centripetal force correction. This is a pretty simple tune. Open up FTC Dashboard and enable everything under the `Follower` tab. Then, run `CurvedBackAndForth` and turn off its timer. If you notice the robot is correcting towards the inside of the curve - as/after running a path, then increase `centripetalScaling`, which can be found on line `89` of + as/after running a path, then increase `centripetalScaling`, which can be found on line `95` of `FollowerConstants`. If the robot is correcting towards the outside of the curve, then decrease `centripetalScaling`. @@ -117,7 +120,7 @@ measurements will be in centimeters. ## Note About the PIDs In versions of Pedro Pathing before early August 2024, there were 2 PIDs used in the translational, heading, and drive control. However, now there is only one main PID. The old system can still be used. -Scroll down to the bottom of `FollowerConstants` and set all the booleans from lines `157` to `159` +Scroll down to the bottom of `FollowerConstants` and set all the booleans from lines `163` to `165` to true. They should be named `useSecondaryTranslationalPID`, `useSecondaryHeadingPID`, and `useSecondaryDrivePID`. This will enable the two PID system that Pedro Pathing originally used. From there, scroll down and all the values pertaining to the secondary PIDs will be there. The two PID system works with From 38856c90dcc33fdbfc048839e10fbddaa6508c9f Mon Sep 17 00:00:00 2001 From: Havish Sripada <139065303+junkjunk123@users.noreply.github.com> Date: Thu, 19 Dec 2024 12:48:00 -0800 Subject: [PATCH 87/94] Update TwoWheelPinpointIMULocalizer.java --- .../TwoWheelPinpointIMULocalizer.java | 23 ++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java index bdf418e..9b8629b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java @@ -103,6 +103,7 @@ public class TwoWheelPinpointIMULocalizer extends Localizer { deltaTimeNano = 1; displacementPose = new Pose(); currentVelocity = new Pose(); + previousHeading = startPose.getHeading(); deltaRadians = 0; } @@ -293,8 +294,28 @@ public class TwoWheelPinpointIMULocalizer extends Localizer { * This resets the IMU. */ - public void resetIMU() { + @Override + public void resetIMU() throws InterruptedException { + pinpoint.recalibrateIMU(); + + try { + Thread.sleep(300); + } catch (InterruptedException e) { + throw new RuntimeException(e); + } + } + + /** + * This resets the pinpoint. + */ + private void resetPinpoint() { pinpoint.resetPosAndIMU(); + + try { + Thread.sleep(300); + } catch (InterruptedException e) { + throw new RuntimeException(e); + } } } From db79dc38dcc6aed64ad2c97ca2cdeba8bd6e632b Mon Sep 17 00:00:00 2001 From: Havish Sripada <139065303+junkjunk123@users.noreply.github.com> Date: Thu, 19 Dec 2024 12:52:26 -0800 Subject: [PATCH 88/94] Update TwoWheelPinpointIMULocalizer.java --- .../localization/localizers/TwoWheelPinpointIMULocalizer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java index 9b8629b..87849b9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java @@ -308,7 +308,7 @@ public class TwoWheelPinpointIMULocalizer extends Localizer { /** * This resets the pinpoint. */ - private void resetPinpoint() { + private void resetPinpoint() throws InterruptedException{ pinpoint.resetPosAndIMU(); try { From 07e903d14862f1fc8f370b73d29370fc3a0f56a6 Mon Sep 17 00:00:00 2001 From: BaronClaps <126834072+BaronClaps@users.noreply.github.com> Date: Fri, 20 Dec 2024 20:05:23 -0600 Subject: [PATCH 89/94] Update README.md --- README.md | 1572 +---------------------------------------------------- 1 file changed, 6 insertions(+), 1566 deletions(-) diff --git a/README.md b/README.md index f7a02c9..164bcb3 100644 --- a/README.md +++ b/README.md @@ -1,1571 +1,11 @@ -## NOTICE +# Welcome to the Offical Pedro Pathing Quickstart! -This repository contains the public FTC SDK for the INTO THE DEEP (2024-2025) competition season. +To begin, clone this repo and then open it in Android Studio. -## Welcome! -This GitHub repository contains the source code that is used to build an Android app to control a *FIRST* Tech Challenge competition robot. To use this SDK, download/clone the entire project to your local computer. +Then, follow the steps on our [website](https://pedropathing.com/)! -## Requirements -To use this Android Studio project, you will need Android Studio 2021.2 (codename Chipmunk) or later. +For Path Generation, you can use this [path generator](https://discord.gg/2GfC4qBP5s). -To program your robot in Blocks or OnBot Java, you do not need Android Studio. +--- -## Getting Started -If you are new to robotics or new to the *FIRST* Tech Challenge, then you should consider reviewing the [FTC Blocks Tutorial](https://ftc-docs.firstinspires.org/programming_resources/blocks/Blocks-Tutorial.html) to get familiar with how to use the control system: - -      [FTC Blocks Online Tutorial](https://ftc-docs.firstinspires.org/programming_resources/blocks/Blocks-Tutorial.html) - -Even if you are an advanced Java programmer, it is helpful to start with the [FTC Blocks tutorial](https://ftc-docs.firstinspires.org/programming_resources/blocks/Blocks-Tutorial.html), and then migrate to the [OnBot Java Tool](https://ftc-docs.firstinspires.org/programming_resources/onbot_java/OnBot-Java-Tutorial.html) or to [Android Studio](https://ftc-docs.firstinspires.org/programming_resources/android_studio_java/Android-Studio-Tutorial.html) afterwards. - -## Downloading the Project -If you are an Android Studio programmer, there are several ways to download this repo. Note that if you use the Blocks or OnBot Java Tool to program your robot, then you do not need to download this repository. - -* If you are a git user, you can clone the most current version of the repository: - -

            git clone https://github.com/FIRST-Tech-Challenge/FtcRobotController.git

- -* Or, if you prefer, you can use the "Download Zip" button available through the main repository page. Downloading the project as a .ZIP file will keep the size of the download manageable. - -* You can also download the project folder (as a .zip or .tar.gz archive file) from the Downloads subsection of the [Releases](https://github.com/FIRST-Tech-Challenge/FtcRobotController/releases) page for this repository. - -* The Releases page also contains prebuilt APKs. - -Once you have downloaded and uncompressed (if needed) your folder, you can use Android Studio to import the folder ("Import project (Eclipse ADT, Gradle, etc.)"). - -## Getting Help -### User Documentation and Tutorials -*FIRST* maintains online documentation with information and tutorials on how to use the *FIRST* Tech Challenge software and robot control system. You can access this documentation using the following link: - -      [FIRST Tech Challenge Documentation](https://ftc-docs.firstinspires.org/index.html) - -Note that the online documentation is an "evergreen" document that is constantly being updated and edited. It contains the most current information about the *FIRST* Tech Challenge software and control system. - -### Javadoc Reference Material -The Javadoc reference documentation for the FTC SDK is now available online. Click on the following link to view the FTC SDK Javadoc documentation as a live website: - -      [FTC Javadoc Documentation](https://javadoc.io/doc/org.firstinspires.ftc) - -### Online User Forum -For technical questions regarding the Control System or the FTC SDK, please visit the FIRST Tech Challenge Community site: - -      [FIRST Tech Challenge Community](https://ftc-community.firstinspires.org/) - -### Sample OpModes -This project contains a large selection of Sample OpModes (robot code examples) which can be cut and pasted into your /teamcode folder to be used as-is, or modified to suit your team's needs. - -Samples Folder:    [/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples](FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples) - -The readme.md file located in the [/TeamCode/src/main/java/org/firstinspires/ftc/teamcode](TeamCode/src/main/java/org/firstinspires/ftc/teamcode) folder contains an explanation of the sample naming convention, and instructions on how to copy them to your own project space. - -# Release Information - -## Version 10.1 (20240919-122750) - -### Enhancements -* Adds new OpenCV-based `VisionProcessor`s (which may be attached to a VisionPortal in either Java or Blocks) to help teams implement color processing via computer vision in the INTO THE DEEP game - * `ColorBlobLocatorProcessor` implements OpenCV color "blob" detection. A new sample program `ConceptVisionColorLocator` demonstrates its use. - * A choice is offered between pre-defined color ranges, or creating a custom one in RGB, HSV, or YCrCb color space - * The ability is provided to restrict detection to a specified Region of Interest on the screen - * Functions for applying erosion / dilation morphing to the threshold mask are provided - * Functions for sorting and filtering the returned data are provided - * `PredominantColorProcessor` allows using a region of the camera as a "long range color sensor" to determine the predominant color of that region. A new sample program `ConceptVisionColorSensor` demonstrates its use. - * The determined predominant color is selected from a discrete set of color "swatches", similar to the MINDSTORMS NXT color sensor - * Documentation on this Color Processing feature can be found here: https://ftc-docs.firstinspires.org/color-processing -* Added Blocks sample programs for color sensors: RobotAutoDriveToLine and SensorColor. -* Updated Self-Inspect to identify mismatched RC/DS software versions as a "caution" rather than a "failure." - -### Bug Fixes -* Fixes [AngularVelocity conversion regression](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/1070) - -## Version 10.0 (20240828-111152) - -### Breaking Changes -* Java classes and Blocks for TensorFlow Object Detection have been removed. -* `AngularVelocity.unit` which was of type `AngleUnit` has been renamed `AngularVelocity.angleUnit` of type `UnnormalizedAngleUnit` - -### Enhancements -* Sample for REV Digital Indicator has been added - ConceptRevLED -* Adds support for the [Sparkfun QWIIC LED Stick](https://www.sparkfun.com/products/18354) - * To connect it directly, you need this [cable](https://www.sparkfun.com/products/25596) -* Adds ConceptLEDStick OpMode -* Adds Blocks for colors black, blue, cyan, dkgray, gray, green, ltgray, magenta, red, white, and yellow. -* Adds an "evaluate but ignore result" Block that executes the connected block and ignores the result. Allows you to call a function and ignore the return value. -* Adds I2C driver for Maxbotix Maxsonar I2CXL sonar rangefinder -* Adds Blocks for setPwmEnable, setPwmDisable, and isPwmEnabled for servos and CR servos. -* In the Blocks editor: a \n in the ExportToBlocks annotation's comment field is displayed as a line break. -* Telemetry has new method setNumDecimalPlaces -* Telemetry now formats doubles and floats (not inside objects, just by themselves) -* Adds support for the Limelight 3A. -* Adds initial support for the REV Servo Hub - * Both the Robot Controller and Driver Station need to be updated to version 10.0 in order for Servo Hubs to be - configurable as Servo Hubs. If the app on either device is outdated, the Servo Hub will show up as an Expansion Hub, - and some functionality will not work as expected. You should wait to create a configuration that includes a Servo Hub - until both the Driver Station and Robot Controller apps have been updated to version 10.0. - * Updating the Servo Hub's firmware and changing its address can only be done using the REV Hardware Client at this time -* Adds support for the REV 9-Axis IMU (REV-31-3332) - * The REV 9-Axis IMU is only supported by the [Universal IMU interface](https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html) - * Adds `Rev9AxisImuOrientationOnRobot` Java class. - * If you mentally substitute this IMU's I2C port for the Control Hub's USB ports, `RevHubOrientationOnRobot` is also compatible with this sensor - * Adds Blocks for Rev9AxisImuOrientationOnRobot, including RevHubImuOrientationOnRobot.xyzOrientation and RevHubImuOrientationOnRobot.zyxOrientation. - * Adds Blocks samples SensorRev9AxisIMUOrthogonal and SensorRev9AxisIMUNonOrthogonal. -* Improves Blocks support for RevHubImuOrientationOnRobot. - * Adds Blocks for RevHubImuOrientationOnRobot.xyzOrientation and RevHubImuOrientationOnRobot.zyxOrientation. - * Adds Blocks samples SensorHubIMUOrthogonal (replaces SensorIMU) and SensorHubIMUNonOrthogonal. -* Updates EasyOpenCV, AprilTag, OpenCV, and `libjpeg-turbo` versions -* Adds Blocks for max and min that take two numbers. -* Adds Blocks OpModes ConceptRevSPARKMini, RobotAutoDriveByEncoder, RobotAutoDriveByGyro, RobotAutoDriveByTime, RobotAutoDriveToAprilTagOmni, and RobotAutoDriveToAprilTagTank. -* Two OpModes with the same name now automatically get renamed with the name followed by a "-" and the class name allowing them to both be on the device. -* Shows the name of the active configuration on the Manage page of the Robot Controller Console -* Updated AprilTag Library for INTO THE DEEP. Notably, `getCurrentGameTagLibrary()` now returns INTO THE DEEP tags. -* Adds Blocks for Telemetry.setMsTransmissionInterval and Telemetry.getMsTransmissionInterval. -* Adds Blocks sample SensorOctoQuad. - -### Bug Fixes -* Fixes a bug where the RevBlinkinLedDriver Blocks were under Actuators in the Blocks editor toolbox. They are now Other Devices. -* Fixes a bug where `Exception`s thrown in user code after a stop was requested by the Driver Station would be silently eaten -* Fixed a bug where if you asked for `AngularVelocity` in a unit different than the device reported it in, it would normalize it between -PI and PI for radians, and -180 and 180 for degrees. - -## Version 9.2 (20240701-085519) - -### Important Notes -* Java classes and Blocks for TensorFlow Object Detection have been deprecated and will be removed in Version 10.0. -* The samples that use TensorFlow Object Detection have been removed. - -### Enhancements -* Adds explanatory text to failed items on the inspection activities. To view the explanatory text tap the red warning icon for a failed item. -* In the Blocks editor: added a new kind of variable set block that sets the variable and also returns the new value. -* Changes the way that camera controls behave for a SwitchableCamera. Now, each method (such as getExposure, getMinExposure, getMaxExposure, setExposure for ExposureControl) acts on the currently active camera. -* Adds support for the REV USB PS4 Compatible Gamepad (REV-31-2983) -* Adds ConceptAprilTagMultiPortal OpMode -* Adds support for OctoQuad Quadrature Encoder & Pulse Width Interface Module -* Adds the ExportAprilTagLibraryToBlocks annotation that indicates that a static method that returns an AprilTagLibrary is exported to the Blocks programming environment. The corresponding block will appear in the Blocks toolbox along with the built-in tag libraries. -* Adds Blocks OpMode ConceptAprilTagOptimizeExposure. -* Adds support for the SparkFun Optical Tracking Odometry sensor. - -### Bug Fixes -* Fixes https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/942 where visionPortal.close() can cause an IndexOutOfBoundsError. -* Fixes a bug in the blocks editor where collapsed function blocks show a warning "Collapsed blocks contain warnings." when the Blocks OpMode is reopened. -* Fixes a bug where the blocks editor wouldn't warn you that you have unsaved changes when you try to leave. This bug was introduced due to a behavior change in Chrome 119. -* [Issue #764](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/764) - Get gain control returns a null pointer for a switchable camera -* Fixes a bug where the correct deadzone for certain gamepads was not applied when Advanced Gamepad Features was enabled - -## Version 9.1 (20240215-115542) - -### Enhancements -* Fixes a problem with Blocks: if the user closes a Block's warning balloon, it will still be closed next time the project is opened in the Blocks editor. -* In the Blocks editor, an alert concerning missing hardware devices is not shown if all the Blocks that use the missing hardware devices are disabled. -* Adds Blocks to support comparing property values CRServo.Direction, DCMotor.Direction, DCMotor.Mode, DCMotor.ZeroPowerBehavior, DigitalChannel.Mode, GyroSensor.HeadingMode, IrSeekerSensor.Mode, and Servo.Direction, to the corresponding enum Block. -* Improves OnBotJava auto-import to correctly import classes when used in certain situations. -* Improves OnBotJava autocomplete to provide better completion options in most cases. - * This fixes an issue where autocomplete would fail if a method with two or more formal parameters was defined. -* In OnBotJava, code folding support was added to expand and collapse code sections -* In OnBotJava, the copyright header is now automatically collapsed loading new files -* For all Blocks OpMode samples, intro comments have been moved to the RunOpMode comment balloon. -* The Clean up Blocks command in the Blocks editor now positions function Blocks so their comment balloons don't overlap other function Blocks. -* Added Blocks OpMode sample SensorTouch. -* Added Java OpMode sample SensorDigitalTouch. -* Several improvements to VisionPortal - * Adds option to control whether the stream is automatically started following a `.build()` call on a VisionPortal Builder - * Adds option to control whether the vision processing statistics overlay is rendered or not - * VisionPortals now implement the `CameraStreamSource` interface, allowing multiportal users to select which portal is routed to the DS in INIT by calling CameraStreamServer.getInstance().setSource(visionPortal). Can be selected via gamepad, between Camera Stream sessions. - * Add option to `AprilTagProcessor` to suppress calibration warnings - * Improves camera calibration warnings - * If a calibration is scaled, the resolution it was scaled from will be listed - * If calibrations exist with the wrong aspect ratio, the calibrated resolutions will be listed - * Fixes race condition which caused app crash when calling `stopStreaming()` immediately followed by `close()` on a VisionPortal - * Fixes IllegalStateException when calling `stopStreaming()` immediately after building a VisionPortal - * Added FTC Blocks counterparts to new Java methods: - * VisionPortal.Builder.setAutoStartStreamOnBuild - * VisionPortal.Builder.setShowStatsOverlay - * AprilTagProcessor.Builder.setSuppressCalibrationWarnings - * CameraStreamServer.setSource​ - -### Bug Fixes -* Fixes a problem where OnBotJava does not apply font size settings to the editor. -* Updates EasyOpenCV dependency to v1.7.1 - * Fixes inability to use EasyOpenCV CameraFactory in OnBotJava - * Fixes entire RC app crash when user pipeline throws an exception - * Fixes entire RC app crash when user user canvas annotator throws an exception - * Use the modern stacktrace display when handling user exceptions instead of the legacy ESTOP telemetry message - -## Version 9.0.1 (20230929-083754) - -### Enhancements -* Updates AprilTag samples to include Decimation and additional Comments. Also corrects misleading tag ID warnings -* Increases maximum size of Blocks inline comments to 140 characters -* Adds Blocks sample BasicOmniOpMode. -* Updated CENTERSTAGE library AprilTag orientation quaternions - * Thanks [@FromenActual](https://github.com/FromenActual) -* Updated Java Sample ConceptTensorFlowObjectDetection.java to include missing elements needed for custom model support. - -### Bug Fixes -* Fixes a problem where after October 1 the Driver Station will report as obsolete on v9.0 and prompt the user to update. - -## Version 9.0 (20230830-154348) - -### Breaking Changes -* Removes Vuforia -* Fields in `AprilTagDetection` and `AprilTagPose(ftc/raw)` objects are now `final` -* VisionPortal builder method `setCameraMonitorViewId()` has been renamed to `setLiveViewContainerId()` and `enableCameraMonitoring()` has been renamed to `enableLiveView()` - -### Enhancements -* Adds support for the DFRobot HuskyLens Vision Sensor. -* Blocks teams can now perform webcam calibration. - * Added a Block for System.currentTimeMillis (under Utilities/Time) - * Added a Block for VisionPortal.saveNextFrameRaw (under Vision/VisionPortal) - * Added a new sample Blocks OpMode called UtilityCameraFrameCapture. -* The RobotDriveByGyro sample has been updated to use the new universal IMU interface. It now supports both IMU types. -* Removed some error-prone ElapsedTime Blocks from the Blocks editor's toolbox. This is not a - breaking change: old Blocks OpModes that use these Blocks will still function, both in the - Blocks editor and at runtime. -* Standardizes on the form "OpMode" for the term OpMode. - * The preferred way to refer to OpModes that specifically extend `LinearOpMode` (including Blocks OpModes) is "linear OpMode". - * The preferred way to refer to OpModes that specifically extend `OpMode` directly is "iterative OpMode". -* Overhauls `OpMode` and `LinearOpMode` Javadoc comments to be easier to read and include more detail. -* Makes minor enhancements to Java samples - * Javadoc comments in samples that could be rendered badly in Android Studio have been converted to standard multi-line comments - * Consistency between samples has been improved - * The SensorDigitalTouch sample has been replaced with a new SensorTouch sample that uses the `TouchSensor` interface instead of `DigitalChannel`. - * The ConceptCompassCalibration, SensorMRCompass, and SensorMRIRSeeker samples have been deleted, as they are not useful for modern FTC competitions. - -### Bug Fixes -* Fixes a bug which prevented PlayStation gamepads from being used in bluetooth mode. Bluetooth is NOT legal for competition but may be useful to allow a DS device to be used while charging, or at an outreach event. -* Fixes a bug where a Blocks OpMode's Date Modified value can change to December 31, 1969, if the Control Hub is rebooted while the Blocks OpMode is being edited. -* Fixes the automatic TeleOp preselection feature (was broken in 8.2) -* Fixes a bug where passing an integer number such as 123 to the Telemetry.addData block that takes a number shows up as 123.0 in the telemetry. -* Fixes OnBotJava autocomplete issues: - * Autocomplete would incorrectly provide values for the current class when autocompleting a local variable - * `hardwareMap` autocomplete would incorrectly include lambda class entries -* Fixes OnBotJava not automatically importing classes. -* Fixes OnBotJava tabs failing to close when their file is deleted. -* Fixes a project view refresh not happening when a file is renamed in OnBotJava. -* Fixes the "Download" context menu item for external libraries in the OnBotJava interface. -* Fixes issue where Driver Station telemetry would intermittently freeze when set to Monospace mode. -* Fixes performance regression for certain REV Hub operations that was introduced in version 8.2. -* Fixes TagID comparison logic in DriveToTag samples. - -## Version 8.2 (20230707-131020) - -### Breaking Changes -* Non-linear (iterative) OpModes are no longer allowed to manipulate actuators in their `stop()` method. Attempts to do so will be ignored and logged. - * When an OpMode attempts to illegally manipulate an actuator, the Robot Controller will print a log message - including the text `CANCELLED_FOR_SAFETY`. - * Additionally, LinearOpModes are no longer able to regain the ability to manipulate actuators by removing their - thread's interrupt or using another thread. -* Removes support for Android version 6.0 (Marshmallow). The minSdkVersion is now 24. -* Increases the Robocol version. - * This means an 8.2 or later Robot Controller or Driver Station will not be able to communicate with an 8.1 or earlier Driver Station or Robot Controller. - * If you forget to update both apps at the same time, an error message will be shown explaining which app is older and should be updated. -* FTC_FieldCoordinateSystemDefinition.pdf has been moved. It is still in the git history, but has been removed from the git snapshot corresponding with the 8.2 tag. The official version now lives at [Field Coordinate System](https://ftc-docs.firstinspires.org/field-coordinate-system). -* `LynxUsbDevice.addConfiguredModule()` and `LynxUsbDevice.getConfiguredModule()` have been replaced with `LynxUsbDevice.getOrAddModule()`. -* Old Blocks for Vuforia and TensorFlow Object Detection are obsolete and have been removed from the - Blocks editor's toolbox. Existing Blocks OpModes that contain the old Blocks for Vuforia or - TensorFlow Object Detection can be opened in the Blocks editor, but running them will not work. - -### New features -* Adds new `VisionPortal` API for computer vision - * **This API may be subject to change for final kickoff release!** - * Several new samples added. - * Adds support for detecting AprilTags. - * `VisionPortal` is the new entry point for both AprilTag and TFOD processing. - * Vuforia will be removed in a future release. - * Updated TensorFlow dependencies. - * Added support for webcam camera controls to blocks. - * The Blocks editor's toolbox now has a Vision category, directly above the Utilities category. -* Related documentation for associated technologies can be found at - * [AprilTag Introduction](https://ftc-docs.firstinspires.org/apriltag-intro) - * [AprilTag SDK Guide](https://ftc-docs.firstinspires.org/apriltag-sdk) - * [AprilTag Detection Values](https://ftc-docs.firstinspires.org/apriltag-detection-values) - * [AprilTag Test Images](https://ftc-docs.firstinspires.org/apriltag-test-images) - * [Camera Calibration](https://ftc-docs.firstinspires.org/camera-calibration) -* Adds Driver Station support for Logitech Dual Action and Sony PS5 DualSense gamepads. - * This **does not** include support for the Sony PS5 DualSense Edge gamepad. - * Always refer to Game Manual 1 to determine gamepad legality in competition. -* Adds support for MJPEG payload streaming to UVC driver (external JPEG decompression routine required for use). -* Shows a hint on the Driver Station UI about how to bind a gamepad when buttons are pressed or the sticks are moved on an unbound gamepad. -* Adds option for fullscreening "Camera Stream" on Driver Station. -* OnBotJava source code is automatically saved as a ZIP file on every build with a rolling window of the last 30 builds kept; allows recovering source code from previous builds if code is accidentally deleted or corrupted. -* Adds support for changing the addresses of Expansion Hubs that are not connected directly via USB. - * The Expansion Hub Address Change screen now has an Apply button that changes the addresses without leaving the screen. - * Addresses that are assigned to other hubs connected to the same USB connection or Control Hub are no longer able to be selected. -* Increases maximum size of Blocks inline comments to 100 characters -* Saves position of open Blocks comment balloons -* Adds new AprilTag Driving samples: RobotDriveToAprilTagTank & RobotDriveToAprilTagOmni -* Adds Sample to illustrate optimizing camera exposure for AprilTags: ConceptAprilTagOptimizeExposure - -### Bug Fixes -* Corrects inspection screen to report app version using the SDK version defined in the libraries instead of the version specified in `AndroidManifest.xml`. This corrects the case where the app could show matching versions numbers to the user but still state that the versions did not match. - * If the version specified in `AndroidManifest.xml` does not match the SDK version, an SDK version entry will be displayed on the Manage webpage. -* Fixes no error being displayed when saving a configuration file with duplicate names from the Driver Station. -* Fixes a deadlock in the UVC driver which manifested in https://github.com/OpenFTC/EasyOpenCV/issues/57. -* Fixes a deadlock in the UVC driver that could occur when hot-plugging cameras. -* Fixes UVC driver compatibility with Arducam OV9281 global shutter camera. -* Fixes Emergency Stop condition when an OnBotJava build with duplicate OpMode names occurs. -* Fixes known causes of "Attempted use of a closed LynxModule instance" logspam. -* Fixes the visual identification LED pattern when configuring Expansion Hubs connected via RS-485. - -## Version 8.1.1 (20221201-150726) - -This is a bug fix only release to address the following four issues. - -* [Issue #492](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/492) - Can't create new blocks opmodes. -* [Issue #495](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/495) - Remove the final modifier from the OpMode's Telemetry object. -* [Issue #500](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/500) - Some devices cannot be configured when the Driver Station app has been updated to 8.1 - * Updating either the Robot Controller app or the Driver Station app to 8.1.1 or later will fix this issue. -* The Modern Robotics touch sensor was configurable as a Digital Device. It can only be used as an Analog Device. - -## Version 8.1 (20221121-115119) - -### Breaking Changes -* Deprecates the `OpMode` fields `msStuckDetectInit`, `msStuckDetectInitLoop`, `msStuckDetectStart`, `msStuckDetectLoop`, and `msStuckDetectStop`. - * OpModes no longer have a time limit for `init()`, `init_loop()`, `start()` or `loop()`, so the fields corresponding to those methods are no longer used. - * `stop()` still has a time limit, but it is now hardcoded to be 1 second, and cannot be changed using `msStuckDetectStop`. -* Deprecates the `OpMode` methods `internalPreInit()`, `internalPostInitLoop()`, and `internalPostLoop()`. - * Iterative `OpMode`s will continue to call these methods in case they were overridden. - * These methods will not be called at all for `LinearOpMode`s. -* Deprecates (and stops respecting) `DeviceProperties.xmlTagAliases`. - -### Enhancements -* Adds a new `IMU` interface to Blocks and Java that can be used with both the original BNO055 IMU - included in all older Control Hubs and Expansion Hubs, and the new alternative BHI260AP IMU. - * You can determine which type of IMU is in your Control Hub by navigating to the Manage page of the web interface. - * To learn how to use the new `IMU` interface, see https://ftc-docs.firstinspires.org/programming_resources/imu/imu.html. The `SensorIMU` Blocks sample was also updated to use the new `IMU` interface, and the following Java samples were added: - * `SensorIMUOrthogonal` - * Use this sample if your REV Hub is mounted so that it is parallel or perpendicular to the - bottom of your robot. - * `SensorIMUNonOrthogonal` - * Use this sample if your REV Hub is mounted to your robot in any other orientation - * `ConceptExploringIMUOrientations` - * This OpMode is a tool to help you understand how the orthogonal orientations work, and - which one applies to your robot. - * The BHI260AP IMU can only be accessed via the new `IMU` interface. The BNO055 IMU can be - programmed using the new `IMU` interface, or you can continue to program it using the old `BNO055IMU` - interface. If you want to be able to quickly switch to a new Control Hub that may contain the - BHI260AP IMU, you should migrate your code to use the new `IMU` interface. - * Unlike the old `BNO055IMU` interface, which only worked correctly when the REV Hub was mounted flat - on your robot, the `IMU` interface allows you to specify the orientation of the REV Hub on your - robot. It will account for this, and give you your orientation in a Robot Coordinate System, - instead of a special coordinate system for the REV Hub. As a result, your pitch and yaw will be - 0 when your *robot* is level, instead of when the REV Hub is level, which will result in much - more reliable orientation angle values for most mounting orientations. - * Because of the new robot-centric coordinate system, the pitch and roll angles returned by the - `IMU` interface will be different from the ones returned by the `BNO055IMU` interface. When you are - migrating your code, pay careful attention to the documentation. - * If you have calibrated your BNO055, you can provide that calibration data to the new `IMU` - interface by passing a `BNO055IMUNew.Parameters` instance to `IMU.initialize()`. - * The `IMU` interface is also suitable for implementation by third-party vendors for IMUs that - support providing the orientation in the form of a quaternion. -* Iterative `OpMode`s (as opposed to `LinearOpMode`s) now run on a dedicated thread. - * Cycle times should not be as impacted by everything else going on in the system. - * Slow `OpMode`s can no longer increase the amount of time it takes to process network commands, and vice versa. - * The `init()`, `init_loop()`, `start()` and `loop()` methods no longer need to return within a certain time frame. -* BNO055 IMU legacy driver: restores the ability to initialize in one OpMode, and then have another OpMode re-use that - initialization. This allows you to maintain the 0-yaw position between OpModes, if desired. -* Allows customized versions of device drivers in the FTC SDK to use the same XML tag. - * Before, if you wanted to customize a device driver, you had to copy it to a new class _and_ give - it a new XML tag. Giving it a new XML tag meant that to switch which driver was being used, you - had to modify your configuration file. - * Now, to use your custom driver, all you have to do is specify your custom driver's class when - calling `hardwareMap.get()`. To go back to the original driver, specify the original driver - class. If you specify an interface that is implemented by both the original driver and the - custom driver, there is no guarantee about which implementation will be returned. - -### Bug Fixes -* Fixes accessing the "Manage TensorFlow Lite Models" and "Manage Sounds" links and performing - Blocks and OnBotJava OpMode downloads from the REV Hardware Client. -* Fixes issue where an I2C device driver would be auto-initialized using the parameters assigned in - a previous OpMode run. -* Improves Driver Station popup menu placement in the landscape layout. -* Fixes NullPointerException when attempting to get a non-configured BNO055 IMU in a Blocks OpMode on an RC phone. -* Fixes problem with Blocks if a variable is named `orientation`. - -## Version 8.0 (20220907-131644) - -### Breaking Changes -* Increases the Robocol version. - * This means an 8.0 or later Robot Controller or Driver Station will not be able to communicate with a 7.2 or earlier Driver Station or Robot Controller. - * If you forget to update both apps at the same time, an error message will be shown explaining which app is older and should be updated. -* Initializing I2C devices now happens when you retrieve them from the `HardwareMap` for the first time. - * Previously, all I2C devices would be initialized before the OpMode even began executing, - whether you were actually going to use them or not. This could result in reduced performance and - unnecessary warnings. - * With this change, it is very important for Java users to retrieve all needed devices from the - `HardwareMap` **during the Init phase of the OpMode**. Namely, declare a variable for each hardware - device the OpMode will use, and assign a value to each. Do not do this during the Run phase, or your - OpMode may briefly hang while the devices you are retrieving get initialized. - * OpModes that do not use all of the I2C devices specified in the configuration file should take - less time to initialize. OpModes that do use all of the specified I2C devices should take the - same amount of time as previously. -* Fixes [issue #251](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/251) by changing the order in which axis rotation rates are read from the angular velocity vector in the BNO055 IMU driver. -* Deprecates `pitchMode` in `BNO055IMU.Parameters`. - * Setting `pitchMode` to `PitchMode.WINDOWS` would break the coordinate conventions used by the driver. -* Moves `OpModeManagerImpl` to the `com.qualcomm.robotcore.eventloop.opmode` package. - * This breaks third party libraries EasyOpenCV (version 1.5.1 and earlier) and FTC Dashboard (version 0.4.4 and earlier). -* Deletes the deprecated `OpMode` method `resetStartTime()` (use `resetRuntime()` instead). -* Deletes the protected `LinearOpMode.LinearOpModeHelper` class (which was not meant for use by OpModes). -* Removes I2C Device (Synchronous) config type (deprecated since 2018) - -### Enhancements -* Uncaught exceptions in OpModes no longer require a Restart Robot - * A blue screen popping up with a stacktrace is not an SDK error; this replaces the red text in the telemetry area. - * Since the very first SDK release, OpMode crashes have put the robot into "EMERGENCY STOP" state, only showing the first line of the exception, and requiring the user to press "Restart Robot" to continue - * Exceptions during an OpMode now open a popup window with the same color scheme as the log viewer, containing 15 lines of the exception stacktrace to allow easily tracing down the offending line without needing to connect to view logs over ADB or scroll through large amounts of logs in the log viewer. - * The exception text in the popup window is both zoomable and scrollable just like a webpage. - * Pressing the "OK" button in the popup window will return to the main screen of the Driver Station and allow an OpMode to be run again immediately, without the need to perform a "Restart Robot" -* Adds new Java sample to demonstrate using a hardware class to abstract robot actuators, and share them across multiple OpModes. - * Sample OpMode is [ConceptExternalHardwareClass.java](FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExternalHardwareClass.java) - * Abstracted hardware class is [RobotHardware.java](FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotHardware.java) -* Updates RobotAutoDriveByGyro_Linear Java sample to use REV Control/Expansion hub IMU. -* Updates Vuforia samples to reference PowerPlay assets and have correct names and field locations of image targets. -* Updates TensorFlow samples to reference PowerPlay assets. -* Adds opt-in support for Java 8 language features to the OnBotJava editor. - * To opt in, open the OnBotJava Settings, and check `Enable beta Java 8 support`. - * Note that Java 8 code will only compile when the Robot Controller runs Android 7.0 Nougat or later. - * Please report issues [here](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues). -* In OnBotJava, clicking on build errors now correctly jumps to the correct location. -* Improves OnBotJava autocomplete behavior, to provide better completion options in most cases. -* Adds a QR code to the Robot Controller Inspection Report when viewed from the Driver Station for scanning by inspectors at competition. -* Improves I2C performance and reliability in some scenarios. - -## Version 7.2 (20220723-130006) - -### Breaking Changes -* Updates the build tooling. For Android Studio users, this change requires Android Studio Chipmunk 2021.2.1. -* Removes support for devices that are not competition legal, including Modern Robotics Core Control Modules, the Matrix Controller, and HiTechnic/NXT controllers and sensors. Support remains for Modern Robotics I2C sensors. - -### Enhancements -* Increases the height of the 3-dots Landscape menu touch area on the Driver Station, making it much easier to select. -* Adds `terminateOpModeNow()` method to allow OpModes to cleanly self-exit immediately. -* Adds `opModeInInit()` method to `LinearOpMode` to facilitate init-loops. Similar to `opModeIsActive()` but for the init phase. -* Warns user if they have a Logitech F310 gamepad connected that is set to DirectInput mode. -* Allows SPARKmini motor controllers to react more quickly to speed changes. -* Hides the version number of incorrectly installed sister app (i.e. DS installed on RC device or vice-versa) on inspection screen. -* Adds support for allowing the user to edit the comment for the runOpMode block. -* Adds parameterDefaultValues field to @ExportToBlocks. This provides the ability for a java method with an @ExportToBlocks annotation to specify default values for method parameters when it is shown in the block editor. -* Make LinearOpMode blocks more readable. The opmode name is displayed on the runOpMode block, but not on the other LinearOpMode blocks. -* Added support to TensorFlow Object Detection for using a different frame generator, instead of Vuforia. - Using Vuforia to pass the camera frame to TFOD is still supported. -* Removes usage of Renderscript. -* Fixes logspam on app startup of repeated stacktraces relating to `"Failed resolution of: Landroid/net/wifi/p2p/WifiP2pManager$DeviceInfoListener"` -* Allows disabling bluetooth radio from inspection screen -* Improves warning messages when I2C devices are not responding -* Adds support for controlling the RGB LED present on PS4/Etpark gamepads from OpModes -* Removes legacy Pushbot references from OpMode samples. Renames "Pushbot" samples to "Robot". Motor directions reversed to be compatible with "direct Drive" drive train. - - -### Bug fixes -* Fixes [issue #316](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/316) (MatrixF.inverted() returned an incorrectly-sized matrix for 1x1 and 2x2 matrixes). -* Self inspect now allows for Driver Station and Robot Controller compatibility between point releases. -* Fixes bug where if the same `RumbleEffect` object instance was queued for multiple gamepads, it - could happen that both rumble commands would be sent to just one gamepad. -* Fixes bug in Driver Station where on the Driver Hub, if Advanced Gamepad Features was disabled and - an officially supported gamepad was connected, then opening the Advanced Gamepad Features or - Gamepad Type Overrides screens would cause the gamepad to be rebound by the custom USB driver even - though advanced gamepad features was disabled. -* Protects against (unlikely) null pointer exception in Vuforia Localizer. -* Harden OnBotJava and Blocks saves to protect against save issues when disconnecting from Program and Manage -* Fixes issue where the RC app would hang if a REV Hub I2C write failed because the previous I2C - operation was still in progress. This hang most commonly occurred during REV 2M Distance Sensor initialization -* Removes ConceptWebcam.java sample program. This sample is not compatible with OnBotJava. -* Fixes bug where using html tags in an @ExportToBlocks comment field prevented the blocks editor from loading. -* Fixes blocks editor so it doesn't ask you to save when you haven't modified anything. -* Fixes uploading a very large blocks project to offline blocks editor. -* Fixes bug that caused blocks for DcMotorEx to be omitted from the blocks editor toolbox. -* Fixes [Blocks Programs Stripped of Blocks (due to using TensorFlow Label block)](https://ftcforum.firstinspires.org/forum/ftc-technology/blocks-programming/87035-blocks-programs-stripped-of-blocks) - -## Version 7.1 (20211223-120805) - -* Fixes crash when calling `isPwmEnabled()` ([issue #223](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/233)). -* Fixes lint error ([issue #4](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/4)). -* Fixes Driver Station crash when attempting to use DualShock4 v1 gamepad with Advanced Gamepad Features enabled ([issue #173](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/173)). -* Fixes possible (but unlikely) Driver Station crash when connecting gamepads of any type. -* Fixes bug where Driver Station would use generic 20% deadzone for Xbox360 and Logitech F310 gamepads when Advanced Gamepad Features was disabled. -* Added SimpleOmniDrive sample OpMode. -* Adds UVC white balance control API. -* Fixes [issue #259](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/259) Most blocks samples for TensorFlow can't be used for a different model. - * The blocks previously labeled TensorFlowObjectDetectionFreightFrenzy (from the subcategory named "Optimized for Freight Frenzy") and TensorFlowObjectDetectionCustomModel (from the subcategory named "Custom Model") have been replaced with blocks labeled TensorFlowObjectDetection. Blocks in existing opmodes will be automatically updated to the new blocks when opened in the blocks editor. -* Fixes [issue #260](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/260) Blocks can't call java method that has a VuforiaLocalizer parameter. - * Blocks now has a block labeled VuforiaFreightFrenzy.getVuforiaLocalizer for this. -* Added a page to manage the TensorFlow Lite models in /sdcard/FIRST/tflitemodels. To get to the TFLite Models page: - * You can click on the link at the bottom of the Manage page. - * You can click on the link at the upper-right the Blocks project page. -* Fixes logspam when `isBusy()` is called on a motor not in RTP mode. -* Hides the "RC Password" item on the inspection screen for phone-based Robot Controllers. (It is only applicable for Control Hubs). -* Adds channel 165 to Wi-Fi Direct channel selection menu in the settings screen. (165 was previously available through the web UI, but not locally in the app). - -## Version 7.0 (20210915-141025) - -### Enhancements and New Features -* Adds support for external libraries to OnBotJava and Blocks. - * Upload .jar and .aar files in OnBotJava. - * Known limitation - RobotController device must be running Android 7.0 or greater. - * Known limitation - .aar files with assets are not supported. - * External libraries can provide support for hardware devices by using the annotation in the - com.qualcomm.robotcore.hardware.configuration.annotations package. - * External libraries can include .so files for native code. - * External libraries can be used from OnBotJava OpModes. - * External libraries that use the following annotations can be used from Blocks OpModes. - * org.firstinspires.ftc.robotcore.external.ExportClassToBlocks - * org.firstinspires.ftc.robotcore.external.ExportToBlocks - * External libraries that use the following annotations can add new hardware devices: - * com.qualcomm.robotcore.hardware.configuration.annotations.AnalogSensorType - * com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties - * com.qualcomm.robotcore.hardware.configuration.annotations.DigitalIoDeviceType - * com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType - * com.qualcomm.robotcore.hardware.configuration.annotations.MotorType - * com.qualcomm.robotcore.hardware.configuration.annotations.ServoType - * External libraries that use the following annotations can add new functionality to the Robot Controller: - * org.firstinspires.ftc.ftccommon.external.OnCreate - * org.firstinspires.ftc.ftccommon.external.OnCreateEventLoop - * org.firstinspires.ftc.ftccommon.external.OnCreateMenu - * org.firstinspires.ftc.ftccommon.external.OnDestroy - * org.firstinspires.ftc.ftccommon.external.WebHandlerRegistrar -* Adds support for REV Robotics Driver Hub. -* Adds fully custom userspace USB gamepad driver to Driver Station (see "Advanced Gamepad Features" menu in DS settings). - * Allows gamepads to work on devices without native Linux kernel support (e.g. some Romanian Motorola devices). - * Allows the DS to read the unique serial number of each gamepad, enabling auto-recovery of dropped gamepads even if two gamepads of the same model drop. *(NOTE: unfortunately this does not apply to Etpark gamepads, because they do not have a unique serial)*. - * Reading the unique serial number also provides the ability to configure the DS to assign gamepads to a certain position by default (so no need to do start+a/b at all). - * The LED ring on the Xbox360 gamepad and the RGB LED bar on the PS4 gamepad is used to indicate the driver position the gamepad is bound to. - * The rumble motors on the Xbox360, PS4, and Etpark gamepads can be controlled from OpModes. - * The 2-point touchpad on the PS4 gamepad can be read from OpModes. - * The "back" and "guide" buttons on the gamepad can now be safely bound to robot controls (Previously, on many devices, Android would intercept these buttons as home button presses and close the app). - * Advanced Gamepad features are enabled by default, but may be disabled through the settings menu in order to revert to gamepad support provided natively by Android. -* Improves accuracy of ping measurement. - * Fixes issue where the ping time showed as being higher than reality when initially connecting to or restarting the robot. - * To see the full improvement, you must update both the Robot Controller and Driver Station apps. -* Updates samples located at [/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples](FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples). - * Added ConceptGamepadRumble and ConceptGamepadTouchpad samples to illustrate the use of these new gampad capabilities. - * Condensed existing Vuforia samples into just 2 samples (ConceptVuforiaFieldNavigation & ConceptVuforiaFieldNavigationWebcam) showing how to determine the robot's location on the field using Vuforia. These both use the current season's Target images. - * Added ConceptVuforiaDriveToTargetWebcam to illustrate an easy way to drive directly to any visible Vuforia target. -* Makes many improvements to the warning system and individual warnings. - * Warnings are now much more spaced out, so that they are easier to read. - * New warnings were added for conditions that should be resolved before competing. - * The mismatched apps warning now uses the major and minor app versions, not the version code. - * The warnings are automatically re-enabled when a Robot Controller app from a new FTC season is installed. -* Adds support for I2C transactions on the Expansion Hub / Control Hub without specifying a register address. - * See section 3 of the [TI I2C spec](https://www.ti.com/lit/an/slva704/slva704.pdf). - * Calling these new methods when using Modern Robotics hardware will result in an UnsupportedOperationException. -* Changes VuforiaLocalizer `close()` method to be public. -* Adds support for TensorFlow v2 object detection models. -* Reduces ambiguity of the Self Inspect language and graphics. -* OnBotJava now warns about potentially unintended file overwrites. -* Improves behavior of the Wi-Fi band and channel selector on the Manage webpage. - -### Bug fixes - * Fixes Robot Controller app crash on Android 9+ when a Driver Station connects. - * Fixes issue where an OpMode was responsible for calling shutdown on the - TensorFlow TFObjectDetector. Now this is done automatically. - * Fixes Vuforia initialization blocks to allow user to chose AxesOrder. Updated - relevant blocks sample opmodes. - * Fixes [FtcRobotController issue #114](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/114) - LED blocks and Java class do not work. - * Fixes match logging for OpModes that contain special characters in their names. - * Fixes Driver Station OpMode controls becoming unresponsive if the Driver Station was set to the landscape layout and an OnBotJava build was triggered while an OpMode was running. - * Fixes the Driver Station app closing itself when it is switched away from, or the screen is turned off. - * Fixes "black swirl of doom" (Infinite "configuring Wi-Fi Direct" message) on older devices. - * Updates the wiki comment on the OnBotJava intro page. - -## Version 6.2 (20210218-074821) - -### Enhancements -* Attempts to automatically fix the condition where a Control Hub's internal Expansion Hub is not - working by re-flashing its firmware -* Makes various improvements to the Wi-Fi Direct pairing screen, especially in landscape mode -* Makes the Robot Controller service no longer be categorically restarted when the main activity is brought to foreground - * (e.g. the service is no longer restarted simply by viewing the Self Inspect screen and pressing the back button) - * It is still restarted if the Settings menu or Configure Robot menu is opened - - -### Bug fixes -* Fixes [FtcRobotController issue #71](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/71) - Cannot open OpModes in v6.1 Blocks offline editor -* Fixes [FtcRobotController issue #79](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/79) - 6.1 causes a soft reboot on the Motorola E5 Play -* Fixes issue where the Control Hub OS's watchdog would restart the Robot Controller app if - the Control Hub was not able to communicate with its internal Expansion Hub -* Fixes certain I2C devices not showing up in the appropriate `HardwareMap` fields (such as `hardwareMap.colorSensor`) -* Fixes issue where performing a Wi-Fi factory reset on the Control Hub would not set the Wi-Fi band to 2.4 GHz -* Fixes issue where OnBotJava might fail to create a new file if the option to "Setup Code for Configured Hardware" was selected -* Fixes issue where performing certain operations after an OpMode crashes would temporarily break Control/Expansion Hub communication -* Fixes issue where a Control Hub with a configured USB-connected Expansion Hub would not work if the Expansion Hub was missing at startup -* Fixes potential issues caused by having mismatched Control/Expansion Hub firmware versions -* Fixes [ftc_app issue 673](https://github.com/ftctechnh/ftc_app/issues/673) Latest matchlog is being deleted instead of old ones by RobotLog -* Fixes ConceptVuforiaUltimateGoalNavigationWebcam sample opmode by correctly orienting camera on robot. -* Fixes issue where logcat would be spammed with InterruptedExceptions when stop is requested from the Driver Station (this behavior was accidentally introduced in v5.3). This change has no impact on functionality. -* Fixes issue where the blocks editor fails to load if the name of any TeleOp opmode contains an apostrophe. - -## Version 6.1 (20201209-113742) -* Makes the scan button on the configuration screen update the list of Expansion Hubs connected via RS-485 - * Fixes [SkyStone issue #143](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/143) -* Improves web interface compatibility with older browser and Android System WebView versions. -* Fixes issue in UVC driver where some cameras (e.g. certain MS Lifecams) which reported frame intervals as rounded rather than truncated values (e.g. `666667*100ns` instead of `666666*100ns` for 15FPS) would fail to start streaming. -* Adds support in UVC driver for virtual PTZ control -* Adds support in UVC driver for gain (ISO) control -* Adds support in UVC driver for enabling/disable AE priority. This setting provides a means to tell the camera firmware either - * A) It can undershoot the requested frame rate in order to provide a theoretically better image (i.e. with a longer exposure than the inter-frame period of the selected frame rate allows) - * B) It *must* meet the inter-frame deadline for the selected frame rate, even if the image may be underexposed as a result -* Adds support for the Control Hub OS 1.1.2 Robot Controller watchdog - * The Robot Controller app will be restarted if it stops responding for more than 10 seconds -* Adds support for using the Driver Station app on Android 10+ -* Introduces an automatic TeleOp preselection feature - * For details and usage guide, please see [this wiki entry](https://github.com/FIRST-Tech-Challenge/FtcRobotController/wiki/Automatically-Loading-a-Driver-Controlled-Op-Mode) -* Shows icon next to OpMode name in the OpMode list dropdown on the Driver Station to indicate the source of the OpMode (i.e. the programming tool used to create it) -* Fixes issue where the Driver Station app would exit after displaying the Configuring Wi-Fi Direct screen -* Fixes Blocks and OnBotJava prompts when accessed via the REV Hardware Client - -## Version 6.0 (20200921-085816) - -### Important Notes -* Version 6.0 is the version for the Ultimate Goal season. -* Requires Android Studio 4.0. -* Android Studio users need to be connected to the Internet the first time they build the app (in order to download needed packages for the build). -* Version 5.5 was a moderately large off-season, August 2020, drop. It's worth reviewing those release notes below also. -* Version 5.5 and greater will not work on older Android 4.x and 5.x phones. Users must upgrade to an approved Android 6.x device or newer. -* The default PIDF values for REV motors have been reverted to the default PID values that were used in the 2018-2019 season - * This change was made because the 2018-2019 values turned out to work better for many mechanisms - * This brings the behavior of the REV motors in line with the behavior of all other motors - * If you prefer the 2019-2020 season's behavior for REV motors, here are the PIDF values that were in place, so that you can manually set them in your OpModes: -
- **HD Hex motors (all gearboxes):** - Velocity PIDF values: `P = 1.17`, `I = 0.117`, `F = 11.7` - Position PIDF values: `P = 5.0` - **Core Hex motor:** - Velocity PIDF values: `P = 4.96`, `I = 0.496`, `F = 49.6` - Position PIDF values: `P = 5.0` - -### New features -* Includes TensorFlow inference model and sample OpModes to detect Ultimate Goal Starter Stacks (four rings vs single ring stack). -* Includes Vuforia Ultimate Goal vision targets and sample OpModes. -* Introduces a digital zoom feature for TensorFlow object detection (to detect objects more accurately at greater distances). -* Adds configuration entry for the REV UltraPlanetary HD Hex motor - -### Enhancements -* Adds setGain() and getGain() methods to the NormalizedColorSensor interface - * By setting the gain of a color sensor, you can adjust for different lighting conditions. - For example, if you detect lower color values than expected, you can increase the gain. - * The gain value is only applied to the argb() and getNormalizedColors() methods, not to the raw color methods. - The getNormalizedColors() method is recommended for ease-of-use and clarity, since argb() has to be converted. - * Updates SensorColor Java sample to demonstrate gain usage -* Merges SensorREVColorDistance Java sample into SensorColor Java sample, which showcases best practices for all color sensors -* Improves retrieving values from the REV Color Sensor V3 - * Updates the normalization calculation of the RGB channels - * Improves the calculation of the alpha channel (can be used as an overall brightness indicator) - * Fixes the default sensor resolution, which caused issues with bright environments - * Adds support for changing the resolution and measuring rate of the Broadcom sensor chip - * Removes IR readings and calculations not meant for the Broadcom sensor chip - -### Bug fixes -* Improves reliability of BNO055IMU IMU initialization to prevent random initialization failures (which manifested as `Problem with 'imu'`). - -## Version 5.5 (20200824-090813) - -Version 5.5 requires Android Studio 4.0 or later. - -### New features -* Adds support for calling custom Java classes from Blocks OpModes (fixes [SkyStone issue #161](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/161)). - * Classes must be in the org.firstinspires.ftc.teamcode package. - * To have easy access to the opMode, hardwareMap, telemetry, gamepad1, and gamepad2, classes can - extends org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion. - * Methods must be public static and have no more than 21 parameters. - * Methods must be annotated with org.firstinspires.ftc.robotcore.external.ExportToBlocks. - * Parameters declared as OpMode, LinearOpMode, Telemetry, and HardwareMap are supported and the - argument is provided automatically, regardless of the order of the parameters. On the block, - the sockets for those parameters are automatically filled in. - * Parameters declared as char or java.lang.Character will accept any block that returns text - and will only use the first character in the text. - * Parameters declared as boolean or java.lang.Boolean will accept any block that returns boolean. - * Parameters declared as byte, java.lang.Byte, short, java.lang.Short, int, java.lang.Integer, - long, or java.lang.Long, will accept any block that returns a number and will round that - value to the nearest whole number. - * Parameters declared as float, java.lang.Float, double, java.lang.Double will accept any - block that returns a number. -* Adds telemetry API method for setting display format - * Classic - * Monospace - * HTML (certain tags only) -* Adds blocks support for switching cameras. -* Adds Blocks support for TensorFlow Object Detection with a custom model. -* Adds support for uploading a custom TensorFlow Object Detection model in the Manage page, which - is especially useful for Blocks and OnBotJava users. -* Shows new Control Hub blink codes when the Wi-Fi band is switched using the Control Hub's button (only possible on Control Hub OS 1.1.2) -* Adds new warnings which can be disabled in the Advanced RC Settings - * Mismatched app versions warning - * Unnecessary 2.4 GHz Wi-Fi usage warning - * REV Hub is running outdated firmware (older than version 1.8.2) -* Adds support for Sony PS4 gamepad, and reworks how gamepads work on the Driver Station - * Removes preference which sets gamepad type based on driver position. Replaced with menu which allows specifying type for gamepads with unknown VID and PID - * Attempts to auto-detect gamepad type based on USB VID and PID - * If gamepad VID and PID is not known, use type specified by user for that VID and PID - * If gamepad VID and PID is not known AND the user has not specified a type for that VID and PID, an educated guess is made about how to map the gamepad -* Driver Station will now attempt to automatically recover from a gamepad disconnecting, and re-assign it to the position it was assigned to when it dropped - * If only one gamepad is assigned and it drops: it can be recovered - * If two gamepads are assigned, and have **different** VID/PID signatures, and only one drops: it will be recovered - * If two gamepads are assigned, and have **different** VID/PID signatures, and BOTH drop: both will be recovered - * If two gamepads are assigned, and have **the same** VID/PID signatures, and only one drops: it will be recovered - * If two gamepads are assigned, and have **the same** VID/PID signatures, and BOTH drop: **neither** will be recovered, because of the ambiguity of the gamepads when they re-appear on the USB bus. - * There is currently one known edge case: if there are **two** gamepads with **the same** VID/PID signature plugged in, **but only one is assigned**, and they BOTH drop, it's a 50-50 chance of which one will be chosen for automatic recovery to the assigned position: it is determined by whichever one is re-enumerated first by the USB bus controller. -* Adds landscape user interface to Driver Station - * New feature: practice timer with audio cues - * New feature (Control Hub only): wireless network connection strength indicator (0-5 bars) - * New feature (Control Hub only): tapping on the ping/channel display will switch to an alternate display showing radio RX dBm and link speed (tap again to switch back) - * The layout will NOT autorotate. You can switch the layout from the Driver Station's settings menu. -### Breaking changes -* Removes support for Android versions 4.4 through 5.1 (KitKat and Lollipop). The minSdkVersion is now 23. -* Removes the deprecated `LinearOpMode` methods `waitOneFullHardwareCycle()` and `waitForNextHardwareCycle()` -### Enhancements -* Handles RS485 address of Control Hub automatically - * The Control Hub is automatically given a reserved address - * Existing configuration files will continue to work - * All addresses in the range of 1-10 are still available for Expansion Hubs - * The Control Hub light will now normally be solid green, without blinking to indicate the address - * The Control Hub will not be shown on the Expansion Hub Address Change settings page -* Improves REV Hub firmware updater - * The user can now choose between all available firmware update files - * Version 1.8.2 of the REV Hub firmware is bundled into the Robot Controller app. - * Text was added to clarify that Expansion Hubs can only be updated via USB. - * Firmware update speed was reduced to improve reliability - * Allows REV Hub firmware to be updated directly from the Manage webpage -* Improves log viewer on Robot Controller - * Horizontal scrolling support (no longer word wrapped) - * Supports pinch-to-zoom - * Uses a monospaced font - * Error messages are highlighted - * New color scheme -* Attempts to force-stop a runaway/stuck OpMode without restarting the entire app - * Not all types of runaway conditions are stoppable, but if the user code attempts to talk to hardware during the runaway, the system should be able to capture it. -* Makes various tweaks to the Self Inspect screen - * Renames "OS version" entry to "Android version" - * Renames "Wi-Fi Direct Name" to "Wi-Fi Name" - * Adds Control Hub OS version, when viewing the report of a Control Hub - * Hides the airplane mode entry, when viewing the report of a Control Hub - * Removes check for ZTE Speed Channel Changer - * Shows firmware version for **all** Expansion and Control Hubs -* Reworks network settings portion of Manage page - * All network settings are now applied with a single click - * The Wi-Fi Direct channel of phone-based Robot Controllers can now be changed from the Manage page - * Wi-Fi channels are filtered by band (2.4 vs 5 GHz) and whether they overlap with other channels - * The current Wi-Fi channel is pre-selected on phone-based Robot Controllers, and Control Hubs running OS 1.1.2 or later. - * On Control Hubs running OS 1.1.2 or later, you can choose to have the system automatically select a channel on the 5 GHz band -* Improves OnBotJava - * New light and dark themes replace the old themes (chaos, github, chrome,...) - * the new default theme is `light` and will be used when you first update to this version - * OnBotJava now has a tabbed editor - * Read-only offline mode -* Improves function of "exit" menu item on Robot Controller and Driver Station - * Now guaranteed to be fully stopped and unloaded from memory -* Shows a warning message if a LinearOpMode exists prematurely due to failure to monitor for the start condition -* Improves error message shown when the Driver Station and Robot Controller are incompatible with each other -* Driver Station OpMode Control Panel now disabled while a Restart Robot is in progress -* Disables advanced settings related to Wi-Fi Direct when the Robot Controller is a Control Hub. -* Tint phone battery icons on Driver Station when low/critical. -* Uses names "Control Hub Portal" and "Control Hub" (when appropriate) in new configuration files -* Improve I2C read performance - * Very large improvement on Control Hub; up to ~2x faster with small (e.g. 6 byte) reads - * Not as apparent on Expansion Hubs connected to a phone -* Update/refresh build infrastructure - * Update to 'androidx' support library from 'com.android.support:appcompat', which is end-of-life - * Update targetSdkVersion and compileSdkVersion to 28 - * Update Android Studio's Android plugin to latest - * Fix reported build timestamp in 'About' screen -* Add sample illustrating manual webcam use: ConceptWebcam - - -### Bug fixes -* Fixes [SkyStone issue #248](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/248) -* Fixes [SkyStone issue #232](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/232) and - modifies bulk caching semantics to allow for cache-preserving MANUAL/AUTO transitions. -* Improves performance when REV 2M distance sensor is unplugged -* Improves readability of Toast messages on certain devices -* Allows a Driver Station to connect to a Robot Controller after another has disconnected -* Improves generation of fake serial numbers for UVC cameras which do not provide a real serial number - * Previously some devices would assign such cameras a serial of `0:0` and fail to open and start streaming - * Fixes [ftc_app issue #638](https://github.com/ftctechnh/ftc_app/issues/638). -* Fixes a slew of bugs with the Vuforia camera monitor including: - * Fixes bug where preview could be displayed with a wonky aspect ratio - * Fixes bug where preview could be cut off in landscape - * Fixes bug where preview got totally messed up when rotating phone - * Fixes bug where crosshair could drift off target when using webcams -* Fixes issue in UVC driver on some devices ([ftc_app 681](https://github.com/ftctechnh/ftc_app/issues/681)) if streaming was started/stopped multiple times in a row - * Issue manifested as kernel panic on devices which do not have [this kernel patch](https://lore.kernel.org/patchwork/patch/352400/). - * On affected devices which **do** have the patch, the issue was manifest as simply a failure to start streaming. - * The Tech Team believes that the root cause of the issue is a bug in the Linux kernel XHCI driver. A workaround was implemented in the SDK UVC driver. -* Fixes bug in UVC driver where often half the frames from the camera would be dropped (e.g. only 15FPS delivered during a streaming session configured for 30FPS). -* Fixes issue where TensorFlow Object Detection would show results whose confidence was lower than - the minimum confidence parameter. -* Fixes a potential exploitation issue of [CVE-2019-11358](https://www.cvedetails.com/cve/CVE-2019-11358/) in OnBotJava -* Fixes changing the address of an Expansion Hub with additional Expansion Hubs connected to it -* Preserves the Control Hub's network connection when "Restart Robot" is selected -* Fixes issue where device scans would fail while the Robot was restarting -* Fix RenderScript usage - * Use androidx.renderscript variant: increased compatibility - * Use RenderScript in Java mode, not native: simplifies build -* Fixes webcam-frame-to-bitmap conversion problem: alpha channel wasn't being initialized, only R, G, & B -* Fixes possible arithmetic overflow in Deadline -* Fixes deadlock in Vuforia webcam support which could cause 5-second delays when stopping OpMode - -## Version 5.4 (20200108-101156) -* Fixes [SkyStone issue #88](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/88) -* Adds an inspection item that notes when a robot controller (Control Hub) is using the factory default password. -* Fixes [SkyStone issue #61](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/61) -* Fixes [SkyStone issue #142](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/142) -* Fixes [ftc_app issue #417](https://github.com/ftctechnh/ftc_app/issues/417) by adding more current and voltage monitoring capabilities for REV Hubs. -* Fixes [a crash sometimes caused by OnBotJava activity](https://ftcforum.firstinspires.org/forum/ftc-technology/76217-onbotjava-crashes-robot-controller) -* Improves OnBotJava autosave functionality [ftc_app #738](https://github.com/ftctechnh/ftc_app/issues/738) -* Fixes system responsiveness issue when an Expansion Hub is disconnected -* Fixes issue where IMU initialization could prevent OpModes from stopping -* Fixes issue where AndroidTextToSpeech.speak() would fail if it was called too early -* Adds telemetry.speak() methods and blocks, which cause the Driver Station (if also updated) to speak text -* Adds and improves Expansion Hub-related warnings - * Improves Expansion Hub low battery warning - * Displays the warning immediately after the hub reports it - * Specifies whether the condition is current or occurred temporarily during an OpMode run - * Displays which hubs reported low battery - * Displays warning when hub loses and regains power during an OpMode run - * Fixes the hub's LED pattern after this condition - * Displays warning when Expansion Hub is not responding to commands - * Specifies whether the condition is current or occurred temporarily during an OpMode run - * Clarifies warning when Expansion Hub is not present at startup - * Specifies that this condition requires a Robot Restart before the hub can be used. - * The hub light will now accurately reflect this state - * Improves logging and reduces log spam during these conditions -* Syncs the Control Hub time and timezone to a connected web browser programming the robot, if a Driver Station is not available. -* Adds bulk read functionality for REV Hubs - * A bulk caching mode must be set at the Hub level with `LynxModule#setBulkCachingMode()`. This applies to all relevant SDK hardware classes that reference that Hub. - * The following following Hub bulk caching modes are available: - * `BulkCachingMode.OFF` (default): All hardware calls operate as usual. Bulk data can read through `LynxModule#getBulkData()` and processed manually. - * `BulkCachingMode.AUTO`: Applicable hardware calls are served from a bulk read cache that is cleared/refreshed automatically to ensure identical commands don't hit the same cache. The cache can also be cleared manually with `LynxModule#clearBulkCache()`, although this is not recommended. - * (advanced users) `BulkCachingMode.MANUAL`: Same as `BulkCachingMode.AUTO` except the cache is never cleared automatically. To avoid getting stale data, the cache must be manually cleared at the beginning of each loop body or as the user deems appropriate. -* Removes PIDF Annotation values added in Rev 5.3 (to AndyMark, goBILDA and TETRIX motor configurations). - * The new motor types will still be available but their Default control behavior will revert back to Rev 5.2 -* Adds new `ConceptMotorBulkRead` sample Opmode to demonstrate and compare Motor Bulk-Read modes for reducing I/O latencies. - -## Version 5.3 (20191004-112306) -* Fixes external USB/UVC webcam support -* Makes various bugfixes and improvements to Blocks page, including but not limited to: - * Many visual tweaks - * Browser zoom and window resize behave better - * Resizing the Java preview pane works better and more consistently across browsers - * The Java preview pane consistently gets scrollbars when needed - * The Java preview pane is hidden by default on phones - * Internet Explorer 11 should work - * Large dropdown lists display properly on lower res screens - * Disabled buttons are now visually identifiable as disabled - * A warning is shown if a user selects a TFOD sample, but their device is not compatible - * Warning messages in a Blocks OpMode are now visible by default. -* Adds goBILDA 5201 and 5202 motors to Robot Configurator -* Adds PIDF Annotation values to AndyMark, goBILDA and TETRIX motor configurations. - This has the effect of causing the RUN_USING_ENCODERS and RUN_TO_POSITION modes to use - PIDF vs PID closed loop control on these motors. This should provide more responsive, yet stable, speed control. - PIDF adds Feedforward control to the basic PID control loop. - Feedforward is useful when controlling a motor's speed because it "anticipates" how much the control voltage - must change to achieve a new speed set-point, rather than requiring the integrated error to change sufficiently. - The PIDF values were chosen to provide responsive, yet stable, speed control on a lightly loaded motor. - The more heavily a motor is loaded (drag or friction), the more noticable the PIDF improvement will be. -* Fixes startup crash on Android 10 -* Fixes [ftc_app issue #712](https://github.com/ftctechnh/ftc_app/issues/712) (thanks to FROGbots-4634) -* Fixes [ftc_app issue #542](https://github.com/ftctechnh/ftc_app/issues/542) -* Allows "A" and lowercase letters when naming device through RC and DS apps. - -## Version 5.2 (20190905-083277) -* Fixes extra-wide margins on settings activities, and placement of the new configuration button -* Adds Skystone Vuforia image target data. - * Includes sample Skystone Vuforia Navigation OpModes (Java). - * Includes sample Skystone Vuforia Navigation OpModes (Blocks). -* Adds TensorFlow inference model (.tflite) for Skystone game elements. - * Includes sample Skystone TensorFlow OpModes (Java). - * Includes sample Skystone TensorFlow OpModes (Blocks). -* Removes older (season-specific) sample OpModes. -* Includes 64-bit support (to comply with [Google Play requirements](https://android-developers.googleblog.com/2019/01/get-your-apps-ready-for-64-bit.html)). -* Protects against Stuck OpModes when a Restart Robot is requested. (Thanks to FROGbots-4634) ([ftc_app issue #709](https://github.com/ftctechnh/ftc_app/issues/709)) -* Blocks related changes: - * Fixes bug with blocks generated code when hardware device name is a java or javascript reserved word. - * Shows generated java code for blocks, even when hardware items are missing from the active configuration. - * Displays warning icon when outdated Vuforia and TensorFlow blocks are used ([SkyStone issue #27](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/27)) - -## Version 5.1 (20190820-222104) -* Defines default PIDF parameters for the following motors: - * REV Core Hex Motor - * REV 20:1 HD Hex Motor - * REV 40:1 HD Hex Motor -* Adds back button when running on a device without a system back button (such as a Control Hub) -* Allows a REV Control Hub to update the firmware on a REV Expansion Hub via USB -* Fixes [SkyStone issue #9](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/9) -* Fixes [ftc_app issue #715](https://github.com/ftctechnh/ftc_app/issues/715) -* Prevents extra DS User clicks by filtering based on current state. -* Prevents incorrect DS UI state changes when receiving new OpMode list from RC -* Adds support for REV Color Sensor V3 -* Adds a manual-refresh DS Camera Stream for remotely viewing RC camera frames. - * To show the stream on the DS, initialize **but do not run** a stream-enabled opmode, select the Camera Stream option in the DS menu, and tap the image to refresh. This feature is automatically enabled when using Vuforia or TFOD—no additional RC configuration is required for typical use cases. To hide the stream, select the same menu item again. - * Note that gamepads are disabled and the selected opmode cannot be started while the stream is open as a safety precaution. - * To use custom streams, consult the API docs for `CameraStreamServer#setSource` and `CameraStreamSource`. -* Adds many Star Wars sounds to RobotController resources. -* Added Skystone Sounds Chooser Sample Program. -* Switches out startup, connect chimes, and error/warning sounds for Star Wars sounds -* Updates OnBot Java to use a WebSocket for communication with the robot - * The OnBot Java page no longer has to do a full refresh when a user switches from editing one file to another - -Known issues: -* Camera Stream - * The Vuforia camera stream inherits the issues present in the phone preview (namely [ftc_app issue #574](https://github.com/ftctechnh/ftc_app/issues/574)). This problem does not affect the TFOD camera stream even though it receives frames from Vuforia. - * The orientation of the stream frames may not always match the phone preview. For now, these frames may be rotated manually via a custom `CameraStreamSource` if desired. -* OnBotJava - * Browser back button may not always work correctly - * It's possible for a build to be queued, but not started. The OnBot Java build console will display a warning if this occurs. - * A user might not realize they are editing a different file if the user inadvertently switches from one file to another since this switch is now seamless. The name of the currently open file is displayed in the browser tab. - -## Version 5.0 (built on 19.06.14) - * Support for the REV Robotics Control Hub. - * Adds a Java preview pane to the Blocks editor. - * Adds a new offline export feature to the Blocks editor. - * Display Wi-Fi channel in Network circle on Driver Station. - * Adds calibration for Logitech C270 - * Updates build tooling and target SDK. - * Compliance with Google's permissions infrastructure (Required after build tooling update). - * Keep Alives to mitigate the Motorola Wi-Fi scanning problem. Telemetry substitute no longer necessary. - * Improves Vuforia error reporting. - * Fixes ftctechnh/ftc_app issues 621, 713. - * Miscellaneous bug fixes and improvements. - -## Version 4.3 (built on 18.10.31) - * Includes missing TensorFlow-related libraries and files. - -## Version 4.2 (built on 18.10.30) - * Includes fix to avoid deadlock situation with WatchdogMonitor which could result in USB communication errors. - - Comm error appeared to require that user disconnect USB cable and restart the Robot Controller app to recover. - - robotControllerLog.txt would have error messages that included the words "E RobotCore: lynx xmit lock: #### abandoning lock:" - * Includes fix to correctly list the parent module address for a REV Robotics Expansion Hub in a configuration (.xml) file. - - Bug in versions 4.0 and 4.1 would incorrect list the address module for a parent REV Robotics device as "1". - - If the parent module had a higher address value than the daisy-chained module, then this bug would prevent the Robot Controller from communicating with the downstream Expansion Hub. - * Added requirement for ACCESS_COARSE_LOCATION to allow a Driver Station running Android Oreo to scan for Wi-Fi Direct devices. - * Added google() repo to build.gradle because aapt2 must be downloaded from the google() repository beginning with version 3.2 of the Android Gradle Plugin. - - Important Note: Android Studio users will need to be connected to the Internet the first time build the ftc_app project. - - Internet connectivity is required for the first build so the appropriate files can be downloaded from the Google repository. - - Users should not need to be connected to the Internet for subsequent builds. - - This should also fix buid issue where Android Studio would complain that it "Could not find com.android.tools.lint:lint-gradle:26.1.4" (or similar). - * Added support for REV Spark Mini motor controller as part of the configuration menu for a servo/PWM port on the REV Expansion Hub. - * Provide examples for playing audio files in an OpMode. - * Block Development Tool Changes - - Includes a fix for a problem with the Velocity blocks that were reported in the FTC Technology forum (Blocks Programming subforum). - - Change the "Save completed successfully." message to a white color so it will contrast with a green background. - - Fixed the "Download image" feature so it will work if there are text blocks in the OpMode. - * Introduce support for Google's TensorFlow Lite technology for object detetion for 2018-2019 game. - - TensorFlow lite can recognize Gold Mineral and Silver Mineral from 2018-2019 game. - - Example Java and Block OpModes are included to show how to determine the relative position of the gold block (left, center, right). - -## Version 4.1 (released on 18.09.24) - -Changes include: - * Fix to prevent crash when deprecated configuration annotations are used. - * Change to allow FTC Robot Controller APK to be auto-updated using FIRST Global Control Hub update scripts. - * Removed samples for non supported / non legal hardware. - * Improvements to Telemetry.addData block with "text" socket. - * Updated Blocks sample OpMode list to include Rover Ruckus Vuforia example. - * Update SDK library version number. - -## Version 4.0 (released on 18.09.12) - -Changes include: - * Initial support for UVC compatible cameras - - If UVC camera has a unique serial number, RC will detect and enumerate by serial number. - - If UVC camera lacks a unique serial number, RC will only support one camera of that type connected. - - Calibration settings for a few cameras are included (see TeamCode/src/main/res/xml/teamwebcamcalibrations.xml for details). - - User can upload calibration files from Program and Manage web interface. - - UVC cameras seem to draw a fair amount of electrical current from the USB bus. - + This does not appear to present any problems for the REV Robotics Control Hub. - + This does seem to create stability problems when using some cameras with an Android phone-based Robot Controller. - + FTC Tech Team is investigating options to mitigate this issue with the phone-based Robot Controllers. - - Updated sample Vuforia Navigation and VuMark OpModes to demonstrate how to use an internal phone-based camera and an external UVC webcam. - - * Support for improved motor control. - - REV Robotics Expansion Hub firmware 1.8 and greater will support a feed forward mechanism for closed loop motor control. - - FTC SDK has been modified to support PIDF coefficients (proportional, integral, derivative, and feed forward). - - FTC Blocks development tool modified to include PIDF programming blocks. - - Deprecated older PID-related methods and variables. - - REV's 1.8.x PIDF-related changes provide a more linear and accurate way to control a motor. - - * Wireless - - Added 5GHz support for wireless channel changing for those devices that support it. - + Tested with Moto G5 and E4 phones. - + Also tested with other (currently non-approved) phones such as Samsung Galaxy S8. - -* Improved Expansion Hub firmware update support in Robot Controller app - - Changes to make the system more robust during the firmware update process (when performed through Robot Controller app). - - User no longer has to disconnect a downstream daisy-chained Expansion Hub when updating an Expansion Hub's firmware. - + If user is updating an Expansion Hub's firmware through a USB connection, he/she does not have to disconnect RS485 connection to other Expansion Hubs. - + The user still must use a USB connection to update an Expansion Hub's firmware. - + The user cannot update the Expansion Hub firmware for a downstream device that is daisy chained through an RS485 connection. - - If an Expansion Hub accidentally gets "bricked" the Robot Controller app is now more likely to recognize the Hub when it scans the USB bus. - + Robot Controller app should be able to detect an Expansion Hub, even if it accidentally was bricked in a previous update attempt. - + Robot Controller app should be able to install the firmware onto the Hub, even if if accidentally was bricked in a previous update attempt. - - * Resiliency - - FTC software can detect and enable an FTDI reset feature that is available with REV Robotics v1.8 Expansion Hub firmware and greater. - + When enabled, the Expansion Hub can detect if it hasn't communicated with the Robot Controller over the FTDI (USB) connection. - + If the Hub hasn't heard from the Robot Controller in a while, it will reset the FTDI connection. - + This action helps system recover from some ESD-induced disruptions. - - Various fixes to improve reliability of FTC software. - - * Blocks - - Fixed errors with string and list indices in blocks export to java. - - Support for USB connected UVC webcams. - - Refactored optimized Blocks Vuforia code to support Rover Ruckus image targets. - - Added programming blocks to support PIDF (proportional, integral, derivative and feed forward) motor control. - - Added formatting options (under Telemetry and Miscellaneous categories) so user can set how many decimal places to display a numerical value. - - Support to play audio files (which are uploaded through Blocks web interface) on Driver Station in addition to the Robot Controller. - - Fixed bug with Download Image of Blocks feature. - - Support for REV Robotics Blinkin LED Controller. - - Support for REV Robotics 2m Distance Sensor. - - Added support for a REV Touch Sensor (no longer have to configure as a generic digital device). - - Added blocks for DcMotorEx methods. - + These are enhanced methods that you can use when supported by the motor controller hardware. - + The REV Robotics Expansion Hub supports these enhanced methods. - + Enhanced methods include methods to get/set motor velocity (in encoder pulses per second), get/set PIDF coefficients, etc.. - - * Modest Improvements in Logging - - Decrease frequency of battery checker voltage statements. - - Removed non-FTC related log statements (wherever possible). - - Introduced a "Match Logging" feature. - + Under "Settings" a user can enable/disable this feature (it's disabled by default). - + If enabled, user provides a "Match Number" through the Driver Station user interface (top of the screen). - * The Match Number is used to create a log file specifically with log statements from that particular OpMode run. - * Match log files are stored in /sdcard/FIRST/matlogs on the Robot Controller. - * Once an OpMode run is complete, the Match Number is cleared. - * This is a convenient way to create a separate match log with statements only related to a specific OpMode run. - - * New Devices - - Support for REV Robotics Blinkin LED Controller. - - Support for REV Robotics 2m Distance Sensor. - - Added configuration option for REV 20:1 HD Hex Motor. - - Added support for a REV Touch Sensor (no longer have to configure as a generic digital device). - - * Miscellaneous - - Fixed some errors in the definitions for acceleration and velocity in our javadoc documentation. - - Added ability to play audio files on Driver Station - - When user is configuring an Expansion Hub, the LED on the Expansion Hub will change blink pattern (purple-cyan) to indicate which Hub is currently being configured. - - Renamed I2cSensorType to I2cDeviceType. - - Added an external sample OpMode that demonstrates localization using 2018-2019 (Rover Ruckus presented by QualComm) Vuforia targets. - - Added an external sample OpMode that demonstrates how to use the REV Robotics 2m Laser Distance Sensor. - - Added an external sample OpMode that demonstrates how to use the REV Robotics Blinkin LED Controller. - - Re-categorized external Java sample OpModes to "TeleOp" instead of "Autonomous". - -Known issues: - * Initial support for UVC compatible cameras - - UVC cameras seem to draw significant amount of current from the USB bus. - + This does not appear to present any problems for the REV Robotics Control Hub. - + This does seem to create stability problems when using some cameras with an Android phone-based Robot Controller. - + FTC Tech Team is investigating options to mitigate this issue with the phone-based Robot Controllers. - - There might be a possible deadlock which causes the RC to become unresponsive when using a UVC webcam with a Nougat Android Robot Controller. - - * Wireless - - When user selects a wireless channel, this channel does not necessarily persist if the phone is power cycled. - + Tech Team is hoping to eventually address this issue in a future release. - + Issue has been present since apps were introduced (i.e., it is not new with the v4.0 release). - - Wireless channel is not currently displayed for Wi-Fi Direct connections. - - * Miscellaneous - - The blink indication feature that shows which Expansion Hub is currently being configured does not work for a newly created configuration file. - + User has to first save a newly created configuration file and then close and re-edit the file in order for blink indicator to work. - -## Version 3.6 (built on 17.12.18) - -Changes include: - * Blocks Changes - - Uses updated Google Blockly software to allow users to edit their OpModes on Apple iOS devices (including iPad and iPhone). - - Improvement in Blocks tool to handle corrupt OpMode files. - - Autonomous OpModes should no longer get switched back to tele-op after re-opening them to be edited. - - The system can now detect type mismatches during runtime and alert the user with a message on the Driver Station. - * Updated javadoc documentation for setPower() method to reflect correct range of values (-1 to +1). - * Modified VuforiaLocalizerImpl to allow for user rendering of frames - - Added a user-overrideable onRenderFrame() method which gets called by the class's renderFrame() method. - -## Version 3.5 (built on 17.10.30) - -Changes with version 3.5 include: - * Introduced a fix to prevent random OpMode stops, which can occur after the Robot Controller app has been paused and then resumed (for example, when a user temporarily turns off the display of the Robot Controller phone, and then turns the screen back on). - * Introduced a fix to prevent random OpMode stops, which were previously caused by random peer disconnect events on the Driver Station. - * Fixes issue where log files would be closed on pause of the RC or DS, but not re-opened upon resume. - * Fixes issue with battery handler (voltage) start/stop race. - * Fixes issue where Android Studio generated OpModes would disappear from available list in certain situations. - * Fixes problem where OnBot Java would not build on REV Robotics Control Hub. - * Fixes problem where OnBot Java would not build if the date and time on the Robot Controller device was "rewound" (set to an earlier date/time). - * Improved error message on OnBot Java that occurs when renaming a file fails. - * Removed unneeded resources from android.jar binaries used by OnBot Java to reduce final size of Robot Controller app. - * Added MR_ANALOG_TOUCH_SENSOR block to Blocks Programming Tool. - -## Version 3.4 (built on 17.09.06) - -Changes with version 3.4 include: - * Added telemetry.update() statement for BlankLinearOpMode template. - * Renamed sample Block OpModes to be more consistent with Java samples. - * Added some additional sample Block OpModes. - * Reworded OnBot Java readme slightly. - -## Version 3.3 (built on 17.09.04) - -This version of the software includes improves for the FTC Blocks Programming Tool and the OnBot Java Programming Tool. - -Changes with verion 3.3 include: - * Android Studio ftc_app project has been updated to use Gradle Plugin 2.3.3. - * Android Studio ftc_app project is already using gradle 3.5 distribution. - * Robot Controller log has been renamed to /sdcard/RobotControllerLog.txt (note that this change was actually introduced w/ v3.2). - * Improvements in I2C reliability. - * Optimized I2C read for REV Expansion Hub, with v1.7 firmware or greater. - * Updated all external/samples (available through OnBot and in Android project folder). - * Vuforia - - Added support for VuMarks that will be used for the 2017-2018 season game. - * Blocks - - Update to latest Google Blockly release. - - Sample OpModes can be selected as a template when creating new OpMode. - - Fixed bug where the blocks would disappear temporarily when mouse button is held down. - - Added blocks for Range.clip and Range.scale. - - User can now disable/enable Block OpModes. - - Fix to prevent occasional Blocks deadlock. - * OnBot Java - - Significant improvements with autocomplete function for OnBot Java editor. - - Sample OpModes can be selected as a template when creating new OpMode. - - Fixes and changes to complete hardware setup feature. - - Updated (and more useful) onBot welcome message. - -Known issues: - * Android Studio - - After updating to the new v3.3 Android Studio project folder, if you get error messages indicating "InvalidVirtualFileAccessException" then you might need to do a File->Invalidate Caches / Restart to clear the error. - * OnBot Java - - Sometimes when you push the build button to build all OpModes, the RC returns an error message that the build failed. If you press the build button a second time, the build typically suceeds. - -## Version 3.2 (built on 17.08.02) - -This version of the software introduces the "OnBot Java" Development Tool. Similar to the FTC Blocks Development Tool, the FTC OnBot Java Development Tool allows a user to create, edit and build OpModes dynamically using only a Javascript-enabled web browser. - -The OnBot Java Development Tool is an integrated development environment (IDE) that is served up by the Robot Controller. OpModes are created and edited using a Javascript-enabled browser (Google Chromse is recommended). OpModes are saved on the Robot Controller Android device directly. - -The OnBot Java Development Tool provides a Java programming environment that does NOT need Android Studio. - - - -Changes with version 3.2 include: - * Enhanced web-based development tools - - Introduction of OnBot Java Development Tool. - - Web-based programming and management features are "always on" (user no longer needs to put Robot Controller into programming mode). - - Web-based management interface (where user can change Robot Controller name and also easily download Robot Controller log file). - - OnBot Java, Blocks and Management features available from web based interface. - -* Blocks Programming Development Tool: - - Changed "LynxI2cColorRangeSensor" block to "REV Color/range sensor" block. - - Fixed tooltip for ColorSensor.isLightOn block. - Added blocks for ColorSensor.getNormalizedColors and LynxI2cColorRangeSensor.getNormalizedColors. - -* Added example OpModes for digital touch sensor and REV Robotics Color Distance sensor. -* User selectable color themes. -* Includes many minor enhancements and fixes (too numerous to list). - -Known issues: -* Auto complete function is incomplete and does not support the following (for now): - - Access via *this* keyword - - Access via *super* keyword - - Members of the super cloass, not overridden by the class - - Any methods provided in the current class - - Inner classes - - Can't handle casted objects - - Any objects coming from an parenthetically enclosed expression - -## Version 3.10 (built on 17.05.09) - -This version of the software provides support for the REV Robotics Expansion Hub. This version also includes improvements in the USB communication layer in an effort to enhance system resiliency. If you were using a 2.x version of the software previously, updating to version 3.1 requires that you also update your Driver Station software in addition to updating the Robot Controller software. - -Also note that in version 3.10 software, the setMaxSpeed and getMaxSpeed methods are no longer available (not deprecated, they have been removed from the SDK). Also note that the new 3.x software incorporates motor profiles that a user can select as he/she configures the robot. - -Changes include: - * Blocks changes - - Added VuforiaTrackableDefaultListener.getPose and Vuforia.trackPose blocks. - - Added optimized blocks support for Vuforia extended tracking. - - Added atan2 block to the math category. - - Added useCompetitionFieldTargetLocations parameter to Vuforia.initialize block. If set to false, the target locations are placed at (0,0,0) with target orientation as specified in https://github.com/gearsincorg/FTCVuforiaDemo/blob/master/Robot_Navigation.java tutorial OpMode. - * Incorporates additional improvements to USB comm layer to improve system resiliency (to recover from a greater number of communication disruptions). - -************************************************************************************** - -Additional Notes Regarding Version 3.00 (built on 17.04.13) - -In addition to the release changes listed below (see section labeled "Version 3.00 (built on 17.04.013)"), version 3.00 has the following important changes: - -1. Version 3.00 software uses a new version of the FTC Robocol (robot protocol). If you upgrade to v3.0 on the Robot Controller and/or Android Studio side, you must also upgrade the Driver Station software to match the new Robocol. -2. Version 3.00 software removes the setMaxSpeed and getMaxSpeed methods from the DcMotor class. If you have an OpMode that formerly used these methods, you will need to remove the references/calls to these methods. Instead, v3.0 provides the max speed information through the use of motor profiles that are selected by the user during robot configuration. -3. Version 3.00 software currently does not have a mechanism to disable extra i2c sensors. We hope to re-introduce this function with a release in the near future. - -************************************************************************************** - -## Version 3.00 (built on 17.04.13) - -*** Use this version of the software at YOUR OWN RISK!!! *** - -This software is being released as an "alpha" version. Use this version at your own risk! - -This pre-release software contains SIGNIFICANT changes, including changes to the Wi-Fi Direct pairing mechanism, rewrites of the I2C sensor classes, changes to the USB/FTDI layer, and the introduction of support for the REV Robotics Expansion Hub and the REV Robotics color-range-light sensor. These changes were implemented to improve the reliability and resiliency of the FTC control system. - -Please note, however, that version 3.00 is considered "alpha" code. This code is being released so that the FIRST community will have an opportunity to test the new REV Expansion Hub electronics module when it becomes available in May. The developers do not recommend using this code for critical applications (i.e., competition use). - -*** Use this version of the software at YOUR OWN RISK!!! *** - -Changes include: - * Major rework of sensor-related infrastructure. Includes rewriting sensor classes to implement synchronous I2C communication. - * Fix to reset Autonomous timer back to 30 seconds. - * Implementation of specific motor profiles for approved 12V motors (includes Tetrix, AndyMark, Matrix and REV models). - * Modest improvements to enhance Wi-Fi P2P pairing. - * Fixes telemetry log addition race. - * Publishes all the sources (not just a select few). - * Includes Block programming improvements - - Addition of optimized Vuforia blocks. - - Auto scrollbar to projects and sounds pages. - - Fixed blocks paste bug. - - Blocks execute after while-opModeIsActive loop (to allow for cleanup before exiting OpMode). - - Added gyro integratedZValue block. - - Fixes bug with projects page for Firefox browser. - - Added IsSpeaking block to AndroidTextToSpeech. - * Implements support for the REV Robotics Expansion Hub - - Implements support for integral REV IMU (physically installed on I2C bus 0, uses same Bosch BNO055 9 axis absolute orientation sensor as Adafruit 9DOF abs orientation sensor). - Implements support for REV color/range/light sensor. - - Provides support to update Expansion Hub firmware through FTC SDK. - - Detects REV firmware version and records in log file. - - Includes support for REV Control Hub (note that the REV Control Hub is not yet approved for FTC use). - - Implements FTC Blocks programming support for REV Expansion Hub and sensor hardware. - - Detects and alerts when I2C device disconnect. - -## Version 2.62 (built on 17.01.07) - * Added null pointer check before calling modeToByte() in finishModeSwitchIfNecessary method for ModernRoboticsUsbDcMotorController class. - * Changes to enhance Modern Robotics USB protocol robustness. - -## Version 2.61 (released on 16.12.19) - * Blocks Programming mode changes: - - Fix to correct issue when an exception was thrown because an OpticalDistanceSensor object appears twice in the hardware map (the second time as a LightSensor). - -## Version 2.6 (released on 16.12.16) - * Fixes for Gyro class: - - Improve (decrease) sensor refresh latency. - - fix isCalibrating issues. - * Blocks Programming mode changes: - - Blocks now ignores a device in the configuration xml if the name is empty. Other devices work in configuration work fine. - -## Version 2.5 (internal release on released on 16.12.13) - * Blocks Programming mode changes: - - Added blocks support for AdafruitBNO055IMU. - - Added Download OpMode button to FtcBocks.html. - - Added support for copying blocks in one OpMode and pasting them in an other OpMode. The clipboard content is stored on the phone, so the programming mode server must be running. - - Modified Utilities section of the toolbox. - - In Programming Mode, display information about the active connections. - - Fixed paste location when workspace has been scrolled. - - Added blocks support for the android Accelerometer. - - Fixed issue where Blocks Upload OpMode truncated name at first dot. - - Added blocks support for Android SoundPool. - - Added type safety to blocks for Acceleration. - - Added type safety to blocks for AdafruitBNO055IMU.Parameters. - - Added type safety to blocks for AnalogInput. - - Added type safety to blocks for AngularVelocity. - - Added type safety to blocks for Color. - - Added type safety to blocks for ColorSensor. - - Added type safety to blocks for CompassSensor. - - Added type safety to blocks for CRServo. - - Added type safety to blocks for DigitalChannel. - - Added type safety to blocks for ElapsedTime. - - Added type safety to blocks for Gamepad. - - Added type safety to blocks for GyroSensor. - - Added type safety to blocks for IrSeekerSensor. - - Added type safety to blocks for LED. - - Added type safety to blocks for LightSensor. - - Added type safety to blocks for LinearOpMode. - - Added type safety to blocks for MagneticFlux. - - Added type safety to blocks for MatrixF. - - Added type safety to blocks for MrI2cCompassSensor. - - Added type safety to blocks for MrI2cRangeSensor. - - Added type safety to blocks for OpticalDistanceSensor. - - Added type safety to blocks for Orientation. - - Added type safety to blocks for Position. - - Added type safety to blocks for Quaternion. - - Added type safety to blocks for Servo. - - Added type safety to blocks for ServoController. - - Added type safety to blocks for Telemetry. - - Added type safety to blocks for Temperature. - - Added type safety to blocks for TouchSensor. - - Added type safety to blocks for UltrasonicSensor. - - Added type safety to blocks for VectorF. - - Added type safety to blocks for Velocity. - - Added type safety to blocks for VoltageSensor. - - Added type safety to blocks for VuforiaLocalizer.Parameters. - - Added type safety to blocks for VuforiaTrackable. - - Added type safety to blocks for VuforiaTrackables. - - Added type safety to blocks for enums in AdafruitBNO055IMU.Parameters. - - Added type safety to blocks for AndroidAccelerometer, AndroidGyroscope, AndroidOrientation, and AndroidTextToSpeech. - -## Version 2.4 (released on 16.11.13) - * Fix to avoid crashing for nonexistent resources. - * Blocks Programming mode changes: - - Added blocks to support OpenGLMatrix, MatrixF, and VectorF. - - Added blocks to support AngleUnit, AxesOrder, AxesReference, CameraDirection, CameraMonitorFeedback, DistanceUnit, and TempUnit. - - Added blocks to support Acceleration. - - Added blocks to support LinearOpMode.getRuntime. - - Added blocks to support MagneticFlux and Position. - - Fixed typos. - - Made blocks for ElapsedTime more consistent with other objects. - - Added blocks to support Quaternion, Velocity, Orientation, AngularVelocity. - - Added blocks to support VuforiaTrackables, VuforiaTrackable, VuforiaLocalizer, VuforiaTrackableDefaultListener. - - Fixed a few blocks. - - Added type checking to new blocks. - - Updated to latest blockly. - - Added default variable blocks to navigation and matrix blocks. - - Fixed toolbox entry for openGLMatrix_rotation_withAxesArgs. - - When user downloads Blocks-generated OpMode, only the .blk file is downloaded. - - When user uploads Blocks-generated OpMode (.blk file), Javascript code is auto generated. - - Added DbgLog support. - - Added logging when a blocks file is read/written. - - Fixed bug to properly render blocks even if missing devices from configuration file. - - Added support for additional characters (not just alphanumeric) for the block file names (for download and upload). - - Added support for OpMode flavor (“Autonomous” or “TeleOp”) and group. - * Changes to Samples to prevent tutorial issues. - * Incorporated suggested changes from public pull 216 (“Replace .. paths”). - * Remove Servo Glitches when robot stopped. - * if user hits “Cancels” when editing a configuration file, clears the unsaved changes and reverts to original unmodified configuration. - * Added log info to help diagnose why the Robot Controller app was terminated (for example, by watch dog function). - * Added ability to transfer log from the controller. - * Fixed inconsistency for AngularVelocity - * Limit unbounded growth of data for telemetry. If user does not call telemetry.update() for LinearOpMode in a timely manner, data added for telemetry might get lost if size limit is exceeded. - -## Version 2.35 (released on 16.10.06) - * Blockly programming mode - Removed unnecesary idle() call from blocks for new project. - -## Version 2.30 (released on 16.10.05) - * Blockly programming mode: - - Mechanism added to save Blockly OpModes from Programming Mode Server onto local device - - To avoid clutter, blocks are displayed in categorized folders - - Added support for DigitalChannel - - Added support for ModernRoboticsI2cCompassSensor - - Added support for ModernRoboticsI2cRangeSensor - - Added support for VoltageSensor - - Added support for AnalogInput - - Added support for AnalogOutput - - Fix for CompassSensor setMode block - * Vuforia - - Fix deadlock / make camera data available while Vuforia is running. - - Update to Vuforia 6.0.117 (recommended by Vuforia and Google to close security loophole). - * Fix for autonomous 30 second timer bug (where timer was in effect, even though it appeared to have timed out). - * opModeIsActive changes to allow cleanup after OpMode is stopped (with enforced 2 second safety timeout). - * Fix to avoid reading i2c twice. - * Updated sample OpModes. - * Improved logging and fixed intermittent freezing. - * Added digital I/O sample. - * Cleaned up device names in sample OpModes to be consistent with Pushbot guide. - * Fix to allow use of IrSeekerSensorV3. - -## Version 2.20 (released on 16.09.08) - * Support for Modern Robotics Compass Sensor. - * Support for Modern Robotics Range Sensor. - * Revise device names for Pushbot templates to match the names used in Pushbot guide. - * Fixed bug so that IrSeekerSensorV3 device is accessible as IrSeekerSensor in hardwareMap. - * Modified computer vision code to require an individual Vuforia license (per legal requirement from PTC). - * Minor fixes. - * Blockly enhancements: - - Support for Voltage Sensor. - - Support for Analog Input. - - Support for Analog Output. - - Support for Light Sensor. - - Support for Servo Controller. - -## Version 2.10 (released on 16.09.03) - * Support for Adafruit IMU. - * Improvements to ModernRoboticsI2cGyro class - - Block on reset of z axis. - - isCalibrating() returns true while gyro is calibration. - * Updated sample gyro program. - * Blockly enhancements - - support for android.graphics.Color. - - added support for ElapsedTime. - - improved look and legibility of blocks. - - support for compass sensor. - - support for ultrasonic sensor. - - support for IrSeeker. - - support for LED. - - support for color sensor. - - support for CRServo - - prompt user to configure robot before using programming mode. - * Provides ability to disable audio cues. - * various bug fixes and improvements. - -## Version 2.00 (released on 16.08.19) - * This is the new release for the upcoming 2016-2017 FIRST Tech Challenge Season. - * Channel change is enabled in the FTC Robot Controller app for Moto G 2nd and 3rd Gen phones. - * Users can now use annotations to register/disable their OpModes. - * Changes in the Android SDK, JDK and build tool requirements (minsdk=19, java 1.7, build tools 23.0.3). - * Standardized units in analog input. - * Cleaned up code for existing analog sensor classes. - * setChannelMode and getChannelMode were REMOVED from the DcMotorController class. This is important - we no longer set the motor modes through the motor controller. - * setMode and getMode were added to the DcMotor class. - * ContinuousRotationServo class has been added to the FTC SDK. - * Range.clip() method has been overloaded so it can support this operation for int, short and byte integers. - * Some changes have been made (new methods added) on how a user can access items from the hardware map. - * Users can now set the zero power behavior for a DC motor so that the motor will brake or float when power is zero. - * Prototype Blockly Programming Mode has been added to FTC Robot Controller. Users can place the Robot Controller into this mode, and then use a device (such as a laptop) that has a Javascript enabled browser to write Blockly-based OpModes directly onto the Robot Controller. - * Users can now configure the robot remotely through the FTC Driver Station app. - * Android Studio project supports Android Studio 2.1.x and compile SDK Version 23 (Marshmallow). - * Vuforia Computer Vision SDK integrated into FTC SDK. Users can use sample vision targets to get localization information on a standard FTC field. - * Project structure has been reorganized so that there is now a TeamCode package that users can use to place their local/custom OpModes into this package. - * Inspection function has been integrated into the FTC Robot Controller and Driver Station Apps (Thanks Team HazMat… 9277 & 10650!). - * Audio cues have been incorporated into FTC SDK. - * Swap mechanism added to FTC Robot Controller configuration activity. For example, if you have two motor controllers on a robot, and you misidentified them in your configuration file, you can use the Swap button to swap the devices within the configuration file (so you do not have to manually re-enter in the configuration info for the two devices). - * Fix mechanism added to all user to replace an electronic module easily. For example, suppose a servo controller dies on your robot. You replace the broken module with a new module, which has a different serial number from the original servo controller. You can use the Fix button to automatically reconfigure your configuration file to use the serial number of the new module. - * Improvements made to fix resiliency and responsiveness of the system. - * For LinearOpMode the user now must for a telemetry.update() to update the telemetry data on the driver station. This update() mechanism ensures that the driver station gets the updated data properly and at the same time. - * The Auto Configure function of the Robot Controller is now template based. If there is a commonly used robot configuration, a template can be created so that the Auto Configure mechanism can be used to quickly configure a robot of this type. - * The logic to detect a runaway OpMode (both in the LinearOpMode and OpMode types) and to abort the run, then auto recover has been improved/implemented. - * Fix has been incorporated so that Logitech F310 gamepad mappings will be correct for Marshmallow users. - -## Release 16.07.08 - - * For the ftc_app project, the gradle files have been modified to support Android Studio 2.1.x. - -## Release 16.03.30 - - * For the MIT App Inventor, the design blocks have new icons that better represent the function of each design component. - * Some changes were made to the shutdown logic to ensure the robust shutdown of some of our USB services. - * A change was made to LinearOpMode so as to allow a given instance to be executed more than once, which is required for the App Inventor. - * Javadoc improved/updated. - -## Release 16.03.09 - - * Changes made to make the FTC SDK synchronous (significant change!) - - waitOneFullHardwareCycle() and waitForNextHardwareCycle() are no longer needed and have been deprecated. - - runOpMode() (for a LinearOpMode) is now decoupled from the system's hardware read/write thread. - - loop() (for an OpMode) is now decoupled from the system's hardware read/write thread. - - Methods are synchronous. - - For example, if you call setMode(DcMotorController.RunMode.RESET_ENCODERS) for a motor, the encoder is guaranteed to be reset when the method call is complete. - - For legacy module (NXT compatible), user no longer has to toggle between read and write modes when reading from or writing to a legacy device. - * Changes made to enhance reliability/robustness during ESD event. - * Changes made to make code thread safe. - * Debug keystore added so that user-generated robot controller APKs will all use the same signed key (to avoid conflicts if a team has multiple developer laptops for example). - * Firmware version information for Modern Robotics modules are now logged. - * Changes made to improve USB comm reliability and robustness. - * Added support for voltage indicator for legacy (NXT-compatible) motor controllers. - * Changes made to provide auto stop capabilities for OpModes. - - A LinearOpMode class will stop when the statements in runOpMode() are complete. User does not have to push the stop button on the driver station. - - If an OpMode is stopped by the driver station, but there is a run away/uninterruptible thread persisting, the app will log an error message then force itself to crash to stop the runaway thread. - * Driver Station UI modified to display lowest measured voltage below current voltage (12V battery). - * Driver Station UI modified to have color background for current voltage (green=good, yellow=caution, red=danger, extremely low voltage). - * javadoc improved (edits and additional classes). - * Added app build time to About activity for driver station and robot controller apps. - * Display local IP addresses on Driver Station About activity. - * Added I2cDeviceSynchImpl. - * Added I2cDeviceSync interface. - * Added seconds() and milliseconds() to ElapsedTime for clarity. - * Added getCallbackCount() to I2cDevice. - * Added missing clearI2cPortActionFlag. - * Added code to create log messages while waiting for LinearOpMode shutdown. - * Fix so Wi-Fi Direct Config activity will no longer launch multiple times. - * Added the ability to specify an alternate i2c address in software for the Modern Robotics gyro. - -## Release 16.02.09 - - * Improved battery checker feature so that voltage values get refreshed regularly (every 250 msec) on Driver Station (DS) user interface. - * Improved software so that Robot Controller (RC) is much more resilient and “self-healing” to USB disconnects: - - If user attempts to start/restart RC with one or more module missing, it will display a warning but still start up. - - When running an OpMode, if one or more modules gets disconnected, the RC & DS will display warnings,and robot will keep on working in spite of the missing module(s). - - If a disconnected module gets physically reconnected the RC will auto detect the module and the user will regain control of the recently connected module. - - Warning messages are more helpful (identifies the type of module that’s missing plus its USB serial number). - * Code changes to fix the null gamepad reference when users try to reference the gamepads in the init() portion of their OpMode. - * NXT light sensor output is now properly scaled. Note that teams might have to readjust their light threshold values in their OpModes. - * On DS user interface, gamepad icon for a driver will disappear if the matching gamepad is disconnected or if that gamepad gets designated as a different driver. - * Robot Protocol (ROBOCOL) version number info is displayed in About screen on RC and DS apps. - * Incorporated a display filter on pairing screen to filter out devices that don’t use the “-“ format. This filter can be turned off to show all Wi-Fi Direct devices. - * Updated text in License file. - * Fixed formatting error in OpticalDistanceSensor.toString(). - * Fixed issue on with a blank (“”) device name that would disrupt Wi-Fi Direct Pairing. - * Made a change so that the Wi-Fi info and battery info can be displayed more quickly on the DS upon connecting to RC. - * Improved javadoc generation. - * Modified code to make it easier to support language localization in the future. - -## Release 16.01.04 - - * Updated compileSdkVersion for apps - * Prevent Wi-Fi from entering power saving mode - * removed unused import from driver station - * Corrrected "Dead zone" joystick code. - * LED.getDeviceName and .getConnectionInfo() return null - * apps check for ROBOCOL_VERSION mismatch - * Fix for Telemetry also has off-by-one errors in its data string sizing / short size limitations error - * User telemetry output is sorted. - * added formatting variants to DbgLog and RobotLog APIs - * code modified to allow for a long list of OpMode names. - * changes to improve thread safety of RobocolDatagramSocket - * Fix for "missing hardware leaves robot controller disconnected from driver station" error - * fix for "fast tapping of Init/Start causes problems" (toast is now only instantiated on UI thread). - * added some log statements for thread life cycle. - * moved gamepad reset logic inside of initActiveOpMode() for robustness - * changes made to mitigate risk of race conditions on public methods. - * changes to try and flag when Wi-Fi Direct name contains non-printable characters. - * fix to correct race condition between .run() and .close() in ReadWriteRunnableStandard. - * updated FTDI driver - * made ReadWriteRunnableStanard interface public. - * fixed off-by-one errors in Command constructor - * moved specific hardware implmentations into their own package. - * moved specific gamepad implemnatations to the hardware library. - * changed LICENSE file to new BSD version. - * fixed race condition when shutting down Modern Robotics USB devices. - * methods in the ColorSensor classes have been synchronized. - * corrected isBusy() status to reflect end of motion. - * corrected "back" button keycode. - * the notSupported() method of the GyroSensor class was changed to protected (it should not be public). - -## Release 15.11.04.001 - - * Added Support for Modern Robotics Gyro. - - The GyroSensor class now supports the MR Gyro Sensor. - - Users can access heading data (about Z axis) - - Users can also access raw gyro data (X, Y, & Z axes). - - Example MRGyroTest.java OpMode included. - * Improved error messages - - More descriptive error messages for exceptions in user code. - * Updated DcMotor API - * Enable read mode on new address in setI2cAddress - * Fix so that driver station app resets the gamepads when switching OpModes. - * USB-related code changes to make USB comm more responsive and to display more explicit error messages. - - Fix so that USB will recover properly if the USB bus returns garbage data. - - Fix USB initializtion race condition. - - Better error reporting during FTDI open. - - More explicit messages during USB failures. - - Fixed bug so that USB device is closed if event loop teardown method was not called. - * Fixed timer UI issue - * Fixed duplicate name UI bug (Legacy Module configuration). - * Fixed race condition in EventLoopManager. - * Fix to keep references stable when updating gamepad. - * For legacy Matrix motor/servo controllers removed necessity of appending "Motor" and "Servo" to controller names. - * Updated HT color sensor driver to use constants from ModernRoboticsUsbLegacyModule class. - * Updated MR color sensor driver to use constants from ModernRoboticsUsbDeviceInterfaceModule class. - * Correctly handle I2C Address change in all color sensors - * Updated/cleaned up OpModes. - - Updated comments in LinearI2cAddressChange.java example OpMode. - - Replaced the calls to "setChannelMode" with "setMode" (to match the new of the DcMotor method). - - Removed K9AutoTime.java OpMode. - - Added MRGyroTest.java OpMode (demonstrates how to use MR Gyro Sensor). - - Added MRRGBExample.java OpMode (demonstrates how to use MR Color Sensor). - - Added HTRGBExample.java OpMode (demonstrates how to use HT legacy color sensor). - - Added MatrixControllerDemo.java (demonstrates how to use legacy Matrix controller). - * Updated javadoc documentation. - * Updated release .apk files for Robot Controller and Driver Station apps. - -## Release 15.10.06.002 - - * Added support for Legacy Matrix 9.6V motor/servo controller. - * Cleaned up build.gradle file. - * Minor UI and bug fixes for driver station and robot controller apps. - * Throws error if Ultrasonic sensor (NXT) is not configured for legacy module port 4 or 5. - - -## Release 15.08.03.001 - - * New user interfaces for FTC Driver Station and FTC Robot Controller apps. - * An init() method is added to the OpMode class. - - For this release, init() is triggered right before the start() method. - - Eventually, the init() method will be triggered when the user presses an "INIT" button on driver station. - - The init() and loop() methods are now required (i.e., need to be overridden in the user's OpMode). - - The start() and stop() methods are optional. - * A new LinearOpMode class is introduced. - - Teams can use the LinearOpMode mode to create a linear (not event driven) program model. - - Teams can use blocking statements like Thread.sleep() within a linear OpMode. - * The API for the Legacy Module and Core Device Interface Module have been updated. - - Support for encoders with the Legacy Module is now working. - * The hardware loop has been updated for better performance. +### Feel Free to reach out on the [Offical Pedro Pathing Discord Server](https://discord.gg/2GfC4qBP5s)! From 61f0b8afe9a11bb11535999676ec9e242593b981 Mon Sep 17 00:00:00 2001 From: BaronClaps <126834072+BaronClaps@users.noreply.github.com> Date: Sat, 21 Dec 2024 16:24:07 -0600 Subject: [PATCH 90/94] Update TwoWheelPinpointIMULocalizer.java Makes position of dead wheels less confusing --- .../localization/localizers/TwoWheelPinpointIMULocalizer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java index 87849b9..9858caa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java @@ -82,8 +82,8 @@ public class TwoWheelPinpointIMULocalizer extends Localizer { */ public TwoWheelPinpointIMULocalizer(HardwareMap map, Pose setStartPose) { // TODO: replace these with your encoder positions - forwardEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0); - strafeEncoderPose = new Pose(-107.9/25.4+0.25, -1.1/25.4-0.23, Math.toRadians(90)); + forwardEncoderPose = new Pose(-0.82, 6.47, 0); + strafeEncoderPose = new Pose(-4, -0.273, Math.toRadians(90)); hardwareMap = map; From 4d6aafc1dd95f7417cd92166818b068cf90a37b5 Mon Sep 17 00:00:00 2001 From: Havish Sripada <139065303+junkjunk123@users.noreply.github.com> Date: Sun, 22 Dec 2024 18:51:46 -0800 Subject: [PATCH 91/94] Update PinpointLocalizer.java --- .../localizers/PinpointLocalizer.java | 463 +++++++++--------- 1 file changed, 244 insertions(+), 219 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java index 73272b8..d3d3fda 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java @@ -1,220 +1,245 @@ -//package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; -// -// -//import com.qualcomm.robotcore.hardware.HardwareMap; -// -//import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -//import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -//import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; -//import org.firstinspires.ftc.teamcode.pedroPathing.localization.GoBildaPinpointDriver; -//import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; -//import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; -//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; -// -///** -// * This is the Pinpoint class. This class extends the Localizer superclass and is a -// * localizer that uses the two wheel odometry set up with the IMU to have more accurate heading -// * readings. The diagram below, which is modified from Road Runner, shows a typical set up. -// * -// * The view is from the top of the robot looking downwards. -// * -// * left on robot is the y positive direction -// * -// * forward on robot is the x positive direction -// * -// * forward (x positive) -// * △ -// * | -// * | -// * /--------------\ -// * | | -// * | | -// * | || | -// * left (y positive) <--- | || | -// * | ____ | -// * | ---- | -// * \--------------/ -// * With the pinpoint your readings will be used in mm -// * to use inches ensure to divide your mm value by 25.4 -// * @author Logan Nash -// * @author Havish Sripada 12808 - RevAmped Robotics -// * @author Ethan Doak - Gobilda -// * @version 1.0, 10/2/2024 -// */ -//public class PinpointLocalizer extends Localizer { -// private HardwareMap hardwareMap; -// private GoBildaPinpointDriver odo; -// private double previousHeading; -// private double totalHeading; -// -// /** -// * This creates a new PinpointLocalizer from a HardwareMap, with a starting Pose at (0,0) -// * facing 0 heading. -// * -// * @param map the HardwareMap -// */ -// public PinpointLocalizer(HardwareMap map){ this(map, new Pose());} -// -// /** -// * This creates a new PinpointLocalizer from a HardwareMap and a Pose, with the Pose -// * specifying the starting pose of the localizer. -// * -// * @param map the HardwareMap -// * @param setStartPose the Pose to start from -// */ -// public PinpointLocalizer(HardwareMap map, Pose setStartPose){ -// hardwareMap = map; -// // TODO: replace this with your Pinpoint port -// odo = hardwareMap.get(GoBildaPinpointDriver.class,"odo"); -// -// //This uses mm, to use inches divide these numbers by 25.4 -// odo.setOffsets(-84.0, -168.0); //these are tuned for 3110-0002-0001 Product Insight #1 -// //TODO: If you find that the gobilda Yaw Scaling is incorrect you can edit this here -// // odo.setYawScalar(1.0); -// //TODO: Set your encoder resolution here, I have the Gobilda Odometry products already included. -// //TODO: If you would like to use your own odometry pods input the ticks per mm in the commented part below -// odo.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); -// //odo.setEncoderResolution(13.26291192); -// //TODO: Set encoder directions -// odo.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD); -// -// resetPinpoint();; +package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; -// setStartPose(setStartPose); -// totalHeading = 0; -// previousHeading = setStartPose.getHeading(); -// } -// -// /** -// * This returns the current pose estimate. -// * -// * @return returns the current pose estimate as a Pose -// */ -// @Override -// public Pose getPose() { -// Pose2D rawPose = odo.getPosition(); -// return new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS)); -// } -// -// /** -// * This returns the current velocity estimate. -// * -// * @return returns the current velocity estimate as a Pose -// */ -// @Override -// public Pose getVelocity() { -// Pose2D pose = odo.getVelocity(); -// return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), odo.getHeadingVelocity()); -// } -// -// /** -// * This returns the current velocity estimate. -// * -// * @return returns the current velocity estimate as a Vector -// */ -// @Override -// public Vector getVelocityVector() { -// Pose2D pose = odo.getVelocity(); -// Vector returnVector = new Vector(); -// returnVector.setOrthogonalComponents(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH)); -// return returnVector; -// } -// -// /** -// * This sets the start pose. Since nobody should be using this after the robot has begun moving, -// * and due to issues with the PinpointLocalizer, this is functionally the same as setPose(Pose). -// * -// * @param setStart the new start pose -// */ -// @Override -// public void setStartPose(Pose setStart) { -// odo.setPosition(new Pose2D(DistanceUnit.INCH, setStart.getX(), setStart.getY(), AngleUnit.RADIANS, setStart.getHeading())); -// } -// -// /** -// * This sets the current pose estimate. Changing this should just change the robot's current -// * pose estimate, not anything to do with the start pose. -// * -// * @param setPose the new current pose estimate -// */ -// @Override -// public void setPose(Pose setPose) { -// odo.setPosition(new Pose2D(DistanceUnit.INCH, setPose.getX(), setPose.getY(), AngleUnit.RADIANS, setPose.getHeading())); -// } -// -// /** -// * This updates the total heading of the robot. The Pinpoint handles all other updates itself. -// */ -// @Override -// public void update() { -// odo.update(); -// totalHeading += MathFunctions.getSmallestAngleDifference(MathFunctions.normalizeAngle(odo.getHeading()), previousHeading); -// previousHeading = MathFunctions.normalizeAngle(odo.getHeading()); -// } -// -// /** -// * This returns how far the robot has turned in radians, in a number not clamped between 0 and -// * 2 * pi radians. This is used for some tuning things and nothing actually within the following. -// * -// * @return returns how far the robot has turned in total, in radians. -// */ -// @Override -// public double getTotalHeading() { -// return totalHeading; -// } -// -// /** -// * This returns the Y encoder value as none of the odometry tuners are required for this localizer -// * @return returns the Y encoder value -// */ -// @Override -// public double getForwardMultiplier() { -// return odo.getEncoderY(); -// } -// -// /** -// * This returns the X encoder value as none of the odometry tuners are required for this localizer -// * @return returns the X encoder value -// */ -// @Override -// public double getLateralMultiplier() { -// return odo.getEncoderX(); -// } -// -// /** -// * This returns either the factory tuned yaw scalar or the yaw scalar tuned by yourself. -// * @return returns the yaw scalar -// */ -// @Override -// public double getTurningMultiplier() { -// return odo.getYawScalar(); -// } -// -// /** -// * This resets the IMU. Note: This does not change the estimated heading orientation. -// */ -// @Override -// public void resetIMU() throws InterruptedException { -// odo.recalibrateIMU(); -// -// try { -// Thread.sleep(300); -// } catch (InterruptedException e) { -// throw new RuntimeException(e); -// } -// } -// -// /** -// * This resets the pinpoint. -// */ -// private void resetPinpoint() { -// odo.resetPosAndIMU(); -// -// try { -// Thread.sleep(300); -// } catch (InterruptedException e) { -// throw new RuntimeException(e); -// } -// } -//} + +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.GoBildaPinpointDriver; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; +import org.opencv.core.Mat; + +/** + * This is the Pinpoint class. This class extends the Localizer superclass and is a + * localizer that uses the two wheel odometry set up with the IMU to have more accurate heading + * readings. The diagram below, which is modified from Road Runner, shows a typical set up. + * + * The view is from the top of the robot looking downwards. + * + * left on robot is the y positive direction + * + * forward on robot is the x positive direction + * + * /--------------\ + * | ____ | + * | ---- | + * | || | + * | || | ----> left (y positive) + * | | + * | | + * \--------------/ + * | + * | + * V + * forward (x positive) + * With the pinpoint your readings will be used in mm + * to use inches ensure to divide your mm value by 25.4 + * @author Logan Nash + * @author Havish Sripada 12808 - RevAmped Robotics + * @author Ethan Doak - Gobilda + * @version 1.0, 10/2/2024 + */ +public class PinpointLocalizer extends Localizer { + private HardwareMap hardwareMap; + private GoBildaPinpointDriver odo; + private double previousHeading; + private double totalHeading; + private Pose startPose; + private long deltaTimeNano; + private NanoTimer timer; + private Pose currentVelocity; + private Pose previousPinpointPose; + + /** + * This creates a new PinpointLocalizer from a HardwareMap, with a starting Pose at (0,0) + * facing 0 heading. + * + * @param map the HardwareMap + */ + public PinpointLocalizer(HardwareMap map){ this(map, new Pose());} + + /** + * This creates a new PinpointLocalizer from a HardwareMap and a Pose, with the Pose + * specifying the starting pose of the localizer. + * + * @param map the HardwareMap + * @param setStartPose the Pose to start from + */ + public PinpointLocalizer(HardwareMap map, Pose setStartPose){ + hardwareMap = map; + + odo = hardwareMap.get(GoBildaPinpointDriver.class,"pinpoint"); + + //The default units are inches, but you can swap the units if you wish. + //If you have already tuned the TwoWheelLocalizer, you can simply use the forwardEncoderPose's y value and strafeEncoderPose's x values. + setOffsets(-2.815, 0.125, DistanceUnit.INCH); //these are tuned for 3110-0002-0001 Product Insight #1 + + //TODO: Tune urself if needed +// odo.setYawScalar(1.0); + + odo.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); + //odo.setEncoderResolution(13.26291192); + + odo.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.REVERSED, GoBildaPinpointDriver.EncoderDirection.FORWARD); + + resetPinpoint(); + + setStartPose(setStartPose); + totalHeading = 0; + timer = new NanoTimer(); + previousPinpointPose = new Pose(); + currentVelocity = new Pose(); + deltaTimeNano = 1; + previousHeading = setStartPose.getHeading(); + } + + /** + * This returns the current pose estimate. + * + * @return returns the current pose estimate as a Pose + */ + @Override + public Pose getPose() { + return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(previousPinpointPose, startPose.getHeading(), false)); + } + + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Pose + */ + @Override + public Pose getVelocity() { + return currentVelocity.copy(); + } + + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Vector + */ + @Override + public Vector getVelocityVector() { + return currentVelocity.getVector(); + } + + /** + * This sets the start pose. Since nobody should be using this after the robot has begun moving, + * and due to issues with the PinpointLocalizer, this is functionally the same as setPose(Pose). + * + * @param setStart the new start pose + */ + @Override + public void setStartPose(Pose setStart) { + this.startPose = setStart; + } + + /** + * This sets the current pose estimate. Changing this should just change the robot's current + * pose estimate, not anything to do with the start pose. + * + * @param setPose the new current pose estimate + */ + @Override + public void setPose(Pose setPose) { + Pose setNewPose = MathFunctions.subtractPoses(setPose, startPose); + odo.setPosition(new Pose2D(DistanceUnit.INCH, setNewPose.getX(), setNewPose.getY(), AngleUnit.RADIANS, setNewPose.getHeading())); + } + + /** + * This updates the total heading of the robot. The Pinpoint handles all other updates itself. + */ + @Override + public void update() { + deltaTimeNano = timer.getElapsedTime(); + timer.resetTimer(); + odo.update(); + Pose2D pinpointPose = odo.getPosition(); + Pose currentPinpointPose = new Pose(pinpointPose.getX(DistanceUnit.INCH), pinpointPose.getY(DistanceUnit.INCH), pinpointPose.getHeading(AngleUnit.RADIANS)); + totalHeading += MathFunctions.getSmallestAngleDifference(currentPinpointPose.getHeading(), previousHeading); + previousHeading = currentPinpointPose.getHeading(); + Pose deltaPose = MathFunctions.subtractPoses(currentPinpointPose, previousPinpointPose); + currentVelocity = new Pose(deltaPose.getX() / (deltaTimeNano / Math.pow(10.0, 9)), deltaPose.getY() / (deltaTimeNano / Math.pow(10.0, 9)), deltaPose.getHeading() / (deltaTimeNano / Math.pow(10.0, 9))); + previousPinpointPose = currentPinpointPose; + } + + /** + * This returns how far the robot has turned in radians, in a number not clamped between 0 and + * 2 * pi radians. This is used for some tuning things and nothing actually within the following. + * + * @return returns how far the robot has turned in total, in radians. + */ + @Override + public double getTotalHeading() { + return totalHeading; + } + + /** + * This returns the Y encoder value as none of the odometry tuners are required for this localizer + * @return returns the Y encoder value + */ + @Override + public double getForwardMultiplier() { + return odo.getEncoderY(); + } + + /** + * This returns the X encoder value as none of the odometry tuners are required for this localizer + * @return returns the X encoder value + */ + @Override + public double getLateralMultiplier() { + return odo.getEncoderX(); + } + + /** + * This returns either the factory tuned yaw scalar or the yaw scalar tuned by yourself. + * @return returns the yaw scalar + */ + @Override + public double getTurningMultiplier() { + return odo.getYawScalar(); + } + + /** + * This sets the offsets and converts inches to millimeters + * @param xOffset How far to the side from the center of the robot is the x-pod? Use positive values if it's to the left and negative if it's to the right. + * @param yOffset How far forward from the center of the robot is the y-pod? Use positive values if it's forward and negative if it's to the back. + * @param unit The units that the measurements are given in + */ + private void setOffsets(double xOffset, double yOffset, DistanceUnit unit) { + odo.setOffsets(unit.toMm(xOffset), unit.toMm(yOffset)); + } + + /** + * This resets the IMU. Does not change heading estimation. + */ + @Override + public void resetIMU() throws InterruptedException { + odo.recalibrateIMU(); + + try { + Thread.sleep(300); + } catch (InterruptedException e) { + throw new RuntimeException(e); + } + } + + /** + * This resets the pinpoint. + */ + private void resetPinpoint() { + odo.resetPosAndIMU(); + + try { + Thread.sleep(300); + } catch (InterruptedException e) { + throw new RuntimeException(e); + } + } +} From 75a2e31f57ad72a420cc3f0a3a3bd35892b04e31 Mon Sep 17 00:00:00 2001 From: Havish Sripada <139065303+junkjunk123@users.noreply.github.com> Date: Sun, 22 Dec 2024 18:53:02 -0800 Subject: [PATCH 92/94] Update PinpointLocalizer.java --- .../pedroPathing/localization/localizers/PinpointLocalizer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java index d3d3fda..32bdc05 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java @@ -233,7 +233,7 @@ public class PinpointLocalizer extends Localizer { /** * This resets the pinpoint. */ - private void resetPinpoint() { + private void resetPinpoint() throws InterruptedException { odo.resetPosAndIMU(); try { From 11bad4df76f8f95c1ddfd7f32f5ff9b14cc1fd2b Mon Sep 17 00:00:00 2001 From: Havish Sripada <139065303+junkjunk123@users.noreply.github.com> Date: Sun, 22 Dec 2024 18:56:55 -0800 Subject: [PATCH 93/94] Update PinpointLocalizer.java --- .../pedroPathing/localization/localizers/PinpointLocalizer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java index 32bdc05..d3d3fda 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java @@ -233,7 +233,7 @@ public class PinpointLocalizer extends Localizer { /** * This resets the pinpoint. */ - private void resetPinpoint() throws InterruptedException { + private void resetPinpoint() { odo.resetPosAndIMU(); try { From 81514f34b45532049044e0333fda5d4cf7ae4b36 Mon Sep 17 00:00:00 2001 From: Havish Sripada <139065303+junkjunk123@users.noreply.github.com> Date: Mon, 23 Dec 2024 09:35:30 -0800 Subject: [PATCH 94/94] Update Follower.java Made the follower optionally take in a second parameter for the localizer --- .../pedroPathing/follower/Follower.java | 53 +++++++++++++++++++ 1 file changed, 53 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index 0a45eb8..0970664 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -154,6 +154,16 @@ public class Follower { initialize(); } + /** + * This creates a new Follower given a HardwareMap and a localizer. + * @param hardwareMap HardwareMap required + * @param localizer the localizer you wish to use + */ + public Follower(HardwareMap hardwareMap, Localizer localizer) { + this.hardwareMap = hardwareMap; + initialize(localizer); + } + /** * This initializes the follower. * In this, the DriveVectorScaler and PoseUpdater is instantiated, the drive motors are @@ -190,6 +200,49 @@ public class Follower { breakFollowing(); } + /** + * This initializes the follower. + * In this, the DriveVectorScaler and PoseUpdater is instantiated, the drive motors are + * initialized and their behavior is set, and the variables involved in approximating first and + * second derivatives for teleop are set. + * @param localizer the localizer you wish to use + */ + + public void initialize(Localizer localizer) { + driveVectorScaler = new DriveVectorScaler(FollowerConstants.frontLeftVector); + poseUpdater = new PoseUpdater(hardwareMap, localizer); + + // rightFront = hardwareMap.get(DcMotorEx.class, "motor_rf"); + // rightRear = hardwareMap.get(DcMotorEx.class, "motor_rb"); + // leftFront = hardwareMap.get(DcMotorEx.class, "motor_lf"); + // leftRear = hardwareMap.get(DcMotorEx.class, "motor_lb"); + + leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName); + leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); + rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); + rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); + + // TODO: Make sure that this is the direction your motors need to be reversed in. + leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + leftRear.setDirection(DcMotorSimple.Direction.REVERSE); + + motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear); + + for (DcMotorEx motor : motors) { + MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); + motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motor.setMotorType(motorConfigurationType); + } + + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + } + + dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); + + breakFollowing(); + } + /** * This sets the maximum power the motors are allowed to use. *

+ * Note: This vector is clamped to be between [0, 1] in magnitude. + * + * @return returns the centripetal force correction vector. + */ + public Vector getCentripetalForceCorrection() { + if (!useCentripetal) return new Vector(); + double curvature; + if (auto) { + curvature = currentPath.getClosestPointCurvature(); + } else { + double yPrime = averageVelocity.getYComponent() / averageVelocity.getXComponent(); + double yDoublePrime = averageAcceleration.getYComponent() / averageVelocity.getXComponent(); + curvature = (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)) / (yDoublePrime); + } + if (Double.isNaN(curvature)) return new Vector(); + centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -1, 1), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta())); + return centripetalVector; + } + + /** + * This returns the closest pose to the robot on the Path the Follower is currently following. + * This closest pose is calculated through a binary search method with some specified number of + * steps to search. By default, 10 steps are used, which should be more than enough. + * + * @return returns the closest pose. + */ + public Pose2d getClosestPose() { + return closestPose; + } + + /** + * This sets whether or not the Follower is being used in auto or teleop. + * + * @param set sets auto or not + */ + public void setAuto(boolean set) { + auto = set; + } + + /** + * This returns whether the follower is at the parametric end of its current Path. + * The parametric end is determined by if the closest Point t-value is greater than some specified + * end t-value. + * If running a PathChain, this returns true only if at parametric end of last Path in the PathChain. + * + * @return returns whether the Follower is at the parametric end of its Path. + */ + public boolean atParametricEnd() { + if (followingPathChain) { + if (chainIndex == currentPathChain.size() - 1) return currentPath.isAtParametricEnd(); + return false; + } + return currentPath.isAtParametricEnd(); + } + + /** + * This returns the t value of the closest point on the current Path to the robot + * In the absence of a current Path, it returns 1.0. + * + * @return returns the current t value. + */ + public double getCurrentTValue() { + if (isBusy) return currentPath.getClosestPointTValue(); + return 1.0; + } + + /** + * This returns the current path number. For following Paths, this will return 0. For PathChains, + * this will return the current path number. For holding Points, this will also return 0. + * + * @return returns the current path number. + */ + public double getCurrentPathNumber() { + if (!followingPathChain) return 0; + return chainIndex; + } + + /** + * This returns a new PathBuilder object for easily building PathChains. + * + * @return returns a new PathBuilder object. + */ + public PathBuilder pathBuilder() { + return new PathBuilder(); + } + + /** + * This writes out information about the various motion Vectors to the Telemetry specified. + * + * @param telemetry this is an instance of Telemetry or the FTC Dashboard telemetry that this + * method will use to output the debug data. + */ + public void telemetryDebug(MultipleTelemetry telemetry) { + telemetry.addData("follower busy", isBusy()); + telemetry.addData("heading error", headingError); + telemetry.addData("heading vector magnitude", headingVector.getMagnitude()); + telemetry.addData("corrective vector magnitude", correctiveVector.getMagnitude()); + telemetry.addData("corrective vector heading", correctiveVector.getTheta()); + telemetry.addData("translational error magnitude", getTranslationalError().getMagnitude()); + telemetry.addData("translational error direction", getTranslationalError().getTheta()); + telemetry.addData("translational vector magnitude", translationalVector.getMagnitude()); + telemetry.addData("translational vector heading", translationalVector.getMagnitude()); + telemetry.addData("centripetal vector magnitude", centripetalVector.getMagnitude()); + telemetry.addData("centripetal vector heading", centripetalVector.getTheta()); + telemetry.addData("drive error", driveError); + telemetry.addData("drive vector magnitude", driveVector.getMagnitude()); + telemetry.addData("drive vector heading", driveVector.getTheta()); + telemetry.addData("x", getPose().getX()); + telemetry.addData("y", getPose().getY()); + telemetry.addData("heading", getPose().getHeading()); + telemetry.addData("velocity magnitude", getVelocity().getMagnitude()); + telemetry.addData("velocity heading", getVelocity().getTheta()); + telemetry.update(); + } + + /** + * This writes out information about the various motion Vectors to the Telemetry specified. + * + * @param telemetry this is an instance of Telemetry or the FTC Dashboard telemetry that this + * method will use to output the debug data. + */ + public void telemetryDebug(Telemetry telemetry) { + telemetryDebug(new MultipleTelemetry(telemetry)); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java new file mode 100644 index 0000000..d8d9b32 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java @@ -0,0 +1,313 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +import java.util.ArrayList; +import java.util.List; + +/** + * This is the PoseUpdater class. This class handles getting pose data from the localizer and returning + * the information in a useful way to the Follower. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/4/2024 + */ +public class PoseUpdater { + private HardwareMap hardwareMap; + + private IMU imu; + + private ThreeWheelLocalizer localizer; + + private Pose2d startingPose = new Pose2d(0,0,0); + + private Pose2d currentPose = startingPose; + + private Pose2d previousPose = startingPose; + + private Vector currentVelocity = new Vector(); + + private Vector previousVelocity = new Vector(); + + private Vector currentAcceleration = new Vector(); + + private double xOffset = 0; + private double yOffset = 0; + private double headingOffset = 0; + + private long previousPoseTime; + private long currentPoseTime; + + /** + * Creates a new PoseUpdater from a HardwareMap. + * + * @param hardwareMap the HardwareMap + */ + public PoseUpdater(HardwareMap hardwareMap) { + this.hardwareMap = hardwareMap; + + for (LynxModule module : hardwareMap.getAll(LynxModule.class)) { + module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); + } + + List lastTrackingEncPositions = new ArrayList<>(); + List lastTrackingEncVels = new ArrayList<>(); + localizer = new ThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels); + } + + /** + * This updates the robot's pose, as well as updating the previous pose, velocity, and + * acceleration. The cache for the current pose, velocity, and acceleration is cleared, and + * the time stamps are updated as well. + */ + public void update() { + previousVelocity = getVelocity(); + previousPose = applyOffset(getRawPose()); + currentPose = null; + currentVelocity = null; + currentAcceleration = null; + previousPoseTime = currentPoseTime; + currentPoseTime = System.nanoTime(); + localizer.update(); + } + + /** + * This sets the starting pose. Do not run this after moving at all. + * + * @param set the Pose to set the starting pose to. + */ + public void setStartingPose(Pose2d set) { + startingPose = set; + previousPose = startingPose; + previousPoseTime = System.nanoTime(); + currentPoseTime = System.nanoTime(); + localizer.setPoseEstimate(set); + } + + /** + * This sets the current pose, using offsets so no reset time delay. This is better than the + * Road Runner reset, in general. Think of using offsets as setting trim in an aircraft. This can + * be reset as well, so beware of using the resetOffset() method. + * + * @param set The pose to set the current pose to. + */ + public void setCurrentPoseWithOffset(Pose2d set) { + Pose2d currentPose = getRawPose(); + setXOffset(set.getX() - currentPose.getX()); + setYOffset(set.getY() - currentPose.getY()); + setHeadingOffset(MathFunctions.getTurnDirection(currentPose.getHeading(), set.getHeading()) * MathFunctions.getSmallestAngleDifference(currentPose.getHeading(), set.getHeading())); + } + + /** + * This sets the offset for only the x position. + * + * @param offset This sets the offset. + */ + public void setXOffset(double offset) { + xOffset = offset; + } + + /** + * This sets the offset for only the y position. + * + * @param offset This sets the offset. + */ + public void setYOffset(double offset) { + yOffset = offset; + } + + /** + * This sets the offset for only the heading. + * + * @param offset This sets the offset. + */ + public void setHeadingOffset(double offset) { + headingOffset = offset; + } + + /** + * This returns the x offset. + * + * @return returns the x offset. + */ + public double getXOffset() { + return xOffset; + } + + /** + * This returns the y offset. + * + * @return returns the y offset. + */ + public double getYOffset() { + return yOffset; + } + + /** + * This returns the heading offset. + * + * @return returns the heading offset. + */ + public double getHeadingOffset() { + return headingOffset; + } + + /** + * This applies the offset to a specified Pose. + * + * @param pose The pose to be offset. + * @return This returns a new Pose with the offset applied. + */ + public Pose2d applyOffset(Pose2d pose) { + return new Pose2d(pose.getX()+xOffset, pose.getY()+yOffset, pose.getHeading()+headingOffset); + } + + /** + * This resets all offsets set to the PoseUpdater. If you have reset your pose using the + * setCurrentPoseUsingOffset(Pose2d set) method, then your pose will be returned to what the + * PoseUpdater thinks your pose would be, not the pose you reset to. + */ + public void resetOffset() { + setXOffset(0); + setYOffset(0); + setHeadingOffset(0); + } + + /** + * This returns the current pose, with offsets applied. If this is called multiple times in + * a single update, the current pose is cached so that subsequent calls don't have to repeat + * localizer calls or calculations. + * + * @return returns the current pose. + */ + public Pose2d getPose() { + if (currentPose == null) { + currentPose = localizer.getPoseEstimate(); + return applyOffset(currentPose); + } else { + return applyOffset(currentPose); + } + } + + /** + * This returns the current raw pose, without any offsets applied. If this is called multiple times in + * a single update, the current pose is cached so that subsequent calls don't have to repeat + * localizer calls or calculations. + * + * @return returns the raw pose. + */ + public Pose2d getRawPose() { + if (currentPose == null) { + currentPose = localizer.getPoseEstimate(); + return currentPose; + } else { + return currentPose; + } + } + + /** + * This sets the current pose using the Road Runner pose reset. This is slow. + * + * @param set the pose to set the current pose to. + */ + public void setPose(Pose2d set) { + resetOffset(); + localizer.setPoseEstimate(set); + } + + /** + * Returns the robot's pose from the previous update. + * + * @return returns the robot's previous pose. + */ + public Pose2d getPreviousPose() { + return previousPose; + } + + /** + * Returns the robot's change in pose from the previous update. + * + * @return returns the robot's delta pose. + */ + public Pose2d getDeltaPose() { + return getPose().minus(previousPose); + } + + /** + * This returns the velocity of the robot as a Vector. If this is called multiple times in + * a single update, the velocity Vector is cached so that subsequent calls don't have to repeat + * localizer calls or calculations. + * + * @return returns the velocity of the robot. + */ + public Vector getVelocity() { + if (currentVelocity == null) { + currentVelocity = new Vector(); + currentVelocity.setOrthogonalComponents(getPose().getX() - previousPose.getX(), getPose().getY() - previousPose.getY()); + currentVelocity.setMagnitude(MathFunctions.distance(getPose(), previousPose) / ((currentPoseTime - previousPoseTime) / Math.pow(10.0, 9))); + return MathFunctions.copyVector(currentVelocity); + } else { + return MathFunctions.copyVector(currentVelocity); + } + } + + /** + * This returns the angular velocity of the robot as a double. + * + * @return returns the angular velocity of the robot. + */ + public double getAngularVelocity() { + return MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(getPose().getHeading(), previousPose.getHeading()) / ((currentPoseTime-previousPoseTime)/Math.pow(10.0, 9)); + } + + /** + * This returns the acceleration of the robot as a Vector. If this is called multiple times in + * a single update, the acceleration Vector is cached so that subsequent calls don't have to + * repeat localizer calls or calculations. + * + * @return returns the acceleration of the robot. + */ + public Vector getAcceleration() { + if (currentAcceleration == null) { + currentAcceleration = MathFunctions.subtractVectors(getVelocity(), previousVelocity); + currentAcceleration.setMagnitude(currentAcceleration.getMagnitude() / ((currentPoseTime - previousPoseTime) / Math.pow(10.0, 9))); + return MathFunctions.copyVector(currentAcceleration); + } else { + return MathFunctions.copyVector(currentAcceleration); + } + } + + /** + * This resets the heading of the robot to the IMU's heading, using Road Runner's pose reset. + */ + public void resetHeadingToIMU() { + localizer.resetHeading(getNormalizedIMUHeading() + startingPose.getHeading()); + } + + /** + * This resets the heading of the robot to the IMU's heading, using offsets instead of Road + * Runner's pose reset. This way, it's faster, but this can be wiped with the resetOffsets() + * method. + */ + public void resetHeadingToIMUWithOffsets() { + setCurrentPoseWithOffset(new Pose2d(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading())); + } + + /** + * This returns the IMU heading normalized to be between [0, 2 PI] radians. + * + * @return returns the normalized IMU heading. + */ + public double getNormalizedIMUHeading() { + return MathFunctions.normalizeAngle(-imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java new file mode 100644 index 0000000..edbe337 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java @@ -0,0 +1,125 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +/** + * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding + * slot's motor direction + */ +public class RoadRunnerEncoder { + private final static int CPS_STEP = 0x10000; + + private static double inverseOverflow(double input, double estimate) { + // convert to uint16 + int real = (int) input & 0xffff; + // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits + // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window + real += ((real % 20) / 4) * CPS_STEP; + // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by + real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP; + return real; + } + + public enum Direction { + FORWARD(1), + REVERSE(-1); + + private int multiplier; + + Direction(int multiplier) { + this.multiplier = multiplier; + } + + public int getMultiplier() { + return multiplier; + } + } + + private DcMotorEx motor; + private NanoClock clock; + + private Direction direction; + + private int lastPosition; + private int velocityEstimateIdx; + private double[] velocityEstimates; + private double lastUpdateTime; + + public RoadRunnerEncoder(DcMotorEx motor, NanoClock clock) { + this.motor = motor; + this.clock = clock; + + this.direction = Direction.FORWARD; + + this.lastPosition = 0; + this.velocityEstimates = new double[3]; + this.lastUpdateTime = clock.seconds(); + } + + public RoadRunnerEncoder(DcMotorEx motor) { + this(motor, NanoClock.system()); + } + + public Direction getDirection() { + return direction; + } + + private int getMultiplier() { + return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1); + } + + /** + * Allows you to set the direction of the counts and velocity without modifying the motor's direction state + * @param direction either reverse or forward depending on if encoder counts should be negated + */ + public void setDirection(Direction direction) { + this.direction = direction; + } + + /** + * Gets the position from the underlying motor and adjusts for the set direction. + * Additionally, this method updates the velocity estimates used for compensated velocity + * + * @return encoder position + */ + public int getCurrentPosition() { + int multiplier = getMultiplier(); + int currentPosition = motor.getCurrentPosition() * multiplier; + if (currentPosition != lastPosition) { + double currentTime = clock.seconds(); + double dt = currentTime - lastUpdateTime; + velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt; + velocityEstimateIdx = (velocityEstimateIdx + 1) % 3; + lastPosition = currentPosition; + lastUpdateTime = currentTime; + } + return currentPosition; + } + + /** + * Gets the velocity directly from the underlying motor and compensates for the direction + * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore) + * + * @return raw velocity + */ + public double getRawVelocity() { + int multiplier = getMultiplier(); + return motor.getVelocity() * multiplier; + } + + /** + * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity + * that are lost in overflow due to velocity being transmitted as 16 bits. + * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly. + * + * @return corrected velocity + */ + public double getCorrectedVelocity() { + double median = velocityEstimates[0] > velocityEstimates[1] + ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2])) + : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2])); + return inverseOverflow(getRawVelocity(), median); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java new file mode 100644 index 0000000..88f1e7c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java @@ -0,0 +1,117 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import java.util.Arrays; +import java.util.List; + +/* + * Sample tracking wheel localizer implementation assuming the standard configuration: + * + * left on robot is y pos + * + * front of robot is x pos + * + * /--------------\ + * | ____ | + * | ---- | + * | || || | + * | || || | + * | | + * | | + * \--------------/ + * + */ + +/** + * This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will + * be replaced with a custom localizer. + */ +@Config +public class ThreeWheelLocalizer extends ThreeTrackingWheelLocalizer { + public static double TICKS_PER_REV = 8192; + public static double WHEEL_RADIUS = 1.37795; // in + public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed + + public static double X_MULTIPLIER = 0.5008239963; + public static double Y_MULTIPLIER = 0.5018874659; + + public static double leftX = -18.5/25.4 - 0.1, leftY = 164.4/25.4, rightX = -18.4/25.4 - 0.1, rightY = -159.6/25.4, strafeX = -107.9/25.4+0.25, strafeY = -1.1/25.4-0.23; + + private RoadRunnerEncoder leftEncoder, rightEncoder, strafeEncoder; + + private List lastEncPositions, lastEncVels; + + public ThreeWheelLocalizer(HardwareMap hardwareMap, List lastTrackingEncPositions, List lastTrackingEncVels) { + super(Arrays.asList( + new Pose2d(leftX, leftY, 0), // left + new Pose2d(rightX, rightY, 0), // right + new Pose2d(strafeX, strafeY, Math.toRadians(90)) // strafe + )); + + lastEncPositions = lastTrackingEncPositions; + lastEncVels = lastTrackingEncVels; + + // TODO: redo the configs here + leftEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "leftRear")); + rightEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "rightFront")); + strafeEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); + + // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) + leftEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); + rightEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); + strafeEncoder.setDirection(RoadRunnerEncoder.Direction.FORWARD); + } + + public void resetHeading(double heading) { + setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading)); + } + + public static double encoderTicksToInches(double ticks) { + return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; + } + + @NonNull + @Override + public List getWheelPositions() { + int leftPos = leftEncoder.getCurrentPosition(); + int rightPos = rightEncoder.getCurrentPosition(); + int frontPos = strafeEncoder.getCurrentPosition(); + + lastEncPositions.clear(); + lastEncPositions.add(leftPos); + lastEncPositions.add(rightPos); + lastEncPositions.add(frontPos); + + return Arrays.asList( + encoderTicksToInches(leftPos) * X_MULTIPLIER, + encoderTicksToInches(rightPos) * X_MULTIPLIER, + encoderTicksToInches(frontPos) * Y_MULTIPLIER + ); + } + + @NonNull + @Override + public List getWheelVelocities() { + int leftVel = (int) leftEncoder.getCorrectedVelocity(); + int rightVel = (int) rightEncoder.getCorrectedVelocity(); + int frontVel = (int) strafeEncoder.getCorrectedVelocity(); + + lastEncVels.clear(); + lastEncVels.add(leftVel); + lastEncVels.add(rightVel); + lastEncVels.add(frontVel); + + return Arrays.asList( + encoderTicksToInches(leftVel) * X_MULTIPLIER, + encoderTicksToInches(rightVel) * X_MULTIPLIER, + encoderTicksToInches(frontVel) * Y_MULTIPLIER + ); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java new file mode 100644 index 0000000..450da65 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java @@ -0,0 +1,317 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; + + +import org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants; + +import java.util.ArrayList; + +/** + * This is the BezierCurve class. This class handles the creation of Bezier curves, which are used + * as the basis of the path for the Path class. Bezier curves are parametric curves defined by a set + * of control points. So, they take in one input, t, that ranges from [0, 1] and that returns a point + * on the curve. Essentially, Bezier curves are a way of defining a parametric line easily. You can + * read more on Bezier curves here: https://en.wikipedia.org/wiki/Bézier_curve + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/5/2024 + */ +public class BezierCurve { + // This contains the coefficients for the curve points + private ArrayList pointCoefficients = new ArrayList<>(); + + // This contains the control points for the Bezier curve + private ArrayList controlPoints = new ArrayList<>(); + + private Vector endTangent = new Vector(); + + private final int APPROXIMATION_STEPS = FollowerConstants.APPROXIMATION_STEPS; + + private double UNIT_TO_TIME; + private double length; + + /** + * This creates an empty BezierCurve. + * IMPORTANT NOTE: Only use this for the constructors of classes extending this. If you try to + * run the robot on a Path containing an empty BezierCurve, then it will explode. + */ + public BezierCurve() { + } + + /** + * This creates a new BezierCurve with an ArrayList of control points and generates the curve. + * IMPORTANT NOTE: The order of the control points is important. That's the order the code will + * process them in, with the 0 index being the start point and the final index being the end point + * + * @param controlPoints This is the ArrayList of control points that define the BezierCurve. + */ + public BezierCurve(ArrayList controlPoints) { + if (controlPoints.size()<3) { + try { + throw new Exception("Too few control points"); + } catch (Exception e) { + e.printStackTrace(); + } + } + this.controlPoints = controlPoints; + generateBezierCurve(); + length = approximateLength(); + UNIT_TO_TIME = 1/length; + endTangent.setOrthogonalComponents(controlPoints.get(controlPoints.size()-1).getX()-controlPoints.get(controlPoints.size()-2).getX(), controlPoints.get(controlPoints.size()-1).getY()-controlPoints.get(controlPoints.size()-2).getY()); + endTangent = MathFunctions.normalizeVector(endTangent); + } + + /** + * This creates a new Bezier curve with some specified control points and generates the curve. + * IMPORTANT NOTE: The order of the control points is important. That's the order the code will + * process them in, with the 0 index being the start point and the final index being the end point. + * + * @param controlPoints This is the specified control points that define the BezierCurve. + */ + public BezierCurve(Point... controlPoints) { + for (Point controlPoint : controlPoints) { + this.controlPoints.add(controlPoint); + } + if (this.controlPoints.size()<3) { + try { + throw new Exception("Too few control points"); + } catch (Exception e) { + e.printStackTrace(); + } + } + generateBezierCurve(); + length = approximateLength(); + UNIT_TO_TIME = 1/length; + endTangent.setOrthogonalComponents(this.controlPoints.get(this.controlPoints.size()-1).getX()-this.controlPoints.get(this.controlPoints.size()-2).getX(), this.controlPoints.get(this.controlPoints.size()-1).getY()-this.controlPoints.get(this.controlPoints.size()-2).getY()); + endTangent = MathFunctions.normalizeVector(endTangent); + } + + /** + * This generates the Bezier curve. It assumes that the ArrayList of control points has been set. + * Well, this actually generates the coefficients for each control point on the Bezier curve. + * These coefficients can then be used to calculate a position, velocity, or accleration on the + * Bezier curve on the fly without much computational expense. + * + * See https://en.wikipedia.org/wiki/Bézier_curve for the explicit formula for Bezier curves + */ + public void generateBezierCurve() { + int n = controlPoints.size()-1; + for (int i = 0; i <= n; i++) { + pointCoefficients.add(new BezierCurveCoefficients(n, i)); + } + } + + /** + * This returns the unit tangent Vector at the end of the BezierCurve. + * + * @return returns the end tangent Vector. + */ + public Vector getEndTangent() { + return MathFunctions.copyVector(endTangent); + } + + /** + * This approximates the length of the BezierCurve in APPROXIMATION_STEPS number of steps. It's + * like a Riemann's sum, but for a parametric function's arc length. + * + * @return returns the approximated length of the BezierCurve. + */ + public double approximateLength() { + Point previousPoint = getPoint(0); + Point currentPoint; + double approxLength = 0; + for (int i = 1; i <= APPROXIMATION_STEPS; i++) { + currentPoint = getPoint(i/(double)APPROXIMATION_STEPS); + approxLength += previousPoint.distanceFrom(currentPoint); + previousPoint = currentPoint; + } + return approxLength; + } + + /** + * This returns the point on the Bezier curve that is specified by the parametric t value. A + * Bezier curve is a parametric function that returns points along it with t ranging from [0, 1], + * with 0 being the beginning of the curve and 1 being at the end. The Follower will follow + * BezierCurves from 0 to 1, in terms of t. + * + * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. + * @return this returns the point requested. + */ + public Point getPoint(double t) { + t = MathFunctions.clamp(t, 0, 1); + double xCoordinate = 0; + double yCoordinate = 0; + + // calculates the x coordinate of the point requested + for (int i = 0; i < controlPoints.size(); i++) { + xCoordinate += pointCoefficients.get(i).getValue(t) * controlPoints.get(i).getX(); + } + + // calculates the y coordinate of the point requested + for (int i = 0; i < controlPoints.size(); i++) { + yCoordinate += pointCoefficients.get(i).getValue(t) * controlPoints.get(i).getY(); + } + return new Point(xCoordinate, yCoordinate, Point.CARTESIAN); + } + + /** + * This returns the curvature of the Bezier curve at a specified t-value. + * + * @param t the parametric t input. + * @return returns the curvature. + */ + public double getCurvature(double t) { + t = MathFunctions.clamp(t, 0, 1); + Vector derivative = getDerivative(t); + Vector secondDerivative = new Vector(getSecondDerivative(t).getMagnitude(), getApproxSecondDerivative(t).getTheta()); + + if (derivative.getMagnitude() == 0) return 0; + return (MathFunctions.crossProduct(derivative, secondDerivative))/Math.pow(derivative.getMagnitude(),3); + } + + /** + * This returns the derivative on the BezierCurve that is specified by the parametric t value. + * This is returned as a Vector, and this Vector is the tangent to the BezierCurve. + * + * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. + * @return this returns the derivative requested. + */ + public Vector getDerivative(double t) { + t = MathFunctions.clamp(t, 0, 1); + double xCoordinate = 0; + double yCoordinate = 0; + Vector returnVector = new Vector(); + + // calculates the x coordinate of the point requested + for (int i = 0; i < controlPoints.size()-1; i++) { + xCoordinate += pointCoefficients.get(i).getDerivativeValue(t) * (MathFunctions.subtractPoints(controlPoints.get(i+1), controlPoints.get(i)).getX()); + } + + // calculates the y coordinate of the point requested + for (int i = 0; i < controlPoints.size()-1; i++) {; + yCoordinate += pointCoefficients.get(i).getDerivativeValue(t) * (MathFunctions.subtractPoints(controlPoints.get(i+1), controlPoints.get(i)).getY()); + } + + returnVector.setOrthogonalComponents(xCoordinate, yCoordinate); + + return returnVector; + } + + /** + * This returns the second derivative on the BezierCurve that is specified by the parametric t value. + * This is returned as a Vector, and this Vector is the acceleration on the BezierCurve. + * + * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. + * @return this returns the second derivative requested. + */ + public Vector getSecondDerivative(double t) { + t = MathFunctions.clamp(t, 0, 1); + double xCoordinate = 0; + double yCoordinate = 0; + Vector returnVector = new Vector(); + + // calculates the x coordinate of the point requested + for (int i = 0; i < controlPoints.size()-2; i++) { + xCoordinate += pointCoefficients.get(i).getSecondDerivativeValue(t) * (MathFunctions.addPoints(MathFunctions.subtractPoints(controlPoints.get(i+2), new Point(2*controlPoints.get(i+1).getX(), 2*controlPoints.get(i+1).getY(), Point.CARTESIAN)), controlPoints.get(i)).getX()); + } + + // calculates the y coordinate of the point requested + for (int i = 0; i < controlPoints.size()-2; i++) { + yCoordinate += pointCoefficients.get(i).getSecondDerivativeValue(t) * (MathFunctions.addPoints(MathFunctions.subtractPoints(controlPoints.get(i+2), new Point(2*controlPoints.get(i+1).getX(), 2*controlPoints.get(i+1).getY(), Point.CARTESIAN)), controlPoints.get(i)).getY()); + } + + returnVector.setOrthogonalComponents(xCoordinate, yCoordinate); + + return returnVector; + } + + /** + * Because, for whatever reason, the second derivative returned by the getSecondDerivative(double t) + * method doesn't return the correct heading of the second derivative, this gets an approximate + * second derivative essentially using the limit method. I use this for its heading only. + * + * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. + * @return this returns the approximated second derivative. + */ + public Vector getApproxSecondDerivative(double t) { + double current = getDerivative(t).getTheta(); + double deltaCurrent = getDerivative(t + 0.0001).getTheta(); + + return new Vector(1, deltaCurrent - current); + } + + /** + * Returns the ArrayList of control points for this BezierCurve. + * + * @return This returns the control points. + */ + public ArrayList getControlPoints() { + return controlPoints; + } + + /** + * Returns the first control point for this BezierCurve. + * + * @return This returns the Point. + */ + public Point getFirstControlPoint() { + return controlPoints.get(0); + } + + /** + * Returns the second control point, or the one after the start, for this BezierCurve. + * + * @return This returns the Point. + */ + public Point getSecondControlPoint() { + return controlPoints.get(1); + } + + /** + * Returns the second to last control point for this BezierCurve. + * + * @return This returns the Point. + */ + public Point getSecondToLastControlPoint() { + return controlPoints.get(controlPoints.size()-2); + } + + /** + * Returns the last control point for this BezierCurve. + * + * @return This returns the Point. + */ + public Point getLastControlPoint() { + return controlPoints.get(controlPoints.size()-1); + } + + /** + * Returns the approximate length of this BezierCurve. + * + * @return This returns the length. + */ + public double length() { + return length; + } + + /** + * Returns the conversion factor of one unit of distance into t-value. Since parametric functions + * are defined by t, which can mean time, I use "time" in some method names for conciseness. + * + * @return returns the conversion factor. + */ + public double UNIT_TO_TIME() { + return UNIT_TO_TIME; + } + + /** + * Returns the type of path. This is used in case we need to identify the type of BezierCurve + * this is. + * + * @return returns the type of path. + */ + public String pathType() { + return "curve"; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurveCoefficients.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurveCoefficients.java new file mode 100644 index 0000000..f8fc51d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurveCoefficients.java @@ -0,0 +1,66 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; + +/** + * This is the BezierCurveCoefficients class. This class handles holding the coefficients for each + * control point for the BezierCurve class to allow for faster on the fly calculations of points, + * derivatives, and second derivatives on Bezier curves. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/8/2024 + */ +public class BezierCurveCoefficients { + private double coefficient; + private double derivativeCoefficient; + private double secondDerivativeCoefficient; + + private int n; + private int i; + + /** + * This creates the coefficients within the summation equations for calculating positions, + * derivatives, and second derivatives on Bezier curves. + * + * @param n this is the degree of the Bezier curve function. + * @param i this is the i within the summation equation, so basically which place it is in the + * summation. + */ + public BezierCurveCoefficients(int n, int i) { + this.n = n; + this.i = i; + coefficient = MathFunctions.nCr(n, i); + derivativeCoefficient = MathFunctions.nCr(n - 1, i); + secondDerivativeCoefficient = MathFunctions.nCr(n - 2, i); + } + + /** + * This returns the coefficient for the summation to calculate a position on BezierCurves. + * + * @param t this is the t value within the parametric equation for the Bezier curve. + * @return this returns the coefficient. + */ + public double getValue(double t) { + return coefficient * Math.pow(1 - t, n - i) * Math.pow(t, i); + } + + /** + * This returns the coefficient for the summation to calculate a derivative on BezierCurves. + * + * @param t this is the t value within the parametric equation for the Bezier curve. + * @return this returns the coefficient. + */ + public double getDerivativeValue(double t) { + return n * derivativeCoefficient * Math.pow(t, i) * Math.pow(1 - t, n - i - 1); + } + + /** + * This returns the coefficient for the summation to calculate a second derivative on BezierCurves. + * + * @param t this is the t value within the parametric equation for the Bezier curve. + * @return this returns the coefficient. + */ + public double getSecondDerivativeValue(double t) { + return n * (n - 1) * secondDerivativeCoefficient * Math.pow(t, i) * Math.pow(1 - t, n - i - 2); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierLine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierLine.java new file mode 100644 index 0000000..c2a2c52 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierLine.java @@ -0,0 +1,208 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; + +import java.util.ArrayList; + +/** + * This is the BezierLine class. This class handles the creation of BezierLines, which is what I + * call Bezier curves with only two control points. The parent BezierCurve class cannot handle Bezier + * curves with less than three control points, so this class handles lines. Additionally, it makes + * the calculations done on the fly a little less computationally expensive and more streamlined. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/9/2024 + */ +public class BezierLine extends BezierCurve { + + private Point startPoint; + private Point endPoint; + + private Vector endTangent; + + private double UNIT_TO_TIME; + private double length; + + /** + * This creates a new BezierLine with specified start and end Points. + * This is just a line but it extends the BezierCurve class so things work. + * + * @param startPoint start point of the line. + * @param endPoint end point of the line. + */ + public BezierLine(Point startPoint, Point endPoint) { + super(); + this.startPoint = startPoint; + this.endPoint = endPoint; + length = approximateLength(); + UNIT_TO_TIME = 1 / length; + endTangent = MathFunctions.normalizeVector(getDerivative(1)); + } + + /** + * This returns the unit tangent Vector at the end of the BezierLine. + * + * @return returns the tangent Vector. + */ + @Override + public Vector getEndTangent() { + return MathFunctions.copyVector(endTangent); + } + + /** + * This gets the length of the BezierLine. + * + * @return returns the length of the BezierLine. + */ + @Override + public double approximateLength() { + return Math.sqrt(Math.pow(startPoint.getX() - endPoint.getX(), 2) + Math.pow(startPoint.getY() - endPoint.getY(), 2)); + } + + /** + * This returns the Point on the Bezier line that is specified by the parametric t value. + * + * @param t this is the t value of the parametric line. t is clamped to be between 0 and 1 inclusive. + * @return this returns the Point requested. + */ + @Override + public Point getPoint(double t) { + t = MathFunctions.clamp(t, 0, 1); + return new Point((endPoint.getX() - startPoint.getX()) * t + startPoint.getX(), (endPoint.getY() - startPoint.getY()) * t + startPoint.getY(), Point.CARTESIAN); + } + + /** + * This returns the curvature of the BezierLine, which is zero. + * + * @param t the parametric t value. + * @return returns the curvature. + */ + @Override + public double getCurvature(double t) { + return 0.0; + } + + /** + * This returns the derivative on the BezierLine as a Vector, which is a constant slope. + * The t value doesn't really do anything, but it's there so I can override methods. + * + * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. + * @return this returns the derivative requested. + */ + @Override + public Vector getDerivative(double t) { + Vector returnVector = new Vector(); + + returnVector.setOrthogonalComponents(endPoint.getX() - startPoint.getX(), endPoint.getY() - startPoint.getY()); + + return returnVector; + } + + /** + * This returns the second derivative on the Bezier line, which is a zero Vector. + * Once again, the t is only there for the override. + * + * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. + * @return this returns the second derivative requested. + */ + @Override + public Vector getSecondDerivative(double t) { + return new Vector(); + } + + /** + * This returns the zero Vector, but it's here so I can override the method in the BezierCurve + * class. + * + * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. + * @return this returns the approximated second derivative, which is the zero Vector. + */ + @Override + public Vector getApproxSecondDerivative(double t) { + return new Vector(); + } + + /** + * Returns the ArrayList of control points for this BezierLine. + * + * @return This returns the control points. + */ + @Override + public ArrayList getControlPoints() { + ArrayList returnList = new ArrayList<>(); + returnList.add(startPoint); + returnList.add(endPoint); + return returnList; + } + + /** + * Returns the first control point for this BezierLine. + * + * @return This returns the Point. + */ + @Override + public Point getFirstControlPoint() { + return startPoint; + } + + /** + * Returns the second control point, or the one after the start, for this BezierLine. + * + * @return This returns the Point. + */ + @Override + public Point getSecondControlPoint() { + return endPoint; + } + + /** + * Returns the second to last control point for this BezierLine. + * + * @return This returns the Point. + */ + @Override + public Point getSecondToLastControlPoint() { + return startPoint; + } + + /** + * Returns the last control point for this BezierLine. + * + * @return This returns the Point. + */ + @Override + public Point getLastControlPoint() { + return endPoint; + } + + /** + * Returns the length of this BezierLine. + * + * @return This returns the length. + */ + @Override + public double length() { + return length; + } + + /** + * Returns the conversion factor of one unit of distance into t value. + * + * @return returns the conversion factor. + */ + @Override + public double UNIT_TO_TIME() { + return UNIT_TO_TIME; + } + + /** + * Returns the type of path. This is used in case we need to identify the type of BezierCurve + * this is. + * + * @return returns the type of path. + */ + @Override + public String pathType() { + return "line"; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierPoint.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierPoint.java new file mode 100644 index 0000000..b97700b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierPoint.java @@ -0,0 +1,208 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; + +import java.util.ArrayList; + +/** + * This is the BezierPoint class. This class handles the creation of BezierPoints, which is what I + * call Bezier curves with only one control point. The parent BezierCurve class cannot handle Bezier + * curves with less than three control points, so this class handles points. Additionally, it makes + * the calculations done on the fly a little less computationally expensive and more streamlined. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/9/2024 + */ +public class BezierPoint extends BezierCurve { + + private Point point; + + private Vector endTangent = new Vector(); + + private double UNIT_TO_TIME; + private double length; + + /** + * This creates a new BezierPoint with a specified Point. + * This is just a point but it extends the BezierCurve class so things work. + * + * @param point the specified point. + */ + public BezierPoint(Point point) { + super(); + this.point = point; + length = approximateLength(); + } + + /** + * This supposedly returns the unit tangent Vector at the end of the path, but since there is + * no end tangent of a point, this returns a zero Vector instead. Holding BezierPoints in the + * Follower doesn't use the drive Vector, so the end tangent Vector is not needed or truly used. + * + * @return returns the zero Vector. + */ + @Override + public Vector getEndTangent() { + return MathFunctions.copyVector(endTangent); + } + + /** + * This gets the length of the BezierPoint. Since points don't have length, this returns zero. + * + * @return returns the length of the BezierPoint. + */ + @Override + public double approximateLength() { + return 0.0; + } + + /** + * This returns the point on the BezierPoint that is specified by the parametric t value. Since + * this is a Point, this just returns the one control point's position. + * + * @param t this is the t value of the parametric line. t is clamped to be between 0 and 1 inclusive. + * @return this returns the Point requested. + */ + @Override + public Point getPoint(double t) { + return new Point(point.getX(), point.getY(), Point.CARTESIAN); + } + + /** + * This returns the curvature of the BezierPoint, which is zero since this is a Point. + * + * @param t the parametric t value. + * @return returns the curvature, which is zero. + */ + @Override + public double getCurvature(double t) { + return 0.0; + } + + /** + * This returns the derivative on the BezierPoint, which is the zero Vector since this is a Point. + * The t value doesn't really do anything, but it's there so I can override methods. + * + * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. + * @return this returns the derivative requested, which is the zero Vector. + */ + @Override + public Vector getDerivative(double t) { + return MathFunctions.copyVector(endTangent); + } + + /** + * This returns the second derivative on the Bezier line, which is the zero Vector since this + * is a Point. + * Once again, the t is only there for the override. + * + * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. + * @return this returns the second derivative requested, which is the zero Vector. + */ + @Override + public Vector getSecondDerivative(double t) { + return new Vector(); + } + + /** + * This returns the zero Vector, but it's here so I can override the method in the BezierCurve + * class. + * + * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. + * @return this returns the approximated second derivative, which is the zero Vector. + */ + @Override + public Vector getApproxSecondDerivative(double t) { + return new Vector(); + } + + /** + * Returns the ArrayList of control points for this BezierPoint + * + * @return This returns the control point. + */ + @Override + public ArrayList getControlPoints() { + ArrayList returnList = new ArrayList<>(); + returnList.add(point); + return returnList; + } + + /** + * Returns the first, and only, control point for this BezierPoint + * + * @return This returns the Point. + */ + @Override + public Point getFirstControlPoint() { + return point; + } + + /** + * Returns the second control point, or the one after the start, for this BezierPoint. This + * returns the one control point of the BezierPoint, and there are several redundant methods + * that return the same control point, but it's here so I can override methods. + * + * @return This returns the Point. + */ + @Override + public Point getSecondControlPoint() { + return point; + } + + /** + * Returns the second to last control point for this BezierPoint. This + * returns the one control point of the BezierPoint, and there are several redundant methods + * that return the same control point, but it's here so I can override methods. + * + * @return This returns the Point. + */ + @Override + public Point getSecondToLastControlPoint() { + return point; + } + + /** + * Returns the last control point for this BezierPoint. This + * returns the one control point of the BezierPoint, and there are several redundant methods + * that return the same control point, but it's here so I can override methods. + * + * @return This returns the Point. + */ + @Override + public Point getLastControlPoint() { + return point; + } + + /** + * Returns the length of this BezierPoint, which is zero since Points don't have length. + * + * @return This returns the length. + */ + @Override + public double length() { + return length; + } + + /** + * Returns the conversion factor of one unit of distance into t value. There is no length or + * conversion factor to speak of for Points. + * + * @return returns the conversion factor. + */ + @Override + public double UNIT_TO_TIME() { + return 0; + } + + /** + * Returns the type of path. This is used in case we need to identify the type of BezierCurve + * this is. + * + * @return returns the type of path. + */ + @Override + public String pathType() { + return "point"; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java new file mode 100644 index 0000000..b66193a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java @@ -0,0 +1,284 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; + +import com.acmerobotics.roadrunner.geometry.Pose2d; + +/** + * This is the MathFunctions class. This contains many useful math related methods that I use in + * other classes to simplify code elsewhere. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/9/2024 + */ +public class MathFunctions { + + /** + * This is a simple implementation of the choose function in math. It's equivalent to the number + * of ways you can choose r items from n total items, if only which items got picked and not the + * order of picking the items mattered. + * + * @param n this is how many you want to choose from. + * @param r this is how many you want to choose. + * @return returns the result of the "n choose r" function. + */ + public static double nCr(int n, int r) { + double num = 1; + double denom = 1; + + // this multiplies up the numerator of the nCr function + for (int i = n; i > n-r; i--) { + num *= i; + } + + // this multiplies up the denominator of the nCr function + for (int i = 1; i <=r; i++) { + denom *= i; + } + + return num/denom; + } + + /** + * This returns the sign (positive/negative) of a number. + * + * @param get the number. + * @return returns the sign of the number. + */ + public static double getSign(double get) { + if (get == 0) return 0; + if (get > 0) return 1; + return -1; + } + + /** + * This clamps down a value to between the lower and upper bounds inclusive. + * + * @param num the number to be clamped. + * @param lower the lower bound. + * @param upper the upper bound. + * @return returns the clamped number. + */ + public static double clamp(double num, double lower, double upper) { + if (num < lower) return lower; + if (num > upper) return upper; + return num; + } + + /** + * This normalizes an angle to be between 0 and 2 pi radians, inclusive. + * + * IMPORTANT NOTE: This method operates in radians. + * + * @param angleRadians the angle to be normalized. + * @return returns the normalized angle. + */ + public static double normalizeAngle(double angleRadians) { + double angle = angleRadians; + while (angle<0) angle += 2*Math.PI; + while (angle>2*Math.PI) angle -= 2*Math.PI; + return angle; + } + + /** + * This returns the smallest angle between two angles. This operates in radians. + * + * @param one one of the angles. + * @param two the other one. + * @return returns the smallest angle. + */ + public static double getSmallestAngleDifference(double one, double two) { + return Math.min(MathFunctions.normalizeAngle(one-two), MathFunctions.normalizeAngle(two-one)); + } + + /** + * This gets the direction to turn between a start heading and an end heading. Positive is left + * and negative is right. This operates in radians. + * + * @return returns the turn direction. + */ + public static double getTurnDirection(double startHeading, double endHeading) { + if (MathFunctions.normalizeAngle(endHeading-startHeading) >= 0 && MathFunctions.normalizeAngle(endHeading-startHeading) <= Math.PI) { + return 1; // counter clock wise + } + return -1; // clock wise + } + + /** + * This returns the distance between a Pose2d and a Point, + * + * @param pose this is the Pose2d. + * @param point this is the Point. + * @return returns the distance between the two. + */ + public static double distance(Pose2d pose, Point point) { + return Math.sqrt(Math.pow(pose.getX()-point.getX(), 2) + Math.pow(pose.getY()-point.getY(), 2)); + } + + /** + * This returns the distance between a Pose2d and another Pose2d. + * + * @param one this is the first Pose2d. + * @param two this is the second Pose2d. + * @return returns the distance between the two. + */ + public static double distance(Pose2d one, Pose2d two) { + return Math.sqrt(Math.pow(one.getX()-two.getX(), 2) + Math.pow(one.getY()-two.getY(), 2)); + } + + /** + * This returns a Point that is the sum of the two input Point. + * + * @param one the first Point + * @param two the second Point + * @return returns the sum of the two Points. + */ + public static Point addPoints(Point one, Point two) { + return new Point(one.getX() + two.getX(), one.getY() + two.getY(), Point.CARTESIAN); + } + + /** + * This subtracts the second Point from the first Point and returns the result as a Point. + * Do note that order matters here. + * + * @param one the first Point. + * @param two the second Point. + * @return returns the difference of the two Points. + */ + public static Point subtractPoints(Point one, Point two) { + return new Point(one.getX() - two.getX(), one.getY() - two.getY(), Point.CARTESIAN); + } + + /** + * This multiplies a Point by a scalar and returns the result as a Point + * + * @param point the Point being multiplied. + * @param scalar the scalar multiplying into the Point. + * @return returns the scaled Point. + */ + public static Point scalarMultiplyPoint(Point point, double scalar) { + return new Point(point.getX()*scalar, point.getY()*scalar, Point.CARTESIAN); + } + + /** + * Copies a Point, but with a different reference location in the memory. So basically a deep + * copy. + * + * @param point the Point to be deep copied. + * @return returns the copied Point. + */ + public static Point copyPoint(Point point) { + return new Point(point.getX(), point.getY(), Point.CARTESIAN); + } + + /** + * Copies a Vector, but with a different reference location in the memory. So basically a deep + * copy. + * + * @param vector Vector to be deep copied. + * @return returns the copied Vector. + */ + public static Vector copyVector(Vector vector) { + return new Vector(vector.getMagnitude(), vector.getTheta()); + } + + /** + * This multiplies a Vector by a scalar and returns the result as a Vector. + * + * @param vector the Vector being multiplied. + * @param scalar the scalar multiplying into the Vector. + * @return returns the scaled Vector. + */ + public static Vector scalarMultiplyVector(Vector vector, double scalar) { + return new Vector(vector.getMagnitude()*scalar, vector.getTheta()); + } + + /** + * This normalizes a Vector to be of magnitude 1, unless the Vector is the zero Vector. + * In that case, it just returns back the zero Vector but with a different memory location. + * + * @param vector the Vector being normalized. + * @return returns the normalized (or zero) Vector. + */ + public static Vector normalizeVector(Vector vector) { + if (vector.getMagnitude() == 0) { + return new Vector(0.0, vector.getTheta()); + } else { + return new Vector(vector.getMagnitude()/Math.abs(vector.getMagnitude()), vector.getTheta()); + } + } + + /** + * This returns a Vector that is the sum of the two input Vectors. + * + * @param one the first Vector. + * @param two the second Vector. + * @return returns the sum of the Vectors. + */ + public static Vector addVectors(Vector one, Vector two) { + Vector returnVector = new Vector(); + returnVector.setOrthogonalComponents(one.getXComponent()+two.getXComponent(), one.getYComponent()+two.getYComponent()); + return returnVector; + } + + /** + * This subtracts the second Vector from the first Vector and returns the result as a Vector. + * Do note that order matters here. + * + * @param one the first Vector. + * @param two the second Vector. + * @return returns the second Vector subtracted from the first Vector. + */ + public static Vector subtractVectors(Vector one, Vector two) { + Vector returnVector = new Vector(); + returnVector.setOrthogonalComponents(one.getXComponent()-two.getXComponent(), one.getYComponent()-two.getYComponent()); + return returnVector; + } + + /** + * This computes the dot product of the two Vectors. + * + * @param one the first Vector. + * @param two the second Vector. + * @return returns the dot product of the two Vectors. + */ + public static double dotProduct(Vector one, Vector two) { + return one.getXComponent()*two.getXComponent() + one.getYComponent()*two.getYComponent(); + } + + /** + * This computes the first Vector crossed with the second Vector, so a cross product. + * Do note that order matters here. + * + * @param one the first Vector. + * @param two the second Vector. + * @return returns the cross product of the two Vectors. + */ + public static double crossProduct(Vector one, Vector two) { + return one.getXComponent()*two.getYComponent() - one.getYComponent()*two.getXComponent(); + } + + /** + * This returns whether a specified value is within a second specified value by plus/minus a + * specified accuracy amount. + * + * @param one first number specified. + * @param two second number specified. + * @param accuracy the level of accuracy specified. + * @return returns if the two numbers are within the specified accuracy of each other. + */ + public static boolean roughlyEquals(double one, double two, double accuracy) { + return (one < two + accuracy && one > two - accuracy); + } + + /** + * This returns whether a specified number is within a second specified number by plus/minus 0.0001. + * + * @param one first number specified. + * @param two second number specified. + * @return returns if a specified number is within plus/minus 0.0001 of the second specified number. + */ + public static boolean roughlyEquals(double one, double two) { + return roughlyEquals(one, two, 0.0001); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java new file mode 100644 index 0000000..4035252 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java @@ -0,0 +1,477 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; + +import com.acmerobotics.roadrunner.geometry.Pose2d; + +import org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants; + +import java.util.ArrayList; + +/** + * This is the Path class. This class handles containing information on the actual path the Follower + * will follow, not just the shape of the path that the BezierCurve class handles. This contains + * information on the stop criteria for a Path, the heading interpolation, and deceleration. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/10/2024 + */ +public class Path { + private BezierCurve curve; + + private double startHeading; + private double endHeading; + private double closestPointCurvature; + private double closestPointTValue; + private double linearInterpolationEndTime; + + private Vector closestPointTangentVector; + private Vector closestPointNormalVector; + + private boolean isTangentHeadingInterpolation = true; + private boolean followTangentReversed; + + // A multiplier for the zero power acceleration to change the speed the robot decelerates at + // the end of paths. + // Increasing this will cause the robot to try to decelerate faster, at the risk of overshoots + // or localization slippage. + // Decreasing this will cause the deceleration at the end of the Path to be slower, making the + // robot slower but reducing risk of end-of-path overshoots or localization slippage. + // This can be set individually for each Path, but this is the default. + private static double zeroPowerAccelerationMultiplier = FollowerConstants.zeroPowerAccelerationMultiplier; + + // When the robot is at the end of its current Path or PathChain and the velocity goes + // this value, then end the Path. This is in inches/second. + // This can be custom set for each Path. + private static double pathEndVelocityConstraint = FollowerConstants.pathEndVelocityConstraint; + + // When the robot is at the end of its current Path or PathChain and the translational error + // goes below this value, then end the Path. This is in inches. + // This can be custom set for each Path. + private static double pathEndTranslationalConstraint = FollowerConstants.pathEndTranslationalConstraint; + + // When the robot is at the end of its current Path or PathChain and the heading error goes + // below this value, then end the Path. This is in radians. + // This can be custom set for each Path. + private static double pathEndHeadingConstraint = FollowerConstants.pathEndHeadingConstraint; + + // When the t-value of the closest point to the robot on the Path is greater than this value, + // then the Path is considered at its end. + // This can be custom set for each Path. + private static double pathEndTValueConstraint = FollowerConstants.pathEndTValueConstraint; + + // When the Path is considered at its end parametrically, then the Follower has this many + // seconds to further correct by default. + // This can be custom set for each Path. + private static double pathEndTimeoutConstraint = FollowerConstants.pathEndTimeoutConstraint; + + /** + * Creates a new Path from a BezierCurve. The default heading interpolation is tangential. + * + * @param curve the BezierCurve. + */ + public Path(BezierCurve curve) { + this.curve = curve; + } + + /** + * This sets the heading interpolation to linear with a specified start heading and end heading + * for the Path. This will interpolate across the entire length of the Path, so there may be + * some issues with end heading accuracy and precision if this is used. If a precise end heading + * is necessary, then use the setLinearHeadingInterpolation(double startHeading, + * double endHeading, double endTime) method. + * + * @param startHeading The start of the linear heading interpolation. + * @param endHeading The end of the linear heading interpolation. + * This will be reached at the end of the Path if no end time is specified. + */ + public void setLinearHeadingInterpolation(double startHeading, double endHeading) { + linearInterpolationEndTime = 1; + isTangentHeadingInterpolation = false; + this.startHeading = startHeading; + this.endHeading = endHeading; + } + + /** + * This sets the heading interpolation to linear with a specified start heading and end heading + * for the Path. This will interpolate from the start of the Path to the specified end time. + * This ensures high accuracy and precision than interpolating across the entire Path. However, + * interpolating too quickly can cause undesired oscillations and inaccuracies of its own, so + * generally interpolating to something like 0.8 of your Path should work best. + * + * @param startHeading The start of the linear heading interpolation. + * @param endHeading The end of the linear heading interpolation. + * This will be reached at the end of the Path if no end time is specified. + * @param endTime The end time on the Path that the linear heading interpolation will finish. + * This value ranges from [0, 1] since Bezier curves are parametric functions. + */ + public void setLinearHeadingInterpolation(double startHeading, double endHeading, double endTime) { + linearInterpolationEndTime = MathFunctions.clamp(endTime, 0.000000001, 1); + isTangentHeadingInterpolation = false; + this.startHeading = startHeading; + this.endHeading = endHeading; + } + + /** + * This sets the heading interpolation to maintain a constant heading. + * + * @param setHeading the constant heading for the Path. + */ + public void setConstantHeadingInterpolation(double setHeading) { + linearInterpolationEndTime = 1; + isTangentHeadingInterpolation = false; + startHeading = setHeading; + endHeading = setHeading; + } + + /** + * This gets the closest Point from a specified pose to the BezierCurve with a binary search + * that is limited to some specified step limit. + * + * @param pose the pose. + * @param searchStepLimit the binary search step limit. + * @return returns the closest Point. + */ + public Pose2d getClosestPoint(Pose2d pose, int searchStepLimit) { + double lower = 0; + double upper = 1; + Point returnPoint; + + // we don't need to calculate the midpoint, so we start off at the 1/4 and 3/4 point + for (int i = 0; i < searchStepLimit; i++) { + if (MathFunctions.distance(pose, getPoint(lower + 0.25 * (upper-lower))) > MathFunctions.distance(pose, getPoint(lower + 0.75 * (upper-lower)))) { + lower += (upper-lower)/2.0; + } else { + upper -= (upper-lower)/2.0; + } + } + + closestPointTValue = lower + 0.5 * (upper-lower); + + returnPoint = getPoint(closestPointTValue); + + closestPointTangentVector = curve.getDerivative(closestPointTValue); + + closestPointNormalVector = curve.getApproxSecondDerivative(closestPointTValue); + + closestPointCurvature = curve.getCurvature(closestPointTValue); + + return new Pose2d(returnPoint.getX(), returnPoint.getY(), getClosestPointHeadingGoal()); + } + + /** + * This sets whether to follow the tangent heading facing away from (reverse) or towards the + * tangent. This will also set your heading interpolation to tangential. + * + * @param set sets tangential heading reversed or not. + */ + public void setReversed(boolean set) { + isTangentHeadingInterpolation = true; + followTangentReversed = set; + } + + /** + * This sets the heading interpolation to tangential. + */ + public void setTangentHeadingInterpolation() { + isTangentHeadingInterpolation = true; + followTangentReversed = false; + } + + /** + * This returns the unit tangent Vector at the end of the BezierCurve. + * + * @return returns the end tangent Vector. + */ + public Vector getEndTangent() { + return curve.getEndTangent(); + } + + /** + * This returns the point on the Bezier curve that is specified by the parametric t value. A + * Bezier curve is a parametric function that returns points along it with t ranging from [0, 1], + * with 0 being the beginning of the curve and 1 being at the end. The Follower will follow + * BezierCurves from 0 to 1, in terms of t. + * + * @param t this is the t value of the parametric curve. t is clamped to be between 0 and 1 inclusive. + * @return this returns the point requested. + */ + public Point getPoint(double t) { + return curve.getPoint(t); + } + + /** + * This returns the t-value of the closest Point on the BezierCurve. + * + * @return returns the closest Point t-value. + */ + public double getClosestPointTValue() { + return closestPointTValue; + } + + /** + * This returns the approximated length of the BezierCurve. + * + * @return returns the length of the BezierCurve. + */ + public double length() { + return curve.length(); + } + + /** + * This returns the curvature of the BezierCurve at a specified t-value. + * + * @param t the specified t-value. + * @return returns the curvature of the BezierCurve at the specified t-value. + */ + public double getCurvature(double t) { + return curve.getCurvature(t); + } + + /** + * This returns the curvature of the BezierCurve at the closest Point. + * + * @return returns the curvature of the BezierCurve at the closest Point. + */ + public double getClosestPointCurvature() { + return closestPointCurvature; + } + + /** + * This returns the normal Vector at the closest Point. + * + * @return returns the normal Vector at the closest Point. + */ + public Vector getClosestPointNormalVector() { + return MathFunctions.copyVector(closestPointNormalVector); + } + + /** + * This returns the tangent Vector at the closest Point. + * + * @return returns the tangent Vector at the closest Point. + */ + public Vector getClosestPointTangentVector() { + return MathFunctions.copyVector(closestPointTangentVector); + } + + /** + * This returns the heading goal at the closest Point. + * + * @return returns the heading goal at the closest Point. + */ + public double getClosestPointHeadingGoal() { + if (isTangentHeadingInterpolation) { + if (followTangentReversed) return MathFunctions.normalizeAngle(closestPointTangentVector.getTheta() + Math.PI); + return closestPointTangentVector.getTheta(); + } else { + return getHeadingGoal(closestPointTValue); + } + } + + /** + * This gets the heading goal at a specified t-value. + * + * @param t the specified t-value. + * @return returns the heading goal at the specified t-value. + */ + public double getHeadingGoal(double t) { + if (isTangentHeadingInterpolation) { + if (followTangentReversed) return MathFunctions.normalizeAngle(curve.getDerivative(t).getTheta() + Math.PI); + return curve.getDerivative(t).getTheta(); + } else { + if (t > linearInterpolationEndTime) { + return MathFunctions.normalizeAngle(endHeading); + } + return MathFunctions.normalizeAngle(startHeading + MathFunctions.getTurnDirection(startHeading, endHeading) * MathFunctions.getSmallestAngleDifference(endHeading, startHeading) * (t / linearInterpolationEndTime)); + } + } + + /** + * This returns if the robot is at the end of the Path, according to the parametric t-value. + * + * @return returns if at end. + */ + public boolean isAtParametricEnd() { + if (closestPointTValue >= pathEndTValueConstraint) return true; + return false; + } + + /** + * This returns if the robot is at the beginning of the Path, according to the parametric t-value. + * + * @return returns if at start. + */ + public boolean isAtParametricStart() { + if (closestPointTValue <= 1- pathEndTValueConstraint) return true; + return false; + } + + /** + * Returns the ArrayList of control points for this BezierCurve. + * + * @return This returns the control points. + */ + public ArrayList getControlPoints() { + return curve.getControlPoints(); + } + + /** + * Returns the first control point for this BezierCurve. + * + * @return This returns the Point. + */ + public Point getFirstControlPoint() { + return curve.getFirstControlPoint(); + } + + /** + * Returns the second control point, or the one after the start, for this BezierCurve. + * + * @return This returns the Point. + */ + public Point getSecondControlPoint() { + return curve.getSecondControlPoint(); + } + + /** + * Returns the second to last control point for this BezierCurve. + * + * @return This returns the Point. + */ + public Point getSecondToLastControlPoint() { + return curve.getSecondToLastControlPoint(); + } + + /** + * Returns the last control point for this BezierCurve. + * + * @return This returns the Point. + */ + public Point getLastControlPoint() { + return curve.getLastControlPoint(); + } + + /** + * This sets the path's deceleration factor in terms of the natural deceleration of the robot + * when power is cut from the drivetrain. + * + * @param set This sets the multiplier. + */ + public void setZeroPowerAccelerationMultiplier(double set) { + zeroPowerAccelerationMultiplier = set; + } + + /** + * This sets the velocity stop criteria. When velocity is below this amount, then this is met. + * + * @param set This sets the velocity end constraint. + */ + public void setPathEndVelocityConstraint(double set) { + pathEndVelocityConstraint = set; + } + + /** + * This sets the translational stop criteria. When the translational error, or how far off the + * end point the robot is, goes below this, then the translational end criteria is met. + * + * @param set This sets the translational end constraint. + */ + public void setPathEndTranslationalConstraint(double set) { + pathEndTranslationalConstraint = set; + } + + /** + * This sets the heading stop criteria. When the heading error is less than this amount, then + * the heading end criteria is met. + * + * @param set This sets the heading end constraint. + */ + public void setPathEndHeadingConstraint(double set) { + pathEndHeadingConstraint = set; + } + + /** + * This sets the parametric end criteria. When the t-value of the closest Point on the Path is + * greater than this amount, then the parametric end criteria is met. + * + * @param set This sets the t-value end constraint. + */ + public void setPathEndTValueConstraint(double set) { + pathEndTValueConstraint = set; + } + + /** + * This sets the Path end timeout. If the Path is at its end parametrically, then the Follower + * has this many seconds to correct before the Path gets ended anyways. + * + * @param set This sets the Path end timeout. + */ + public void setPathEndTimeoutConstraint(double set) { + pathEndTimeoutConstraint = set; + } + + /** + * This gets the deceleration multiplier. + * + * @return This returns the deceleration multiplier. + */ + public double getZeroPowerAccelerationMultiplier() { + return zeroPowerAccelerationMultiplier; + } + + /** + * This gets the velocity stop criteria. + * + * @return This returns the velocity stop criteria. + */ + public double getPathEndVelocityConstraint() { + return pathEndVelocityConstraint; + } + + /** + * This gets the translational stop criteria. + * + * @return This returns the translational stop criteria. + */ + public double getPathEndTranslationalConstraint() { + return pathEndTranslationalConstraint; + } + + /** + * This gets the heading stop criteria. + * + * @return This returns the heading stop criteria. + */ + public double getPathEndHeadingConstraint() { + return pathEndHeadingConstraint; + } + + /** + * This gets the parametric end criteria. + * + * @return This returns the parametric end criteria. + */ + public double getPathEndTValueConstraint() { + return pathEndTValueConstraint; + } + + /** + * This gets the Path end correction time. + * + * @return This returns the Path end correction time. + */ + public double getPathEndTimeoutConstraint() { + return pathEndTimeoutConstraint; + } + + /** + * Returns the type of path. This is used in case we need to identify the type of BezierCurve + * this is. + * + * @return returns the type of path. + */ + public String pathType() { + return curve.pathType(); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java new file mode 100644 index 0000000..5bfff7d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java @@ -0,0 +1,217 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; + +import java.util.ArrayList; + +/** + * This is the PathBuilder class. This class makes it easier to create PathChains, so you don't have + * to individually create Path instances to create a PathChain. A PathBuilder can be accessed + * through running the pathBuilder() method on an instance of the Follower class, or just creating + * an instance of PathBuilder regularly. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/11/2024 + */ +public class PathBuilder { + private ArrayList paths = new ArrayList<>(); + + private ArrayList callbacks = new ArrayList<>(); + + /** + * This is an empty constructor for the PathBuilder class so it can get started. + * The PathBuilder allows for easier construction of PathChains. + * The proper usage is using an instance of the Follower class: + * Follower follower = new Follower(hardwareMap); + * Then calling "follower.pathBuilder.[INSERT PATH BUILDING METHODS].build(); + * Of course, you can split up the method calls onto separate lines for readability. + */ + public PathBuilder() { + } + + /** + * This adds a Path to the PathBuilder. + * + * @param path The Path being added. + * @return This returns itself with the updated data. + */ + public PathBuilder addPath(Path path) { + this.paths.add(path); + return this; + } + + /** + * This adds a default Path defined by a specified BezierCurve to the PathBuilder. + * + * @param curve The curve is turned into a Path and added. + * @return This returns itself with the updated data. + */ + public PathBuilder addPath(BezierCurve curve) { + this.paths.add(new Path(curve)); + return this; + } + + /** + * This sets a linear heading interpolation on the last Path added to the PathBuilder. + * + * @param startHeading The start of the linear heading interpolation. + * @param endHeading The end of the linear heading interpolation. + * This will be reached at the end of the Path if no end time is specified. + * @return This returns itself with the updated data. + */ + public PathBuilder setLinearHeadingInterpolation(double startHeading, double endHeading) { + this.paths.get(paths.size()-1).setLinearHeadingInterpolation(startHeading, endHeading); + return this; + } + + /** + * This sets a linear heading interpolation on the last Path added to the PathBuilder. + * + * @param startHeading The start of the linear heading interpolation. + * @param endHeading The end of the linear heading interpolation. + * This will be reached at the end of the Path if no end time is specified. + * @param endTime The end time on the Path that the linear heading interpolation will end. + * This value goes from [0, 1] since Bezier curves are parametric functions. + * @return This returns itself with the updated data. + */ + public PathBuilder setLinearHeadingInterpolation(double startHeading, double endHeading, double endTime) { + this.paths.get(paths.size()-1).setLinearHeadingInterpolation(startHeading, endHeading, endTime); + return this; + } + + /** + * This sets a constant heading interpolation on the last Path added to the PathBuilder. + * + * @param setHeading The constant heading specified. + * @return This returns itself with the updated data. + */ + public PathBuilder setConstantHeadingInterpolation(double setHeading) { + this.paths.get(paths.size()-1).setConstantHeadingInterpolation(setHeading); + return this; + } + + /** + * This sets a reversed or tangent heading interpolation on the last Path added to the PathBuilder. + * + * @param set This sets the heading to reversed tangent following if this parameter is true. + * Conversely, this sets a tangent following if this parameter is false. + * @return This returns itself with the updated data. + */ + public PathBuilder setReversed(boolean set) { + this.paths.get(paths.size()-1).setReversed(set); + return this; + } + /** + * This sets the heading interpolation to tangential on the last Path added to the PathBuilder. + * There really shouldn't be a reason to use this since the default heading interpolation is + * tangential but it's here. + */ + public PathBuilder setTangentHeadingInterpolation() { + this.paths.get(paths.size()-1).setTangentHeadingInterpolation(); + return this; + } + + /** + * This sets the deceleration multiplier on the last Path added to the PathBuilder. + * + * @param set This sets the multiplier for the goal for the deceleration of the robot. + * @return This returns itself with the updated data. + */ + public PathBuilder setZeroPowerAccelerationMultiplier(double set) { + this.paths.get(paths.size()-1).setZeroPowerAccelerationMultiplier(set); + return this; + } + + /** + * This sets the path end velocity constraint on the last Path added to the PathBuilder. + * + * @param set This sets the path end velocity constraint. + * @return This returns itself with the updated data. + */ + public PathBuilder setPathEndVelocityConstraint(double set) { + this.paths.get(paths.size()-1).setPathEndVelocityConstraint(set); + return this; + } + + /** + * This sets the path end translational constraint on the last Path added to the PathBuilder. + * + * @param set This sets the path end translational constraint. + * @return This returns itself with the updated data. + */ + public PathBuilder setPathEndTranslationalConstraint(double set) { + this.paths.get(paths.size()-1).setPathEndTranslationalConstraint(set); + return this; + } + + /** + * This sets the path end heading constraint on the last Path added to the PathBuilder. + * + * @param set This sets the path end heading constraint. + * @return This returns itself with the updated data. + */ + public PathBuilder setPathEndHeadingConstraint(double set) { + this.paths.get(paths.size()-1).setPathEndHeadingConstraint(set); + return this; + } + + /** + * This sets the path end t-value (parametric time) constraint on the last Path added to the PathBuilder. + * + * @param set This sets the path end t-value (parametric time) constraint. + * @return This returns itself with the updated data. + */ + public PathBuilder setPathEndTValueConstraint(double set) { + this.paths.get(paths.size()-1).setPathEndTValueConstraint(set); + return this; + } + + /** + * This sets the path end timeout constraint on the last Path added to the PathBuilder. + * + * @param set This sets the path end timeout constraint. + * @return This returns itself with the updated data. + */ + public PathBuilder setPathEndTimeoutConstraint(double set) { + this.paths.get(paths.size()-1).setPathEndTimeoutConstraint(set); + return this; + } + + /** + * This adds a temporal callback on the last Path added to the PathBuilder. + * This callback is set to run at a specified number of milliseconds after the start of the path. + * + * @param time This sets the number of milliseconds of wait between the start of the Path and + * the calling of the callback. + * @param runnable This sets the code for the callback to run. Use lambda statements for this. + * @return This returns itself with the updated data. + */ + public PathBuilder addTemporalCallback(double time, Runnable runnable) { + this.callbacks.add(new PathCallback(time, runnable, PathCallback.TIME, paths.size()-1)); + return this; + } + + /** + * This adds a parametric callback on the last Path added to the PathBuilder. + * This callback is set to run at a certain point on the Path. + * + * @param t This sets the t-value (parametric time) on the Path for when to run the callback. + * @param runnable This sets the code for the callback to run. Use lambda statements for this. + * @return This returns itself with the updated data. + */ + public PathBuilder addParametricCallback(double t, Runnable runnable) { + this.callbacks.add(new PathCallback(t, runnable, PathCallback.PARAMETRIC, paths.size()-1)); + return this; + } + + /** + * This builds all the Path and callback information together into a PathChain. + * + * @return This returns a PathChain made of all the specified paths and callbacks. + */ + public PathChain build() { + PathChain returnChain = new PathChain(paths); + returnChain.setCallbacks(callbacks); + return returnChain; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathCallback.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathCallback.java new file mode 100644 index 0000000..c02e1d5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathCallback.java @@ -0,0 +1,78 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; + +import org.firstinspires.ftc.teamcode.pedroPathing.util.SingleRunAction; + +/** + * This is the PathCallback class. This class handles callbacks of Runnables in PathChains. + * Basically, this allows you to run non-blocking code in the middle of PathChains. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/11/2024 + */ +public class PathCallback extends SingleRunAction { + + private double startCondition; + + private int type; + private int index; + + public static final int TIME = 0; + public static final int PARAMETRIC = 1; + + /** + * This creates a new PathCallback with a specified start condition (either time or parametric), + * a Runnable of code to run (preferably a lambda statement), a type (using the class constants), + * and an index for which Path within a PathChain the callback is to run on. + * + * @param startCondition This defines when the callback is to be run, either as a wait time in + * milliseconds or a t-value (parametric time) point. + * @param runnable This contains the code to run when the callback is called. + * @param type This defines the type of callback using the class constants. + * @param index This defines which Path within the PathChain the callback is to run on. + */ + public PathCallback(double startCondition, Runnable runnable, int type, int index) { + super(runnable); + this.startCondition = startCondition; + this.type = type; + if (this.type != TIME || this.type != PARAMETRIC) { + this.type = PARAMETRIC; + } + if (this.type == TIME && this.startCondition < 0) { + this.startCondition = 0.0; + } + if (this.type == PARAMETRIC) { + this.startCondition = MathFunctions.clamp(this.startCondition, 0, 1); + } + this.index = index; + } + + /** + * This returns the type of callback this is (time or parametric). + * + * @return This returns the type of callback. + */ + public int getType() { + return type; + } + + /** + * This returns the start condition for this callback. This will be the wait time in milliseconds + * if this is a time callback or a t-value if this is a parametric callback. + * + * @return This returns the start condition. + */ + public double getStartCondition() { + return startCondition; + } + + /** + * This returns the index of which Path the callback is to run on within the PathChain. + * + * @return This returns the Path index of this callback. + */ + public int getIndex() { + return index; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathChain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathChain.java new file mode 100644 index 0000000..f350f5e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathChain.java @@ -0,0 +1,94 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; + +import java.util.ArrayList; + +/** + * This is the PathChain class. This class handles chaining together multiple Paths into a larger + * collection of Paths that can be run continuously. Additionally, this allows for the addition of + * PathCallbacks to specific Paths in the PathChain, allowing for non-blocking code to be run in + * the middle of a PathChain. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/11/2024 + */ +public class PathChain { + private ArrayList pathChain = new ArrayList<>(); + + private ArrayList callbacks = new ArrayList<>(); + + /** + * This creates a new PathChain from some specified Paths. + * + * IMPORTANT NOTE: Order matters here. The order in which the Paths are input is the order in + * which they will be run. + * + * @param paths the specified Paths. + */ + public PathChain(Path... paths) { + for (Path path : paths) { + pathChain.add(path); + } + } + + /** + * This creates a new PathChain from an ArrayList of Paths. + * + * IMPORTANT NOTE: Order matters here. The order in which the Paths are input is the order in + * which they will be run. + * + * @param paths the ArrayList of Paths. + */ + public PathChain(ArrayList paths) { + pathChain = paths; + } + + /** + * This returns the Path on the PathChain at a specified index. + * + * @param index the index. + * @return returns the Path at the index. + */ + public Path getPath(int index) { + return pathChain.get(index); + } + + /** + * This returns the size of the PathChain. + * + * @return returns the size of the PathChain. + */ + public int size() { + return pathChain.size(); + } + + /** + * This sets the PathCallbacks of the PathChain with some specified PathCallbacks. + * + * @param callbacks the specified PathCallbacks. + */ + public void setCallbacks(PathCallback... callbacks) { + for (PathCallback callback : callbacks) { + this.callbacks.add(callback); + } + } + + /** + * This sets the PathCallbacks of the PathChain with an ArrayList of PathCallbacks. + * + * @param callbacks the ArrayList of PathCallbacks. + */ + public void setCallbacks(ArrayList callbacks) { + this.callbacks = callbacks; + } + + /** + * This returns the PathCallbacks of this PathChain in an ArrayList. + * + * @return returns the PathCallbacks. + */ + public ArrayList getCallbacks() { + return callbacks; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java new file mode 100644 index 0000000..b37a140 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java @@ -0,0 +1,175 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; + +import com.acmerobotics.roadrunner.geometry.Pose2d; + +/** + * This is the Point class. This class handles storing information about the location of points in + * 2D space in both Cartesian, or rectangular, and polar coordinates. Additionally, this contains + * the method to find the distance between two Points. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/11/2024 + */ +public class Point { + + // IMPORTANT NOTE: theta is defined in radians. + // These are the values of the coordinate defined by this Point, in both polar and + // Cartesian systems. + private double r; + private double theta; + private double x; + private double y; + + // these are used for ease of changing/setting identification + public static final int POLAR = 0; + public static final int CARTESIAN = 1; + + + /** + * This creates a new Point with coordinate inputs and a specified coordinate system. + * + * @param rOrX Depending on the coordinate system specified, this is either the r or x value. + * In polar coordinates, the r value is the distance from the origin. + * In Cartesian coordinates, the x value is the distance left/right from the origin. + * @param thetaOrY Depending on the coordinate system specified, this is either the theta or + * y value. + * In polar coordinates, the theta value is the angle from the positive x-axis. + * Increasing theta moves in the counter-clockwise direction. + * In Cartesian coordinates, the y value is the distance up/down from the origin. + * @param identifier this specifies what coordinate system the coordinate inputs are in. + */ + public Point(double rOrX, double thetaOrY, int identifier) { + setCoordinates(rOrX, thetaOrY, identifier); + } + + /** + * This creates a new Point from a Pose2d. + * + * @param pose the Pose2d. + */ + public Point(Pose2d pose) { + setCoordinates(pose.getX(), pose.getY(), CARTESIAN); + } + + /** + * This sets the coordinates of the Point using the specified coordinate system. + * + * @param rOrX Depending on the coordinate system specified, this is either the r or x value. + * In polar coordinates, the r value is the distance from the origin. + * In Cartesian coordinates, the x value is the distance left/right from the origin. + * @param thetaOrY Depending on the coordinate system specified, this is either the theta or + * y value. + * In polar coordinates, the theta value is the angle from the positive x-axis. + * Increasing theta moves in the counter-clockwise direction. + * In Cartesian coordinates, the y value is the distance up/down from the origin. + * @param identifier this specifies what coordinate system to use when setting values. + */ + public void setCoordinates(double rOrX, double thetaOrY, int identifier) { + double[] setOtherCoordinates; + switch (identifier) { // this detects which coordinate system to use + // there is no POLAR case since that's covered by the default + case CARTESIAN: + x = rOrX; + y = thetaOrY; + setOtherCoordinates = cartesianToPolar(x, y); + r = setOtherCoordinates[0]; + theta = setOtherCoordinates[1]; + break; + default: + if (rOrX<0) { + r = -rOrX; + theta = MathFunctions.normalizeAngle(thetaOrY+Math.PI); + } else { + r = rOrX; + theta = MathFunctions.normalizeAngle(thetaOrY); + } + setOtherCoordinates = polarToCartesian(r, theta); + x = setOtherCoordinates[0]; + y = setOtherCoordinates[1]; + break; + } + } + + /** + * Calculates the distance between this Point and some other specified Point. + * + * @param otherPoint the other specified Point. + * @return returns the distance between the two Points. + */ + public double distanceFrom(Point otherPoint) { + return Math.sqrt(Math.pow(otherPoint.getX()-x, 2) + Math.pow(otherPoint.getY()-y, 2)); + } + + /** + * This takes in an r and theta value and converts them to Cartesian coordinates. + * + * @param r this is the r value of the Point being converted. + * @param theta this is the theta value of the Point being converted. + * @return this returns the x and y values, in that order, in an Array of doubles. + */ + public static double[] polarToCartesian(double r, double theta) { + return new double[] {r * Math.cos(theta), r * Math.sin(theta)}; + } + + /** + * This takes in an x and y value and converts them to polar coordinates. + * + * @param x this is the x value of the Point being converted. + * @param y this is the y value of the Point being converted. + * @return this returns the r and theta values, in that order, in an Array of doubles. + */ + public static double[] cartesianToPolar(double x, double y) { + if (x == 0) { + if (y > 0) { + return new double[] {Math.abs(y), Math.PI/2}; + } else { + return new double[] {Math.abs(y), (3 * Math.PI) / 2}; + } + } + double r = Math.sqrt(x*x+y*y); + if (x < 0) return new double[] {r, Math.PI+Math.atan(y/x)}; + if (y > 0) { + return new double[]{r, Math.atan(y / x)}; + } else { + return new double[]{r, (2*Math.PI) + Math.atan(y / x)}; + } + } + + /** + * Returns the r value of this Point. This is a polar coordinate value. + * + * @return returns the r value. + */ + public double getR() { + return r; + } + + /** + * Returns the theta value of this Point. This is a polar coordinate value. + * + * @return returns the theta value. + */ + public double getTheta() { + return theta; + } + + /** + * Returns the x value of this Point. This is a Cartesian coordinate value. + * + * @return returns the x value. + */ + public double getX() { + return x; + } + + /** + * Returns the y value of this Point. This is a Cartesian coordinate value. + * + * @return returns the y value. + */ + public double getY() { + return y; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Vector.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Vector.java new file mode 100644 index 0000000..f1a093b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Vector.java @@ -0,0 +1,141 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; + +/** + * This is the Point class. This class handles storing information about vectors, which are + * basically Points but using polar coordinates as the default. The main reason this class exists + * is because some vector math needs to be done in the Follower, and dot products and cross + * products of Points just don't seem right. Also, there are a few more methods in here that make + * using Vectors a little easier than using a Point in polar coordinates. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/11/2024 + */ +public class Vector { + + // IMPORTANT NOTE: theta is defined in radians. + // These are the values of the coordinate defined by this Point, in both polar and + // Cartesian systems. + private double magnitude; + private double theta; + private double xComponent; + private double yComponent; + + /** + * This creates a new Vector with zero magnitude and direction. + */ + public Vector() { + setComponents(0, 0); + } + + /** + * This creates a new Vector with a specified magnitude and direction. + * + * @param magnitude magnitude of the Vector. + * @param theta the direction of the Vector in radians. + */ + public Vector(double magnitude, double theta) { + setComponents(magnitude, theta); + } + + /** + * This sets the components of the Vector in regular vector coordinates. + * + * @param magnitude sets the magnitude of this Vector. + * @param theta sets the theta value of this Vector. + */ + public void setComponents(double magnitude, double theta) { + double[] orthogonalComponents; + if (magnitude<0) { + this.magnitude = -magnitude; + this.theta = MathFunctions.normalizeAngle(theta+Math.PI); + } else { + this.magnitude = magnitude; + this.theta = MathFunctions.normalizeAngle(theta); + } + orthogonalComponents = Point.polarToCartesian(magnitude, theta); + xComponent = orthogonalComponents[0]; + yComponent = orthogonalComponents[1]; + } + + /** + * This sets only the magnitude of the Vector. + * + * @param magnitude sets the magnitude of this Vector. + */ + public void setMagnitude(double magnitude) { + setComponents(magnitude, theta); + } + + /** + * This sets only the angle, theta, of the Vector. + * + * @param theta sets the angle, or theta value, of this Vector. + */ + public void setTheta(double theta) { + setComponents(magnitude, theta); + } + + /** + * This rotates the Vector by an angle, theta. + * + * @param theta2 the angle to be added. + */ + public void rotateVector(double theta2) { + setTheta(theta+theta2); + } + + /** + * This sets the orthogonal components of the Vector. These orthogonal components are assumed + * to be in the direction of the x-axis and y-axis. In other words, this is setting the + * coordinates of the Vector using x and y coordinates instead of a direction and magnitude. + * + * @param xComponent sets the x component of this Vector. + * @param yComponent sets the y component of this Vector. + */ + public void setOrthogonalComponents(double xComponent, double yComponent) { + double[] polarComponents; + this.xComponent = xComponent; + this.yComponent = yComponent; + polarComponents = Point.cartesianToPolar(xComponent, yComponent); + magnitude = polarComponents[0]; + theta = polarComponents[1]; + } + + /** + * Returns the magnitude of this Vector. + * + * @return returns the magnitude. + */ + public double getMagnitude() { + return magnitude; + } + + /** + * Returns the theta value, or angle, of this Vector. + * + * @return returns the theta value. + */ + public double getTheta() { + return theta; + } + + /** + * Returns the x component of this Vector. + * + * @return returns the x component. + */ + public double getXComponent() { + return xComponent; + } + + /** + * Returns the y component of this Vector. + * + * @return returns the y component. + */ + public double getYComponent() { + return yComponent; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/Circle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/Circle.java new file mode 100644 index 0000000..196b378 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/Circle.java @@ -0,0 +1,74 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; + +/** + * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite + * a circle, but some Bezier curves that have control points set essentially in a square. However, + * it turns enough to tune your centripetal force correction and some of your heading. Some lag in + * heading is to be expected. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +@Config +@Autonomous (name = "Circle", group = "Autonomous Pathing Tuning") +public class Circle extends OpMode { + private Telemetry telemetryA; + + public static double RADIUS = 10; + + private Follower follower; + + private PathChain circle; + + /** + * This initializes the Follower and creates the PathChain for the "circle". Additionally, this + * initializes the FTC Dashboard telemetry. + */ + @Override + public void init() { + follower = new Follower(hardwareMap); + + circle = follower.pathBuilder() + .addPath(new BezierCurve(new Point(0,0, Point.CARTESIAN), new Point(RADIUS,0, Point.CARTESIAN), new Point(RADIUS, RADIUS, Point.CARTESIAN))) + .addPath(new BezierCurve(new Point(RADIUS, RADIUS, Point.CARTESIAN), new Point(RADIUS,2*RADIUS, Point.CARTESIAN), new Point(0,2*RADIUS, Point.CARTESIAN))) + .addPath(new BezierCurve(new Point(0,2*RADIUS, Point.CARTESIAN), new Point(-RADIUS,2*RADIUS, Point.CARTESIAN), new Point(-RADIUS, RADIUS, Point.CARTESIAN))) + .addPath(new BezierCurve(new Point(-RADIUS, RADIUS, Point.CARTESIAN), new Point(-RADIUS,0, Point.CARTESIAN), new Point(0,0, Point.CARTESIAN))) + .build(); + + follower.followPath(circle); + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("This will run in a roughly circular shape of radius " + RADIUS + + ", starting on the right-most edge. So, make sure you have enough " + + "space to the left, front, and back to run the OpMode."); + telemetryA.update(); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the FTC Dashboard. + */ + @Override + public void loop() { + follower.update(); + if (follower.atParametricEnd()) { + follower.followPath(circle); + } + + follower.telemetryDebug(telemetryA); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/CurvedBackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/CurvedBackAndForth.java new file mode 100644 index 0000000..a93dc28 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/CurvedBackAndForth.java @@ -0,0 +1,85 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; + +/** + * This is the CurvedBackAndForth autonomous OpMode. It runs the robot in a specified distance + * forward and to the left. On reaching the end of the forward Path, the robot runs the backward + * Path the same distance back to the start. Rinse and repeat! This is good for testing a variety + * of Vectors, like the drive Vector, the translational Vector, the heading Vector, and the + * centripetal Vector. Remember to test your tunings on StraightBackAndForth as well, since tunings + * that work well for curves might have issues going in straight lines. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/13/2024 + */ +@Config +@Autonomous (name = "Curved Back And Forth", group = "Autonomous Pathing Tuning") +public class CurvedBackAndForth extends OpMode { + private Telemetry telemetryA; + + public static double DISTANCE = 20; + + private boolean forward = true; + + private Follower follower; + + private Path forwards; + private Path backwards; + + /** + * This initializes the Follower and creates the forward and backward Paths. Additionally, this + * initializes the FTC Dashboard telemetry. + */ + @Override + public void init() { + follower = new Follower(hardwareMap); + + forwards = new Path(new BezierCurve(new Point(0,0, Point.CARTESIAN), new Point(DISTANCE,0, Point.CARTESIAN), new Point(DISTANCE,DISTANCE, Point.CARTESIAN))); + backwards = new Path(new BezierCurve(new Point(DISTANCE,DISTANCE, Point.CARTESIAN), new Point(DISTANCE,0, Point.CARTESIAN), new Point(0,0, Point.CARTESIAN))); + + backwards.setReversed(true); + + follower.followPath(forwards); + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("This will run the robot in a curve going " + DISTANCE + " inches" + + " to the left and the same number of inches forward. The robot will go" + + "forward and backward continuously along the path. Make sure you have" + + "enough room."); + telemetryA.update(); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the FTC Dashboard. + */ + @Override + public void loop() { + follower.update(); + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryA.addData("going forward", forward); + follower.telemetryDebug(telemetryA); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java new file mode 100644 index 0000000..be954e2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java @@ -0,0 +1,181 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.tuning; + +import com.acmerobotics.dashboard.config.Config; + +import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomPIDFCoefficients; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +/** + * This is the FollowerConstants class. It holds many constants and parameters for various parts of + * the Follower. This is here to allow for easier tuning of Pedro Pathing, as well as concentrate + * everything tunable for the Paths themselves in one place. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/4/2024 + */ +@Config +public class FollowerConstants { + + // This section is for setting the actual drive vector for the front left wheel, if the robot + // is facing a heading of 0 radians with the wheel centered at (0,0) + private static double xMovement = 81.34056; + private static double yMovement = 65.43028; + private static double[] convertToPolar = Point.cartesianToPolar(xMovement, -yMovement); + public static Vector frontLeftVector = MathFunctions.normalizeVector(new Vector(convertToPolar[0],convertToPolar[1])); + + // Large heading error PIDF coefficients + public static CustomPIDFCoefficients largeHeadingPIDFCoefficients = new CustomPIDFCoefficients( + 1, + 0, + 0, + 0); + + // Feed forward constant added on to the large heading PIDF + public static double largeHeadingPIDFFeedForward = 0.01; + + // the limit at which the heading PIDF switches between the large and small heading PIDFs + public static double headingPIDFSwitch = Math.PI/20; + + // Small heading error PIDF coefficients + public static CustomPIDFCoefficients smallHeadingPIDFCoefficients = new CustomPIDFCoefficients( + 5, + 0, + 0.08, + 0); + + // Feed forward constant added on to the small heading PIDF + public static double smallHeadingPIDFFeedForward = 0.01; + + // Large translational PIDF coefficients + public static CustomPIDFCoefficients largeTranslationalPIDFCoefficients = new CustomPIDFCoefficients( + 0.1, + 0, + 0, + 0); + + // Feed forward constant added on to the large translational PIDF + public static double largeTranslationalPIDFFeedForward = 0.015; + + // Large translational Integral + public static CustomPIDFCoefficients largeTranslationalIntegral = new CustomPIDFCoefficients( + 0, + 0, + 0, + 0); + + // the limit at which the heading PIDF switches between the large and small translational PIDFs + public static double translationalPIDFSwitch = 3; + + // Small translational PIDF coefficients + public static CustomPIDFCoefficients smallTranslationalPIDFCoefficients = new CustomPIDFCoefficients( + 0.3, + 0, + 0.01, + 0); + + // Small translational Integral value + public static CustomPIDFCoefficients smallTranslationalIntegral = new CustomPIDFCoefficients( + 0, + 0, + 0, + 0); + + // Feed forward constant added on to the small translational PIDF + public static double smallTranslationalPIDFFeedForward = 0.015; + + // Large drive PIDF coefficients + public static CustomPIDFCoefficients largeDrivePIDFCoefficients = new CustomPIDFCoefficients( + 0.025, + 0, + 0.00001, + 0); + + // Feed forward constant added on to the large drive PIDF + public static double largeDrivePIDFFeedForward = 0.01; + + // the limit at which the heading PIDF switches between the large and small drive PIDFs + public static double drivePIDFSwitch = 20; + + // Small drive PIDF coefficients + public static CustomPIDFCoefficients smallDrivePIDFCoefficients = new CustomPIDFCoefficients( + 0.02, + 0, + 0.000005, + 0); + + // Feed forward constant added on to the small drive PIDF + public static double smallDrivePIDFFeedForward = 0.01; + + // Mass of robot in kilograms + public static double mass = 10.65942; + + // Centripetal force to power scaling + public static double centripetalScaling = 0.001; + + // Acceleration of the drivetrain when power is cut in inches/second^2 (should be negative) + // if not negative, then the robot thinks that its going to go faster under 0 power + // this is for curves + public static double forwardZeroPowerAcceleration = -34.62719; + + // Acceleration of the drivetrain when power is cut in inches/second^2 (should be negative) + // if not negative, then the robot thinks that its going to go faster under 0 power + // this is for curves + public static double lateralZeroPowerAcceleration = -78.15554; + + // A multiplier for the zero power acceleration to change the speed the robot decelerates at + // the end of paths. + // Increasing this will cause the robot to try to decelerate faster, at the risk of overshoots + // or localization slippage. + // Decreasing this will cause the deceleration at the end of the Path to be slower, making the + // robot slower but reducing risk of end-of-path overshoots or localization slippage. + // This can be set individually for each Path, but this is the default. + public static double zeroPowerAccelerationMultiplier = 4; + + // When the robot is at the end of its current Path or PathChain and the velocity goes below + // this value, then end the Path. This is in inches/second. + // This can be custom set for each Path. + public static double pathEndVelocityConstraint = 0.1; + + // When the robot is at the end of its current Path or PathChain and the translational error + // goes below this value, then end the Path. This is in inches. + // This can be custom set for each Path. + public static double pathEndTranslationalConstraint = 0.1; + + // When the robot is at the end of its current Path or PathChain and the heading error goes + // below this value, then end the Path. This is in radians. + // This can be custom set for each Path. + public static double pathEndHeadingConstraint = 0.007; + + // When the t-value of the closest point to the robot on the Path is greater than this value, + // then the Path is considered at its end. + // This can be custom set for each Path. + public static double pathEndTValueConstraint = 0.995; + + // When the Path is considered at its end parametrically, then the Follower has this many + // seconds to further correct by default. + // This can be custom set for each Path. + public static double pathEndTimeoutConstraint = 1.5; + + // This is how many steps the BezierCurve class uses to approximate the length of a BezierCurve. + public static int APPROXIMATION_STEPS = 1000; + + // This is scales the translational error correction power when the Follower is holding a Point. + public static double holdPointTranslationalScaling = 0.45; + + // This is scales the heading error correction power when the Follower is holding a Point. + public static double holdPointHeadingScaling = 0.35; + + // This is the number of times the velocity is recorded for averaging when approximating a first + // and second derivative for on the fly centripetal correction. The velocity is calculated using + // half of this number of samples, and the acceleration uses all of this number of samples. + public static int AVERAGED_VELOCITY_SAMPLE_NUMBER = 8; + + // This is the number of steps the binary search for closest point uses. More steps is more + // accuracy, and this increases at an exponential rate. However, more steps also does take more + // time. + public static int BEZIER_CURVE_BINARY_STEP_LIMIT = 10; +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java new file mode 100644 index 0000000..c2ce2ac --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java @@ -0,0 +1,147 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +/** + * This is the ForwardVelocityTuner autonomous tuning OpMode. This runs the robot forwards at max + * power until it reaches some specified distance. It records the most recent velocities, and on + * reaching the end of the distance, it averages them and prints out the velocity obtained. It is + * recommended to run this multiple times on a full battery to get the best results. What this does + * is, when paired with StrafeVelocityTuner, allows FollowerConstants to create a Vector that + * empirically represents the direction your mecanum wheels actually prefer to go in, allowing for + * more accurate following. + * You can adjust the distance the robot will travel on FTC Dashboard: 192/168/43/1:8080/dash + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/13/2024 + */ +@Config +@Autonomous (name = "Forward Velocity Tuner", group = "Autonomous Pathing Tuning") +public class ForwardVelocityTuner extends OpMode { + private ArrayList velocities = new ArrayList<>(); + + private DcMotorEx leftFront; + private DcMotorEx leftRear; + private DcMotorEx rightFront; + private DcMotorEx rightRear; + private List motors; + + private PoseUpdater poseUpdater; + + public static double DISTANCE = 40; + public static double RECORD_NUMBER = 10; + + private Telemetry telemetryA; + + private boolean end; + + /** + * This initializes the drive motors as well as the cache of velocities and the FTC Dashboard + * telemetry. + */ + @Override + public void init() { + poseUpdater = new PoseUpdater(hardwareMap); + + leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); + leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); + rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); + rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + + // TODO: Make sure that this is the direction your motors need to be reversed in. + leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + leftRear.setDirection(DcMotorSimple.Direction.REVERSE); + + motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear); + + for (DcMotorEx motor : motors) { + MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); + motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motor.setMotorType(motorConfigurationType); + } + + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + } + + for (int i = 0; i < RECORD_NUMBER; i++) { + velocities.add(0.0); + } + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward."); + telemetryA.addLine("Make sure you have enough room, since the robot has inertia after cutting power."); + telemetryA.addLine("After running the distance, the robot will cut power from the drivetrain and display the forward velocity."); + telemetryA.addLine("Press CROSS or A on game pad 1 to stop."); + telemetryA.update(); + + } + + /** + * This starts the OpMode by setting the drive motors to run forward at full power. + */ + @Override + public void start() { + leftFront.setPower(1); + leftRear.setPower(1); + rightFront.setPower(1); + rightRear.setPower(1); + end = false; + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing CROSS or A on + * game pad 1 will stop the OpMode. This continuously records the RECORD_NUMBER most recent + * velocities, and when the robot has run forward enough, these last velocities recorded are + * averaged and printed. + */ + @Override + public void loop() { + if (gamepad1.cross || gamepad1.a) { + requestOpModeStop(); + } + + poseUpdater.update(); + if (!end) { + if (Math.abs(poseUpdater.getPose().getX()) > DISTANCE) { + end = true; + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } + } else { + double currentVelocity = Math.abs(MathFunctions.dotProduct(poseUpdater.getVelocity(), new Vector(1, 0))); + velocities.add(currentVelocity); + velocities.remove(0); + } + } else { + double average = 0; + for (Double velocity : velocities) { + average += velocity; + } + average /= (double) velocities.size(); + + telemetryA.addData("forward velocity:", average); + telemetryA.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java new file mode 100644 index 0000000..c6d212e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java @@ -0,0 +1,153 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +/** + * This is the ForwardZeroPowerAccelerationTuner autonomous tuning OpMode. This runs the robot + * forward until a specified velocity is achieved. Then, the robot cuts power to the motors, setting + * them to zero power. The deceleration, or negative acceleration, is then measured until the robot + * stops. The accelerations across the entire time the robot is slowing down is then averaged and + * that number is then printed. This is used to determine how the robot will decelerate in the + * forward direction when power is cut, making the estimations used in the calculations for the + * drive Vector more accurate and giving better braking at the end of Paths. + * You can adjust the max velocity the robot will hit on FTC Dashboard: 192/168/43/1:8080/dash + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/13/2024 + */ +@Config +@Autonomous (name = "Forward Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") +public class ForwardZeroPowerAccelerationTuner extends OpMode { + private ArrayList accelerations = new ArrayList<>(); + + private DcMotorEx leftFront; + private DcMotorEx leftRear; + private DcMotorEx rightFront; + private DcMotorEx rightRear; + private List motors; + + private PoseUpdater poseUpdater; + + public static double VELOCITY = 30; + + private double previousVelocity; + + private long previousTimeNano; + + private Telemetry telemetryA; + + private boolean stopping; + private boolean end; + + /** + * This initializes the drive motors as well as the FTC Dashboard telemetry. + */ + @Override + public void init() { + poseUpdater = new PoseUpdater(hardwareMap); + + leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); + leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); + rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); + rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + + // TODO: Make sure that this is the direction your motors need to be reversed in. + leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + leftRear.setDirection(DcMotorSimple.Direction.REVERSE); + + motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear); + + for (DcMotorEx motor : motors) { + MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); + motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motor.setMotorType(motorConfigurationType); + } + + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + } + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("The robot will run forward until it reaches " + VELOCITY + " inches per second."); + telemetryA.addLine("Then, it will cut power from the drivetrain and roll to a stop."); + telemetryA.addLine("Make sure you have enough room."); + telemetryA.addLine("After stopping, the forward zero power acceleration (natural deceleration) will be displayed."); + telemetryA.addLine("Press CROSS or A on game pad 1 to stop."); + telemetryA.update(); + } + + /** + * This starts the OpMode by setting the drive motors to run forward at full power. + */ + @Override + public void start() { + leftFront.setPower(1); + leftRear.setPower(1); + rightFront.setPower(1); + rightRear.setPower(1); + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing CROSS or A on + * game pad 1 will stop the OpMode. When the robot hits the specified velocity, the robot will + * record its deceleration / negative acceleration until it stops. Then, it will average all the + * recorded deceleration / negative acceleration and print that value. + */ + @Override + public void loop() { + if (gamepad1.cross || gamepad1.a) { + requestOpModeStop(); + } + + poseUpdater.update(); + Vector heading = new Vector(1.0, poseUpdater.getPose().getHeading()); + if (!end) { + if (!stopping) { + if (MathFunctions.dotProduct(poseUpdater.getVelocity(), heading) > VELOCITY) { + previousVelocity = MathFunctions.dotProduct(poseUpdater.getVelocity(), heading); + previousTimeNano = System.nanoTime(); + stopping = true; + for (DcMotorEx motor : motors) { + motor.setPower(0); + } + } + } else { + double currentVelocity = MathFunctions.dotProduct(poseUpdater.getVelocity(), heading); + accelerations.add((currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / Math.pow(10.0, 9))); + previousVelocity = currentVelocity; + previousTimeNano = System.nanoTime(); + if (currentVelocity < FollowerConstants.pathEndVelocityConstraint) { + end = true; + } + } + } else { + double average = 0; + for (Double acceleration : accelerations) { + average += acceleration; + } + average /= (double)accelerations.size(); + + telemetryA.addData("forward zero power acceleration (deceleration):", average); + telemetryA.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java new file mode 100644 index 0000000..8692e1b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java @@ -0,0 +1,138 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +@Config +@Autonomous (name = "Lateral Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") +public class LateralZeroPowerAccelerationTuner extends OpMode { + private ArrayList accelerations = new ArrayList<>(); + + private DcMotorEx leftFront; + private DcMotorEx leftRear; + private DcMotorEx rightFront; + private DcMotorEx rightRear; + private List motors; + + private PoseUpdater poseUpdater; + + public static double VELOCITY = 30; + + private double previousVelocity; + + private long previousTimeNano; + + private Telemetry telemetryA; + + private boolean stopping; + private boolean end; + + /** + * This initializes the drive motors as well as the FTC Dashboard telemetry. + */ + @Override + public void init() { + poseUpdater = new PoseUpdater(hardwareMap); + + leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); + leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); + rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); + rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + + // TODO: Make sure that this is the direction your motors need to be reversed in. + leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + leftRear.setDirection(DcMotorSimple.Direction.REVERSE); + + motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear); + + for (DcMotorEx motor : motors) { + MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); + motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motor.setMotorType(motorConfigurationType); + } + + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + } + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("The robot will run to the right until it reaches " + VELOCITY + " inches per second."); + telemetryA.addLine("Then, it will cut power from the drivetrain and roll to a stop."); + telemetryA.addLine("Make sure you have enough room."); + telemetryA.addLine("After stopping, the lateral zero power acceleration (natural deceleration) will be displayed."); + telemetryA.addLine("Press CROSS or A on game pad 1 to stop."); + telemetryA.update(); + } + + /** + * This starts the OpMode by setting the drive motors to run forward at full power. + */ + @Override + public void start() { + leftFront.setPower(1); + leftRear.setPower(-1); + rightFront.setPower(-1); + rightRear.setPower(1); + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing CROSS or A on + * game pad 1 will stop the OpMode. When the robot hits the specified velocity, the robot will + * record its deceleration / negative acceleration until it stops. Then, it will average all the + * recorded deceleration / negative acceleration and print that value. + */ + @Override + public void loop() { + if (gamepad1.cross || gamepad1.a) { + requestOpModeStop(); + } + + poseUpdater.update(); + Vector heading = new Vector(1.0, poseUpdater.getPose().getHeading() - Math.PI/2); + if (!end) { + if (!stopping) { + if (MathFunctions.dotProduct(poseUpdater.getVelocity(), heading) > VELOCITY) { + previousVelocity = MathFunctions.dotProduct(poseUpdater.getVelocity(), heading); + previousTimeNano = System.nanoTime(); + stopping = true; + for (DcMotorEx motor : motors) { + motor.setPower(0); + } + } + } else { + double currentVelocity = MathFunctions.dotProduct(poseUpdater.getVelocity(), heading); + accelerations.add((currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / Math.pow(10.0, 9))); + previousVelocity = currentVelocity; + previousTimeNano = System.nanoTime(); + if (currentVelocity < FollowerConstants.pathEndVelocityConstraint) { + end = true; + } + } + } else { + double average = 0; + for (Double acceleration : accelerations) { + average += acceleration; + } + average /= (double)accelerations.size(); + + telemetryA.addData("lateral zero power acceleration (deceleration):", average); + telemetryA.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java new file mode 100644 index 0000000..aedccbd --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java @@ -0,0 +1,145 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +/** + * This is the StrafeVelocityTuner autonomous tuning OpMode. This runs the robot right at max + * power until it reaches some specified distance. It records the most recent velocities, and on + * reaching the end of the distance, it averages them and prints out the velocity obtained. It is + * recommended to run this multiple times on a full battery to get the best results. What this does + * is, when paired with ForwardVelocityTuner, allows FollowerConstants to create a Vector that + * empirically represents the direction your mecanum wheels actually prefer to go in, allowing for + * more accurate following. + * You can adjust the distance the robot will travel on FTC Dashboard: 192/168/43/1:8080/dash + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/13/2024 + */ +@Config +@Autonomous (name = "Strafe Velocity Tuner", group = "Autonomous Pathing Tuning") +public class StrafeVelocityTuner extends OpMode { + private ArrayList velocities = new ArrayList<>(); + + private DcMotorEx leftFront; + private DcMotorEx leftRear; + private DcMotorEx rightFront; + private DcMotorEx rightRear; + private List motors; + + private PoseUpdater poseUpdater; + + public static double DISTANCE = 40; + public static double RECORD_NUMBER = 10; + + private Telemetry telemetryA; + + private boolean end; + + /** + * This initializes the drive motors as well as the cache of velocities and the FTC Dashboard + * telemetry. + */ + @Override + public void init() { + poseUpdater = new PoseUpdater(hardwareMap); + + leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); + leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); + rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); + rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + + // TODO: Make sure that this is the direction your motors need to be reversed in. + leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + leftRear.setDirection(DcMotorSimple.Direction.REVERSE); + + motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear); + + for (DcMotorEx motor : motors) { + MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); + motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motor.setMotorType(motorConfigurationType); + } + + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + } + + for (int i = 0; i < RECORD_NUMBER; i++) { + velocities.add(0.0); + } + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches to the right."); + telemetryA.addLine("Make sure you have enough room, since the robot has inertia after cutting power."); + telemetryA.addLine("After running the distance, the robot will cut power from the drivetrain and display the strafe velocity."); + telemetryA.addLine("Press CROSS or A on game pad 1 to stop."); + telemetryA.update(); + } + + /** + * This starts the OpMode by setting the drive motors to run right at full power. + */ + @Override + public void start() { + leftFront.setPower(1); + leftRear.setPower(-1); + rightFront.setPower(-1); + rightRear.setPower(1); + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing CROSS or A on + * game pad1 will stop the OpMode. This continuously records the RECORD_NUMBER most recent + * velocities, and when the robot has run sideways enough, these last velocities recorded are + * averaged and printed. + */ + @Override + public void loop() { + if (gamepad1.cross || gamepad1.a) { + requestOpModeStop(); + } + + poseUpdater.update(); + if (!end) { + if (Math.abs(poseUpdater.getPose().getY()) > DISTANCE) { + end = true; + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } + } else { + double currentVelocity = Math.abs(MathFunctions.dotProduct(poseUpdater.getVelocity(), new Vector(1, Math.PI/2))); + velocities.add(currentVelocity); + velocities.remove(0); + } + } else { + double average = 0; + for (Double velocity : velocities) { + average += velocity; + } + average /= (double) velocities.size(); + + telemetryA.addData("strafe velocity:", average); + telemetryA.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StraightBackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StraightBackAndForth.java new file mode 100644 index 0000000..6224a92 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StraightBackAndForth.java @@ -0,0 +1,84 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; + +/** + * This is the StraightBackAndForth autonomous OpMode. It runs the robot in a specified distance + * straight forward. On reaching the end of the forward Path, the robot runs the backward Path the + * same distance back to the start. Rinse and repeat! This is good for testing a variety of Vectors, + * like the drive Vector, the translational Vector, and the heading Vector. Remember to test your + * tunings on CurvedBackAndForth as well, since tunings that work well for straight lines might + * have issues going in curves. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +@Config +@Autonomous (name = "Straight Back And Forth", group = "Autonomous Pathing Tuning") +public class StraightBackAndForth extends OpMode { + private Telemetry telemetryA; + + public static double DISTANCE = 40; + + private boolean forward = true; + + private Follower follower; + + private Path forwards; + private Path backwards; + + /** + * This initializes the Follower and creates the forward and backward Paths. Additionally, this + * initializes the FTC Dashboard telemetry. + */ + @Override + public void init() { + follower = new Follower(hardwareMap); + + forwards = new Path(new BezierLine(new Point(0,0, Point.CARTESIAN), new Point(DISTANCE,0, Point.CARTESIAN))); + forwards.setConstantHeadingInterpolation(0); + backwards = new Path(new BezierLine(new Point(DISTANCE,0, Point.CARTESIAN), new Point(0,0, Point.CARTESIAN))); + backwards.setConstantHeadingInterpolation(0); + + follower.followPath(forwards); + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("This will run the robot in a straight line going " + DISTANCE + + " inches forward. The robot will go forward and backward continuously" + + " along the path. Make sure you have enough room."); + telemetryA.update(); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the FTC Dashboard. + */ + @Override + public void loop() { + follower.update(); + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryA.addData("going forward", forward); + follower.telemetryDebug(telemetryA); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomPIDFCoefficients.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomPIDFCoefficients.java new file mode 100644 index 0000000..65a783a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomPIDFCoefficients.java @@ -0,0 +1,67 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +import kotlin.jvm.JvmField; + +/** + * This is the CustomPIDFCoefficients class. This class handles holding coefficients for PIDF + * controllers. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/5/2024 + */ +public class CustomPIDFCoefficients { + @JvmField public double P; + @JvmField public double I; + @JvmField public double D; + @JvmField public double F; + + public FeedForwardConstant feedForwardConstantEquation; + + private boolean usingEquation; + + /** + * This creates a new CustomPIDFCoefficients with constant coefficients. + * + * @param p the coefficient for the proportional factor. + * @param i the coefficient for the integral factor. + * @param d the coefficient for the derivative factor. + * @param f the coefficient for the feedforward factor. + */ + public CustomPIDFCoefficients(double p, double i, double d, double f) { + P = p; + I = i; + D = d; + F = f; + } + + /** + * This creates a new CustomPIDFCoefficients with constant PID coefficients and a variable + * feedforward equation using a FeedForwardConstant. + * + * @param p the coefficient for the proportional factor. + * @param i the coefficient for the integral factor. + * @param d the coefficient for the derivative factor. + * @param f the equation for the feedforward factor. + */ + public CustomPIDFCoefficients(double p, double i, double d, FeedForwardConstant f) { + usingEquation = true; + P = p; + I = i; + D = d; + feedForwardConstantEquation = f; + } + + /** + * This returns the coefficient for the feedforward factor. + * + * @param input this is inputted into the feedforward equation, if applicable. If there's no + * equation, then any input can be used. + * @return This returns the coefficient for the feedforward factor. + */ + public double getCoefficient(double input) { + if (!usingEquation) return F; + return feedForwardConstantEquation.getConstant(input); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FeedForwardConstant.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FeedForwardConstant.java new file mode 100644 index 0000000..0085e7c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FeedForwardConstant.java @@ -0,0 +1,21 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +/** + * This is the FeedForwardConstant interface. This interface holds a feedforward equation for PIDFs. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/5/2024 + */ +public interface FeedForwardConstant { + + /** + * This returns the coefficient for the feedforward factor. + * + * @param input this is inputted into the feedforward equation, if applicable. If there's no + * equation, then any input can be used. + * @return This returns the coefficient for the feedforward factor. + */ + double getConstant(double input); +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/NanoTimer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/NanoTimer.java new file mode 100644 index 0000000..6501093 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/NanoTimer.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +/** + * This is the NanoTimer class. It is an elapsed time clock with nanosecond precision, or at least + * as precise as the System.nanoTime() is. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/5/2024 + */ +public class NanoTimer { + private long startTime; + + /** + * This creates a new NanoTimer with the start time set to its creation time. + */ + public NanoTimer() { + resetTimer(); + } + + /** + * This resets the NanoTimer's start time to the current time using System.nanoTime(). + */ + public void resetTimer() { + startTime = System.nanoTime(); + } + + /** + * This returns the elapsed time in nanoseconds since the start time of the NanoTimer. + * + * @return this returns the elapsed time in nanoseconds. + */ + public long getElapsedTime() { + return System.nanoTime() - startTime; + } + + /** + * This returns the elapsed time in seconds since the start time of the NanoTimer. + * + * @return this returns the elapsed time in seconds. + */ + public double getElapsedTimeSeconds() { + return (getElapsedTime() / Math.pow(10.0,9)); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/PIDFController.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/PIDFController.java new file mode 100644 index 0000000..0b59050 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/PIDFController.java @@ -0,0 +1,223 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +/** + * This is the PIDFController class. This class handles the running of PIDFs. PIDF stands for + * proportional, integral, derivative, and feedforward. PIDFs take the error of a system as an input. + * Coefficients multiply into the error, the integral of the error, the derivative of the error, and + * a feedforward value. Then, these values are added up and returned. In this way, error in the + * system is corrected. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/5/2024 + */ +public class PIDFController { + private CustomPIDFCoefficients coefficients; + + private double previousError; + private double error; + private double position; + private double targetPosition; + private double errorIntegral; + private double errorDerivative; + private double feedForwardInput; + + private long previousUpdateTimeNano; + private long deltaTimeNano; + + /** + * This creates a new PIDFController from a CustomPIDFCoefficients. + * + * @param set the coefficients to use. + */ + public PIDFController(CustomPIDFCoefficients set) { + setCoefficients(set); + reset(); + } + + /** + * This takes the current error and runs the PIDF on it. + * + * @return this returns the value of the PIDF from the current error. + */ + public double runPIDF() { + return error * P() + errorDerivative * D() + errorIntegral * I() + F(); + } + + /** + * This can be used to update the PIDF's current position when inputting a current position and + * a target position to calculate error. This will update the error from the current position to + * the target position specified. + * + * @param update This is the current position. + */ + public void updatePosition(double update) { + position = update; + previousError = error; + error = targetPosition - position; + + deltaTimeNano = System.nanoTime() - previousUpdateTimeNano; + previousUpdateTimeNano = System.nanoTime(); + + errorIntegral += error * (deltaTimeNano / Math.pow(10.0, 9)); + errorDerivative = (error - previousError) / (deltaTimeNano / Math.pow(10.0, 9)); + } + + /** + * As opposed to updating position against a target position, this just sets the error to some + * specified value. + * + * @param error The error specified. + */ + public void updateError(double error) { + previousError = this.error; + this.error = error; + + deltaTimeNano = System.nanoTime() - previousUpdateTimeNano; + previousUpdateTimeNano = System.nanoTime(); + + errorIntegral += error * (deltaTimeNano / Math.pow(10.0, 9)); + errorDerivative = (error - previousError) / (deltaTimeNano / Math.pow(10.0, 9)); + } + + /** + * This can be used to update the feedforward equation's input, if applicable. + * + * @param input the input into the feedforward equation. + */ + public void updateFeedForwardInput(double input) { + feedForwardInput = input; + } + + /** + * This resets all the PIDF's error and position values, as well as the time stamps. + */ + public void reset() { + previousError = 0; + error = 0; + position = 0; + targetPosition = 0; + errorIntegral = 0; + errorDerivative = 0; + previousUpdateTimeNano = System.nanoTime(); + } + + /** + * This is used to set the target position if the PIDF is being run with current position and + * target position inputs rather than error inputs. + * + * @param set this sets the target position. + */ + public void setTargetPosition(double set) { + targetPosition = set; + } + + /** + * This returns the target position of the PIDF. + * + * @return this returns the target position. + */ + public double getTargetPosition() { + return targetPosition; + } + + /** + * This is used to set the coefficients of the PIDF. + * + * @param set the coefficients that the PIDF will use. + */ + public void setCoefficients(CustomPIDFCoefficients set) { + coefficients = set; + } + + /** + * This returns the PIDF's current coefficients. + * + * @return this returns the current coefficients. + */ + public CustomPIDFCoefficients getCoefficients() { + return coefficients; + } + + /** + * This sets the proportional (P) coefficient of the PIDF only. + * + * @param set this sets the P coefficient. + */ + public void setP(double set) { + coefficients.P = set; + } + + /** + * This returns the proportional (P) coefficient of the PIDF. + * + * @return this returns the P coefficient. + */ + public double P() { + return coefficients.P; + } + + /** + * This sets the integral (I) coefficient of the PIDF only. + * + * @param set this sets the I coefficient. + */ + public void setI(double set) { + coefficients.I = set; + } + + /** + * This returns the integral (I) coefficient of the PIDF. + * + * @return this returns the I coefficient. + */ + public double I() { + return coefficients.I; + } + + /** + * This sets the derivative (D) coefficient of the PIDF only. + * + * @param set this sets the D coefficient. + */ + public void setD(double set) { + coefficients.D = set; + } + + /** + * This returns the derivative (D) coefficient of the PIDF. + * + * @return this returns the D coefficient. + */ + public double D() { + return coefficients.D; + } + + /** + * This sets the feedforward (F) constant of the PIDF only. + * + * @param set this sets the F constant. + */ + public void setF(double set) { + coefficients.F = set; + } + + /** + * This returns the feedforward (F) constant of the PIDF. + * + * @return this returns the F constant. + */ + public double F() { + return coefficients.getCoefficient(feedForwardInput); + } + + /** + * This returns the current error of the PIDF. + * + * @return this returns the error. + */ + public double getError() { + return error; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/SingleRunAction.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/SingleRunAction.java new file mode 100644 index 0000000..48a5702 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/SingleRunAction.java @@ -0,0 +1,60 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +/** + * This is the SingleRunAction class. It handles running Runnables once, until a reset is called. + * It also forms the basis of the PathCallback class. Basically, if you want to run a certain action + * once despite looping through a section of code multiple times, then this is for you. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/6/2024 + */ +public class SingleRunAction { + private boolean hasBeenRun; + + private Runnable runnable; + + /** + * This creates a new SingleRunAction with a Runnable containing the code to be run for this action. + * The name is a slight bit misleading, as this can actually be run multiple times. However, the + * run() method only runs once before the reset() method needs to be called to allow the + * SingleRunAction to run again. + * + * @param runnable This is a Runnable containing the code to be run. Preferably, use lambda statements. + */ + public SingleRunAction(Runnable runnable) { + this.runnable = runnable; + } + + /** + * This returns if the SingleRunAction has been run yet. Running reset() will reset this. + * + * @return This returns if it has been run. + */ + public boolean hasBeenRun() { + return hasBeenRun; + } + + /** + * This runs the Runnable of the SingleRunAction. It will only run once before requiring a reset. + * + * @return This returns if the Runnable was successfully run. If not, then a reset is needed to run again. + */ + public boolean run() { + if (!hasBeenRun) { + hasBeenRun = true; + runnable.run(); + return true; + } + return false; + } + + /** + * This resets the SingleRunAction and makes it able to run again. The SingleRunAction is set + * to "has not been run", allowing for multiple uses of the Runnable. + */ + public void reset() { + hasBeenRun = false; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Timer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Timer.java new file mode 100644 index 0000000..a606043 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Timer.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +/** + * This is the Timer class. It is an elapsed time clock with millisecond precision, or at least as + * precise as the System.currentTimeMillis() is. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/5/2024 + */ +public class Timer { + private long startTime; + + /** + * This creates a new Timer with the start time set to its creation time. + */ + public Timer() { + resetTimer(); + } + + /** + * This resets the Timer's start time to the current time using System.currentTimeMillis(). + */ + public void resetTimer() { + startTime = System.currentTimeMillis(); + } + + /** + * This returns the elapsed time in milliseconds since the start time of the Timer. + * + * @return this returns the elapsed time in milliseconds. + */ + public long getElapsedTime() { + return System.currentTimeMillis() - startTime; + } + + /** + * This returns the elapsed time in seconds since the start time of the Timer. + * + * @return this returns the elapsed time in seconds. + */ + public double getElapsedTimeSeconds() { + return (getElapsedTime() / 1000.0); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md new file mode 100644 index 0000000..4d1da42 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md @@ -0,0 +1,131 @@ +## TeamCode Module + +Welcome! + +This module, TeamCode, is the place where you will write/paste the code for your team's +robot controller App. This module is currently empty (a clean slate) but the +process for adding OpModes is straightforward. + +## Creating your own OpModes + +The easiest way to create your own OpMode is to copy a Sample OpMode and make it your own. + +Sample opmodes exist in the FtcRobotController module. +To locate these samples, find the FtcRobotController module in the "Project/Android" tab. + +Expand the following tree elements: + FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples + +### Naming of Samples + +To gain a better understanding of how the samples are organized, and how to interpret the +naming system, it will help to understand the conventions that were used during their creation. + +These conventions are described (in detail) in the sample_conventions.md file in this folder. + +To summarize: A range of different samples classes will reside in the java/external/samples. +The class names will follow a naming convention which indicates the purpose of each class. +The prefix of the name will be one of the following: + +Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure + of a particular style of OpMode. These are bare bones examples. + +Sensor: This is a Sample OpMode that shows how to use a specific sensor. + It is not intended to drive a functioning robot, it is simply showing the minimal code + required to read and display the sensor values. + +Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base. + It may be used to provide a common baseline driving OpMode, or + to demonstrate how a particular sensor or concept can be used to navigate. + +Concept: This is a sample OpMode that illustrates performing a specific function or concept. + These may be complex, but their operation should be explained clearly in the comments, + or the comments should reference an external doc, guide or tutorial. + Each OpMode should try to only demonstrate a single concept so they are easy to + locate based on their name. These OpModes may not produce a drivable robot. + +After the prefix, other conventions will apply: + +* Sensor class names are constructed as: Sensor - Company - Type +* Robot class names are constructed as: Robot - Mode - Action - OpModetype +* Concept class names are constructed as: Concept - Topic - OpModetype + +Once you are familiar with the range of samples available, you can choose one to be the +basis for your own robot. In all cases, the desired sample(s) needs to be copied into +your TeamCode module to be used. + +This is done inside Android Studio directly, using the following steps: + + 1) Locate the desired sample class in the Project/Android tree. + + 2) Right click on the sample class and select "Copy" + + 3) Expand the TeamCode/java folder + + 4) Right click on the org.firstinspires.ftc.teamcode folder and select "Paste" + + 5) You will be prompted for a class name for the copy. + Choose something meaningful based on the purpose of this class. + Start with a capital letter, and remember that there may be more similar classes later. + +Once your copy has been created, you should prepare it for use on your robot. +This is done by adjusting the OpMode's name, and enabling it to be displayed on the +Driver Station's OpMode list. + +Each OpMode sample class begins with several lines of code like the ones shown below: + +``` + @TeleOp(name="Template: Linear OpMode", group="Linear Opmode") + @Disabled +``` + +The name that will appear on the driver station's "opmode list" is defined by the code: + ``name="Template: Linear OpMode"`` +You can change what appears between the quotes to better describe your opmode. +The "group=" portion of the code can be used to help organize your list of OpModes. + +As shown, the current OpMode will NOT appear on the driver station's OpMode list because of the + ``@Disabled`` annotation which has been included. +This line can simply be deleted , or commented out, to make the OpMode visible. + + + +## ADVANCED Multi-Team App management: Cloning the TeamCode Module + +In some situations, you have multiple teams in your club and you want them to all share +a common code organization, with each being able to *see* the others code but each having +their own team module with their own code that they maintain themselves. + +In this situation, you might wish to clone the TeamCode module, once for each of these teams. +Each of the clones would then appear along side each other in the Android Studio module list, +together with the FtcRobotController module (and the original TeamCode module). + +Selective Team phones can then be programmed by selecting the desired Module from the pulldown list +prior to clicking to the green Run arrow. + +Warning: This is not for the inexperienced Software developer. +You will need to be comfortable with File manipulations and managing Android Studio Modules. +These changes are performed OUTSIDE of Android Studios, so close Android Studios before you do this. + +Also.. Make a full project backup before you start this :) + +To clone TeamCode, do the following: + +Note: Some names start with "Team" and others start with "team". This is intentional. + +1) Using your operating system file management tools, copy the whole "TeamCode" + folder to a sibling folder with a corresponding new name, eg: "Team0417". + +2) In the new Team0417 folder, delete the TeamCode.iml file. + +3) the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder + to a matching name with a lowercase 'team' eg: "team0417". + +4) In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that contains + package="org.firstinspires.ftc.teamcode" + to be + package="org.firstinspires.ftc.team0417" + +5) Add: include ':Team0417' to the "/settings.gradle" file. + +6) Open up Android Studios and clean out any old files by using the menu to "Build/Clean Project"" \ No newline at end of file diff --git a/TeamCode/src/main/res/raw/readme.md b/TeamCode/src/main/res/raw/readme.md new file mode 100644 index 0000000..355a3c4 --- /dev/null +++ b/TeamCode/src/main/res/raw/readme.md @@ -0,0 +1 @@ +Place your sound files in this folder to use them as project resources. \ No newline at end of file diff --git a/TeamCode/src/main/res/values/strings.xml b/TeamCode/src/main/res/values/strings.xml new file mode 100644 index 0000000..d781ec5 --- /dev/null +++ b/TeamCode/src/main/res/values/strings.xml @@ -0,0 +1,4 @@ + + + + diff --git a/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml b/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml new file mode 100644 index 0000000..22ae7a8 --- /dev/null +++ b/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml @@ -0,0 +1,161 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/build.common.gradle b/build.common.gradle new file mode 100644 index 0000000..12e6acb --- /dev/null +++ b/build.common.gradle @@ -0,0 +1,125 @@ +/** + * build.common.gradle + * + * Try to avoid editing this file, as it may be updated from time to time as the FTC SDK + * evolves. Rather, if it is necessary to customize the build process, do those edits in + * the build.gradle file in TeamCode. + * + * This file contains the necessary content of the 'build.gradle' files for robot controller + * applications built using the FTC SDK. Each individual 'build.gradle' in those applications + * can simply contain the one line: + * + * apply from: '../build.common.gradle' + * + * which will pick up this file here. This approach allows makes it easier to integrate + * updates to the FTC SDK into your code. + */ + +import java.util.regex.Pattern + +apply plugin: 'com.android.application' + +android { + + compileSdkVersion 29 + + signingConfigs { + release { + def apkStoreFile = System.getenv("APK_SIGNING_STORE_FILE") + if (apkStoreFile != null) { + keyAlias System.getenv("APK_SIGNING_KEY_ALIAS") + keyPassword System.getenv("APK_SIGNING_KEY_PASSWORD") + storeFile file(System.getenv("APK_SIGNING_STORE_FILE")) + storePassword System.getenv("APK_SIGNING_STORE_PASSWORD") + } else { + keyAlias 'androiddebugkey' + keyPassword 'android' + storeFile rootProject.file('libs/ftc.debug.keystore') + storePassword 'android' + } + } + + debug { + keyAlias 'androiddebugkey' + keyPassword 'android' + storeFile rootProject.file('libs/ftc.debug.keystore') + storePassword 'android' + } + } + + aaptOptions { + noCompress "tflite" + } + + defaultConfig { + signingConfig signingConfigs.debug + applicationId 'com.qualcomm.ftcrobotcontroller' + minSdkVersion 24 + //noinspection ExpiredTargetSdkVersion + targetSdkVersion 28 + + /** + * We keep the versionCode and versionName of robot controller applications in sync with + * the master information published in the AndroidManifest.xml file of the FtcRobotController + * module. This helps avoid confusion that might arise from having multiple versions of + * a robot controller app simultaneously installed on a robot controller device. + * + * We accomplish this with the help of a funky little Groovy script that maintains that + * correspondence automatically. + * + * @see Configure Your Build + * @see Versioning Your App + */ + def manifestFile = project(':FtcRobotController').file('src/main/AndroidManifest.xml'); + def manifestText = manifestFile.getText() + // + def vCodePattern = Pattern.compile("versionCode=\"(\\d+(\\.\\d+)*)\"") + def matcher = vCodePattern.matcher(manifestText) + matcher.find() + def vCode = Integer.parseInt(matcher.group(1)) + // + def vNamePattern = Pattern.compile("versionName=\"(.*)\"") + matcher = vNamePattern.matcher(manifestText); + matcher.find() + def vName = matcher.group(1) + // + versionCode vCode + versionName vName + } + + // http://google.github.io/android-gradle-dsl/current/com.android.build.gradle.internal.dsl.BuildType.html + buildTypes { + release { + signingConfig signingConfigs.release + + ndk { + abiFilters "armeabi-v7a", "arm64-v8a" + } + } + debug { + debuggable true + jniDebuggable true + ndk { + abiFilters "armeabi-v7a", "arm64-v8a" + } + } + } + + compileOptions { + sourceCompatibility JavaVersion.VERSION_1_8 + targetCompatibility JavaVersion.VERSION_1_8 + } + + packagingOptions { + pickFirst '**/*.so' + } + sourceSets.main { + jni.srcDirs = [] + jniLibs.srcDir rootProject.file('libs') + } + ndkVersion '21.3.6528147' +} + +repositories { +} + diff --git a/build.dependencies.gradle b/build.dependencies.gradle new file mode 100644 index 0000000..9d26ae5 --- /dev/null +++ b/build.dependencies.gradle @@ -0,0 +1,25 @@ +repositories { + mavenCentral() + google() // Needed for androidx + + maven { url = 'https://maven.brott.dev/' } +} + +dependencies { + implementation 'org.firstinspires.ftc:Inspection:9.1.0' + implementation 'org.firstinspires.ftc:Blocks:9.1.0' + implementation 'org.firstinspires.ftc:Tfod:9.1.0' + implementation 'org.firstinspires.ftc:RobotCore:9.1.0' + implementation 'org.firstinspires.ftc:RobotServer:9.1.0' + implementation 'org.firstinspires.ftc:OnBotJava:9.1.0' + implementation 'org.firstinspires.ftc:Hardware:9.1.0' + implementation 'org.firstinspires.ftc:FtcCommon:9.1.0' + implementation 'org.firstinspires.ftc:Vision:9.1.0' + implementation 'org.firstinspires.ftc:gameAssets-CenterStage:1.0.0' + implementation 'org.tensorflow:tensorflow-lite-task-vision:0.4.3' + runtimeOnly 'org.tensorflow:tensorflow-lite:2.12.0' + implementation 'androidx.appcompat:appcompat:1.2.0' + + implementation 'com.acmerobotics.dashboard:dashboard:0.4.5' +} + diff --git a/build.gradle b/build.gradle new file mode 100644 index 0000000..b7a9997 --- /dev/null +++ b/build.gradle @@ -0,0 +1,29 @@ +/** + * Top-level build file for ftc_app project. + * + * It is extraordinarily rare that you will ever need to edit this file. + */ + +buildscript { + repositories { + mavenCentral() + google() + } + dependencies { + // Note for FTC Teams: Do not modify this yourself. + classpath 'com.android.tools.build:gradle:8.2.0' + } +} + +// This is now required because aapt2 has to be downloaded from the +// google() repository beginning with version 3.2 of the Android Gradle Plugin +allprojects { + repositories { + mavenCentral() + google() + } +} + +repositories { + mavenCentral() +} diff --git a/doc/legal/AudioBlocksSounds.txt b/doc/legal/AudioBlocksSounds.txt new file mode 100644 index 0000000..4eab3bc --- /dev/null +++ b/doc/legal/AudioBlocksSounds.txt @@ -0,0 +1,21 @@ +The sound files listed below in this SDK were purchased from www.audioblocks.com under the +following license. + + http://support.audioblocks.com/customer/en/portal/topics/610636-licensing-faq-s/articles + + How am I allowed to use your content? + Last Updated: Aug 11, 2016 01:51PM EDT + Our content may be used for nearly any project, commercial or otherwise, including feature + films, broadcast, commercial, industrial, educational video, print projects, multimedia, + games, and the internet, so long as substantial value is added to the content. (For example, + incorporating an audio clip into a commercial qualifies, while reposting our audio clip on + YouTube with no modification or no visual component does not.) Once you download a file it is + yours to keep and use forever, royalty- free, even if you change your subscription or cancel + your account. + +List of applicable sound files + + chimeconnect.wav + chimedisconnect.wav + errormessage.wav + warningmessage.wav \ No newline at end of file diff --git a/doc/legal/Exhibit A - LEGO Open Source License Agreement.txt b/doc/legal/Exhibit A - LEGO Open Source License Agreement.txt new file mode 100644 index 0000000..10c13b9 --- /dev/null +++ b/doc/legal/Exhibit A - LEGO Open Source License Agreement.txt @@ -0,0 +1,15 @@ +EXHIBIT A - LEGO® Open Source License Agreement + +The contents of the file 'nxtstartupsound.wav' contained in this SDK are subject to the +LEGO® Open Source License Agreement Version 1.0 (the "License"); you may not use this +file except in compliance with the License. You may obtain a copy of the License +at "LEGO Open Source License.pdf" contained in the same directory as this exhibit. + +Software distributed under the License is distributed on an "AS IS" basis, WITHOUT +WARRANTY OF ANY KIND, either express or implied. See the License for the specific +language governing rights and limitations under the License. + +The Original Code is \AT91SAM7S256\Resource\SOUNDS\!Startup.rso. +LEGO is the owner of the Original Code. Portions created by Robert Atkinson are +Copyright (C) 2015. All Rights Reserved. +Contributor(s): Robert Atkinson. \ No newline at end of file diff --git a/doc/legal/LEGO Open Source License.pdf b/doc/legal/LEGO Open Source License.pdf new file mode 100644 index 0000000000000000000000000000000000000000..9188498471221a53237117c43b1630ed8c605358 GIT binary patch literal 34100 zcmdSAb9iN2vo9Rmw#^mWwr$(C(@8o`I<`BuyJL22+qTpB())eSIs1IizUSP3?|jyp zbFMiDs%reIM%AdX$Q4D!>6sYV;K_G(CuiYdn3&j!07MSP*6_T%jIvhtW=76d-e#sm zOpLNbEUZib0HXpC6B8$+7!e0E2RoxY5eK6x5eo|kI}4*Ekq!|n2Nw|&7qcEeKfIZ} z>7QnZ{{P2kAIx0;)rY8=iG!(`qLH(m%V&=w4z_M~_AW%MjEc@?rdB4d4$eeO%>4X} z;#RhA!6nWt#LB@85a;0HVqpacGjWImM1@(Hm_%6EMMXvU zc>$la33IZthyd6{xR_bRxY)VanK?z+MA^95g_u~``1Kg2>`l!)b^dV94EXcQ+~Jc& zwoek38D-7vEnF>$*t!1D8=rLLz4h^$W+_l0`s)Iqe!o zrI!Y38Vl}xX9qAze2w{TrtjhtOQoz2YQ zVF2(j`Ve4S;G?xb#6w_27CGuNSLF|-4~ZIBSn5@V@gSfGP=YBUAZvDED`y_d$#sC{E+H^$ei3zMNgT@tF5OfhP!UuX(d27l!9ZZ3_yx3{e_-%mQvMG( z8rfS={r&(F1Cj@#2T}u4p=Ojba<=(1V4u+WbNdNYS7$e~PrS&Pxf+=oxf*?P_SY~e z8d;dh8hJUmxiTu)oBa!BjDH@we2(P5il}^6HE~sSHZ%Lz0CWCT@vmVO`E1G7!Q#JA z{iyTZ!{QLS>PoMAqLf7Z@kFxzK{}-0@7)6{N z9EBY`bpQ;X4SjyunV3J%99*Agy-$`*t&IMUZ$CBYlZO9fM%m28!Ohvk?B7b|;H=_k zWMcM5!&CsDaQ%zUKWgGS?CLisKqY(@n7=!S3fSF zzg3NlT^LpVh5J8}{JV7irr`gex#CvNF0Mo@T>k+%4t9=DOTzZArWA~=Rb1`WH(uc-fD<7JlX1ys@7=@0e5p5y#dEQ-*jI6O_7D|Wh`=y6R9oj(CY(i??m#=%T zGnewE2BsuwhRg}F=CxEMaJEUIQdM-etV!RcmPos><0QQF=+aaXNG@_17@ytv7|X+@ zjFkB&a=Xy>o#$&=)!tO#H)REni=+d$AEh+psPp)D6Nd`NLBEzOX6w zT}DZ~7HqH&s}D{$JO4C<5KeTS1tB76If14q|}@T*ch#{#A7p zh}jAId56F*eZj}z?ZN-bPS!j53ru7Zg_6?X2vD@3aBX9*_PB6LNmv{x*flzRKuB%e>z+2!w z-f-%DRvYGs+%yPo+@wA`nb!HxFM+l-n?pJ-NT_#I zo%UND%wLe*=E${=e8{$I<2|8LgWJuxZ8(=t9;^Yf;`?9`97^`43!?~w1F%B#{WL9I zrb%!8v1GDn&1%8z?RSkw=dCc7Ld%+q2L$7gK?e|Jp1-U;=7O~Dze7mH0G@p1Xe6|6~;ZGBf`+ivOTq|1oJX z|ILO=Muw`c+OGoO{2n!~roH1yMp|&FVD@&SO#{8u=j$lX3ttuUp!@|Ik`>&z9hEGpa4tt zi9k(e=dC2f#?3G#2Y>$%Y{lnhVWKsfgh;IT*u8m?hLV6Ay}lDth6JF2qoOqq@CgOu zN8SpbH2WC0a%04L${Stq2^pjBkM2^zId5Gq-%x4%$Uo{({<2lnj5Vw|G?o~^Ljh_x zqn?@Ejm+}bCX8VRq3j7l$w)O;Oj3u;1_YkITQ`mN5gqmC0HxqrJiQrwYW=g#ETjtV_a2vK3z;uf z5!%f~H&7PSOwg}{J+w^`-8)xtJo0xMpR3H62>me^x6g)1K!v@{SCntXD$?d9KbWgf z?Z|{U7$_k{bYGFemMLpZ=kbW0*5SX17H-@oL!)_5Q^LF57z)pGJK_h`Z=-pKNx_-1 zewPm}vO7dv96qT;s{7rbC|uBii5-fK?iY=dZP<25MzElQ!&ktun;*riXKO!Vj>h_e z7mZo#$CBTN4Ru49R2u?t!dSVRHJm1@u0mbM$sMCZ`>H&7Xy_sbE7p};Gs3|2IP|cf zsfZ0N^+D_??;KG0zM6Ne!F6%(Ut?^LQko0NEyp7)f*y??wbf=RK|G#;DOF3*Or6nf zP(1>yoS4MmIyRCb8R^FxWBc;z7Ti!qMavTF21>gL?;HB%NlZnZ6^I~Ir(xUEs%giA zp+T%j62E47dmv}xj*gI#Os+s{5y>=nt^4@cRL_8$YWwVLEg|!#R=RJ)hHM_yeeWpJ zMcSp!(n#vnQ%ob9LE7)QOOlZ3Ef8vmbH#D*z^q@dudN1aA%hlAwB10y$0U*e2+%*? z&;JTgHkSWpQ!7a*Uaj{R!EdbV} zwe8tchWn5idTfRcH%KSwqDW(kyvH&rgIZ{O9&(PW3`RzgKq7C2FILFT3B)=CV(z9J zrn!!t*e)26p(ITyB^o99JN^LcePn-RpYFRL{teq#47b4bd*_fjAid@TjB=j}Vh9Pii%XlO5N;j%JVy60Quc3jE%>wu?M<`CbU7c&=zHl+? z{#C*0ClhOS{3LB>@)IRxp&Ro^1$t}Bg@|W zeu47ZKte^V`f=pBX2n=7b1b&HYL{Ml_Xe`fA}%r!@&-S`GU%PCR<5X%0X?MGThOVQuIz zf=4APh--cOIGU#AhVkoZ%BK8`({-tZ^UEvWkyk+@Jrw0nIN#KL9VvbPBK@aT+f1B$ z(W4yO0_)g(#4DLk%LfA{?Dmhq`s4fm@4#aIZ z@MuSSa+^&~2xz{Rs6jYpFIb2iIFBrTU?=J0 z3M*j){!zf5M@hS(^i$Iidbg^IN~c zzqM}!Y~i|iroGmySmAGgbyZEJAR94mcol@hA)nBpWsBM2axpT>+-OTF?qRHi)Bd_F zSPjbahoz*vw?DkW0~{Y8FLPn1N=c<5M@~JhgUF(-nuikl?1d(4p9jCblQ=WuqtdjF zwY?&)6B$ZE`R3G$RH?ffw2{x?8H5fi(Di#M@;2n+mKEV9W2#Eg5g^q&$kC!bC2&y2 zD{tgzC^CA+t2&m#*8vU#WUEwlD;&gnBw>u35M4FE{eE>Ak|75XKp#P_Gz#f8c)qSp zpG68R!(9}M)WtSg5pU5=t(Ofoqo=;E#Rw1@3PrgnWmT(m3ojO^8v|4VFf6KGl5T!OCG5!kubu-#*uzMCm($e(UK%{ z$cDHivyVur$}KTcjiKtkW7r3+*oMiTi%V7!`J-LfY$&xMWB0RBU*!cu+SnE=Z2uIj zR)8hG&EDfPQ1oy3t!|ZaQFgBxMkDP=u1p(W&>x8Poq}|YlIY^ARFOg=EBJo5=HU)k zds-|JcW2P-Y~S&bGaPqVL&I$y4;>6RPLx>@O0hDPbbRM)ZSo#Nylj#zDoaQYy1C9b zklRBjLfUycL5kCeq&!<`1-KFvu(C zSjh7DFr>CuFe;TPJ7r?L9z#0G(>7!diw*m z_ra2$rF9wHW@h!T?}-ON>dH!5!@StoiQ&y#}pXV#_*bWAwgeGPJJz^aejWIW(Z$4aK2c-8oyqx zKdUXK8TrEM&LYDVCbfkD4=u5tg4s#<=IDBQF&4<7^T-{+7$kBQt6e$9g^qDWz>AF= zXM@~&n+;yJ=$9E%D-~dDvqbq-r#|GG!JPr?RNmJ^=Ys?B4%RU)`r-$tUNF>~e+J#_ z6?r$1L6d&dKceez3r7Fv*n*9d_3!8^QkDC3)R2C>YD9B-(JpvqFDXJhSq9T6hE&vl zkuLh=D57F8R(Q7TBPX51#+c08it+S4E&cIeJo?=(Ingch*N8&l7f-7}G>>ye6P74U zXUkfGF{`6|fT9E8JfTbP% zp~g>^LFF~k^AS;Pff_ZzKfAxbZ0pQI&QVfPfvD1U$4@Tdk6tX5VZN7Y+IJz?jO5NP zBc9I8D@3tB0hl~cQng$mYMlGRf_`mg5T?!5%nOVv?hGuYs0_@hz$~Y41>zO0v5t2{ zydn6_yGIZ&E9sY3vd^8*mkPcG{CfR`f(Ne;Rimo5$h_@tMVzuJ2pEjs>9(TbS7+9I z+Da22Gp8w2Hc>eo$9r=@yAXx&HeHdXxGVa)iH6R=8jO*jmI2B9E+b+aAJCz(cfeoT zn6L}it)7ng1o^O%4f4j}EMM|CXF@y-<|EjAOfgqF3BI`+x8 z5`I073ROYe$qpB)fHLW{pM&Jko$>g~7-U-8gV;9ch zx?;t3-$iQ%xg(40xoyqt?8(Y&?Z;IMsxFM^I++rVTBd90V(vE%Am6I$E`=?pePC?& zE1V(+pP8kgfbH=MtOEG^%xM`d;F&Lyq~610W9k|LHCdoeHOEwbJ0+IXWlE8FFr!}lTwczn9Y%7Fc%WwXOF5H1i}&u7YPKj6We%#88de;^ zpWM6-EVboMXc|04xOFU_Ko|XJ3Uphgw6gh`WAX#6xN(ebqGZ?pV`hYA#bw70-c-P4 zu^&ObhIQ~sP3@g3nDsF8g&H9j)>;w zh=tsDFa?#VsIib(Ko<&i z+$Z%@h*dwy^mB6%Mb&lOI&mO5fgyzB*N9^TBaD84GRINUPV<%CE^WgUT@yVu zjqHH8=G^8UO)gHQ!DpL6olMCw;Y6h@m~Qc@v}$3Y6F{HL-OCrIsk`q3!lsyTzL^-d zac!q!RqTrPITofcG^Tf{(MaY8u9RGadz5bDqVdl42SsayxK4z14^=RbK}Gc*gBFWxI;x3{1B{hUy# zl*guU^Cy<|wGsX2SoY6q?f*Ifv$C`N9h0t~9$lXxT<0OJFX`7MDj7*wi#b@79YOmD zk(IjvQXO>3flU*iPNcHdq7MIPJ`;z`%*qTjx4gs*2A|=UiCmT62flf{4^QNeOKYYC zPW)*3;0J-sin)-w-1FZc%w&HyPAE_X0N*v; z;^FcXNeEh#`teq~DSEOIZ7SxxdO9j^!?pEAzzEL)hOnC|Z))e<(G2kM^T!3Ut71pW z)5fV!E$aBzNMCLd(*(~%2Wpk*z)Vn)Tc#+`n)3$1tB5JuJ#u2MMSxikHC`78Zt`)* zjaa7hBNCJYpUQLA5iJ@+Ya$H`B}rvS1@qx+i5tcZ_up8N#e!IbBRPpWS&jlkRco`0 z&%{Z}cl}t~5%iAt5lN4?BTx*Db0wWfmDyCqxCUHCe%RzN8(^4u35^GzIc|xl6=Tj9 zoar-R;^L2~-(!heEIWJ2$(Ib>+f-LxsCg@&ZHXcDM%Rt*MCqhh$-%hln#@k38kZi+ zOt+YtxYDqn_+APe(?^O3p~=Na8jfwKn^yM!&q) zgjoz~kK5N!XR=}YK?8t_HJ{b~(Ng^_Bk+H=RLo3&l4eYQM~1PXVb=E1&fX|lcXxcI zhVmZ49u_t-c5%0VPEbrxIxrkc(pOkKP!M0iv(C3pRS3ANa~x38h@X|P%&;m1jD~Uy zR5ZXdkbz57gK)d7LI7@$`ww@a7?Nk)~h3xImZj1=$UxtidItd$_vt z>Zw@x3PpRSW@eOon8*dF)rec`Tglj3G<#SWxw^`Tx~S^N*ucv$tRbPUIxxxAX<8`{ z4k&1_jn$N$w50t-9GfAgIxHCF9+#Zf-jWm!S!#C0X{652HfA%IaP zZQ}>v>I327KqmPWIb^USKj$pU=hyRcmGw${<$B&_OugZsbN5*)0_z4p-dmo3zqSxM z1bA7z9DE`4_x7-<-q_f1S+^cA)JaJCvKWC-C^uo>3H%|KrHt zf1YXN{nAtf0+)bWhuT<~SQ0229*U%%&&jShnjv&5;#Gq9}c4vJD zTkbgT`QS;OE5a<}0G&;q01B_oA0K`EC$sJkeL38P2AT*R{REXhvPZL5qiphhh4VcQ z#0IhZo&5E*)#LDxL)Cd#clE5eN)ZH(7Gd3I>?^~~zq%+^h+o^7N9`~R$iGNp5OLqs zu-jIwCFAS0h<)ka`i&@cvCFiJib)gjwpt8<8&Aal7-16kh9i@^J9X-*VYb)HS$vQ!H;%8t$+(T{%B6)nWR;Q;Kw)wbE2J9}i5P4uriX=D z@-gmUWWC6~DteD&=YI>KM)ZxTLP20+UY(ljwcrx==efHx=j6;O@X9^$337GQH<{;n zYxvSU(|s)WA1h^w9wFVCNfjJTzg$=brd!)>Xx_wh`|ezDdu->$g=VqWRdrHplEO6= ze3DwL$gYgSJ<7KAibuzqkm&TwF)SQo$uBvo~kw%|x4izh062QhIaU$Be{OG|nds5Qg6(@p-l(WJSI(#O5? z4FRl6HLYf}`m~wk*Ni}-#a`o{3Zbh#`JQqT4<^hpGWe8n*j0VG#&0PHo4lnq*kO^d z0mv#M@%OIjR2D@KnfR9zPsQq;mOhphl42zoVq@&HP+%dI!RhSY`c`Q4Ye;R8-lb7B zCphv(%mtNEcC$X$yr!AgxT0-qy;Lu~!!X^N+=o{Y7Xg-=-HI?F?vm7`dP6{`39rr= zLaZuuPuiHYzlrnjq?^(n>rR22%XeENdnJ@N*VtR$d*w8z6I&yNPI(7Ts>0-`*?I*( zRQC)-<+n;KLZJ$u+sJ^esCP{L=YDCFp6_8Mx$EQ_(2o+_#^+AI^wzwW1Vx!R@ehPp z#(>-Q`tSBa$8UXpzol7RXzp0iZp+}=RhWvc;Se@8(TQX2Y_Ny1+ow}}azgeNwDMubQJhdV&LykPr;hVRh>%c?p;BDF?TRbov zEICogG1~4k0nkhoUcV! zp0vq=kO-0>o(n|rNQsZ*Y6c{=LE~Y%ZTkU&=D-JHOgdH(4{0mE?1x3>-)D69M;KCe zqF1TrP62Rkn2C<=QDKue4l?!)O5Q57jOa<0w4-*Zs2vL8c>T7$;rsF&b_lgKVcwLb z-#w6qce*5hd_c~`L`!J!yRL9SqUXfT5G~WovWmFiDX6Kl=`GpgKGy}c@Qr4!}Rk0r*sKs28eL-n{1Xd$Ptc8rv1TD*@2>jJ@l zMtG!#*o1M6fpXXB)Y0tA{2IIlsDLiIY*yS3BlaD1Jv zr}0!v8u8zWrpYB(HK)xSY(RWH=tJe5gU47!r)AJ1EeLD`%tS47SZEkQBoh8tn}r!} z&m+wjjQR1uXvsQig_GT^$2jecO*hIEs9o4B#B&C~o!lkv#ZJQcHzPA3_;giBAcYOW&Uh@;jqMvdyvA&c-6O zx3E!oXQ!;A^i^@#h60T*MxMLMhOv4ztdB<#(Ho%Nap#K6%BaeG6A>s7E@*tX$5GFR zKpnNM?c%4gVZ2(0$&OXQc#|u=u=eZjx{EDaG1SY@gPUWv$vuzT$Wse*-aGgTO^!b% z+w4nixy=bO9NrC05$b>0uiRvZBkIn8YNxxjJwltDH*ws(2VFbyB`qf-ZoztekmL(R zQ!oS@Hym)B-G#rKgq*1`0~26H+q72hU`B*4SGJsjz6NM65{rniy|8N%vn|B=4DZBz z6XW`(s~3!j@#6Np?li96z1N;h`r`^umPJ)l`$~JB?#U;Aj+eT;#PcKS+T5Ov=icE$ zfji~a^TB!%r*Ba@230!9DSKJpT&;~iO*7`eOQO~sT3A(kr!0k@^CB7LZP7Zf7Z*|f zuE~{DDef1FGuSUNB~OORn5yxyCHc0MK~~a4H2V6qU4GD4X+!apSV#nx10;c+E&LUG zh|^@0V z)+Y7`W0ZU4Y-N&EpOBkdh$i=14w4hDFB>&SRjddeQ+4|98bkxd4jy;9g3L`oy?mSw z8f^DozoH$T>8RcbM5Fq=i3`4qkkTa`(Jb_{;eP8|QLuo0t7NL7?@lM373_dm-pee) z@Q^bSCR8cr8h5dkf*Y|=l9{AW1r6(KsDx;A1WL@eVCq}Qu07NS6i17vROZU_#vV;| z3W>Wt0ZgldfvQy*xbRXy<_gi6(u$V1vVuFURN^rt^YdIvi9SEMuzZkcDwzGeXvuWs zA8`AHv`WJXMye&r{6e$t$Rh_9eXe`P4~4VyZik&@v_89<8v1!ukky!(QOYY>zaSXP zT_6i+8f;EQKqG(Mmd(Mu&njxh1Exsv1iTI=iL7GWQDD4~MYDSI43Fv%A+Q*_Z%=l? zis*5jeA7L`?HUVN6$SZm@_+rBHZq!e8o+=v+4HHcEd+=G5r#Rw`yi|k7MPh!1c7qr zxHdFSh$HY*J%Ul*-CjHHAS0AYytsE@HtwOq7NyBBaHNC?B zwoSxASL-QV2`6arYD0WoI=)M$QrJ@gbwEtr#dc#p@dzA$r{~y2ZzT|=wE|Iyw2m(q zmAl*q4<(E_h`&OaEdm<3B9VcA6|W52sHc!2R=Dw|H_IN#Ktt)eugs#RxGl7-dPdd_ zxFs%2&(NvP9ekbhju6UwDoJ^Ksc~*f`4tFLpZBNz&*%=siBTDBAW@_v2n;nN8=)Is z=j!&Uv+SI@E~i3-weRV{^~oyj-ZwHg_e|R_+t1NH@jQmIna95=y??wDp6@5F62XOT z`zdVGD@REO(QpacPw8nWk{Cft(g52z7)B;yCJdBMcLCQ9U2@D%K16F#otC5udGd^;gOsRQUdk z2fh%LI;H3S%KUOAD{z4`#r3ZB6qq51E1R^db>xYHl0RS%9%nB#mQJBb4I6_WgWeL; zEY0bd4f_;C@H2^bE&C`)!U_29g?q#7XEvYf_G`w2J(o36i-%%kIomf}umeW?(IUK` zlE6wiSE4i4h#MFK#>BdaIo3x*gh`E-=zi#{eMl%m=$@QFSCJ()26aH_eAboh(|}- z>iPPDP7qGk!Og6mwZl08ai;BpSkBQQ0)^5FdM3y^@jW7yMn^3=$ ztD^eaod*VkHe zS4N9N@4Sa=xfl9Bo-6&Iw#Q372sbP8U#^F8H@Z}6HacJ3%2MJEg1mLrYc|%_;nhkm z7HaBsi$!jid@cIEx8qN^R$n~STtJ2~G%8;v>B6gzNWv2&v8sd=zk4+ju9C6UAV^vJ zb4YCy%%Srs-E&Jhj6YI#G&39%t5^0JbKCj<#PvAd+w=%%Xg05P%TslJBs5uZSSWib zdT2Zhorix{;+*lna$(k=PQs_}DfLb*I#_wJ0*`~y*J{Tc+_ODu-o4W1Tg~L?eTs1& zjIT4d$T+sGypT4btSY0m<%eLzq~wj7j8&f$iJ--*5nYRhP2V(FXuNGiw74PeB#eE_ zSyK3E7gFU2e0hx0qHd}?FkNCs-kkL7F=R`SJ0ZWQIEM8bqX%QPbjYxQ+RkV`?2S(V z507e8;dN0-E%l+EedlY+k^;B}ad!7CM3ZoF(Ganku=Yu?I&s90HZ6{QHg#23M$a7)`^I@r|$BKwwNZndg-*&Z% zpNDgj4%&y6^1;95luRZ(Rm}~N;j3`T3gHd@%>I=_KSfTEW7(6%z8&)-i-2e2?Vk5Y zoAf(9r!DE{cC3PnIvreB^S1iXP|6%uBJ9GOZP5^EY;Xwx?w+#)Ev1u`POJKhNt94H zmZOI=$9lp&E^(ryu@D>fW@W-Hsh70zai&z72J=^NXihFaku?f z*_rHcDGDay1zwhILcY<4*LZpF{@X^|a5g27h_oaYwdWZbSA^DKZ4@IK>+w$3v-~D& zVlrts!ure5ce&!?#dr`kvRLNf*OSCiP7tUd07{T?2iZtZO1vcktlS zp|Kj(xML#BE2ix`MyR9RR;<@p&Sc~~#EOt}mY8jQ1oE;-^}-AVIB4QL6#Pc2;8VhR z?taokHjKufv-l9AZxqC}{IhV#XMTz!D=(MR=Hk+dmHw(|yK|fJ65tt{ZF=Bt6p-}M zx)njsG*N=46f}KbU`4sc<{XV5?BiQEYC0-h(6>-cKt`g^BK(};?-Y)5Pj0j=#YsBK zrCi@TWl7|cy*m;QukGl=ZRUv$10IeR00F9{2&4SPj4^ zPJmilP%TPae|F6$^F*`iihK58vbW5gL=yG@ka45)gSNG6S;)zOS;gGnRab7DS>vA93F$=SxQ8SeGgBU2vzX_r zuFWBVoD znUM8R(}7eTo`n~YNyC)aITarBs zZD3~DQYO1^+6NR%=m%$I0PH70b(%RvEI!ddKfQs+pBySSFVgHa`)$&-Yx>I&F{iyguM#YDq8l}c+^oB;?wmt#%29Z z^`TiuDNV#Ql8H9o5h&|JAyI^Moc0$HwwX^m16wisz4Ij~WDwppJ@b-IDW^LZPdx!; zU_{vz(O|ycd-UGUW-51@K7K_A3D66tTpu92jYb zJ5r3|y5i!!DAw^VOQYf5tnND&Ry|PH^}}=Gqq=bnCqJ&;1g-A%8SeFkQmtR`zHWl0 zv(pA3i5lMl(-3HozpDgzNLltAByKkmQdxMDWMiNB?(AIQjw_bR6VNGOifQx*JgLmf z`GD#>exP~m=@B|Uks`Hp$rT%d zx>-3Y6RL~OwEcxl^BQewi-KlLTJHohP__&0t=jRz)AhKJncS*3YaP`7S#r`>#YJi| z2sRdC9@*OjGE|Rmx}0l69YMWvg6By!uNV7*Fwkl# z(l>yX#^4L&!i=>(K5op(nt2w6utj89YEug+Uc#~M-(B&F{Ij~aCCWzN>&Q&7YW@3?O z5fjcm_ZuVr>a_y4!3${VF6w39Oj_$xs1UqcGkJr+xR1=Tm*eAFoD(a`z89sv=f75O zyP&Rw_OI27;IR9kZ@^l1B+k={pYLG4YfK=6_-+Gix5vvW6&IZ?{vYMVHez<$6_9BBa6cHOH9- z$LGR(*^pQ((6}=BIb~=9A{;Ad2iqWUPDwmtcu≪93VoD#r=U^w|{NX(M#v$3KBH zRb%fKkl#O2Z_Bmpu)=#D`NAemz;s8KV9(&4SFQSK5n82;#$P8>&~y^l$=0iinhTB& zU#(;`hl}XLna6K>)`XxZO@WxQu)yD5v~EcX_w8i<7}#>=LpD#2Qi-)-o~Cb~U@ zlCKPrQ%34KO<`=6mP_79G|Gz12t@lSby5QgUgvhyagVXZ^ZgAK@O7@5<=d$bg|xg) zu^CROeu}tVsAYdcn2>RsFwv!DD0?Hfsvy^cy>fxurkpMgdEDgdz2u=X7kqJbjy8Zk zkDR>RjShvnZ=|XcfP;{Ks)N9~JC;T}ko#)sq-;5jtb2Ei?lYQf@&nCx&ur_AciVu& zJs|zqH>|pNpb6e>#!z0j!ND|r$Y6tAArW+ytF6od%UH%n@x^{nL-)FZi;z`!f+nms zN{F#?sSm7P*6dJt8?7PZ37v-e*w4cVD-Ir;w7 zZ-_dT=IVB_fY=WJ#c{eaHo+*oo|iUB5PRgbSRjUJ#%8Qdo2kE7v2JQS#JLN8MWd?-zvjUrLP=os>-0=m&4B&&9xiEyzFmCi+i2S_GYb}BU zTV!hxop^P@4~wPXD4Bd|O&mIQRIY+5j7j}#tS7FSDfpA%Glpw}S_)dxv!HM%q=nOZ zj_L6gwL}F=1}pF!DiALMQkU4l1-m3jK`gNx>b-|(e(4Im(@ul|t+p@nK=&}u8@H$v zrhMf1y$cUpY1nNqGuCm*k@3suNXu3er?=F^yaOvcWo!X#^*aJB#wd>kbVhBQ8qpZS z9y-w+=3V=a_y)|8Dc|}zC@)Z?8`K8F{5p<)k8VCxt+4eq2g;!f=sHSDK&&JjtKhd6 zCOz89Yy3Oj>a4Z|&m8yflzMCKw5OaN8FsSxUL)bn1v*LX5q~X(0aSZl`V0;Ji7;6H3fnm}rWQHSQ!{i5)7$^Bb+TsUt zNX0VIjjlv0NjkQN`Itm3^t0=5fQd8)r`lb8*8*~%CXw^kB zz4p!wbI@^H`5o7F%M%RKbmx?lQ~jZr+NK#o(0zWIX(O{S1mU9Z6WR~4MC{HHiI`Gh zU$q(L!Bu;WHMZ>07@!)-3e+uAYS^uo)W)EN=_cS-&0Op9Ia(2O4a~|4C5I@_m15VU z?&I?Snv2L^s3vbI`yVc0rcv5mCh)?h^t?wu_NaQM&&n?F68yrCu|7_nu!>7&PSnWAPT;_5e-jmI1m(FZyKDqSUjNI@(L&+&N1HMXTZW$w(1K%wlZvSXjW z#Ay#IcyQ2XO_%d5yNsdLukR{SS+P*?#@ zt(5@dRP=9N7d$L91B9pS0!Z(=uaZV2k7kVMZCeZvp4>~AqpfjdJq|QgR^4i&C28~TxiDo(jHpN~=!1B$RBw`lGcY=eivGV}Q|==T=Fn%%19NG2$(WF9+ELPRaH zzW|OkWp^5@ZOb`5W@T37pfhjY{5sMAaeyn>#00g=qD zM;0YmrQ0~71Z4Gr*}#Z8EJhZZV@30T_+2z43^J7F!eaIvG2;;z<1lyaoRCger@=i4 z-3Sji%UP**q7MjFg7iKW5?J^L90!w~dCU1b7{0O2Ie2bLf%;tggIkM3oT)Twk?kZO z?)Qnlr@Oq9(z)*sZI8HCUdih2P6gAE4Jr05WU~sH07rJ^%BU(?6 zGt~Fx1r#ETP$UST=xD>TYrW6L()0kj66dsV?~?Y4s{RqnhE^L=%*88pR}&X7T>G+xb@U1{(D* zh^_-6oC!*v`b_32V5U*{U;LtcDkZ9*vps|0`961_yz3jYxxgz<)oVgatGZmbjy0v{ zkw_B1NkBA7aA&lG8Q3^wrpu;38~T|~xPWUDF7PL2z|oeAF?_Ss)0k+c;i8sgF1M*q z!x1Ovp6L8IY)-Zu_1)h^Drqse3CSEiHtjssY4atQlRrw*@+becCH!#xLF&ZH5y!XV|av8$@ocTec-Xj_NC{*C{0HHlorb`6^K1 z;S>^rXI1iX7VjJ8iQw_DV9<3kzmb%JR;`qgH-dQMj&zIxi4z2=wrjC}O5)8?e2r;w z2rVe<@vepqlxuCZ{i7|L$Zb)$sD0UB5TE3_2Nod%G_Vzj&A5wJ(ySPWpnA{JuV|$4 zl1hb`*qYH=^GZ@Gt{mwcPUX9EY;on;cJOXE3KTkuhhYPd@b4L9iqi@4`QfCfrb&K6 zcWjY!Ux0}yedD@@!pPP`_PAK7NCftnQcfAgf`%w}kbcalq2_~E*yk}j6%Y>;Hr3Em zLrz}fZWo&^On5HJ@X86}%W9IqbK(Jp5!S|_In2hcNHiCOZPOi8Ry;-!c->veVV7*g zcacpXdJsVLNW+&jQGuBPAhH^kKmA5Hv_%L@Umq4&*;){<~BZQ!GoB` zgcKUh3Y0R4`m{7Kg_uJW5{uYMNiKS`5asOkhfIehM~V?ji>;oB)cm*!JS7F9m<8-bn|x!9iC4^;yuB?^b0(~y=H8r6-`q~VLh zKO8+N=jX_4#*3*4ie_iI9*o`OXNo=r;TP%7!Bsr>+A@`8g&jYnDrjYM906Llm3B2k zBG`ts%LYpZ@fAmbf>b$iM)(p+ORjq_Q2=qn;b1UQ@7^!GIZ z7~w@h2(xI$z^TA=^pzEKK5U!n)A;W->(a%zCVcgGx#4cvV{YBGr4Uru`L$TmuBCQg ziDAd*)Ubm--egQyiF0q-y^Y%P_EBW5y{X?rzmlVgqRH%=NTb&Q_G`J-CmXp7j+N_W zS=R0O|G)OW11QR*YZs6pNs*ixa?UX1oF(TdFu;(RfguOUL6VZQWRWBYh$tBZ1Vlg( zBqzxkBqMMK-Cf;XcklmI-CMVA)yGg%GyR_P^y$;#?e2H_JY)W~Ch8v|o;-3l+mgsT zU$@hk_0-E921#;_YKL@{EN(esxXfJbHC4%sEVPlOTdQNb5j6U)yN*uZnPmG7txjY&Xy+xemn;75Qx}=}B}FUED+0m!rIlcB6o5!< zs|RV|g%$EJvgTvN5fs(vuH3?6%t(Cay@a4RMLC1CrwA{_I8oMpEXZ}DxMcH)vuu>9 zGuo<2FqCed$~tEUMp+7zETdZ92uqSxe2=+D+4tep{FsT3x|*7gZ;k{j!CVsL=vZoF zA8aBUI9!EZZs&xgG#I%QXRgAA)F4WLD>0X~NZO79kFdiU`Ki@@T?|WdF z|F@4!zdyG99;@ZsZ>aw*Rtw+1toHs#zgp)N5d70&hH`xe_0Nig7ya-@rm1bLj$akA z%8l4R49XlAdvD@cht(g4^n_ReBWayi(*s&`kEcrOmjr{d@1dhi>bh@;4~QMVG2hvh zZ}Ro@@;*8>d9r?SetX5nerIep zo5jwCzgQ$jZDj^i|H!2q`QgF4H;JdfhYMO;`pfSIzpCK5ldh?{PDGgAH4+{A*L)u>(a`I#9{shyJQ#>2YRucM=sev)r36!fBK*=nw0@`tH6nEnQ&iIkR`J zn8q$^(!-puqgS%+NcAacKbhn8#Rd9B?w2oluYw4}&hgywI*%sKU9~CH0uS*5=-Yqk4A`d$a zlccjNt?@5rMO2?1ncHDt_*~3ae_VU31}rPQdi#AjaNt1d!{-i1&G#Qg2eJzQs77Vu zgXFZJiHo;g4J0kBK3T%!Uxb>=3$md8)OiQWjJWkFc#0cYr0z95YgCzUKV+MdeQiG8 z8^Uiw3+_W~Gmp#L|6I-$u^`F}6ZXnlj1-nMlkHRi5WCJ~x=& zxS9wwx?+u?$jkTETGkr4^(kOFqSEF))^>Qo^4SHqG) z+x5}apof@D{j@y#rK9^seo0ATa+sgDK8)pY&MD~VT@=buy-xT_QOj>(9^JRo9`Zh6 zqtISo&ER~Nrqp~h7{~CD)Y8O90lVD~vUBYQ9S3;`-qTL{fz4tc~1#UH^m*v)eP@(`?nq@P1pW27<=xJ)v zf^8&${j*)PMzjdAJUJ;U11?YcYL&`rl0_n;&LZiMbOyRj&5l*e)S9<%@;k+XvWp96 z^K-Yt@2khkp6}CtdiB6}rnSzQ@U`m$6IR4!XOWymTD;JSl7ZMWO|8OP3{|+voS0|1 z1w@<;$aBMrPCTS#Dk+l*IlxT58?4V?;9@v8izBCoCC_87M+^grLO?^dGipS+#E3&YXKflU12@bR*h0OrLv|$u?wUV+v8wL)-rR@!lI48j4J-D5CQ2>3MhHF$qvW<8!(sjIqnAw{3h(g--7ww7Hu>2h@oAY@r2iYHasHMN`$4Ww@jB#yx=uPyKnO}h~!VVYgi z^oHzkdwPCpe8n}n?082aD-#+bRdjtrv!iy&#wVc=R*UNREZ~1Qif=4ZDyTlB0`Qxy ze&A86N3BaFTi=$kuQY4mL3M0`Vx9{aFP8IB7}rc@?@(;DkrqT~I_~EF^6?4!M!fZd z(KMOG=%t~Tr&$%js75KEJ6<3~gx=O%$o??Lo94YIOmA9vrY~i^`sgRD@i;gsg5r30 zAV}P}Pln~gy$1;mnNmwdY>Z~SjQJPs5*kEFffSARZqYE^5sG*<9>RhSmeU~#6=l7^ zN1FfaL>rdw(s!QpiG=qpdlM$x^CAqmsTJ_oJ;)FktB znwfTO{;0B+0Lewfo37`UPs=dsNwk&TWV``AtEMA*xkX1o{TZ9afZIg?UBB{~BT!fo zb#sO7IbL9HzkiDPrm_el65)lbgQRV?KZd_}*kuoX4g@g=w;)fVZB zFf=}AVCEd_zjgIN1b1J~l!5W$9@p?>Xu29i7KXKt522}bGJkNVP4f}GiV*hz`k}`m zN}sdTm+qJ&y$(#E+DOMFW3IhlInB&2!k1;iOU6PnvQELpUcL?bCInOdx{a-Unb|X` znw~<@d<@Q)P|j(O750^K1j_A-eu)HvmiEFj2VP`ifz|oG`{D(&4a%gqA?OjPZtDja z%5gyi?mq4ng;OyowCXLgj2>5=B`1Y|Jv?#H8~%zGiNG)Ete47fTX);l$2wM~+yN=X zBbBl~~oPEHq>k~g2vSi%bZ zqCOC6-I!Uc`^ky*d@*8^iF)hM(b?jnvTkq^@phf@%#$DUwk1Zg z(RUp}<)=HC9h=-CH{N(pYIXT`joqf$hOEwF7ZZ77Wgy>r9loy-B9klk+>{O7Ge9fO zYh9ZU6FjrPq4!YqR8#AIdZZuG6wN|Od4yn+jzde1PR1=(m z&-6v4pTiz!p{!&50rEZ_gLh>e=o8?obES+0Q#;R7!=HU5G4pQja9MMLbK%JiTdQLSi`;N{`xvC7&S!;l)F@RT|UnGEA3g z#M3k?0~HIBr-7r?WObU9$4w1yY^vJ>5JOnSDh<$r# zRufup6v*vaJBPLiy=AcB2{LBlaG#dHyRt7-!a+JlLVbT4W@dLZ@X@G*o5+a#6Nv3} zN}5>T(X*_M%EpjHo%!ga51ol5DTQ}UxxTRQ)gE!(K!Ct_&Lw{08PU<6#aoZ$*|eM~ zG0OJkO=c6b95KpfRS*>f0(tuN9-)VLNqhLP=RoEa5*9*nloI(bR{KANE&XG)&;2b{ z>ifGGy-_P{2#@bh-9aLLgv`#<2h6y0fdm8GRI^{fpSX zw_W~O`6jWH@CUpF^}(}AYIsllWBW3Z^%>V?UU0SK)M<)p7ucM1bVewyxREype))LL z)PJfP`LsRAtiWJ+-{w4MK6GWPWqNsdm+st#+n4td9JUA)+q1h9z=s-1rT6S`opZiyxMs-bTi%6F4! zcxkokhOp^pytGnBM@D^_GUKXb--oIqqWJtT3c`Jzdz3p?a4;P8eD^hzIlk;p=0@&) zrJ5A9r68mU!T*xLz#vmbq|KLM&dDjIyz;(>hrOx=(y*CfzMqN8-dQL*oC~)wK4C|F-*ov7w&lg10 z?H>JmTU9SEA5?#IwPMH0yD9FwUND$VClQ6c$d+Wv4*c@CL9YNnY{~nMu!pAQV-xIC zQ+3I`b?NaIzXhY_jlGU`*O)7ZvwGnDiiGh@nS+;{G&7Op!Vn5Ax%f%#fzz z&IuHbql~5%e|v?9hxt{HpG~g5Yu~_Q1D69pk8op^NY!eCHr|#lE_m{DB!%$fOev5A z77ZhUGx^Vsz|IP)^ZH|=H}d4VfYqm4oyQVwnCp))%@q7b32^O7wO%-fE5|9K8Z8Ic z>~SS+eV||D<#G*)G`x7Xc*Ea&x2p9JS=dS`{qm{!0v2K=`^;xKvfTj8h5o?mNe|zb z`5*G?0jY(B+;l>ix^iS^(`*YUI`RzO@%j*h@Tt&HatxO4=nUd*qT1(#NJ?kv#W{d& z{8$p~7Y|+T-stX0G^15cKxjNmt zD7}*>zhV)3{as3_;arg||q+c61^x^Bn23Fl=r&TD!)^2t}z`Ql>x79E8d#yzb-xA3E?Y7^T`O9f(d z+d*8Er-%=zds{UJ`$wd~Fxx`6qPTZxg4oe!@-J{nrCO~;pGQCFx&1)Bu~|$yiy0WqbNy<|j?%M4(DZEXjVh<_nkrkq&a1ku_xYp^^W9J#NKk`l#5zb9w zDVgB4&3u{HT=5S$yYA_SHfD$RiPmz=YoU4M)(Xoq`dx0@_t$FN{aUr1FTQ%JibBtb zLJrvUN)ogGtn<}})29q~fwW`vgvJ;-Tdsd9e<-^Gptf8RP^po|+wtv!ncNd9#e04|=nAM~aG(M;ashKu_Na2S)|IZug&=bYFk* zMhX`GC0FjreHX!2Rzb`o3Bn`S$~memq=+*0I~o#|_eI4hg(JLc)Jm6BW2o;i7w9Rq zUpyF`a1VdUQiW@+L1v4nG1qcSdP(sPptT1%xE-K)8Yb4FLw)y#$ybc{R3!dZf$r`0 zkZ8MyCJT3#*~W99sa@EP@b2M1b4k1v{`jy)hoM?fA{eJ!EtqZ)3e?3Ek)>u{gzTZJ z(<@7~#%ZR4Jh36o-9{IeGCZCZTO3&`9$TAjb?$(9x+}^Hnx1;!kKCg2uX5i#Bir$k z8Gh8ZZsXjbDNdxWi>>DxgCHN{Y^y1p&H;|Lh<$+v!Z;~@8c)vM_97;qUl8|>S3f=@ z>-5di15Deo<8rj#oqLwC?HG+Aq#ZCO&0CvX*o`d1!yG0Ur{ZSTeso?y`S5D(T2rR z7lB~bhJGg9ZmyGA}U{p=lcxt{NalkZ_8`9aIue zl_GI=-hb7h3JUu)h33{U9^~m0 zM)5oC=2eynsCDDJj>+_>55^D)DM5Jh9?`yaMMjZbmH|CSfs81h(u6xM-w0LtSVfDl z%$tWDi3g@BxVp$1zF8q^^puIn2cw9`t9lL8;G+{uy}~w;q1~>kp?(<0DIu@yUy1Q9 z5APkpXnH^L9lEHy7i=gPBMh4jWVOmKJpH=EmyHF)=Xy0PAx-K$OOf_0p@pg_49hc3QXJho0H0H%uH$L!1|p6W?lYw z1RrothC*!l)wO(wsz4e}`h~1<9*#P4qIe%2K`Pz#JIh(g z)tra6>CU9EUgas{c;<5MM)$TfdOAhpN`uM?TIvD?$Bj2Qk3*xwF71MY^hi+@;#(6Y zrPY-=54OYj?jSkhOsg$MmfUG|2uWp6T#^Moq|9%jZIEBJI1&MtvqIx?01i%CBBPob!P%<})g3q%BG9Qo&2 z{{JWv5jU66pAJH1>1jeIfjIuhm4cf}q%1~-bZ@t#kW?3H$0v$xU$_VHBEKx>W5(@q z-Yhylrrn`cWpT*@`{%BH-t#)1o>tgSZGs;cJ}kYOpIeR7^11N2+70x%2uVyTHD=G9 zEL}T%$3w5QB*#f$m!WB>X<_3@mwZ{kf2e;esT-A#c*&x$xN%K2f6vTC>O+HpmK#26 z)R8yaTI?O|ub!vIocu>xmMnJ{_A`Z47hXtXxz_OP*y)xVNSvHx30S}s5?AkMZEEr@ z?>S{@yOA^Ai0aqjHYe%iac`H)!eTBZyBtIhm=OGsIVx(V$KN(Xu%WMasJHPTex`(0 zu`vDq(!F=}qgS=3WEbParY?cv0IE`Q43co*rMdZWNgM}3Bx#i2BV78A-NowpLxJ9g znFu93mXqZRvIV^~A}wHn;-&|@7eSl*EG7#CiWL_g8L(H%H8&AIeyV|*MyiLYI@G%> zm_Ncie?6Y?*lnx#NpFUC~2cjW>J)r9rCFTL+8Je5 z^ZSLM4KQI-2DB2VB484ZvK){ZCpcRMZRw9P;;G`0Om}9=ZFHsZ+@)I5i@CYf{(4>ucDo)tz=4p#{Et7HkU`V#Po#BB#;A7wM9 z8h1%)S0e$}jtvnDM#!U;MByi=Y>Y;keb&P^^*ys`V>s0W8?4Nh@qN#3y$`kudmM9$ zc@~`CuEwzF*|k|ka%s*1qGs{JyhxUCm};5B;TpDC=8B2)TdevRBlm$BV>wNtB}1D@ zM;N+mH$TqzgN!8Y$|CZ z7oKP~pM?P>m0z_C8fdr}02mHL2}izL(7rC%&6M)&GWC+F?05YXv9v4rF!~H5YYTPv zHM^xI6O^-OTg1bj>h4)SQ6jY1)=Kr+mR8(28DvSvTA51>sx-x zlMeME=PT-0<>!-+`yUT8pR>Mk06RANzeDL&^SrleKF%WpMEv-yYyjan5b=r=D|i)e z)q8F2)U>~hL$hHaF8ijPgyfeUPurO%=aPu< z9ZfWRdN+43S<0vv((E0QVlPOwu%y}PFou{5@tGF1dxMG<73npGoWh?y=hxUKPe5kL zlC}@B@2G#1Ldavw>cxYSU?Pp0^B@$f8P(J;U(dCBZlfw3tGU8|ZiC`nkfxp4(||b- zfS*r_Ax#U(o9o86WFjGq)-oq^zDZ5{DmWCjHIFY0qVfw9!rx7vK?->!)iq~%VuwM{ z!*x>H(K?6M8^kYHfq0d0KuPSUswH`|1n+C>rz*p?Pi@BWFxlq>d2dsKixhDxrIAWo zxeFh(QZI!9Sm=}}1PimNF{Jwq?QxQC`e2@5bh}tBrv(>VDH0}F$&VoW5#&eTy%~3x ztwT!ZE;93+L%g)M()_2^PZ^;_L+zD8m7j5vIcR24BWV&)-6JUkgBF>jLl%)&P?hjF zQ10sBCP+mpKg5lb?!l?EG4R`*V`TzXC^Fxy4tazwxXUdYqN9i@xT}jF`yz#4GxU+% zK?YzlUq75ikZhY?F79QiIlGk(m#qjZabpv!frBxAjD=jGRH%KzjLZ-$+F$A-0oOhV z@9y5__^#e0AbexqzLrWLV)8=JUis_cyQPz*T@ODj9up{4SM-S5q#SkKqXw5(LYt2~ z8K(|Cd<8|cN98`@9~ES$avoH zeB%+~A>$$PYV!5;!z~Yc-U#bkPWeFi`+M>XA)U#xm-y&U(RI#VV)UtEYSf>mW`n7)w&(k0U1nfg{ehe-43$r^B#SfXm3 zT;g;cAc0bp_|>iA;eH~6@Vn#_*#p0taZ0_2Ll54mO})(d)63_V?(Uw%72!l1$G11O zF8piHPl}`UGv8dg2wuIKpbG!AasJt#+_XY}?CRjf6x4I2Y?|twe&%feRfO`z4!_az z#`^nZP!kDMykbxf?=1DRREr_8ZS5iS^O09t%G=|douiHEVVJ=7)z^8#`{x$tas^Z~ zPU11=j72pyJCk|uIs=CCbJNEQ-gSC94J|F00XJZK5}Tv%_vS&bcaJyMju~!AR05AB z8tcEVO}~FM(K);`aQ}H(uzzzkRlKCX?Y>M6mC;v?Bbk0xzZ zuP3vooOS}jhlb5Yvs1k?_wb>-GYd{#mW}VdVg&-4E`yRu#(*~_S3X+k%`fXbTuyqp z{PE#3{-dms^Ly|xvt9URl)oA_e+p*t8>0^&p8%Yj&E3N78`Yeu%6G!7->J@iY}krQ zc){7e9Kdb>3$TqHM2u#qp@jxuXC+3X&#%g*>MR4cwNvzgfwg_qbS!-wEQPFSB*Za9 zy@b7-oSoprX#g)LM~JJimlzEQ&fy0ZhJU_J=A;396LE78qmjD405DM10LVaLU;sY{ zklm6CPMRmk$HC1nC?vqo1}D>l$2#TW;pFCJ=i(9O;}V9`{{8kxBYw?NCknH&7S@uL z|1CTCH!&JpH#cWtPEJoxPYzFB4k*lqlUqng=o*HHhaE1#?&=M319`DSTxq|9{Ej0F zcD01rIlI|GA%JUKkOiFUO^k--CptX9@8xoG{v#uZE62Aoa#%v0IK4p5oZK8-od2Mt zs`~G0PENnnw6gp|&DkC1_^nV@mYiUCfaz=2ICzfSztnU7$sd0*6Mg}%#QRT5@M`^UI{0}I9wglcDulfDH$^HfBTMkf|4ixGr zE@cUWT7cXDaGFb1kOLS7fVWX=IH{yQ6lV3kOg~Hi8Aw%CSQcvOeqBpSvf}RUc2>gH z7D7CNTo!`t)>iyrc0M?9AG?qRh@ah3h+BY%A0!}X#V7a;_RqS1!&QR7sT(1d;NNjA z!SJ=1CBHR07mydo&Ijh>Vh0IY0ojGDEcpcmfM7T@@ORvwbpM9y>TY2VwsiZchJWuL z+F+MIr~ZDy(N3I4l=HgFz(4%}yuSP`_0NjG8}avyM6E1^;huE@xru|EogM8gLDzke z6W&-wIe%RGk-@dRo1L2@_%E4WTO#H7mm>Ty_PP%X!{d0`h|zelTY;@X?v8FW;wo|q z>g^Q(bqzT+fVR4>mW&)gMM*|ZOf37vS`+j&(Tt;0QW(S7+n?L@n{>>kM&r1|u zx564QsFk}V7$*MxjsWjx02Mn67zpMK?`jQagA$mfc6jYUx58A|2N!gu3bB*yX&uf%@3asa0v-; z^ZpL{FT#HV{$6i(5S#0JgR8bJ)Dxl(ABF#De&4&qPg99+_5VE;K2d?skvRXDB>mJh zzIU(xEAKyw@P7sVJLrEi`H!6YUw8f2UH_2>{v+Z4wyyuW>p$|qeV;P0E?K*4Kzn15nI=i%ei*3h8AD*(l1Wv<%})sp8VIjT~*z;71(S8*s0FW#GRD zeEGgV|A&FRg4dh#|7f9O+$5xc3r8{lJcDr?C6mFfrnFRR*Q1OcF-+(D6%Uu{R4^Jz z%^isms0rhEnNH7b+JSUlqW+cExg%hVLIdMx`qL22;tQ_43K=J~9sDCLbyJ#J`WxuA zBer~2hVwAgC%T2&b{~`_#B8-!fKhTq8_@+NzXiNTr{IOM=agj~`A=qB)i&cgNVd zpXkH;7!mVw)2f#h<9g$SO)8?xJ8?u7*x<`XC1fl0NP|?Qg}1RdtW`5-^T)4Fr4La9 zm_+Hcrx&!>?{Tc$i6-1k6Qj^{)2cOnM5xMTKNV?8#>!Ki~I(_9=8s3~d zY~-;@@K%fIDn0qMYy*k-HPb6~M;{pG8~QSzfP_xZy;koWE$OO0veH0z&dUQhWkDT=D`!LcDxj z(%b^_T(Z)<+}tv}Kv`KCaZxT_0id)XAFm7-P)3M{PfiFZ1YfYp@XPWG0j0S4_{HyW zDnYEF0B-m`%r*1RZ#$a;f}C1Vs2e=_+Ey)2Z95+@{L62TfDE+2)|~pr+;Fxa?t4GB zIc;Yw;Ncw^l_iqDUNZ@L1g-H_;B z;*_RU>ERWo1-T><7a6a1)zzQss+lkaIdMcdTsSZ=FhofSQAIGYZ-rlYBt?HJyRDpc zx+W5+`D{M7V(6Y>@f0`}w8yc*LNR|M#s-68Ml}07BY|eDF)CozxNr?s-ML$D7IP*@ z?x|+LusSP|E&8J$p+6S?Kn9uE5fnAe?b`jOyWZ}r+__Z!EfznnUErQSwEx(!vuqRb ze!P1VLy9IC9*a@C#r_&Y#Hpm`e{aA?|9{Ny@&Ehq|L8Ux`TrFDf3y!n;=eC%|34Z` z7yCc=|Bpsk1v$vN(gd&Sjr>ogUf;nb<1X!~kclDtvgrP|X%qOuS(kDt4f5TD!UUGr zHoCO`sbesOBo?kt0;a+`JZ#3Pa3{fBm{{9jukXKlx&ZK_EHQ}tAQ$^|8401D5JLj z|Mu+v(U$)k#{Z-C|Njr;ARo5g{y0Y2-XG1cUU*@)4EMI!f1ky|6n^-Gqd;!k#DEQg8-?V={J46rAbZQ%+L64?QXkHgM*_Wi;dIRQ7qS$i?Y){YoQ}n-ISEqt&ab| zs&LB|UItf4(nbNjs$5$XRmHyDe$Rj6`)v!R8yLlP8FstI0G_A_^Nrf!dwO|HrK6lw zPJzPhhMI>gEb=OpO>k zbTNK;d$DEJ;M_E;u=!-w5Ghaqe7aN&u42W=ktS76`*j^7x%+S7eq49T9d`l?+m~JP z*8OlE(*jkDF~daP-ft1qEvjZi#6P2(nAGbG_-}F=?MWty85w@WoDCyj12_OA3}K?W zlZj9F%s;QWt5w?b(w)hDCL4SR)Bl0jX?T6p#6*=ifm<9@`RKxB>soMra)uqAlA1bP z;q8XHk@hh=f|5m)MaU%DnqT_eR^V-mKl)$)bz0Gki;Ow=1B*}1g~_Z;uvBUHF%tLa zTKURwB+_sBR2(L0+k_JB;~9--7)q4G@TQ-alH&KL#=F&geht3DF;)j0iYY$$1g8r! z6RbZv)E< zN=PXP*X2=R&Cwj>q8lbHqsCXuI30UeWKEIsn`W=09k=$tgrDw0wVDl%_!NXAA6_R8 z5~AMs8Yw$DOo$<%zk5_^Tpv3umHJhIkCmSgRCVSiBw(zh|3)D!H~Jj;S2OZB-9j0D zN>RX8@kNFLL%2K@T7~p4^$_*Vy^a(O;5REMo<;i8*u zJ-TLCC+~R*FZ6|!w{GmE)R9CHTj|)Y=$wac)kp<~X1bboqr7awzwO3{CPzB08)i)Q z&E>zXg4e)i2Ix z!%~{%-35+2Q!4}`+4G{L39g(?7TBQerc4r2@QG0;-Yvax|97lm4rLryqHURS7IOUs*>e$=bwsR{rR0z=68cqUCbtgB| z3KrnzVybigj^zd&*bgbw;?h1wWOj2DH6+h@MF|WkzCfCIl3wZ|g9IEI$iFP#3+ayJ zH%AhVe1Y9fwYiRee2>F#O=7nMj8Y7Df1bNLRr4w^Jw7_j%4t(n{>|@{HT*rPqIMoE z?SfL;)4cekcWRu*AK3x?4Sty7TYa@$cxF%6xQ!H4xz9QYKV|mi{&%>~I)P_Ft24v2Z4mg~}BF4&kjD7PAwTb(FF{`B*O5vRz|PqbF4?*ZJc8e`3iJ3q< z1G;svZ*L=z+TD^`BC4ey3W^Am54OTc?^5`lPMf0!%u}ZMRz}QKZBh=`tblWiQWly0 z_9|_`=={2GdhX^PJGP&h}% z;XgSPa@UFxzAX3MDT)l7&n>7a{37&YVN zvadzM&|9XAjj7G#E~kCX_d9sVG8>0P#i8PdI4yx^?~Nk7ptMS>u8nqn(8icU*>`QF zqz*lm+bSAoF%QwCR>pej!^zIJy3l(#+g~b%4k?mTbZwhAMoKCe;j@ROR#)Mw8}+%; zKz|k;_Tj>nm4nW7wefV|e-J{=WdJ>mSL~n0q$?T2-X>=x7Q(VY9XtR<<+fYL^Wtn{wHiy&EfhFA2R=KCjTbJQIj?&P4BbgrYKR^JMNBW zU;SNJ`JN1}Xl!x?^=Fk7{c3*EiPZ2^H+tEc)>h)M*RqplDf;1KC#s&DMX#FUTae=H zkC8GfWL*{PVBtL2Ko>o#;tLa6{xP5nzDTOEa(LsmYz3U%-m`xBKbrO&WDh1648 zMa`HpjyxJnVPX>olsWh`K!_NArSS#7pgY}C8(lJKrA`!dD%w1KD@LNDrLx#ke)C`@ zN!Z%gIp$y-)-g*0cS|qob6#blHD2eT=u9!bjccx>uIv?JjP7hQGs*sMwDhEE?YnO^ z?GW+!M+UG?r8~A;7$%W9d^7Yf;53n-e2mKOKqpJs6&RbD;q)dNh-2v*!oL9=D3TEjydq{kUP@rQ5J{ zU0)HHr8TzDx#4Bo1D3Rpyz|$jRP}Os1B+22sJMJ~Af*m6Uf`%ke=DLPOfNP3S$Waq zqaZh_deQcgWp@FDD7s?L7ke`Cb z!#Rhk6KP_SCKKsk?}mj{A|D4^GVKi|ypk?EGo-ROkV@<2vcHQV7FC)kKvys#=VxkZ z{yUginhobbi!S&{#^NAU`IhZxRX!N~644(=tUz!8(6(&-*L0bYCf>gK0~x6Ye|d(j zxa8oUqeGbk{XCb0lflWd7e34ecIWS=UDGYfgW75+rom@P@s9`X@`-INQuU}(i{wbUr-O9Gn(x@{epyl-555%gzBUFjo zC$D17p}5d8NFO*f?0C9i;`Z&z5qwVY?X5GBCdn}IVOB|W&#tmKFPqVG+*I4GbK>a8 zdeDhtQ30w@o8P+x2nRL?mrh`t9Y)8hN=8q3^@U$Kuc`ZTMtBi`xdEA?JmA4(yExjc zRImE>iR@i2{-SC9z7@vhrVEUNsj0k2xj0)7vq+Smvf+QU2f7JXYd_J5q}8W}Sj|3x<;*_3CaF%v?g9Pso>&eCxZmr z6u7-^cAYutJ3m9`zlo|O336yED+)9wShZzu7_{_KDhtu#FrLMQMb@t}5Qvpt0!a;Q zClxzNfIRNXth3gt$A-G9Qy1JxOgXb3_RJ3g$#WYP2HTp?C|JQ|iIy8LUaGFa2Q8+3 z+sJs;?Swkb7Ne$42natMZ|*FzeO_>iOU&HPYQ?Xhy;9piI~TUIr3h^XOJgS4P_j&_ zCsG!lBEdb#;J^ZE{aZN84}oEJTRl3QcGY}|12l~wFde4##N3K-1l(_Jx2s-Wb-BoT zgA`fKhRy{?bc{;H1h|08rw;He3fr@>p$QOw-?mL#@$T)*+4BT%Q#tGrAOEu}xcb6d zD@OE=DK{dI%BbCq5%T_+&<8IYVixG(M@Mm}pP>Gfv#$M7{@xnHa1oV>!(=drukJ07 zMun07_AasFRxS2H_9t7-Y7Q68f{6nR?Fu>i`vdCWb8EugLdEvbOM5CPvR1$YFR;{S zgTNi7h<;(ghBP~)K+pwWClv^tGIM5SCOsz-NeP`gDhI}+DUhT_U08>#kwrkTY3%g) z<26t%K5YV$JBiGCVdtNBg=T|)F4aKH)F*Ut#m|ZcAye`;c`7QW*qLc|MEQ}PI!EIK z9OYk37dj&Op3X8c$JF8B$~;jM)PU0E(;}KeQv(m@w=S?_<@D&6qP6)}96$IHxp)~b zsh&%5T8WbQzmY=f52SRH!l_%g9$Aw{&6za(4)b;Nvkvei=zR(gVN2YhEGHTWIU&P$ zq(!6|AVU)dfb7iOJVgJc544rb zdGv@^>P`BMkRfXQcOEs4IIOT?$ofw}nV4^5rhTo(Kk{H@a{O(vsmarnq3Fm+P8*p1 zVPK=?Y(^x<&`CuYlLjZVk9g(>=k+K*z3Do(BC!8T%vfngyShEG(Hwfs#Jy_a)=poKYbw zj!Zu>E-V@hBwVCGwgd^le-ki{3tlpZLr-yy3l&19U#K`CD~~2mUPCYWmr|4qUwAG^ zHl^6VI6;CkH7koGB?-G&&IS;^c-VMZnmGrwF(xvqig~ctc&UhC5y;aLXOfMRShi3n-@Co+3a)TNohSgL|b#jp`00iqC%nrSGm zPBonH6BpO({0o-1;PVcbtwaI-HPlgEL!}s&Tn`CrZ`#vU=E)^04zSka6MQCaFZ;lF zPO}FNXa`X8Xv_iG*t40#tTT9$O)fHE=HVM_zwDeURVw*2xck&0z{AVi z^4_$1e0u(qCc-S+9W!Ql90Ywqrj9169-kS|yx|ktyKQi>)|QstRa9tLfA$O%^n5%w zadbo-uQN9{-#W;!-Fb;x+@4`)mIY<<4X$2xjZbeervY(uriO3!$Co;uf8gSs*yJf2 zM8O!q*A)y$UmtIDB4`s5XUS|MJ(b#78{Sqn&o&ogDNIaE-i)jHh#85H`Rn_2zCN&V z@&+y*mekj0F}SXQ@{=)xO#CezAMS8AJU>N3LPKH|XaD^ZV@B5Rjkg$P4WArz+izT) zan9<>9_+aNcy*IW7$))#3Q6W*e0;q9fF_#W^z{dFh;X47S68Q0YBi(6Ei&^k4>(Qn zi#s{7tv1;daiIQ`44Vc(g@(B8Mm57iy|4MaR^oHHC5DAbUbI=k%OWDe$eI}&C&mc8 z`OTSuVkL@UpaOudt`6hHvJK8IY#ztpQ{7Lfjg7wv-JgE)=`xiXr@L-?$J`!*9Cl?n z>uk?85k$;vT|!dyya{9%TTVf|VId+gU3U$a;ZoytJ~9|8>A^BoKNrUA4Lz8&u=r&L zl!ljE3o_IAW#;F41_vy337%-A`%Xtj@pnrNK$8Rq3&ox+&P=$tQW|pmUIMO?I~W2B z^P7>IFn)e{#)3?GA1o=TX3T(uxIGHXPT7j8PFR90O+B>xY_YZOCrJwJS+55YimYiv8 z?Y&$!_j?Y`SmuzCN0#mU!|ZJ)N1)`ar==hmA7Ai=?7#S ztX$_+$LkEc9$&*QY@kD~D*ETTy{75^8AzpgeFjxoO6?#hIQhW539P|HdUrY_be1`XF*P2fLr zLA-X|qfEMPVJ~f(=^0pYQYFjXzJl*qjQZvQjd-@k9)^i}e4nK40?V=@A_P%UvJ*1$ zw-=t5Y%Btx=I&P=Ts+(-;N`csZ$!LJn@aY<8r*|nN+nz_Z;+=8m3iV8XD`N_g1#SO zK-q$#qVJ{XB~zJspM7E#+R>{`)(1DiK0bE|IjjIaVOMdy4iXEkco;js4=1xP&*_kBb5mZ;3~PHuv$&X&tMSwbl*2nL_*o#BTT~fkOgrcw z82V=rEwxyZ`G^WPjG2pT;Hx*tFfIaj#2@-WAN3{fFUMxwK0D-<5$UC{!AqTHuC)an znoEhYMRWMvj)}e7C?}_uID~`-gEvLy#i7#g4_xv&+#=)orj`Z28FkwR^2o$-+W1E7 zd@q}4WCQ(szx^QK9=b^?|65rpCb7!Q<+jIF-F=VgYS)g2jL&87SAPhqb=Y+VFjyJ^ zfk9^3KI!k!NHf#ZLL%M}N=j&NH~YJpf}2y#-R}g`2+>(w(Rny)9^;fN?H;Ka1p7U` zB4>j3$acgd;YsDwoj+fg2)8R;Hw^Y~Fpi2fmV)%^#;cYtwRk2~%1vM~wE8FX2-58G zH5C(##8ZIia7u1mysnXXdt$7rP%#I24Z7mo7s8TpG*T2OZE{O!4CYdYhjPq`9YWJE zBtgH%q;3wj>haT7f}=;(J&=P3N}#{Y!bS*2@aDCuUD}=>^lSY}GMv@B+^P0c5)SA}mWXF<}Mowf}!^mrgDH*HHE4FxP1M%G8zy2(kWAUc=X0*=DAy7^^$xkgF zm1zYagO}3NlW3q@(p}*y-R1`okg*}6iQpXdHHcsf=fNZC&K+hR8l%h>@U7hvZ~!(# zs*a5-(7BT+^n=X9fL8ScaBx2yD%e1VGnP)LE3A#D%@)ors^SW&B?yuV)Pt8c%OJVv9*Xwv9^%Pj23Qs!a zw!dN~@~w-g0x@=7KN;QKJutKK4JF9)kNS&*wq8t=aM*1GOda+g-mWm#qduSAyIr|= zVz>RI&C=iW7m=WOuH1aXv|6+iI+*Ra1@ZE|-s;tSZvEs~cZAQm;2mkbeYfsDv?cVs zgAGb9`ds7Aep_byLKc5KLqm>Ej(?iW24AWjLD6PxC+M_uyPQF~+O`IY6M6qxS%IRY zq;fwo6Wo1A64?2I0S!Q4E-Yfs;_%%B!lIq}ROh2@NuEGow*2Gugj4s{QW)Br%P8ofVWQ3i>tW62=^>kkWJywn>9St8VN~i( zUNqlwa!PW1(hJ0699JtdT0Xl7=q)V|Ceqm6j;Q3Xw=Z<)wYnlFwVG;t`kg!8ZpO8{ zmZom@rC9(!`Lc7rhlzAOS0kn{xz# znj(#A$2UfC{wA{St8?sdzi`*(ekEUQ-stTg?-q%o8rV5<^hhW-FFh8e8P%o)At8N; zGsMEWPL~n$UTWl60U|ktw#!wvo=t z&0K~S?3@cTvGw~%Q|J-(<9sV6N#53;_(8US8yyH^i_U1a;WF*48Zl%%8L-XRq`%kd~UgL)cV@&1& z*1PX82y}^jzu;u-fpm%R9#DIS3{{+9sT1!Rfbff^Qc z{p|I%@5RfNz|}r`(SO0;1es!HK-O|A_OWq*0uY6l$OEQ(s<5##@VHd+2R_%I#og_V z_U2l{d&?2yBki@x_eUElLiyOqgV^M(H6_VJ#lek}hjxZK99}3EtcV1u2~-45$H&gc z{8OG@KO$L}_%L!|*t%l?ni3nmB|;r=&?Eb)ZENpXsj0l8E_vKxo0F5>Vc4emW@(YI zq^lOKbZ35%69qPO1{h^csg>$0tqJ%l@EOQ!VPgf6WU~Hp?{zgZEF%+12Kj~GxAscs zAOpXBZM`u3-ro8}t@qKBGIG(keG%OqmOD9#@#*@puddc+b2eJv0t>_+?BAXw+uC^C z-)ij?HrM=3iqvZ#T$!k=#RC8iIMd`djr=TUsd}^1PgW(GI?E- zLD4Rnnwr$;dUV0aJaY+TUTv;SQTK>^FH4I@x*Z78#TKWQd#8GwEmvl~L(6O>BO9$z z57CS+=P!vfaRxnx`ueHr^uNgQP2G%xuCHlG8}GGSfN}O*qI!1C1XPJGj{nrQ>d;Jt zOEpbrgez^N;M3;cpf(-^pJmv9;tDG2`acqspDFF!Cv}&XE!>BaCQa~vsYDu;B8()4 zD5ht*5TKjj?T6ZtrvZI`X4_}5aVTB=Jsv|By!9}yqqvQC*%;ldaihT zS7-ns2=WA0EHUSl=TzOQs?re1L2Pk2jFC=U4q*{aB}*%B_CNhj`*NAuRz9MbGnMSv zSA-)B`Uv${cy+iZh0mY)SUW>Gp>hludBqmSuG*zPIRUZ-phXFxY8qZs-6Vo8rTpH5 z;27Fb$x`5Wa$Dxj@lFzduwRV0?(V&R3C=jewf?H>*xPGFKb?dY4b?azAa4%coNT9Z zxC9z(iT8uC8i`M&FCBu!&G4ECuE`HvlKa%DS&UftH!gADMTY znm$#FPbaCQQlESaF2b-S_9T2fF3OadLIQQPhuRq7T9kedPZ6 zvXoX>70O82IeAw#sxN%H-Fld9u-U~A005?EWkU9C5S_4J5dAI^=e-`H8x0QGEa+JxAS zsQV49c0%4Z=-u5=Nzr7$b{h>@+1TS!8U=3C5~|dU2#56-=Z0p60PDVJy<K|@1> zy}SOOu(+Rn+X9RaIc6TF(NTDuidt&oJZaQd(EdYvdk6_iN|0~gHhd0sCXfWE8{k34 z=Stn;!&HAL;^~ji3n#gqZq6Gy#3rnHXXDUXFsbWg6zWfy)Ye+0p=>7dv8BgK8Z|&== ztA3RJQ=&Y$BIIU8n8wJYwDtrhoH`xZxZ}F6ZoDFCf9bBoybq`?#V!Z+x7SAJG1u7+ znm9?CH6@+Irbx07?dcvjnWLhp+y&MOb6wv`i#Al*g-JkK!>tjFaGMS?ud^*D7lpA< z4nCJ{McnmVvE6<~iX|hRk+rJhL1%}Qj$hx7lsh0eSnZJo{gNt87Wr;rV%&>f_C)b7 zX+Qe&oRV}rF21|qr?T>~`R6{S?Z>tv4dXaDxVVR;Ki38LizTdnIT4QkKlLB43fAR;-u)bN&r~XDTm~t?Zg-5D< zQziMunUW?VPGv0WR{4p8+vuX%zaHd;qRLUpw1x%fk1T@5j?NjbhDH&Fwth5-LZKv^ zqrmKKBnYo*Mc7sOsG1LdgbC zEsq=L^Fe+10$!=(5D;I1K=irO?fw`Shtp!&HJ8=OHT(Wa^Yq?VBA=0^BkUZnx{R;T zpg585#7R>5lEddaU_mS^8j?aIGJUJZ|Gnep#7euv?b{pxCb$pM%JRyC&t+Cbqb`~h z9d7#;<9GowwQXA+3{HCGQLGOISlM6|s^dysmB%Or!}~hbba38vDZFH{>vRn%9*s|P zMpZ;k7!$LRJ%}BmQ>d$kWpaMs9>pJ(z>aem%ER~9{U91qQgjxZOnkJ57 zxF4@CkP!hvhQ12$Sf^tiZre;5@&(24R+GtVY%>YTF7cgq~K*O zO@QI3diJzQPq1YH>ghiUXanVetpHSgwdowSH^skhCX+V&*WL4 zbi_)|xz&dFgLDo{Z^S1A!HCn8o0`w&u&gxVv-FYsGQ#FLwuVX@M;|slk&%_F^?WP=GU>`}##6WyTX3k=xuOdP z8q;bZD@&P-NDfvuZ;VZHuJ>$BBNr5)W%4)&1VSTS4{6!Wu6L%sgqXHrKF&59!_Ms% z-09(dA&EZ_&=3EeD$XkCO+`Z!{oFGppp*1BU=1=W8DFL*W>UiN^0fM^u`t%w@8al;iriOt?2x}%JQMY z13X0&@!4X>2D#at{$MmNe&xh?oT%2+KdyGTZkyDphD0$m*LRtqk?0L@{Dy{>+*cGk zI4CTmGl*-fn>e5>E&3*FjxeQtaQ@CTgL`q1y)pb1KEWB&B_=V2akG{-Hh*CCy}#}@ zVdVx9Z9KgPDAb%OLIVxZISR9HSlPK{6)E9p%C7J5r-)u)lPBsb4^i4qlXESg!Uh0} zQVqhV6qRoxiqlvcV(m>q*H6^V59d8*D+}tRSY&i;a@U2GX^lrZndt(cx{K4lEd7Ir z>dGy#%95EifDbhp>ybqSRWIFOt~^Sh>Fll;os855?>lXZ8K~aqc=5PYOHS1kXC#}# zrs&L@1VL4F(aFso$>{ngN5lHB0X?-Q`N%i7pj*@Q7;4o1v{2?7Egw9pX#tYY+#`DU5Ko@3Jm6e-7ijc{ z3NZcSp~tpcn%Ad$H$psmOi!BGZwFPjN>#QdtCEaOep_|OL>d>)Pz>zvIgJ6+Wv=s! zj(6B_f0cb8T8gnfTB~s&Bg}~r?fOtC+7u+kxv(q1jh?$w6^^dPH=+~qy z`9zzmXCtPNjk^ERkD%Mb7Yo~C;0axzSKT3dw&#Z!;qo&wKR-XrH#DUL@&rlJ`^sEN zs3`(7L&I52iS(hn0WLw$8!7+?`}GxBjKb!8NMbmiU02ez;OApxlzgnzp!;j(BA538 zLia20W?s1+KLI~%;_Mgx+AR_Ej;{@aUHz`d^O-(0KM&YNLu+VktZ_OF6g}0%-_Fo? zG{m=%l!<|6&U7Ao#6*|d9VbPr>*mZ-AP?WXJRGUrML_&9JJ*aa+pMGe{(e}!ZT@P> zuultsb3qC?q6C0oKAW2Pk zP!Y3SwTe`86NXl4VigEyZbe3tKSNzmQ1Ard)8?7-4TpH-st3pC`VAiF(lE2K5zVPz zW4rFL@oA3*jkJ1Rw6daES{^k!voS>FDPUr39QpFXqlw9o&e>PJ_XI0mL7z&YTuG;~ zk7z-sF-`5L8WNc#oJJ7}VF10CI2KnI?{djUZb7%L4nMoW6?_{--Y&jrY;#VTyinC! z;MrPW23?M&BM%z;jb&Y<)aO55TYrY<`-O^VCDR($Rc7n{jB7;8UgVXV(h#nXgZ8@z z>)IMcArR?pv;*^SGE-Nk@M>*u=Hsz2gewO*qfD{x-8f4kLdB)Z6P;3GlH%GUw%xOiD|4blwX90Y#a+GKx3cW9}*x^{cyrR@_DP84Eo)BZ2vF6QJq zHz)n{ezbOVmG-!=MM0O&dDZ^$nI`Iz3_6MzmTFXNY6N<{fN^qeB1+YC-J9lj{mSps zOb=ji(S|^jmI~Q%_$9+&Yw`bgxYhP)xGICk*nbN`i0tg)S0o;9WKTh(+8YIqAyotES zax%a6;F|z}t|M{;ql#B?tvE7_94+I~sN`9hnMWWVH^yI_Pw^kygy-1X(SpI}n6dHk z_rA8CysoZnJkIrihFN@mm&BPFmX*~Nb4v@Oe`Wu^yj`OIf=d2EevMYD*B{V!SLKTv zqQvHxXG3={nYI+T)gLG9gg$Omfq^1?kGF?uS!opNN`VUc*ic4BX!rsinO`c|WW63@ zke?P!PMsJ$0D~t10A$6nf~)%E4k36)rRL6; zOgzP@I{DYnmfK?veLnEzz1|%(wKUy#C!3#-cIEL;<=cg|r$G&K&Xp|}igjMkieJ)l zIo?qVdY{mAJv542*xDK_R;jsv1nW6oyt&)9UH_VPR_2kdDGu2mPq9Cql+G0Jj5Y`K zjbV&aH()vOh{MQ=82;AU>^s4rbHBe|bbDDgbK5zMnVVVfA2rIkxnmPNyh~$ia0PBX zJ_||WW@wq4|26UQ3SUSToBu^-UYxSf#L>|CnwFMkupf?RVQHCe=I2E*sghK;S|g$H zuADk{VX^NEU#A-VAjITD1|uY$8v!LxheH><2#XBiq7%t_1YbRX3I+Il2ckBLVR(6sj79THQE# z>}q368t}~1(slgfBv97+q^mqO3Tp~<(>qOfHA8547w@d&~t;zztlLOWd6 z_^p6%DSbOq4%=-yvv~x4h=fBX)*-0IY|my0H83-EW9p<0@A^ph1Uzc~JMQ>_C>Aap zA_=(x8=TNILLNLp^@TVVuCO`eor3?e6-DUxgF>+p7_18B>Wlt(R1xgRl91>2-OuPv z?HfUbUD)~x3kGStMf4#z#}i~Cgsur=k3@)FqS_)Gsu0_0ja4N~BXpQ{mda^vzOD%W zrrU;p^psn$wT?3{b(5ylfquXHTbFk`T&~UdHXdJ}?s__0FNt3LRrXN|7VQ$l5D1Z=0S`0akZRDDYSWDX>-E+B{7cu%yFcWE zgxMqIML$e@cZO5TFaz_jkE=y{8;%POlflB8}p0@gtxII7k1gdSeqoSi~ zJ@#pBJKtS`WYAaa^Jqp)Ei7yd{*9@8g`iTNkCo)F$I7c&csWBCPxC4(lhof31mCU` zWaMRQO;$O+k_yZ%Eo&;t4__O}zfLJxczEP^5(bE1>b+lo3=IuE12?}h>frY<1B=ay z!$k9Ch#F7SrtX7AyiRprs}3*34i7 z+NO1HDiJb~FQ3g%It2;_5`1C;#LUj$U&*;S(PZPga?z|EEkP~hX+@WFwlTcIUT$t} z2?jX9!!i;RkySy%~RLJra{&P?(8eBJ!~cLSoO*+Sc2T!3OF2?YaB#><*==QLs;_Bjj!Y`BBP1-&@sqoeJ?pMF+InzWs#c>&Y<3NkwEf zMkN+W3B|?low}>!nKmx7x`9N2Ev37)v{&NBF8GrzQiwT@a6c(R{@U;pDODz>AkWG4 z0A}tO_QY#4H9gc{U63fI?J7x&5Xg_yk?H79E^BmqZqht%Fyz?^aFZ1j^GGa6> zlR_UA4ZyfRRo`gAe6PnB%TwOYhz=sF(C7?jFITH~nZ#YF)aYONXSM9M?(yFq-Mr?4 z=M!Vm{OPbkcm=J|YDppO;dzCP8<9d^Wb!+Kpgk6@AE4gqg%iW~#>}~9&#%tip^Mb% z?G0F1G;-q;B{O*O@K~;W*R|dB;w2P;9?!3D&p0UlCEN=ZXP#K(E$#MZ6=T2tg4od1 zP;0r*DjbJQU{l%9*s$k{Dhz|kbg}pG>Qdu=iwPiaj6$S8KNnkcPAr-R{1NS2t2;I& zbiYHvwImyqLzH9UC?ap$E~>7Mc`-*X?IF6V1xgYj30xubHrWjyvYY>{Hu@Ky)X>nh z^$4Y2>&0zjQ|V$*t7s1X_5tSI*-5Xn5;&zizrZ=WxGLRq`cq1h9M@!SlJj;%1ZjF> zqc_}S++26wug)>p_R4*+a+(;s!eF^>pI&P=aM5T<7nV+1M}&+nnW=;lyC)vgCnu(> zOTcBLJJ9CQdN{U~RcZC|{_F5^>c(We<$XaGz_CE$#Ler8?7w(u_FQLDq27d^vzSY% z-hYEZ_myI^=jei$8y)R(vC@_VP2}+va1C-~y#VTvp@z+_%=Q&$*cCRKpnyl@ZpwS> z5Ll^J9)#dkl~8VpOKDK51(n%rA81Wa$sw|E05h9F6i*#Ki49$TLW;TWX`tfgZh<#F zE>vDoEhAoTx@JEoVfxftvZO(8olRp&V+LQJExp(Yik3VJI;4BAm`}&EPkrVEJ2nOb ztb&j7(@!GMRMwt2)hB{KD5v6+jdGTZ&;5monOzXBydg|#n*fJSEUP*Umn}ds+>H?= z>H(wV2DfaJ+#oTQdd`CqfJ>%$gC86$#*>l@T9oO535=I98 zN<@_@5|=hRmjv=zO!^~JbLDC?ZXQm6u}GTgI4_@i|2Otlrdv2Tz@RqOB^cBr;>nsqy}Y@ zC1;7z!qYGW`T61e1Pjx~r>Q?m57R>;t^G2Uxsb&dy(O;^lYLn|)yM&bXw|4vTWoLG z>5JUJpIqN-X#kLjU1CS+k~}HK^z47cA6FQ;P0SjLOhP>L$i9&c08In_XiO1ReF#JZ z5PaE`DLujuHJCgjnsz45%93x=`>CTc#BnZy!ruk@W5gfx{6t+ab1-uk7XReokUPPC z4XYcVI?IrWcz0U&&`uc3C8tl+B|7Ym`ddG!N@><{VRgVRB$icN)l>*=0_i)0*MK<7 zb)TW7Vfa^OK}M*W51NvxpcZZmFx}Y$93GvVre-#F95dPmMQXXqw%-nZL}qqW{kwt3 zXkk1#nv10;Gx`AYd#o&aM&1bzHf1Xi&O#%V5J~t^h{I%Dn+6_Gs-3RQ5w(52j2Q_Q z1lvk(Tu;3&luEe_OgCl?GtmgL1jp_orp@>zu_{K-=fc#wq>Ug=`;}CS496Sw(@DjW zAZx3*D)52?BgTj(LWuZCU6H#ztRe|QM`WM853Q#9P;t<-8;r1B#KRz40VW2Te89-b zyv<(_8>o2Jnadh0hmp~!db9SXX_5dcU8RV+M1%ljxzUHz(ZBT)w;lTf>v|wWEkbQ# zh^Bv<5@*oPYOCuZI>OfAi-~gzElLg?)X>A{EQTH<32_)bo)i?S)LH}_I6cU>jX5a| z61wZzAi#!>N@DU<$tF4fW#qT%`Qb}d?;tZNvZNHMfa|>Ri$0CQsOakl06;$lskn(j z7`)#B9*`nz*Em0SYL~r8;%}{0_-bcnHabIMS|gxI|**iqv^1#W}5v1Wb5fPF>v;3OT2eCQtyx&9!c!;laoM4q?F>I+|yYHBOeS5 zf$zWn1?a^-a7?veB`TGT#T%MywccDkAz4ndc32QL-Vvc07)<|`)avSfQHcrqh93bR zDNYti#1k~N(3z=$WuWzVkE>qI5}=%*ssX;=u~PX>!Kmy|J5z0$J^VMjgNB26oLE+& zvb26{Pd3UK0bxrj)Ry4|M&ST9QlQ&Qur=WC%A2zvBbG~ZTa6KZthl+0Dpq!@;HJ^v5?ukZWq{&L^v{C?ND&UKypT-PbL zxNq%D3r)F^E?F~2=!xle5|3tIh++1;01e-FXR^As{fRn^I#S>DaQl)fa9Q)crNklo z0%iZifXgeqK=>@Jg& z%;7X3jbP`{16;&`!{43b`&}n9oU?`YI9+SjMUDhoNqid29WCw`{0M0}m{}ElwH+at z1v9pP{qr%=g+?p^Bcb9r&pX9YB}H`tw=%B=;r4WCh2NXv8I*#XE2jaNZr1c&)F*pT z4Sj+ygOy146oEhGIX4>`e`y%fRdc+rX?QTzc;EKYBAFi3N$9DWQlzBXi@IYQZRR^w z9U&idA;*Rmpr6bQ$kH1U|p(;!4p>!s;g+$+*i|Y6*oWm_ZZqwKc zszmw4uvvJ2P<~u{vfas$4X-LiZK$tP^S8gsX&Oh`sEpJhuiBH3Nkoy_QZBNFfW$C_ zYI)4ec8JXGUace{TU^H{hQEI^WlE3d2K8uM*<4g@bS26e$8#%GTcy`&u{ZZb(iufn zK!P=23d-bJ*-KUE;qlVP&}jz^j%@4H1*yO~l$sy5t#0pBKYla6^9|s8j|8lGJnf<9|%v8T?2!w%t z4RB1A*z}Gn>%k+PbT%D-(rN!n#!TN8&(O*n7~&a1Di%>}!yoJ z`=|kL;q$cJEkp?D)`0!k#qjC%KgL6bla;mJTDHO`0VU*`d;_0Nu$n-;eLY>mfRTYH zkja8wstrwzs)LP(asb`Oo#w+}pi#Hd6EEwhU)E(Ym1wGbSfJRN(gUnFoE2~~&7|8K zPACQ1`fAQGjUTW^ujA2Iu4!mCc}1@+b}VWuNcj~9&^Y7m>ohxeb%flz?48P0JnGktT4x^5kYZEX$OBp7`sKIq>2is@n ziB?jom$%z0a3pE_l#eO%xYw$m{G+A#D@*4#F@o`;I}sPH{>lxqluSB{!{?)`w1b;) z7FeJ8F`|M&pqtp?h8$zdK=-Du+{)*uxScT%hfDp>cb%1Opv><}F!k`N|DOBbgN4 z9wg*6^W|a%XgFyYqZM)^X{d9Ip$L*+ugSN!UO0F?7;Tr;bVFrp>i4<5vCF#ZA>IkU zS>)mHKblxS*{0oD;oF@HKMn=&0XtGheeRNWyCI#uHO9^I#KV_2w!cP9$9}9b=ElbQ zc^H;#$7SW@4mOPh8GMX5t-Ct2XTc}9X4_SiQfua)*6b*aGdk8Xg~tj37JG_lKs+O7 zU5W=`{i+z;Rfao{W>-L-$~zdFxVFT_Qw(jRGQ~Zp;Lx2->EnRgek5*BN;j(P;ZdLP z*2J348J#QoP&}SJ_xbJWIpL6#Pc~v_OoLfV;b%vb0qosckAPvi12@#&gwN;Y?iI8db^`)lL{2aEU<&e3)qO83!Nkz!$jd%Or5ef>JQqc zkj9yk;xZU8IMVOju9H9_LzmKau3%8`J4Gy})fv&SS<&hTZrYeD9=i z!sN;VG4dQ($)sT!AJxkz01jt`^0Gorr2j_@z8pvO;5^|_%dz+h7~+IGd)s8p_(H1W ziNX(IDO_D%TsB=6E6K4GeIdi`p5X@{t@j@eXEXW3E&rt*b8_zkpq_6DRy@JKd%x%z zwViS2+U?HVlRlwfn_x+mexWg_yKSwV-{*MLTEb6*oU}#K@&D#rB`ITL#aTgAhd@3o zEwwVAc)(7=yJ$;61(mMf%Y&Q$tN&O^=KqAG)iAvO|1u6fzW<7oz=y)s-Kef*9M{v6 QlVhh4X4WvG$sO!}02-HC^Z)<= literal 0 HcmV?d00001 diff --git a/gradle.properties b/gradle.properties new file mode 100644 index 0000000..104e43c --- /dev/null +++ b/gradle.properties @@ -0,0 +1,10 @@ +# AndroidX package structure to make it clearer which packages are bundled with the +# Android operating system, and which are packaged with your app's APK +# https://developer.android.com/topic/libraries/support-library/androidx-rn +android.useAndroidX=true + +# Automatically convert third-party libraries to use AndroidX +android.enableJetifier=true + +# Allow Gradle to use up to 1 GB of RAM +org.gradle.jvmargs=-Xmx1024M diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 0000000000000000000000000000000000000000..f3d88b1c2faf2fc91d853cd5d4242b5547257070 GIT binary patch literal 58695 zcma&OV~}Oh(k5J8>Mq;vvTfV8ZQE5{wr$(iDciPf+tV}m-if*I+;_h3N1nY;M6TF7 zBc7A_WUgl&IY|&uNFbnJzkq;%`2QLZ5b*!{1OkHidzBVe;-?mu5upVElKVGD>pC88 zzP}E3wRHBgaO?2nzdZ5pL;m-xf&RU>buj(E-s=DK zf%>P9se`_emGS@673tqyT^;o8?2H}$uO&&u^TlmHfPgSSfPiTK^AZ7DTPH`Szw4#- z&21E&^c|dx9f;^@46XDX9itS+ZRYuqx#wG*>5Bs&gxwSQbj8grds#xkl;ikls1%(2 zR-`Tn(#9}E_aQ!zu~_iyc0gXp2I`O?erY?=JK{M`Ew(*RP3vy^0=b2E0^PSZgm(P6 z+U<&w#)I=>0z=IC4 zh4Q;eq94OGttUh7AGWu7m){;^Qk*5F6eTn+Ky$x>9Ntl~n0KDzFmB0lBI6?o!({iX zQt=|-9TPjAmCP!eA{r|^71cIvI(1#UCSzPw(L2>8OG0O_RQeJ{{MG)tLQ*aSX{AMS zP-;|nj+9{J&c9UV5Ww|#OE*Ah6?9WaR?B04N|#`m0G-IqwdN~Z{8)!$@UsK>l9H81 z?z`Z@`dWZEvuABvItgYLk-FA(u-$4mfW@2(Eh(9fe`5?WUda#wQa54 z3dXE&-*@lsrR~U#4NqkGM7Yu4#pfGqAmxmGr&Ep?&MwQ9?Z*twtODbi;vK|nQ~d_N z;T5Gtj_HZKu&oTfqQ~i`K!L||U1U=EfW@FzKSx!_`brOs#}9d(!Cu>cN51(FstP_2dJh>IHldL~vIwjZChS-*KcKk5Gz zyoiecAu;ImgF&DPrY6!68)9CM-S8*T5$damK&KdK4S6yg#i9%YBH>Yuw0f280eAv3 za@9e0+I>F}6&QZE5*T8$5__$L>39+GL+Q(}j71dS!_w%B5BdDS56%xX1~(pKYRjT; zbVy6V@Go&vbd_OzK^&!o{)$xIfnHbMJZMOo``vQfBpg7dzc^+&gfh7_=oxk5n(SO3 zr$pV6O0%ZXyK~yn++5#x`M^HzFb3N>Vb-4J%(TAy#3qjo2RzzD*|8Y} z7fEdoY5x9b3idE~-!45v?HQ$IQWc(c>@OZ>p*o&Om#YU904cMNGuEfV=7=&sEBWEO z0*!=GVSv0>d^i9z7Sg{z#So+GM2TEu7$KXJ6>)Bor8P5J(xrxgx+fTLn1?Jlotz*U z(ekS*a2*ml5ft&R;h3Gc2ndTElB!bdMa>UptgIl{pA+&b+z_Y&aS7SWUlwJf-+PRv z$#v|!SP92+41^ppe}~aariwztUtwKA8BBLa5=?j3@~qHfjxkvID8CD`t5*+4s|u4T zLJ9iEfhO4YuAl$)?VsWcln|?(P=CA|!u}ab3c3fL8ej9fW;K|@3-c@y4I;^8?K!i0 zS(5Cm#i85BGZov}qp+<-5!Fh+KZev3(sA2D_4Z~ZLmB5B$_Yw2aY{kA$zuzggbD{T zE>#yd3ilpjM4F^dmfW#p#*;@RgBg{!_3b6cW?^iYcP!mjj!}pkNi{2da-ZCD2TKKz zH^x^+YgBb=dtg@_(Cy33D|#IZ&8t?w8$E8P0fmX#GIzq~w51uYmFs{aY76e0_~z2M z(o%PNTIipeOIq(H5O>OJ*v8KZE>U@kw5(LkumNrY>Rv7BlW7{_R9v@N63rK)*tu|S zKzq|aNs@81YUVZ5vm>+pc42CDPwQa>oxrsXkRdowWP!w?=M(fn3y6frEV*;WwfUV$s31D!S_;_~E@MEZ>|~wmIr05#z2J+& zBme6rnxfCp&kP@sP)NwG>!#WqzG>KN7VC~Gdg493So%%-P%Rk!<|~-U|L3VASMj9K zk(Pfm1oj~>$A>MFFdAC8M&X0i9-cV7Q($(R5C&nR5RH$T&7M=pCDl`MpAHPOha!4r zQnYz$7B1iLK$>_Ai%kZQaj-9)nH$)tESWUSDGs2|7plF4cq1Oj-U|+l4Ga}>k!efC z*ecEudbliG+%wI8J#qI!s@t%0y9R$MBUFB)4d47VmI`FjtzNd_xit&l1T@drx z&4>Aj<2{1gUW8&EihwT1mZeliwrCN{R|4@w4@@Btov?x5ZVzrs&gF0n4jGSE33ddUnBg_nO4Zw)yB$J-{@a8 z);m%fvX2fvXxogriNb}}A8HxA)1P-oK+Da4C3pofK3>U_6%DsXFpPX}3F8O`uIpLn zdKjq(QxJTJ4xh->(=lxWO#^XAa~<7UxQl8~8=izS!TcPmAiBP5Et7y?qEbFd9Q=%IJ;%Kn$lto-~3`}&`x=AVS+Uo7N*hbUxhqVH_w^sn!74z{Ka#*U6s z=8jIrHpUMBC@@9Jn~GS<$lse*EKuX%3Swl5&3~GiK_$vn8Vjqe{mjhBlH}m4I8qK+ ztU50COh7)d-gXpq-|}T;biGa^e=VjxjjFuoGIA8`2jJ}wNBRcsx24?7lJ7W4ksNPv zA7|gcXT@~7KTID#0|EX#OAXvgaBJ8Jg!7X#kc1^Tvl;I(=~(jtn-(5bhB=~J^w5bw z8^Hifeupm;nwsSDkT{?x?E(DgLC~Nh8HKQGv`~2jMYrz9PwS^8qs3@nz4ZBCP5}%i z=w}jr2*$X-f(zDhu%D8(hWCpix>TQpi{e`-{p^y?x4?9%)^wWc?L}UMcfp~lL|;g) zmtkcXGi9#?cFOQQi_!Z8b;4R%4y{$SN~fkFedDJ&3eBfHg|DRSx09!tjoDHgD510Z z_aJLHdS&7;Dl;X|WBVyl_+d+2_MK07^X1JEi_)v$Z*ny-()VrD6VWx|Un{)gO0*FQ zX{8Ss3JMrV15zXyfCTsVO@hs49m&mN(QMdL3&x@uQqOyh2gnGJYocz0G=?BX7qxA{ zXe0bn4ij^;wfZfnRlIYkWS^usYI@goI9PccI>}Ih*B!%zv6P$DoXsS%?G)|HHevkG z>`b#vtP=Lx$Ee(t??%_+jh(nuc0Q&mCU{E3U z1NqNK!XOE#H2Pybjg0_tYz^bzX`^RR{F2ML^+<8Q{a;t(#&af8@c6K2y2m zP|parK=qf`I`#YxwL=NTP>tMiLR(d|<#gEu=L-c!r&(+CpSMB5ChYW1pUmTVdCWw|!Ao?j&-*~50S`=) z9#Knf7GPA19g%Y7wip@`nj$aJcV|SakXZ*Q2k$_SZlNMx!eY8exF;navr&R)?NO9k z#V&~KLZ0c9m|Mf4Gic}+<=w9YPlY@|Pw*z?70dwOtb<9-(0GOg>{sZaMkZc9DVk0r zKt%g5B1-8xj$Z)>tWK-Gl4{%XF55_Ra3}pSY<@Y&9mw`1jW8|&Zm{BmHt^g=FlE{` z9Lu7fI2v3_0u~apyA;wa|S4NaaG>eHEw&3lNFVd_R9E=Y? zgpVQxc9{drFt2pP#ZiN~(PL%9daP4pWd*5ABZYK{a@e&Vb`TYiLt$1S>KceK36Ehz z;;MI%V;I`#VoSVAgK3I%-c>ViA>nt=5EZ zjr$Jv~$_vg<$q<@CpZ1gdqP_3v^)uaqZ`?RS_>f(pWx3(H;gWpjR?W8L++YPW;)Vw3)~tozdySrB3A2;O<%1F8?Il4G|rO0mEZYHDz!?ke!$^bEiWRC1B%j~ws0+hHS;B8l5Wh)e+Ms7f4M4CbL%Q_*i~cP}5-B(UkE&f7*pW6OtYk5okQCEoN4v|7;(+~~nyViqo5 z(bMGQi$)KN6EmfVHv4pf2zZMJbcAKyYy>jY@>LB5eId|2Vsp{>NMlsee-tmh({;@b z@g;wiv8@a1qrDf-@7$(MR^M^*dKYBewhIDFX%;*8s zR#u?E;DJO;VnTY6IfbO=dQ61V0DisUAs4~t|9`9ZE(jG}ax#-xikDhsO_4^RaK ziZ?9AJQP_{9WuzVk^s_U+3V8gOvVl5(#1>}a|RL>};+uJB%nQM-J>M4~yK)cioytFXtnmOaJZSiE+3g}C`Im~6H z*+-vjI>ng5w>>Y!L(+DwX2gs0!&-BFEaDie4i5ln*NGP$te7$F9iUlJl4`XpkAsPm z0l?GQ17uN^=g~u1*$)S`30xL%!`LW*flwT*#svAtY(kHXFfvA`dj*pDfr0pBZ`!La zWmX$Z@qyv|{nNsRS|+CzN-Pvb>47HEDeUGFhpp5C_NL0Vp~{Wc{bsm_5J!#tuqW@? z)Be zb&Gj&(l*bHQDq7w-b`F9MHEH*{Dh~0`Gn8t`pz}!R+q~4u$T@cVaUu`E^%0f-q*hM z1To6V31UGJN7a-QW5;nhk#C26vmHyjTVZkdV zqYMI9jQY)3oZt=V0L7JZQ=^c2k){Y_lHp&V_LIi*iX^Ih3vZ_K<@Di(hY<&g^f?c$wwF-wX1VLj>ZC4{0#e`XhbL_$a9uXS zKph*4LupSV2TQBCJ4AfOXD8fs2;bAGz-qU4=Qj$^1ZJX z2TtaVdq>OjaWGvv9)agwV)QW9eTZ-xv`us2!yXSARnD5DwX_Vg*@g4w!-zT|5<}-7 zsnllGRQz>k!LwdU`|i&!Bw^W7CTUU3x`Zg8>XgHj=bo!cd<#pI8*pa*1N`gg~I0ace!wzZoJ)oGScm~D_Sc;#wFed zUo;-*0LaWVCC2yqr6IbeW3`hvXyMfAH94qP2|cN``Z%dSuz8HcQ!WT0k38!X34<6l zHtMV%4fH5<6z-lYcK;CTvzzT6-^xSP>~a*8LfbByHyp$|X*#I6HCAi){gCu1nvN%& zvlSbNFJRCc&8>f`$2Qa`fb@w!C11v1KCn)P9<}ei0}g*cl~9A9h=7(}FO!=cVllq3 z7nD)E%gt;&AYdo{Ljb2~Fm5jy{I><%i*GUlU8crR4k(zwQf#nima@xb%O71M#t-4< z(yjX(m^mp_Y;5()naqt2-VibylPS)Oof9uBp$3Gj`>7@gjKwnwRCc>rx%$esn);gI z5B9;~uz57n7Rpm8K^o=_sFPyU?>liHM&8&#O%f)}C5F7gvj#n#TLp@!M~Q?iW~lS}(gy%d&G3p?iBP z(PZQUv07@7!o3~1_l|m5m;Xr)^QK_JaVAY3v1UREC*6>v;AT$BO`nA~KZa1x3kV2F z%iwG7SaaAcT8kalCa^Hg&|eINWmBQA_d8$}B+-Q_@6j_{>a- zwT3CMWG!A}Ef$EvQsjK>o)lJ;q!~#F%wo`k-_mT=+yo%6+`iGe9(XeUl;*-4(`G;M zc@+ep^Xv&<3e7l4wt48iwaLIC1RhSsYrf6>7zXfVD zNNJ1#zM;CjKgfqCabzacX7#oEN{koCnq1-stV+-CMQ=ZX7Fpd*n9`+AEg9=p&q7mTAKXvcbo?$AVvOOp{F>#a;S?joYZl_f}BECS%u&0x!95DR;|QkR9i}`FEAsPb=)I z8nb=4iwjiLRgAF}8WTwAb^eA>QjL4Srqb#n zTwx^-*Z38Uzh@bX$_1tq>m{o8PBX*t3Lqaf$EBqiOU*2NFp{LJX#3}p9{|v{^Hg4f zlhllKI>F+>*%mu6i9V7TT*Wx-zdK z(p8faUOwGOm5mBC%UGA1jO0@IKkG;i&+6Ur8XR2ZuRb$*a}R^-H6eKxcYodlXsF`& z{NkO+;_Yh-Ni@vV9iyzM43Yibn;oC7hPAzC24zs&+RYdY&r`3&&fg2hs62ysV^G`N zHMfBEFo8E3S$0C_m({bL8QCe$B@M{n1dLsaJYIU;(!n*V?0I1OvBB=iYh&`?u8 z&~n-$nbVIhO3mMhCQRlq%XRr1;Hvl=9E_F0sc9!VLnM>@mY~=Cx3K5}wxHKEZF9pC zIdyu1qucM!gEiomw7bW0-RwbX7?o=FE#K0l4`U2KhC8*kMWaEWJyVNZVu_tY2e&4F zb54Lh=Oz>(3?V$!ArXFXh8Cb3i;%KQGCrW$W#;kvx$YA2gofNeu?@nt>Yq8?2uJQp zUTo14hS%&dHF3Uhm~Z1>W)yb%&HoM!3z?%a%dmKT#>}}kKy2B=V3{Nu=bae%V%wU$ zb4%^m?&qn==QeHo`nAs3H}wtiK~!!&i|iBLfazh6!y9F)ToKNyE0B385!zq{p)5vB zvu`R#ULIS|2{3w52c*c$4}Pe>9Fw&U^>Bb_LUWn!xPx3X-uQsv(b1XFvFzn#voq0* z5~o`V_G805QXdgAOwOjoqmZ?uzwBVYSNP0Ie8FL`P0VK1J4CzV@t&%0duHB{;yIL$FZ9 zz#s#%ZG6ya&AwE;0_~^$1K