API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jul 27th, 2017
91
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 9.02 KB | None | 0 0
raw download clone embed print report
  1. void Autonomous() {
  2.     /*
  3.      * Start arm inside robot
  4.      * Track line while moving arm to EXT_HIG_VOLTAGE
  5.      * Once hit the "T" at the end, stop and reverse for 1 second
  6.      * Move arm to HIG_INNER/OUTER_VOLTAGE
  7.      * Release the claw
  8.      * Drop the arm
  9.      * Back up for some time
  10.      * Turn around (?)
  11.      */
  12.    
  13.     double ArmMult = 0;
  14.    
  15.     bool quitAll = fase;
  16.     bool L = false;
  17.     bool M = false;
  18.     bool R = false;
  19.     bool turning = false;
  20.     bool yHit = false;
  21.     bool stageComplete = false;
  22.     bool ArmAtTargetVoltage = false;
  23.    
  24.     int ArmDir = 0;
  25.    
  26.     double leftMotorSpeed = 0;
  27.     double rightMotorSpeed = 0;
  28.     double SpeedScale = 0;
  29.     double targVolt = 0;
  30.     double curVolt = 0;
  31.  
  32.     Timer *reverseTimer;
  33.     reverseTimer = new Timer; //timer for backing up
  34.    
  35.     Timer *findLineTimer;
  36.     findLineTimer = new Timer; //timer for finding the line
  37.  
  38.     if(tapeSelect1->Get() || tapeSelect2->Get()) { //Do we have autonomous?
  39.         if(tapeSelect1->Get() && !tapeSelect2->Get()) { //Are we in T mode?
  40.             /*=====================================
  41.              // STAGE PURPOSES:
  42.              // -Track straight line
  43.              // -Raise the arm to the maximum height
  44.              //=====================================*/
  45.            
  46.             output->Clear();
  47.             printUserMessage(output, 1, 1, "STAGE 1 - T MODE");
  48.             output->UpdateLCD();
  49.  
  50.             stageComplete = false;
  51.             SpeedScale = 0.5;
  52.             targVolt = EXT_HIG_VOLTAGE;
  53.            
  54.             while(!stageComplete && IsAutonomous())
  55.             {
  56.                 L = trackerL->Get();
  57.                 M = trackerM->Get();
  58.                 R = trackerR->Get();
  59.                
  60.                 //Move arm to the correct voltage
  61.                 curVolt = pot.GetVoltage();
  62.                 if(targVolt-curVolt < -ARM_VOLT_RANGE) {
  63.                     ArmDir = -1;
  64.                 } else if(targVolt-curVolt > ARM_VOLT_RANGE) {
  65.                     ArmDir = 1;
  66.                 } else {
  67.                     ArmDir = 0;
  68.                     ArmAtTargetVoltage = true;
  69.                 }
  70.                
  71.                 if(!armLimitUp.Get()) {
  72.                     motorArm.Set(clamp(0.65 * ArmDir, -1, 0) * ArmMult);
  73.                 } else if(!armLimitDown.Get()) {
  74.                     motorArm.Set(clamp(0.65 * ArmDir, 0, 1) * ArmMult);
  75.                 } else {
  76.                     motorArm.Set(0.65 * ArmDir * ArmMult);
  77.                 }
  78.  
  79.                 if (!L && M && !R) { //OXO
  80.                     //Forward
  81.                     leftMotorSpeed = 1;
  82.                     rightMotorSpeed = 1;
  83.                 }
  84.  
  85.                 if (!L && !M && R) { //OOX
  86.                     //Right
  87.                     leftMotorSpeed = 1;
  88.                     rightMotorSpeed = 0.5;
  89.                 }
  90.  
  91.                 if (L && !M && !R) { //XOO
  92.                     //Left
  93.                     leftMotorSpeed = 0.5;
  94.                     rightMotorSpeed = 1;
  95.                 }
  96.  
  97.                 if ( (L && M && !R) || (!L && M && R) ) { //XXO or OXX
  98.                     //Slow Forward
  99.                     leftMotorSpeed = 0.75;
  100.                     rightMotorSpeed = 0.75;
  101.                 }
  102.  
  103.                 if (L && M && R) //XXX, ALL ON
  104.                 {
  105.                     leftMotorSpeed = 0;
  106.                     rightMotorSpeed = 0;
  107.  
  108.                     if (ArmAtTargetVoltage) {
  109.                         stageComplete = true;
  110.                     }
  111.                 }
  112.  
  113.                 if (!L && !M && !R) { //OOO, ALL OFF
  114.                     if(findLineTimer->Get() <= 0) {
  115.                         findLineTimer->Start();
  116.                     } else if(findLineTimer->Get() < 2) {
  117.                         //Move slowly to find the line
  118.                         leftMotorSpeed = 0.5;
  119.                         rightMotorSpeed = 0.5;
  120.                     } else {
  121.                         leftMotorSpeed = 0;
  122.                         rightMotorSpeed = 0;
  123.                         quitAll = true;
  124.                         break;
  125.                     }
  126.                 }
  127.  
  128.                 setMotorSpeeds(-1*leftMotorSpeed * SpeedScale, rightMotorSpeed * SpeedScale, motorFLeft, motorBLeft, motorFRight, motorBRight);
  129.             }
  130.         }
  131.         else if (tapeSelect1->Get() && tapeSelect2->Get())
  132.         {
  133.             output->Clear();
  134.             printUserMessage(output, 1, 1, "STAGE 1 - Y MODE");
  135.             output->UpdateLCD();
  136.            
  137.             /*=====================================
  138.              // STAGE PURPOSES:
  139.              // -Track Y Tape
  140.              // -Raise the arm to the maximum height
  141.              //=====================================*/
  142.  
  143.             stageComplete = false;
  144.  
  145.             yHit = false; //already turned at the junction
  146.             turning = false; //in the process of turning at the junction or turning before the T
  147.             ArmAtTargetVoltage = false;
  148.            
  149.             SpeedScale = 0.5;
  150.  
  151.             targVolt = EXT_HIG_VOLTAGE;
  152.  
  153.             while(!stageComplete && IsAutonomous() && !quitAll)
  154.             {
  155.                 L = trackerL->Get();
  156.                 M = trackerM->Get();
  157.                 R = trackerR->Get();
  158.                
  159.                 //Move arm to the correct voltage
  160.                 curVolt = pot.GetVoltage();
  161.                 if(targVolt-curVolt < -ARM_VOLT_RANGE) {
  162.                     ArmDir = -1;
  163.                 } else if(targVolt-curVolt> ARM_VOLT_RANGE) {
  164.                     ArmDir = 1;
  165.                 } else {
  166.                     ArmDir = 0;
  167.                     ArmAtTargetVoltage = true;
  168.                 }
  169.  
  170.                 if(!armLimitUp.Get()) {
  171.                     motorArm.Set(clamp(0.65 * ArmDir, -1, 0) * ArmMult);
  172.                 } else if(!armLimitDown.Get()) {
  173.                     motorArm.Set(clamp(0.65 * ArmDir, 0, 1) * ArmMult);
  174.                 } else {
  175.                     motorArm.Set(0.65 * ArmDir * ArmMult);
  176.                 }
  177.                
  178.                 if(!turning) {
  179.                     if (!L && M && !R) { //OXO
  180.                         //Forward
  181.                         leftMotorSpeed = 1;
  182.                         rightMotorSpeed = 1;
  183.                     }
  184.  
  185.                     if (!L && !M && R) { //OOX
  186.                         //Right
  187.                         leftMotorSpeed = 1;
  188.                         rightMotorSpeed = 0.5;
  189.                     }
  190.  
  191.                     if (L && !M && !R) { //XOO
  192.                         //Left
  193.                         leftMotorSpeed = 0.5;
  194.                         rightMotorSpeed = 1;
  195.                     }
  196.  
  197.                     if ( (L && M && !R) || (!L && M && R) ) { //XXO or OXX
  198.                         //Slow Forward
  199.                         leftMotorSpeed = 0.75;
  200.                         rightMotorSpeed = 0.75;
  201.                     }
  202.  
  203.                     if (L && M && R) //XXX, ALL ON
  204.                     {
  205.                         leftMotorSpeed = 0;
  206.                         rightMotorSpeed = 0;
  207.  
  208.                         if (ArmAtTargetVoltage) {
  209.                             stageComplete = true;
  210.                         }
  211.                     }
  212.  
  213.                     if (!L && !M && !R) { //OOO, ALL OFF
  214.                         if(!yHit) {
  215.                             //start turning next iteration
  216.                             turning = true;
  217.                         }
  218.                     }
  219.                 } else { //turning == true
  220.                     //Turning around
  221.                     if(!L && !M && !R) { //None are on
  222.                         if(!yHit) { //Turn in specified direction
  223.                             if(dirSwitch->Get()) {
  224.                                 //right
  225.                                 leftMotorSpeed = 0.5;
  226.                                 rightMotorSpeed = -0.5;
  227.                             } else {
  228.                                 //left
  229.                                 leftMotorSpeed = -0.5;
  230.                                 rightMotorSpeed = 0.5;
  231.                             }
  232.                         } else { //turn opposite to how we turned before
  233.                             if(dirSwitch->Get()) {
  234.                                 //left
  235.                                 leftMotorSpeed = -0.5;
  236.                                 rightMotorSpeed = 0.5;
  237.                             } else {
  238.                                 //right
  239.                                 leftMotorSpeed = 0.5;
  240.                                 rightMotorSpeed = -0.5;
  241.                             }
  242.                         }
  243.                     } else { //hit the line
  244.                         if(!yHit) {
  245.                             yHit = true;
  246.                         } else {
  247.                             //hit the line after the second time we turn, break
  248.                             stageComplete = true;
  249.                         }
  250.                         turning = false;
  251.                         //go back to driving along the line
  252.                     }
  253.                 }
  254.  
  255.                 setMotorSpeeds(-1*leftMotorSpeed * SpeedScale, rightMotorSpeed * SpeedScale, motorFLeft, motorBLeft, motorFRight, motorBRight);
  256.             }
  257.         }
  258.        
  259.         output->Clear();
  260.         printUserMessage(output, 2, 1, "STAGE 1 COMPLETED");
  261.         printUserMessage(output, 1, 1, "STAGE 2 STARTING");
  262.         output->UpdateLCD();
  263.        
  264.         /*=====================================
  265.          // STAGE PURPOSES:
  266.          // -move backwards for 1 second
  267.          // -lower the arm to AUTO_PLACEMENT_VOLTAGE
  268.          //=====================================*/
  269.        
  270.         stageComplete = false;
  271.        
  272.         targVolt = AUTO_PLACEMENT_VOLTAGE;
  273.        
  274.         reverseTimer->Start();
  275.         while(!stageComplete && IsAutonomous() && !quitAll)
  276.         {
  277.             if(reverseTimer->Get() < 1) //if it hasn't been one second yet
  278.             {
  279.                 setMotorSpeeds(-0.25 * -1, -0.25, motorFLeft, motorBLeft, motorFRight, motorBRight);
  280.             } else { //it's been 1 second
  281.                 setMotorSpeeds(0, 0, motorFLeft, motorBLeft, motorFRight, motorBRight);
  282.                
  283.                 curVolt = pot.GetVoltage();
  284.                 if(targVolt-curVolt < -ARM_VOLT_RANGE) {
  285.                     ArmDir = -1;
  286.                 } else if(targVolt-curVolt> ARM_VOLT_RANGE) { //once its backed up, lower the arm
  287.                     ArmDir = 1;
  288.                 } else {
  289.                     ArmDir = 0;
  290.                     stageComplete = true;
  291.                 }
  292.                
  293.                 if(!armLimitUp.Get()) {
  294.                     motorArm.Set(clamp(0.65 * ArmDir, -1, 0) * ArmMult);
  295.                 } else if(!armLimitDown.Get()) {
  296.                     motorArm.Set(clamp(0.65 * ArmDir, 0, 1) * ArmMult);
  297.                 } else {
  298.                     motorArm.Set(0.65 * ArmDir * ArmMult);
  299.                 }
  300.             }
  301.         }
  302.        
  303.         output->Clear();
  304.         printUserMessage(output, 2, 1, "STAGE 2 COMPLETED");
  305.         printUserMessage(output, 2, 1, "STAGE 3 STARTING");
  306.         output->UpdateLCD();
  307.        
  308.         /*=====================================
  309.          // STAGE PURPOSES:
  310.          // -Release the claw
  311.          // -lower the arm to AUTO_AFTER_PLACEMENT_VOLTAGE
  312.          // -Back up for 2.5 seconds
  313.          //=====================================*/
  314.  
  315.         stageComplete = false;
  316.  
  317.         curVolt = 0;
  318.         targVolt = AUTO_AFTER_PLACEMENT_VOLTAGE;
  319.  
  320.         reverseTimer = new Timer; //timer for backing up
  321.  
  322.         clawPiston->Set(true); //open claw
  323.  
  324.         while(!stageComplete && IsAutonomous() && !quitAll)
  325.         {
  326.             curVolt = pot.GetVoltage();
  327.             if(targVolt-curVolt < -ARM_VOLT_RANGE) {
  328.                 ArmDir = -1;
  329.             } else if(targVolt-curVolt> ARM_VOLT_RANGE) { //lower the arm
  330.                 ArmDir = 1;
  331.             } else {
  332.                 ArmDir = 0;
  333.  
  334.                 if(reverseTimer->Get() <= 0)
  335.                 {
  336.                     reverseTimer->Start();
  337.                 } else if(reverseTimer->Get() < 2.5) { //if it hasn't been two and a half seconds yet
  338.                     setMotorSpeeds(-1*-0.25, -0.25, motorFLeft, motorBLeft, motorFRight, motorBRight); //back up
  339.                 } else {
  340.                     setMotorSpeeds(0, 0, motorFLeft, motorBLeft, motorFRight, motorBRight);
  341.                     stageComplete=true;
  342.                 }
  343.             }
  344.            
  345.             if(!armLimitUp.Get()) {
  346.                 motorArm.Set(clamp(0.65 * ArmDir, -1, 0) * ArmMult);
  347.             } else if(!armLimitDown.Get()) {
  348.                 motorArm.Set(clamp(0.65 * ArmDir, 0, 1) * ArmMult);
  349.             } else {
  350.                 motorArm.Set(0.65 * ArmDir * ArmMult);
  351.             }
  352.         }
  353.     }
  354. }
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!