SHARE
TWEET

5203R Skills Program

a guest Jan 17th, 2020 73 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  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. }
RAW Paste Data
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
Top