API tools faq
paste
Advertisement
Guest User

5203R Updated PID V.2

a guest
Jan 18th, 2020
121
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 17.21 KB | None | 0 0
raw download clone embed print report
  1. #include <stdio.h>
  2. #include <stdlib.h>
  3. #include <string.h>
  4. #include <math.h>
  5. #include "C:/Program Files (x86)/VEX Robotics/VEXcode/sdk/vexv5/include/v5_apitypes.h"
  6. #include "C:/Program Files (x86)/VEX Robotics/VEXcode/sdk/vexv5/include/v5_apiuser.h"
  7. #include "C:/Program Files (x86)/VEX Robotics/VEXcode/sdk/vexv5/include/vex_units.h"
  8. #include "v5.h"
  9. #include "v5_vcs.h"
  10. #include <cmath>
  11.  
  12. #include "vex.h"
  13.  
  14. using namespace vex;
  15.  
  16. vex::brain Brain;
  17. vex::motor FL_mtr(vex::PORT10, vex::gearSetting::ratio18_1, true);
  18. vex::motor BL_mtr(vex::PORT3, vex::gearSetting::ratio18_1, true);
  19. vex::motor FR_mtr(vex::PORT2, vex::gearSetting::ratio18_1, false);
  20. vex::motor BR_mtr(vex::PORT4, vex::gearSetting::ratio18_1, false);
  21. vex::motor Tilt(vex::PORT5, vex::gearSetting::ratio18_1, false);
  22. vex::motor Lift(vex::PORT6, vex::gearSetting::ratio18_1, false);
  23. vex::motor LeftIntake(vex::PORT8, vex::gearSetting::ratio18_1, false);
  24. vex::motor RightIntake(vex::PORT7, vex::gearSetting::ratio18_1, false);
  25. vex::line Line(Brain.ThreeWirePort.C);
  26. vex::controller (Controller1);
  27.  
  28.  
  29. vex::competition Competition;
  30.  
  31. void wait_until_settled(){
  32.  
  33. int average_velocity = 1;
  34.  
  35. while (average_velocity >= 1){
  36.  
  37. average_velocity = (FL_mtr.velocity(vex::velocityUnits::rpm) + FR_mtr.velocity(vex::velocityUnits::rpm) + BL_mtr.velocity(vex::velocityUnits::rpm) + BR_mtr.velocity(vex::velocityUnits::rpm)) / 4;
  38.  
  39. vex::task::sleep(10);
  40.  
  41. FL_mtr.setStopping(vex::brakeType::brake);
  42. FR_mtr.setStopping(vex::brakeType::brake);
  43. BL_mtr.setStopping(vex::brakeType::brake);
  44. BR_mtr.setStopping(vex::brakeType::brake);
  45.  
  46. vex::task::sleep(50);
  47.  
  48. }
  49.  
  50. FL_mtr.setStopping(vex::brakeType::coast);
  51. FR_mtr.setStopping(vex::brakeType::coast);
  52. BL_mtr.setStopping(vex::brakeType::coast);
  53. BR_mtr.setStopping(vex::brakeType::coast);
  54.  
  55. vex::task::sleep(50);
  56.  
  57. }
  58.  
  59. void drive_inches(int distance, int max_power = 90, int min_power = 35, bool wait_for_rest = true){
  60.  
  61. float error;
  62. float previous_error;
  63. float drive_power;
  64. float previous_drive_power;
  65. float distance_covered;
  66. float max_voltage = 80 * max_power;
  67. float min_voltage = 80 * min_power;
  68. const int Kp = 200;
  69. const int Kd = -100;
  70.  
  71. BL_mtr.resetRotation();
  72. BR_mtr.resetRotation();
  73.  
  74. if (distance <= 2){
  75.  
  76. distance = 3;
  77.  
  78. }
  79.  
  80. error = distance - (fabs((BL_mtr.rotation(vex::rotationUnits::deg)) + fabs(BR_mtr.rotation(vex::rotationUnits::deg)) / 2) / 20.64);
  81.  
  82. while(error > 2){
  83.  
  84. error = distance - (fabs((BL_mtr.rotation(vex::rotationUnits::deg)) + fabs(BR_mtr.rotation(vex::rotationUnits::deg)) / 2) / 20.64);
  85.  
  86.  
  87. drive_power = (error * Kp) + ((previous_error - error) * Kd);
  88.  
  89. if (distance_covered < 0.25 * distance){
  90.  
  91. drive_power = (drive_power - previous_drive_power) * 0.6;
  92.  
  93. }
  94.  
  95. if (drive_power < min_voltage && error > 2){
  96.  
  97. drive_power = min_voltage;
  98.  
  99. } else if (drive_power > min_voltage){
  100.  
  101. drive_power = max_voltage;
  102.  
  103. } else if (drive_power < min_voltage && error <= 2){
  104.  
  105. drive_power = min_voltage * 0.6;
  106.  
  107. }
  108.  
  109. FL_mtr.spin(vex::directionType::undefined, drive_power*-1, vex::voltageUnits::mV);
  110. BR_mtr.spin(vex::directionType::undefined, drive_power*-1, vex::voltageUnits::mV);
  111. BL_mtr.spin(vex::directionType::undefined, drive_power*-1, vex::voltageUnits::mV);
  112. FR_mtr.spin(vex::directionType::undefined, drive_power*-1, vex::voltageUnits::mV);
  113.  
  114.  
  115.  
  116. previous_error = error;
  117. previous_drive_power = drive_power;
  118.  
  119. }
  120.  
  121.  
  122. FL_mtr.spin(vex::directionType::undefined, 0, vex::voltageUnits::mV);
  123. BR_mtr.spin(vex::directionType::undefined, 0, vex::voltageUnits::mV);
  124. BL_mtr.spin(vex::directionType::undefined, 0, vex::voltageUnits::mV);
  125. FR_mtr.spin(vex::directionType::undefined, 0, vex::voltageUnits::mV);
  126.  
  127.  
  128. if (wait_for_rest == true){
  129.  
  130. wait_until_settled();
  131.  
  132. } else {
  133.  
  134. FL_mtr.setStopping(vex::brakeType::brake);
  135. FR_mtr.setStopping(vex::brakeType::brake);
  136. BL_mtr.setStopping(vex::brakeType::brake);
  137. BR_mtr.setStopping(vex::brakeType::brake);
  138.  
  139. vex::task::sleep(100);
  140.  
  141. FL_mtr.setStopping(vex::brakeType::coast);
  142. FR_mtr.setStopping(vex::brakeType::coast);
  143. BL_mtr.setStopping(vex::brakeType::coast);
  144. BR_mtr.setStopping(vex::brakeType::coast);
  145.  
  146. }
  147. }
  148.  
  149.  
  150.  
  151. void drive_inches_reverse(int distance, int max_power = 90, int min_power = 35, bool wait_for_rest = true){
  152.  
  153. float error;
  154. float previous_error;
  155. float drive_power;
  156. float previous_drive_power;
  157. float distance_covered;
  158. float max_voltage = 80 * max_power;
  159. float min_voltage = 80 * min_power;
  160. const int Kp = 200;
  161. const int Kd = -100;
  162.  
  163. BL_mtr.resetRotation();
  164. BR_mtr.resetRotation();
  165.  
  166. if (distance <= 2){
  167.  
  168. distance = 3;
  169.  
  170. }
  171.  
  172. error = distance - (fabs((BL_mtr.rotation(vex::rotationUnits::deg)) + fabs(BR_mtr.rotation(vex::rotationUnits::deg)) / 2) / 7);
  173.  
  174. while(error > 2){
  175.  
  176. error = distance - (fabs((BL_mtr.rotation(vex::rotationUnits::deg)) + fabs(BR_mtr.rotation(vex::rotationUnits::deg)) / 2) / 7);
  177.  
  178.  
  179. drive_power = (error * Kp) + ((previous_error - error) * Kd);
  180.  
  181. if (distance_covered < 0.25 * distance){
  182.  
  183. drive_power = (drive_power - previous_drive_power) * 0.6;
  184.  
  185. }
  186.  
  187. if (drive_power < min_voltage && error > 2){
  188.  
  189. drive_power = min_voltage;
  190.  
  191. } else if (drive_power > min_voltage){
  192.  
  193. drive_power = max_voltage;
  194.  
  195. } else if (drive_power < min_voltage && error <= 2){
  196.  
  197. drive_power = min_voltage * 0.6;
  198.  
  199. }
  200.  
  201. FL_mtr.spin(vex::directionType::undefined, drive_power, vex::voltageUnits::mV);
  202. BR_mtr.spin(vex::directionType::undefined, drive_power, vex::voltageUnits::mV);
  203. BL_mtr.spin(vex::directionType::undefined, drive_power, vex::voltageUnits::mV);
  204. FR_mtr.spin(vex::directionType::undefined, drive_power, vex::voltageUnits::mV);
  205.  
  206.  
  207.  
  208. previous_error = error;
  209. previous_drive_power = drive_power;
  210.  
  211. }
  212.  
  213.  
  214. FL_mtr.spin(vex::directionType::undefined, 0, vex::voltageUnits::mV);
  215. BR_mtr.spin(vex::directionType::undefined, 0, vex::voltageUnits::mV);
  216. BL_mtr.spin(vex::directionType::undefined, 0, vex::voltageUnits::mV);
  217. FR_mtr.spin(vex::directionType::undefined, 0, vex::voltageUnits::mV);
  218.  
  219.  
  220. if (wait_for_rest == true){
  221.  
  222. wait_until_settled();
  223.  
  224. } else {
  225.  
  226. FL_mtr.setStopping(vex::brakeType::brake);
  227. FR_mtr.setStopping(vex::brakeType::brake);
  228. BL_mtr.setStopping(vex::brakeType::brake);
  229. BR_mtr.setStopping(vex::brakeType::brake);
  230.  
  231. vex::task::sleep(100);
  232.  
  233. FL_mtr.setStopping(vex::brakeType::coast);
  234. FR_mtr.setStopping(vex::brakeType::coast);
  235. BL_mtr.setStopping(vex::brakeType::coast);
  236. BR_mtr.setStopping(vex::brakeType::coast);
  237.  
  238. }
  239. }
  240.  
  241.  
  242.  
  243. void turnright(int inches, bool speed){
  244.  
  245. bool autonomous_running = true;
  246.  
  247. int mastermotorpower;
  248. int slavemotorpower;
  249. FL_mtr.resetRotation();
  250. FR_mtr.resetRotation();
  251. float Kp1 = 0.65;
  252. float Kp2 = 0.4;
  253.  
  254. if (speed == true){
  255.  
  256. while (autonomous_running == true && std::abs(FL_mtr.rotation(vex::rotationUnits::deg)) < inches * 2){
  257.  
  258.  
  259. mastermotorpower = ((inches*2) - std::abs(FL_mtr.rotation(vex::rotationUnits::deg))) * Kp1;
  260.  
  261. if (mastermotorpower > 110){
  262.  
  263. mastermotorpower = 110;
  264.  
  265. } else if (mastermotorpower < 50){
  266.  
  267. mastermotorpower = 50;
  268.  
  269. }
  270.  
  271. slavemotorpower = mastermotorpower + ((std::abs(FL_mtr.rotation(vex::rotationUnits::deg)) - std::abs(FL_mtr.rotation(vex::rotationUnits::deg))));
  272.  
  273. FL_mtr.spin(vex::directionType::fwd,mastermotorpower, vex::velocityUnits::rpm);
  274. FR_mtr.spin(vex::directionType::rev,slavemotorpower, vex::velocityUnits::rpm);
  275. BL_mtr.spin(vex::directionType::rev,mastermotorpower*-1, vex::velocityUnits::rpm);
  276. BR_mtr.spin(vex::directionType::fwd,slavemotorpower*-1, vex::velocityUnits::rpm);
  277.  
  278. vex::task::sleep(5);
  279.  
  280. }
  281.  
  282. FL_mtr.stop(vex::brakeType::brake);
  283. FR_mtr.stop(vex::brakeType::brake);
  284. BL_mtr.stop(vex::brakeType::brake);
  285. BR_mtr.stop(vex::brakeType::brake);
  286.  
  287. } else {
  288.  
  289. while(autonomous_running == true && std::abs(FL_mtr.rotation(vex::rotationUnits::deg))< inches * 2){
  290.  
  291. mastermotorpower = ((inches*2) - std::abs(FL_mtr.rotation(vex::rotationUnits::deg)))* Kp2;
  292.  
  293. if (mastermotorpower > 70){
  294.  
  295. mastermotorpower = 70;
  296.  
  297. } else if (mastermotorpower < 30){
  298.  
  299. mastermotorpower = 30;
  300.  
  301. }
  302.  
  303. slavemotorpower = mastermotorpower + ((std::abs(FL_mtr.rotation(vex::rotationUnits::deg))- std::abs(FR_mtr.rotation(vex::rotationUnits::deg))));
  304.  
  305. FL_mtr.spin(vex::directionType::fwd,mastermotorpower, vex::velocityUnits::rpm);
  306. FR_mtr.spin(vex::directionType::rev,slavemotorpower, vex::velocityUnits::rpm);
  307. BL_mtr.spin(vex::directionType::rev,mastermotorpower*-1, vex::velocityUnits::rpm);
  308. BR_mtr.spin(vex::directionType::fwd,slavemotorpower*-1, vex::velocityUnits::rpm);
  309.  
  310. vex::task::sleep(5);
  311.  
  312. }
  313.  
  314. FL_mtr.stop(vex::brakeType::brake);
  315. BL_mtr.stop(vex::brakeType::brake);
  316. FR_mtr.stop(vex::brakeType::brake);
  317. BR_mtr.stop(vex::brakeType::brake);
  318.  
  319. }
  320. }
  321.  
  322.  
  323. void turnleft(int inches, bool speed){
  324.  
  325. bool autonomous_running = true;
  326.  
  327. int mastermotorpower;
  328. int slavemotorpower;
  329. FL_mtr.resetRotation();
  330. FR_mtr.resetRotation();
  331. float Kp1 = 0.65;
  332. float Kp2 = 0.4;
  333.  
  334. if (speed == true){
  335.  
  336. while (autonomous_running == true && std::abs(FL_mtr.rotation(vex::rotationUnits::deg)) < inches * 2){
  337.  
  338.  
  339. mastermotorpower = ((inches*2) - std::abs(FL_mtr.rotation(vex::rotationUnits::deg))) * Kp1;
  340.  
  341. if (mastermotorpower > 150){
  342.  
  343. mastermotorpower = 150;
  344.  
  345. } else if (mastermotorpower < 50){
  346.  
  347. mastermotorpower = 50;
  348.  
  349. }
  350.  
  351. slavemotorpower = mastermotorpower + ((std::abs(FL_mtr.rotation(vex::rotationUnits::deg)) - std::abs(FL_mtr.rotation(vex::rotationUnits::deg))));
  352.  
  353.  
  354. FL_mtr.spin(vex::directionType::fwd,mastermotorpower*-1, vex::velocityUnits::rpm);
  355. FR_mtr.spin(vex::directionType::rev,slavemotorpower*-1, vex::velocityUnits::rpm);
  356. BL_mtr.spin(vex::directionType::rev,mastermotorpower, vex::velocityUnits::rpm);
  357. BR_mtr.spin(vex::directionType::fwd,slavemotorpower, vex::velocityUnits::rpm);
  358.  
  359. vex::task::sleep(5);
  360.  
  361. }
  362.  
  363. FL_mtr.stop(vex::brakeType::brake);
  364. FR_mtr.stop(vex::brakeType::brake);
  365. BL_mtr.stop(vex::brakeType::brake);
  366. BR_mtr.stop(vex::brakeType::brake);
  367.  
  368. } else {
  369.  
  370. while(autonomous_running == true && std::abs(FL_mtr.rotation(vex::rotationUnits::deg))< inches * 2){
  371.  
  372. mastermotorpower = ((inches*2) - std::abs(FL_mtr.rotation(vex::rotationUnits::deg)))* Kp2;
  373.  
  374. if (mastermotorpower > 70){
  375.  
  376. mastermotorpower = 70;
  377.  
  378. } else if (mastermotorpower < 30){
  379.  
  380. mastermotorpower = 30;
  381.  
  382. }
  383.  
  384. slavemotorpower = mastermotorpower + ((std::abs(FL_mtr.rotation(vex::rotationUnits::deg))- std::abs(FR_mtr.rotation(vex::rotationUnits::deg))));
  385. FL_mtr.spin(vex::directionType::fwd,mastermotorpower*-1, vex::velocityUnits::rpm);
  386. FR_mtr.spin(vex::directionType::rev,slavemotorpower*-1, vex::velocityUnits::rpm);
  387. BL_mtr.spin(vex::directionType::rev,mastermotorpower, vex::velocityUnits::rpm);
  388. BR_mtr.spin(vex::directionType::fwd,slavemotorpower, vex::velocityUnits::rpm);
  389.  
  390. vex::task::sleep(5);
  391.  
  392. }
  393.  
  394. FL_mtr.stop(vex::brakeType::brake);
  395. BL_mtr.stop(vex::brakeType::brake);
  396. FR_mtr.stop(vex::brakeType::brake);
  397. BR_mtr.stop(vex::brakeType::brake);
  398.  
  399. }
  400. }
  401.  
  402.  
  403.  
  404.  
  405.  
  406.  
  407.  
  408. void pre_auton( void ) {
  409.  
  410. Tilt.resetRotation();
  411. Lift.resetRotation();
  412.  
  413. FL_mtr.resetRotation();
  414. FR_mtr.resetRotation();
  415. BL_mtr.resetRotation();
  416. BR_mtr.resetRotation();
  417.  
  418. }
  419.  
  420.  
  421.  
  422.  
  423. void autonomous( void ) {
  424.  
  425.  
  426.  
  427. Lift.rotateTo(-600, vex::rotationUnits::deg, 90, vex::velocityUnits::pct);// Deploy
  428.  
  429. Lift.rotateTo(0, vex::rotationUnits::deg, 90, vex::velocityUnits::pct);// Deploy
  430.  
  431. vex::task::sleep(300);
  432.  
  433. RightIntake.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);// Intake
  434. LeftIntake.spin(vex::directionType::fwd, 100, vex::velocityUnits::pct);// Intake
  435.  
  436. drive_inches(48,90,35, true);//1st Row
  437.  
  438. drive_inches_reverse(48,90,35, true);//Backs Up
  439.  
  440.  
  441. drive_inches(6,90,35, true);//Towards Zone
  442.  
  443.  
  444.  
  445.  
  446. }
  447.  
  448. void resetTilt(int tiltSpeed){
  449.  
  450. Tilt.rotateTo(0, vex::rotationUnits::deg, tiltSpeed, vex::velocityUnits::pct);
  451.  
  452. Tilt.stop(vex::brakeType::hold);
  453.  
  454. }
  455.  
  456. void lowTower(){
  457.  
  458. RightIntake.startRotateFor(30, vex::rotationUnits::deg, 70, vex::velocityUnits::pct);
  459. LeftIntake.rotateFor(-30, vex::rotationUnits::deg, 70, vex::velocityUnits::pct);
  460.  
  461. vex::task::sleep(10);
  462.  
  463. Lift.rotateTo(-115, vex::rotationUnits::deg, 90, vex::velocityUnits::pct);// Moves Lift up to Score in Tower
  464.  
  465. RightIntake.startRotateFor(150, vex::rotationUnits::deg, 25, vex::velocityUnits::pct);
  466. LeftIntake.startRotateFor(-150, vex::rotationUnits::deg, 25, vex::velocityUnits::pct);
  467.  
  468. Lift.rotateTo(-625, vex::rotationUnits::deg, 90, vex::velocityUnits::pct);// Moves Lift up to Score in Tower
  469.  
  470. }
  471.  
  472. void highTower(){
  473.  
  474. RightIntake.startRotateFor(30, vex::rotationUnits::deg, 70, vex::velocityUnits::pct);
  475. LeftIntake.rotateFor(-30, vex::rotationUnits::deg, 70, vex::velocityUnits::pct);
  476.  
  477. vex::task::sleep(10);
  478.  
  479. Lift.rotateTo(-115, vex::rotationUnits::deg, 90, vex::velocityUnits::pct);// Moves Lift up to Score in Tower
  480.  
  481. RightIntake.startRotateFor(150, vex::rotationUnits::deg, 25, vex::velocityUnits::pct);
  482. LeftIntake.startRotateFor(-150, vex::rotationUnits::deg, 25, vex::velocityUnits::pct);
  483.  
  484. Lift.rotateTo(-915, vex::rotationUnits::deg, 90, vex::velocityUnits::pct);// Moves Lift up to Score in Tower
  485.  
  486. }
  487.  
  488. int tilterTasks (){
  489.  
  490. Tilt.resetRotation();
  491.  
  492. while(1){
  493.  
  494. int TilterSpeed(-1175-(Tilt.rotation(vex::rotationUnits::deg)));
  495.  
  496. if(Controller1.ButtonL1.pressing()){
  497.  
  498. Lift.spin(vex::directionType::rev, 90, vex::percentUnits::pct);
  499.  
  500. }
  501.  
  502. else if (Controller1.ButtonL2.pressing()) {
  503.  
  504. Lift.spin(vex::directionType::fwd, 90, vex::percentUnits::pct);
  505.  
  506. }
  507.  
  508. else if(Controller1.ButtonA.pressing()){
  509.  
  510. Lift.stop(vex::brakeType::coast);
  511.  
  512. Tilt.spin(vex::directionType::rev, ((TilterSpeed)/-8), vex::velocityUnits::pct);
  513.  
  514. }
  515.  
  516. else if(Controller1.ButtonB.pressing()){
  517.  
  518. resetTilt(90);
  519.  
  520. }
  521.  
  522. else if(Controller1.ButtonX.pressing()){
  523.  
  524. lowTower();
  525.  
  526. }
  527.  
  528. else if(Controller1.ButtonY.pressing()){
  529.  
  530. highTower();
  531.  
  532. }
  533.  
  534. else{
  535. Tilt.stop(vex::brakeType::hold);
  536. Lift.stop(vex::brakeType::hold);
  537. }
  538.  
  539. if(Controller1.ButtonR2.pressing()){
  540. RightIntake.spin(vex::directionType::fwd, 55, vex::velocityUnits::pct);
  541. LeftIntake.spin(vex::directionType::rev, 55, vex::velocityUnits::pct);
  542. }
  543.  
  544. else if(Controller1.ButtonR1.pressing()){
  545. RightIntake.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);
  546. LeftIntake.spin(vex::directionType::fwd, 100, vex::velocityUnits::pct);
  547. }
  548.  
  549. else{
  550. RightIntake.stop(vex::brakeType::hold);
  551. LeftIntake.stop(vex::brakeType::hold);
  552. }
  553.  
  554. }
  555. return 1;
  556. }
  557.  
  558.  
  559.  
  560. void usercontrol( void ) {
  561.  
  562. vex::task tilter = vex::task(tilterTasks);
  563.  
  564. while (1) {
  565.  
  566. FR_mtr.spin(directionType::fwd, (Controller1.Axis3.value() + (Controller1.Axis1.value())*.75)/1.25, velocityUnits::pct); //(Axis3+Axis4)/2;
  567. FR_mtr.spin(directionType::fwd, (Controller1.Axis3.value() - (Controller1.Axis1.value())*.75)/1.25, velocityUnits::pct);//(Axis3-Axis4)/2;
  568. BL_mtr.spin(directionType::fwd, (Controller1.Axis3.value() + (Controller1.Axis1.value())*.75)/1.25, velocityUnits::pct); //(Axis3+Axis4)/2;
  569. BR_mtr.spin(directionType::fwd, (Controller1.Axis3.value() - (Controller1.Axis1.value())*.75)/1.25, velocityUnits::pct);//(Axis3-Axis4)/2;
  570.  
  571. }
  572. vex::task::sleep(10);
  573. }
  574.  
  575.  
  576. int main() {
  577. //Set up callbacks for autonomous and driver control periods.
  578. Competition.autonomous( autonomous );
  579. Competition.drivercontrol( usercontrol );
  580.  
  581. //Run the pre-autonomous function.
  582. pre_auton();
  583.  
  584. //Prevent main from exiting with an infinite loop.
  585. while(1) {
  586. vex::task::sleep(100);//Sleep the task for a short amount of time to prevent wasted resources.
  587. }
  588.  
  589. }
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!