Advertisement
Mikkel_Serrantes

New Program of 11/13

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