API tools faq
paste
Advertisement
Guest User

3142

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