API tools faq
paste
Advertisement
Guest User

5203R Skills Program

a guest
Jan 17th, 2020
114
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 9.94 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. }
  42.  
  43. }
  44.  
  45. void drive_inches(int distance, int max_power = 90, int min_power = 35, bool wait_for_rest = true){
  46.  
  47. float error;
  48. float previous_error;
  49. float drive_power;
  50. float previous_drive_power;
  51. float distance_covered;
  52. float max_voltage = 120 * max_power;
  53. float min_voltage = 120 * min_power;
  54. const int Kp = 200;
  55. const int Kd = -100;
  56.  
  57. BL_mtr.resetRotation();
  58. BR_mtr.resetRotation();
  59.  
  60. if (distance <= 2){
  61.  
  62. distance = 3;
  63.  
  64. }
  65.  
  66. while(error > 2){
  67.  
  68. error = distance - (fabs((BL_mtr.rotation(vex::rotationUnits::deg)) - (BR_mtr.rotation(vex::rotationUnits::deg)) / 2) / 28.86);
  69.  
  70.  
  71.  
  72. drive_power = (error * Kp) + ((previous_error - error) * Kd);
  73.  
  74. if (distance_covered < 0.25 * distance){
  75.  
  76. drive_power = (drive_power - previous_drive_power) * 0.75;
  77.  
  78. }
  79.  
  80. if (drive_power < min_voltage && error > 2){
  81.  
  82. drive_power = min_voltage;
  83.  
  84. } else if (drive_power > min_voltage){
  85.  
  86. drive_power = max_voltage;
  87.  
  88. } else if (drive_power < min_voltage && error <= 2){
  89.  
  90. drive_power = min_voltage * 0.75;
  91.  
  92. }
  93.  
  94. FL_mtr.spin(vex::directionType::undefined, drive_power, vex::voltageUnits::mV);
  95. BR_mtr.spin(vex::directionType::undefined, drive_power, vex::voltageUnits::mV);
  96. BL_mtr.spin(vex::directionType::undefined, drive_power, vex::voltageUnits::mV);
  97. FR_mtr.spin(vex::directionType::undefined, drive_power, vex::voltageUnits::mV);
  98.  
  99.  
  100.  
  101. previous_error = error;
  102. previous_drive_power = drive_power;
  103.  
  104. }
  105.  
  106. if (wait_for_rest == true){
  107.  
  108. wait_until_settled();
  109.  
  110. } else {
  111.  
  112. FL_mtr.setStopping(vex::brakeType::brake);
  113. FR_mtr.setStopping(vex::brakeType::brake);
  114. BL_mtr.setStopping(vex::brakeType::brake);
  115. BR_mtr.setStopping(vex::brakeType::brake);
  116.  
  117. vex::task::sleep(100);
  118.  
  119. FL_mtr.setStopping(vex::brakeType::coast);
  120. FR_mtr.setStopping(vex::brakeType::coast);
  121. BL_mtr.setStopping(vex::brakeType::coast);
  122. BR_mtr.setStopping(vex::brakeType::coast);
  123.  
  124. }
  125. }
  126.  
  127.  
  128.  
  129.  
  130.  
  131. void drive_inches_reverse (int distance, int max_power = 90, int min_power = 35, bool wait_for_rest = true){
  132.  
  133. float error;
  134. float previous_error;
  135. float drive_power;
  136. float previous_drive_power;
  137. float distance_covered;
  138. float max_voltage = 120 * max_power;
  139. float min_voltage = 120 * min_power;
  140. const int Kp = 200;
  141. const int Kd = -100;
  142.  
  143. BL_mtr.resetRotation();
  144. BR_mtr.resetRotation();
  145.  
  146. if (distance <= 2){
  147.  
  148. distance = 3;
  149.  
  150. }
  151.  
  152. while(error > 1){
  153.  
  154. error = distance - (fabs((BL_mtr.rotation(vex::rotationUnits::deg)) - (BR_mtr.rotation(vex::rotationUnits::deg)) / 2) / 28.86);
  155.  
  156. drive_power = (error * Kp) + ((previous_error - error) * Kd);
  157.  
  158. if (distance_covered < 0.25 * distance){
  159.  
  160. drive_power = (drive_power - previous_drive_power) * 0.75;
  161.  
  162. }
  163.  
  164. if (drive_power < min_voltage && error > 2){
  165.  
  166. drive_power = min_voltage;
  167.  
  168. } else if (drive_power > min_voltage){
  169.  
  170. drive_power = max_voltage;
  171.  
  172. } else if (drive_power < min_voltage && error <= 2){
  173.  
  174. drive_power = min_voltage * 0.75;
  175.  
  176. }
  177.  
  178. FL_mtr.spin(vex::directionType::undefined, -(drive_power), vex::voltageUnits::mV);
  179. BR_mtr.spin(vex::directionType::undefined, -(drive_power), vex::voltageUnits::mV);
  180. BL_mtr.spin(vex::directionType::undefined, -(drive_power), vex::voltageUnits::mV);
  181. FR_mtr.spin(vex::directionType::undefined, -(drive_power), vex::voltageUnits::mV);
  182.  
  183. previous_error = error;
  184. previous_drive_power = drive_power;
  185.  
  186. }
  187.  
  188. if (wait_for_rest == true){
  189.  
  190. wait_until_settled();
  191.  
  192. } else {
  193.  
  194. FL_mtr.setStopping(vex::brakeType::brake);
  195. FR_mtr.setStopping(vex::brakeType::brake);
  196. BL_mtr.setStopping(vex::brakeType::brake);
  197. BR_mtr.setStopping(vex::brakeType::brake);
  198.  
  199. vex::task::sleep(100);
  200.  
  201. FL_mtr.setStopping(vex::brakeType::coast);
  202. FR_mtr.setStopping(vex::brakeType::coast);
  203. BL_mtr.setStopping(vex::brakeType::coast);
  204. BR_mtr.setStopping(vex::brakeType::coast);
  205. }
  206. }
  207.  
  208.  
  209.  
  210.  
  211.  
  212.  
  213.  
  214.  
  215. void pre_auton( void ) {
  216.  
  217. Tilt.resetRotation();
  218. Lift.resetRotation();
  219.  
  220. FL_mtr.resetRotation();
  221. FR_mtr.resetRotation();
  222. BL_mtr.resetRotation();
  223. BR_mtr.resetRotation();
  224.  
  225. }
  226.  
  227.  
  228.  
  229.  
  230. void autonomous( void ) {
  231.  
  232. drive_inches(24);
  233.  
  234. drive_inches_reverse(24);
  235.  
  236.  
  237. }
  238.  
  239. void resetTilt(int tiltSpeed){
  240.  
  241. Tilt.rotateTo(0, vex::rotationUnits::deg, tiltSpeed, vex::velocityUnits::pct);
  242.  
  243. Tilt.stop(vex::brakeType::hold);
  244.  
  245. }
  246.  
  247. void lowTower(){
  248.  
  249. RightIntake.startRotateFor(30, vex::rotationUnits::deg, 70, vex::velocityUnits::pct);
  250. LeftIntake.rotateFor(-30, vex::rotationUnits::deg, 70, vex::velocityUnits::pct);
  251.  
  252. vex::task::sleep(10);
  253.  
  254. Lift.rotateTo(-115, vex::rotationUnits::deg, 90, vex::velocityUnits::pct);// Moves Lift up to Score in Tower
  255.  
  256. RightIntake.startRotateFor(150, vex::rotationUnits::deg, 25, vex::velocityUnits::pct);
  257. LeftIntake.startRotateFor(-150, vex::rotationUnits::deg, 25, vex::velocityUnits::pct);
  258.  
  259. Lift.rotateTo(-625, vex::rotationUnits::deg, 90, vex::velocityUnits::pct);// Moves Lift up to Score in Tower
  260.  
  261. }
  262.  
  263. void highTower(){
  264.  
  265. RightIntake.startRotateFor(30, vex::rotationUnits::deg, 70, vex::velocityUnits::pct);
  266. LeftIntake.rotateFor(-30, vex::rotationUnits::deg, 70, vex::velocityUnits::pct);
  267.  
  268. vex::task::sleep(10);
  269.  
  270. Lift.rotateTo(-115, vex::rotationUnits::deg, 90, vex::velocityUnits::pct);// Moves Lift up to Score in Tower
  271.  
  272. RightIntake.startRotateFor(150, vex::rotationUnits::deg, 25, vex::velocityUnits::pct);
  273. LeftIntake.startRotateFor(-150, vex::rotationUnits::deg, 25, vex::velocityUnits::pct);
  274.  
  275. Lift.rotateTo(-915, vex::rotationUnits::deg, 90, vex::velocityUnits::pct);// Moves Lift up to Score in Tower
  276.  
  277. }
  278.  
  279. int tilterTasks (){
  280.  
  281. Tilt.resetRotation();
  282.  
  283. while(1){
  284.  
  285. int TilterSpeed(-1175-(Tilt.rotation(vex::rotationUnits::deg)));
  286.  
  287. if(Controller1.ButtonL1.pressing()){
  288.  
  289. Lift.spin(vex::directionType::rev, 90, vex::percentUnits::pct);
  290.  
  291. }
  292.  
  293. else if (Controller1.ButtonL2.pressing()) {
  294.  
  295. Lift.spin(vex::directionType::fwd, 90, vex::percentUnits::pct);
  296.  
  297. }
  298.  
  299. else if(Controller1.ButtonA.pressing()){
  300.  
  301. Lift.stop(vex::brakeType::coast);
  302.  
  303. Tilt.spin(vex::directionType::rev, ((TilterSpeed)/-8), vex::velocityUnits::pct);
  304.  
  305. }
  306.  
  307. else if(Controller1.ButtonB.pressing()){
  308.  
  309. resetTilt(90);
  310.  
  311. }
  312.  
  313. else if(Controller1.ButtonX.pressing()){
  314.  
  315. lowTower();
  316.  
  317. }
  318.  
  319. else if(Controller1.ButtonY.pressing()){
  320.  
  321. highTower();
  322.  
  323. }
  324.  
  325. else{
  326. Tilt.stop(vex::brakeType::hold);
  327. Lift.stop(vex::brakeType::hold);
  328. }
  329.  
  330. if(Controller1.ButtonR2.pressing()){
  331. RightIntake.spin(vex::directionType::fwd, 55, vex::velocityUnits::pct);
  332. LeftIntake.spin(vex::directionType::rev, 55, vex::velocityUnits::pct);
  333. }
  334.  
  335. else if(Controller1.ButtonR1.pressing()){
  336. RightIntake.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);
  337. LeftIntake.spin(vex::directionType::fwd, 100, vex::velocityUnits::pct);
  338. }
  339.  
  340. else{
  341. RightIntake.stop(vex::brakeType::hold);
  342. LeftIntake.stop(vex::brakeType::hold);
  343. }
  344.  
  345. }
  346. return 1;
  347. }
  348.  
  349.  
  350.  
  351. void usercontrol( void ) {
  352.  
  353. vex::task tilter = vex::task(tilterTasks);
  354.  
  355. while (1) {
  356.  
  357. FR_mtr.spin(directionType::fwd, (Controller1.Axis3.value() + (Controller1.Axis1.value())*.75)/1.25, velocityUnits::pct); //(Axis3+Axis4)/2;
  358. FR_mtr.spin(directionType::fwd, (Controller1.Axis3.value() - (Controller1.Axis1.value())*.75)/1.25, velocityUnits::pct);//(Axis3-Axis4)/2;
  359. BL_mtr.spin(directionType::fwd, (Controller1.Axis3.value() + (Controller1.Axis1.value())*.75)/1.25, velocityUnits::pct); //(Axis3+Axis4)/2;
  360. BR_mtr.spin(directionType::fwd, (Controller1.Axis3.value() - (Controller1.Axis1.value())*.75)/1.25, velocityUnits::pct);//(Axis3-Axis4)/2;
  361.  
  362. }
  363. vex::task::sleep(10);
  364. }
  365.  
  366.  
  367. int main() {
  368. //Set up callbacks for autonomous and driver control periods.
  369. Competition.autonomous( autonomous );
  370. Competition.drivercontrol( usercontrol );
  371.  
  372. //Run the pre-autonomous function.
  373. pre_auton();
  374.  
  375. //Prevent main from exiting with an infinite loop.
  376. while(1) {
  377. vex::task::sleep(100);//Sleep the task for a short amount of time to prevent wasted resources.
  378. }
  379.  
  380. }
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!