API tools faq
paste
Advertisement
Mikkel_Serrantes

MJS 10955m Neyyear

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