API tools faq
paste
willieshi232

Blue Depo Back

willieshi232
Dec 5th, 2018
110
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 18.01 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 = "MecanumAutonBlueDepoBack", group = "Testing")
  23. public class MecanumAutonBlueDepoBack 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 = .5;
  51. static final double TURN_SPEED = .2;
  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. //Knocks out center mineral
  79. encoderDrive(29,"r",10, DRIVE_SPEED);
  80. sleep(200);
  81. //turns/moves to deposit marker
  82. turnDegrees(-133,TURN_SPEED,5.5);
  83.  
  84. //dropAmerica();
  85. //turnDegrees(30,TURN_SPEED, 5);//TODO: FIND OUT WHY THIS TURNS THE WRONG WAY
  86. /*sleep(500);
  87. //drive to crater
  88. encoderDrive(40,"f", 15,.6);*/
  89. while(robot.sensordist.getDistance(DistanceUnit.INCH) > 4.6)
  90. {
  91. telemetry.addData("dist:",(robot.sensordist.getDistance(DistanceUnit.INCH)));
  92. telemetry.update();
  93. robot.fLMotor.setPower(.2);
  94. robot.fRMotor.setPower(-.2);
  95. robot.bLMotor.setPower(-.2);
  96. robot.bRMotor.setPower(.2);
  97.  
  98. }
  99. robot.fLMotor.setPower(0);
  100. robot.fRMotor.setPower(0);
  101. robot.bLMotor.setPower(0);
  102. robot.bRMotor.setPower(0);
  103. sleep(100);
  104. runtime.reset();
  105. //encoderDrive(30,"f", 15,DRIVE_SPEED);
  106. while(runtime.seconds() < 7)
  107. {
  108. telemetry.addData("time", runtime.seconds());
  109. telemetry.addData("dist:",(robot.sensordist.getDistance(DistanceUnit.INCH)));
  110. telemetry.update();
  111. if(robot.sensordist.getDistance(DistanceUnit.INCH) > 4.5)
  112. {
  113.  
  114. telemetry.addData("C1:",(robot.sensordist.getDistance(DistanceUnit.INCH)));
  115. telemetry.update();
  116. //foward
  117. robot.fLMotor.setPower(-.5);
  118. robot.fRMotor.setPower(-.5);
  119. robot.bLMotor.setPower(-.5);
  120. robot.bRMotor.setPower(-.5);
  121. }
  122. else
  123. {
  124. telemetry.addData("C2:",(robot.sensordist.getDistance(DistanceUnit.INCH)));
  125. telemetry.update();
  126. //right?
  127. robot.fRMotor.setPower(.07);
  128. robot.bRMotor.setPower(.07);
  129. robot.fLMotor.setPower(-.07);
  130. robot.bLMotor.setPower(-.07);
  131. }
  132.  
  133. }
  134. robot.fLMotor.setPower(0);
  135. robot.fRMotor.setPower(0);
  136. robot.bLMotor.setPower(0);
  137. robot.bRMotor.setPower(0);
  138. sleep(100);
  139.  
  140.  
  141.  
  142.  
  143.  
  144. }
  145. public void stopAndReset() {
  146. robot.fLMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  147. robot.fLMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  148. robot.bLMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  149. robot.bLMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  150. robot.fRMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  151. robot.fRMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  152. robot.bRMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  153. robot.bRMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  154. robot.elevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  155. robot.elevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  156. }
  157.  
  158. public void encoderDrive(double inches, String direction, double timeoutS, double Speed)
  159. {
  160.  
  161. int TargetFL = 0;
  162. int TargetFR = 0;
  163. int TargetBL = 0;
  164. int TargetBR = 0;
  165.  
  166.  
  167. String heading = direction;
  168.  
  169. // Ensure that the opmode is still active
  170. if (opModeIsActive()) {
  171. if(heading == "f")
  172. {
  173. TargetFL = robot.fLMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH);
  174. TargetFR = robot.fRMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH);
  175. TargetBL = robot.bLMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH);
  176. TargetBR = robot.bRMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH);
  177.  
  178. }
  179.  
  180. else if(heading == "b")
  181. {
  182. TargetFL = robot.fLMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH);
  183. TargetFR = robot.fRMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH);
  184. TargetBL = robot.bLMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH);
  185. TargetBR = robot.bRMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH);
  186.  
  187.  
  188. }
  189.  
  190. else if(heading == "r")
  191. {
  192. TargetFL = robot.fLMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH);
  193. TargetFR = robot.fRMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH);
  194. TargetBL = robot.bLMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH);
  195. TargetBR = robot.bRMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH); //weird should be +
  196.  
  197.  
  198. }
  199.  
  200. else if(heading == "l")
  201. {
  202. TargetFL = robot.fLMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH);
  203. TargetFR = robot.fRMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH);
  204. TargetBL = robot.bLMotor.getCurrentPosition() - (int)( inches* COUNTS_PER_INCH); // weird should be +
  205. TargetBR = robot.bRMotor.getCurrentPosition() + (int)( inches* COUNTS_PER_INCH);
  206.  
  207. }
  208.  
  209. else
  210. {
  211. telemetry.addData("not a valid direction", heading );
  212. }
  213.  
  214. // Determine new target position, and pass to motor controller
  215.  
  216. robot.fLMotor.setTargetPosition(TargetFL);
  217. robot.fRMotor.setTargetPosition(TargetFR);
  218. robot.bRMotor.setTargetPosition(TargetBR);
  219. robot.bLMotor.setTargetPosition(TargetBL);
  220.  
  221.  
  222. // Turn On RUN_TO_POSITION
  223. robot.fLMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  224. robot.fRMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  225. robot.bRMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  226. robot.bLMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  227.  
  228.  
  229. // reset the timeout time and start motion.
  230. runtime.reset();
  231. robot.fLMotor.setPower(Math.abs(Speed));
  232. robot.fRMotor.setPower(Math.abs(Speed));
  233. robot.bRMotor.setPower(Math.abs(Speed));
  234. robot.bLMotor.setPower(Math.abs(Speed));
  235.  
  236.  
  237. // keep looping while we are still active, and there is time left, and both motors are running.
  238. // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits
  239. // its target position, the motion will stop. This is "safer" in the event that the robot will
  240. // always end the motion as soon as possible.
  241. // However, if you require that BOTH motors have finished their moves before the robot continues
  242. // onto the next step, use (isBusy() || isBusy()) in the loop test.
  243. while (opModeIsActive() &&
  244. (runtime.seconds() < timeoutS) && ((robot.fLMotor.isBusy() && robot.fRMotor.isBusy()) && robot.bLMotor.isBusy() && robot.bRMotor.isBusy()))
  245. {
  246.  
  247. //Display it for the driver.
  248. telemetry.addData("Path1", "Running to %7d :%7d :%7d :%7d", TargetFL, TargetFR, TargetBL, TargetBR);
  249.  
  250. telemetry.addData("Path2", "Running at %7d :%7d :%7d :%7d", robot.fLMotor.getCurrentPosition(), robot.fRMotor.getCurrentPosition(), robot.bLMotor.getCurrentPosition(), robot.bRMotor.getCurrentPosition());
  251. //telemetry.addData("speeds", "Running to %7f :%7f :%7f :%7f", speedfL, speedfR, speedfL, speedbR);
  252. telemetry.update();
  253. }
  254.  
  255. // Stop all motion;
  256. robot.fLMotor.setPower(0);
  257. robot.bLMotor.setPower(0);
  258. robot.fRMotor.setPower(0);
  259. robot.bRMotor.setPower(0);
  260.  
  261. // Turn off RUN_TO_POSITION
  262. robot.bRMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  263. robot.bLMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  264. robot.fRMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  265. robot.fLMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  266. // sleep(250); // optional pause after each move
  267. }
  268. }
  269. public void setCardinalDir()
  270. {
  271. updateAngles();
  272. NORTH = angles.firstAngle;
  273. EAST = NORTH + 90;
  274. if (EAST >= 180)
  275. {
  276. EAST -= 360;
  277. }
  278. else if (EAST <= -180)
  279. {
  280. EAST += 360;
  281. }
  282. SOUTH = NORTH + 180;
  283. if (SOUTH >= 180)
  284. {
  285. SOUTH -= 360;
  286. }
  287. else if (SOUTH <= -180)
  288. {
  289. SOUTH += 360;
  290. }
  291. WEST = NORTH - 90;
  292. if (WEST >= 180)
  293. {
  294. WEST -= 360;
  295. }
  296. else if (WEST <= 180)
  297. {
  298. WEST += 360;
  299. }
  300. }
  301. public void updateAngles()
  302. {
  303. angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  304. }
  305. public double readAngle(String xyz)
  306. {
  307. Orientation angles;
  308. Acceleration gravity;
  309. angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  310. if(xyz.equals("x")){
  311. return angles.thirdAngle;
  312. }else if(xyz.equals("y")){
  313. return angles.secondAngle;
  314. }else if(xyz.equals("z")){
  315. return angles.firstAngle;
  316. }else{
  317. return 0;
  318. }
  319. }
  320. public void turnDegrees(double target, double power, double timeoutS)
  321. {
  322. //Write code to correct to a target position (NOT FINISHED)
  323. runtime.reset();
  324. updateAngles(); //variable for gyro correction around z axis
  325. target *= -1;//switches clockwise and counterclockwise directions
  326. if(target > 0) {//this fixes a problem where the turn undershoots by 6ish degrees for some reason
  327. target += 6;
  328. }
  329. else if(target < 0){
  330. target -= 6;
  331. }
  332. //target += 6;
  333. double error = angles.firstAngle - target;
  334. double errorAbs;
  335. //wrapping error to have it remain in the field
  336. if (error > 180) error -= 360;
  337. if (error <= -180) error += 360;
  338.  
  339. double powerScaled = power;
  340. do
  341. {
  342. updateAngles();
  343. error = angles.firstAngle - target;
  344. errorAbs = Math.abs(error);
  345.  
  346. if (errorAbs <= 10)
  347. {
  348. powerScaled /= 2;
  349. }
  350. telemetry.addData("error", error);
  351. telemetry.addData("NORTH", NORTH);
  352. telemetry.addData("angle", angles.firstAngle);
  353. telemetry.update();
  354. if(error > 0)
  355. {
  356. robot.fRMotor.setPower(powerScaled);
  357. robot.bRMotor.setPower(powerScaled);
  358. robot.fLMotor.setPower(-powerScaled);
  359. robot.bLMotor.setPower(-powerScaled);
  360. }
  361. else if(error < 0)
  362. {
  363. robot.fRMotor.setPower(-powerScaled);
  364. robot.bRMotor.setPower(-powerScaled);
  365. robot.fLMotor.setPower(powerScaled);
  366. robot.bLMotor.setPower(powerScaled);
  367. }
  368. }
  369. while ((Math.abs(error) > 1.5) && (runtime.seconds() < timeoutS) && opModeIsActive());
  370.  
  371. robot.fRMotor.setPower(0);
  372. robot.bRMotor.setPower(0);
  373. robot.fLMotor.setPower(0);
  374. robot.bLMotor.setPower(0);
  375. }
  376. /*
  377. public void colorSensor() {
  378. float alpha;
  379. float red;
  380. float green;
  381. float blue;
  382. float redDifGreen;
  383. float redDifBlue;
  384. float blueDifGreen;
  385. float blueDifRed;
  386.  
  387. alpha = robot.sensorCol.alpha();
  388. red = robot.sensorCol.red();
  389. green = robot.sensorCol.green();
  390. blue = robot.sensorCol.blue();
  391.  
  392. redDifGreen = red - green;
  393. redDifBlue = red - blue;
  394. blueDifGreen = blue - green;
  395. blueDifRed = blue - red;
  396.  
  397. if (redDifBlue > 100) && (redDifGreen > 100) {
  398.  
  399. }
  400.  
  401. if (blueDifRed > 100) && (blueDifGreen > 100) {
  402.  
  403. }
  404. }
  405. */
  406. public void encoderElevator(double speed,double distance, double timeoutS) {
  407. int newElevatorTarget;
  408.  
  409. // Ensure that the opmode is still active
  410. if (opModeIsActive()) {
  411. // Determine new target position, and pass to motor controller
  412. newElevatorTarget = robot.elevator.getCurrentPosition() + (int)(distance*COUNTS_PER_MOTOR_REV);
  413. robot.elevator.setTargetPosition(newElevatorTarget);
  414.  
  415. // Turn On RUN_TO_POSITION
  416. robot.elevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  417.  
  418. // reset the timeout time and start motion.
  419. runtime.reset();
  420. robot.elevator.setPower(Math.abs(speed));
  421.  
  422. // keep looping while we are still active, and there is time left, and both motors are running.
  423. // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits
  424. // its target position, the motion will stop. This is "safer" in the event that the robot will
  425. // always end the motion as soon as possible.
  426. // However, if you require that BOTH motors have finished their moves before the robot continues
  427. // onto the next step, use (isBusy() || isBusy()) in the loop test.
  428. while (opModeIsActive() &&
  429. (runtime.seconds() < timeoutS) &&
  430. (robot.elevator.isBusy())) {
  431.  
  432. // Display it for the driver.
  433. telemetry.addData("Path1", "Running to %7d", newElevatorTarget);
  434. telemetry.addData("Path2", "Running at %7d",
  435. robot.elevator.getCurrentPosition());
  436. telemetry.update();
  437. }
  438.  
  439. // Stop all motion;
  440. robot.elevator.setPower(0);
  441.  
  442. // Reset encoders
  443. robot.elevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  444. // sleep(250); // optional pause after each move
  445.  
  446. }
  447. }
  448.  
  449. public void dropAmerica()
  450. {
  451. robot.bucket.setPosition(1);
  452.  
  453. }
  454. public void gyroinit()
  455. {
  456.  
  457. // Set up the parameters with which we will use our IMU. Note that integration
  458. // algorithm here just reports accelerations to the logcat log; it doesn't actually
  459. // provide positional information.
  460. BNO055IMU.Parameters Gparameters = new BNO055IMU.Parameters();
  461. Gparameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
  462. Gparameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
  463. Gparameters.calibrationDataFile = "BNO055IMUCalibration.json"; // see the calibration sample opmode
  464. Gparameters.loggingEnabled = true;
  465. Gparameters.loggingTag = "IMU";
  466. Gparameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
  467.  
  468. // Retrieve and initialize the IMU. We expect the IMU to be attached to an I2C port
  469. // on a Core Device Interface Module, configured to be a sensor of type "AdaFruit IMU",
  470. // and named "imu".
  471. imu = hardwareMap.get(BNO055IMU.class, "imu");
  472. imu.initialize(Gparameters);
  473.  
  474. updateAngles();
  475. NORTH = angles.firstAngle;
  476. }
  477. }
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!