API tools faq
paste
Advertisement
Guest User

The Code

a guest
Jan 18th, 2017
146
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 13.84 KB | None | 0 0
raw download clone embed print report
  1. package org.firstinspires.ftc.teamcode;
  2.  
  3. import android.media.MediaPlayer;
  4.  
  5. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  6. import com.qualcomm.robotcore.hardware.ColorSensor;
  7. import com.qualcomm.robotcore.hardware.DcMotor;
  8. import com.qualcomm.robotcore.hardware.I2cAddr;
  9. import com.qualcomm.robotcore.hardware.OpticalDistanceSensor;
  10. import com.qualcomm.robotcore.hardware.Servo;
  11.  
  12.  
  13. /**
  14. * Created by Andrew on 1/17/2017.
  15. */
  16.  
  17. public abstract class InitCode extends LinearOpMode {
  18. //Line Sensor
  19. final int LLMIN = 20;
  20. final int LRMIN = 7;
  21.  
  22. double leftLinePowerBCK = -0.2;
  23. double RightLinePowerBCK = -0.2;
  24. double leftLinePowerFWD = 0.15;
  25. double RightLinePowerFWD = 0.15;
  26.  
  27. MediaPlayer Vader;
  28.  
  29. DcMotor motor1; //left wheel
  30. DcMotor motor2; //right wheel
  31. DcMotor motor3; //shooting motor
  32.  
  33. ColorSensor color1; //left color sensor
  34. ColorSensor color2; //right color sensor
  35.  
  36. ColorSensor line1; //left line sensor
  37. ColorSensor line2; //right line sensor
  38.  
  39. OpticalDistanceSensor optical1;
  40.  
  41. Servo servo1; //left button pusher
  42. Servo servo2; //right button pusher
  43.  
  44. public void initSound()
  45. {
  46. Vader = MediaPlayer.create(hardwareMap.appContext, R.raw.bottheme);
  47. }
  48.  
  49. public void initMotors()
  50. {
  51. motor1 = hardwareMap.dcMotor.get("motor1");
  52. motor2 = hardwareMap.dcMotor.get("motor2");
  53. motor3 = hardwareMap.dcMotor.get("motor3");
  54.  
  55. motor1.setDirection(DcMotor.Direction.FORWARD); // remember to change directions for team color
  56. motor2.setDirection(DcMotor.Direction.REVERSE);
  57. motor3.setDirection(DcMotor.Direction.FORWARD);
  58.  
  59. motor1.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  60. motor2.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  61. motor3.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  62. }
  63.  
  64. public void initSensors()
  65. {
  66.  
  67.  
  68. color1 = hardwareMap.colorSensor.get("color1");
  69. color2 = hardwareMap.colorSensor.get("color2");
  70. optical1 = hardwareMap.opticalDistanceSensor.get("optical1");
  71.  
  72. color1.setI2cAddress(I2cAddr.create8bit(0x6c));
  73. color2.setI2cAddress(I2cAddr.create8bit(0x7c));
  74.  
  75.  
  76. line1 = hardwareMap.colorSensor.get("line1"); //left line sensor
  77. line2 = hardwareMap.colorSensor.get("line2"); //right line sensor
  78.  
  79. line1.setI2cAddress(I2cAddr.create8bit(0x4c));
  80. line2.setI2cAddress(I2cAddr.create8bit(0x5c));
  81. }
  82.  
  83. public void initServos()
  84. {
  85. servo1 = hardwareMap.servo.get("servo1");
  86. servo2 = hardwareMap.servo.get("servo2");
  87.  
  88. servo1.setPosition(1);
  89. servo2.setPosition(0);
  90. }
  91.  
  92. public void initAll()
  93. {
  94. initMotors();
  95. initServos();
  96. initSensors();
  97. initSound();
  98. }
  99.  
  100. }
  101.  
  102. Above is our init class. I have a methods class and the end product (one for each side). I'll be posting methods and Red Side below.
  103.  
  104. package org.firstinspires.ftc.teamcode;
  105.  
  106. import android.media.MediaPlayer;
  107.  
  108. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  109. import com.qualcomm.robotcore.hardware.ColorSensor;
  110. import com.qualcomm.robotcore.hardware.DcMotor;
  111. import com.qualcomm.robotcore.hardware.I2cAddr;
  112. import com.qualcomm.robotcore.hardware.OpticalDistanceSensor;
  113. import com.qualcomm.robotcore.hardware.Servo;
  114.  
  115. /**
  116. * Created by Cole on 12/27/2016.
  117. */
  118.  
  119. public abstract class BaseAutonomous extends InitCode {
  120. //21 pos: 1 degree
  121. //280/pi pos : 1 inch
  122.  
  123. //RED CNST BELOW
  124. private boolean dank = false;
  125.  
  126. //1st turn
  127. public final int M1T1 = 0;
  128. public final int M2T1 = degreesToPositions(43);
  129. public final double PWRT1 = 0.5;
  130.  
  131. //2nd turn
  132. public final int M1T2 = 0;
  133. public final int M2T2 = degreesToPositions(-35);
  134. public final double PWRT2 = -0.5;
  135.  
  136. //2.5th turn
  137. public final int M1T25 = degreesToPositions(5);
  138. public final int M2T25 = 0;
  139. public final double PWRT25 = 0.5;
  140.  
  141.  
  142. //3rd turn
  143. public final int M1T3 = degreesToPositions(-45);
  144. public final int M2T3 = 0;
  145. public final double PWRT3 = -.25;
  146.  
  147. //4th turn
  148. public final int M1T4 = degreesToPositions(37);
  149. public final int M2T4 = 0;
  150. public final double PWRT4 = 0.25;
  151.  
  152. //1st move
  153. public final int PM1 = inchesToPositions(60);
  154. public final double PWRM1 = 0.5;
  155.  
  156. //linefollow increment
  157. public int LMCNST1 = inchesToPositions(-2);
  158. public int LMCNST2 = inchesToPositions(-2);
  159. public double LPWR = -0.2;
  160. public double LPWR2 = 0.2;
  161.  
  162. //2nd move
  163. public final int PM2 = (int) inchesToPositions(-10);
  164. public final double PWRM2 = 0.2;
  165.  
  166. //3rd move
  167. public final int PM3 = inchesToPositions(6);
  168. public final double PWRM3 = 0.2;
  169.  
  170. //4th move
  171. public final int PM4 = inchesToPositions(54);
  172. public final double PWRM4 = 0.25;
  173.  
  174. //5th move
  175. public final int PM5 = inchesToPositions(15);
  176. public final double PWRM5 = 0.5;
  177.  
  178. //6th move
  179. public final int PM6 = inchesToPositions(-75);
  180.  
  181. public final double PWRM6 = -0.7;
  182.  
  183. //Beacon
  184. public final int PBM = 37;
  185. public final double PWRBM = 1.0;
  186.  
  187. //BLUE CNST BELOW
  188. //Move 1
  189. public final int MOVE1B = inchesToPositions(18);
  190. public final double MOVE1SPD = 0.5;
  191.  
  192. //Move 2
  193. public final int MOVE2B = inchesToPositions(-60);
  194. public final double MOVE2SPD = -0.5;
  195.  
  196. //Move 3
  197. public final int MOVE3B = inchesToPositions(-12);
  198. public final double MOVE3SPD = -0.2;
  199.  
  200. //Move 4
  201. public final int MOVE4B = inchesToPositions(15);
  202. public final double MOVE4SPD = 0.5;
  203.  
  204.  
  205. //Turn 1
  206. public final int TURN1B = degreesToPositions(-160);
  207. public final int TURN1B2 = 0;
  208. public final double TURN1SPD = -0.5;
  209.  
  210. //Turn 2
  211. public final int TURN2B = 0;
  212. public final int TURN2B2 = degreesToPositions(47);
  213. public final double TURN2SPD = 0.5;
  214.  
  215. //Turn 3
  216. public final int TURN3B = 0;
  217. public final int TURN3B2 = degreesToPositions(-90);
  218. public final double TURN3SPD = -0.5;
  219.  
  220. //Turn 4
  221. public final int TURN4B = degreesToPositions(-23);
  222. public final int TURN4B2 = degreesToPositions(22);
  223. public final double TURN4SPD = 0.5;
  224.  
  225. public void move(int addPos, double power) {
  226. int currentPos1 = motor1.getCurrentPosition();
  227. int currentPos2 = motor2.getCurrentPosition();
  228.  
  229. motor1.setTargetPosition(currentPos1 + addPos);
  230. motor2.setTargetPosition(currentPos2 + addPos);
  231. motor1.setPower(power);
  232. motor2.setPower(power);
  233.  
  234. while (opModeIsActive() && motor1.isBusy() && motor2.isBusy());
  235. motor1.setPower(0);
  236. motor2.setPower(0);
  237. }
  238.  
  239. public void shoot(double power) {
  240. final int POS_INCREMENT = 1000;
  241. int currentPos3 = motor3.getCurrentPosition();
  242. motor3.setTargetPosition(currentPos3 + POS_INCREMENT);
  243. motor3.setPower(power);
  244. while (opModeIsActive() && motor3.isBusy());
  245. motor3.setPower(0);
  246. }
  247.  
  248. public void turn(int pos1, int pos2, double power) {
  249. int currentPos1 = motor1.getCurrentPosition();
  250. int currentPos2 = motor2.getCurrentPosition();
  251. motor1.setTargetPosition(pos1 + currentPos1);
  252. motor2.setTargetPosition(pos2 + currentPos2);
  253. motor1.setPower(power);
  254. motor2.setPower(power);
  255. while (motor1.isBusy() || motor2.isBusy() && opModeIsActive());
  256. motor1.setPower(0);
  257. motor2.setPower(0);
  258. }
  259.  
  260.  
  261. private boolean seesLineLeft() {
  262. return ((line1.alpha() > LLMIN));
  263. }
  264.  
  265. private boolean seesLineRight() {
  266. return ((line2.alpha() > LRMIN));
  267. }
  268.  
  269. public void pushBeaconRed() throws InterruptedException{
  270. if(color1.red() > color2.red()|| color1.blue() < color2.blue()) {
  271. servo2.setPosition(1);
  272. sleep(500);
  273. servo2.setPosition(0);
  274. sleep(500);
  275. servo2.setPosition(1);
  276. }
  277.  
  278. else if (color1.red() < color2.red() || color2.blue() < color1.blue()) {
  279. servo1.setPosition(0);
  280. sleep(500);
  281. servo1.setPosition(1);
  282. sleep(500);
  283. servo1.setPosition(0);
  284. }
  285. sleep(500);
  286. servo1.setPosition(1);
  287. servo2.setPosition(0);
  288. }
  289.  
  290. public void pushBeaconBlue() throws InterruptedException{
  291. if(color1.red() < color2.red()|| color1.blue() > color2.blue()) {
  292. servo2.setPosition(1);
  293. sleep(500);
  294. servo2.setPosition(0);
  295. sleep(500);
  296. servo2.setPosition(1);
  297. }
  298.  
  299. else if (color1.red() > color2.red() || color2.blue() > color1.blue()) {
  300. servo1.setPosition(0);
  301. sleep(500);
  302. servo1.setPosition(1);
  303. sleep(500);
  304. servo1.setPosition(0);
  305. }
  306. sleep(500);
  307. servo1.setPosition(1);
  308. servo2.setPosition(0);
  309. }
  310.  
  311. public void MLG360quickscopeNOSCOPEbillythekidmtndewDORITOSBasti0nMaIn(boolean dank) {
  312. if(!dank) {
  313. motor1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  314. motor2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  315.  
  316. motor1.setPower(0.75);
  317. motor2.setPower(-0.75);
  318. }
  319. }
  320.  
  321. private int degreesToPositions(int degrees){
  322. return degrees*21;
  323. }
  324. private int inchesToPositions(double inches) {
  325. return (int) Math.round(inches*280/Math.PI);
  326. }
  327.  
  328. public boolean seesLineBCK()
  329. {
  330. motor1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  331. motor2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  332.  
  333. motor1.setPower(leftLinePowerBCK);
  334. motor2.setPower(RightLinePowerBCK);
  335.  
  336. if (motor1.getPower() == 0 && motor2.getPower() == 0) //only if both see the line
  337. {
  338. motor1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  339. motor2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  340. motor1.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  341. motor2.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  342. leftLinePowerBCK = -0.2;
  343. RightLinePowerBCK = -0.2;
  344. return true;
  345. }
  346.  
  347. else if (seesLineLeft()) //runs left sensor function
  348. {
  349. leftLinePowerBCK = 0;
  350. motor1.setPower(0);
  351. return false;
  352. }
  353.  
  354. else if (seesLineRight()) //runs right sensor function
  355. {
  356. RightLinePowerBCK = 0;
  357. motor2.setPower(0);
  358. return false;
  359. }
  360. return false;
  361. }
  362. public boolean seesLineFWD()
  363. {
  364. motor1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  365. motor2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  366.  
  367. motor1.setPower(leftLinePowerFWD);
  368. motor2.setPower(RightLinePowerFWD);
  369.  
  370. if (motor1.getPower() == 0 && motor2.getPower() == 0) //only if both see the line
  371. {
  372. motor1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  373. motor2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  374. motor1.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  375. motor2.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  376. return true;
  377. }
  378.  
  379. else if (seesLineLeft()) //runs left sensor function
  380. {
  381. leftLinePowerFWD = 0;
  382. motor1.setPower(0);
  383. return false;
  384. }
  385.  
  386. else if (seesLineRight()) //runs right sensor function
  387. {
  388. RightLinePowerFWD = 0;
  389. motor2.setPower(0);
  390. return false;
  391. }
  392. return false;
  393. }
  394.  
  395. public void resetEncoders()
  396. {
  397. motor1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  398. motor2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  399. }
  400. }
  401.  
  402.  
  403. Below is our red side autonomous code.
  404.  
  405. package org.firstinspires.ftc.teamcode;
  406.  
  407. import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
  408. import com.qualcomm.robotcore.hardware.DcMotor;
  409.  
  410. /**
  411. * Created by Andrew on 1/17/2017.
  412. */
  413.  
  414. @Autonomous
  415. public class RedAutonomous extends BaseAutonomous {
  416. @Override
  417. public void runOpMode() throws InterruptedException {
  418. initAll();
  419. waitForStart();
  420. Vader.start();
  421. motor3.setPower(0.8); //Shooting motor
  422. sleep(1600);
  423. motor3.setPower(0); //Stop shooting
  424. optical1.enableLed(true);
  425. turn(M1T1, M2T1, PWRT1); //45 deg turn
  426.  
  427. move(PM1, PWRM1); //runs to the wall
  428. move(PM3, PWRM3);
  429. while(optical1.getLightDetected() < 0.06 && opModeIsActive()) {
  430. motor1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  431. motor2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  432. motor1.setPower(0.2);
  433. motor2.setPower(0.2);
  434. }
  435. motor1.setPower(0);
  436. motor2.setPower(0);
  437. motor1.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  438. motor2.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  439. turn(M1T2, M2T2, PWRT2); //turns parallel to the wall
  440. resetEncoders();
  441. while(!seesLineBCK() && opModeIsActive()); // runs until it detects a line
  442. pushBeaconRed(); //hits beacons
  443. move(PM4,PWRM4);
  444. turn(M1T25, M2T25, PWRT25);
  445. resetEncoders();
  446. while(!seesLineBCK() && opModeIsActive());
  447. pushBeaconRed(); //hits beacons
  448. move(PM5, PWRM5);
  449. turn(M1T3,M2T3,PWRT3); //turns towards cap ball
  450. //turn(M1T4,M2T4, PWRT4);
  451. move(PM6,PWRM6); //runs into cap ball, knocks off the ball and parks
  452. Vader.stop();
  453. }
  454. }
  455.  
  456. Everything does what it's supposed to, but it's just that seesline function that is strange, because other parts of our code have the motors running with no issue at that power.
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!