API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Apr 19th, 2018
64
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 34.98 KB | None | 0 0
raw download clone embed print report
  1. package org.firstinspires.ftc.teamcode;
  2.  
  3. import com.kauailabs.navx.ftc.AHRS;
  4. import com.kauailabs.navx.ftc.navXPIDController;
  5. import com.qualcomm.hardware.adafruit.AdafruitI2cColorSensor;
  6. import com.qualcomm.hardware.kauailabs.NavxMicroNavigationSensor;
  7. import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cRangeSensor;
  8. import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
  9. import com.qualcomm.robotcore.eventloop.opmode.Disabled;
  10. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  11. import com.qualcomm.robotcore.eventloop.opmode.OpMode;
  12. import com.qualcomm.robotcore.hardware.ColorSensor;
  13. import com.qualcomm.robotcore.hardware.DcMotor;
  14. import com.qualcomm.robotcore.hardware.Servo;
  15. import com.qualcomm.robotcore.util.ElapsedTime;
  16. import com.qualcomm.robotcore.util.ThreadPool;
  17.  
  18. import org.firstinspires.ftc.robotcore.external.ClassFactory;
  19. import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
  20. import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
  21. import org.firstinspires.ftc.robotcore.external.navigation.RelicRecoveryVuMark;
  22. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
  23. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
  24. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
  25. import org.firstinspires.ftc.robotcore.internal.opmode.OpModeManagerImpl;
  26.  
  27. import java.util.concurrent.TimeUnit;
  28.  
  29. import static java.lang.System.arraycopy;
  30. import static java.lang.System.currentTimeMillis;
  31. import static java.lang.System.in;
  32.  
  33. /**
  34. * Created by (for example John) on 2/14/2018.
  35. */
  36. @Autonomous(name = "AutonomieRosu2Test", group = "AutonomieRosu2")
  37. public class AutonomieRosu2Test extends LinearOpMode {
  38.  
  39. public ModernRoboticsI2cRangeSensor rangeSensor;
  40. private final int NAVX_DIM_I2C_PORT = 0;
  41. private AHRS navx_device;
  42. private navXPIDController yawPIDController;
  43. private ElapsedTime runtime = new ElapsedTime();
  44.  
  45. private final byte NAVX_DEVICE_UPDATE_RATE_HZ = 50;
  46.  
  47. DcMotor motorFrontRight;
  48. DcMotor motorFrontLeft;
  49. DcMotor motorBackRight;
  50. DcMotor motorBackLeft;
  51.  
  52. DcMotor mRid;
  53.  
  54. private final double TARGET_ANGLE_DEGREES = 90.0;
  55. private final double TOLERANCE_DEGREES = 2.0;
  56. private final double MIN_MOTOR_OUTPUT_VALUE = -1.0;
  57. private final double MAX_MOTOR_OUTPUT_VALUE = 1.0;
  58. private final double YAW_PID_P = 0.005;
  59. private final double YAW_PID_I = 0.0;
  60. private final double YAW_PID_D = 0.0;
  61. double stanga = -0.6411;
  62. double dreapta = 0.2988;
  63. double pozitieServoBileJos = 1; // de vazut
  64. double pozitieServoBileSus = 0;
  65. int rotireDreapta = 1;
  66. int rotireStanga = -1;
  67. double vitezaIntoarcere = 0.07;
  68. double bilaStanga = -10;
  69. double bilaDreapta = 10;
  70. double treshHold = 2;
  71. double vitezaMiscare = 0.1;
  72. int raft = 0;
  73. Servo servoBile;
  74. Servo CubSJ;
  75. Servo CubDJ;
  76. Servo CubSS;
  77. Servo CubDS;
  78. double rollInitial;
  79. double altInitial;
  80. double vitezeHolonom[] = new double[4];
  81. double alpha;
  82. double beta = 0;
  83. double zeroRoll = 0;
  84. private boolean calibration_complete = false;
  85. double ServS = 0.26;
  86. double ServD = 0.67;
  87. double ServSLas = 0.4614;
  88. double ServDLas = 0.4785;
  89. boolean terminat;
  90. double distRaft1 = 47;
  91. double distRaft2 = 65;
  92. double distRaft3 = 83;
  93. Servo servoBileSD;
  94. double bilaAlbastra = 35000;
  95. double bilaRosie = 55000;
  96. public static final String TAG = "Vuforia VuMark Sample";
  97. double VitezaIntoarcereMare = 0.23;
  98.  
  99. OpenGLMatrix lastLocation = null;
  100.  
  101. //Servo servo = hardwareMap.servo.get("servo_jewel");
  102. AdafruitI2cColorSensor colorSensor;
  103. ColorSensor colorSensorPrioritar;
  104.  
  105. VuforiaLocalizer vuforia;
  106.  
  107. @Override
  108. public void runOpMode() throws InterruptedException {
  109. terminat = false;
  110. navx_device = AHRS.getInstance(hardwareMap.deviceInterfaceModule.get("dim"),
  111. NAVX_DIM_I2C_PORT,
  112. AHRS.DeviceDataType.kProcessedData,
  113. NAVX_DEVICE_UPDATE_RATE_HZ);
  114.  
  115.  
  116. int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
  117. VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
  118.  
  119. parameters.vuforiaLicenseKey = "AcXkeIj/////AAAAmRV1vqYUzkRcn7rBLlUty9FuNYEzPAn4gEn+mrBl7eKeI3qA1oEWAVrY8uLi+jewlHe7i56Zoza2WB+sEq6MXpjOjQm4MCVCe3CmeVuCQEyVDU9QVGqoO1swzzY6x2k9yRoQgmuIW1BxEwxj4mNeFwPGZEsWJlofpFHnxuKNdO2DzQ0SeNAl+iksEodJBR1NKv22ORk0snNYX1u1b9dzsqZeq/ONXHhwm9KXDqCjDhwCndzm2oMHu7tASBsRTDjVJXnWTqVFsi9QQQqq2IX4Kxybhu/vck51l/f9gPItPE/YoVIL3UqyvKz5YDHpZbqrOYrpFLxtf8NFgNI/Aq5pNwcupiqayd5FM0hQzsWC2IHu";
  120. parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
  121. this.vuforia = ClassFactory.createVuforiaLocalizer(parameters);
  122.  
  123. VuforiaTrackables relicTrackables = this.vuforia.loadTrackablesFromAsset("RelicVuMark");
  124. VuforiaTrackable relicTemplate = relicTrackables.get(0);
  125.  
  126. double position = 0;
  127.  
  128. motorFrontRight = hardwareMap.dcMotor.get("motor front right");
  129. motorFrontLeft = hardwareMap.dcMotor.get("motor front left");
  130. motorBackLeft = hardwareMap.dcMotor.get("motor back left");
  131. motorBackRight = hardwareMap.dcMotor.get("motor back right");
  132. colorSensor = (AdafruitI2cColorSensor) hardwareMap.colorSensor.get("color_sensor");
  133. servoBileSD=hardwareMap.servo.get("servo bile SD");
  134. colorSensorPrioritar = hardwareMap.colorSensor.get("color_sensor_2");
  135. servoBile = hardwareMap.servo.get("servo bile");
  136. CubSJ = hardwareMap.servo.get("servo rid s 0");
  137. CubDJ = hardwareMap.servo.get("servo rid d 0");
  138. CubSS = hardwareMap.servo.get("servo rid s 1");
  139. CubDS = hardwareMap.servo.get("servo rid d 1");
  140. mRid = hardwareMap.dcMotor.get("motor rid");
  141. rangeSensor = (ModernRoboticsI2cRangeSensor) hardwareMap.opticalDistanceSensor.get("sensor_distance");
  142.  
  143. /*CubSJ.setPosition(ServS);
  144. CubDJ.setPosition(ServD);
  145. CubSS.setPosition(ServS);
  146. CubDS.setPosition(ServD);*/
  147.  
  148. /*while (colorSensor.blue()==colorSensor.red() && position>0)
  149. {
  150.  
  151. servoBile.setPosition(position);
  152. position += 0.01;
  153.  
  154. }*/
  155.  
  156. while (!calibration_complete && !Thread.currentThread().isInterrupted()) {
  157. /* navX-Micro Calibration completes automatically ~15 seconds after it is
  158. powered on, as long as the device is still. To handle the case where the
  159. navX-Micro has not been able to calibrate successfully, hold off using
  160. the navX-Micro Yaw value until calibration is complete.
  161. */
  162.  
  163. calibration_complete = !navx_device.isCalibrating();
  164. if (!calibration_complete) {
  165. telemetry.addData("navX-Micro", "Startup Calibration in Progress");
  166. }
  167. if (calibration_complete)
  168. telemetry.addData("navX-Micro", "M-am calibrat");
  169. telemetry.update();
  170. }
  171.  
  172.  
  173. rollInitial = navx_device.getPitch();
  174. telemetry.addData("Roll: ", rollInitial);
  175. telemetry.addLine();
  176. altInitial = navx_device.getAltitude();
  177.  
  178. navx_device.zeroYaw();
  179.  
  180. relicTrackables.activate();
  181.  
  182. RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate);
  183.  
  184.  
  185. while (!opModeIsActive() && !Thread.currentThread().isInterrupted()) {
  186. vuMark = RelicRecoveryVuMark.from(relicTemplate);
  187. switch (vuMark) {
  188.  
  189. case LEFT:
  190. raft = 4;
  191. break;
  192. case CENTER:
  193. raft = 3;
  194. break;
  195. case RIGHT:
  196. raft = 2;
  197. break;
  198. default:
  199. raft = 3;
  200. break;
  201.  
  202.  
  203. }
  204.  
  205. telemetry.addData("Rotatie: ", navx_device.getYaw());
  206. telemetry.addLine();
  207. telemetry.addData("Albastru: ", colorSensor.blue());
  208. telemetry.addLine();
  209. telemetry.addData("Rosu: ", colorSensor.red());
  210. telemetry.addLine();
  211. telemetry.addData("distanta initiala:", rangeSensor.getDistance(DistanceUnit.CM));
  212. //telemetry.addData("Altitudine: ", navx_device.getAltitude());
  213. telemetry.addData("Roll: ", navx_device.getPitch());
  214. telemetry.addLine();
  215. telemetry.addData("VuMark:", vuMark);
  216.  
  217. telemetry.addData("poz servo", CubSJ.getPosition());
  218. telemetry.update();
  219.  
  220. }
  221.  
  222.  
  223. while (opModeIsActive() && !terminat) {
  224.  
  225. zeroRoll = navx_device.getPitch();
  226. servoBile.setPosition(pozitieServoBileJos);
  227. prindeCub(ServS, ServD);
  228.  
  229. TimeUnit.MILLISECONDS.sleep(1000);
  230. telemetry.addData("Rotatie: ", navx_device.getYaw());
  231. telemetry.update();
  232.  
  233.  
  234. servoBile.setPosition(pozitieServoBileJos);
  235.  
  236. TimeUnit.MILLISECONDS.sleep(1000);
  237. telemetry.addData("Rotatie: ", navx_device.getYaw());
  238. telemetry.update();
  239.  
  240.  
  241. if (colorSensorPrioritar.red() > colorSensorPrioritar.blue()) {
  242. josBila(true); // am dat jos bila rosie
  243. servoBile.setPosition(1);
  244. TimeUnit.MILLISECONDS.sleep(500);
  245.  
  246. } else if (colorSensorPrioritar.red() < colorSensorPrioritar.blue()) {
  247. josBila(false);
  248. servoBile.setPosition(1);
  249. TimeUnit.MILLISECONDS.sleep(500);
  250. }
  251.  
  252. else {
  253.  
  254. if (colorSensor.red() > bilaRosie) {
  255.  
  256. josBila(false);
  257. servoBile.setPosition(1);
  258. TimeUnit.MILLISECONDS.sleep(500);
  259.  
  260. }
  261.  
  262. else if(colorSensor.blue() > bilaAlbastra){
  263.  
  264. josBila(true); // am dat jos bila rosie
  265. servoBile.setPosition(1);
  266. TimeUnit.MILLISECONDS.sleep(500);
  267.  
  268. }
  269.  
  270.  
  271. }
  272. servoBile.setPosition(pozitieServoBileSus);
  273. TimeUnit.MILLISECONDS.sleep(500);
  274. /*
  275. // formuleHolonom(navx_device.getYaw(),beta,vitezeHolonom);
  276.  
  277. //da_teJosDePeRampa(-vitezaMiscare+0.015 , zeroRoll, altInitial);
  278. mergiFata(-0.1, 0);
  279. TimeUnit.MILLISECONDS.sleep(1900);
  280. rotire(-90, treshHold, vitezaIntoarcere);
  281. mergiFata(0.1, 0);
  282. TimeUnit.MILLISECONDS.sleep(2000);
  283. oprire();
  284. TimeUnit.MILLISECONDS.sleep(200);
  285. // mergiDreapta(vitezaMiscare);
  286. // TimeUnit.MILLISECONDS.sleep(500);
  287. //du_teLaRaft2(raft,vitezaMiscare-0.01);
  288. pune_teFataDeRaft(42, 'd');
  289. du_teLaRaft(raft, -vitezaMiscare);
  290.  
  291. // mergiFata(-vitezaMiscare,0);
  292. // TimeUnit.MILLISECONDS.sleep(300);
  293. // mergiDreapta(vitezaMiscare);
  294. // TimeUnit.MILLISECONDS.sleep(500);
  295. //mergiFata(-vitezaMiscare, 0);
  296. // TimeUnit.MILLISECONDS.sleep(150);
  297. //rotire(-180, treshHold, vitezaIntoarcere);
  298. rotireSpeciala(-180,treshHold,vitezaIntoarcere);
  299. oprire();
  300. //if(raft == 3) {
  301. // mergiDreapta(vitezaMis
  302. car
  303.  
  304. e);
  305. // TimeUnit.MILLISECONDS.sleep(550);
  306. // }*/
  307. mergiFata(-0.1, 0);
  308. TimeUnit.MILLISECONDS.sleep(1900);
  309. // rotire(0,treshHold,vitezaIntoarcere);
  310. //TimeUnit.MILLISECONDS.sleep(500);
  311. du_teLaRaft2(raft, vitezaMiscare);
  312. // calibrareRampa(-10,treshHold,vitezaIntoarcere);
  313. rotireSpecialaTreptata(-180,treshHold,vitezaIntoarcere);
  314. puneCub(vitezaMiscare);
  315. oprireTot();
  316.  
  317.  
  318. //telemetry.addData("Blue ", colorSensor.blue());
  319. //telemetry.addData("Red ", colorSensor.red());
  320. //telemetry.addLine();
  321. //telemetry.addLine(Double.toString(servo.getPosition()));
  322.  
  323. terminat = true;
  324. }
  325. oprireTot();
  326. }
  327.  
  328.  
  329.  
  330.  
  331. public void rotireSpecialaTreptata(double tinta, double treshHold, double viteza) throws InterruptedException {
  332.  
  333. int incercari = 0;
  334. boolean suntPeTinta = false;
  335. boolean amTrecut;
  336. while (!suntPeTinta && opModeIsActive()) {
  337. double mergiStanga;
  338. double mergiDreapta;
  339. mergiStanga = viteza;
  340. mergiDreapta = -viteza;
  341. amTrecut = false;
  342. while (navx_device.getYaw() >= tinta + treshHold && opModeIsActive() && !amTrecut) { // cat timp sunt in dreapta tintei vreau sa merg in stanga
  343. mergiStanga = actualizeazaViteza(tinta,navx_device.getYaw(),incercari);
  344. mergiDreapta = -actualizeazaViteza(tinta,navx_device.getYaw(),incercari);
  345. telemetry.addData("Rotatie: ", navx_device.getYaw());
  346. telemetry.addLine();
  347. telemetry.addData("incercari:", incercari);
  348. telemetry.addLine();
  349. telemetry.addData("treshhold:", treshHold);
  350. telemetry.addLine();
  351. telemetry.addData("viteza:", viteza);
  352. telemetry.update();
  353. motorFrontLeft.setPower(mergiStanga);
  354. motorFrontRight.setPower(mergiStanga);
  355. motorBackRight.setPower(mergiStanga);
  356. motorBackLeft.setPower(mergiStanga);
  357. if (navx_device.getYaw() >= 0)
  358. amTrecut = true;
  359. }
  360. oprire();
  361. TimeUnit.MILLISECONDS.sleep(500);
  362.  
  363.  
  364. if (navx_device.getYaw() >= 180 - treshHold || navx_device.getYaw() <= tinta + treshHold) { // daca sunt pe o parte sau o alta a tintei si in limitele tresholdului ma opresc
  365. suntPeTinta = true;
  366.  
  367. }
  368. amTrecut = false;
  369. if (!suntPeTinta) { // daca nu m-am pozitionat bine, incerc sa merg in dreapta. Robotul nu se poate opri decat in limitele treshHoldului sau in partea stanga
  370. while (navx_device.getYaw() <= -tinta - treshHold && opModeIsActive() && !amTrecut) { // cat timp sunt in partea stanga a tintei vreau sa merg in dreapta
  371. mergiStanga = actualizeazaViteza(tinta,navx_device.getYaw(),incercari);
  372. mergiDreapta = -actualizeazaViteza(tinta,navx_device.getYaw(),incercari);
  373. telemetry.addData("Rotatie: ", navx_device.getYaw());
  374. telemetry.addLine();
  375. telemetry.addData("incercari:", incercari);
  376. telemetry.addLine();
  377. telemetry.addData("treshhold:", treshHold);
  378. telemetry.addLine();
  379. telemetry.addData("viteza:", viteza);
  380. telemetry.update();
  381.  
  382. motorFrontLeft.setPower(mergiDreapta);
  383. motorFrontRight.setPower(mergiDreapta);
  384. motorBackRight.setPower(mergiDreapta);
  385. motorBackLeft.setPower(mergiDreapta);
  386. if (navx_device.getYaw() >= -180 && navx_device.getYaw() <= -160)
  387. amTrecut = true;
  388. }
  389. oprire();
  390. TimeUnit.MILLISECONDS.sleep(500);
  391. }
  392.  
  393. if (navx_device.getYaw() >= 180 - treshHold || navx_device.getYaw() <= tinta + treshHold) // daca sunt pe o parte sau o alta a tintei si in limitele tresholdului ma opresc
  394. suntPeTinta = true;
  395. amTrecut = false;
  396.  
  397. incercari++;
  398. /*if (incercari % 2 == 0 && incercari != 0) { // daca dupa doua incercari nu a reusit, il ajut putin . O incercare inseamna o rotire spre stanga si o rotire spre dreapta sau viceversa
  399. // treshHold += 0.5;
  400. viteza -= 0.01;
  401. }
  402. */
  403.  
  404. if (suntPeTinta)
  405. oprire();
  406.  
  407. }
  408.  
  409. }
  410.  
  411. public double actualizeazaViteza(double tinta, double unghiActual,int incercari){
  412. double diferenta = Math.abs(Math.abs(tinta) - Math.abs(unghiActual));
  413. double scadere = 0;
  414. if(incercari % 2 == 0) {
  415. scadere = incercari / 2 * 0.01;
  416. }
  417. else
  418. scadere = (incercari-1) / 2 * 0.01;
  419.  
  420. if(diferenta <= 30){
  421.  
  422. return vitezaIntoarcere - scadere;
  423. }
  424. else
  425. return VitezaIntoarcereMare;
  426. }
  427.  
  428. public void calibrareRampa(double tinta, double treshHold, double viteza) {
  429.  
  430.  
  431. double mergiStanga = viteza;
  432. double mergiDreapta = -viteza;
  433.  
  434. while (navx_device.getYaw() >= tinta + treshHold && opModeIsActive()) { // cat timp sunt in dreapta tintei vreau sa merg in stanga
  435.  
  436.  
  437. telemetry.addData("Rotatie: ", navx_device.getYaw());
  438. telemetry.addLine();
  439. telemetry.addData("treshhold:", treshHold);
  440. telemetry.addLine();
  441. telemetry.addData("viteza:", viteza);
  442. telemetry.update();
  443.  
  444.  
  445. motorFrontLeft.setPower(mergiStanga);
  446. motorFrontRight.setPower(mergiStanga);
  447. motorBackRight.setPower(mergiStanga);
  448. motorBackLeft.setPower(mergiStanga);
  449. }
  450. oprire();
  451.  
  452.  
  453. while (navx_device.getYaw() <= tinta - treshHold && opModeIsActive()) { // cat timp sunt in partea stanga a tintei vreau sa merg in dreapta
  454.  
  455. telemetry.addData("Rotatie: ", navx_device.getYaw());
  456. telemetry.addLine();
  457. telemetry.addData("treshhold:", treshHold);
  458. telemetry.addLine();
  459. telemetry.addData("viteza:", viteza);
  460. telemetry.update();
  461.  
  462. motorFrontLeft.setPower(mergiDreapta);
  463. motorFrontRight.setPower(mergiDreapta);
  464. motorBackRight.setPower(mergiDreapta);
  465. motorBackLeft.setPower(mergiDreapta);
  466. }
  467. oprire();
  468.  
  469. }
  470.  
  471.  
  472. public void formuleHolonom(double alpha, double beta, double[] vitezeHolonom) {
  473.  
  474. double xM, yM, scale = 0.1;
  475. xM = (Math.cos(alpha) + 1) / 2;
  476. yM = Math.sin(alpha) / 2;
  477. vitezeHolonom[0] = scale * (-xM - yM);
  478. vitezeHolonom[1] = scale * (-xM + yM);
  479. vitezeHolonom[2] = scale * (xM + yM);
  480. vitezeHolonom[3] = scale * (+xM - yM);
  481.  
  482. }
  483.  
  484. public void urcaServo(Servo servo) {
  485.  
  486. servo.setPosition(1);
  487.  
  488. }
  489.  
  490.  
  491. public void rotire(double tinta, double treshHold, double viteza) throws InterruptedException {
  492. int incercari = 0;
  493. boolean suntPeTinta = false;
  494. while (!suntPeTinta && opModeIsActive()) {
  495. double mergiStanga = viteza;
  496. double mergiDreapta = -viteza;
  497.  
  498. while (navx_device.getYaw() >= tinta + treshHold && opModeIsActive()) { // cat timp sunt in dreapta tintei vreau sa merg in stanga
  499. telemetry.addData("Rotatie: ", navx_device.getYaw());
  500. telemetry.addLine();
  501. telemetry.addData("incercari:", incercari);
  502. telemetry.addLine();
  503. telemetry.addData("treshhold:", treshHold);
  504. telemetry.addLine();
  505. telemetry.addData("viteza:", viteza);
  506. telemetry.update();
  507. motorFrontLeft.setPower(mergiStanga);
  508. motorFrontRight.setPower(mergiStanga);
  509. motorBackRight.setPower(mergiStanga);
  510. motorBackLeft.setPower(mergiStanga);
  511. }
  512. oprire();
  513. TimeUnit.MILLISECONDS.sleep(500);
  514.  
  515.  
  516. if (navx_device.getYaw() >= tinta - treshHold && navx_device.getYaw() <= tinta + treshHold) // daca sunt pe o parte sau o alta a tintei si in limitele tresholdului ma opresc
  517. suntPeTinta = true;
  518.  
  519. if (!suntPeTinta) { // daca nu m-am pozitionat bine, incerc sa merg in dreapta. Robotul nu se poate opri decat in limitele treshHoldului sau in partea stanga
  520. while (navx_device.getYaw() <= tinta - treshHold && opModeIsActive()) { // cat timp sunt in partea stanga a tintei vreau sa merg in dreapta
  521.  
  522. telemetry.addData("Rotatie: ", navx_device.getYaw());
  523. telemetry.addLine();
  524. telemetry.addData("incercari:", incercari);
  525. telemetry.addLine();
  526. telemetry.addData("treshhold:", treshHold);
  527. telemetry.addLine();
  528. telemetry.addData("viteza:", viteza);
  529. telemetry.update();
  530.  
  531. motorFrontLeft.setPower(mergiDreapta);
  532. motorFrontRight.setPower(mergiDreapta);
  533. motorBackRight.setPower(mergiDreapta);
  534. motorBackLeft.setPower(mergiDreapta);
  535. }
  536. oprire();
  537. TimeUnit.MILLISECONDS.sleep(500);
  538. }
  539.  
  540. if (navx_device.getYaw() >= tinta - treshHold && navx_device.getYaw() <= tinta + treshHold) // daca sunt pe o parte sau o alta a tintei si in limitele tresholdului ma opresc
  541. suntPeTinta = true;
  542.  
  543.  
  544. incercari++;
  545. if (incercari % 2 == 0 && incercari != 0) { // daca dupa doua incercari nu a reusit, il ajut putin . O incercare inseamna o rotire spre stanga si o rotire spre dreapta sau viceversa
  546. // treshHold += 0.5;
  547. viteza -= 0.01;
  548. }
  549.  
  550. if (suntPeTinta)
  551. oprire();
  552.  
  553. }
  554.  
  555.  
  556. }
  557.  
  558. public void rotireSpeciala(double tinta, double treshHold, double viteza) throws InterruptedException {
  559.  
  560. int incercari = 0;
  561. boolean suntPeTinta = false;
  562. boolean amTrecut;
  563. while (!suntPeTinta && opModeIsActive()) {
  564. double mergiStanga = viteza;
  565. double mergiDreapta = -viteza;
  566. amTrecut = false;
  567. while (navx_device.getYaw() >= tinta + treshHold && opModeIsActive() && !amTrecut) { // cat timp sunt in dreapta tintei vreau sa merg in stanga
  568. telemetry.addData("Rotatie: ", navx_device.getYaw());
  569. telemetry.addLine();
  570. telemetry.addData("incercari:", incercari);
  571. telemetry.addLine();
  572. telemetry.addData("treshhold:", treshHold);
  573. telemetry.addLine();
  574. telemetry.addData("viteza:", viteza);
  575. telemetry.update();
  576. motorFrontLeft.setPower(mergiStanga);
  577. motorFrontRight.setPower(mergiStanga);
  578. motorBackRight.setPower(mergiStanga);
  579. motorBackLeft.setPower(mergiStanga);
  580. if (navx_device.getYaw() >= 0)
  581. amTrecut = true;
  582. }
  583. oprire();
  584. TimeUnit.MILLISECONDS.sleep(500);
  585.  
  586.  
  587. if (navx_device.getYaw() >= 180 - treshHold || navx_device.getYaw() <= tinta + treshHold) { // daca sunt pe o parte sau o alta a tintei si in limitele tresholdului ma opresc
  588. suntPeTinta = true;
  589.  
  590. }
  591. amTrecut = false;
  592. if (!suntPeTinta) { // daca nu m-am pozitionat bine, incerc sa merg in dreapta. Robotul nu se poate opri decat in limitele treshHoldului sau in partea stanga
  593. while (navx_device.getYaw() <= -tinta - treshHold && opModeIsActive() && !amTrecut) { // cat timp sunt in partea stanga a tintei vreau sa merg in dreapta
  594.  
  595. telemetry.addData("Rotatie: ", navx_device.getYaw());
  596. telemetry.addLine();
  597. telemetry.addData("incercari:", incercari);
  598. telemetry.addLine();
  599. telemetry.addData("treshhold:", treshHold);
  600. telemetry.addLine();
  601. telemetry.addData("viteza:", viteza);
  602. telemetry.update();
  603.  
  604. motorFrontLeft.setPower(mergiDreapta);
  605. motorFrontRight.setPower(mergiDreapta);
  606. motorBackRight.setPower(mergiDreapta);
  607. motorBackLeft.setPower(mergiDreapta);
  608. if (navx_device.getYaw() >= -180 && navx_device.getYaw() <= -160)
  609. amTrecut = true;
  610. }
  611. oprire();
  612. TimeUnit.MILLISECONDS.sleep(500);
  613. }
  614.  
  615. if (navx_device.getYaw() >= 180 - treshHold || navx_device.getYaw() <= tinta + treshHold) // daca sunt pe o parte sau o alta a tintei si in limitele tresholdului ma opresc
  616. suntPeTinta = true;
  617. amTrecut = false;
  618.  
  619. incercari++;
  620. if (incercari % 2 == 0 && incercari != 0) { // daca dupa doua incercari nu a reusit, il ajut putin . O incercare inseamna o rotire spre stanga si o rotire spre dreapta sau viceversa
  621. // treshHold += 0.5;
  622. viteza -= 0.01;
  623. }
  624.  
  625. if (suntPeTinta)
  626. oprire();
  627.  
  628. }
  629.  
  630. }
  631.  
  632. public void josBila(boolean stanga) throws InterruptedException {
  633.  
  634. if (stanga) {
  635.  
  636. servoBileSD.setPosition(1);
  637.  
  638. }
  639. else {
  640.  
  641. servoBileSD.setPosition(0);
  642.  
  643. }
  644.  
  645. TimeUnit.MILLISECONDS.sleep(500);
  646. servoBileSD.setPosition(0.5);
  647.  
  648. }
  649.  
  650. public void puneCub(double viteza) throws InterruptedException {
  651.  
  652. mergiFata(viteza, 0);
  653. TimeUnit.MILLISECONDS.sleep(1300);
  654.  
  655. oprire();
  656.  
  657. lasaCub(ServSLas, ServDLas);
  658. TimeUnit.MILLISECONDS.sleep(400);
  659.  
  660. mergiFata(-viteza, 0);
  661. TimeUnit.MILLISECONDS.sleep(500);
  662.  
  663. mRid.setPower(-1);
  664. TimeUnit.MILLISECONDS.sleep(500);
  665.  
  666. mRid.setPower(0);
  667. inchideServo();
  668.  
  669. mergiFata(viteza, 0);
  670. TimeUnit.MILLISECONDS.sleep(1300);
  671. mergiFata(-viteza, 0);
  672. TimeUnit.MILLISECONDS.sleep(500);
  673. oprire();
  674.  
  675. }
  676.  
  677. public void rotireSpeciala2(double tinta, double treshHold, double viteza) throws InterruptedException {
  678.  
  679. int incercari = 0;
  680. boolean suntPeTinta = false;
  681. boolean amTrecut;
  682. while (!suntPeTinta && opModeIsActive()) {
  683. double mergiStanga = viteza;
  684. double mergiDreapta = -viteza;
  685. amTrecut = false;
  686. while (navx_device.getYaw() >= tinta + treshHold && opModeIsActive() && !amTrecut) { // cat timp sunt in dreapta tintei vreau sa merg in stanga
  687. telemetry.addData("Rotatie: ", navx_device.getYaw());
  688. telemetry.addLine();
  689. telemetry.addData("incercari:", incercari);
  690. telemetry.addLine();
  691. telemetry.addData("treshhold:", treshHold);
  692. telemetry.addLine();
  693. telemetry.addData("viteza:", viteza);
  694. telemetry.update();
  695. motorFrontLeft.setPower(mergiStanga);
  696. motorFrontRight.setPower(mergiStanga);
  697. motorBackRight.setPower(mergiStanga);
  698. motorBackLeft.setPower(mergiStanga);
  699. double unghi1 = navx_device.getYaw();
  700. TimeUnit.MILLISECONDS.sleep(100);
  701. double unghi2 = navx_device.getYaw();
  702. if (unghi2 > unghi1)
  703. amTrecut = true;
  704.  
  705. }
  706. oprire();
  707. TimeUnit.MILLISECONDS.sleep(500);
  708.  
  709.  
  710. if (navx_device.getYaw() >= 180 - treshHold && navx_device.getYaw() <= tinta + treshHold) { // daca sunt pe o parte sau o alta a tintei si in limitele tresholdului ma opresc
  711. suntPeTinta = true;
  712.  
  713. }
  714. amTrecut = false;
  715. if (!suntPeTinta) { // daca nu m-am pozitionat bine, incerc sa merg in dreapta. Robotul nu se poate opri decat in limitele treshHoldului sau in partea stanga
  716. while (navx_device.getYaw() <= 180 - treshHold && opModeIsActive() && !amTrecut) { // cat timp sunt in partea stanga a tintei vreau sa merg in dreapta
  717.  
  718. telemetry.addData("Rotatie: ", navx_device.getYaw());
  719. telemetry.addLine();
  720. telemetry.addData("incercari:", incercari);
  721. telemetry.addLine();
  722. telemetry.addData("treshhold:", treshHold);
  723. telemetry.addLine();
  724. telemetry.addData("viteza:", viteza);
  725. telemetry.update();
  726.  
  727. motorFrontLeft.setPower(mergiDreapta);
  728. motorFrontRight.setPower(mergiDreapta);
  729. motorBackRight.setPower(mergiDreapta);
  730. motorBackLeft.setPower(mergiDreapta);
  731. double unghi1 = navx_device.getYaw();
  732. TimeUnit.MILLISECONDS.sleep(100);
  733. double unghi2 = navx_device.getYaw();
  734. if (unghi2 < unghi1)
  735. amTrecut = true;
  736. }
  737. oprire();
  738. TimeUnit.MILLISECONDS.sleep(500);
  739. }
  740.  
  741. if (navx_device.getYaw() >= 180 - treshHold && navx_device.getYaw() <= tinta + treshHold) // daca sunt pe o parte sau o alta a tintei si in limitele tresholdului ma opresc
  742. suntPeTinta = true;
  743. amTrecut = false;
  744.  
  745. incercari++;
  746. if (incercari % 2 == 0 && incercari != 0) { // daca dupa doua incercari nu a reusit, il ajut putin . O incercare inseamna o rotire spre stanga si o rotire spre dreapta sau viceversa
  747. // treshHold += 0.5;
  748. viteza -= 0.01;
  749. }
  750.  
  751. if (suntPeTinta)
  752. oprire();
  753.  
  754. }
  755.  
  756. }
  757.  
  758.  
  759. public void mergiFata(double viteza, double plus) {
  760. motorFrontLeft.setPower(-viteza);
  761. motorFrontRight.setPower(viteza + plus);
  762. motorBackRight.setPower(viteza);
  763. motorBackLeft.setPower(-viteza - plus);
  764. }
  765.  
  766. public void mergiDreapta(double viteza) {
  767. motorFrontLeft.setPower(viteza);
  768. motorFrontRight.setPower(viteza);
  769. motorBackRight.setPower(-viteza);
  770. motorBackLeft.setPower(-viteza);
  771. }
  772.  
  773. public void du_teLaRaft(int raft, double viteza) throws InterruptedException {
  774.  
  775. double dist1 = rangeSensor.getDistance(DistanceUnit.CM);
  776. double dist2 = dist1;
  777. mergiFata(viteza, 0);
  778. do {
  779.  
  780. telemetry.addLine();
  781. telemetry.addData("distanta initiala:", dist1);
  782. telemetry.addLine();
  783. telemetry.addData("distanta curenta:", dist2);
  784. telemetry.update();
  785.  
  786. dist2 = rangeSensor.getDistance(DistanceUnit.CM);
  787.  
  788. if (dist1 - dist2 > 5) {
  789.  
  790. raft--; TimeUnit.MILLISECONDS.sleep(1000);
  791. telemetry.addData("sunt la raftul:", raft);
  792. telemetry.update();
  793. }
  794.  
  795. } while (raft > 0 && opModeIsActive());
  796.  
  797. oprire();
  798.  
  799.  
  800. }
  801.  
  802. public void oprireTot() {
  803.  
  804. navx_device.close();
  805. rangeSensor.close();
  806. colorSensor.close();
  807. oprire();
  808.  
  809. }
  810.  
  811. /* public void prindeCub(double stanga, double dreapta) throws InterruptedException {
  812.  
  813. CubSJ.setPosition(stanga);
  814. CubDJ.setPosition(dreapta);
  815. TimeUnit.MILLISECONDS.sleep(500);
  816. mRid.setPower(1);
  817. TimeUnit.MILLISECONDS.sleep(500);
  818. mRid.setPower(0);
  819.  
  820. }*/
  821. public void prindeCub(double stanga, double dreapta) throws InterruptedException {
  822.  
  823. CubSJ.setPosition(0.4614);
  824. CubDJ.setPosition(0.4785);
  825. CubSS.setPosition(0.4614);
  826. CubDS.setPosition(0.4785);
  827.  
  828. TimeUnit.MILLISECONDS.sleep(300);
  829. mRid.setPower(-1);
  830. TimeUnit.MILLISECONDS.sleep(300);
  831. mRid.setPower(0);
  832.  
  833.  
  834. CubSJ.setPosition(stanga);
  835. CubDJ.setPosition(dreapta);
  836. TimeUnit.MILLISECONDS.sleep(500);
  837. mRid.setPower(1);
  838. TimeUnit.MILLISECONDS.sleep(500);
  839. mRid.setPower(0);
  840.  
  841. }
  842.  
  843. public void lasaCub(double stanga, double dreapta) throws InterruptedException {
  844. CubSJ.setPosition(stanga);
  845. CubDJ.setPosition(dreapta);
  846. CubSS.setPosition(stanga);
  847. CubDS.setPosition(dreapta);
  848. }
  849. public void puneServo(double poz) {
  850. servoBile.setPosition(poz);
  851. }
  852.  
  853.  
  854. public void da_teJosDePeRampa(double viteza, double rollInitial, double altInitial) {
  855.  
  856. mergiFata(viteza, 0);
  857.  
  858. double treshHold = .2;
  859. while (Math.abs(navx_device.getPitch()) <= 10 && opModeIsActive()) {
  860. idle();
  861. }
  862. while (Math.abs(navx_device.getPitch()) >= 4 && opModeIsActive()) {
  863. telemetry.addData("Roll: ", Math.abs(navx_device.getPitch()));
  864. telemetry.update();
  865. }
  866. oprire();
  867. }
  868.  
  869. public void puneServoTreptat(Servo servo, double tinta, double pozInitial) {
  870.  
  871. while (servo.getPosition() > tinta) {
  872. servo.setPosition(pozInitial);
  873. pozInitial -= 0.005;
  874. }
  875.  
  876. }
  877.  
  878. public void du_teLaRaft2(int raft, double viteza) {
  879. double tinta;
  880. switch (raft) {
  881.  
  882. case 2:
  883. tinta = distRaft1;
  884. break;
  885. case 3:
  886. tinta = distRaft2;
  887. break;
  888. case 4:
  889. tinta = distRaft3;
  890. break;
  891. default:
  892. tinta = distRaft1;
  893.  
  894. }
  895.  
  896. double distantaCurenta = rangeSensor.getDistance(DistanceUnit.CM);
  897. while (distantaCurenta < tinta && opModeIsActive()) {
  898. distantaCurenta = rangeSensor.getDistance(DistanceUnit.CM);
  899. mergiDreapta(viteza);
  900. telemetry.addData("distanta Curenta :", distantaCurenta);
  901. telemetry.update();
  902. }
  903. oprire();
  904. }
  905.  
  906. public void oprire() {
  907. motorFrontLeft.setPower(0);
  908. motorFrontRight.setPower(0);
  909. motorBackLeft.setPower(0);
  910. motorBackRight.setPower(0);
  911. }
  912.  
  913. public void inchideServo() {
  914. CubSJ.setPosition(ServS);
  915. CubDJ.setPosition(ServD);
  916. }
  917.  
  918. public void pune_teFataDeRaft(double tinta, char directie) throws InterruptedException {
  919. if (rangeSensor.getDistance(DistanceUnit.CM) > tinta) {
  920. while (rangeSensor.getDistance(DistanceUnit.CM) > tinta) {
  921. if (directie == 's')
  922. mergiDreapta(-vitezaMiscare);
  923. if (directie == 'd')
  924. mergiDreapta(vitezaMiscare);
  925. }
  926.  
  927. } else {
  928. while (rangeSensor.getDistance(DistanceUnit.CM) < tinta) {
  929. if (directie == 's')
  930. mergiDreapta(-vitezaMiscare);
  931. if (directie == 'd')
  932. mergiDreapta(vitezaMiscare);
  933.  
  934. }
  935.  
  936. }
  937. oprire();
  938.  
  939. }
  940.  
  941. }
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!