API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jan 21st, 2016
82
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 21.02 KB | None | 0 0
raw download clone embed print report
  1. //Autonomous Functions
  2.  
  3.  
  4.  
  5. // Look Up Table for Motor Values
  6. // Used for the purpose of linearzing motor speed
  7. const unsigned int TRUESPEED[128] = {
  8.     0, 0, 0, 0, 0, 0 ,0 ,0 ,0 ,20,
  9.     20,21,21,21,22,22,22,23,24,24,
  10.     25,25,25,25,26,27,27,28,28,28,
  11.     28,29,30,30,30,31,31,32,32,32,
  12.     33,33,34,34,35,35,35,36,36,37,
  13.     37,37,37,38,38,39,39,39,40,40,
  14.     41,41,42,42,43,44,44,45,45,46,
  15.     46,47,47,48,48,49,50,50,51,52,
  16.     52,53,54,55,56,57,57,58,59,60,
  17.     61,62,63,64,65,66,67,67,68,70,
  18.     71,72,72,73,74,76,77,78,79,79,
  19.     80,81,83,84,84,86,86,87,87,88,
  20.     88,89,89,90,127,127,127
  21. };
  22.  /**
  23.  ** This variable return 1 or a -1 for other codes.
  24.  *@author Alex Miller <amm@albion.edu>
  25.  *@since 2015-12-2
  26.  *
  27. */
  28.  int signOf(int value)
  29.  {
  30.     int sign = (abs(value) / value);
  31.  
  32.     return sign;
  33.  }
  34.  
  35.  /**
  36.  ** This function sets the motors for the autonomous functions and tasks.
  37.  *@author Alex Miller <amm@albion.edu>
  38.  
  39.  *
  40. */
  41.  void autonSetMotors(int motor, int speed)
  42.  {
  43.     if (speed > 127)    speed = 127;
  44.     if (speed < -127)   speed = -127;
  45.  
  46.         //Abs values allow the function/motor speed to retain its sign (since TRUESPEED values are all positive)
  47.         //The "- 1" matches the code to the array
  48.     vexMotorSet(motor, signOf(speed) * TRUESPEED[abs(speed) - 1]);
  49.  }
  50.  
  51.  
  52.  /**
  53.  ** This function allows the robot to move forward a given centimeter value in autonomous, using PID techniques.
  54.  *@author Derek Cheyne <dkcheyne399@gmail.com>
  55.  *
  56. */
  57.  void moveForward(float targetDistance)
  58.  {
  59.     vexMotorPositionSet(MOT_NORTH_EAST, 0);
  60.     vexMotorPositionSet(MOT_NORTH_WEST, 0);
  61.  
  62.         float ticksToDistance = (targetDistance / .083705);  // Check Derek's Paper for 2015 Conversion rates (answer is cm forward)
  63.        
  64.        
  65.  
  66.         const float pValue = 0.35;                              //These values converts the error into workable units
  67.         const float iValue = 0.3;                               //Remeber can be changed to make sure that it is actually working
  68.         //const float dValue = 3;                                 //See http://blog.oscarliang.net/quadcopter-pid-explained-tuning/
  69.         float error, speed, iError = 0;                          
  70.         //float wasHere = 0;
  71.  
  72.         int i = 1;                                              //i is the number of times the robot has hit correct location
  73.         bool hitTarget = false;                                 //true or false depending on if robot has hit target destination
  74.        
  75.         while(!hitTarget && !(escapeTime()) )   //while robot has not hit target and kill switch is not activated
  76.         {
  77.  
  78.  
  79.             error = (targetDistance * .083705) - ticksToDistance;               //How far off the right encoder's count is from the targetDistance count
  80.             //double dError = ticksToDistance - wasHere;
  81.             speed = error * pValue + iError * iValue; //ß+ dError * dValue;
  82.  
  83.             if (abs(speed) <= 123)
  84.             {
  85.                     iError += error * 0.025;        //iError is the area under error curve (the "current")
  86.                 }
  87.                 else
  88.                 {
  89.                     speed = 123;
  90.                 }
  91.  
  92.             speed *= signOf(targetDistance);                            //If number of cm is negative, then speed of motors will be (-) for going backward
  93.  
  94.             //Setting Motors (With Corrections for Straightening)
  95.             autonSetMotors(MOT_NORTH_EAST, -(speed + 4) );
  96.             autonSetMotors(MOT_NORTH_WEST, (speed + 4) );
  97.             autonSetMotors(MOT_SOUTH_EAST, -(speed + 4) );
  98.             autonSetMotors(MOT_SOUTH_WEST, (speed + 4) );
  99.  
  100.             if(ticksToDistance <=  targetDistance + 6 && ticksToDistance >= targetDistance - 6)
  101.             {  
  102.                     i += 1;                                         //Each time it hits target, 1 is added to i
  103.                     if (i > 2) hitTarget = true;                   
  104.                 }
  105.             else i = 1;                                         //If robot does not hit target 7 times in a row, i becomes 1
  106.  
  107.             //wasHere = ticksToDistance;
  108.             vexSleep(25);
  109.         }
  110.     }
  111. /**
  112.  ** This function allows the robot to move forward a given amount of centimeters in autonomous. This does not involve the PID techniques.
  113.  *@author Annelise Comai <annelise871@aol.com>
  114.  *
  115. */
  116.  void moveForwardNoPID(int targetDistance)
  117.  {
  118.     vexMotorPositionSet(MOT_NORTH_EAST, 0);
  119.     vexMotorPositionSet(MOT_NORTH_WEST, 0);
  120.  
  121.         float ticksToDistance = (targetDistance / .083705); //See Dereks Paper for Conversion from encoder counts to distance
  122.  
  123.         float intialDegree = vexGyroGet();
  124.  
  125.         while(vexMotorPositionGet(MOT_NORTH_WEST) < (ticksToDistance) && (vexControllerGet(Btn7L) != 1)) // Get to 85% of the distance at "full" speed along with angle helping  
  126.         {
  127.  
  128.             vexMotorSet(MOT_NORTH_WEST, 120);    // Tell the motors to run forward
  129.             vexMotorSet(MOT_SOUTH_WEST, 120);
  130.             vexMotorSet(MOT_SOUTH_EAST, -120);
  131.             vexMotorSet(MOT_NORTH_EAST, -120);
  132.  
  133.             float MotNorthWestSpeed = vexMotorGet(MOT_NORTH_WEST);
  134.  
  135.             float MotNorthEastSpeed = vexMotorGet(MOT_NORTH_EAST);
  136.  
  137.             float degreeNow = vexGyroGet();
  138.            
  139.             if (degreeNow > intialDegree)
  140.             {
  141.                 vexMotorSet(MOT_NORTH_WEST, MotNorthWestSpeed + 1);
  142.                 vexMotorSet(MOT_NORTH_EAST, MotNorthEastSpeed - 1);
  143.             }
  144.             if (degreeNow < intialDegree)
  145.             {
  146.                 vexMotorSet(MOT_NORTH_WEST, MotNorthWestSpeed - 1);
  147.                 vexMotorSet(MOT_NORTH_EAST, MotNorthEastSpeed + 1);
  148.             }
  149.             if (vexControllerGet(Btn7D) = 1)
  150.             {
  151.                 break;
  152.             }
  153.         }
  154.         /*
  155.         while(vexMotorPositionGet(MOT_NORTH_WEST) > (.85 * ticksToDistance) && (MOT_NORTH_WEST < ticksToDistance) && !escapeTime()) // Once it gets to 85% go about 1/4th speed to end
  156.         {
  157.             vexMotorSet(MOT_NORTH_WEST, 30);    // Tell the motors to run forward
  158.             vexMotorSet(MOT_SOUTH_WEST, 30);
  159.             vexMotorSet(MOT_SOUTH_EAST, -30);
  160.             vexMotorSet(MOT_NORTH_EAST, -30);
  161.  
  162.             float MotNorthWestSpeed = vexMotorGet(MOT_NORTH_WEST);
  163.  
  164.             float MotNorthEastSpeed = vexMotorGet(MOT_NORTH_EAST);
  165.  
  166.             float degreeNow = vexGyroGet();
  167.  
  168.  
  169.             if (degreeNow > intialDegree)
  170.             {
  171.               vexMotorSet(MOT_NORTH_WEST, MotNorthWestSpeed + 1);
  172.               vexMotorSet(MOT_NORTH_EAST, MotNorthEastSpeed - 1);
  173.             }
  174.             if (degreeNow < intialDegree)
  175.             {
  176.               vexMotorSet(MOT_NORTH_WEST, MotNorthWestSpeed - 1);
  177.               vexMotorSet(MOT_NORTH_EAST, MotNorthEastSpeed + 1);  
  178.             }
  179.  
  180.             if ((MOT_NORTH_WEST >= ticksToDistance) || vexControllerGet(5U))
  181.             {
  182.                 break;
  183.             }
  184.             */
  185.  
  186.  
  187.         }
  188.  
  189. /*void turnRight(int degrees)
  190.     {
  191.         float turnTarget = vexGyroGet() + degrees;      //This is the intended amount of degrees the robot will turn
  192.         bool hitTarget = false;                         //This is whether the robot has hit the turnTarget or not
  193.         int i = 0;                                      //This is a counting variable that assists with determining hitTarget's truth value
  194.         while( ( abs(vexGyroGet()) < abs(vexGyroGet + degrees) ) && !hitTarget)
  195.         {
  196.             float currentTurn = vexGyroGet();               //This is the current amount of turn the robot has completed (this is within the while loop so that it refreshes itself)
  197.             float error = (turnTarget - currentTurn);       //This is the amount of turn the robot still has to go - how "off" of the target the robot is
  198.            
  199.             autonSetMotors(MOT_NORTH_EAST, error);          //As the error decreases, the speed that the motors run at will also decrease (robot slows down as it approaches its target)
  200.             autonSetMotors(MOT_NORTH_WEST, error);
  201.             autonSetMotors(MOT_SOUTH_EAST, error);
  202.             autonSetMotors(MOT_SOUTH_WEST, error);
  203.             if (vexGyroGet() <= (turnTarget + 4) && vexGyroGet() >= (turnTarget - 4) )
  204.                 {
  205.                     i += 1;
  206.                     if (i >= 4)
  207.                         {
  208.                             hitTarget = true;
  209.                         }
  210.                     else
  211.                         {
  212.                             i = 1;
  213.                         }
  214.                 }
  215.         }
  216.     }
  217. */
  218.  /**
  219.  ** This function allows the robot to turn a given amount of degrees to the right in autonomous without PID techniques.
  220.  *@author Annelise Comai <annelise871@aol.com>
  221.  *
  222. */
  223.  void turnRightNoPID(int degrees)
  224.  {
  225.     int turnCurrent = abs((vexGyroGet() / 10));
  226.  
  227.         while(abs(vexGyroGet() / 10) < abs((degrees)) && !escapeTime()) // && !escapeTime) //while(abs(vexGyroGet() / 10) < abs((turnCurrent + degrees)) ) //took out turnCurrent
  228. {
  229.     vexMotorSet(MOT_NORTH_WEST, 96);
  230.     vexMotorSet(MOT_SOUTH_WEST, 96);
  231.     vexMotorSet(MOT_SOUTH_EAST, 96);
  232.     vexMotorSet(MOT_NORTH_EAST, 96);
  233. }
  234. }
  235.  /**
  236.  ** This function allows the robot to turn a given amount of degrees to the left in autonomous without PID techniques.
  237.  *@author Annelise Comai <annelise871@aol.com>
  238.  *
  239. */
  240.  
  241.  void turnLeftNoPID(int degrees)
  242.  {  
  243.  
  244.  
  245.     float turnCurrent = (vexGyroGet() / 10);
  246.  
  247.             while(fabs(vexGyroGet() / 10) < fabs((turnCurrent + degrees - 20 )) && !escapeTime() ) // The -20 is to say once withen 20 degress change motors to move slower to minimize error
  248.             {
  249.                 vexMotorSet(MOT_NORTH_WEST, -96);
  250.                 vexMotorSet(MOT_SOUTH_WEST, -96);
  251.                 vexMotorSet(MOT_SOUTH_EAST, -96);
  252.                 vexMotorSet(MOT_NORTH_EAST, -96);
  253.  
  254.  
  255.                 if ((fabs(vexGyroGet()) /10 > fabs((turnCurrent + degrees))) || vexControllerGet(5U))  
  256.                 {
  257.                     break;
  258.                 }
  259.             }
  260.             while(fabs(vexGyroGet()) / 10 < fabs((turnCurrent + degrees)) && !escapeTime())
  261.             {
  262.                 vexMotorSet(MOT_NORTH_WEST, -20);
  263.                 vexMotorSet(MOT_SOUTH_WEST, -20);
  264.                 vexMotorSet(MOT_SOUTH_EAST, -20);
  265.                 vexMotorSet(MOT_NORTH_EAST, -20);
  266.  
  267.                 if ((fabs(vexGyroGet()) /10 > fabs((turnCurrent + degrees))) || vexControllerGet(5U))  
  268.                 {
  269.                     break;
  270.                 }
  271.             }
  272.         }
  273. /**
  274.  ** This function allows the robot to autonoumously run the intake mechanism and bring the ball through the elevator and the launcher.
  275.  *@author Maycee McClure <maymcc01@gmail.com>
  276.  *
  277. */
  278.  /* void autonIntakeBall(void)
  279.  {
  280.     isIntakeOn = true;                                                              //Turns intake on
  281.             while(vexMotorPositionGet(MOT_ELEVATOR) < 800 && !escapeTime())         //Brings single ball up elevator and hopefully through launcher
  282.             {
  283.                 chThdSleepMilliseconds(25);
  284.             }
  285.             isIntakeOn = false;
  286.         }
  287. /**
  288.  ** This task governs the intake during both autonomous and driver control based on the isIntakeOn global variable.
  289.  *@author Annelise Comai <annelise871@aol.com>
  290.  *
  291. */
  292.  /*
  293.  task autonRunIntake(void *arg)
  294.  {
  295.     (void)arg;
  296.     vexTaskRegister("autonIntake");
  297.     while(1)
  298.     {
  299.        
  300.         //while(!escapeTime)
  301.         //{
  302.             if(isIntakeOn)  
  303.             {
  304.  
  305.                 vexMotorSet(MOT_INTAKE, 127);
  306.                 vexMotorSet(MOT_ELEVATOR, 127);
  307.             }
  308.             else {
  309.  
  310.                 vexMotorSet(MOT_INTAKE, 0);
  311.                 vexMotorSet(MOT_ELEVATOR, 0);
  312.  
  313.             }
  314.     //  }
  315.  
  316.         //}
  317.     }
  318.  }
  319.  
  320.  
  321.  
  322. /**
  323.  ** This function was orginally supposed to "Run Intake and Start Launcher from red square to launch pre-loaded balls" in autonomous. It does not work because the motors will autmoatically start at 80, so the intake will start with the launcher.
  324.  *@author Maycee McClure <maymcc01@gmail.com>
  325.  *
  326. */
  327. // Run Intake and Start Launcher from red square to launch pre-loaded balls
  328. /* void runIntakeLauncher(void)
  329.  {
  330.  
  331.         isLaunchOn = true; //starts launcher
  332.         while (vexMotorGet(MOT_FLY_ONE) < 80 && !escapeTime())
  333.         { //once fly wheel motors are at full speed
  334.             isIntakeOn = false; //starts intake
  335.         }
  336.         isIntakeOn = true;
  337.  
  338.     }
  339.  
  340.  
  341. /**
  342.  ** This function will run the intake and launcher to launch the four balls that are placed in the robot before a match.
  343.  *@author Analise Comai
  344.  *
  345. */
  346. /*
  347.  void autonShootFourBalls(void) {
  348.  
  349.             //////////This is code for an auton program focused on launching the four preloads (currently written for three).  Place robot on starting tile directly facing goal with three balls in the intake, with the ball furthest in pressed up against the Moibus strip chain guard box (this auton works with no balls actually in elevator, just one at the bottom of the elevator, this can be changed later)
  350. //while(!escapeTime)
  351. //{
  352.  
  353.     vexMotorPositionSet(MOT_ELEVATOR, 0);
  354.  
  355.             //Warm Up Launcher
  356.             vexMotorSet(MOT_FLY_ONE, 127);
  357.             vexMotorSet(MOT_FLY_TWO, 127);
  358.             vexMotorSet(MOT_FLY_THREE, 127);
  359.             vexMotorSet(MOT_FLY_FOUR, 127);                                 //Turns on launcher (make sure that autonShootBalls task is started before trying this)
  360.            
  361.             chThdSleepMilliseconds(2500);                       //Time needed for launcher to get up to full speed
  362.             vexMotorPositionSet(MOT_ELEVATOR, 0);               //Resets encoder on elevator motor
  363.  
  364.             //Shooting the first ball
  365.             vexMotorSet(MOT_INTAKE, 127);
  366.             vexMotorSet(MOT_ELEVATOR, 127);                                                         //Turns on the intake and the elevator motors (make sure that autonRunIntake task is started before trying this)
  367.             autonIntakeBall();                                  //The elevator motor will run and the elevator encoder will count up to 800 (this is approx. number of elevator encoder counts needed to get ball at the bottom of the elevator shaft up the elevator and through launcher without the other balls coming up the elevator getting stuck on the bottom wheel of the launcher - this could be changed if we shoved a ball halfway up the elevator shaft as well)
  368.                                                                 //Tells robot to keep doing what it's doing until encoder reaches its designated value
  369.             isIntakeOn = false;                                 //Turns off the intake so that the launcher motors can get back up to speed after launching ball
  370.             vexMotorPositionSet(MOT_ELEVATOR, 0);               //Resets elevator encoder
  371.  
  372.             chThdSleepMilliseconds(2500);                       //Time needed for launcher motors to get back up to speed
  373.            
  374.             //Second ball
  375.             autonIntakeBall();                                  //Turns off intake after ball is launched
  376.  
  377.             vexMotorPositionSet(MOT_ELEVATOR, 0);               //Resets elevator encoder
  378.  
  379.             chThdSleepMilliseconds(2500);                       //Launcher motors get back up to speed
  380.  
  381.             //Third Ball
  382.                 autonIntakeBall();                              //Turns on intake
  383.                                                                 //Brings single ball up elevator
  384.  
  385.             //Stop everything
  386.             isIntakeOn = false;                                 //Stops intake
  387.             isLaunchOn = false;                                 //Stops launcher
  388.  
  389.         }
  390.  
  391.  /* void autonShootFourBallsNoTasks(void) {
  392.  
  393.  //////////This is code for an auton program focused on launching the four preloads (currently written for three).  Place robot on starting tile directly facing goal with three balls in the intake, with the ball furthest in pressed up against the Moibus strip chain guard box (this auton works with no balls actually in elevator, just one at the bottom of the elevator, this can be changed later)
  394. //while(!escapeTime)
  395. //{
  396.  
  397.             vexMotorPositionSet(MOT_ELEVATOR, 0);
  398.  
  399.             //Warm Up Launcher
  400.             vexMotorSet(MOT_FLY_ONE, 127);
  401.             vexMotorSet(MOT_FLY_TWO, 127);
  402.             vexMotorSet(MOT_FLY_THREE, 127);
  403.             vexMotorSet(MOT_FLY_FOUR, 127);                                         //Turns on launcher
  404.            
  405.             chThdSleepMilliseconds(2500);                                           //Time needed for launcher to get up to full speed
  406.             vexMotorPositionSet(MOT_ELEVATOR, 0);                                   //Resets encoder on elevator motor
  407.  
  408.             //Shooting the first ball
  409.             vexMotorSet(MOT_INTAKE, 127);
  410.             vexMotorSet(MOT_ELEVATOR, 127);                                         //Turns on the intake and the elevator motors (make sure that autonRunIntake task is started before trying this)
  411.             while(vexMotorPositionGet(MOT_ELEVATOR) < 800 && !escapeTime())         //Brings single ball up elevator and hopefully through launcher
  412.             {
  413.                 chThdSleepMilliseconds(25);
  414.             }                                                                       //The elevator motor will run and the elevator encoder will count up to 800 (this is approx. number of elevator encoder counts needed to get ball at the bottom of the elevator shaft up the elevator and through launcher without the other balls coming up the elevator getting stuck on the bottom wheel of the launcher - this could be changed if we shoved a ball halfway up the elevator shaft as well)
  415.             vexMotorSet(MOT_INTAKE, 0);
  416.             vexMotorSet(MOT_ELEVATOR, 0);                                           //Tells robot to keep doing what it's doing until encoder reaches its designated value
  417.                                                                                     //Turns off the intake so that the launcher motors can get back up to speed after launching ball
  418.             vexMotorPositionSet(MOT_ELEVATOR, 0);                                   //Resets elevator encoder
  419.  
  420.             chThdSleepMilliseconds(2500);                                           //Time needed for launcher motors to get back up to speed
  421.            
  422.             //Second ball
  423.             vexMotorSet(MOT_INTAKE, 127);
  424.             vexMotorSet(MOT_ELEVATOR, 127);                                         //Turns on the intake and the elevator motors (make sure that autonRunIntake task is started before trying this)
  425.             while(vexMotorPositionGet(MOT_ELEVATOR) < 800 && !escapeTime())         //Brings single ball up elevator and hopefully through launcher
  426.             {
  427.                 chThdSleepMilliseconds(25);
  428.             }                                                                       //The elevator motor will run and the elevator encoder will count up to 800 (this is approx. number of elevator encoder counts needed to get ball at the bottom of the elevator shaft up the elevator and through launcher without the other balls coming up the elevator getting stuck on the bottom wheel of the launcher - this could be changed if we shoved a ball halfway up the elevator shaft as well)
  429.             vexMotorSet(MOT_INTAKE, 0);
  430.             vexMotorSet(MOT_ELEVATOR, 0);                                           //Turns off intake after ball is launched
  431.  
  432.             vexMotorPositionSet(MOT_ELEVATOR, 0);                                   //Resets elevator encoder
  433.  
  434.             chThdSleepMilliseconds(2500);                                           //Launcher motors get back up to speed
  435.  
  436.             //Third Ball
  437.             vexMotorSet(MOT_INTAKE, 127);
  438.             vexMotorSet(MOT_ELEVATOR, 127);                                         //Turns on the intake and the elevator motors (make sure that autonRunIntake task is started before trying this)
  439.             while(vexMotorPositionGet(MOT_ELEVATOR) < 800 && !escapeTime())         //Brings single ball up elevator and hopefully through launcher
  440.             {
  441.                 chThdSleepMilliseconds(25);
  442.             }                                                                       //The elevator motor will run and the elevator encoder will count up to 800 (this is approx. number of elevator encoder counts needed to get ball at the bottom of the elevator shaft up the elevator and through launcher without the other balls coming up the elevator getting stuck on the bottom wheel of the launcher - this could be changed if we shoved a ball halfway up the elevator shaft as well)
  443.             vexMotorSet(MOT_INTAKE, 0);
  444.             vexMotorSet(MOT_ELEVATOR, 0);                                           //Turns on intake
  445.                                                                                     //Brings single ball up elevator
  446.  
  447.             //Stop everything
  448.             vexMotorSet(MOT_INTAKE, 0);
  449.             vexMotorSet(MOT_ELEVATOR, 0);                                           //Stops intake
  450.            
  451.             vexMotorSet(MOT_FLY_ONE, 0);
  452.             vexMotorSet(MOT_FLY_TWO, 0);
  453.             vexMotorSet(MOT_FLY_THREE, 0);
  454.             vexMotorSet(MOT_FLY_FOUR, 0);                                           //Stops launcher
  455.  
  456.         }
  457.     //}
  458.  /**
  459.  ** This task allows the robot to launch balls at speed = 127 when called in autonomous.
  460.  *@author Annelise Comai <annelise871@aol.com>
  461.  *
  462. */
  463.  task autonShootBalls(void *arg)
  464.  {
  465.     (void)arg;
  466.     vexTaskRegister("autonLauncher");
  467.     while(1)
  468.     {
  469.         while(!escapeTime())
  470.         {
  471.             float speed = vexMotorGet(MOT_FLY_LEFT);           
  472.         if(isLaunchOn == true)                                  //If the global isLaunchOn variable is declared to be true within the function
  473.         {
  474.             vexMotorSet(MOT_FLY_LEFT, 127);                     //Set all of the launch motors to 127 (can be changed later if needed)
  475.             vexMotorSet(MOT_FLY_RIGHT, 127);
  476.            
  477.  
  478.                     //chThdSleepMilliseconds(time);//
  479.         }
  480.  
  481.  
  482.         else if(speed > 0)                                      //Else, slow down the launcher motors
  483.         {
  484.             speed -= 3;
  485.             chThdSleepMilliseconds(100);
  486.             vexMotorSet(MOT_FLY_ONE, speed);
  487.             vexMotorSet(MOT_FLY_TWO, speed);
  488.             vexMotorSet(MOT_FLY_THREE, speed);
  489.             vexMotorSet(MOT_FLY_FOUR, speed);
  490.         }
  491.     }
  492.  
  493.     /*  else {
  494.            
  495.         vexMotorSet(MOT_FLY_ONE,0);
  496.         vexMotorSet(MOT_FLY_TWO,0);
  497.         vexMotorSet(MOT_FLY_THREE,0);
  498.         vexMotorSet(MOT_FLY_FOUR,0);
  499.         }
  500.     */
  501.     }
  502. }
  503.  
  504.  
  505.  
  506.  
  507.  
  508.  
  509.  
  510.  /**
  511.  ** This task allows the robot to launch balls at speed = 120 when called in autonomous.
  512.  *@author Annelise Comai <annelise871@aol.com>
  513.  *
  514. */
  515.  task autonShootBalls120(void *arg)
  516.  {
  517.     (void)arg;
  518.     vexTaskRegister("autonLauncher");
  519.     while(1)
  520.     {
  521.         //while(!escapeTime())
  522.         //{
  523.         float speed = vexMotorGet(MOT_FLY_ONE);        
  524.        
  525.         if(isLaunchOn == true)                                  //If the global isLaunchOn variable is declared to be true within the function
  526.         {
  527.             vexMotorSet(MOT_FLY_ONE, 120);                      //Set all of the launch motors to 120
  528.             vexMotorSet(MOT_FLY_TWO, 120);
  529.             vexMotorSet(MOT_FLY_THREE, 120);
  530.             vexMotorSet(MOT_FLY_FOUR, 120);
  531.  
  532.                     //chThdSleepMilliseconds(time);//
  533.         }
  534.  
  535.  
  536.         else if(speed > 0)                                      //Else, slow down the launcher motors
  537.         {
  538.             speed -= 3;
  539.             chThdSleepMilliseconds(100);
  540.             vexMotorSet(MOT_FLY_ONE, speed);
  541.             vexMotorSet(MOT_FLY_TWO, speed);
  542.             vexMotorSet(MOT_FLY_THREE, speed);
  543.             vexMotorSet(MOT_FLY_FOUR, speed);
  544.         }
  545.     //}
  546.     }
  547. }
  548.  
  549.  
  550.  
  551.  
  552.  
  553.  
  554.  
  555.   /**
  556.  ** This function will run our entire strategy for the autonomous period using the functions above.
  557.  *@author Maycee McClure <maymcc01@gmail.com>
  558.  *
  559. */
  560.   //Complete Autonomous
  561.   void allAuton(void)                                           //See Albion College Notebook on Auton for written instructions. Need to test.
  562.   {
  563.     autonShootFourBalls();
  564.     moveForwardNoPID(60);
  565.     turnRightNoPID(90);
  566.     moveForwardNoPID(30);
  567.     turnRightNoPID(45);
  568.     moveForwardNoPID(50);                                       //only moving 50, don't wanna hit wall
  569.     autonIntakeBall();                                          //need to do this four times
  570.     chThdSleepMilliseconds(250);                                //wait to take up next one
  571.     autonIntakeBall();
  572.     chThdSleepMilliseconds(250);
  573.     autonIntakeBall();
  574.     chThdSleepMilliseconds(250);
  575.     autonIntakeBall();
  576.     vexMotorSet(MOT_NORTH_WEST, -127);
  577.     vexMotorSet(MOT_SOUTH_WEST, -127);
  578.     vexMotorSet(MOT_SOUTH_EAST, -127);
  579.     vexMotorSet(MOT_NORTH_EAST, -127);
  580.     turnLeftNoPID(135);
  581.     autonShootFourBalls();
  582.   }
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!