API tools faq
paste
willieshi232

BlueDepoMain

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