List of usage examples for org.opencv.core Point Point
public Point(double x, double y)
From source file:com.orange.documentare.core.image.segmentation.SegmentationDrawer.java
License:Open Source License
private void drawSpace(DigitalType r) { Scalar colorSpace = new Scalar(64, 64, 64, 0); Core.rectangle(imageMat, new Point(r.x() + 1, r.y()), new Point(r.x() + r.width() - 1, r.y() + r.height()), colorSpace, -1);// ww w. j a va 2 s .c o m }
From source file:com.projectcs2103t.openglestest.OpenGLES20Activity.java
License:Apache License
@Override public Mat onCameraFrame(CameraBridgeViewBase.CvCameraViewFrame inputFrame) { Mat rgba = inputFrame.rgba();/*from w w w. ja v a2 s. co m*/ float projection[] = mCameraProjectionAdapter.getProjectionGL(); Mat CameraMat = mCameraProjectionAdapter.getCVCameraMat(); Mat DistortionMat = mCameraProjectionAdapter.getCVDistortionMat(); Mat ModelViewMat = new Mat(4, 4, CvType.CV_64FC1); int detected = nl.processFrame(rgba.getNativeObjAddr(), CameraMat.getNativeObjAddr(), DistortionMat.getNativeObjAddr(), ModelViewMat.getNativeObjAddr()); float mGLModelView[] = null; if (detected == 1) { mGLModelView = new float[16]; mGLModelView[0] = (float) ModelViewMat.get(0, 0)[0]; mGLModelView[1] = (float) ModelViewMat.get(0, 1)[0]; mGLModelView[2] = (float) ModelViewMat.get(0, 2)[0]; mGLModelView[3] = (float) ModelViewMat.get(0, 3)[0]; mGLModelView[4] = (float) ModelViewMat.get(1, 0)[0]; mGLModelView[5] = (float) ModelViewMat.get(1, 1)[0]; mGLModelView[6] = (float) ModelViewMat.get(1, 2)[0]; mGLModelView[7] = (float) ModelViewMat.get(1, 3)[0]; mGLModelView[8] = (float) ModelViewMat.get(2, 0)[0]; mGLModelView[9] = (float) ModelViewMat.get(2, 1)[0]; mGLModelView[10] = (float) ModelViewMat.get(2, 2)[0]; mGLModelView[11] = (float) ModelViewMat.get(2, 3)[0]; mGLModelView[12] = (float) ModelViewMat.get(3, 0)[0]; mGLModelView[13] = (float) ModelViewMat.get(3, 1)[0]; mGLModelView[14] = (float) ModelViewMat.get(3, 2)[0]; mGLModelView[15] = (float) ModelViewMat.get(3, 3)[0]; //showMatrices(rgba, ModelViewMat); } mCameraProjectionAdapter.setModelViewGL(mGLModelView); Imgproc.putText(rgba, mCameraProjectionAdapter.toString(), new Point(50, 50), Core.FONT_HERSHEY_PLAIN, 1.0, new Scalar(0, 255, 0)); Imgproc.putText(rgba, mGLView.toString(), new Point(50, 75), Core.FONT_HERSHEY_PLAIN, 1.0, new Scalar(0, 255, 0)); return rgba; }
From source file:com.projectcs2103t.openglestest.OpenGLES20Activity.java
License:Apache License
private void showMatrices(Mat rgba, Mat mat) { double m00 = mat.get(0, 0)[0]; double m01 = mat.get(0, 1)[0]; double m02 = mat.get(0, 2)[0]; double m03 = mat.get(0, 3)[0]; double m10 = mat.get(1, 0)[0]; double m11 = mat.get(1, 1)[0]; double m12 = mat.get(1, 2)[0]; double m13 = mat.get(1, 3)[0]; double m20 = mat.get(2, 0)[0]; double m21 = mat.get(2, 1)[0]; double m22 = mat.get(2, 2)[0]; double m23 = mat.get(2, 3)[0]; double m30 = mat.get(3, 0)[0]; double m31 = mat.get(3, 1)[0]; double m32 = mat.get(3, 2)[0]; double m33 = mat.get(3, 3)[0]; //String camMatStr = cameraMat.dump(); String mRow0 = "|" + m00 + "," + m01 + "," + m02 + "," + m03 + "|"; String mRow1 = "|" + m10 + "," + m11 + "," + m12 + "," + m13 + "|"; String mRow2 = "|" + m20 + "," + m21 + "," + m22 + "," + m23 + "|"; String mRow3 = "|" + m30 + "," + m31 + "," + m32 + "," + m33 + "|"; Imgproc.putText(rgba, "Model-View-Mat:", new Point(50, 100), Core.FONT_HERSHEY_PLAIN, 1.0, new Scalar(0, 255, 0)); Imgproc.putText(rgba, mRow0, new Point(50, 125), Core.FONT_HERSHEY_PLAIN, 1.0, new Scalar(0, 255, 0)); Imgproc.putText(rgba, mRow1, new Point(50, 150), Core.FONT_HERSHEY_PLAIN, 1.0, new Scalar(0, 255, 0)); Imgproc.putText(rgba, mRow2, new Point(50, 175), Core.FONT_HERSHEY_PLAIN, 1.0, new Scalar(0, 255, 0)); Imgproc.putText(rgba, mRow3, new Point(50, 200), Core.FONT_HERSHEY_PLAIN, 1.0, new Scalar(0, 255, 0)); }
From source file:com.qualcomm.ftcrobotcontroller.ManualVisionSample.java
License:Open Source License
@Override public Mat frame(Mat rgba, Mat gray) { try {/*from w w w. ja va2 s . com*/ //Process the frame for the color blobs detectorRed.process(rgba); detectorBlue.process(rgba); //Get the list of contours List<Contour> contoursRed = detectorRed.getContours(); List<Contour> contoursBlue = detectorBlue.getContours(); //Get color analysis Beacon beacon = new Beacon(Beacon.AnalysisMethod.DEFAULT); colorAnalysis = beacon.analyzeFrame(contoursRed, contoursBlue, rgba, gray); //Draw red and blue contours Drawing.drawContours(rgba, contoursRed, new ColorRGBA(255, 0, 0), 2); Drawing.drawContours(rgba, contoursBlue, new ColorRGBA(0, 0, 255), 2); //Transform.enlarge(mRgba, originalSize, true); //Transform.enlarge(mGray, originalSize, true); //Draw text Drawing.drawText(rgba, colorAnalysis.toString(), new Point(0, 8), 1.0f, new ColorGRAY(255), Drawing.Anchor.BOTTOMLEFT); noError = true; } catch (Exception e) { Drawing.drawText(rgba, "Analysis Error", new Point(0, 8), 1.0f, new ColorRGBA("#F44336"), Drawing.Anchor.BOTTOMLEFT); noError = false; e.printStackTrace(); } Drawing.drawText(rgba, "FPS: " + fps.getFPSString(), new Point(0, 24), 1.0f, new ColorRGBA("#ffffff")); //"#2196F3" return rgba; }
From source file:com.qualcomm.ftcrobotcontroller.opmodes.DreamTeamAutonomous.java
License:Open Source License
@Override public void loop() { //This is the switch for the state machine switch (state) { case START://from w w w . j a va 2 s . c o m telemetry.addData("STATE", "START"); targetPosition = ((stepCount / 12.56)) * 20;//9.8cm blDone = motorBL.getCurrentPosition() > targetPosition; flDone = motorFL.getCurrentPosition() > targetPosition; frDone = motorFR.getCurrentPosition() > targetPosition; brDone = motorBR.getCurrentPosition() > targetPosition; telemetry.addData("MotorFL", motorBL.getCurrentPosition()); //Check to see if each motor has finished if (blDone) { motorBLPow = 0; } else { motorBLPow = .3; } if (flDone) { motorFLPow = 0; } else { motorFLPow = .3; } if (brDone) { motorBRPow = 0; } else { motorBRPow = .3; } if (frDone) { motorFRPow = 0; } else { motorFRPow = .3; } //Check to see if all states are done, and if they are, move on to the next state if (blDone && frDone && brDone && flDone) { state = nextState; //After the following state finishes, what state should should go next nextState = State.FORWARD; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } break; case TURN: telemetry.addData("STATE", "TURN"); targetPosition = rotateDistance(43) * (stepCount / 12.56); blDone = motorBL.getCurrentPosition() > encBL + targetPosition; flDone = motorFL.getCurrentPosition() < encFL - targetPosition; frDone = motorFR.getCurrentPosition() < encFR - targetPosition; brDone = motorBR.getCurrentPosition() > encBR + targetPosition; //Check to see if each motor has finished if (blDone) { motorBLPow = 0; } else { motorBLPow = .3; } if (flDone) { motorFLPow = 0; } else { motorFLPow = -.3; } if (brDone) { motorBRPow = 0; } else { motorBRPow = .3; } if (frDone) { motorFRPow = 0; } else { motorFRPow = -.3; } //Check to see if all states are done, and if they are, move on to the next state if (blDone && frDone && brDone && flDone) { state = nextState; //After the following state finishes, what state should should go next nextState = State.TURN; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } break; case FORWARD: telemetry.addData("STATE", "FORWARD"); targetPosition = ((stepCount / 12.56)) * 42;//9.8cm blDone = motorBL.getCurrentPosition() > encBL + targetPosition; flDone = motorFL.getCurrentPosition() > encFL + targetPosition; frDone = motorFR.getCurrentPosition() > encFR + targetPosition; brDone = motorBR.getCurrentPosition() > encBR + targetPosition; telemetry.addData("MotorFL", motorBL.getCurrentPosition()); //Check to see if each motor has finished if (blDone) { motorBLPow = 0; } else { motorBLPow = .3; } if (flDone) { motorFLPow = 0; } else { motorFLPow = .3; } if (brDone) { motorBRPow = 0; } else { motorBRPow = .3; } if (frDone) { motorFRPow = 0; } else { motorFRPow = .3; } //Check to see if all states are done, and if they are, move on to the next state if (blDone && frDone && brDone && flDone) { state = nextState; //After the following state finishes, what state should should go next nextState = State.ALIGN; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } break; case ALIGN: telemetry.addData("STATE", "ALIGN"); servoButton.setPosition(.45); cd.caploop(); if (cd.getCon() != null && Imgproc.contourArea(cd.getCon()) > 100) { Point center; //I want to store the centroid of the mask in the point 'center' //I also have access to the contour I want to use above //but however I try to compute the moments, I get a crash Rect rect = Imgproc.boundingRect(cd.getCon()); center = new Point((rect.x + rect.width) / 2, (rect.y + rect.height) / 2); points.add(center); if (points.size() > 3) { points.remove(0); } Point average = new Point(); for (Point p : points) { average.x = average.x + p.x; average.y = average.y + p.y; } average.y = (average.y / points.size()); average.x = (average.x / points.size()); telemetry.addData("X, Y", average.x + " " + average.y); if (average.y > 60) { motorBLPow = -.04; motorFLPow = .04; motorBRPow = .04; motorFRPow = -.04; telemetry.addData("Turn", "Left"); } else if (average.y < 45) { motorBLPow = .04; motorFLPow = -.04; motorBRPow = -.04; motorFRPow = .04; telemetry.addData("Turn", "Right"); } else { //Detected and tracked successfully. //wait(100); //The beacon is now centered telemetry.addData("Contour Area", Imgproc.contourArea(cd.getCon())); if (Imgproc.contourArea(cd.getCon()) < 3000) { motorBLPow = 0.07; motorFLPow = 0.07; motorBRPow = 0.07; motorFRPow = 0.07; } else { motorBLPow = 0; motorFLPow = 0; motorBRPow = 0; motorFRPow = 0; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); state = State.BUTTON; } } } else { points.clear(); // motorBLPow = -.1; motorFLPow = .1; motorBRPow = .1; motorFRPow = -.1; } break; case BUTTON: telemetry.addData("STATE", "BUTTON"); targetPosition = ((stepCount / 12.56)) * 2.25; blDone = motorBL.getCurrentPosition() > encBL + targetPosition; flDone = motorFL.getCurrentPosition() < encFL - targetPosition; frDone = motorFR.getCurrentPosition() > encFR + targetPosition; brDone = motorBR.getCurrentPosition() < encBR - targetPosition; telemetry.addData("MotorFL", motorBL.getCurrentPosition()); //Check to see if each motor has finished if (blDone && !buttonSubForward) { motorBLPow = 0; } else { motorBLPow = .1; } if (flDone && !buttonSubForward) { motorFLPow = 0; } else { motorFLPow = -.1; } if (brDone && !buttonSubForward) { motorBRPow = 0; } else { motorBRPow = -.1; } if (frDone && !buttonSubForward) { motorFRPow = 0; } else { motorFRPow = .1; } boolean tempDone = (blDone && frDone && brDone && flDone); //Check to see if all states are done, and if they are, move on to the next state if (tempDone || buttonSubForward) { if (tempDone) { encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } buttonSubForward = true; targetPosition = ((stepCount / 12.56)) * 4.2; blDone = motorBL.getCurrentPosition() > encBL + targetPosition; flDone = motorFL.getCurrentPosition() > encFL + targetPosition; frDone = motorFR.getCurrentPosition() > encFR + targetPosition; brDone = motorBR.getCurrentPosition() > encBR + targetPosition; if (blDone) { motorBLPow = 0; } else { motorBLPow = .1; } if (flDone) { motorFLPow = 0; } else { motorFLPow = .1; } if (brDone) { motorBRPow = 0; } else { motorBRPow = .1; } if (frDone) { motorFRPow = 0; } else { motorFRPow = .1; } tempDone = (blDone && frDone && brDone && flDone); if (tempDone) { state = State.CLIMBER_ALIGN_BACKWARD; nextState = State.CLIMBER_ALIGN_FORWARD; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } } break; case CLIMBERS: telemetry.addData("STATE", "CLIMBERS"); if (servoArmPos < .9) { servoArmPos += climberServoInc; } if (servoClimberPos < .9) { servoClimberPos += climberServoInc; } boolean done = !(servoClimberPos < .8 && servoArmPos < .7); servoArm.setPosition(servoArmPos); servoClimbers.setPosition(servoClimberPos); if (done) { servoClimbers.setPosition(0); servoArm.setPosition(.7); state = State.MOVE_PARK; } break; case CLIMBER_ALIGN_BACKWARD: telemetry.addData("STATE", "CLIMBER_ALIGN_BACKWARD"); targetPosition = ((stepCount / 12.56)) * 1.5; blDone = motorBL.getCurrentPosition() < encBL - targetPosition; flDone = motorFL.getCurrentPosition() < encFL - targetPosition; frDone = motorFR.getCurrentPosition() < encFR - targetPosition; brDone = motorBR.getCurrentPosition() < encBR - targetPosition; if (blDone) { motorBLPow = 0; } else { motorBLPow = -.1; } if (flDone) { motorFLPow = 0; } else { motorFLPow = -.1; } if (brDone) { motorBRPow = 0; } else { motorBRPow = -.1; } if (frDone) { motorFRPow = 0; } else { motorFRPow = -.1; } tempDone = (blDone && frDone && brDone && flDone); if (tempDone) { state = nextState; nextState = State.CLIMBERS; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } break; case CLIMBER_ALIGN_FORWARD: telemetry.addData("STATE", "CLIMBER_ALIGN_FORWARD"); targetPosition = ((stepCount / 12.56)) * 6.5; servoButton.setPosition(1); blDone = motorBL.getCurrentPosition() > encBL + targetPosition; flDone = motorFL.getCurrentPosition() > encFL + targetPosition; frDone = motorFR.getCurrentPosition() > encFR + targetPosition; brDone = motorBR.getCurrentPosition() > encBR + targetPosition; if (blDone) { motorBLPow = 0; } else { motorBLPow = .1; } if (flDone) { motorFLPow = 0; } else { motorFLPow = .1; } if (brDone) { motorBRPow = 0; } else { motorBRPow = .1; } if (frDone) { motorFRPow = 0; } else { motorFRPow = .1; } tempDone = (blDone && frDone && brDone && flDone); if (tempDone) { state = nextState; nextState = State.TEST; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); startClimberTime = System.currentTimeMillis(); } break; case MOVE_PARK: targetPosition = ((stepCount / 12.56)) * 18; blDone = motorBL.getCurrentPosition() > encBL + targetPosition; flDone = motorFL.getCurrentPosition() < encFL - targetPosition; frDone = motorFR.getCurrentPosition() > encFR + targetPosition; brDone = motorBR.getCurrentPosition() < encBR - targetPosition; telemetry.addData("MotorFL", motorBL.getCurrentPosition()); /* motorBLPow = .1; motorFLPow = -.1; motorBRPow = -.1; motorFRPow = .1; */ //Check to see if each motor has finished if (blDone) { motorBLPow = 0; } else { motorBLPow = .1; } if (flDone) { motorFLPow = 0; } else { motorFLPow = -.1; } if (brDone) { motorBRPow = 0; } else { motorBRPow = -.1; } if (frDone) { motorFRPow = 0; } else { motorFRPow = .1; } tempDone = (blDone && frDone && brDone && flDone); //Check to see if all states are done, and if they are, move on to the next state if (tempDone) { state = State.TEST; servoClimbers.setPosition(.35); servoArm.setPosition(.01); encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } break; default: break; } //Always execute after every loop- this will update motor speeds. motorBR.setPower(motorBRPow); motorFL.setPower(motorFLPow); motorFR.setPower(motorFRPow); motorBL.setPower(motorBLPow); //Servos servoLeft.setPosition(servoLeftPos); servoRight.setPosition(servoRightPos); }
From source file:com.qualcomm.ftcrobotcontroller.opmodes.DreamTeamAutonomousRed.java
License:Open Source License
@Override public void loop() { //This is the switch for the state machine switch (state) { case INIT:// w w w . j a va 2s . co m initGame(); state = nextState; nextState = State.TURN; break; case START: telemetry.addData("STATE", "START"); targetPosition = ((stepCount / 12.56)) * 20;//9.8cm blDone = motorBL.getCurrentPosition() > targetPosition; flDone = motorFL.getCurrentPosition() > targetPosition; frDone = motorFR.getCurrentPosition() > targetPosition; brDone = motorBR.getCurrentPosition() > targetPosition; telemetry.addData("MotorFL", motorBL.getCurrentPosition()); //Check to see if each motor has finished if (blDone) { motorBLPow = 0; } else { motorBLPow = .3; } if (flDone) { motorFLPow = 0; } else { motorFLPow = .3; } if (brDone) { motorBRPow = 0; } else { motorBRPow = .3; } if (frDone) { motorFRPow = 0; } else { motorFRPow = .3; } //Check to see if all states are done, and if they are, move on to the next state if (blDone && frDone && brDone && flDone) { state = nextState; //After the following state finishes, what state should should go next nextState = State.FORWARD; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } break; case TURN: telemetry.addData("STATE", "TURN"); targetPosition = rotateDistance(43) * (stepCount / 12.56); blDone = motorBL.getCurrentPosition() < encBL - targetPosition; flDone = motorFL.getCurrentPosition() > encFL + targetPosition; frDone = motorFR.getCurrentPosition() > encFR + targetPosition; brDone = motorBR.getCurrentPosition() < encBR - targetPosition; //Check to see if each motor has finished if (blDone) { motorBLPow = 0; } else { motorBLPow = -.3; } if (flDone) { motorFLPow = 0; } else { motorFLPow = .3; } if (brDone) { motorBRPow = 0; } else { motorBRPow = -.3; } if (frDone) { motorFRPow = 0; } else { motorFRPow = .3; } //Check to see if all states are done, and if they are, move on to the next state if (blDone && frDone && brDone && flDone) { state = nextState; //After the following state finishes, what state should should go next timeSinceStateSwitch = System.currentTimeMillis(); nextState = State.TURN; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } break; case FORWARD: telemetry.addData("STATE", "FORWARD"); targetPosition = ((stepCount / 12.56)) * 42;//9.8cm blDone = motorBL.getCurrentPosition() > encBL + targetPosition; flDone = motorFL.getCurrentPosition() > encFL + targetPosition; frDone = motorFR.getCurrentPosition() > encFR + targetPosition; brDone = motorBR.getCurrentPosition() > encBR + targetPosition; telemetry.addData("MotorFL", motorBL.getCurrentPosition()); //Check to see if each motor has finished if (blDone) { motorBLPow = 0; } else { motorBLPow = .3; } if (flDone) { motorFLPow = 0; } else { motorFLPow = .3; } if (brDone) { motorBRPow = 0; } else { motorBRPow = .3; } if (frDone) { motorFRPow = 0; } else { motorFRPow = .3; } //Check to see if all states are done, and if they are, move on to the next state if (blDone && frDone && brDone && flDone) { state = nextState; //After the following state finishes, what state should should go next nextState = State.ALIGN; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } break; case ALIGN: telemetry.addData("STATE", "ALIGN"); servoButton.setPosition(.45); cd.caploop(); if (cd.getCon() != null && Imgproc.contourArea(cd.getCon()) > 100) { Point center; //I want to store the centroid of the mask in the point 'center' //I also have access to the contour I want to use above //but however I try to compute the moments, I get a crash Rect rect = Imgproc.boundingRect(cd.getCon()); center = new Point((rect.x + rect.width) / 2, (rect.y + rect.height) / 2); points.add(center); if (points.size() > 3) { points.remove(0); } Point average = new Point(); for (Point p : points) { average.x = average.x + p.x; average.y = average.y + p.y; } average.y = (average.y / points.size()); average.x = (average.x / points.size()); telemetry.addData("X, Y", average.x + " " + average.y); if (average.y > 60) { motorBLPow = -.04; motorFLPow = .04; motorBRPow = .04; motorFRPow = -.04; telemetry.addData("Turn", "Left"); } else if (average.y < 45) { motorBLPow = .04; motorFLPow = -.04; motorBRPow = -.04; motorFRPow = .04; telemetry.addData("Turn", "Right"); } else { //Detected and tracked successfully. //wait(100); //The beacon is now centered telemetry.addData("Contour Area", Imgproc.contourArea(cd.getCon())); if (Imgproc.contourArea(cd.getCon()) < 3000) { motorBLPow = 0.07; motorFLPow = 0.07; motorBRPow = 0.07; motorFRPow = 0.07; } else { motorBLPow = 0; motorFLPow = 0; motorBRPow = 0; motorFRPow = 0; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); if (didGoLeft && didGoRight) { state = State.BUTTON_SIDE; nextState = State.CLIMBER_ALIGN_FORWARD; } else { state = State.BUTTON_RIGHT; } } } } else { points.clear(); // motorBLPow = .1; motorFLPow = -.1; motorBRPow = -.1; motorFRPow = .1; } break; case BUTTON: telemetry.addData("STATE", "BUTTON"); targetPosition = ((stepCount / 12.56)) * 2.25; blDone = motorBL.getCurrentPosition() > encBL + targetPosition; flDone = motorFL.getCurrentPosition() < encFL - targetPosition; frDone = motorFR.getCurrentPosition() > encFR + targetPosition; brDone = motorBR.getCurrentPosition() < encBR - targetPosition; telemetry.addData("MotorFL", motorBL.getCurrentPosition()); //Check to see if each motor has finished if (blDone && !buttonSubForward) { motorBLPow = 0; } else { motorBLPow = .1; } if (flDone && !buttonSubForward) { motorFLPow = 0; } else { motorFLPow = -.1; } if (brDone && !buttonSubForward) { motorBRPow = 0; } else { motorBRPow = -.1; } if (frDone && !buttonSubForward) { motorFRPow = 0; } else { motorFRPow = .1; } boolean tempDone = (blDone && frDone && brDone && flDone); //Check to see if all states are done, and if they are, move on to the next state if (tempDone || buttonSubForward) { if (tempDone) { encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } buttonSubForward = true; targetPosition = ((stepCount / 12.56)) * 3.2; blDone = motorBL.getCurrentPosition() > encBL + targetPosition; flDone = motorFL.getCurrentPosition() > encFL + targetPosition; frDone = motorFR.getCurrentPosition() > encFR + targetPosition; brDone = motorBR.getCurrentPosition() > encBR + targetPosition; if (blDone) { motorBLPow = 0; } else { motorBLPow = .1; } if (flDone) { motorFLPow = 0; } else { motorFLPow = .1; } if (brDone) { motorBRPow = 0; } else { motorBRPow = .1; } if (frDone) { motorFRPow = 0; } else { motorFRPow = .1; } tempDone = (blDone && frDone && brDone && flDone); if (tempDone) { state = State.CLIMBER_ALIGN_BACKWARD; nextState = State.CLIMBER_ALIGN_FORWARD; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } } break; case BUTTON_LEFT: didGoLeft = true; telemetry.addData("STATE", "BUTTON_LEFT"); targetPosition = ((stepCount / 12.56)) * 6.8; blDone = motorBL.getCurrentPosition() < encBL - targetPosition; flDone = motorFL.getCurrentPosition() > encFL + targetPosition; frDone = motorFR.getCurrentPosition() < encFR - targetPosition; brDone = motorBR.getCurrentPosition() > encBR + targetPosition; telemetry.addData("MotorFL", motorBL.getCurrentPosition()); //Check to see if each motor has finished if (blDone && !buttonSubForward) { motorBLPow = 0; } else { motorBLPow = -.1; } if (flDone && !buttonSubForward) { motorFLPow = 0; } else { motorFLPow = .1; } if (brDone && !buttonSubForward) { motorBRPow = 0; } else { motorBRPow = .1; } if (frDone && !buttonSubForward) { motorFRPow = 0; } else { motorFRPow = -.1; } tempDone = (blDone && frDone && brDone && flDone); //Check to see if all states are done, and if they are, move on to the next state if (tempDone || buttonSubForward) { if (tempDone) { encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } buttonSubForward = true; targetPosition = ((stepCount / 12.56)) * 4; blDone = motorBL.getCurrentPosition() > encBL + targetPosition; flDone = motorFL.getCurrentPosition() > encFL + targetPosition; frDone = motorFR.getCurrentPosition() > encFR + targetPosition; brDone = motorBR.getCurrentPosition() > encBR + targetPosition; if (blDone) { motorBLPow = 0; } else { motorBLPow = .1; } if (flDone) { motorFLPow = 0; } else { motorFLPow = .1; } if (brDone) { motorBRPow = 0; } else { motorBRPow = .1; } if (frDone) { motorFRPow = 0; } else { motorFRPow = .1; } tempDone = (blDone && frDone && brDone && flDone); if (tempDone) { state = State.BUTTON_BACKUP; nextState = State.ALIGN; button_backup = 8; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } } break; case BUTTON_RIGHT: didGoRight = true; telemetry.addData("STATE", "BUTTON_RIGHT"); targetPosition = ((stepCount / 12.56)) * 5.7; blDone = motorBL.getCurrentPosition() > encBL + targetPosition; flDone = motorFL.getCurrentPosition() < encFL - targetPosition; frDone = motorFR.getCurrentPosition() > encFR + targetPosition; brDone = motorBR.getCurrentPosition() < encBR - targetPosition; telemetry.addData("MotorFL", motorBL.getCurrentPosition()); //Check to see if each motor has finished if (blDone && !buttonSubForward) { motorBLPow = 0; } else { motorBLPow = .1; } if (flDone && !buttonSubForward) { motorFLPow = 0; } else { motorFLPow = -.1; } if (brDone && !buttonSubForward) { motorBRPow = 0; } else { motorBRPow = -.1; } if (frDone && !buttonSubForward) { motorFRPow = 0; } else { motorFRPow = .1; } tempDone = (blDone && frDone && brDone && flDone); //Check to see if all states are done, and if they are, move on to the next state if (tempDone || buttonSubForward) { if (tempDone) { encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } buttonSubForward = true; targetPosition = ((stepCount / 12.56)) * 4.2; blDone = motorBL.getCurrentPosition() > encBL + targetPosition; flDone = motorFL.getCurrentPosition() > encFL + targetPosition; frDone = motorFR.getCurrentPosition() > encFR + targetPosition; brDone = motorBR.getCurrentPosition() > encBR + targetPosition; if (blDone) { motorBLPow = 0; } else { motorBLPow = .1; } if (flDone) { motorFLPow = 0; } else { motorFLPow = .1; } if (brDone) { motorBRPow = 0; } else { motorBRPow = .1; } if (frDone) { motorFRPow = 0; } else { motorFRPow = .1; } tempDone = (blDone && frDone && brDone && flDone); if (tempDone) { state = State.BUTTON_BACKUP; nextState = State.BUTTON_LEFT; buttonSubForward = false; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } } break; case CLIMBERS: telemetry.addData("STATE", "CLIMBERS"); if (servoArmPos < .9) { servoArmPos += climberServoInc; } if (servoClimberPos < .9) { servoClimberPos += climberServoInc; } boolean done = !(servoClimberPos < .8 && servoArmPos < .7); servoArm.setPosition(servoArmPos); servoClimbers.setPosition(servoClimberPos); if (done) { servoClimbers.setPosition(0); servoArm.setPosition(.7); state = State.MOVE_PARK; } break; case BUTTON_BACKUP: telemetry.addData("STATE", "BUTTON_BACKUP"); targetPosition = ((stepCount / 12.56)) * button_backup; blDone = motorBL.getCurrentPosition() < encBL - targetPosition; flDone = motorFL.getCurrentPosition() < encFL - targetPosition; frDone = motorFR.getCurrentPosition() < encFR - targetPosition; brDone = motorBR.getCurrentPosition() < encBR - targetPosition; if (blDone) { motorBLPow = 0; } else { motorBLPow = -.1; } if (flDone) { motorFLPow = 0; } else { motorFLPow = -.1; } if (brDone) { motorBRPow = 0; } else { motorBRPow = -.1; } if (frDone) { motorFRPow = 0; } else { motorFRPow = -.1; } tempDone = (blDone && frDone && brDone && flDone); if (tempDone) { state = nextState; nextState = State.CLIMBER_ALIGN_FORWARD; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } break; case CLIMBER_ALIGN_BACKWARD: telemetry.addData("STATE", "CLIMBER_ALIGN_BACKWARD"); targetPosition = ((stepCount / 12.56)) * 1.5; blDone = motorBL.getCurrentPosition() < encBL - targetPosition; flDone = motorFL.getCurrentPosition() < encFL - targetPosition; frDone = motorFR.getCurrentPosition() < encFR - targetPosition; brDone = motorBR.getCurrentPosition() < encBR - targetPosition; if (blDone) { motorBLPow = 0; } else { motorBLPow = -.1; } if (flDone) { motorFLPow = 0; } else { motorFLPow = -.1; } if (brDone) { motorBRPow = 0; } else { motorBRPow = -.1; } if (frDone) { motorFRPow = 0; } else { motorFRPow = -.1; } tempDone = (blDone && frDone && brDone && flDone); if (tempDone) { state = nextState; nextState = State.CLIMBERS; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } break; case CLIMBER_ALIGN_FORWARD: telemetry.addData("STATE", "CLIMBER_ALIGN_FORWARD"); targetPosition = ((stepCount / 12.56)) * 6.5; servoButton.setPosition(1); blDone = motorBL.getCurrentPosition() > encBL + targetPosition; flDone = motorFL.getCurrentPosition() > encFL + targetPosition; frDone = motorFR.getCurrentPosition() > encFR + targetPosition; brDone = motorBR.getCurrentPosition() > encBR + targetPosition; if (blDone) { motorBLPow = 0; } else { motorBLPow = .1; } if (flDone) { motorFLPow = 0; } else { motorFLPow = .1; } if (brDone) { motorBRPow = 0; } else { motorBRPow = .1; } if (frDone) { motorFRPow = 0; } else { motorFRPow = .1; } tempDone = (blDone && frDone && brDone && flDone); if (tempDone) { state = nextState; nextState = State.TEST; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); startClimberTime = System.currentTimeMillis(); } break; case BUTTON_SIDE: telemetry.addData("STATE", "FORWARD"); targetPosition = ((stepCount / 12.56)) * 1;//9.8cm blDone = motorBL.getCurrentPosition() > encBL + targetPosition; flDone = motorFL.getCurrentPosition() < encFL - targetPosition; frDone = motorFR.getCurrentPosition() > encFR + targetPosition; brDone = motorBR.getCurrentPosition() < encBR - targetPosition; if (blDone && !buttonSubForward) { motorBLPow = 0; } else { motorBLPow = .1; } if (flDone && !buttonSubForward) { motorFLPow = 0; } else { motorFLPow = -.1; } if (brDone && !buttonSubForward) { motorBRPow = 0; } else { motorBRPow = -.1; } if (frDone && !buttonSubForward) { motorFRPow = 0; } else { motorFRPow = .1; } //Check to see if all states are done, and if they are, move on to the next state if (blDone && frDone && brDone && flDone) { state = nextState; //After the following state finishes, what state should should go next nextState = State.CLIMBERS; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } break; case BUTTON_RESET_SCAN: targetPosition = ((stepCount / 12.56)) * 36; blDone = motorBL.getCurrentPosition() < encBL - targetPosition; flDone = motorFL.getCurrentPosition() > encFL + targetPosition; frDone = motorFR.getCurrentPosition() < encFR - targetPosition; brDone = motorBR.getCurrentPosition() > encBR + targetPosition; telemetry.addData("MotorFL", motorBL.getCurrentPosition()); //Check to see if each motor has finished if (blDone) { motorBLPow = 0; } else { motorBLPow = -.1; } if (flDone) { motorFLPow = 0; } else { motorFLPow = .1; } if (brDone) { motorBRPow = 0; } else { motorBRPow = .1; } if (frDone) { motorFRPow = 0; } else { motorFRPow = -.1; } tempDone = (blDone && frDone && brDone && flDone); //Check to see if all states are done, and if they are, move on to the next state if (tempDone) { state = State.ALIGN; timeSinceStateSwitch = System.currentTimeMillis(); encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } break; case MOVE_PARK: targetPosition = ((stepCount / 12.56)) * 18; blDone = motorBL.getCurrentPosition() < encBL - targetPosition; flDone = motorFL.getCurrentPosition() > encFL + targetPosition; frDone = motorFR.getCurrentPosition() < encFR - targetPosition; brDone = motorBR.getCurrentPosition() > encBR + targetPosition; telemetry.addData("MotorFL", motorBL.getCurrentPosition()); /* motorBLPow = .1; motorFLPow = -.1; motorBRPow = -.1; motorFRPow = .1; */ //Check to see if each motor has finished if (blDone) { motorBLPow = 0; } else { motorBLPow = -.1; } if (flDone) { motorFLPow = 0; } else { motorFLPow = .1; } if (brDone) { motorBRPow = 0; } else { motorBRPow = .1; } if (frDone) { motorFRPow = 0; } else { motorFRPow = -.1; } tempDone = (blDone && frDone && brDone && flDone); //Check to see if all states are done, and if they are, move on to the next state if (tempDone) { state = State.TEST; servoArm.setPosition(0.02); servoClimbers.setPosition(0.35); encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } break; default: break; } //Always execute after every loop- this will update motor speeds. motorBR.setPower(motorBRPow); motorFL.setPower(motorFLPow); motorFR.setPower(motorFRPow); motorBL.setPower(motorBLPow); //Servos servoLeft.setPosition(servoLeftPos); servoRight.setPosition(servoRightPos); }
From source file:com.qualcomm.ftcrobotcontroller.opmodes.DreamTeamAutonomousRedClose.java
License:Open Source License
@Override public void loop() { //Joy1Y = gamepad1.left_stick_y; //Joy1X = gamepad1.left_stick_x; // clip the right/left values so that the values never exceed +/- 1 //Joy1Y = Range.clip(Joy1Y, -1, 1); //Joy1X = Range.clip(Joy1X, -1, 1); //write to motors //Keep in mind that the power for opposite motors is the same //powerBR_FL = calculateMotorPower(Joy1X, Joy1Y); //powerFR_BL = calculateMotorPower(Joy1X, -Joy1Y); //double offset = scaleInput(gamepad1.right_stick_x); //This is the switch for the state machine if (delayTime + 5000 < System.currentTimeMillis()) { switch (state) { case START: telemetry.addData("STATE", "START"); targetPosition = ((stepCount / 12.56)) * 20;//9.8cm blDone = motorBL.getCurrentPosition() > targetPosition; flDone = motorFL.getCurrentPosition() > targetPosition; frDone = motorFR.getCurrentPosition() > targetPosition; brDone = motorBR.getCurrentPosition() > targetPosition; telemetry.addData("MotorFL", motorBL.getCurrentPosition()); //Check to see if each motor has finished if (blDone) { motorBLPow = 0;/*from w ww. ja va 2 s. co m*/ } else { motorBLPow = .3; } if (flDone) { motorFLPow = 0; } else { motorFLPow = .3; } if (brDone) { motorBRPow = 0; } else { motorBRPow = .3; } if (frDone) { motorFRPow = 0; } else { motorFRPow = .3; } //Check to see if all states are done, and if they are, move on to the next state if (blDone && frDone && brDone && flDone) { state = nextState; //After the following state finishes, what state should should go next nextState = State.FORWARD; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } break; case TURN: telemetry.addData("STATE", "TURN"); targetPosition = rotateDistance(43) * (stepCount / 12.56); blDone = motorBL.getCurrentPosition() < encBL - targetPosition; flDone = motorFL.getCurrentPosition() > encFL + targetPosition; frDone = motorFR.getCurrentPosition() > encFR + targetPosition; brDone = motorBR.getCurrentPosition() < encBR - targetPosition; //Check to see if each motor has finished if (blDone) { motorBLPow = 0; } else { motorBLPow = -.3; } if (flDone) { motorFLPow = 0; } else { motorFLPow = .3; } if (brDone) { motorBRPow = 0; } else { motorBRPow = -.3; } if (frDone) { motorFRPow = 0; } else { motorFRPow = .3; } //Check to see if all states are done, and if they are, move on to the next state if (blDone && frDone && brDone && flDone) { state = nextState; //After the following state finishes, what state should should go next nextState = State.TURN; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } break; case FORWARD: telemetry.addData("STATE", "FORWARD"); targetPosition = ((stepCount / 12.56)) * 55;//9.8cm blDone = motorBL.getCurrentPosition() > encBL + targetPosition; flDone = motorFL.getCurrentPosition() > encFL + targetPosition; frDone = motorFR.getCurrentPosition() > encFR + targetPosition; brDone = motorBR.getCurrentPosition() > encBR + targetPosition; telemetry.addData("MotorFL", motorBL.getCurrentPosition()); //Check to see if each motor has finished if (blDone) { motorBLPow = 0; } else { motorBLPow = .3; } if (flDone) { motorFLPow = 0; } else { motorFLPow = .3; } if (brDone) { motorBRPow = 0; } else { motorBRPow = .3; } if (frDone) { motorFRPow = 0; } else { motorFRPow = .3; } //Check to see if all states are done, and if they are, move on to the next state if (blDone && frDone && brDone && flDone) { state = nextState; //After the following state finishes, what state should should go next nextState = State.ALIGN; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } break; case ALIGN: telemetry.addData("STATE", "ALIGN"); cd.caploop(); if (cd.getCon() != null && Imgproc.contourArea(cd.getCon()) > 100) { Point center; //I want to store the centroid of the mask in the point 'center' //I also have access to the contour I want to use above //but however I try to compute the moments, I get a crash Rect rect = Imgproc.boundingRect(cd.getCon()); center = new Point((rect.x + rect.width) / 2, (rect.y + rect.height) / 2); points.add(center); if (points.size() > 3) { points.remove(0); } Point average = new Point(); for (Point p : points) { average.x = average.x + p.x; average.y = average.y + p.y; } average.y = (average.y / points.size()); average.x = (average.x / points.size()); telemetry.addData("X, Y", average.x + " " + average.y); if (average.y < 45) { motorBLPow = .04; motorFLPow = -.04; motorBRPow = -.04; motorFRPow = .04; telemetry.addData("Turn", "Left"); } else if (average.y > 60) { motorBLPow = -.04; motorFLPow = .04; motorBRPow = .04; motorFRPow = -.04; telemetry.addData("Turn", "Right"); } else { //Detected and tracked successfully. //wait(100); //The beacon is now centered telemetry.addData("Contour Area", Imgproc.contourArea(cd.getCon())); if (Imgproc.contourArea(cd.getCon()) < 2000) { motorBLPow = 0.07; motorFLPow = 0.07; motorBRPow = 0.07; motorFRPow = 0.07; } else { motorBLPow = 0; motorFLPow = 0; motorBRPow = 0; motorFRPow = 0; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); state = State.BUTTON; } } } else { points.clear(); // motorBLPow = -.05; motorFLPow = .05; motorBRPow = .05; motorFRPow = -.05; } break; case BUTTON: telemetry.addData("STATE", "BUTTON"); targetPosition = (stepCount / 12.56) * 8;//9.8cm /* motorBLPow = .04; motorFLPow = -.04; motorBRPow = -.04; motorFRPow = .04; */ blDone = motorBL.getCurrentPosition() < encBL - targetPosition; flDone = motorFL.getCurrentPosition() > encFL + targetPosition; frDone = motorFR.getCurrentPosition() < encFR - targetPosition; brDone = motorBR.getCurrentPosition() > encBR + targetPosition; telemetry.addData("MotorFL", motorBL.getCurrentPosition()); //Check to see if each motor has finished if (blDone && !buttonSubForward) { motorBLPow = 0; } else { motorBLPow = -.1; } if (flDone && !buttonSubForward) { motorFLPow = 0; } else { motorFLPow = .1; } if (brDone && !buttonSubForward) { motorBRPow = 0; } else { motorBRPow = .1; } if (frDone && !buttonSubForward) { motorFRPow = 0; } else { motorFRPow = -.1; } boolean tempDone = (blDone && frDone && brDone && flDone); //Check to see if all states are done, and if they are, move on to the next state if (tempDone || buttonSubForward) { if (tempDone) { encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); } buttonSubForward = true; targetPosition = ((stepCount / 12.56)) * 8; blDone = motorBL.getCurrentPosition() > encBL + targetPosition; flDone = motorFL.getCurrentPosition() > encFL + targetPosition; frDone = motorFR.getCurrentPosition() > encFR + targetPosition; brDone = motorBR.getCurrentPosition() > encBR + targetPosition; if (blDone) { motorBLPow = 0; } else { motorBLPow = .1; } if (flDone) { motorFLPow = 0; } else { motorFLPow = .1; } if (brDone) { motorBRPow = 0; } else { motorBRPow = .1; } if (frDone) { motorFRPow = 0; } else { motorFRPow = .1; } tempDone = (blDone && frDone && brDone && flDone); if (tempDone) { // servoArm.setPosition(.8); servoClimbers.setPosition(1); // state = State.CLIMBERS; encBL = motorBL.getCurrentPosition(); encBR = motorBR.getCurrentPosition(); encFR = motorFR.getCurrentPosition(); encFL = motorFL.getCurrentPosition(); startClimberTime = System.currentTimeMillis(); } } break; case CLIMBERS: telemetry.addData("STATE", "CLIMBERS"); if (servoArmPos < .89) { servoArmPos += climberServoInc; } if (servoClimberPos > .05) { servoClimberPos -= climberServoInc / 3; } boolean done = !(servoClimberPos > .05 && servoArmPos < .850); servoArm.setPosition(servoArmPos); servoClimbers.setPosition(servoClimberPos); if (done) state = State.TEST; break; default: break; } } //Always execute after every loop- this will update motor speeds. motorBR.setPower(motorBRPow); motorFL.setPower(motorFLPow); motorFR.setPower(motorFRPow); motorBL.setPower(motorBLPow); //Servos servoLeft.setPosition(servoLeftPos); servoRight.setPosition(servoRightPos); servoPan.setPosition(servoPanPos); //Debug output if (debug) { //Current State telemetry.addData("DEBUG", "State: " + state.name()); //Motors telemetry.addData("DEBUG", "BR: " + motorBRPow); telemetry.addData("DEBUG", "FR: " + motorFRPow); telemetry.addData("DEBUG", "BL: " + motorBLPow); telemetry.addData("DEBUG", "FL: " + motorFLPow); //Servos telemetry.addData("DEBUG", "Right: " + motorFLPow); telemetry.addData("DEBUG", "Left: " + motorFLPow); telemetry.addData("DEBUG", "Pan: " + motorFLPow); telemetry.addData("DEBUG", "Button: Disabled");// + motorFLPow); //telemetry.addData("DEBUG", "Climbers: Disabled"); } }
From source file:com.raspoid.additionalcomponents.camera.opencv.FaceDetector.java
License:Open Source License
/** * Creates a new output image from the input image with faces surrounded with green boxes. * @param image the input image previously analyzed. * @param faces array of coordinates corresponding to the previously detected faces. * @param outputFilename the output file name. * @return true in cas of success when creating the output file. False in case of failure. */// ww w . j ava 2 s. c om public static boolean surroundFaces(Mat image, Rect[] faces, String outputFilename) { if (outputFilename == null || outputFilename.isEmpty()) throw new RaspoidException("The output filename can't be empty."); for (Rect face : faces) Core.rectangle(image, new Point(face.x, face.y), new Point(face.x + face.width, face.y + face.height), new Scalar(0, 255, 0)); return Highgui.imwrite(outputFilename, image); }
From source file:com.raulh82vlc.face_detection_sample.opencv.domain.EyesDetectionInteractorImpl.java
License:Apache License
/** * Matches concrete point of the eye by using template with TM_SQDIFF_NORMED *///from ww w. jav a 2 s . c o m private static void matchEye(Rect area, Mat builtTemplate, Mat matrixGray, Mat matrixRGBA) { Point matchLoc; try { // when there is not builtTemplate we skip it if (builtTemplate.cols() == 0 || builtTemplate.rows() == 0) { return; } Mat submatGray = matrixGray.submat(area); int cols = submatGray.cols() - builtTemplate.cols() + 1; int rows = submatGray.rows() - builtTemplate.rows() + 1; Mat outputTemplateMat = new Mat(cols, rows, CvType.CV_8U); Imgproc.matchTemplate(submatGray, builtTemplate, outputTemplateMat, Imgproc.TM_SQDIFF_NORMED); Core.MinMaxLocResult minMaxLocResult = Core.minMaxLoc(outputTemplateMat); // when is difference in matching methods, the best match is max / min value matchLoc = minMaxLocResult.minLoc; Point matchLocTx = new Point(matchLoc.x + area.x, matchLoc.y + area.y); Point matchLocTy = new Point(matchLoc.x + builtTemplate.cols() + area.x, matchLoc.y + builtTemplate.rows() + area.y); FaceDrawerOpenCV.drawMatchedEye(matchLocTx, matchLocTy, matrixRGBA); } catch (Exception e) { e.printStackTrace(); } }
From source file:com.raulh82vlc.face_detection_sample.opencv.render.FaceDrawerOpenCV.java
License:Apache License
public static void drawFaceShapes(Rect face, Mat matrixRGBA) { Point start = face.tl();/*from w w w .j av a 2 s.c o m*/ int h = (int) start.y + (face.height / 2); int w = (int) start.x + (face.width / 2); Imgproc.rectangle(matrixRGBA, start, face.br(), FACE_RECT_COLOR, 3); Point center = new Point(w, h); Imgproc.circle(matrixRGBA, center, 10, new Scalar(255, 0, 0, 255), 3); }