API tools faq
paste
Advertisement
roll11226

Vuforia21.1

roll11226
Jan 22nd, 2017
115
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 8.72 KB | None | 0 0
raw download clone embed print report
  1. package org.firstinspires.ftc.teamcode;
  2.  
  3. import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
  4. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  5. import com.qualcomm.robotcore.hardware.DcMotor;
  6. import com.qualcomm.robotcore.hardware.DcMotorSimple;
  7. import com.vuforia.HINT;
  8. import com.vuforia.Vuforia;
  9.  
  10. import org.firstinspires.ftc.robotcore.external.ClassFactory;
  11. import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
  12. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
  13. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener;
  14. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
  15.  
  16. @Autonomous(name="VuforiaOP_test")
  17. public class VuforiaOP_Test extends LinearOpMode {
  18.  
  19. public void runOpMode() throws InterruptedException {
  20. HardWare11226 m_robot = new HardWare11226();
  21. final double CAMERA_FORWARD_DISPLACEMENT = 100; // Camera is 100 mm in front of robot center
  22. final double WHEEL_CIRCUMFERENCE = 315.993955469;
  23. final double MOTOR_CPR = 1120;
  24. // final int CAMERA_VERTICAL_DISPLACEMENT = 200; // Camera is 200 mm above ground
  25. // final int CAMERA_LEFT_DISPLACEMENT = 0; // Camera is ON the robots center line
  26. boolean ran_once = false;
  27.  
  28. VuforiaLocalizer.Parameters params = new VuforiaLocalizer.Parameters(R.id.cameraMonitorViewId); // Show the camera on the screen of the phone, to save battery you can stop it by removing "R.id.cameraMonitorViewId".
  29. params.cameraDirection = VuforiaLocalizer.CameraDirection.BACK; // tell which camera is used.
  30. params.vuforiaLicenseKey = "ASGVUHP/////AAAAGfG8A3nPG0JxkZdKAEiS+7xCi4mxG2B2msUP9NQyzuGr6Py/efOiOgwPc5NQtnY3IgLHHTC0o7UtcDSMVbXG+XJ2UUEE7DzELEFsCcaM94K+awMwovz4QyctBwZYloUG1Itj2rSzByEMP8zMFPfftW474YRrZdnrnirdYu/JtwUaQgcdK2ZrYmvbfBFZlouyPZeVaWt1a559yOhOnEj2lhrfjnL7TYb7m/lth87yFTs4DTswazns1Y5RUB+Em3yCwUQnttIpsRrBRF4b42mysHQWNK3lDyBkrsiFI0BlTbfTYQR3pUp0Us3y+5TivoVBy6n+5IpBwPysU8Da5YLHdmroPcLDZcV58WPR0QtT2ew5"; // THe LicenseKey of Vuforia.
  31. params.cameraMonitorFeedback = VuforiaLocalizer.Parameters.CameraMonitorFeedback.AXES; // show the axes on the screen.
  32.  
  33. VuforiaLocalizer vuforia = ClassFactory.createVuforiaLocalizer(params);
  34. Vuforia.setHint(HINT.HINT_MAX_SIMULTANEOUS_IMAGE_TARGETS, 4); // Set 4 becaons on the field.
  35.  
  36. VuforiaTrackables beacons = vuforia.loadTrackablesFromAsset("FTC_2016-17");
  37. beacons.get(0).setName("Wheels");
  38. beacons.get(1).setName("Tools");
  39. beacons.get(2).setName("Legos");
  40. beacons.get(3).setName("Gears");
  41.  
  42. VuforiaTrackableDefaultListener wheels = (VuforiaTrackableDefaultListener) beacons.get(0).getListener();
  43.  
  44. m_robot.init(hardwareMap);
  45.  
  46. waitForStart();
  47. while (opModeIsActive() && !ran_once)
  48. {
  49.  
  50. beacons.activate();
  51.  
  52. m_robot.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  53. m_robot.rightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  54. m_robot.leftMotor.setDirection(DcMotor.Direction.FORWARD);
  55. m_robot.rightMotor.setDirection(DcMotor.Direction.REVERSE);
  56.  
  57. while (wheels.getRawPose() == null)
  58. {
  59. m_robot.leftMotor.setPower(0.1);
  60. m_robot.rightMotor.setPower(0.1);
  61. }
  62.  
  63. m_robot.leftMotor.setPower(0);
  64. m_robot.rightMotor.setPower(0);
  65.  
  66. telemetry.addData("flag 1","made it");
  67.  
  68. VectorF angles = anglesFromTarget(wheels);
  69.  
  70. VectorF trans = navOffWall(wheels.getPose().getTranslation(), Math.toDegrees(angles.get(0)) - 90, new VectorF(500, 0, 0));
  71.  
  72. while(wheels.getPose().getTranslation().get(0)> -50 && wheels.getPose().getTranslation().get(0) < 50)
  73. {
  74. if (trans.get(0) > 0)
  75. {
  76. m_robot.leftMotor.setPower(0.1);
  77. m_robot.rightMotor.setPower(-0.1);
  78. m_robot.collectMotor.setPower(0.1);
  79. sleep(50);
  80. m_robot.collectMotor.setPower(0);
  81. } else
  82. {
  83. m_robot.leftMotor.setPower(-0.1);
  84. m_robot.rightMotor.setPower(0.1);
  85. m_robot.collectMotor.setPower(0.1);
  86. sleep(50);
  87. m_robot.collectMotor.setPower(0);
  88.  
  89. }
  90. trans = navOffWall(wheels.getPose().getTranslation(), Math.toDegrees(angles.get(0)) - 90, new VectorF(500, 0, 0));
  91. }
  92.  
  93.  
  94. m_robot.leftMotor.setPower(0);
  95. m_robot.rightMotor.setPower(0);
  96. /*
  97. if (trans.get(0) > 0)
  98. {
  99. m_robot.leftMotor.setPower(0.3);
  100. m_robot.rightMotor.setPower(-0.3);
  101. } else
  102. {
  103. m_robot.leftMotor.setPower(-0.3);
  104. m_robot.rightMotor.setPower(0.3);
  105. }
  106. telemetry.addData("flag 2","made it");
  107.  
  108. do
  109. {
  110. if (wheels.getPose() != null)
  111. {
  112. trans = navOffWall(wheels.getPose().getTranslation(), Math.toDegrees(angles.get(0)) -90, new VectorF(500, 0, 0));
  113. }
  114. idle();
  115. }
  116. while (opModeIsActive() && Math.abs(trans.get(0)) > 30);*/
  117.  
  118. m_robot.leftMotor.setPower(0);
  119. m_robot.rightMotor.setPower(0);
  120.  
  121. telemetry.addData("flag 3","made it");
  122.  
  123. m_robot.leftMotor.setTargetPosition((int) (m_robot.leftMotor.getCurrentPosition() + ((Math.hypot(trans.get(0), trans.get(2)) + CAMERA_FORWARD_DISPLACEMENT)
  124. / WHEEL_CIRCUMFERENCE * MOTOR_CPR)));
  125. m_robot.rightMotor.setTargetPosition((int) (m_robot.rightMotor.getCurrentPosition() + ((Math.hypot(trans.get(0), trans.get(2)) + CAMERA_FORWARD_DISPLACEMENT)
  126. / WHEEL_CIRCUMFERENCE * MOTOR_CPR)));
  127.  
  128. m_robot.leftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  129. m_robot.rightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  130.  
  131. m_robot.leftMotor.setPower(0.3);
  132. m_robot.rightMotor.setPower(0.3);
  133.  
  134. while (m_robot.leftMotor.isBusy() && m_robot.rightMotor.isBusy()) {
  135. idle();
  136. }
  137.  
  138. m_robot.leftMotor.setPower(0);
  139. m_robot.rightMotor.setPower(0);
  140.  
  141. telemetry.addData("flag 4","made it");
  142.  
  143. m_robot.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  144. m_robot.rightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  145.  
  146. while ((wheels.getPose() == null || Math.abs(wheels.getPose().getTranslation().get(0)) > 10)) {
  147. if (wheels.getPose() != null) {
  148. if (wheels.getPose().getTranslation().get(0) > 0) {
  149. m_robot.leftMotor.setPower(-0.3);
  150. m_robot.rightMotor.setPower(0.3);
  151. }
  152. else {
  153. m_robot.leftMotor.setPower(0.3);
  154. m_robot.rightMotor.setPower(-0.3);
  155. }
  156. }
  157. else {
  158. m_robot.leftMotor.setPower(0.3);
  159. m_robot.rightMotor.setPower(-0.3);
  160. }
  161.  
  162. }
  163.  
  164. m_robot.leftMotor.setPower(0);
  165. m_robot.rightMotor.setPower(0);
  166. ran_once = true;
  167. telemetry.addData("flag 5","made it");
  168. }
  169. }
  170.  
  171.  
  172. private VectorF navOffWall(VectorF p_trans, double p_robotAngle, VectorF p_offWall) { // Set where the robot need to reach
  173. return new VectorF((float) (p_trans.get(0) - p_offWall.get(0) * Math.sin(Math.toRadians(p_robotAngle)) - p_offWall.get(2) * Math.cos(Math.toRadians(p_robotAngle))), p_trans.get(1), (float) (p_trans.get(2) + p_offWall.get(0) * Math.cos(Math.toRadians(p_robotAngle)) - p_offWall.get(2) * Math.sin(Math.toRadians(p_robotAngle))));
  174. }
  175.  
  176. private VectorF anglesFromTarget(VuforiaTrackableDefaultListener p_image) {
  177. float[] data = p_image.getRawPose().getData();
  178. float[][] rotation = {{data[0], data[1]}, {data[4], data[5], data[6]}, {data[8], data[9], data[10]}};
  179.  
  180. double thetaX = Math.atan2(rotation[2][1], rotation[2][2]);
  181. double thetaY = Math.atan2(-rotation[2][0], Math.sqrt(rotation[2][1] * rotation[2][1] + rotation[2][2] * rotation[2][2]));
  182. double thetaZ = Math.atan2(rotation[1][0], rotation[0][0]);
  183. return new VectorF((float) thetaX, (float) thetaY, (float) thetaZ);
  184. }
  185.  
  186. }
  187.  
  188. //https://github.com/gearsincorg/FTCVuforiaDemo
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!