API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Mar 19th, 2018
96
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 17.90 KB | None | 0 0
raw download clone embed print report
  1. package org.firstinspires.ftc.teamcode;
  2. import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cColorSensor;
  3. import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro;
  4. import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
  5. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  6. import com.qualcomm.robotcore.hardware.CRServo;
  7. import com.qualcomm.robotcore.hardware.DcMotor;
  8. import com.qualcomm.robotcore.hardware.DcMotorSimple;
  9. import com.qualcomm.robotcore.hardware.HardwareMap;
  10. import com.qualcomm.robotcore.hardware.Servo;
  11. import com.qualcomm.robotcore.util.ElapsedTime;
  12. import com.qualcomm.robotcore.util.Range;
  13. import org.firstinspires.ftc.robotcore.external.ClassFactory;
  14. import org.firstinspires.ftc.robotcore.external.Telemetry;
  15. import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
  16. import org.firstinspires.ftc.robotcore.external.navigation.RelicRecoveryVuMark;
  17. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
  18. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
  19. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener;
  20. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
  21. import java.util.concurrent.TimeUnit;
  22.  
  23.  
  24. @Autonomous (name = "Rosu_aproape")
  25. public class Rosu_aproape extends LinearOpMode {
  26.  
  27. public static class Initialization {
  28.  
  29. public DcMotor Roata_Stanga_Fata = null;
  30. public DcMotor Roata_Stanga_Spate = null;
  31. public DcMotor Roata_Dreapta_Fata = null;
  32. public DcMotor Roata_Dreapta_Spate = null;
  33. public DcMotor Brats = null;
  34. public DcMotor Bratd = null;
  35. HardwareMap hwMap = null;
  36. Servo brat;
  37. Servo bratsecund;
  38. Servo servo_stanga;
  39. Servo servo_dreapta;
  40. ModernRoboticsI2cColorSensor colorSensor;
  41.  
  42. public Initialization() {
  43.  
  44. }
  45.  
  46.  
  47. /* Initialize standard Hardware interfaces */
  48. public void init(HardwareMap ahwMap) {
  49. // Save reference to Hardware map
  50. hwMap = ahwMap;
  51. bratsecund = hwMap.get(Servo.class, "bratsecund");
  52. brat = hwMap.get(Servo.class, "bratmingi");
  53. colorSensor = hwMap.get(ModernRoboticsI2cColorSensor.class, "jewels_sensor");
  54. servo_stanga = hwMap.get(Servo.class, "servostanga");
  55. servo_dreapta = hwMap.get(Servo.class, "servodreapta");
  56. Brats = hwMap.get(DcMotor.class, "brats");
  57. Bratd = hwMap.get(DcMotor.class, "bratd");
  58. Roata_Stanga_Fata = hwMap.get(DcMotor.class, "sf");
  59. Roata_Stanga_Spate = hwMap.get(DcMotor.class, "ss");
  60. Roata_Dreapta_Fata = hwMap.get(DcMotor.class, "df");
  61. Roata_Dreapta_Spate = hwMap.get(DcMotor.class, "ds");
  62.  
  63. Roata_Stanga_Fata.setDirection(DcMotor.Direction.REVERSE);
  64. Roata_Stanga_Spate.setDirection(DcMotor.Direction.REVERSE);
  65. Roata_Dreapta_Fata.setDirection(DcMotor.Direction.FORWARD);
  66. Roata_Dreapta_Spate.setDirection(DcMotor.Direction.FORWARD);
  67.  
  68. Bratd.setDirection(DcMotor.Direction.FORWARD);
  69. Brats.setDirection(DcMotor.Direction.FORWARD);
  70. // Set all motors to zero power
  71. Roata_Stanga_Fata.setPower(0);
  72. Roata_Stanga_Spate.setPower(0);
  73. Roata_Dreapta_Fata.setPower(0);
  74. Roata_Dreapta_Spate.setPower(0);
  75. }
  76. }
  77.  
  78. Initialization robot = new Initialization();
  79. ModernRoboticsI2cGyro gyro = null;
  80.  
  81. static final double COUNTS_PER_MOTOR_REV = 1440;
  82. static final double WHEEL_DIAMETER_INCHES = 10.16;
  83. static final double COUNTS_PER_CM = COUNTS_PER_MOTOR_REV /
  84. (WHEEL_DIAMETER_INCHES * 3.1415);
  85.  
  86.  
  87. @Override
  88. public void runOpMode() throws InterruptedException {
  89.  
  90. ElapsedTime runtime = new ElapsedTime();
  91. robot.init(hardwareMap);
  92. gyro = (ModernRoboticsI2cGyro) hardwareMap.gyroSensor.get("gyro");
  93.  
  94. telemetry.addData(">", "Giroscopul se calibreaza");
  95. telemetry.update();
  96.  
  97. gyro.calibrate();
  98. gyro.resetZAxisIntegrator();
  99.  
  100. while (!isStopRequested() && gyro.isCalibrating()) {
  101. sleep(50);
  102. idle();
  103. }
  104.  
  105. telemetry.addData(">", "Robot Pregatit.");
  106. telemetry.update();
  107.  
  108. robot.colorSensor.enableLed(true);
  109. robot.brat.setPosition(1);
  110. robot.bratsecund.setPosition(1);
  111. robot.servo_dreapta.setPosition(0.42);
  112. robot.servo_stanga.setPosition(0.58);
  113.  
  114. waitForStart();
  115. runtime.reset();
  116.  
  117.  
  118. //int col=ceva();
  119. //int z=gyro.getIntegratedZValue();
  120. //Rotire_dreapta(0.175, 30);
  121. //Rotire_stanga(0.175, 30);
  122. Drive(0.4,90);
  123. //brat_mingi();
  124. /*if(col==0)
  125. {
  126. Drive(-0.175,90);
  127. }
  128. else if(col==1)
  129. {
  130. Drive(-0.175,110);
  131. }
  132. else if(col==2)
  133. {
  134. Drive(-0.175,130);
  135. }*/
  136. //brat_roti();
  137. //Corectare_beta(0.1,z);
  138. //Rotire_dreapta(0.2,90);
  139. //pune_cub();
  140. Stop();
  141.  
  142.  
  143. }
  144.  
  145. public void Corectare_beta(double speed, double z) {
  146. while (opModeIsActive() && gyro.getHeading() != z) {
  147. if (gyro.getHeading() < z)
  148. Dreapta(speed);
  149. if (gyro.getHeading() > z)
  150. Stanga(speed);
  151. if (gyro.getHeading() == z)
  152. stop();
  153. }
  154. }
  155.  
  156. public void Run(double speed) {
  157. robot.Roata_Stanga_Fata.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  158. robot.Roata_Stanga_Spate.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  159. robot.Roata_Dreapta_Fata.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  160. robot.Roata_Dreapta_Spate.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  161. robot.Roata_Stanga_Fata.setPower(speed);
  162. robot.Roata_Stanga_Spate.setPower(speed);
  163. robot.Roata_Dreapta_Spate.setPower(speed);
  164. robot.Roata_Dreapta_Fata.setPower(speed);
  165. robot.Roata_Stanga_Fata.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  166. robot.Roata_Stanga_Spate.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  167. robot.Roata_Dreapta_Fata.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  168. robot.Roata_Dreapta_Spate.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  169. }
  170.  
  171. public void Drive(double speed,
  172. double distance) {
  173. int stanga;
  174. int dreapta;
  175. int moveCounts;
  176.  
  177. robot.Roata_Stanga_Fata.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  178. robot.Roata_Stanga_Spate.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  179. robot.Roata_Dreapta_Fata.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  180. robot.Roata_Dreapta_Spate.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  181.  
  182. if (opModeIsActive()) {
  183. distance = distance * 0.58;
  184. moveCounts = (int) (distance * COUNTS_PER_CM);
  185. //double directie;
  186. //directie = gyro.getIntegratedZValue();
  187. telemetry.addData("moveCounts= ", moveCounts);
  188. telemetry.update();
  189. stanga = robot.Roata_Stanga_Spate.getCurrentPosition() + moveCounts;
  190. dreapta = robot.Roata_Dreapta_Spate.getCurrentPosition() + moveCounts;
  191. if (speed > 0) {
  192. while (robot.Roata_Dreapta_Fata.getCurrentPosition() > -dreapta && robot.Roata_Dreapta_Spate.getCurrentPosition() > -dreapta
  193. && robot.Roata_Stanga_Fata.getCurrentPosition() > -stanga && robot.Roata_Dreapta_Spate.getCurrentPosition() > -stanga && opModeIsActive()) {
  194. Run(speed);
  195. }
  196. } else if (speed < 0) {
  197. while (robot.Roata_Dreapta_Fata.getCurrentPosition() < dreapta && robot.Roata_Dreapta_Spate.getCurrentPosition() < dreapta
  198. && robot.Roata_Stanga_Fata.getCurrentPosition() < stanga && robot.Roata_Dreapta_Spate.getCurrentPosition() < stanga && opModeIsActive()) {
  199. Run(speed);
  200. }
  201. }
  202. Stop();
  203.  
  204. }
  205. }
  206.  
  207.  
  208. public void Stop() {
  209. robot.Roata_Stanga_Fata.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  210. robot.Roata_Stanga_Spate.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  211. robot.Roata_Dreapta_Fata.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  212. robot.Roata_Dreapta_Spate.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  213. robot.Roata_Stanga_Fata.setPower(0);
  214. robot.Roata_Stanga_Spate.setPower(0);
  215. robot.Roata_Dreapta_Fata.setPower(0);
  216. robot.Roata_Dreapta_Spate.setPower(0);
  217. robot.Roata_Stanga_Fata.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  218. robot.Roata_Stanga_Spate.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  219. robot.Roata_Dreapta_Fata.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  220. robot.Roata_Dreapta_Spate.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  221. }
  222.  
  223. public void Dreapta(double speed) {
  224. if (opModeIsActive()) {
  225. robot.Roata_Stanga_Fata.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  226. robot.Roata_Stanga_Spate.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  227. robot.Roata_Dreapta_Fata.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  228. robot.Roata_Dreapta_Spate.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  229. robot.Roata_Stanga_Fata.setPower(speed);
  230. robot.Roata_Stanga_Spate.setPower(speed);
  231. robot.Roata_Dreapta_Spate.setPower(-speed);
  232. robot.Roata_Dreapta_Fata.setPower(-speed);
  233. robot.Roata_Stanga_Fata.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  234. robot.Roata_Stanga_Spate.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  235. robot.Roata_Dreapta_Fata.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  236. robot.Roata_Dreapta_Spate.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  237. }
  238. }
  239.  
  240. public void Stanga(double speed) {
  241. if (opModeIsActive()) {
  242. robot.Roata_Stanga_Fata.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  243. robot.Roata_Stanga_Spate.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  244. robot.Roata_Dreapta_Fata.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  245. robot.Roata_Dreapta_Spate.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  246. robot.Roata_Stanga_Fata.setPower(-speed);
  247. robot.Roata_Stanga_Spate.setPower(-speed);
  248. robot.Roata_Dreapta_Spate.setPower(speed);
  249. robot.Roata_Dreapta_Fata.setPower(speed);
  250. robot.Roata_Stanga_Fata.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  251. robot.Roata_Stanga_Spate.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  252. robot.Roata_Dreapta_Fata.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  253. robot.Roata_Dreapta_Spate.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  254. }
  255. //stop();
  256. }
  257.  
  258. //De pregatit dupa regionala
  259. /*
  260. public void Corectare(double directie) {
  261.  
  262. robot.Roata_Stanga_Fata.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  263. robot.Roata_Stanga_Spate.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  264. robot.Roata_Dreapta_Fata.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  265. robot.Roata_Dreapta_Spate.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  266.  
  267. Stop();
  268.  
  269. while(opModeIsActive() && gyro.getIntegratedZValue() < directie)
  270. {
  271. if(gyro.getIntegratedZValue() < directie)
  272. {
  273. Dreapta(0.2);
  274. }
  275. Dreapta(0.2);
  276. if(gyro.getIntegratedZValue() > directie)
  277. {
  278. Stanga(0.2);
  279. }
  280. }
  281.  
  282. Stop();
  283.  
  284. robot.Roata_Stanga_Fata.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  285. robot.Roata_Stanga_Spate.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  286. robot.Roata_Dreapta_Fata.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  287. robot.Roata_Dreapta_Spate.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  288.  
  289. }
  290.  
  291. */
  292. //SCHIMBATE INTRE ELE
  293. public void Rotire_stanga(double speed, int angle) {
  294.  
  295. gyro.resetZAxisIntegrator();
  296. int currentHeading = gyro.getIntegratedZValue();
  297. int target = currentHeading + angle;
  298. robot.Roata_Stanga_Fata.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  299. robot.Roata_Stanga_Spate.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  300. robot.Roata_Dreapta_Fata.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  301. robot.Roata_Dreapta_Spate.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  302. while (currentHeading < target && opModeIsActive()) {
  303. telemetry.addData(">", "Orientarea robotului = %d grade", gyro.getIntegratedZValue());
  304. telemetry.update();
  305. Stanga(speed);
  306. currentHeading = gyro.getIntegratedZValue();
  307. }
  308. Stop();
  309. }
  310.  
  311. public void Rotire_dreapta(double speed, int angle) {
  312. angle = -angle;
  313. gyro.resetZAxisIntegrator();
  314. int currentHeading = gyro.getIntegratedZValue();
  315. int target = currentHeading + angle;
  316. robot.Roata_Stanga_Fata.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  317. robot.Roata_Stanga_Spate.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  318. robot.Roata_Dreapta_Fata.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  319. robot.Roata_Dreapta_Spate.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  320. while (currentHeading > target && opModeIsActive()) {
  321. telemetry.addData(">", "Orientarea robotului = %d grade", gyro.getIntegratedZValue());
  322. telemetry.update();
  323. Dreapta(speed);
  324. currentHeading = gyro.getIntegratedZValue();
  325. }
  326. Stop();
  327. }
  328.  
  329.  
  330. VuforiaLocalizer vuforia;
  331.  
  332. public int ceva() {
  333. int col = -1;
  334. int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
  335. VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
  336.  
  337. parameters.vuforiaLicenseKey = "AVR36zX/////AAAAmf4wKl7jz04GkSKCzFf6A/gIfIqXUXFiD8Q8smknNL1/7mQ7bfNDDX/GKq/vejsAFCypbnHQwmHunWd2pk/dOZY2Wi4Nj64UbWmDawkA3Jy9oiIfS4C7sfViotjeuhQZuuUuHOEDh63tJEX54rWgXUHYYpBUApz+2pB4ijjg+YipO05M9EEInFeUqL3rpEu1vuLq942L/tc7r2C/Am9W37dHlCMlIBYJ2f1RpTdi/6+WVFuiD5lhIyL6hGU9OMhmRzBrZeJWF0GNFHqi21JxiD9IpRIsk6KDqMdS/gYEWg20Lkk/vPEDbKXCrdm9iAQjlBAzjPq0gIE6i785JI8y61qzxohgI4PXEF0ciUqiC2UD";
  338.  
  339.  
  340. parameters.cameraDirection = VuforiaLocalizer.CameraDirection.FRONT;
  341. this.vuforia = ClassFactory.createVuforiaLocalizer(parameters);
  342.  
  343. VuforiaTrackables relicTrackables = this.vuforia.loadTrackablesFromAsset("RelicVuMark");
  344. VuforiaTrackable relicTemplate = relicTrackables.get(0);
  345. relicTemplate.setName("relicVuMarkTemplate"); // can help in debugging; otherwise not necessary
  346.  
  347. telemetry.update();
  348. waitForStart();
  349.  
  350. relicTrackables.activate();
  351.  
  352. while (opModeIsActive() && col == -1) {
  353.  
  354. RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate);
  355. if (vuMark == RelicRecoveryVuMark.LEFT) {
  356. telemetry.addData("left", vuMark);
  357. OpenGLMatrix pose = ((VuforiaTrackableDefaultListener) relicTemplate.getListener()).getPose();
  358. col = 0;
  359. return col;
  360. }
  361. if (vuMark == RelicRecoveryVuMark.CENTER) {
  362. telemetry.addData("center", vuMark);
  363. OpenGLMatrix pose = ((VuforiaTrackableDefaultListener) relicTemplate.getListener()).getPose();
  364. col = 1;
  365. return col;
  366. }
  367. if (vuMark == RelicRecoveryVuMark.RIGHT) {
  368. telemetry.addData("right", vuMark);
  369. OpenGLMatrix pose = ((VuforiaTrackableDefaultListener) relicTemplate.getListener()).getPose();
  370. col = 2;
  371. return col;
  372. }
  373. telemetry.update();
  374.  
  375. }
  376. return col;
  377.  
  378. }
  379.  
  380.  
  381. //Pentru Rosu
  382. public void brat_mingi()
  383. {
  384. boolean ok=false;
  385. int okbrat=0;
  386. int okbrats=1;
  387. double power= 0.5;
  388.  
  389. robot.brat.setPosition(0);
  390. sleep(1000);
  391. robot.bratsecund.setPosition(0);
  392. int v=1;
  393. while(opModeIsActive() && (v != 0) )
  394. {
  395. telemetry.addData("Color Name", robot.colorSensor.readUnsignedByte(ModernRoboticsI2cColorSensor.Register.COLOR_NUMBER));
  396. telemetry.update();
  397.  
  398.  
  399. if (robot.colorSensor.readUnsignedByte(ModernRoboticsI2cColorSensor.Register.COLOR_NUMBER) == 10 && ok == false) {
  400. ok = true;
  401. Rotire_dreapta(0.175,10);
  402. sleep(250);
  403. robot.bratsecund.setPosition(1);
  404. sleep(500);
  405. robot.brat.setPosition(0.65);
  406. Rotire_stanga(0.175,10);
  407. sleep(250);
  408. v=0;
  409.  
  410. }
  411. if (robot.colorSensor.readUnsignedByte(ModernRoboticsI2cColorSensor.Register.COLOR_NUMBER) == 3 && ok == false) {
  412. ok = true;
  413. Rotire_stanga(0.175,10);
  414. sleep(250);
  415. robot.bratsecund.setPosition(1);
  416. sleep(500);
  417. robot.brat.setPosition(0.65);
  418. Rotire_dreapta(0.175,10);
  419. sleep(250);
  420. v=0;
  421. }
  422. }
  423. }
  424.  
  425.  
  426. public void Roti_verzi()
  427. {
  428. robot.Brats.setPower(-1);
  429. robot.Bratd.setPower(1);
  430. sleep(2000);
  431. robot.Bratd.setPower(0);
  432. robot.Brats.setPower(0);
  433.  
  434. }
  435.  
  436. public void Plasare_cub()
  437. {
  438. sleep(1500);
  439. robot.servo_stanga.setPosition(0.15);
  440. robot.servo_dreapta.setPosition(0.85);
  441. sleep(1000);
  442. robot.servo_dreapta.setPosition(0.42);
  443. robot.servo_stanga.setPosition(0.58);
  444. }
  445. }
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!