Advertisement
Guest User

Untitled

a guest
Feb 17th, 2019
238
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 20.08 KB | None | 0 0
  1. package org.firstinspires.ftc.teamcode;
  2.  
  3. import com.qualcomm.hardware.bosch.BNO055IMU;
  4. import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
  5. import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
  6. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  7. import com.qualcomm.robotcore.hardware.ColorSensor;
  8. import com.qualcomm.robotcore.hardware.DcMotor;
  9. import com.qualcomm.robotcore.hardware.DcMotorSimple;
  10. import com.qualcomm.robotcore.hardware.DistanceSensor;
  11. import com.qualcomm.robotcore.hardware.Servo;
  12. import com.qualcomm.robotcore.util.ElapsedTime;
  13.  
  14. import org.firstinspires.ftc.robotcore.external.ClassFactory;
  15. import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
  16. import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
  17. import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
  18. import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
  19. import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
  20. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
  21. import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
  22. import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
  23.  
  24. import java.util.List;
  25.  
  26. @Autonomous(name="Autonoma_Bila_Crater", group="Autonoma_Final")
  27.  
  28. public class Autonoma_Bila_Crater extends LinearOpMode
  29. {
  30. //Timp
  31. private ElapsedTime runtime = new ElapsedTime();
  32.  
  33. //Motoare
  34.  
  35. DcMotor motorDF;
  36. DcMotor motorDS;
  37. DcMotor motorSF;
  38. DcMotor motorSS;
  39.  
  40. DcMotor motorTija;
  41. DcMotor motorExten_fata;
  42. DcMotor motorExten_spate;
  43. DcMotor motorSwitch;
  44.  
  45. //Senzori
  46.  
  47. BNO055IMU imu;
  48. DistanceSensor senzorTija;
  49. DistanceSensor senzorFata;
  50. ColorSensor senzorCuloare;
  51.  
  52. //Servo
  53.  
  54. Servo servo_colect;
  55. Servo servo_lander;
  56.  
  57. //Variabile pentru detectare
  58.  
  59. private static final String TFOD_MODEL_ASSET = "RoverRuckus.tflite";
  60. private static final String LABEL_GOLD_MINERAL = "Gold Mineral";
  61. private static final String LABEL_SILVER_MINERAL = "Silver Mineral";
  62. private static final String VUFORIA_KEY = "Ad94F5T/////AAABmXHK2XiLFk+MoiPfQGybpl8fbSSDPMTEpMn4lwDMSxox2HG3QIgFb3MFGa/wO+tOLca7Tw/dvKNqD2IaQMLInqAnRPZPozTL/NE7fvbpOviIVdjNcRP3bsFzS4umkgbgtTdMahlIRW+KPSbFWnSZZ1diFpfqhIPLdMiUGBDNCU/Cizx7E1y8RlwQYH7n5nPfYskKSomRW4ZpFxTRkrDgxNbY6g+6xF/QAPEXj72MWYqjDfHi6UMaP8pfXPYqwotNlf3BDZbHiHLEBLY93jsr4Y/aXUo2bt6qWZEk/kvxiCoFTRAJFXPJwbFqWvwO+ajBbabdbTCrdd/IwtXu4CjxuHJB5veBRBIIwcQlU7+PocgD";
  63. private VuforiaLocalizer vuforia;
  64. private TFObjectDetector tfod;
  65.  
  66. @Override
  67. public void runOpMode()
  68. {
  69. initMotoare();
  70. initSenzori();
  71.  
  72. detect();
  73.  
  74. waitForStart();
  75. if (opModeIsActive())
  76. {
  77. coborare(17200, 8.6, 8);
  78. Movement_Lateral_Timp(0.6, 0.7);
  79. Movement_Fata_Culoare(0.4, 1.5);
  80.  
  81. angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  82. unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
  83.  
  84. sleep(100);
  85. Punctare();
  86.  
  87. sleep(100);
  88. gyroTurn_left(-70 ,-0.3, -0.8);
  89.  
  90. sleep(100);
  91. Miscare_Timp(0.8, 1);
  92.  
  93. sleep(100);
  94. gyroTurn_left(-35, -0.8, -0.8 );
  95.  
  96. sleep(100);
  97. Movement_Lateral_Timp(-0.8, 2);
  98.  
  99. sleep(100);
  100. Miscare_Distanta(0.8, 15, 2);
  101.  
  102. sleep(100);
  103. gyroTurn_left(-170, -0.8, -0.8);
  104.  
  105. sleep(100);
  106. Miscare_Distanta(0.8, 30, 4);
  107. }
  108. }
  109.  
  110. public void ExtindereBrat_Team_Marker()
  111. {
  112.  
  113. }
  114.  
  115. public void ExtindereBrat_Sampling()
  116. {
  117.  
  118. }
  119.  
  120. int a;
  121.  
  122. int slide_fata = 0;
  123.  
  124. public void Miscare_Slide(double viteza, int tickuri, double timeout)
  125. {
  126. /*
  127. Daca e inchis la apelare se va deschide si reciproc
  128. */
  129. runtime.reset();
  130.  
  131. if(slide_fata == 0)
  132. {
  133. motor.Exten_fata.setPower(viteza);
  134. while(motor.Exten_fata.getCurrentPosition() <= tickuri && runtime.seconds <= timeout);
  135. motor.Exten_fata.setPower(0);
  136. slide_fata = 1;
  137. {
  138. else
  139. {
  140. motor.Exten_fata.setPower(- viteza);
  141. while(motor.Exten_fata.getCurrentPosition() >= 10 && runtime.seconds <= timeout);
  142. motor.Exten_fata.setPower(0);
  143. slide_fata = 0;
  144. }
  145. }
  146.  
  147. int sistem_colectare = -1;
  148.  
  149. public void Coborare_Sistem_Colectare(double viteza, int tickuri, double timeout)
  150. {
  151. runtime.reset();
  152. motorSwitch.setPower(viteza);
  153. while(motor.Swtich.getCurrentPosition() <= tickuri && runtime.seconds <=timeout);
  154. motorSwitch.setPower(0);
  155. sistem_colectare = 1;
  156. }
  157.  
  158. public void Miscare_Sistem colectare(double viteza, int tickuri_coborare, double timeout)
  159. {
  160. runtime.reset();
  161.  
  162. if(sistem_colectare == 0)
  163. {
  164. motorSwitch.setPower(viteza);
  165. while(motorSwitch.getCurrentPosition() <= tickuri && runtime.seconds <=timeout);
  166. motorSwitch.setPower(0);
  167. sistem_colectare = 1;
  168. }
  169. else if(sistem_colectare == 1)
  170. {
  171. motorSwitch.setPower(- viteza)
  172. while(motorSwitch.getCurrentPosition() >= 0 && runtime.seconds <= timeout);
  173. motorSwitch.setPower(0);
  174. sistem_colectare = 0;
  175. }
  176. }
  177.  
  178.  
  179. double distantaIntitiala;
  180.  
  181. public void Miscare_Timp(double viteza, double timeout)
  182. {
  183. runtime.reset();
  184.  
  185. motorDS.setPower(viteza);
  186. motorDF.setPower(viteza);
  187. motorSS.setPower(viteza);
  188. motorSF.setPower(viteza);
  189.  
  190. while (runtime.seconds() <= timeout);
  191.  
  192. motorDS.setPower(0);
  193. motorDF.setPower(0);
  194. motorSS.setPower(0);
  195. motorSF.setPower(0);
  196. }
  197.  
  198. public void Miscare_Distanta(double distanta, double viteza, double timeout)
  199. {
  200. runtime.reset();
  201.  
  202. distantaIntitiala = senzorFata.getDistance(DistanceUnit.CM);
  203.  
  204. motorDS.setPower(viteza);
  205. motorDF.setPower(viteza);
  206. motorSS.setPower(viteza);
  207. motorSF.setPower(viteza);
  208.  
  209. while (senzorFata.getDistance(DistanceUnit.CM) >= distantaIntitiala - distanta && runtime.seconds() <= timeout )
  210. {
  211. telemetry.addData("DISTANTA", senzorFata.getDistance(DistanceUnit.CM));
  212. telemetry.addData("MAXIM", distantaIntitiala - distanta);
  213. telemetry.update();
  214. }
  215.  
  216. motorDS.setPower(0);
  217. motorDF.setPower(0);
  218. motorSS.setPower(0);
  219. motorSF.setPower(0);
  220. }
  221.  
  222. Orientation angles;
  223. double unghiCurent, unghiFinal;
  224.  
  225. public void gyroTurn_right(double angle, double viteza_left, double viteza_right) // unghi si viteza pozitive
  226. {
  227. runtime.reset();
  228. angle = -angle;
  229. if (unghiCurent + angle < -180)
  230. {
  231. unghiFinal = 360 + (unghiCurent + angle);
  232.  
  233. motorDF.setPower(-viteza_right);
  234. motorDS.setPower(-viteza_right);
  235. motorSF.setPower(viteza_left);
  236. motorSS.setPower(viteza_left);
  237.  
  238. angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  239. unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
  240. while((unghiCurent < 0 || unghiCurent > unghiFinal) && runtime.seconds() < 5)
  241. {
  242. angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  243. unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
  244. telemetry.addData("Unghi_final", unghiFinal);
  245. telemetry.addData("Curent", unghiCurent);
  246. telemetry.update();
  247. }
  248.  
  249. motorDF.setPower(0);
  250. motorDS.setPower(0);
  251. motorSF.setPower(0);
  252. motorSS.setPower(0);
  253. telemetry.update();
  254. }
  255. else
  256. {
  257. unghiFinal = unghiCurent + angle;
  258.  
  259. motorDF.setPower(-viteza_right);
  260. motorDS.setPower(-viteza_right);
  261. motorSF.setPower(viteza_left);
  262. motorSS.setPower(viteza_left);
  263.  
  264. angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  265. unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
  266.  
  267. while(unghiFinal < unghiCurent && runtime.seconds() < 5)
  268. {
  269. angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  270. unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
  271. telemetry.addData("Unghi_final", unghiFinal);
  272. telemetry.addData("Curent", unghiCurent);
  273. telemetry.update();
  274. }
  275.  
  276. motorDF.setPower(0);
  277. motorDS.setPower(0);
  278. motorSF.setPower(0);
  279. motorSS.setPower(0);
  280. telemetry.update();
  281. }
  282. unghiCurent = unghiFinal;
  283. }
  284.  
  285. public void gyroTurn_left(double angle, double viteza_left, double viteza_right)
  286. {
  287. runtime.reset();
  288.  
  289. angle = -angle;
  290. if (unghiCurent + angle > 180)
  291. {
  292. unghiFinal = -360 + (unghiCurent + angle);
  293.  
  294. motorDF.setPower(-viteza_right);
  295. motorDS.setPower(-viteza_right);
  296. motorSF.setPower(viteza_left);
  297. motorSS.setPower(viteza_left);
  298.  
  299. angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  300. unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
  301. while((unghiCurent > 0 || unghiCurent < unghiFinal) && runtime.seconds() < 5)
  302. {
  303. angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  304. unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
  305. telemetry.addData("Unghi_final", unghiFinal);
  306. telemetry.addData("Curent", unghiCurent);
  307. telemetry.update();
  308. }
  309.  
  310. motorDF.setPower(0);
  311. motorDS.setPower(0);
  312. motorSF.setPower(0);
  313. motorSS.setPower(0);
  314. telemetry.update();
  315. }
  316. else
  317. {
  318. unghiFinal = unghiCurent + angle;
  319.  
  320. motorDF.setPower(-viteza_right);
  321. motorDS.setPower(-viteza_right);
  322. motorSF.setPower(viteza_left);
  323. motorSS.setPower(viteza_left);
  324.  
  325. angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  326. unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
  327.  
  328. while(unghiFinal > unghiCurent && runtime.seconds() < 5)
  329. {
  330. angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  331. unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
  332. telemetry.addData("Unghi_final", unghiFinal);
  333. telemetry.addData("Curent", unghiCurent);
  334. telemetry.update();
  335. }
  336.  
  337. motorDF.setPower(0);
  338. motorDS.setPower(0);
  339. motorSF.setPower(0);
  340. motorSS.setPower(0);
  341. telemetry.update();
  342. }
  343. unghiCurent = unghiFinal;
  344. }
  345.  
  346. double formatAngle(AngleUnit angleUnit, double angle) {return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));}
  347. double formatDegrees(double degrees){return AngleUnit.DEGREES.normalize(degrees); }
  348.  
  349. int pozitieCurenta;
  350. double distantaCurenta;
  351.  
  352. public void Movement_Lateral_Timp(double viteza, double timeout)
  353. {
  354. runtime.reset();
  355.  
  356. motorSF.setPower(-viteza);
  357. motorDF.setPower( viteza);
  358. motorDS.setPower(-viteza);
  359. motorSS.setPower( viteza);
  360.  
  361. while (runtime.seconds() <= timeout);
  362.  
  363. motorSF.setPower(0);
  364. motorDF.setPower(0);
  365. motorDS.setPower(0);
  366. motorSS.setPower(0);
  367. }
  368.  
  369. int rosu, verde, albastru;
  370.  
  371. public void Movement_Fata_Culoare(double viteza, double timeout)
  372. {
  373. runtime.reset();
  374.  
  375. telemetry.addData("Red ", senzorCuloare.red());
  376. telemetry.addData("Green", senzorCuloare.green());
  377. telemetry.addData("Blue ", senzorCuloare.blue());
  378.  
  379. rosu = senzorCuloare.red();
  380. verde = senzorCuloare.green();
  381. albastru = senzorCuloare.blue();
  382.  
  383. while (senzorCuloare.red() <= rosu + 10 && senzorCuloare.blue() <= albastru + 10 && runtime.seconds() <= timeout)
  384. {
  385. motorSF.setPower( viteza);
  386. motorDF.setPower( viteza);
  387. motorDS.setPower( viteza);
  388. motorSS.setPower( viteza);
  389.  
  390. telemetry.addData("Red ", senzorCuloare.red());
  391. telemetry.addData("Green", senzorCuloare.green());
  392. telemetry.addData("Blue ", senzorCuloare.blue());
  393. telemetry.update();
  394. }
  395.  
  396. motorSF.setPower(0);
  397. motorDF.setPower(0);
  398. motorDS.setPower(0);
  399. motorSS.setPower(0);
  400.  
  401. }
  402.  
  403.  
  404. public void coborare(int tickuri_encoder,double distanta, double timeout )
  405. {
  406. runtime.reset();
  407.  
  408. pozitieCurenta = motorTija.getCurrentPosition();
  409. distantaCurenta = senzorTija.getDistance(DistanceUnit.CM);
  410. telemetry.addData("Distanta", distantaCurenta);
  411. telemetry.addData("Encoder", pozitieCurenta);
  412.  
  413. motorTija.setPower(1);
  414.  
  415. while (runtime.seconds() <= timeout && (pozitieCurenta <= tickuri_encoder || distantaCurenta >= distanta + 0.1))
  416. {
  417. pozitieCurenta = motorTija.getCurrentPosition();
  418. distantaCurenta = senzorTija.getDistance(DistanceUnit.CM);
  419. telemetry.addData("Distanta", distantaCurenta);
  420. telemetry.addData("Encoder", pozitieCurenta);
  421. telemetry.update();
  422. }
  423.  
  424. motorTija.setPower(0);
  425. }
  426.  
  427. public void Punctare()
  428. {
  429. if (poz == 2)
  430. {
  431. gyroTurn_right(10, 0.4, 0.4);
  432. //
  433. sleep(100);
  434. gyroTurn_left(-10, -0.4, -0.4);
  435. }
  436. else if (poz == 1)
  437. {
  438. gyroTurn_right(40, 0.4, 0.4);
  439. //
  440. sleep(100);
  441. gyroTurn_left(-40, -0.4, -0.4);
  442. }
  443. else
  444. {
  445. gyroTurn_left(-25, -0.4, -0.4);
  446. //
  447. sleep(100);
  448. gyroTurn_right(10, 0.4, 0.4);
  449. }
  450.  
  451. }
  452.  
  453. int poz = 0;
  454.  
  455. public void detect ()
  456. {
  457.  
  458. initVuforia();
  459.  
  460. if (ClassFactory.getInstance().canCreateTFObjectDetector()) {
  461. initTfod();
  462. } else {
  463. telemetry.addData("Sorry!", "This device is not compatible with TFOD");
  464. }
  465.  
  466. if (tfod != null) {
  467. tfod.activate();
  468. }
  469.  
  470. while(!opModeIsActive())
  471. {
  472. if (tfod != null)
  473. {
  474. List<Recognition> updatedRecognitions = tfod.getUpdatedRecognitions();
  475. if (updatedRecognitions != null)
  476. {
  477. telemetry.addData("# Object Detected", updatedRecognitions.size());
  478. if (updatedRecognitions.size() >= 1) {
  479. int goldMineralX = -1;
  480. for (Recognition recognition : updatedRecognitions)
  481. {
  482. if (recognition.getLabel().equals(LABEL_GOLD_MINERAL))
  483. {
  484. goldMineralX = (int) recognition.getTop();
  485. }
  486. }
  487. telemetry.addData("GOLD", goldMineralX);
  488. if(goldMineralX != -1)
  489. {
  490. if (goldMineralX <= 300)
  491. {
  492. poz = 1;
  493. } else if (goldMineralX <= 500)
  494. {
  495. poz = 2;
  496. } else
  497. {
  498. poz = 3;
  499. }
  500. }
  501. }
  502. telemetry.update();
  503. }
  504. }
  505. }
  506. tfod.shutdown();
  507. }
  508.  
  509. private void initVuforia() {
  510.  
  511. VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
  512.  
  513. parameters.vuforiaLicenseKey = VUFORIA_KEY;
  514. parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
  515.  
  516. vuforia = ClassFactory.getInstance().createVuforia(parameters);
  517. }
  518.  
  519. private void initTfod() {
  520. int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
  521. "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
  522. TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
  523. tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
  524. tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_GOLD_MINERAL, LABEL_SILVER_MINERAL);
  525. }
  526.  
  527. public void initMotoare()
  528. {
  529. motorTija = hardwareMap.get(DcMotor.class, "motorTija");
  530. motorTija.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  531. motorTija.setDirection(DcMotorSimple.Direction.REVERSE);
  532. motorTija.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  533.  
  534. motorExten_fata = hardwareMap.get(DcMotor.class, "motorExten_fata");
  535. motorExten_fata.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  536. motorExten_fata.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  537.  
  538. motorExten_spate = hardwareMap.get(DcMotor.class, "motorExte_spate");
  539. motorExten_spate.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  540. motorExten_spate.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  541.  
  542. motorSwitch = hardwareMap.get(DcMotor.class, "motorSwitch");
  543. motorSwitch.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  544. motorSwitch.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  545.  
  546. motorDS = hardwareMap.get(DcMotor.class, "motorDS");
  547. motorDS.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  548.  
  549. motorSS = hardwareMap.get(DcMotor.class, "motorSS");
  550. motorSS.setDirection(DcMotor.Direction.REVERSE);
  551. motorSS.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  552.  
  553. motorDF = hardwareMap.get(DcMotor.class, "motorDF");
  554. motorSS.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  555.  
  556. motorSF = hardwareMap.get(DcMotor.class, "motorSF");
  557. motorSF.setDirection(DcMotor.Direction.REVERSE);
  558. motorSF.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  559.  
  560. servo_colect = hardwareMap.get(Servo.class, "servo_colect");
  561. servo_lander = hardwareMap.get(Servo.class, "servo_lander");
  562. }
  563.  
  564. public void initSenzori()
  565. {
  566. BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
  567. parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
  568. parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
  569. parameters.calibrationDataFile = "BNO055IMUCalibration.json";
  570. parameters.loggingEnabled = true;
  571. parameters.loggingTag = "IMU";
  572. parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
  573.  
  574. imu = hardwareMap.get(BNO055IMU.class, "imu");
  575. imu.initialize(parameters);
  576.  
  577. senzorTija = hardwareMap.get(DistanceSensor.class, "senzorTija");
  578. senzorFata = hardwareMap.get(DistanceSensor.class, "senzorFata");
  579. senzorCuloare = hardwareMap.get(ColorSensor.class, "senzorCuloare");
  580. }
  581. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement