API tools faq
paste
willieshi232

Red Crater

willieshi232
Dec 5th, 2018
115
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 18.94 KB | None | 0 0
raw download clone embed print report
  1. package org.firstinspires.ftc.teamcode;
  2.  
  3. import com.qualcomm.robotcore.eventloop.opmode.OpMode;
  4. import com.qualcomm.hardware.bosch.BNO055IMU;
  5. import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
  6. import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
  7. import com.qualcomm.robotcore.eventloop.opmode.Disabled;
  8. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  9. import com.qualcomm.robotcore.hardware.DcMotor;
  10. import com.qualcomm.robotcore.util.ElapsedTime;
  11.  
  12. import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
  13. import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
  14. import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
  15. import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
  16. import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
  17. import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
  18.  
  19.  
  20.  
  21.  
  22. @Autonomous(name = "MecanumAutonRedCraterMain", group = "Testing")
  23. public class MecanumAutonRedCraterMain extends LinearOpMode
  24. {
  25. MecanumHardware robot = new MecanumHardware();
  26. private ElapsedTime runtime = new ElapsedTime();
  27.  
  28.  
  29. // The IMU sensor object
  30. BNO055IMU imu;
  31.  
  32. // State used for updating telemetry
  33. Orientation angles;
  34. Acceleration gravity;
  35.  
  36. //turn headings
  37. public float NORTH;
  38. public float EAST;
  39. public float WEST;
  40. public float SOUTH;
  41.  
  42. static final double COUNTS_PER_MOTOR_REV = 1120 ; // Currently: Andymark Neverest 40
  43. static final double DRIVE_GEAR_REDUCTION = 2.0 ; // This is < 1.0 if geared UP //On OUR CENTER MOTOR THE GEAR REDUCTION IS .5
  44. static final double DRIVE_GEAR_REDUCTION_CM = 0.5 ;
  45. static final double WHEEL_DIAMETER_INCHES = 3.54331; // For figuring circumference
  46. static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
  47. (WHEEL_DIAMETER_INCHES * 3.1415);
  48. static final double COUNTS_PER_INCH_CM = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION_CM) /
  49. (WHEEL_DIAMETER_INCHES * 3.1415);
  50. static final double DRIVE_SPEED = .6;
  51. static final double TURN_SPEED = .25;
  52.  
  53. //Encoder position tracking variables
  54. double lefttrack;
  55. double righttrack;
  56.  
  57. double lefttarget;
  58. double righttarget;
  59.  
  60.  
  61. public void runOpMode()
  62. {
  63. robot.init(hardwareMap);
  64.  
  65. //run using and stop and reset encoders for all relevant motors
  66. stopAndReset();
  67.  
  68. waitForStart();
  69. encoderElevator(1, -7.9,40);
  70. gyroinit();
  71. //BACKS OUT FROM HOOK
  72. encoderDrive(1,"b",10, DRIVE_SPEED);
  73. sleep(200);
  74. encoderDrive(4.5,"r",10, DRIVE_SPEED);
  75. sleep(200);
  76. encoderDrive(.7,"f",5, DRIVE_SPEED);
  77. sleep(200);
  78.  
  79. //Knocks out center mineral
  80. encoderDrive(13,"r",10, DRIVE_SPEED);
  81. sleep(200);
  82.  
  83. //go back
  84. encoderDrive(3,"l",10, DRIVE_SPEED);
  85. sleep(200);
  86.  
  87. //Go to the wall
  88. encoderDrive(14,"f",10, DRIVE_SPEED);
  89. sleep(200);
  90.  
  91. //turns/moves to deposit marker
  92. turnDegrees(133,TURN_SPEED,4.5);
  93. while(robot.sensordist.getDistance(DistanceUnit.INCH) > 4.9)
  94. {
  95. telemetry.addData("dist:",(robot.sensordist.getDistance(DistanceUnit.INCH)));
  96. telemetry.update();
  97. robot.fLMotor.setPower(.35);
  98. robot.fRMotor.setPower(-.35);
  99. robot.bLMotor.setPower(-.35);
  100. robot.bRMotor.setPower(.35);
  101.  
  102. }
  103. robot.fLMotor.setPower(0);
  104. robot.fRMotor.setPower(0);
  105. robot.bLMotor.setPower(0);
  106. robot.bRMotor.setPower(0);
  107. sleep(100);
  108. telemetry.addData("Z", readAngle("z"));
  109. telemetry.addData("y", readAngle("y"));
  110. telemetry.addData("x", readAngle("x"));
  111. telemetry.update();
  112.  
  113. encoderDrive(18,"b",10, DRIVE_SPEED);
  114. sleep(200);
  115.  
  116. //back to the wall again(avoid hitting silver)
  117. while(robot.sensordist.getDistance(DistanceUnit.INCH) > 4.9)
  118. {
  119. telemetry.addData("dist:",(robot.sensordist.getDistance(DistanceUnit.INCH)));
  120. telemetry.update();
  121. robot.fLMotor.setPower(.35);
  122. robot.fRMotor.setPower(-.35);
  123. robot.bLMotor.setPower(-.35);
  124. robot.bRMotor.setPower(.35);
  125.  
  126. }
  127. robot.fLMotor.setPower(0);
  128. robot.fRMotor.setPower(0);
  129. robot.bLMotor.setPower(0);
  130. robot.bRMotor.setPower(0);
  131. sleep(100);
  132.  
  133. dropAmerica();
  134. sleep(500);
  135.  
  136.  
  137. while(readAngle("x") < .5)
  138. {
  139.  
  140. telemetry.addData("Z", readAngle("z"));
  141. telemetry.addData("y", readAngle("y"));
  142. telemetry.addData("x", readAngle("x"));
  143. telemetry.addData("time", runtime.seconds());
  144. telemetry.addData("dist:",(robot.sensordist.getDistance(DistanceUnit.INCH)));
  145. telemetry.update();
  146.  
  147. if(Math.abs(readAngle("z")) > 130)
  148. {
  149.  
  150. telemetry.addData("C1:",(robot.sensordist.getDistance(DistanceUnit.INCH)));
  151. telemetry.update();
  152. //foward
  153. robot.fLMotor.setPower(-.6);
  154. robot.fRMotor.setPower(-.6);
  155. robot.bLMotor.setPower(-.6);
  156. robot.bRMotor.setPower(-.6);
  157. }
  158. else
  159. {
  160. telemetry.addData("C2:",(robot.sensordist.getDistance(DistanceUnit.INCH)));
  161. telemetry.update();
  162. //right?
  163. robot.fRMotor.setPower(.1);
  164. robot.bRMotor.setPower(.1);
  165. robot.fLMotor.setPower(-.1);
  166. robot.bLMotor.setPower(-.1);
  167. }
  168.  
  169. }
  170. robot.fLMotor.setPower(0);
  171. robot.fRMotor.setPower(0);
  172. robot.bLMotor.setPower(0);
  173. robot.bRMotor.setPower(0);
  174. sleep(100);
  175.  
  176.  
  177.  
  178.  
  179.  
  180. }
  181. public void stopAndReset() {
  182. robot.fLMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  183. robot.fLMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  184. robot.bLMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  185. robot.bLMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  186. robot.fRMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  187. robot.fRMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  188. robot.bRMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  189. robot.bRMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  190. robot.elevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  191. robot.elevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  192. }
  193.  
  194. public void encoderDrive(double inches, String direction, double timeoutS, double Speed)
  195. {
  196.  
  197. int TargetFL = 0;
  198. int TargetFR = 0;
  199. int TargetBL = 0;
  200. int TargetBR = 0;
  201.  
  202.  
  203. String heading = direction;
  204.  
  205. // Ensure that the opmode is still active
  206. if (opModeIsActive()) {
  207. if(heading == "f")
  208. {
  209. TargetFL = robot.fLMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH);
  210. TargetFR = robot.fRMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH);
  211. TargetBL = robot.bLMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH);
  212. TargetBR = robot.bRMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH);
  213.  
  214. }
  215.  
  216. else if(heading == "b")
  217. {
  218. TargetFL = robot.fLMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH);
  219. TargetFR = robot.fRMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH);
  220. TargetBL = robot.bLMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH);
  221. TargetBR = robot.bRMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH);
  222.  
  223.  
  224. }
  225.  
  226. else if(heading == "r")
  227. {
  228. TargetFL = robot.fLMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH);
  229. TargetFR = robot.fRMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH);
  230. TargetBL = robot.bLMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH);
  231. TargetBR = robot.bRMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH); //weird should be +
  232.  
  233.  
  234. }
  235.  
  236. else if(heading == "l")
  237. {
  238. TargetFL = robot.fLMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH);
  239. TargetFR = robot.fRMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH);
  240. TargetBL = robot.bLMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH); // weird should be +
  241. TargetBR = robot.bRMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH);
  242.  
  243. }
  244.  
  245. else
  246. {
  247. telemetry.addData("not a valid direction", heading );
  248. }
  249.  
  250. // Determine new target position, and pass to motor controller
  251.  
  252. robot.fLMotor.setTargetPosition(TargetFL);
  253. robot.fRMotor.setTargetPosition(TargetFR);
  254. robot.bRMotor.setTargetPosition(TargetBR);
  255. robot.bLMotor.setTargetPosition(TargetBL);
  256.  
  257.  
  258. // Turn On RUN_TO_POSITION
  259. robot.fLMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  260. robot.fRMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  261. robot.bRMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  262. robot.bLMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  263.  
  264.  
  265. // reset the timeout time and start motion.
  266. runtime.reset();
  267. robot.fLMotor.setPower(Math.abs(Speed));
  268. robot.fRMotor.setPower(Math.abs(Speed));
  269. robot.bRMotor.setPower(Math.abs(Speed));
  270. robot.bLMotor.setPower(Math.abs(Speed));
  271.  
  272.  
  273. // keep looping while we are still active, and there is time left, and both motors are running.
  274. // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits
  275. // its target position, the motion will stop. This is "safer" in the event that the robot will
  276. // always end the motion as soon as possible.
  277. // However, if you require that BOTH motors have finished their moves before the robot continues
  278. // onto the next step, use (isBusy() || isBusy()) in the loop test.
  279. while (opModeIsActive() &&
  280. (runtime.seconds() < timeoutS) && ((robot.fLMotor.isBusy() && robot.fRMotor.isBusy()) && robot.bLMotor.isBusy() && robot.bRMotor.isBusy()))
  281. {
  282.  
  283. //Display it for the driver.
  284. telemetry.addData("Path1", "Running to %7d :%7d :%7d :%7d", TargetFL, TargetFR, TargetBL, TargetBR);
  285.  
  286. telemetry.addData("Path2", "Running at %7d :%7d :%7d :%7d", robot.fLMotor.getCurrentPosition(), robot.fRMotor.getCurrentPosition(), robot.bLMotor.getCurrentPosition(), robot.bRMotor.getCurrentPosition());
  287. //telemetry.addData("speeds", "Running to %7f :%7f :%7f :%7f", speedfL, speedfR, speedfL, speedbR);
  288. telemetry.update();
  289. }
  290.  
  291. // Stop all motion;
  292. robot.fLMotor.setPower(0);
  293. robot.bLMotor.setPower(0);
  294. robot.fRMotor.setPower(0);
  295. robot.bRMotor.setPower(0);
  296.  
  297. // Turn off RUN_TO_POSITION
  298. robot.bRMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  299. robot.bLMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  300. robot.fRMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  301. robot.fLMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  302. // sleep(250); // optional pause after each move
  303. }
  304. }
  305. public void setCardinalDir()
  306. {
  307. updateAngles();
  308. NORTH = angles.firstAngle;
  309. EAST = NORTH + 90;
  310. if (EAST >= 180)
  311. {
  312. EAST -= 360;
  313. }
  314. else if (EAST <= -180)
  315. {
  316. EAST += 360;
  317. }
  318. SOUTH = NORTH + 180;
  319. if (SOUTH >= 180)
  320. {
  321. SOUTH -= 360;
  322. }
  323. else if (SOUTH <= -180)
  324. {
  325. SOUTH += 360;
  326. }
  327. WEST = NORTH - 90;
  328. if (WEST >= 180)
  329. {
  330. WEST -= 360;
  331. }
  332. else if (WEST <= 180)
  333. {
  334. WEST += 360;
  335. }
  336. }
  337. public void updateAngles()
  338. {
  339. angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  340. }
  341. public double readAngle(String xyz)
  342. {
  343. Orientation angles;
  344. Acceleration gravity;
  345. angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  346. if(xyz.equals("x")){
  347. return angles.thirdAngle;
  348. }else if(xyz.equals("y")){
  349. return angles.secondAngle;
  350. }else if(xyz.equals("z")){
  351. return angles.firstAngle;
  352. }else{
  353. return 0;
  354. }
  355. }
  356. public void turnDegrees(double target, double power, double timeoutS)
  357. {
  358. //Write code to correct to a target position (NOT FINISHED)
  359. runtime.reset();
  360. updateAngles(); //variable for gyro correction around z axis
  361. target *= -1;//switches clockwise and counterclockwise directions
  362. if(target > 0) {//this fixes a problem where the turn undershoots by 6ish degrees for some reason
  363. target += 6;
  364. }
  365. else if(target < 0){
  366. target -= 6;
  367. }
  368. //target += 6;
  369. double error = angles.firstAngle - target;
  370. double errorAbs;
  371. //wrapping error to have it remain in the field
  372. if (error > 180) error -= 360;
  373. if (error <= -180) error += 360;
  374.  
  375. double powerScaled = power;
  376. do
  377. {
  378. updateAngles();
  379. error = angles.firstAngle - target;
  380. errorAbs = Math.abs(error);
  381.  
  382. if (errorAbs <= 10)
  383. {
  384. powerScaled /= 2;
  385. }
  386. telemetry.addData("error", error);
  387. telemetry.addData("NORTH", NORTH);
  388. telemetry.addData("angle", angles.firstAngle);
  389. telemetry.update();
  390. if(error > 0)
  391. {
  392. robot.fRMotor.setPower(powerScaled);
  393. robot.bRMotor.setPower(powerScaled);
  394. robot.fLMotor.setPower(-powerScaled);
  395. robot.bLMotor.setPower(-powerScaled);
  396. }
  397. else if(error < 0)
  398. {
  399. robot.fRMotor.setPower(-powerScaled);
  400. robot.bRMotor.setPower(-powerScaled);
  401. robot.fLMotor.setPower(powerScaled);
  402. robot.bLMotor.setPower(powerScaled);
  403. }
  404. }
  405. while ((Math.abs(error) > 1.5) && (runtime.seconds() < timeoutS) && opModeIsActive());
  406.  
  407. robot.fRMotor.setPower(0);
  408. robot.bRMotor.setPower(0);
  409. robot.fLMotor.setPower(0);
  410. robot.bLMotor.setPower(0);
  411. }
  412. /*
  413. public void colorSensor() {
  414. float alpha;
  415. float red;
  416. float green;
  417. float blue;
  418. float redDifGreen;
  419. float redDifBlue;
  420. float blueDifGreen;
  421. float blueDifRed;
  422.  
  423. alpha = robot.sensorCol.alpha();
  424. red = robot.sensorCol.red();
  425. green = robot.sensorCol.green();
  426. blue = robot.sensorCol.blue();
  427.  
  428. redDifGreen = red - green;
  429. redDifBlue = red - blue;
  430. blueDifGreen = blue - green;
  431. blueDifRed = blue - red;
  432.  
  433. if (redDifBlue > 100) && (redDifGreen > 100) {
  434.  
  435. }
  436.  
  437. if (blueDifRed > 100) && (blueDifGreen > 100) {
  438.  
  439. }
  440. }
  441. */
  442. public void encoderElevator(double speed,double distance, double timeoutS) {
  443. int newElevatorTarget;
  444.  
  445. // Ensure that the opmode is still active
  446. if (opModeIsActive()) {
  447. // Determine new target position, and pass to motor controller
  448. newElevatorTarget = robot.elevator.getCurrentPosition() + (int)(distance*COUNTS_PER_MOTOR_REV);
  449. robot.elevator.setTargetPosition(newElevatorTarget);
  450.  
  451. // Turn On RUN_TO_POSITION
  452. robot.elevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  453.  
  454. // reset the timeout time and start motion.
  455. runtime.reset();
  456. robot.elevator.setPower(Math.abs(speed));
  457.  
  458. // keep looping while we are still active, and there is time left, and both motors are running.
  459. // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits
  460. // its target position, the motion will stop. This is "safer" in the event that the robot will
  461. // always end the motion as soon as possible.
  462. // However, if you require that BOTH motors have finished their moves before the robot continues
  463. // onto the next step, use (isBusy() || isBusy()) in the loop test.
  464. while (opModeIsActive() &&
  465. (runtime.seconds() < timeoutS) &&
  466. (robot.elevator.isBusy())) {
  467.  
  468. // Display it for the driver.
  469. telemetry.addData("Path1", "Running to %7d", newElevatorTarget);
  470. telemetry.addData("Path2", "Running at %7d",
  471. robot.elevator.getCurrentPosition());
  472. telemetry.update();
  473. }
  474.  
  475. // Stop all motion;
  476. robot.elevator.setPower(0);
  477.  
  478. // Reset encoders
  479. robot.elevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  480. // sleep(250); // optional pause after each move
  481.  
  482. }
  483. }
  484.  
  485. public void dropAmerica()
  486. {
  487. robot.bucket.setPosition(1);
  488.  
  489. }
  490. public void gyroinit()
  491. {
  492.  
  493. // Set up the parameters with which we will use our IMU. Note that integration
  494. // algorithm here just reports accelerations to the logcat log; it doesn't actually
  495. // provide positional information.
  496. BNO055IMU.Parameters Gparameters = new BNO055IMU.Parameters();
  497. Gparameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
  498. Gparameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
  499. Gparameters.calibrationDataFile = "BNO055IMUCalibration.json"; // see the calibration sample opmode
  500. Gparameters.loggingEnabled = true;
  501. Gparameters.loggingTag = "IMU";
  502. Gparameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
  503.  
  504. // Retrieve and initialize the IMU. We expect the IMU to be attached to an I2C port
  505. // on a Core Device Interface Module, configured to be a sensor of type "AdaFruit IMU",
  506. // and named "imu".
  507. imu = hardwareMap.get(BNO055IMU.class, "imu");
  508. imu.initialize(Gparameters);
  509.  
  510. updateAngles();
  511. NORTH = angles.firstAngle;
  512. }
  513. }
Advertisement
Add Comment
Please, Sign In to add comment
Public Pastes
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!