API tools faq
paste
Advertisement
Mikkel_Serrantes

Untitled

Mikkel_Serrantes
Dec 5th, 2019
246
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 27.90 KB | None | 0 0
raw download clone embed print report
  1. #include "robot-config.h"
  2. #include <cmath>
  3. #include "math.h"
  4.  
  5. enum class TurnDir { Left, Right };
  6. enum class StartSide { Blue, Red };
  7. enum class AutoRoutine { Routine1, Routine2 };
  8.  
  9. int count = 0;
  10. int target = 0;
  11. bool driveMode = false;
  12. int PreviousErrorLeft = 0;
  13. int PreviousErrorRight = 0;
  14. int PreviousErrorArm =0;
  15. vex::digital_in AutoStartSide = vex::digital_in(Brain.ThreeWirePort.B); //Top
  16. vex::digital_in Routine = vex::digital_in(Brain.ThreeWirePort.C); //Bottom
  17.  
  18. vex::competition Competition;
  19.  
  20. /*---------------------------------------------------------------------------*/
  21. void pre_auton(void) {
  22. }
  23.  
  24. /*---------------------------------------------------------------------------*/
  25. /* */
  26. /* Autonomous Task */
  27. /* */
  28. /* This task is used to control your robot during the autonomous phase of */
  29. /* a VEX Competition. */
  30. /* */
  31. /* You must modify the code to add your own robot specific commands here. */
  32. /*---------------------------------------------------------------------------*/
  33.  
  34.  
  35. void RightMotorFB(double speed) {
  36. RightMotorF.spin(directionType::fwd,speed,velocityUnits::rpm);
  37. RightMotorB.spin(directionType::fwd,speed,velocityUnits::rpm);
  38. }
  39.  
  40. void LeftMotorFB(double speed) {
  41. LeftMotorF.spin(directionType::fwd,speed,velocityUnits::rpm);
  42. LeftMotorB.spin(directionType::fwd,speed,velocityUnits::rpm);
  43. }
  44.  
  45. void slop(int distance) {
  46. if (distance < 0) {
  47. LeftMotorFB(-40);
  48.  
  49. vex::task::sleep(35);
  50. }
  51. }
  52.  
  53.  
  54.  
  55. const int accel_step = 18;
  56. const int deccel_step = 100; // no decel slew
  57. static int leftSpeed = 0;
  58. static int rightSpeed = 0;
  59.  
  60. void LeftMotorFBS(int leftTarget){
  61. int step;
  62.  
  63. if(abs(leftSpeed) < abs(leftTarget))
  64. step = accel_step;
  65. else
  66. step = deccel_step;
  67.  
  68. if(leftTarget > leftSpeed + step)
  69. leftSpeed += step;
  70. else if(leftTarget < leftSpeed - step)
  71. leftSpeed -= step;
  72. else
  73. leftSpeed = leftTarget;
  74.  
  75. LeftMotorFB(leftSpeed);
  76. }
  77.  
  78. //slew control
  79. void RightMotorFBS(int rightTarget){
  80. int step;
  81.  
  82. if(abs(rightSpeed) < abs(rightTarget))
  83. step = accel_step;
  84. else
  85. step = deccel_step;
  86.  
  87. if(rightTarget > rightSpeed + step)
  88. rightSpeed += step;
  89. else if(rightTarget < rightSpeed - step)
  90. rightSpeed -= step;
  91. else
  92. rightSpeed = rightTarget;
  93.  
  94. RightMotorFB(rightSpeed);
  95. }
  96.  
  97.  
  98. int drivePos() {
  99. return (LeftMotorF.rotation(rotationUnits::deg) + RightMotorF.rotation(rotationUnits::deg))/2;
  100. }
  101. bool isDriving() {
  102. return(LeftMotorF.isSpinning() || LeftMotorB.isSpinning() || RightMotorF.isSpinning() || RightMotorB.isSpinning());
  103. }
  104.  
  105. void PidBasicAsync(int distance) {
  106. slop(distance);
  107. count = 0;
  108. target = distance;
  109. driveMode = true;
  110. LeftMotorF.resetRotation();
  111. RightMotorF.resetRotation();
  112. LeftMotorB.resetRotation();
  113. RightMotorB.resetRotation();
  114. PreviousErrorLeft = 0;
  115. PreviousErrorRight = 0;
  116. LeftMotorFBS(distance);
  117. RightMotorFBS(distance);
  118.  
  119. }
  120.  
  121. void BigArm (int degrees) {
  122.  
  123. while(true) {
  124.  
  125. double kp = 2.2;////1.2
  126.  
  127. int LiftPos= P2.value(rotationUnits::deg);
  128.  
  129. int power = (LiftPos - degrees) * kp + 10;//////+10
  130.  
  131. LiftPos= P2.value(rotationUnits::deg);
  132.  
  133. if (LiftPos <= 37) { ///starts at 114 to 44/// /////37
  134.  
  135. Arm2000.stop(brakeType::brake);
  136. break;
  137.  
  138. } else {
  139.  
  140. Arm2000.spin(directionType::rev, power, velocityUnits::pct);
  141. }
  142. }
  143. }
  144.  
  145. /*
  146.  
  147. int LiftPos1=Arm2000.rotation(rotationUnits::deg);
  148.  
  149. int ArmError = Arm - power;
  150.  
  151. Arm=Arm2000.rotation(rotationUnits::deg);
  152.  
  153. int ArmProError = Kp * ArmError;
  154.  
  155. PreviousErrorArm = ;
  156.  
  157.  
  158. if (abs(ArmError) < 10 {
  159. break;
  160. }
  161.  
  162. */
  163.  
  164.  
  165.  
  166. void PidBasic (int distance) {
  167. if (driveMode) {
  168.  
  169. double Kp = 0.16192;///0.2 or .215 .162
  170. double Kd = .4995;//465 or .52 or .45
  171.  
  172.  
  173. int Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
  174. int Rightdegrees=RightMotorF.rotation(rotationUnits::deg);
  175.  
  176. int ErrorLeft = distance - Leftdegrees;
  177. int ErrorRight = distance - Rightdegrees;
  178.  
  179. Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
  180. Rightdegrees=RightMotorF.rotation(rotationUnits::deg);
  181.  
  182. ErrorLeft = distance - Leftdegrees;
  183. ErrorRight = distance - Rightdegrees;
  184.  
  185. int LeftsideProportionalError = Kp * ErrorLeft;
  186. int RightsideProportionalError = Kp * ErrorRight;
  187.  
  188. double LeftsidedDerivativeError = Kd * (ErrorLeft - PreviousErrorLeft);
  189. double RightsidedDerivativeError = Kd * (ErrorRight - PreviousErrorRight);
  190.  
  191. LeftMotorFB(LeftsideProportionalError + LeftsidedDerivativeError);
  192. RightMotorFB(RightsideProportionalError + RightsidedDerivativeError);
  193.  
  194. PreviousErrorLeft = ErrorLeft;
  195. PreviousErrorRight = ErrorRight;
  196.  
  197. /*
  198. if (abs(ErrorLeft) < 6 && abs(ErrorRight) < 6) {
  199. break;
  200. }
  201. */
  202.  
  203.  
  204. if (!isDriving()) {
  205. count++;
  206. }
  207. if (count > 30) {
  208. driveMode = false;
  209. }
  210.  
  211.  
  212. vex::task::sleep(30);
  213. }
  214. }
  215.  
  216. void Turning (int distance) {
  217. LeftMotorF.resetRotation();
  218. RightMotorF.resetRotation();
  219. LeftMotorB.resetRotation();
  220. RightMotorB.resetRotation();
  221. float Kp = .395; //// best.17 //// ..30 ///4
  222. float Kd = .4;/// best .35 //// .34 ////next best .32 next best// 25 //smallest movement.1 ///.395
  223.  
  224. int Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
  225. int Rightdegrees=RightMotorF.rotation(rotationUnits::deg);
  226.  
  227. int ErrorLeft = -1*Leftdegrees - distance;
  228. int ErrorRight = distance - Rightdegrees;
  229.  
  230. int PreviousErrorLeft = 0;
  231. int PreviousErrorRight = 0;
  232.  
  233. while(1) {
  234. Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
  235. Rightdegrees=RightMotorF.rotation(rotationUnits::deg);
  236.  
  237. ErrorLeft = -1*Leftdegrees - distance;
  238. ErrorRight = distance - Rightdegrees;
  239.  
  240. int LeftsideProportionalError = Kp * ErrorLeft;
  241. int RightsideProportionalError = Kp * ErrorRight;
  242.  
  243. double LeftsidedDerivativeError = Kd * (ErrorLeft - PreviousErrorLeft);
  244. double RightsidedDerivativeError = Kd * (ErrorRight - PreviousErrorRight);
  245.  
  246. LeftMotorFB(LeftsideProportionalError + LeftsidedDerivativeError);
  247. RightMotorFB(RightsideProportionalError + RightsidedDerivativeError);
  248.  
  249. PreviousErrorLeft = ErrorLeft;
  250. PreviousErrorRight = ErrorRight;
  251. if (abs(ErrorLeft) < 10 && abs(ErrorRight) < 10) {
  252. LeftMotorFB(0);
  253. RightMotorFB(0);
  254. break;
  255. }
  256. vex::task::sleep(30);
  257. }
  258. }
  259.  
  260.  
  261. void PidFoward (int distance) {
  262. LeftMotorF.resetRotation();
  263. RightMotorF.resetRotation();
  264. LeftMotorB.resetRotation();
  265. RightMotorB.resetRotation();
  266.  
  267.  
  268. double Kp = 0.265;///0.2 or .215 //.3 ////0.25
  269. double Kd = .0255; ////.275 or .0325 //.5 ///4375 //////0.438109 ///.7 .54 ////290
  270.  
  271. int Leftdegrees=LeftMotorB.rotation(rotationUnits::deg);
  272. int Rightdegrees=RightMotorB.rotation(rotationUnits::deg);
  273.  
  274. int ErrorLeft = distance - Leftdegrees;
  275. int ErrorRight = distance - Rightdegrees;
  276.  
  277. int PreviousErrorLeft = 0;
  278. int PreviousErrorRight = 0;
  279.  
  280. while(1) {
  281.  
  282. Leftdegrees=LeftMotorB.rotation(rotationUnits::deg);
  283. Rightdegrees=RightMotorB.rotation(rotationUnits::deg);
  284.  
  285. ErrorLeft = distance - Leftdegrees;
  286. ErrorRight = distance - Rightdegrees;
  287.  
  288. int LeftsideProportionalError = Kp * ErrorLeft;
  289. int RightsideProportionalError = Kp * ErrorRight;
  290.  
  291. double LeftsidedDerivativeError = Kd * (ErrorLeft - PreviousErrorLeft);
  292. double RightsidedDerivativeError = Kd * (ErrorRight - PreviousErrorRight);
  293.  
  294. int leftPower = LeftsideProportionalError + LeftsidedDerivativeError;
  295. int rightPower = RightsideProportionalError + RightsidedDerivativeError;
  296.  
  297.  
  298. int velocitydiff = Leftdegrees - Rightdegrees;
  299. rightPower = leftPower;
  300. leftPower -= (velocitydiff*0.0); //////0.7 ////0.6 is nice////.62955///.62955 good///
  301. LeftMotorFB(leftPower);
  302. RightMotorFB(rightPower);
  303.  
  304. PreviousErrorLeft = ErrorLeft;
  305. PreviousErrorRight = ErrorRight;
  306.  
  307.  
  308. if (abs(ErrorLeft) < 10 && abs(ErrorRight) < 10) {
  309.  
  310. LeftMotorFB(0);
  311. RightMotorFB(0);
  312.  
  313.  
  314. break;
  315. }
  316. vex::task::sleep(20);
  317.  
  318. }
  319. }
  320.  
  321.  
  322.  
  323. /////////////////////////////////////////////////////////////////////////////////////////////
  324.  
  325. void StopMotor(vex::brakeType type = brakeType::coast){
  326. LeftMotorF.stop(type);
  327. RightMotorF.stop(type);
  328. LeftMotorB.stop(type);
  329. RightMotorB.stop(type);
  330. }
  331. void SetMotorBrakingType(brakeType type = brakeType::coast) {
  332. LeftMotorF.setStopping(type);
  333. RightMotorF.setStopping(type);
  334. LeftMotorB.setStopping(type);
  335. RightMotorB.setStopping(type);
  336. }
  337. int CubeIntake2() {
  338. int timeout = 4000;
  339. Scooper.setStopping(brakeType::coast);
  340. int sleepTimeout = 20;
  341. Scooper.spin(directionType::rev, 100, velocityUnits::pct);
  342. Scooper.spin(directionType::rev, 100, velocityUnits::pct);
  343.  
  344. while (timeout > 0) {
  345. task::sleep(sleepTimeout);
  346. timeout = timeout - sleepTimeout;
  347. }
  348.  
  349. Scooper.stop(brakeType::coast);
  350.  
  351. return(0);
  352. }
  353. int CubeIntake() {
  354. int timeout = 4000;
  355. Scooper.setStopping(brakeType::coast);
  356. Scooper2.setStopping(brakeType::coast);
  357. int sleepTimeout = 20;
  358. Scooper.spin(directionType::fwd, 100, velocityUnits::pct);
  359. Scooper2.spin(directionType::fwd, 100, velocityUnits::pct);
  360.  
  361. while (timeout > 0) {
  362. task::sleep(sleepTimeout);
  363. timeout = timeout - sleepTimeout;
  364. }
  365. Scooper.stop(brakeType::coast);
  366. Scooper2.stop(brakeType::coast);
  367. return(0);
  368. }
  369. int AutoBallIntakeTakeBall(int timeout = 5000) {
  370.  
  371. Scooper.setStopping(brakeType::coast);
  372. int sleepTimeout = 20;
  373. Scooper.spin(directionType::fwd, 60, velocityUnits::pct);
  374. while (timeout > 0) {
  375. task::sleep(sleepTimeout);
  376. timeout = timeout - sleepTimeout;
  377. }
  378.  
  379. Scooper.stop(brakeType::coast);
  380.  
  381. return(0);
  382. }
  383.  
  384. void Drive(double numOfRevs, int velocity = 50, bool fwd = true, int timeout = 5000)
  385. {
  386. Brain.Screen.printAt(1, 40, "numofrevs: %f, vel: %f", numOfRevs, velocity);
  387. SetMotorBrakingType();
  388.  
  389. int sleepTimeout = 20;
  390.  
  391. if (fwd == false) {
  392. numOfRevs = numOfRevs * -1;
  393. }
  394.  
  395. LeftMotorF.startRotateFor(numOfRevs, rotationUnits::rev, velocity, velocityUnits::pct);
  396. RightMotorF.startRotateFor(numOfRevs, rotationUnits::rev, velocity, velocityUnits::pct);
  397. LeftMotorB.startRotateFor(numOfRevs, rotationUnits::rev, velocity, velocityUnits::pct);
  398. RightMotorB.startRotateFor(numOfRevs, rotationUnits::rev, velocity, velocityUnits::pct);
  399.  
  400. while(LeftMotorF.isSpinning()||RightMotorF.isSpinning()||
  401. LeftMotorB.isSpinning()||RightMotorB.isSpinning()) {
  402.  
  403. if (timeout <= 0) {
  404. break;
  405. }
  406. task::sleep(sleepTimeout);
  407. timeout = timeout - sleepTimeout;
  408. }
  409.  
  410. StopMotor();
  411. }
  412.  
  413. void DriveInCM(double distanceInCM, int velocity, bool fwd = true, int timeout = 5000) {
  414. // double gearRatio = 1.67;
  415. // double wheelDiameter = 10.16;
  416. // double circumference = wheelDiameter * M_PI;
  417. // double numOfRevs = (distanceInCM)/ (circumference * gearRatio);
  418. double numOfRevs = distanceInCM/47.9;////old 53.2
  419. Drive(numOfRevs, velocity, fwd, timeout);
  420. }
  421.  
  422. void Turn(double degrees, TurnDir turnDir, int velocity = 25, int timeout = 5000){
  423. int sleepTimeout = 10;
  424.  
  425. SetMotorBrakingType();
  426. LeftMotorF.resetRotation();
  427. RightMotorF.resetRotation();
  428.  
  429. if (turnDir == TurnDir::Right) {
  430. LeftMotorF.spin(directionType::fwd, velocity, velocityUnits::pct);
  431. LeftMotorB.spin(directionType::fwd, velocity, velocityUnits::pct);
  432.  
  433. RightMotorF.spin(directionType::rev, velocity, velocityUnits::pct);
  434. RightMotorB.spin(directionType::rev, velocity, velocityUnits::pct);
  435. }
  436. else
  437. {
  438. LeftMotorF.spin(directionType::rev, velocity, velocityUnits::pct);
  439. LeftMotorB.spin(directionType::rev, velocity, velocityUnits::pct);
  440.  
  441. RightMotorF.spin(directionType::fwd, velocity, velocityUnits::pct);
  442. RightMotorB.spin(directionType::fwd, velocity, velocityUnits::pct);
  443. }
  444. while(true) {
  445.  
  446. double flmDeg = std::abs(LeftMotorF.rotation(rotationUnits::deg));
  447. double frmDeg = std::abs(RightMotorF.rotation(rotationUnits::deg));
  448. Brain.Screen.clearScreen();
  449. Brain.Screen.printAt(1, 20, "FL: %f FR: %f", flmDeg, frmDeg);
  450.  
  451. if (frmDeg > degrees) {
  452. Brain.Screen.printAt(1, 40, "Deg Break: FL: %f FR: %f", flmDeg, frmDeg);
  453. break;
  454. }
  455.  
  456. if (timeout <= 0) {
  457. Brain.Screen.printAt(1, 40, "Timeout Break: FL: %f FR: %f", flmDeg, frmDeg);
  458. break;
  459. }
  460.  
  461. task::sleep(sleepTimeout);
  462. timeout = timeout - sleepTimeout;
  463. }
  464.  
  465. StopMotor(brakeType::coast);
  466. }
  467.  
  468. void TurnRight90Degrees()
  469. {
  470. Turn(85, TurnDir::Right);
  471. }
  472.  
  473. void TurnLeft90Degrees()
  474. {
  475. Turn(85, TurnDir::Left);
  476. }
  477.  
  478. /////////////////////////////////////////////////////////////////////////////////
  479. void LiftPosition1 () {
  480.  
  481. int timeout = 5000;
  482.  
  483. Arm2000.setStopping(brakeType::coast);
  484.  
  485. int sleepTimeout = 30;
  486. while (timeout > 0) {
  487.  
  488. Brain.Screen.clearScreen();
  489.  
  490. float LiftPos = P2.value(rotationUnits::deg);
  491.  
  492. Brain.Screen.printAt(2, 40, "LiftPos: %f", LiftPos);
  493.  
  494. if (LiftPos == 0)
  495. {
  496. task::sleep(sleepTimeout);
  497. continue;
  498. }
  499.  
  500. // Lowest position for launch
  501. if (LiftPos >= 107)
  502. {
  503. Brain.Screen.printAt(2, 80, "Stopped: %f", LiftPos);
  504. break;
  505. }
  506.  
  507. Arm2000.spin(directionType::fwd, 100, velocityUnits::pct);
  508.  
  509. task::sleep(sleepTimeout);
  510.  
  511. timeout = timeout - sleepTimeout;
  512.  
  513. Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
  514. }
  515.  
  516. Arm2000.stop(brakeType::coast);
  517. }
  518.  
  519. void LiftPosition2() {
  520.  
  521. int timeout = 5000;
  522.  
  523. Arm2000.setStopping(brakeType::coast);
  524.  
  525. int sleepTimeout = 30;
  526.  
  527. while (timeout > 0) {
  528.  
  529. Brain.Screen.clearScreen();
  530.  
  531. float LiftPos = P2.value(rotationUnits::deg);
  532.  
  533. Brain.Screen.printAt(2, 40, "LiftPos: %f", LiftPos);
  534.  
  535. if (LiftPos == 0)
  536. {
  537. task::sleep(sleepTimeout);
  538. continue;
  539. }
  540.  
  541. // load position
  542. if (LiftPos <= 44)
  543. {
  544. break;
  545. }
  546.  
  547.  
  548. /*
  549. double kp = 0.1;
  550. double target = 1000;
  551.  
  552. int power = (target - LiftPos) * kp + 10;;
  553.  
  554.  
  555. */
  556. Arm2000.spin(directionType::rev, 45, velocityUnits::pct);
  557.  
  558. task::sleep(sleepTimeout);
  559.  
  560. timeout = timeout - sleepTimeout;
  561. Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
  562. }
  563.  
  564. Arm2000.stop(brakeType::hold);
  565. }
  566.  
  567.  
  568. void LiftPosition3() {
  569. int timeout = 2500;
  570.  
  571. Arm2000.setStopping(brakeType::coast);
  572.  
  573. int sleepTimeout = 30;
  574.  
  575. while (timeout > 0) {
  576.  
  577.  
  578.  
  579.  
  580. Brain.Screen.clearScreen();
  581.  
  582. float LiftPos = P2.value(rotationUnits::deg);
  583.  
  584. Brain.Screen.printAt(2, 40, "LiftPos: %f", LiftPos);
  585.  
  586.  
  587. if(P2.value(rotationUnits::deg) <44) {
  588. while(P2.value(rotationUnits::deg) <44) { ///////////13
  589. Arm2000.spin(vex::directionType::fwd, 45, vex::velocityUnits::pct);
  590.  
  591. }
  592.  
  593. break;
  594.  
  595. }
  596.  
  597. else if
  598. (P2.value(rotationUnits::deg) > 45) {
  599. while(P2.value(rotationUnits::deg) > 45) { /////////16
  600. Arm2000.spin(vex::directionType::rev, 45, vex::velocityUnits::pct);
  601. }
  602.  
  603. break;
  604.  
  605. }
  606.  
  607.  
  608.  
  609. Arm2000.spin(directionType::fwd, 40, velocityUnits::pct);
  610.  
  611. task::sleep(sleepTimeout);
  612.  
  613. timeout = timeout - sleepTimeout;
  614. Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
  615. }
  616.  
  617. Arm2000.stop(brakeType::coast);
  618. }
  619. ///////////////////////////////////////////////??///////////////////////
  620.  
  621. void Lift10() {
  622. int timeout = 2500;
  623.  
  624. Lift.setStopping(brakeType::hold);
  625.  
  626.  
  627. int sleepTimeout = 30;
  628.  
  629. while (timeout > 0) {
  630.  
  631. Brain.Screen.clearScreen();
  632.  
  633. float LiftCube = P1.value(rotationUnits::deg);
  634.  
  635.  
  636. Brain.Screen.printAt(2, 40, "LiftCube %f", LiftCube);
  637.  
  638.  
  639. if (LiftCube >= 87)
  640. {
  641.  
  642. Brain.Screen.printAt(2, 80, "Stopped: %f", LiftCube);
  643. break;
  644. }
  645.  
  646.  
  647. Lift.spin(directionType::rev, 127, velocityUnits::pct);
  648.  
  649. task::sleep(sleepTimeout);
  650.  
  651. timeout = timeout - sleepTimeout;
  652. Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
  653. }
  654.  
  655. Lift.stop(brakeType::hold);
  656.  
  657. }
  658.  
  659. void Lift11() {
  660. int timeout = 2500;
  661.  
  662. Lift.setStopping(brakeType::hold);
  663.  
  664.  
  665. int sleepTimeout = 30;
  666.  
  667. while (timeout > 0) {
  668.  
  669. Brain.Screen.clearScreen();
  670.  
  671. float LiftCube = P1.value(rotationUnits::deg);
  672.  
  673.  
  674. Brain.Screen.printAt(2, 40, "LiftCube %f", LiftCube);
  675.  
  676.  
  677.  
  678. if (LiftCube >= 67)
  679. {
  680.  
  681. Brain.Screen.printAt(2, 80, "Stopped: %f", LiftCube);
  682. break;
  683. }
  684.  
  685.  
  686. Lift.spin(directionType::rev, 127, velocityUnits::pct);
  687.  
  688. task::sleep(sleepTimeout);
  689.  
  690. timeout = timeout - sleepTimeout;
  691. Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
  692. }
  693.  
  694. Lift.stop(brakeType::hold);
  695.  
  696. }
  697. void Lift12() {
  698. int timeout = 2500;
  699.  
  700. Lift.setStopping(brakeType::coast);
  701.  
  702.  
  703. int sleepTimeout = 30;
  704.  
  705. while (timeout > 0) {
  706.  
  707. Brain.Screen.clearScreen();
  708.  
  709. float LiftCube = P1.value(rotationUnits::deg);
  710.  
  711. Brain.Screen.printAt(2, 40, "LiftCube %f", LiftCube);
  712.  
  713.  
  714. if(P1.value(rotationUnits::deg) <11) {
  715. while(P1.value(rotationUnits::deg) <11) { ///////////13
  716. Lift.spin(vex::directionType::rev, 127, vex::velocityUnits::pct);
  717.  
  718. }
  719.  
  720. break;
  721.  
  722. }
  723.  
  724. else
  725.  
  726. if(P1.value(rotationUnits::deg) > 12) {
  727. while(P1.value(rotationUnits::deg) > 12) { /////////16
  728. Lift.spin(vex::directionType::fwd, 100, vex::velocityUnits::pct);
  729. }
  730.  
  731. break;
  732.  
  733. }
  734.  
  735.  
  736. Lift.spin(directionType::rev, 100, velocityUnits::pct);
  737.  
  738. task::sleep(sleepTimeout);
  739.  
  740. timeout = timeout - sleepTimeout;
  741. Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
  742. }
  743.  
  744.  
  745. Lift.stop(brakeType::coast);
  746. }
  747. //////////////
  748. /*
  749. void CubeTracker() {
  750. int timeout = 2500;
  751.  
  752. Scooper.setStopping(brakeType::coast);
  753. Scooper2.setStopping(brakeType::coast);
  754.  
  755. int sleepTimeout = 30;
  756.  
  757. while (timeout > 0) {
  758.  
  759.  
  760. if(CubeDetector.value() <1000) {
  761. while(P1.value() <1000) { ///////////13
  762. Scooper.spin(vex::directionType::rev, 127, vex::velocityUnits::pct);
  763. Scooper2.spin(vex::directionType::rev, 127, vex::velocityUnits::pct);
  764.  
  765. }
  766.  
  767. break;
  768.  
  769. }
  770.  
  771. else if {
  772.  
  773.  
  774.  
  775. }
  776.  
  777.  
  778.  
  779. Lift.spin(directionType::rev, 100, velocityUnits::pct);
  780.  
  781. task::sleep(sleepTimeout);
  782.  
  783. timeout = timeout - sleepTimeout;
  784. Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
  785. }
  786.  
  787.  
  788. Lift.stop(brakeType::coast);
  789. }
  790. */
  791.  
  792. /////////////////////////
  793. void AutoRoutine1(StartSide startSide)
  794. {
  795.  
  796. Scooper.spin(directionType::fwd, 100, velocityUnits::pct);
  797. Scooper2.spin(directionType::fwd, 100, velocityUnits::pct);
  798.  
  799. PidFoward(792);
  800.  
  801.  
  802. Scooper.stop();
  803. Scooper2.stop();
  804.  
  805.  
  806.  
  807. return;
  808.  
  809.  
  810.  
  811. }
  812.  
  813.  
  814. void CheckLiftPotValues() {
  815.  
  816. while (1)
  817. {
  818. Brain.Screen.clearScreen();
  819. Brain.Screen.printAt(1, 30, "CheckLiftPot");
  820. float LiftPos = P2.value(rotationUnits::deg);
  821. Brain.Screen.printAt(1, 20, "rotation: %f degrees", LiftPos);
  822.  
  823. //Sleep the task for a short amount of time to prevent wasted resources.
  824. task::sleep(20);
  825. }
  826. }
  827.  
  828.  
  829.  
  830. ///////////////////////////
  831. void AutoRoutine2(StartSide startSide)
  832. {
  833.  
  834. Scooper.spin(directionType::rev, 100, velocityUnits::pct);
  835. Scooper2.spin(directionType::rev, 100, velocityUnits::pct);
  836.  
  837. vex::task::sleep(250);
  838.  
  839. Scooper.stop();
  840. Scooper2.stop();
  841.  
  842.  
  843. Scooper.spin(directionType::fwd, 100, velocityUnits::pct);
  844. Scooper2.spin(directionType::fwd, 100, velocityUnits::pct);
  845.  
  846. vex::task::sleep(250);
  847.  
  848. DriveInCM(100, 30);///100
  849.  
  850. DriveInCM(15, 35);
  851.  
  852.  
  853. vex::task::sleep(100);
  854.  
  855. DriveInCM(-77, 35); ///51
  856.  
  857. LeftMotorF.setStopping(brakeType::brake);
  858. LeftMotorB.setStopping(brakeType::brake);
  859. RightMotorF.setStopping(brakeType::brake);
  860. RightMotorB.setStopping(brakeType::brake);
  861.  
  862. LeftMotorF.resetRotation();
  863. RightMotorF.resetRotation();
  864. LeftMotorB.resetRotation();
  865. RightMotorB.resetRotation();
  866.  
  867. vex::task::sleep(100);
  868.  
  869. Scooper.stop();
  870. Scooper2.stop();
  871.  
  872.  
  873. if ( startSide == StartSide::Blue)
  874. {
  875. Turning(237);
  876. }
  877. else
  878. {
  879. Turning(-237);
  880. }
  881.  
  882. vex::task::sleep(50);
  883.  
  884. LeftMotorF.resetRotation();
  885. RightMotorF.resetRotation();
  886. LeftMotorB.resetRotation();
  887. RightMotorB.resetRotation();
  888.  
  889. DriveInCM(35, 33); ///
  890.  
  891. LeftMotorF.setStopping(brakeType::coast);
  892. LeftMotorB.setStopping(brakeType::coast);
  893. RightMotorF.setStopping(brakeType::coast);
  894. RightMotorB.setStopping(brakeType::coast);
  895.  
  896. Scooper.spin(directionType::rev, 30, velocityUnits::pct);
  897. Scooper2.spin(directionType::rev, 30, velocityUnits::pct);
  898.  
  899. vex::task::sleep(450);
  900.  
  901. Scooper.stop();
  902. Scooper2.stop();
  903.  
  904. vex::task::sleep(250);
  905.  
  906. BigArm(37);
  907.  
  908. Arm2000.stop(brakeType::brake);
  909.  
  910.  
  911. vex::task::sleep(50);
  912.  
  913. DriveInCM(-20, 30);
  914.  
  915.  
  916. }
  917.  
  918. void autonomous(void) {
  919. int startSideValue = AutoStartSide.value();
  920. StartSide startSide;
  921.  
  922. if (startSideValue == 1)
  923. {
  924. startSide = StartSide::Blue;
  925. }
  926. else
  927. {
  928. startSide = StartSide::Red;
  929. }
  930.  
  931. int routineValue = Routine.value();
  932. AutoRoutine routine;
  933.  
  934. // no pin
  935. if (routineValue == 1)
  936. {
  937.  
  938. routine = AutoRoutine::Routine1;
  939. }
  940. else
  941. {
  942.  
  943. routine = AutoRoutine::Routine2;
  944. }
  945.  
  946. if (routine == AutoRoutine::Routine1)
  947. {
  948.  
  949. AutoRoutine1(startSide);
  950. }
  951. else
  952. {
  953. AutoRoutine2(startSide);
  954. }
  955.  
  956. return;
  957. }
  958.  
  959. int DriveTask() {
  960. while (true) {
  961. if (driveMode) {
  962. PidBasic(target);
  963. }
  964. }
  965. }
  966.  
  967. int LiftTask()
  968. {
  969. while (true) {
  970.  
  971. Brain.Screen.clearScreen();
  972. float LiftPos = P2.value(rotationUnits::deg);
  973.  
  974. Brain.Screen.printAt(1, 40, "LiftPos: %f", LiftPos);
  975.  
  976. if (Controller1.ButtonY.pressing()) {
  977. // Load
  978. LiftPosition1();
  979. }
  980.  
  981. else if (Controller1.ButtonB.pressing()) {
  982. // Launch
  983. BigArm(37);
  984. } else {
  985. Arm2000.stop(brakeType::brake);
  986. }
  987.  
  988. if (Controller2.ButtonL1.pressing()) {
  989. // Launch
  990. Lift10();
  991. }
  992.  
  993. else if (Controller2.ButtonL2.pressing()) {
  994. // Launch
  995. Lift11();
  996. }
  997.  
  998. else if (Controller2.ButtonDown.pressing()) {
  999. // Launch
  1000. Lift12();
  1001. }
  1002.  
  1003.  
  1004.  
  1005.  
  1006.  
  1007. task::sleep(30);
  1008. }
  1009. return (0);
  1010. }
  1011.  
  1012. /*----------------------------------------------------------------------------*/
  1013. /* */
  1014. /* User Control Task */
  1015. /* */
  1016. /* This task is used to control your robot during the user control phase of */
  1017. /* a VEX Competition. */
  1018. /* */
  1019. /* You must modify the code to add your own robot specific commands here. */
  1020. /*----------------------------------------------------------------------------*/
  1021. void usercontrol(void) {
  1022.  
  1023.  
  1024. task CallLiftTask(LiftTask);
  1025.  
  1026.  
  1027. while (true) {
  1028.  
  1029.  
  1030. RightMotorF.spin(directionType::fwd, Controller1.Axis2.position(percentUnits::pct), velocityUnits::pct);
  1031. RightMotorB.spin(directionType::fwd, Controller1.Axis2.position(percentUnits::pct), velocityUnits::pct);
  1032. LeftMotorF.spin(directionType::fwd, Controller1.Axis3.position(percentUnits::pct), velocityUnits::pct);
  1033. LeftMotorB.spin(directionType::fwd, Controller1.Axis3.position(percentUnits::pct), velocityUnits::pct);
  1034.  
  1035.  
  1036. LeftMotorF.setStopping(brakeType::coast);
  1037. RightMotorB.setStopping(brakeType::coast);
  1038. LeftMotorB.setStopping(brakeType::coast);
  1039. RightMotorF.setStopping(brakeType::coast);
  1040.  
  1041.  
  1042.  
  1043. if (Controller1.ButtonL2.pressing()) {
  1044. //If button up is pressed...Spin the arm motor forward
  1045. Scooper.spin(vex::directionType::fwd, 127, velocityUnits::pct);
  1046. Scooper2.spin(vex::directionType::fwd, 127, velocityUnits::pct);
  1047. }
  1048. else if (Controller1.ButtonL1.pressing()) { //If the down button is pressed...
  1049.  
  1050. Scooper.spin(vex::directionType::rev, 70, velocityUnits::pct);
  1051. Scooper2.spin(vex::directionType::rev, 70, velocityUnits::pct);
  1052. }
  1053. else { //If the the up or down button is not pressed...
  1054. Scooper.stop(brakeType::brake);
  1055. Scooper2.stop(brakeType::brake);
  1056. }
  1057.  
  1058. }
  1059. }
  1060.  
  1061.  
  1062.  
  1063.  
  1064. int main() {
  1065. ///////ARM D
  1066. Brain.Screen.clearScreen();
  1067. float LiftPos = P2.value(rotationUnits::deg);
  1068. Brain.Screen.printAt(1, 40, "LiftPos: %f", LiftPos);
  1069. //////Lift G
  1070. Brain.Screen.clearScreen();
  1071. float LiftCube = P1.value(rotationUnits::deg);
  1072. Brain.Screen.printAt(1, 40, "LiftCube: %e", LiftCube);
  1073.  
  1074. pre_auton();
  1075. Competition.autonomous(autonomous);
  1076. Competition.drivercontrol(usercontrol);
  1077.  
  1078. //Prevent main from exiting with an infinite loop.
  1079.  
  1080. while (1) {
  1081.  
  1082. vex::task::sleep(100);//Sleep the task for a short amount of time to prevent wasted resources.
  1083. }
  1084. }
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!