API tools faq
paste
Advertisement
Guest User

Custom OpenCV + RR opmode

a guest
Jan 23rd, 2020
164
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 14.32 KB | None | 0 0
raw download clone embed print report
  1. package org.firstinspires.ftc.teamcode.drive.opmode;
  2.  
  3. import com.acmerobotics.roadrunner.geometry.Pose2d;
  4. import com.acmerobotics.roadrunner.geometry.Vector2d;
  5. import com.acmerobotics.roadrunner.path.heading.HeadingInterpolator;
  6. import com.acmerobotics.roadrunner.path.heading.LinearInterpolator;
  7. import com.acmerobotics.roadrunner.trajectory.Trajectory;
  8. import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
  9. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  10. import com.qualcomm.robotcore.hardware.DcMotor;
  11. import com.qualcomm.robotcore.hardware.VoltageSensor;
  12.  
  13. import org.firstinspires.ftc.teamcode.drive.mecanum.SampleMecanumDriveBase;
  14. import org.firstinspires.ftc.teamcode.drive.mecanum.SampleMecanumDriveREVOptimized;
  15. import org.firstinspires.ftc.teamcode.robotx.modules.FlywheelIntake;
  16. import org.firstinspires.ftc.teamcode.robotx.modules.FoundationPins;
  17. import org.firstinspires.ftc.teamcode.robotx.modules.OrientationDrive;
  18. import org.firstinspires.ftc.teamcode.robotx.modules.StoneArm;
  19. import org.firstinspires.ftc.teamcode.robotx.modules.StoneClaw;
  20. import org.firstinspires.ftc.teamcode.robotx.modules.StoneLift;
  21. import org.opencv.core.Core;
  22. import org.opencv.core.Mat;
  23. import org.opencv.core.MatOfPoint;
  24. import org.opencv.core.Point;
  25. import org.opencv.core.Scalar;
  26. import org.opencv.imgproc.Imgproc;
  27. import org.openftc.easyopencv.OpenCvCameraFactory;
  28. import org.openftc.easyopencv.OpenCvCameraRotation;
  29. import org.openftc.easyopencv.OpenCvInternalCamera;
  30. import org.openftc.easyopencv.OpenCvPipeline;
  31.  
  32. import java.util.ArrayList;
  33. import java.util.List;
  34.  
  35. import kotlin.Unit;
  36.  
  37. @Autonomous
  38. public class LoadingBlueMain extends LinearOpMode {
  39.  
  40. //0 means skystone, 1+ means yellow stone
  41. //-1 for debug, but we can keep it like this because if it works, it should change to either 0 or 255
  42. private static int valMid = -1;
  43. private static int valLeft = -1;
  44. private static int valRight = -1;
  45.  
  46. private static float rectHeight = .6f / 8f;
  47. private static float rectWidth = 1.5f / 8f;
  48.  
  49. private static float offsetX = 0f
  50. / 8f;//changing this moves the three rects and the three circles left or right, range : (-2, 2) not inclusive
  51. private static float offsetY = 3f
  52. / 8f;//changing this moves the three rects and circles up or down, range: (-4, 4) not inclusive
  53.  
  54. private static float[] midPos = {4f / 8f + offsetX, 4f / 8f + offsetY};//0 = col, 1 = row
  55. private static float[] leftPos = {2f / 8f + offsetX, 4f / 8f + offsetY};
  56. private static float[] rightPos = {6f / 8f + offsetX, 4f / 8f + offsetY};
  57. //moves all rectangles right or left by amount. units are in ratio to monitor
  58.  
  59. private final int rows = 1280;
  60. private final int cols = 720;
  61.  
  62. public boolean isLeft;
  63. public boolean isCenter;
  64. public boolean isRight;
  65.  
  66. public double multiplier = 0;
  67.  
  68. OpenCvInternalCamera phoneCam;
  69. FlywheelIntake flywheelIntake;
  70. //OrientationDrive movement;
  71. StoneArm stoneArm;
  72. StoneClaw stoneClaw;
  73. StoneLift stoneLift;
  74. FoundationPins pins;
  75.  
  76. @Override
  77. public void runOpMode() throws InterruptedException {
  78. int cameraMonitorViewId = hardwareMap.appContext.getResources()
  79. .getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
  80.  
  81. phoneCam = OpenCvCameraFactory.getInstance().createInternalCamera(OpenCvInternalCamera.CameraDirection.BACK, cameraMonitorViewId);
  82. //phoneCam = new OpenCvWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId);
  83.  
  84. phoneCam.openCameraDevice();//open camera
  85. phoneCam.setPipeline(new StageSwitchingPipeline());//different stages
  86. phoneCam.startStreaming(rows, cols, OpenCvCameraRotation.SIDEWAYS_RIGHT);//display on RC
  87. //width, height
  88. //width = height in this case, because camera is in portrait mode.
  89.  
  90. /*if(getBatteryVoltage() >= 14.00){
  91. multiplier = 0.07;
  92. telemetry.addData("Multiplier", multiplier);
  93. }else if(getBatteryVoltage() < 14.00 && getBatteryVoltage() >= 13.80 ){
  94. multiplier = 0.06;
  95. telemetry.addData("Multiplier", multiplier);
  96. }else if(getBatteryVoltage() < 13.80 && getBatteryVoltage() >= 13.65){
  97. multiplier = 0.04;
  98. telemetry.addData("Multiplier", multiplier);
  99. }else if(getBatteryVoltage() < 13.65 && getBatteryVoltage() >= 13.40){
  100. multiplier = 0.023;
  101. telemetry.addData("Multiplier", multiplier);
  102. }else{
  103. multiplier = 0;
  104. telemetry.addData("Multiplier", multiplier);
  105. }*/
  106.  
  107. SampleMecanumDriveBase drive = new SampleMecanumDriveREVOptimized(hardwareMap);
  108.  
  109.  
  110. //movement = new OrientationDrive(this);
  111. //movement.init();
  112.  
  113. flywheelIntake = new FlywheelIntake(this);
  114. flywheelIntake.init();
  115.  
  116. stoneClaw = new StoneClaw(this);
  117. stoneClaw.init();
  118.  
  119. stoneArm = new StoneArm(this);
  120. stoneArm.init();
  121.  
  122. pins = new FoundationPins(this);
  123. pins.init();
  124.  
  125. stoneLift = new StoneLift(this);
  126. stoneLift.init();
  127.  
  128.  
  129.  
  130. //movement.start();
  131. stoneClaw.start();
  132. flywheelIntake.start();
  133. stoneArm.start();
  134. stoneClaw.start();
  135. pins.start();
  136. telemetry.addData("Starting Side: ", "Loading/Skystone");
  137. telemetry.addData("Position: ","Facing back wall, Color Sensor lines up with middle of tile");
  138. telemetry.update();
  139.  
  140. /**movement.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  141. movement.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  142. movement.frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  143. movement.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);**/
  144.  
  145. telemetry.update();
  146. //HeadingInterpolator interp = new LinearInterpolator(Math.toRadians(drive.getExternalHeading()), Math.toRadians(45));
  147.  
  148. Trajectory path2 = drive.trajectoryBuilder()
  149. .lineTo(new Vector2d(-14,15))
  150. .lineTo(new Vector2d(10,15))
  151. .addMarker(1, ()->{
  152. flywheelIntake.flywheelRight.setPower(0.);
  153. flywheelIntake.flywheelLeft.setPower(1.0);
  154. sleep(1200);
  155. flywheelIntake.flywheelRight.setPower(0.0);
  156. flywheelIntake.flywheelLeft.setPower(0.0);
  157. sleep(1000);
  158. pins.deployPins();
  159. stoneArm.clawServo.setPosition(0.6);
  160. return Unit.INSTANCE;
  161. })
  162. .build();
  163.  
  164.  
  165. Trajectory path1 = drive.trajectoryBuilder()
  166. .setReversed(true)
  167. .splineTo(new Pose2d(-14,15,0))
  168. .build();
  169.  
  170.  
  171. waitForStart();
  172.  
  173. if (isStopRequested()) return;
  174.  
  175. drive.followTrajectorySync(path1);
  176. sleep(2500);
  177.  
  178.  
  179. sleep(10000);
  180.  
  181. while (opModeIsActive()) {
  182.  
  183.  
  184.  
  185.  
  186. telemetry.addData("Values", valLeft + " " + valMid + " " + valRight);
  187. telemetry.addData("Height", rows);
  188. telemetry.addData("Width", cols);
  189.  
  190. telemetry.addData("Frame Count", phoneCam.getFrameCount());
  191. telemetry.addData("FPS", String.format("%.2f", phoneCam.getFps()));
  192. telemetry.addData("Total frame time ms", phoneCam.getTotalFrameTimeMs());
  193. telemetry.addData("Pipeline time ms", phoneCam.getPipelineTimeMs());
  194.  
  195.  
  196. telemetry.update();
  197. sleep(100);
  198.  
  199. /////////////////////Movement///////////////////////
  200.  
  201.  
  202.  
  203. if(valLeft == 0 && valMid >= 1 && valRight >= 1){
  204. telemetry.addData("Skystone Position: ", "Left");
  205. telemetry.update();
  206.  
  207. /**Go to skystone 1**/
  208.  
  209.  
  210. /**Collect Skystone 1**/
  211.  
  212.  
  213.  
  214.  
  215. }else if(valLeft >= 1 && valMid >= 1 && valRight == 0){
  216. telemetry.addData("Skystone Position: ", "Right");
  217. telemetry.update();
  218.  
  219. /**Collect Skystone 1**/
  220.  
  221.  
  222.  
  223.  
  224. }else if(valLeft >= 1 && valMid == 0 && valRight >= 1){
  225. telemetry.addData("Skystone Position: ", "center");
  226. telemetry.update();
  227.  
  228. /**Collect first skystone**/
  229.  
  230.  
  231.  
  232. }
  233. /**Reposition Foundation**/ //ONLY CHANGE THINGS BELOW THIS LINE
  234.  
  235.  
  236.  
  237. /**Place stone on foundation**/
  238.  
  239.  
  240. /**Go to second skystone**/
  241.  
  242.  
  243. /**Collect Skystone 2**/
  244.  
  245.  
  246. }
  247. }
  248.  
  249.  
  250. //detection pipeline
  251. static class StageSwitchingPipeline extends OpenCvPipeline {
  252.  
  253. Mat yCbCrChan2Mat = new Mat();
  254. Mat thresholdMat = new Mat();
  255. Mat all = new Mat();
  256. List<MatOfPoint> contoursList = new ArrayList<>();
  257.  
  258. enum Stage {//color difference. greyscale
  259. detection,//includes outlines
  260. THRESHOLD,//b&w
  261. RAW_IMAGE,//displays raw view
  262. }
  263.  
  264. private Stage stageToRenderToViewport = Stage.detection;
  265. private Stage[] stages = Stage.values();
  266.  
  267. @Override
  268. public void onViewportTapped() {
  269. /*
  270. * Note that this method is invoked from the UI thread
  271. * so whatever we do here, we must do quickly.
  272. */
  273.  
  274. int currentStageNum = stageToRenderToViewport.ordinal();
  275.  
  276. int nextStageNum = currentStageNum + 1;
  277.  
  278. if (nextStageNum >= stages.length) {
  279. nextStageNum = 0;
  280. }
  281.  
  282. stageToRenderToViewport = stages[nextStageNum];
  283. }
  284.  
  285. @Override
  286. public Mat processFrame(Mat input) {
  287. contoursList.clear();
  288. /*
  289. * This pipeline finds the contours of yellow blobs such as the Gold Mineral
  290. * from the Rover Ruckus game.
  291. */
  292.  
  293. //color diff cb.
  294. //lower cb = more blue = skystone = white
  295. //higher cb = less blue = yellow stone = grey
  296. Imgproc.cvtColor(input, yCbCrChan2Mat, Imgproc.COLOR_RGB2YCrCb);//converts rgb to ycrcb
  297. Core.extractChannel(yCbCrChan2Mat, yCbCrChan2Mat, 2);//takes cb difference and stores
  298.  
  299. //b&w
  300. Imgproc.threshold(yCbCrChan2Mat, thresholdMat, 102, 255, Imgproc.THRESH_BINARY_INV);
  301.  
  302. //outline/contour
  303. Imgproc.findContours(thresholdMat, contoursList, new Mat(), Imgproc.RETR_LIST,
  304. Imgproc.CHAIN_APPROX_SIMPLE);
  305. yCbCrChan2Mat.copyTo(all);//copies mat object
  306. //Imgproc.drawContours(all, contoursList, -1, new Scalar(255, 0, 0), 3, 8);//draws blue contours
  307.  
  308. //get values from frame
  309. double[] pixMid = thresholdMat.get((int) (input.rows() * midPos[1]),
  310. (int) (input.cols() * midPos[0]));//gets value at circle
  311. valMid = (int) pixMid[0];
  312.  
  313. double[] pixLeft = thresholdMat.get((int) (input.rows() * leftPos[1]),
  314. (int) (input.cols() * leftPos[0]));//gets value at circle
  315. valLeft = (int) pixLeft[0];
  316.  
  317. double[] pixRight = thresholdMat.get((int) (input.rows() * rightPos[1]),
  318. (int) (input.cols() * rightPos[0]));//gets value at circle
  319. valRight = (int) pixRight[0];
  320.  
  321. //create three points
  322. Point pointMid = new Point((int) (input.cols() * midPos[0]),
  323. (int) (input.rows() * midPos[1]));
  324. Point pointLeft = new Point((int) (input.cols() * leftPos[0]),
  325. (int) (input.rows() * leftPos[1]));
  326. Point pointRight = new Point((int) (input.cols() * rightPos[0]),
  327. (int) (input.rows() * rightPos[1]));
  328.  
  329. //draw circles on those points
  330. Imgproc.circle(all, pointMid, 5, new Scalar(255, 0, 0), 1);//draws circle
  331. Imgproc.circle(all, pointLeft, 5, new Scalar(255, 0, 0), 1);//draws circle
  332. Imgproc.circle(all, pointRight, 5, new Scalar(255, 0, 0), 1);//draws circle
  333.  
  334. //draw 3 rectangles
  335. Imgproc.rectangle(//1-3
  336. all,
  337. new Point(
  338. input.cols() * (leftPos[0] - rectWidth / 2),
  339. input.rows() * (leftPos[1] - rectHeight / 2)),
  340. new Point(
  341. input.cols() * (leftPos[0] + rectWidth / 2),
  342. input.rows() * (leftPos[1] + rectHeight / 2)),
  343. new Scalar(0, 255, 0), 3);
  344. Imgproc.rectangle(//3-5
  345. all,
  346. new Point(
  347. input.cols() * (midPos[0] - rectWidth / 2),
  348. input.rows() * (midPos[1] - rectHeight / 2)),
  349. new Point(
  350. input.cols() * (midPos[0] + rectWidth / 2),
  351. input.rows() * (midPos[1] + rectHeight / 2)),
  352. new Scalar(0, 255, 0), 3);
  353. Imgproc.rectangle(//5-7
  354. all,
  355. new Point(
  356. input.cols() * (rightPos[0] - rectWidth / 2),
  357. input.rows() * (rightPos[1] - rectHeight / 2)),
  358. new Point(
  359. input.cols() * (rightPos[0] + rectWidth / 2),
  360. input.rows() * (rightPos[1] + rectHeight / 2)),
  361. new Scalar(0, 255, 0), 3);
  362.  
  363. switch (stageToRenderToViewport) {
  364. case THRESHOLD: {
  365. return thresholdMat;
  366. }
  367.  
  368. case detection: {
  369. return all;
  370. }
  371.  
  372. case RAW_IMAGE: {
  373. return input;
  374. }
  375.  
  376. default: {
  377. return input;
  378. }
  379. }
  380. }
  381.  
  382. }
  383.  
  384.  
  385. double getBatteryVoltage() {
  386. double result = Double.POSITIVE_INFINITY;
  387. for (VoltageSensor sensor : hardwareMap.voltageSensor) {
  388. double voltage = sensor.getVoltage();
  389. if (voltage > 0) {
  390. result = Math.min(result, voltage);
  391. }
  392. }
  393. return result;
  394. }
  395.  
  396. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!