Public Pastes
SHARE
TWEET

VCEmblem's FRC 2013 Code

a guest Feb 12th, 2013 163 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1.  
  2. #include "WPILib.h"
  3. #include "math.h"
  4.  
  5.  
  6. class RobotDemo : public SimpleRobot
  7. {
  8.        
  9.         Jaguar frontLeft; // robot drive system
  10.         Jaguar frontRight;
  11.         Jaguar backRight;
  12.         Jaguar backLeft;
  13.         Jaguar shooter;
  14.        
  15.         Victor conveyor;      
  16.         Victor vacuum;
  17.         Victor leftArm;
  18.         Victor rightArm;
  19.        
  20.         Relay leftIntake;
  21.         Relay rightIntake;
  22.        
  23.         Joystick driveStick;
  24.        
  25.         Encoder armEncoder;
  26.         Digital rangeFinder;
  27.        
  28.  
  29. public:
  30.         RobotDemo(void):
  31.                
  32.                 //JAGUARS//
  33.                 frontLeft(1),   // These must be initialized in the same order
  34.                 frontRight(2),  // as they are declared above.
  35.                 backRight(3),
  36.                 backLeft(4),
  37.                 shooter(7),
  38.                
  39.                 //VICTORS//
  40.                 conveyor(6),
  41.                 vacuum(5),
  42.                 leftArm(9),
  43.                 rightArm(8),
  44.        
  45.                 //RELAYS//
  46.                 leftIntake(1),
  47.                 rightIntake(2),
  48.                
  49.                 //JOYSTICKS//
  50.                 driveStick(1),
  51.        
  52.                         //SENSORS//
  53.                         armEncoder(1)
  54.                
  55.         {
  56.                
  57.                 frontLeft.SetExpiration(0.1);
  58.                 frontRight.SetExpiration(0.1);
  59.                 backRight.SetExpiration(0.1);
  60.                 backLeft.SetExpiration(0.1);
  61.                
  62.                 vacuum.SetExpiration(0.1);
  63.                 leftArm.SetExpiration(0.1);            
  64.                 rightArm.SetExpiration(0.1);   
  65.                                                                
  66.         }                                                              
  67.         void shoot()                                                   
  68.         {                                                                              
  69.                 if(armEncoder.Get() < x) //x = 0 degress
  70.                 {
  71.                         while(armEncoder.Get() < x)
  72.                         {
  73.                                 leftArm.Set(-1.0);
  74.                                 rightArm.Set(-1.0);
  75.                                
  76.                         }
  77.                 }
  78.                 int Ct = 0.1;
  79.                 int deg = ((asine((rangeFinder.Get()*9.8*Ct)/(16^+2)))/2);
  80.                 lift_to_value(deg, 10);
  81.                
  82.         }
  83.        
  84.         void lift_to_value(int target, int error)
  85.         {
  86.           while( (armEncoder.Get() < (target-error)) || (armEncoder.Get() > (target+error)) ) {
  87.             if(armEncoder.Get() < (target-error)) {
  88.                 leftArm.Set(-0.6);
  89.                 rightArm.Set(-0.6);
  90.             }
  91.             if(armEncoder.Get() > (target+error)) {
  92.                 leftArm.Set(0.6);
  93.                 rightArm.Set(0.6);
  94.             }
  95.             Wait(0.1);
  96.           }
  97.         }
  98. //  //  //  //  //  //  //  //  //  //  //  //  //  //  //  //  //  //  //  //  //  //  //  //  //  //
  99.         /*void Autonomous(void)
  100.         {
  101.                 // PART ONE || PART ONE //
  102.                 Wait(1.0);
  103.                 leftArm.Set(-1.0);
  104.                 rightArm.Set(-1.0);
  105.                 Wait(1.5);
  106.                 leftArm.Set(0);
  107.                 rightArm.Set(0);
  108.                 Wait(0.5);
  109.                 shooter(0.7);
  110.                 Wait(0.5);
  111.                 conveyer.Set(0.8);
  112.                 Wait(2.0);
  113.                 shooter(0);
  114.                 conveyer.Set(0);
  115.                 // PART TWO || PART TWO //
  116.         }*/
  117. //  //  //  //  //  //  //  //  //  //  //  //  //  //  //  //  //  //  //  //  //  //  //  //  //  //
  118.         void OperatorControl(void)
  119.         {
  120.                 while (IsOperatorControl())
  121.                 {
  122.                        
  123.                           //-------------~~--------------~~---------------//
  124.                          // DRIVE MOTORS || DRIVE MOTORS || DRIVE MOTORS //
  125.                         //---------------~~--------------~~-------------//
  126.                        
  127.                         frontLeft.Set( - ( driveStick.GetRawAxis(2) )
  128.                         + ( driveStick.GetRawAxis(3) ) + ( driveStick.GetRawAxis(1) ) );  
  129.                        
  130.                         frontRight.Set( ( driveStick.GetRawAxis(2) )
  131.                         + ( driveStick.GetRawAxis(3) ) + ( driveStick.GetRawAxis(1) ) );
  132.                        
  133.                         backRight.Set( ( driveStick.GetRawAxis(2) )
  134.                         + ( driveStick.GetRawAxis(3) ) - ( driveStick.GetRawAxis(1) ) );
  135.                        
  136.                         backLeft.Set( - ( driveStick.GetRawAxis(2) )
  137.                         + ( driveStick.GetRawAxis(3) ) - ( driveStick.GetRawAxis(1) ) );
  138.                        
  139.                           //-----------~~------------~~-------------//
  140.                          // ARM MOTORS || ARM MOTORS || ARM MOTORS //
  141.                         //-------------~~------------~~-----------//
  142.                        
  143.                         if(driveStick.GetRawButton(5))
  144.                         {
  145.                                 leftArm.Set(-1.0);
  146.                                 rightArm.Set(-1.0);
  147.                         }
  148.                         else if(driveStick.GetRawButton(7))
  149.                         {
  150.                                 leftArm.Set(0.5);
  151.                                 rightArm.Set(0.5);
  152.                         }
  153.                         else
  154.                         {
  155.                                 leftArm.Set(0.0);
  156.                                 rightArm.Set(0.0);
  157.                         }
  158.                           //---------~~----------~~-----------//
  159.                          // CONVEYER || CONVEYER || CONVEYER //
  160.                         //-----------~~----------~~---------//
  161.                        
  162.                         conveyor.Set( - (driveStick.GetRawAxis(6)) );
  163.                        
  164.                           //--------~~---------~~----------//
  165.                          // SHOOTER || SHOOTER || SHOOTER //
  166.                         //----------~~---------~~--------//
  167.                        
  168.                         if(driveStick.GetRawButton(8))
  169.                         {
  170.                                 shooter.Set(0.7);
  171.                         }
  172.                         else if(driveStick.GetRawButton(6))
  173.                         {
  174.                                 shooter.Set(1.0);
  175.                         }
  176.                         else if(driveStick.GetRawButton(3))
  177.                         {
  178.                                 shooter.Set(-0.6);
  179.                         }
  180.                         else
  181.                         {
  182.                                 shooter.Set(0.0);
  183.                         }
  184.                           //-------~~--------~~---------//
  185.                          // INTAKE || INTAKE || INTAKE //
  186.                         //---------~~--------~~-------//
  187.                        
  188.                         if(driveStick.GetRawButton(2))
  189.                         {
  190.                                 leftIntake.Set(Relay::kForward);
  191.                                 rightIntake.Set(Relay::kForward);
  192.                                 //vacuum.Set(1.0);
  193.                         }
  194.                         else if(driveStick.GetRawButton(4))
  195.                         {
  196.                                 leftIntake.Set(Relay::kReverse);
  197.                                 rightIntake.Set(Relay::kReverse);
  198.                                 //vacuum.Set(-1.0);
  199.                         }
  200.                         else
  201.                         {
  202.                                 leftIntake.Set(Relay::kOff);
  203.                                 rightIntake.Set(Relay::kOff);
  204.                                 //vacuum.Set(0.0);
  205.                         }
  206.                           //---------------~~----------------~~-----------------//
  207.                          // STATION OUTPUT || STATION OUTPUT || STATION OUTPUT //
  208.                         //-----------------~~----------------~~---------------//
  209.                        
  210.                         DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
  211.                
  212.                         dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "AX1: %4.1f", driveStick.GetRawAxis(1) );
  213.                         dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "AX2: %4.1f", driveStick.GetRawAxis(2) );
  214.                         dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "AX3: %4.1f", driveStick.GetRawAxis(3) );
  215.                         dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "AX4: %4.1f", driveStick.GetRawAxis(4) );
  216.                         dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "AX5: %4.1f", driveStick.GetRawAxis(5) );
  217.                         dsLCD->Printf(DriverStationLCD::kUser_Line6, 1, "AX6: %4.1f", driveStick.GetRawAxis(6) );
  218.  
  219.                         dsLCD->UpdateLCD();
  220.                        
  221.                         Wait(0.005);  // wait for a motor update time
  222.                 }
  223.         }
  224.        
  225.         /**
  226.          * Runs during test mode
  227.          */
  228.         void Test() {
  229.  
  230.         }
  231. };
  232.  
  233. START_ROBOT_CLASS(RobotDemo);
RAW Paste Data
Top