API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Dec 13th, 2019
94
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 20.85 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. // LeftFront            motor         12              
  14. // RightFront           motor         10              
  15. // LeftBack             motor         17              
  16. // MGL                  motor         18              
  17. // Lift                 motor         19              
  18. // RightBack            motor         13              
  19. // Controller1          controller                    
  20. // IntakeL              motor         14              
  21. // IntakeR              motor         20              
  22. // MGL_LIMIT            limit         H              
  23. // Auton                limit         F              
  24. // ---- END VEXCODE CONFIGURED DEVICES ----
  25.  
  26. #include "vex.h"
  27.  
  28. using namespace vex;
  29.  
  30. // A global instance of competition
  31. competition Competition;
  32.  
  33. // define your global instances of motors and other devices here
  34.  
  35. /*---------------------------------------------------------------------------*/
  36. /*                          Pre-Autonomous Functions                         */
  37. /*                                                                           */
  38. /*  You may want to perform some actions before the competition starts.      */
  39. /*  Do them in the following function.  You must return from this function   */
  40. /*  or the autonomous and usercontrol tasks will not be started.  This       */
  41. /*  function is only called once after the V5 has been powered on and        */
  42. /*  not every time that the robot is disabled.                               */
  43. /*---------------------------------------------------------------------------*/
  44.  
  45. int count;
  46. int press;
  47.  
  48. int select = 0;
  49.  
  50. int SL = 15.1/2;   //left right distance from tracking center to tracking wheels.
  51. int SR = 15.1/2;
  52.  
  53. int SS; //forward backward distance from tracking center to back tracking wheel.
  54. float PreviousAbsTheta; // absolute orientation from previous loop.
  55. float NewAbsTheta; // new absolute orientation.
  56. float DeltaTheta; // difference between thetas of each loop.
  57. float LeftEncoderRotation, RightEncoderRotation,StrafeEncoderRotation, SrotPrev, LrotPrev, RrotPrev;
  58. float DLcurrent, DRcurrent, DScurrent, DLprevious, DRprevious, DSprevious, DeltaLeftInches , DeltaRightInches , DeltaStrafeInches  ; // change in each encoder value of each tracking wheel since last cycle as inches
  59. float DLr, DRr,DSr ; //change in each encoder value as degrees
  60. double localOffsetX, localOffsetY;
  61. float GlobalTheta; //global theta, ultimate orientation of the robot.
  62. float AvgTheta; // average orientation.
  63. float xl,yl; //cartesian coordinates for this loop
  64. float x, y;
  65. double r; //this is just the hypotenuse made by xl and xy
  66. double q; // q=r^
  67. float LIMIT;
  68.  
  69.    
  70. int PositionTracking(void)
  71. {    
  72.    {  
  73.        LeftEncoderRotation = LeftFront.rotation(rotationUnits::deg);   //collecting current encoder values
  74.        RightEncoderRotation = RightFront.rotation(rotationUnits::deg);
  75.        //StrafeEncoderRotation = StrafeTr.rotation(rotationUnits::deg);
  76.        
  77.        DLr = LeftEncoderRotation - LrotPrev;
  78.        DRr = RightEncoderRotation - RrotPrev; //Difference between encoder values in rotationUnits degrees
  79.        //DSr = StrafeEncoderRotation - SrotPrev; //since last loop
  80.            
  81.        DLcurrent = (DLr/360) * (12.959);
  82.        DRcurrent = (DRr/360) * (12.959); //The difference between encoder values since last loop
  83.        //DScurrent = (DSr/360) * (12.959); // converted to inches.
  84.        
  85.        DeltaLeftInches = DLcurrent - DLprevious;
  86.        DeltaRightInches = DRcurrent - DRprevious;  //Distance in inches each wheel has turned since last loop.
  87.        //DeltaStrafeInches = DScurrent - DSprevious;
  88.        
  89.        NewAbsTheta = PreviousAbsTheta + (DLr-DRr)/15.1; //orientation of robot in this loop.
  90.        
  91.        DeltaTheta = NewAbsTheta-PreviousAbsTheta; //difference between orientation of robot between this loop and last loop.
  92.        
  93.        DScurrent = (DeltaTheta/360) * (47.43);
  94.        DeltaStrafeInches = DScurrent - DSprevious;
  95.        
  96.        GlobalTheta = GlobalTheta+NewAbsTheta; //updating global orientation of robot.
  97.        
  98.             if(DeltaTheta==0)
  99.             {localOffsetX = DeltaStrafeInches;
  100.              localOffsetY = DeltaRightInches;
  101.             }
  102.        
  103.             if(DeltaTheta!=0)
  104.             {
  105.                 localOffsetX = 2*sin(GlobalTheta/2)+DeltaStrafeInches/DeltaTheta +SR ;
  106.                 localOffsetY = 2*sin(GlobalTheta/2)+DeltaRightInches/DeltaTheta + SR;
  107.             }
  108.        
  109.        AvgTheta = PreviousAbsTheta + DeltaTheta/2; // average orientation of robot in maneuver.
  110.        q = (((localOffsetX)*(localOffsetX))+((localOffsetY)*(localOffsetY)));
  111.        r = sqrt(q); //calculate the length of hypotenuse made by xl and yl.
  112.        
  113.        xl = r * (cos(DeltaTheta-AvgTheta) ); //Here we convert the cartesian coordinates to polar coordinates,
  114.        yl = r * (sin(DeltaTheta-AvgTheta) ); //then rotate those polar coordinates by our Tm. Then we can convert back to cartesian coordinates
  115.        
  116.        x = x + xl;  //updating global xy coordinates
  117.        y = y + yl;
  118.        
  119.        DLprevious =  DLcurrent;
  120.        DRprevious =  DRcurrent;  //Reassigning current distance each wheel has traveled
  121.        DSprevious =  DScurrent;  //in inches as past values for the next loop.
  122.        
  123.       LrotPrev = LeftEncoderRotation;
  124.       RrotPrev = RightEncoderRotation;  //Reassigning current encoder values of each wheel as
  125.       SrotPrev = StrafeEncoderRotation;  //past value for the next loop.
  126.       Brain.Screen.clearScreen();
  127.       Brain.Screen.printAt(5,10,"X = %f",x);
  128.       Brain.Screen.printAt(5,40,"Y = %f",y);
  129.       Brain.Screen.printAt(5,70,"GlobalTheta = %f",GlobalTheta);  
  130.        task::sleep(20);
  131.    }
  132.    
  133.     return(0);
  134. }
  135.    
  136.  
  137.  
  138. double circumference = 12.959; // circumference of wheel in inches
  139. double rotateDegree;
  140. double turningCircumference = 33.725;
  141.  
  142. void moveForInch(double target,double speed) {
  143.     rotateDegree = (360*target)/circumference ;
  144.    LeftFront.setVelocity(speed,vex::velocityUnits::rpm);
  145.   RightFront.setVelocity(speed,vex::velocityUnits::rpm);
  146.   LeftBack.setVelocity(speed,vex::velocityUnits::rpm);
  147.   RightBack.setVelocity(speed,vex::velocityUnits::rpm);
  148.  
  149.     LeftFront.rotateFor(rotateDegree, vex::rotationUnits::deg, false);  
  150.     LeftBack.rotateFor(rotateDegree, vex::rotationUnits::deg, false);
  151.     RightFront.rotateFor(rotateDegree, vex::rotationUnits::deg,false);
  152.     RightBack.rotateFor(rotateDegree, vex::rotationUnits::deg);
  153.  
  154.    
  155. }
  156.  
  157. void driveTrainStop(void){
  158.    
  159.     LeftFront.stop(brakeType::hold);
  160.     RightFront.stop(brakeType::hold);
  161.     LeftBack.stop(brakeType::hold);
  162.     RightBack.stop(brakeType::hold);
  163. }
  164.  
  165. void turnForDegrees(double degrees){
  166.    
  167.     double  rotateinch = ((degrees/360)*(turningCircumference)) ;
  168.     double leftRotateDegree = ((-1)*(360*rotateinch)/circumference );
  169.     double rotateDegree = ((360*rotateinch)/circumference);
  170.  
  171.     LeftFront.setVelocity(100,vex::velocityUnits::rpm);
  172.     RightFront.setVelocity(100,vex::velocityUnits::rpm);
  173.     LeftBack.setVelocity(100,vex::velocityUnits::rpm);
  174.     RightBack.setVelocity(100,vex::velocityUnits::rpm);
  175.     LeftFront.startRotateFor(leftRotateDegree, vex::rotationUnits::deg);
  176.     LeftBack.startRotateFor(leftRotateDegree, vex::rotationUnits::deg);
  177.     RightFront.startRotateFor(rotateDegree, vex::rotationUnits::deg);
  178.     RightBack.rotateFor(rotateDegree, vex::rotationUnits::deg);
  179. }
  180.  
  181. int leftRotateDegree;
  182. void turnForInch(double target)
  183. {
  184.     rotateDegree = (360*target)/circumference ;
  185.     leftRotateDegree = ((-1)*(360*target)/circumference);
  186.     LeftFront.rotateFor(leftRotateDegree, vex::rotationUnits::deg, false);
  187.     LeftBack.rotateFor(leftRotateDegree, vex::rotationUnits::deg, false);
  188.     RightBack.rotateFor(rotateDegree, vex::rotationUnits::deg,false);
  189.     RightFront.rotateFor(rotateDegree, vex::rotationUnits::deg);
  190. }
  191. void moveLeftForInch(float target) {
  192.               rotateDegree = (360*target)/circumference ;
  193.               LeftFront.rotateFor(rotateDegree, vex::rotationUnits::deg, false);
  194.               LeftBack.rotateFor(rotateDegree, vex::rotationUnits::deg); }
  195. void moveRightForInch(float target)
  196.         {
  197.               rotateDegree = (360*target)/circumference ;
  198.               RightFront.rotateFor(rotateDegree, vex::rotationUnits::deg, false);
  199.               RightBack.rotateFor(rotateDegree, vex::rotationUnits::deg);
  200.         }
  201.  
  202. void speed(double target)
  203. {
  204.   LeftFront.setVelocity(target,vex::velocityUnits::rpm);
  205.   RightFront.setVelocity(target,vex::velocityUnits::rpm);
  206.   LeftBack.setVelocity(target,vex::velocityUnits::rpm);
  207.   RightBack.setVelocity(target,vex::velocityUnits::rpm);
  208. }
  209.  
  210. void autonLiftTo(double height)
  211. {
  212.     Lift.rotateTo(height,rotationUnits::deg);
  213.    
  214.     Lift.stop(brakeType::hold);
  215. }
  216.  
  217. void pre_auton( void ) {
  218.   // All activities that occur before the competition starts
  219.   // Example: clearing encoders, setting servo positions, ...
  220.   count = 0;
  221.   press = 0;
  222.  
  223.    
  224.   LeftFront.setStopping(brakeType::brake);
  225.   RightFront.setStopping(brakeType::brake);
  226.   RightBack.setStopping(brakeType::brake);
  227.   LeftBack.setStopping(brakeType::brake);
  228.  
  229.  
  230. }
  231.  
  232.  
  233. /*---------------------------------------------------------------------------*/
  234. /*                                                                           */
  235. /*                              Autonomous Task                              */
  236. /*                                                                           */
  237. /*  This task is used to control your robot during the autonomous phase of   */
  238. /*  a VEX Competition.                                                       */
  239. /*                                                                           */
  240. /*  You must modify the code to add your own robot specific commands here.   */
  241. /*---------------------------------------------------------------------------*/
  242.  
  243. int Delay;
  244.  
  245. void autonomous( void )
  246. {
  247.  
  248.     int number = select;
  249.  
  250.   /*
  251.     MGL.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);
  252.     vex::task::sleep(750);
  253.     MGL.spin(vex::directionType::fwd, 100, vex::velocityUnits::pct);
  254.     vex::task::sleep(750);
  255.     MGL.stop(brakeType::coast);
  256.  
  257.  
  258.     Lift.spin(vex::directionType::fwd, 100, vex::velocityUnits::pct);
  259.     vex::task::sleep(750);
  260.     Lift.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);
  261.     vex::task::sleep(750);
  262.     Lift.stop(brakeType::coast);
  263.     */
  264.     IntakeL.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);
  265.     IntakeR.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);
  266.    
  267.     moveForInch(-20,100); //get first cubes
  268.     moveForInch(-22,50); //get first cubes
  269.     IntakeL.stop(brakeType::coast);
  270.     IntakeR.stop(brakeType::coast);
  271.    
  272.     vex::task::sleep(100);
  273.    
  274.     moveForInch(27.5,100);//move back
  275.    
  276.     vex::task::sleep(100);
  277.    
  278.     turnForDegrees(-75); //angle
  279.    
  280.     vex::task::sleep(100);
  281.  
  282.     moveForInch(25,100); //move back
  283.        
  284.     vex::task::sleep(100);
  285.    
  286.     turnForDegrees(86); //straighten
  287.    
  288.     vex::task::sleep(100);
  289.    
  290.     RightFront.spin(vex::directionType::fwd, 30, vex::velocityUnits::pct);
  291.     RightBack.spin(vex::directionType::fwd, 30, vex::velocityUnits::pct);
  292.     LeftBack.spin(vex::directionType::fwd, 30, vex::velocityUnits::pct);
  293.     LeftFront.spin(vex::directionType::fwd, 30, vex::velocityUnits::pct);
  294.  
  295.     vex::task::sleep(1000);
  296.  
  297.     RightFront.stop(brakeType::coast);
  298.     LeftFront.stop(brakeType::coast);
  299.     RightBack.stop(brakeType::coast);
  300.     LeftBack.stop(brakeType::coast);
  301.  
  302.     vex::task::sleep(100);
  303.  
  304.     IntakeL.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);
  305.     IntakeR.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);
  306.    
  307.     IntakeL.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);
  308.     IntakeR.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);
  309.  
  310.     moveForInch(-16,100); //drive into second cube stack
  311.  
  312.     moveForInch(-15,40); //drive into second cube stack
  313.    
  314.     vex::task::sleep(100);
  315.    
  316.     IntakeL.spin(vex::directionType::rev, 30, vex::velocityUnits::pct);
  317.     IntakeR.spin(vex::directionType::rev, 30, vex::velocityUnits::pct);
  318.    
  319.     turnForDegrees(148*number); //turn to zone
  320.    
  321.     Controller1.Screen.clearScreen();  
  322.    
  323.     IntakeR.stop(brakeType::coast);
  324.     IntakeL.stop(brakeType::coast);
  325.  
  326.     vex::task::sleep(100);
  327.  
  328.     moveForInch(-25,80); //move into zone
  329.    
  330.     vex::task::sleep(100);
  331.    
  332.     IntakeL.spin(vex::directionType::fwd, 30, vex::velocityUnits::pct);
  333.     IntakeR.spin(vex::directionType::fwd, 30, vex::velocityUnits::pct);
  334.     MGL.spin(vex::directionType::rev, 25, vex::velocityUnits::pct);
  335.  
  336.     task::sleep(1000);
  337.    
  338.     IntakeR.stop(brakeType::coast);
  339.     IntakeL.stop(brakeType::coast);
  340.  
  341.     MGL.spin(vex::directionType::rev, 25, vex::velocityUnits::pct);
  342.  
  343.     task::sleep(3500);
  344.    
  345.     MGL.stop(brakeType::hold);
  346.     IntakeL.spin(vex::directionType::fwd, 20, vex::velocityUnits::pct);
  347.     IntakeR.spin(vex::directionType::fwd, 20, vex::velocityUnits::pct);
  348.     task::sleep(200);
  349.     moveForInch(15,50);
  350.    
  351.    
  352. }
  353. int DriveTrain(void){
  354.  
  355.          //Set the left and right motor to spin forward using the controller Axis values as the velocity value.
  356.         LeftFront.spin(vex::directionType::rev, Controller1.Axis3.value(), vex::velocityUnits::pct);
  357.         LeftBack.spin(vex::directionType::rev, Controller1.Axis3.value(), vex::velocityUnits::pct);
  358.        
  359.        
  360.         RightBack.spin(vex::directionType::rev, Controller1.Axis2.value(), vex::velocityUnits::pct);
  361.         RightFront.spin(vex::directionType::rev, Controller1.Axis2.value(), vex::velocityUnits::pct);    
  362.         if (Controller1.Axis2.value() < 5 && Controller1.Axis2.value() > -5)
  363.         {
  364.           RightBack.stop(vex::brakeType::brake);
  365.           RightFront.stop(vex::brakeType::brake);
  366.         }
  367.         if (Controller1.Axis3.value() < 5 && Controller1.Axis3.value() > -5)
  368.         {
  369.           LeftBack.stop(vex::brakeType::brake);
  370.           LeftFront.stop(vex::brakeType::brake);
  371.         }
  372.         if (Controller1.ButtonY.pressing())
  373.         {
  374.           //-1070
  375.           MGL.stop(brakeType::hold);
  376.           IntakeL.spin(vex::directionType::fwd, 70, vex::velocityUnits::pct);
  377.           IntakeR.spin(vex::directionType::fwd, 70, vex::velocityUnits::pct);
  378.          
  379.          
  380.           moveForInch(10,100);
  381.  
  382.           MGL.spin(vex::directionType::fwd, 80, vex::velocityUnits::pct);
  383.           IntakeL.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);
  384.           IntakeR.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);
  385.         }
  386.    
  387.         while(Controller1.ButtonLeft.pressing()) { //If button up is pressed...
  388.  
  389.         LeftFront.spin(vex::directionType::fwd, 100, vex::velocityUnits::pct);
  390.         LeftBack.spin(vex::directionType::fwd, 100, vex::velocityUnits::pct);
  391.        
  392.        
  393.         RightBack.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);
  394.         RightFront.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);
  395.         }
  396.         while(Controller1.ButtonRight.pressing()) { //If button up is pressed...
  397.  
  398.         LeftFront.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);
  399.         LeftBack.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);
  400.        
  401.        
  402.         RightBack.spin(vex::directionType::fwd, 100, vex::velocityUnits::pct);
  403.         RightFront.spin(vex::directionType::fwd, 100, vex::velocityUnits::pct);
  404.         }
  405.        
  406.        while(Controller1.ButtonUp.pressing()) { //If button up is pressed...
  407.  
  408.         LeftFront.spin(vex::directionType::rev, 50, vex::velocityUnits::pct);
  409.         LeftBack.spin(vex::directionType::rev, 50, vex::velocityUnits::pct);
  410.        
  411.        
  412.         RightBack.spin(vex::directionType::rev, 50, vex::velocityUnits::pct);
  413.         RightFront.spin(vex::directionType::rev, 50, vex::velocityUnits::pct);
  414.         }
  415.  
  416.         while(Controller1.ButtonDown.pressing()) { //If button up is pressed...
  417.  
  418.         LeftFront.spin(vex::directionType::fwd, 50, vex::velocityUnits::pct);
  419.         LeftBack.spin(vex::directionType::fwd, 50, vex::velocityUnits::pct);
  420.        
  421.        
  422.         RightBack.spin(vex::directionType::fwd, 50, vex::velocityUnits::pct);
  423.         RightFront.spin(vex::directionType::fwd, 50, vex::velocityUnits::pct);
  424.         }
  425.         return(0);
  426.     }
  427.  
  428.  
  429. int Arm(void){
  430.     //Arm Control
  431.         if(Controller1.ButtonX.pressing()) { //If button up is pressed...
  432.             //...Spin the arm motor forward.
  433.            
  434.              Lift.spin(vex::directionType::fwd, 100, vex::velocityUnits::pct);
  435.         }
  436.         else if(Controller1.ButtonB.pressing()) { //If the down button is pressed...
  437.             //...Spin the arm motor backward.
  438.            
  439.             Lift.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);
  440.         }
  441.        
  442.         else { //If the the up or down button is not pressed...
  443.             //...Stop the arm motor.
  444.      
  445.             Lift.stop(vex::brakeType::hold);
  446.         }
  447.     return(0);
  448. }
  449.  
  450.  
  451. int Intake(void){
  452.  
  453.    
  454.  
  455.     if(Controller1.ButtonL2.pressing())
  456.     {
  457.         IntakeL.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);
  458.         IntakeR.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);
  459.     }
  460.    
  461.     else if(Controller1.ButtonL1.pressing())
  462.     {
  463.         IntakeL.spin(vex::directionType::fwd, 100, vex::velocityUnits::pct);
  464.         IntakeR.spin(vex::directionType::fwd, 100, vex::velocityUnits::pct);
  465.     }
  466.    
  467.     else
  468.     {
  469.         IntakeL.stop(brakeType::coast);
  470.         IntakeR.stop(brakeType::coast);
  471.     }
  472.    
  473.     return(0);
  474. }
  475.  
  476.  int MobileGoal(void)
  477.  {
  478.     /*if(Controller1.ButtonR2.pressing()) { //If button up is pressed...
  479.      
  480.       IntakeL.spin(vex::directionType::rev, 75, vex::velocityUnits::pct);
  481.       IntakeR.spin(vex::directionType::rev, 75, vex::velocityUnits::pct);
  482.       MGL.spin(vex::directionType::fwd, 100, vex::velocityUnits::pct);
  483.    
  484.     }*/
  485.  
  486.  
  487.      if(Controller1.ButtonR1.pressing())
  488.     {
  489.         MGL.spin(vex::directionType::rev, 25, vex::velocityUnits::pct);
  490.         IntakeR.spin(vex::directionType::fwd, 20, vex::velocityUnits::pct);
  491.         IntakeL.spin(vex::directionType::fwd, 20, vex::velocityUnits::pct);
  492.     }
  493.      
  494.     else if(Controller1.ButtonA.pressing())
  495.     {
  496.         MGL.spin(vex::directionType::fwd, 80, vex::velocityUnits::pct);
  497.         IntakeL.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);
  498.         IntakeR.spin(vex::directionType::rev, 100, vex::velocityUnits::pct);
  499.     }
  500.     else
  501.     {
  502.         MGL.stop(brakeType::coast);
  503.     }
  504.      
  505.      return(0);
  506.  }
  507.  
  508. /*---------------------------------------------------------------------------*/
  509. /*                                                                           */
  510. /*                              User Control Task                            */
  511. /*                                                                           */
  512. /*  This task is used to control your robot during the user control phase of */
  513. /*  a VEX Competition.                                                       */
  514. /*                                                                           */
  515. /*  You must modify the code to add your own robot specific commands here.   */
  516. /*---------------------------------------------------------------------------*/
  517.  
  518. void usercontrol( void )
  519. {
  520.    
  521.     while (1){      
  522.       Brain.Screen.clearScreen();
  523.     DriveTrain();
  524.     Intake();
  525.     Arm();
  526.     MobileGoal();
  527.     PositionTracking();
  528.       vex::task::sleep(20); //Sleep the task for a short amount of time to prevent wasted resources.
  529.   }
  530.  
  531. }
  532.  
  533. //
  534. // Main will set up the competition functions and callbacks.
  535. //
  536. int main() {
  537.   // Set up callbacks for autonomous and driver control periods.
  538.   Competition.autonomous(autonomous);
  539.   Competition.drivercontrol(usercontrol);
  540.  
  541.   // Run the pre-autonomous function.
  542.  
  543.   // Prevent main from exiting with an infinite loop.
  544.   while (true) {
  545.  
  546.   if (Auton.pressing())
  547.  
  548.   {
  549.  
  550.     if (select < 1)
  551.  
  552.     {
  553.  
  554.       select++;
  555.  
  556.     }
  557.  
  558.   else
  559.  
  560.   {
  561.  
  562.     select = -1;
  563.  
  564.   }
  565.     Controller1.Screen.print("1:RED -1:BLUE 0:BAD");
  566.     Controller1.Screen.print( select);
  567.  
  568.  
  569.   }
  570.  
  571.   wait(100, msec);
  572.  
  573.   }
  574.  
  575.  
  576. }
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!