Public Pastes
SHARE
TWEET

FRC Robot Main

a guest Feb 7th, 2012 94 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include "WPILib.h"
  2. #include "cmath"
  3. #include "CANMechanumDrive.cpp"
  4.  
  5. class Sehome_Robot : public IterativeRobot
  6. {
  7.        
  8.         PWM belt1;
  9.         PWM belt2;
  10.         Joystick joy1;
  11.         Joystick joy2;
  12.         CANJaguar J1;
  13.         CANJaguar J2;
  14.         CANJaguar J3;
  15.         CANJaguar J4;
  16.         CANJaguar J5;
  17.         CANJaguar J6;
  18.         CANMechanumDrive MecDrive;
  19.        
  20.         float angle;
  21.         float speed;
  22.         float rotat;
  23.        
  24.         float x1;
  25.         float x2;
  26.         float y1;
  27.         float y2;
  28.        
  29. public:
  30.        
  31.         Sehome_Robot ():
  32.         belt1 ( 1, 2 ),
  33.         belt2 ( 2, 2 ),
  34.         joy1 ( 1 ),
  35.         joy2 ( 2 ),
  36.         J1 ( 1, CANJaguar::kPercentVbus ),
  37.         J2 ( 2, CANJaguar::kPercentVbus ),
  38.         J3 ( 3, CANJaguar::kPercentVbus ),
  39.         J4 ( 4, CANJaguar::kPercentVbus ),
  40.         J5 ( 5, CANJaguar::kPercentVbus ),
  41.         J6 ( 6, CANJaguar::kPercentVbus ),
  42.         MecDrive ( &J2, &J1, &J6, &J5 )
  43.         {
  44.                
  45.                 printf ( "The 2605 robot has been initialized!\n" );
  46.                
  47.                
  48.                
  49.         };
  50.        
  51.         void RobotInit ()
  52.         {
  53.                
  54.                 printf ( "Robot initializing!" );
  55.                
  56.         };
  57.        
  58.         void TeleopInit ()
  59.         {
  60.                
  61.                 MecDrive.enable ();
  62.                
  63.         };
  64.        
  65.         void TeleopPeriodic ()
  66.         {
  67.                
  68.                 x1 = joy1.GetX ();
  69.                 y1 = joy1.GetY ();
  70.                 x2 = joy2.GetX ();
  71.                 y2 = joy2.GetY ();
  72.                
  73.                 angle = atan2 ( x1, y1 );
  74.                 speed = sqrt ( x1 * x1 + y1 * y1 );
  75.                 rotat = x2;
  76.                
  77.                 MecDrive.clear ();
  78.                 MecDrive.setMechanumStrafe ( speed, angle );
  79.                 MecDrive.setMechanumRotation ( rotat );
  80.                 //MecDrive.apply ();
  81.                
  82.         };
  83.        
  84.         void DisabledInit()
  85.         {
  86.                
  87.                 MecDrive.disable ();
  88.                
  89.         };
  90.         void AutonomousInit() { return; };
  91.        
  92.        
  93.        
  94. };
  95.  
  96. START_ROBOT_CLASS ( Sehome_Robot );
RAW Paste Data
Top