API tools faq
paste
Mikkel_Serrantes

Untitled

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