API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jan 23rd, 2020
99
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 13.67 KB | None | 0 0
raw download clone embed print report
  1. package org.firstinspires.ftc.teamcode.Autonomous;
  2.  
  3. import com.disnodeteam.dogecv.CameraViewDisplay;
  4. import com.disnodeteam.dogecv.DogeCV;
  5. import com.disnodeteam.dogecv.detectors.roverrukus.GoldAlignDetector;
  6. import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
  7. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  8. import com.qualcomm.robotcore.hardware.DcMotor;
  9. import com.qualcomm.robotcore.util.ElapsedTime;
  10.  
  11. /*
  12. * Created by A.J on 1/13/19
  13. */
  14.  
  15. @Autonomous(name="Autodrive Red", group="Pushbot")
  16. //@Disabled
  17. public class AutoRed extends LinearOpMode {
  18.  
  19. Hardware robot = new Hardware();
  20. private ElapsedTime runtime = new ElapsedTime();
  21. private GoldAlignDetector detector;
  22. private boolean found = false;
  23. private int location = 3;
  24.  
  25. static final double COUNTS_PER_MOTOR_REV = 1440 ; // eg: Motor Encoder
  26. static final double DRIVE_GEAR_REDUCTION = 2.0 ; // This is < 1.0 if geared UP
  27. static final double WHEEL_DIAMETER_INCHES = 4.0 ; // For figuring circumference
  28. static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
  29. (WHEEL_DIAMETER_INCHES * 3.1415);
  30. static final double DRIVE_SPEED = 0.4; //full speed is 1.0
  31. static final double TURN_SPEED = 0.125;
  32.  
  33. @Override
  34. public void runOpMode() {
  35.  
  36. robot.init(hardwareMap);
  37.  
  38. // Send telemetry message to signify robot waiting;
  39. telemetry.addData("Status", "Resetting Encoders");
  40. telemetry.update();
  41.  
  42. // Resets encoders and initialize them zero values out
  43. robot.LeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  44. robot.RightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  45. robot.LeftMotorback.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  46. robot.RightMotorback.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  47. robot.Pulley.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  48.  
  49. robot.LeftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  50. robot.RightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  51. robot.LeftMotorback.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  52. robot.RightMotorback.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  53. robot.Pulley.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  54.  
  55. // Send telemetry message to indicate successful Encoder reset
  56. telemetry.addData("Path0", "Starting at %7d :%7d and %7d :%7d",
  57. robot.LeftMotor.getCurrentPosition(),
  58. robot.LeftMotorback.getCurrentPosition(),
  59. robot.RightMotor.getCurrentPosition(),
  60. robot.RightMotorback.getCurrentPosition());
  61.  
  62. telemetry.update();
  63. waitForStart();
  64.  
  65. /* AUTONOMOUS MODE SEQUENCE */
  66.  
  67. /* Unhooks itself */
  68. pulleyControl(1,1,5.625); // moves down to touch the ground 5.6
  69. encoderDrive(1,-2,-2,2); // move left
  70. middleWheelControl(1,6,2 ); // move forwards
  71. encoderDrive(1,-5,-5,5); // move more left
  72.  
  73. /* initalizes detector */
  74. detector = new GoldAlignDetector(); // Create detector
  75. detector.init(hardwareMap.appContext, CameraViewDisplay.getInstance()); // Initialize it with the app context and camera
  76. detector.useDefaults(); // Set detector to use default settings
  77.  
  78. // Optional tuning
  79. detector.alignSize = 100; // How wide (in pixels) is the range in which the gold object will be aligned. (Represented by green bars in the preview)
  80. detector.alignPosOffset = 0; // How far from center frame to offset this alignment zone.
  81. detector.downscale = 0.4; // How much to downscale the input frames
  82.  
  83. detector.areaScoringMethod = DogeCV.AreaScoringMethod.MAX_AREA; // Can also be PERFECT_AREA
  84. //detector.perfectAreaScorer.perfectArea = 10000; // if using PERFECT_AREA scoring
  85. detector.maxAreaScorer.weight = 0.005;
  86.  
  87. detector.ratioScorer.weight = 5;
  88. detector.ratioScorer.perfectRatio = 1.0; // Ratio adjustment
  89.  
  90. detector.enable(); // Start the detector!
  91.  
  92. /* Scans to find cube */
  93. ElapsedTime holdTimer = new ElapsedTime();
  94. holdTimer.reset();
  95.  
  96. int i = 0;
  97. while(opModeIsActive() && (holdTimer.time() < 10 || !found || i<=2)) {
  98. sleep(1000);
  99. if (detector.isFound()) {
  100. middleWheelControl(1,15,10);
  101. found = true;
  102. location = i;
  103. telemetry.addData("boolean: ", found);
  104. telemetry.addData("location ", i);
  105. telemetry.update();
  106. detector.disable();
  107. break;
  108. }else{
  109. if(i==2){
  110. detector.disable();
  111. break;
  112. }else{
  113. if(i==1){
  114. encoderDrive(1, 9, 9, 7);
  115. }else{
  116. encoderDrive(1, 7, 7, 6);
  117. }
  118. i++;
  119. }
  120. }
  121. }
  122.  
  123. switch (location){
  124. /* Moves based on where the cube was located */
  125.  
  126. case 0: // Left
  127. encoderDrive(1,5,-5,5); // turn back toward marker square
  128. encoderDrive(1,6,6,6); // move back towards back
  129. teamMarkerControl(1,0); // drops teammarker
  130. teamMarkerControl(1,1); // picks up servo
  131. encoderDrive(1,-8.5,8.5,6); // turn back toward crater
  132. middleWheelControl(1,10,8); // moves robot toward wall
  133. encoderDrive(1,50,50,20); // toward crater
  134. break;
  135.  
  136. case 1: // Middle
  137. middleWheelControl(1,3,3); // moves forward
  138. encoderDrive(1,10,-10,10); // turns
  139. middleWheelControl(1,-7,5);
  140. teamMarkerControl(1,0);
  141. teamMarkerControl(1,1);
  142. encoderDrive(1,2,-2,2);
  143. encoderDrive(1,-50,-50,15);
  144. break;
  145.  
  146. case 2: // Right
  147. encoderDrive(1,11,-11,10); // turns back toward marker square
  148. encoderDrive(1,5,5,5); // move toward marker square
  149. teamMarkerControl(1,0);
  150. teamMarkerControl(1,1);
  151. middleWheelControl(1,-8,5);
  152. encoderDrive(1,-45,-45,15); // move more left
  153. break;
  154.  
  155. case 3: // Right for 33.3%
  156. encoderDrive(1,11,-11,10); // turns back toward marker square
  157. encoderDrive(1,5,5,5); // move toward marker square
  158. teamMarkerControl(1,0);
  159. teamMarkerControl(1,1);
  160. middleWheelControl(1,-6,5);
  161. encoderDrive(1,-45,-45,15); // move more left
  162. break;
  163. }
  164. }
  165.  
  166.  
  167. public void encoderDrive(double speed,
  168. double leftInches, double rightInches,
  169. double timeoutS) {
  170. /* Used to move/control the drive train (inches) */
  171.  
  172. int newLeftTarget;
  173. int newRightTarget;
  174. int newLeftbackTarget;
  175. int newRightbackTarget;
  176.  
  177. if (opModeIsActive()) {
  178.  
  179. newLeftTarget = robot.LeftMotor.getCurrentPosition() + (int)(leftInches * COUNTS_PER_INCH);
  180. newLeftbackTarget = robot.LeftMotorback.getCurrentPosition() + (int)(leftInches * COUNTS_PER_INCH);
  181. newRightTarget = robot.RightMotor.getCurrentPosition() + (int)(rightInches * COUNTS_PER_INCH);
  182. newRightbackTarget = robot.RightMotorback.getCurrentPosition() + (int)(rightInches * COUNTS_PER_INCH);
  183.  
  184. robot.LeftMotor.setTargetPosition(newLeftTarget);
  185. robot.LeftMotorback.setTargetPosition(newLeftbackTarget);
  186. robot.RightMotor.setTargetPosition(newRightTarget);
  187. robot.RightMotorback.setTargetPosition(newRightbackTarget);
  188.  
  189. robot.LeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  190. robot.LeftMotorback.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  191. robot.RightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  192. robot.RightMotorback.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  193.  
  194.  
  195. runtime.reset();
  196. robot.LeftMotor.setPower(Math.abs(speed));
  197. robot.LeftMotorback.setPower(Math.abs(speed));
  198. robot.RightMotor.setPower(Math.abs(speed));
  199. robot.RightMotorback.setPower(Math.abs(speed));
  200.  
  201.  
  202. while (opModeIsActive() &&
  203. (runtime.seconds() < timeoutS) &&
  204. ((robot.LeftMotor.isBusy() && robot.LeftMotorback.isBusy()) && (robot.RightMotor.isBusy() && robot.RightMotorback.isBusy()))) {
  205.  
  206. // Display it for the driver.
  207. telemetry.addData("Path1", "Running to %7d :%7d", newLeftTarget, newRightTarget);
  208. telemetry.addData("Path2", "Running at %7d :%7d",
  209. robot.LeftMotorback.getCurrentPosition(),
  210. robot.RightMotor.getCurrentPosition());
  211. telemetry.update();
  212. }
  213.  
  214. robot.LeftMotor.setPower(0);
  215. robot.LeftMotorback.setPower(0);
  216. robot.RightMotor.setPower(0);
  217. robot.RightMotorback.setPower(0);
  218.  
  219. robot.LeftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  220. robot.LeftMotorback.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  221. robot.RightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  222. robot.RightMotorback.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  223. sleep(150); // optional pause after each move
  224. }
  225. }
  226.  
  227.  
  228. public void teamMarkerControl(double holdTime, double position){
  229. /* Method to move/control the marker servo */
  230.  
  231. ElapsedTime holdTimerServo = new ElapsedTime();
  232. holdTimerServo.reset();
  233. while(opModeIsActive() && holdTimerServo.time() < holdTime){
  234. robot.teamMarker.setPosition(position);
  235. }
  236. }
  237.  
  238. public void pulleyControl(double speed,
  239. double downinches, double timeoutS) {
  240. /*
  241. Used to move the pulley motor(inches)
  242. */
  243. int Target;
  244.  
  245. if (opModeIsActive()) {
  246.  
  247. Target = robot.Pulley.getCurrentPosition() + (int)(downinches * COUNTS_PER_INCH);
  248. robot.Pulley.setTargetPosition(Target);
  249.  
  250. robot.Pulley.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  251.  
  252. runtime.reset();
  253. robot.Pulley.setPower(Math.abs(speed));
  254.  
  255.  
  256. while (opModeIsActive() &&
  257. (runtime.seconds() < timeoutS) &&
  258. (robot.Pulley.isBusy())) {
  259.  
  260. //Display it for the driver.
  261. telemetry.addData("Path1", "Running to %7d", Target);
  262. telemetry.addData("Path2", "Running at %7d",
  263. robot.Pulley.getCurrentPosition());
  264. telemetry.update();
  265. }
  266.  
  267. robot.Pulley.setPower(0);
  268.  
  269. robot.Pulley.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  270.  
  271. }
  272. }
  273. public void middleWheelControl(double speed,
  274. double inches, double timeoutS) {
  275. /*
  276. Used to move the pulley motor(inches)
  277. */
  278. int Target;
  279.  
  280. if (opModeIsActive()) {
  281.  
  282. Target = robot.MiddleWheel.getCurrentPosition() + (int)(inches * COUNTS_PER_INCH);
  283. robot.MiddleWheel.setTargetPosition(Target);
  284.  
  285. robot.MiddleWheel.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  286.  
  287. runtime.reset();
  288. robot.MiddleWheel.setPower(Math.abs(speed));
  289.  
  290.  
  291. while (opModeIsActive() &&
  292. (runtime.seconds() < timeoutS) &&
  293. (robot.MiddleWheel.isBusy())) {
  294.  
  295. //Display it for the driver.
  296. telemetry.addData("Path1", "Running to %7d", Target);
  297. telemetry.addData("Path2", "Running at %7d",
  298. robot.MiddleWheel.getCurrentPosition());
  299. telemetry.update();
  300. }
  301.  
  302. robot.MiddleWheel.setPower(0);
  303.  
  304. robot.MiddleWheel.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  305. sleep(100);
  306.  
  307. }
  308. }
  309.  
  310. public void findCube(double timeS){
  311. /*
  312. * Scans 135 degrees +- to find a yellow cube while opmode is on.
  313. * Checks from left to right then right to left if still not found.
  314. */
  315.  
  316. ElapsedTime holdTimer = new ElapsedTime();
  317. holdTimer.reset();
  318.  
  319. while(opModeIsActive() && (holdTimer.time() < timeS || found)) {
  320. for (int i = 0; i <= 2 || found; i++) {
  321. if (detector.isFound() && detector.getAligned()) {
  322. encoderDrive(.5, 1, 1, 1);
  323. found = true;
  324. location = i;
  325. } else {
  326. encoderDrive(.5, 1, -1, 1);
  327. sleep(250); // Just to make sure it turns fully before checking again
  328. }
  329. }
  330. if (!found) {
  331. for (int i = 2; i >= 0 || found; i--) {
  332. if (detector.isFound() && detector.getAligned()) {
  333. encoderDrive(.5, 1, 1, 1);
  334. found = true;
  335. location = i;
  336. } else {
  337. encoderDrive(.7, -1, 1, 1);
  338. sleep(250);
  339. }
  340. }
  341. }
  342. }
  343. telemetry.addData("Location found: ", location);
  344. telemetry.update();
  345.  
  346. }
  347.  
  348. }
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!