API tools faq
paste
Advertisement
Mikkel_Serrantes

Untitled

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