API tools faq
paste
Advertisement
Mikkel_Serrantes

MJS 10955m 12/30/19

Mikkel_Serrantes
Dec 30th, 2019
144
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 24.75 KB | None | 0 0
raw download clone embed print report
  1. /*----------------------------------------------------------------------------*/
  2. /*                                                                            */
  3. /*    Module:       main.cpp                                                  */
  4. /*    Author:       VEX                                                       */
  5. /*    Created:      Thu Sep 26 2019                                           */
  6. /*    Description:  Competition Template                                      */
  7. /*                                                                            */
  8. /*----------------------------------------------------------------------------*/
  9.  
  10. // ---- START VEXCODE CONFIGURED DEVICES ----
  11. // Robot Configuration:
  12. // [Name]               [Type]        [Port(s)]
  13. // LeftMotorF           motor         1              
  14. // LeftMotorB           motor         17              
  15. // RightMotorB          motor         12              
  16. // RightMotorF          motor         19              
  17. // Scooper2             motor         2              
  18. // Scooper              motor         6              
  19. // Lift                 motor         8              
  20. // Arm2000              motor         16              
  21. // Controller1          controller                    
  22. // Controller2          controller                    
  23. // P1                   pot           D              
  24. // P2                   pot           G              
  25. // Inertial11           inertial      11              
  26. // LeftEncoder          encoder       A, B            
  27. // RightEncoder         encoder       E, F            
  28. // ---- END VEXCODE CONFIGURED DEVICES ----
  29.  
  30. #include "vex.h"
  31.  
  32.  
  33. using namespace vex;
  34.  
  35.  
  36. int count =0;
  37. int target = 0;
  38. bool driveMode = false;
  39. int PreviousErrorLeft =0;
  40. int PreviousErrorRight =0;
  41. int PreviousErrorArm=0;
  42.  
  43. ////This is problem
  44.  
  45.  
  46.  
  47.  
  48.  
  49. // A global instance of competition
  50. competition Competition;
  51.  
  52. // define your global instances of motors and other devices here
  53.  
  54. /*---------------------------------------------------------------------------*/
  55. /*                          Pre-Autonomous Functions                         */
  56. /*                                                                           */
  57. /*  You may want to perform some actions before the competition starts.      */
  58. /*  Do them in the following function.  You must return from this function   */
  59. /*  or the autonomous and usercontrol tasks will not be started.  This       */
  60. /*  function is only called once after the V5 has been powered on and        */
  61. /*  not every time that the robot is disabled.                               */
  62. /*---------------------------------------------------------------------------*/
  63.  
  64. void pre_auton(void) {
  65.   // Initializing Robot Configuration. DO NOT REMOVE!
  66.   vexcodeInit();
  67.  
  68.   // All activities that occur before the competition starts
  69.   // Example: clearing encoders, setting servo positions, ...
  70. }
  71.  
  72.  
  73. /*---------------------------------------------------------------------------*/
  74. /*                                                                           */
  75. /*                              Autonomous Task                              */
  76. /*                                                                           */
  77. /*  This task is used to control your robot during the autonomous phase of   */
  78. /*  a VEX Competition.                                                       */
  79. /*                                                                           */
  80. /*  You must modify the code to add your own robot specific commands here.   */
  81. /*---------------------------------------------------------------------------*/
  82. void RightMotorFB(double speed) {
  83.   RightMotorF.spin(directionType::fwd,speed,velocityUnits::rpm);
  84.   RightMotorB.spin(directionType::fwd,speed,velocityUnits::rpm);
  85. }
  86.  
  87. void LeftMotorFB(double speed) {
  88.   LeftMotorF.spin(directionType::fwd,speed,velocityUnits::rpm);
  89.   LeftMotorB.spin(directionType::fwd,speed,velocityUnits::rpm);
  90. }
  91.  
  92. void slop(int distance) {
  93.   if(distance <0) {
  94.     LeftMotorFB(-40);
  95.     vex::task::sleep(35);
  96.   }
  97. }
  98.  
  99. const int accel_step =18;
  100. const int deccel_step =100;
  101. static int leftSpeed =0;
  102. static int rightSpeed =0;
  103.  
  104. void LeftMotorFBS(int leftTarget){
  105.   int step;
  106.  
  107.   if(abs(leftSpeed) < abs(leftTarget))
  108.     step= accel_step;
  109.   else
  110.     step= deccel_step;
  111.  
  112.   if(leftTarget > leftSpeed + step)
  113.     leftSpeed +=step;
  114.   else if(leftTarget < leftSpeed - step)
  115.     leftSpeed -= step;
  116.   else
  117.     leftSpeed = leftTarget;
  118.  
  119.   LeftMotorFB(leftSpeed);
  120.  
  121. }
  122.  
  123. void RightMotorFBS(int rightTarget){
  124.   int step;
  125.  
  126.   if(abs(rightSpeed) < abs(rightTarget))
  127.     step= accel_step;
  128.   else
  129.     step= deccel_step;
  130.  
  131.   if(rightTarget > rightSpeed + step)
  132.     rightSpeed +=step;
  133.   else if(rightTarget < rightSpeed - step)
  134.     leftSpeed -= step;
  135.   else
  136.     rightSpeed = rightTarget;
  137.  
  138.   RightMotorFB(rightSpeed);
  139.  
  140. }
  141.  
  142. int drivePos() {
  143. return (LeftMotorF.rotation(rotationUnits::deg) + RightMotorF.rotation(rotationUnits::deg))/2;
  144. }
  145. bool isDriving() {
  146.     return(LeftMotorF.isSpinning() || LeftMotorB.isSpinning() || RightMotorF.isSpinning() || RightMotorB.isSpinning());
  147. }
  148.  
  149. void PidBasicAsync(int distance) {
  150.     slop(distance);
  151.     count = 0;
  152.     target = distance;
  153.     driveMode = true;
  154.     LeftMotorF.resetRotation();
  155.     RightMotorF.resetRotation();
  156.     LeftMotorB.resetRotation();
  157.     RightMotorB.resetRotation();
  158.     PreviousErrorLeft = 0;
  159.     PreviousErrorRight = 0;
  160.     LeftMotorFBS(distance);
  161.     RightMotorFBS(distance);
  162.  
  163. }
  164.  
  165. void BigArm (int degrees) {
  166.    
  167.    while(true) {
  168.        
  169.     double kp = 2.0;////1.2
  170.  
  171.     int LiftPos= P2.value(rotationUnits::deg);  
  172.      
  173.     int power = (LiftPos - degrees) * kp + 10;//////+10
  174.        
  175.     LiftPos= P2.value(rotationUnits::deg);
  176.            
  177.     if (LiftPos <= degrees) { ///starts at 114 to 44/// /////37
  178.        
  179.     Arm2000.stop(brakeType::brake);
  180.         break;
  181.    
  182.     } else {
  183.  
  184.          Arm2000.spin(directionType::rev, power, velocityUnits::pct);
  185. }
  186. }
  187.   }
  188.  
  189.     void PidBasic (int distance) {
  190.     if (driveMode) {
  191.    
  192.     double Kp = 0.16192;///0.2 or .215 .162
  193.     double Kd = .4995;//465 or .52  or .45
  194.    
  195.    
  196.     int Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
  197.     int Rightdegrees=RightMotorF.rotation(rotationUnits::deg);
  198.  
  199.     int ErrorLeft = distance - Leftdegrees;
  200.     int ErrorRight = distance - Rightdegrees;
  201.  
  202.     Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
  203.     Rightdegrees=RightMotorF.rotation(rotationUnits::deg);
  204.  
  205.     ErrorLeft = distance - Leftdegrees;
  206.     ErrorRight = distance - Rightdegrees;
  207.        
  208.     int LeftsideProportionalError = Kp * ErrorLeft;
  209.     int RightsideProportionalError = Kp * ErrorRight;
  210.  
  211.     double LeftsidedDerivativeError = Kd * (ErrorLeft - PreviousErrorLeft);
  212.     double RightsidedDerivativeError = Kd * (ErrorRight - PreviousErrorRight);
  213.        
  214.     LeftMotorFB(LeftsideProportionalError + LeftsidedDerivativeError);
  215.     RightMotorFB(RightsideProportionalError + RightsidedDerivativeError);
  216.        
  217.     PreviousErrorLeft = ErrorLeft;
  218.     PreviousErrorRight = ErrorRight;
  219.        
  220.         /*
  221.         if (abs(ErrorLeft) < 6 && abs(ErrorRight) < 6) {
  222.             break;    
  223.         }
  224.         */
  225.        
  226.         if (!isDriving()) {
  227.             count++;
  228.         }
  229.         if (count > 30) {
  230.             driveMode = false;
  231.         }
  232.        
  233.        
  234.         vex::task::sleep(30);
  235.     }
  236. }
  237.  
  238.  
  239. void Turning (int distance) {
  240.     LeftMotorF.resetRotation();
  241.     RightMotorF.resetRotation();
  242.       LeftMotorB.resetRotation();
  243.     RightMotorB.resetRotation();
  244.    
  245.             LeftMotorF.setStopping(brakeType::brake);
  246.     RightMotorF.setStopping(brakeType::brake);
  247.     LeftMotorB.setStopping(brakeType::brake);
  248.     RightMotorB.setStopping(brakeType::brake);
  249.    
  250.     float Kp = .395; //// best.17 //// ..30 ///4
  251.     float Kd = .4;/// best .35 //// .34 ////next best .32 next best// 25 //smallest movement.1 ///.395
  252.    
  253.     int Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
  254.     int Rightdegrees=RightMotorF.rotation(rotationUnits::deg);
  255.  
  256.     int ErrorLeft = -1*Leftdegrees - distance;
  257.     int ErrorRight = distance - Rightdegrees;
  258.    
  259.     int PreviousErrorLeft = 0;
  260.     int PreviousErrorRight = 0;
  261.    
  262.     while(1) {
  263.     Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
  264.     Rightdegrees=RightMotorF.rotation(rotationUnits::deg);
  265.  
  266.      ErrorLeft = -1*Leftdegrees - distance;
  267.      ErrorRight = distance - Rightdegrees;
  268.        
  269.         int LeftsideProportionalError = Kp * ErrorLeft;
  270.         int RightsideProportionalError = Kp * ErrorRight;
  271.  
  272.         double LeftsidedDerivativeError = Kd * (ErrorLeft - PreviousErrorLeft);
  273.         double RightsidedDerivativeError = Kd * (ErrorRight - PreviousErrorRight);
  274.        
  275.         LeftMotorFB(LeftsideProportionalError + LeftsidedDerivativeError);
  276.         RightMotorFB(RightsideProportionalError + RightsidedDerivativeError);
  277.        
  278.         PreviousErrorLeft = ErrorLeft;
  279.         PreviousErrorRight = ErrorRight;
  280.         if (abs(ErrorLeft) < 10 && abs(ErrorRight) < 10) {
  281.         LeftMotorFB(0);
  282.         RightMotorFB(0);
  283.      
  284.             break;    
  285.     }
  286.         vex::task::sleep(30);
  287.     }
  288.     }
  289.  
  290.     void PidFoward (int distance) {
  291.     LeftMotorF.resetRotation();
  292.     RightMotorF.resetRotation();
  293.     LeftMotorB.resetRotation();
  294.     RightMotorB.resetRotation();
  295.  
  296.     double Kp = 0.5;///0.2 or .215 //.3  ////0.25 ///0.4
  297.     double Kd = 0; ////.275 or .0325 //.5 ///4375 //////0.438109 ///.7 .54 ////290 /////0.255
  298.  ////0.8 ////0.6
  299.    
  300.     int Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
  301.     int Rightdegrees=RightMotorF.rotation(rotationUnits::deg);
  302.  
  303.     int ErrorLeft = distance - Leftdegrees;
  304.     int ErrorRight = distance - Rightdegrees;
  305.  
  306.     int PreviousErrorLeft = 0;
  307.     int PreviousErrorRight = 0;
  308.  
  309.     while(1) {
  310.  
  311.     Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
  312.     Rightdegrees=RightMotorF.rotation(rotationUnits::deg);
  313.  
  314.     ErrorLeft = distance - Leftdegrees;
  315.     ErrorRight = distance - Rightdegrees;
  316.  
  317.     int LeftsideProportionalError = Kp * ErrorLeft;
  318.     int RightsideProportionalError = Kp * ErrorRight;
  319.  
  320.     double LeftsidedDerivativeError = Kd * (ErrorLeft - PreviousErrorLeft);
  321.     double RightsidedDerivativeError = Kd * (ErrorRight - PreviousErrorRight);
  322.  
  323.     int leftPower = LeftsideProportionalError + LeftsidedDerivativeError;
  324.     int rightPower = RightsideProportionalError + RightsidedDerivativeError;
  325.        
  326.        
  327.  int velocitydiff = Leftdegrees - Rightdegrees;
  328.  rightPower = leftPower;
  329.  leftPower -= (velocitydiff*0.0695); //////0.7 ////0.6 is nice////.62955///.62955 good/// 0.068
  330.  LeftMotorFB(leftPower);
  331.  RightMotorFB(rightPower);
  332.      
  333.         PreviousErrorLeft = ErrorLeft;
  334.         PreviousErrorRight = ErrorRight;
  335.  
  336.  
  337.         if (abs(ErrorLeft) < 10 && abs(ErrorRight) < 10) {
  338.  
  339.        
  340.  
  341.         break;
  342.     }
  343.         vex::task::sleep(20);
  344.  
  345.     }
  346. }
  347.  
  348. void DriveInInches(double distanceinInches) {
  349.  
  350.   double in = 3;
  351.   double out = 2;
  352.   double wheelDiameter = 4; // in inches
  353.   double gearratio = (360 * out) / (wheelDiameter * 3.14 * in);
  354.   double targetDistance = distanceinInches * gearratio;
  355.   PidFoward(targetDistance);
  356. }
  357.  
  358. /////////////////////////////////////////////////////////////////////////////////////////////
  359.  
  360. void StopMotor(vex::brakeType type = brakeType::coast){
  361.     LeftMotorF.stop(type);
  362.     RightMotorF.stop(type);
  363.     LeftMotorB.stop(type);
  364.     RightMotorB.stop(type);
  365. }
  366. void SetMotorBrakingType(brakeType type = brakeType::coast) {
  367.     LeftMotorF.setStopping(type);
  368.     RightMotorF.setStopping(type);
  369.     LeftMotorB.setStopping(type);
  370.     RightMotorB.setStopping(type);
  371. }
  372. int CubeIntake2() {
  373.     int timeout = 4000;        
  374.     Scooper.setStopping(brakeType::coast);    
  375.     int sleepTimeout = 20;
  376.     Scooper.spin(directionType::rev, 100, velocityUnits::pct);
  377.      Scooper.spin(directionType::rev, 100, velocityUnits::pct);
  378.    
  379.     while (timeout > 0) {
  380.         task::sleep(sleepTimeout);
  381.         timeout = timeout - sleepTimeout;
  382.     }
  383.    
  384.     Scooper.stop(brakeType::coast);
  385.    
  386.     return(0);
  387. }
  388. int CubeIntake() {
  389.     int timeout = 4000;        
  390.     Scooper.setStopping(brakeType::coast);  
  391.      Scooper2.setStopping(brakeType::coast);
  392.     int sleepTimeout = 20;
  393.     Scooper.spin(directionType::fwd, 100, velocityUnits::pct);
  394.      Scooper2.spin(directionType::fwd, 100, velocityUnits::pct);
  395.    
  396.     while (timeout > 0) {
  397.         task::sleep(sleepTimeout);
  398.         timeout = timeout - sleepTimeout;
  399.     }
  400.     Scooper.stop(brakeType::coast);
  401.         Scooper2.stop(brakeType::coast);
  402.     return(0);
  403. }
  404. int AutoBallIntakeTakeBall(int timeout = 5000) {          
  405.    
  406.     Scooper.setStopping(brakeType::coast);    
  407.     int sleepTimeout = 20;
  408.     Scooper.spin(directionType::fwd, 60, velocityUnits::pct);
  409.     while (timeout > 0) {
  410.         task::sleep(sleepTimeout);
  411.         timeout = timeout - sleepTimeout;
  412.     }
  413.    
  414.     Scooper.stop(brakeType::coast);
  415.    
  416.     return(0);
  417. }
  418. void Drive(double numOfRevs, int velocity = 50, bool fwd = true, int timeout = 500)
  419. {
  420.     Brain.Screen.printAt(1, 40, "numofrevs: %f, vel: %f", numOfRevs, velocity);
  421.     SetMotorBrakingType();
  422.    
  423.     int sleepTimeout = 20;
  424.    
  425.     if (fwd == false) {
  426.         numOfRevs = numOfRevs * -1;
  427.     }
  428.    
  429.     LeftMotorF.startRotateFor(numOfRevs, rotationUnits::rev, velocity, velocityUnits::pct);
  430.     RightMotorF.startRotateFor(numOfRevs, rotationUnits::rev, velocity, velocityUnits::pct);
  431.     LeftMotorB.startRotateFor(numOfRevs, rotationUnits::rev, velocity, velocityUnits::pct);
  432.     RightMotorB.startRotateFor(numOfRevs, rotationUnits::rev, velocity, velocityUnits::pct);
  433.  
  434.     while(LeftMotorF.isSpinning()||RightMotorF.isSpinning()||
  435.           LeftMotorB.isSpinning()||RightMotorB.isSpinning()) {
  436.  
  437.         if (timeout <= 0) {
  438.             break;
  439.         }
  440.         task::sleep(sleepTimeout);
  441.         timeout = timeout - sleepTimeout;
  442.     }
  443.     StopMotor();
  444. }
  445.   void DriveInCM(double distanceInCM, int velocity, bool fwd = true, int timeout = 5000) {
  446.   //  double gearRatio = 1.67;
  447.   //  double wheelDiameter = 10.16;
  448.   //  double circumference = wheelDiameter * M_PI;    
  449.   //  double numOfRevs = (distanceInCM)/ (circumference * gearRatio);
  450.   double numOfRevs = distanceInCM/47.9;////old 53.2 47.9
  451.   Drive(numOfRevs, velocity, fwd, timeout);    
  452. }
  453.  
  454. /////////////////////////////////////////////////////////////////////////////////
  455. void LiftPosition1 () {
  456.  
  457.     int timeout = 5000;
  458.  
  459.     Arm2000.setStopping(brakeType::coast);
  460.  
  461.     int sleepTimeout = 30;
  462.     while (timeout > 0) {
  463.  
  464.     Brain.Screen.clearScreen();
  465.  
  466.     float LiftPos = P2.value(rotationUnits::deg);
  467.  
  468.    Brain.Screen.printAt(2, 40, "LiftPos: %f", LiftPos);
  469.  
  470.         if (LiftPos == 0)
  471.         {
  472.             task::sleep(sleepTimeout);
  473.             continue;
  474.         }
  475.  
  476.         // Lowest position for launch
  477.         if (LiftPos >= 108)
  478.         {
  479.             Brain.Screen.printAt(2, 80, "Stopped: %f", LiftPos);
  480.             break;
  481.         }
  482.  
  483.         Arm2000.spin(directionType::fwd, 100, velocityUnits::pct);
  484.  
  485.         task::sleep(sleepTimeout);
  486.  
  487.         timeout = timeout - sleepTimeout;
  488.  
  489.         Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
  490.     }
  491.  
  492.     Arm2000.stop(brakeType::coast);    
  493. }
  494.  
  495. void LiftPosition2() {
  496.  
  497.     int timeout = 5000;
  498.  
  499.     Arm2000.setStopping(brakeType::coast);
  500.  
  501.     int sleepTimeout = 30;
  502.  
  503.     while (timeout > 0) {
  504.  
  505.         Brain.Screen.clearScreen();
  506.  
  507.         float LiftPos = P2.value(rotationUnits::deg);  
  508.  
  509.         Brain.Screen.printAt(2, 40, "LiftPos: %f", LiftPos);  
  510.  
  511.         if (LiftPos == 0)
  512.         {
  513.             task::sleep(sleepTimeout);
  514.             continue;
  515.         }
  516.  
  517.         // load position
  518.         if (LiftPos <= 44)
  519.         {
  520.             break;
  521.         }
  522.  
  523.      
  524.         Arm2000.spin(directionType::rev, 45, velocityUnits::pct);
  525.  
  526.         task::sleep(sleepTimeout);
  527.  
  528.         timeout = timeout - sleepTimeout;
  529.         Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
  530.     }
  531.  
  532.     Arm2000.stop(brakeType::hold);
  533. }
  534.  
  535.  
  536. void LiftPosition3() {
  537.     int timeout = 2500;
  538.  
  539.     Arm2000.setStopping(brakeType::coast);
  540.  
  541.     int sleepTimeout = 30;
  542.  
  543.     while (timeout > 0) {
  544.        
  545.         Brain.Screen.clearScreen();
  546.  
  547.         float LiftPos = P2.value(rotationUnits::deg);  
  548.  
  549.         Brain.Screen.printAt(2, 40, "LiftPos: %f", LiftPos);  
  550.        
  551.        
  552.          if(P2.value(rotationUnits::deg) <44) {
  553.              while(P2.value(rotationUnits::deg) <44) { ///////////13
  554.                  Arm2000.spin(vex::directionType::fwd, 45, vex::velocityUnits::pct);
  555.                  
  556.          }
  557.            
  558.       break;
  559.            
  560.         }        
  561.             else if
  562.                (P2.value(rotationUnits::deg) > 45) {
  563.                   while(P2.value(rotationUnits::deg) > 45) { /////////16
  564.                  Arm2000.spin(vex::directionType::rev, 45, vex::velocityUnits::pct);
  565.                          }
  566.            
  567.           break;
  568.            
  569.         }
  570.            
  571.         Arm2000.spin(directionType::fwd, 40, velocityUnits::pct);
  572.  
  573.         task::sleep(sleepTimeout);
  574.  
  575.         timeout = timeout - sleepTimeout;
  576.         Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
  577.     }
  578.  
  579.     Arm2000.stop(brakeType::coast);
  580. }
  581. ///////////////////////////////////////////////??///////////////////////
  582.  
  583. void Lift10() {
  584.     int timeout = 2500;
  585.  
  586.     Lift.setStopping(brakeType::hold);    
  587.  
  588.  
  589.     int sleepTimeout = 30;
  590.  
  591.     while (timeout > 0) {
  592.  
  593.         Brain.Screen.clearScreen();
  594.  
  595.         float LiftCube = P1.value(rotationUnits::deg);  
  596.        
  597.      
  598.         Brain.Screen.printAt(2, 40, "LiftCube %f", LiftCube);  
  599.        
  600.      
  601.    if (LiftCube >= 87)
  602.         {
  603.        
  604.         Brain.Screen.printAt(2, 80, "Stopped: %f", LiftCube);
  605.             break;
  606.         }
  607.        
  608.             Lift.spin(directionType::rev, 127, velocityUnits::pct);
  609.  
  610.         task::sleep(sleepTimeout);
  611.  
  612.         timeout = timeout - sleepTimeout;
  613.         Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
  614.     }
  615.  
  616.     Lift.stop(brakeType::hold);
  617.    
  618. }
  619.  
  620. void Lift11() {
  621.     int timeout = 2500;
  622.  
  623.     Lift.setStopping(brakeType::hold);    
  624.    
  625.  
  626.     int sleepTimeout = 30;
  627.  
  628.     while (timeout > 0) {
  629.  
  630.         Brain.Screen.clearScreen();
  631.  
  632.         float LiftCube = P1.value(rotationUnits::deg);  
  633.        
  634.      
  635.         Brain.Screen.printAt(2, 40, "LiftCube %f", LiftCube);  
  636.        
  637.      
  638.    if (LiftCube >= 67)
  639.         {
  640.        
  641.         Brain.Screen.printAt(2, 80, "Stopped: %f", LiftCube);
  642.             break;
  643.         }
  644.        
  645.        
  646.             Lift.spin(directionType::rev, 127, velocityUnits::pct);
  647.  
  648.         task::sleep(sleepTimeout);
  649.  
  650.         timeout = timeout - sleepTimeout;
  651.         Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
  652.     }
  653.  
  654.     Lift.stop(brakeType::hold);
  655.    
  656. }
  657. void Lift12() {
  658.     int timeout = 2500;
  659.  
  660.     Lift.setStopping(brakeType::coast);
  661.      
  662.  
  663.     int sleepTimeout = 30;
  664.  
  665.     while (timeout > 0) {
  666.  
  667.         Brain.Screen.clearScreen();
  668.  
  669.         float LiftCube = P1.value(rotationUnits::deg);  
  670.  
  671.         Brain.Screen.printAt(2, 40, "LiftCube %f", LiftCube);  
  672.        
  673.      
  674.        if(P1.value(rotationUnits::deg) <9) {
  675.              while(P1.value(rotationUnits::deg) <9) { ///////////13
  676.                  Lift.spin(vex::directionType::rev, 127, vex::velocityUnits::pct);
  677.                  
  678.          }
  679.            
  680.       break;
  681.            
  682.         }
  683.            
  684.             else
  685.                
  686.                 if(P1.value(rotationUnits::deg) > 10) {
  687.                   while(P1.value(rotationUnits::deg) > 10) { /////////16
  688.                  Lift.spin(vex::directionType::fwd, 127, vex::velocityUnits::pct);
  689.                          }
  690.            
  691.           break;
  692.            
  693.         }
  694.            
  695.  
  696.             Lift.spin(directionType::rev, 100, velocityUnits::pct);
  697.  
  698.         task::sleep(sleepTimeout);
  699.  
  700.         timeout = timeout - sleepTimeout;
  701.         Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
  702.     }
  703.  
  704.  
  705.      Lift.stop(brakeType::brake);
  706. }
  707.  
  708. void CheckLiftPotValues() {
  709.  
  710.     while (1)
  711.     {
  712.         Brain.Screen.clearScreen();
  713.         Brain.Screen.printAt(1, 30, "CheckLiftPot");
  714.         float LiftPos = P2.value(rotationUnits::deg);
  715.         Brain.Screen.printAt(1, 20, "rotation: %f degrees", LiftPos);
  716.  
  717.         //Sleep the task for a short amount of time to prevent wasted resources.    
  718.         task::sleep(20);
  719.     }
  720. }
  721.  
  722. int DriveTask() {
  723.     while (true) {
  724.         if (driveMode) {
  725.             PidBasic(target);
  726.         }
  727.     }
  728. }
  729.  
  730. int LiftTask()
  731. {
  732.     while (true) {
  733.        
  734.            Brain.Screen.clearScreen();
  735.         float LiftPos = P2.value(rotationUnits::deg);
  736.  
  737.         Brain.Screen.printAt(1, 40, "LiftPos: %f", LiftPos);
  738.        
  739.         if (Controller1.ButtonY.pressing()) {
  740.             // Load
  741.            LiftPosition1();
  742.         }
  743.      
  744.          else if (Controller1.ButtonB.pressing()) {
  745.             // Launch
  746.              BigArm(43);
  747.         } else {
  748. Arm2000.stop(brakeType::brake);
  749.          }
  750.        
  751.            if (Controller2.ButtonL1.pressing()) {
  752.             // Launch
  753.             Lift10();
  754.         }
  755.        
  756.              else if (Controller2.ButtonL2.pressing()) {
  757.             // Launch
  758.             Lift11();
  759.         }
  760.        
  761.               else if (Controller2.ButtonDown.pressing()) {
  762.             // Launch
  763.             Lift12();
  764.         }
  765.        
  766.         task::sleep(30);
  767.     }
  768.     return (0);
  769. }
  770.  
  771. /*
  772.  Inertial20.calibrate();
  773.   // waits for the Inertial Sensor to calibrate
  774.   while (Inertial11.isCalibrating()) {
  775.     wait(100, msec);
  776.   }
  777.   // Turns the robot to the right
  778.   LeftMotor.spin(forward);
  779.   RightMotor.spin(reverse);
  780.   // Waits until the motor reaches a 90 degree turn and stops the Left and
  781.   // Right Motors.
  782.   waitUntil((Inertial20.rotation(degrees) >= 90.0));
  783.   LeftMotor.stop();
  784.   RightMotor.stop();
  785.   wait(1, seconds);
  786. }
  787.  
  788. */
  789.  
  790. void GyroTurn (double Degree, int veloc= 50)
  791.  
  792.  
  793. {
  794.  
  795.   LeftMotorB.setVelocity(veloc, velocityUnits::pct);
  796.   LeftMotorF.setVelocity(veloc, velocityUnits::pct);
  797.   RightMotorF.setVelocity(veloc, velocityUnits::pct);
  798.   RightMotorB.setVelocity(veloc, velocityUnits::pct);
  799.  
  800.   while((Inertial11.rotation(degrees) <= Degree))
  801.   {
  802. LeftMotorB.spin(forward);
  803. LeftMotorF.spin(forward);
  804. RightMotorB.spin(reverse);
  805. RightMotorF.spin(reverse);
  806.   }
  807.  
  808. LeftMotorB.stop();
  809. LeftMotorF.stop();
  810. RightMotorB.stop();
  811. RightMotorF.stop();
  812. }
  813.  
  814.  
  815.  
  816. void autonomous(void) {
  817.  
  818. PidFoward(1200);
  819.  
  820. return;
  821.  
  822. }
  823.  
  824.  
  825.  
  826. /*---------------------------------------------------------------------------*/
  827. /*                                                                           */
  828. /*                              User Control Task                            */
  829. /*                                                                           */
  830. /*  This task is used to control your robot during the user control phase of */
  831. /*  a VEX Competition.                                                       */
  832. /*                                                                           */
  833. /*  You must modify the code to add your own robot specific commands here.   */
  834. /*---------------------------------------------------------------------------*/
  835.  
  836. void usercontrol(void) {
  837.   // User control code here, inside the loop
  838.   while (true) {
  839.  
  840. task CalliftTask(LiftTask);
  841.  
  842. RightMotorB.spin(directionType::fwd, Controller1.Axis2.position(percentUnits::pct),velocityUnits::pct);
  843. RightMotorF.spin(directionType::fwd, Controller1.Axis2.position(percentUnits::pct),velocityUnits::pct);
  844. LeftMotorF.spin(directionType::fwd, Controller1.Axis3.position(percentUnits::pct),velocityUnits::pct);
  845. LeftMotorB.spin(directionType::fwd, Controller1.Axis3.position(percentUnits::pct),velocityUnits::pct);
  846.  
  847. if(Controller1.ButtonL2.pressing()) {
  848.   Scooper.spin(vex::directionType::fwd, 100, velocityUnits::pct);
  849.     Scooper2.spin(vex::directionType::fwd, 100, velocityUnits::pct);
  850. }
  851. else if (Controller1.ButtonL1.pressing()) {
  852.    Scooper.spin(vex::directionType::rev, 100, velocityUnits::pct);
  853.     Scooper2.spin(vex::directionType::rev, 100, velocityUnits::pct);
  854. }
  855. else {
  856.   Scooper.stop(brakeType::brake);
  857.   Scooper2.stop(brakeType::brake);
  858. }
  859.  
  860.  
  861.     wait(20, msec); // Sleep the task for a short amount of time to
  862.                     // prevent wasted resources.
  863.   }
  864. }
  865.  
  866. //
  867. // Main will set up the competition functions and callbacks.
  868. //
  869. int main() {
  870.  
  871.  
  872.  
  873.  
  874.   // Set up callbacks for autonomous and driver control periods.
  875.  
  876.   Competition.autonomous(autonomous);
  877.  
  878.   Competition.drivercontrol(usercontrol);
  879.  
  880.   // Run the pre-autonomous function.
  881.   pre_auton();
  882.  
  883.   // Prevent main from exiting with an infinite loop.
  884.   while (true) {
  885.     wait(100, msec);
  886.   }
  887. }
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!