Public Pastes
SHARE
TWEET

Untitled

a guest Jan 22nd, 2015 176 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #!/usr/bin/env python3
  2.  
  3. from __future__ import division #Needed for the math#
  4. import wpilib #Library for robot#
  5. import wpilib.buttons
  6. #####NOTE: Might not need future divison on Python 3.4#######
  7.  
  8. #####NOTE: This is kinda set up for Omni with the slide motor but more ajustments might be needed!##########
  9. class MyRobot(wpilib.IterativeRobot):
  10.  
  11.     def robotInit(self):
  12.         #Set up for pins 8 and 9 on RIO#
  13.         self.motor1=wpilib.Jaguar(8)
  14.         self.motor2=wpilib.Jaguar(9)
  15.         self.slide_motor=wpilib.Jaguar(7)
  16.        
  17.         #Sets up motor1 and motor2 to drive#
  18.         self.robot_drive=wpilib.RobotDrive(self.motor1,self.motor2)
  19.        
  20.         #Using joystick 0(Works best with xbox controller)#
  21.         #Note that this joystick number may need to change#
  22.         self.stick=wpilib.Joystick(0)
  23.         self.right_stickX=self.stick.getAxis(4)
  24.         self.xbox_a=wpilib.buttons.JoystickButton(self.stick, 4)#Might need to change button number#
  25.         #If you want Y...it is here.... self.right_stickY=self.stick.getAxis(5)
  26.         #Calls function for human controlled robot#
  27.  
  28.         #Compressor and Solenoid#
  29.  
  30.         #It is set up on 0 channel CAN ID and the 4th channel#
  31.         self.solenoid=wpilib.Solenoid(0,4)
  32.         self.compressor=wpilib.Compressor()
  33.        
  34.         self.teleopPeriodic()
  35.  
  36.        
  37.     def teleopPeriodic(self):
  38.        
  39.         #while self.isOperatorControl() and self.isEnabled(): #https://github.com/robotpy/robotpy-wpilib/blob/master/examples/sample/robot.py
  40.         self.compressor.setClosedLoopControl(True)
  41.         if self.xbox_a.get()==True:
  42.             self.solenoid.set(True)
  43.        
  44.         #Get the x and y axis from controller#
  45.         controller=self.stick.getY()
  46.         x=self.stick.getX()
  47.        
  48.         #Controller is the y#
  49.         #Depending on value, it will determine the curve#
  50.  
  51. ##########################################NOT DONE, DON'T RUN####################################################################################
  52.         ###################NEED TO FIGURE OUT THE RADIUS OF XBOX JOYSTICK######################################
  53.        
  54.         controller=(controller/255)*100
  55.         x=(x/255)*100
  56.        
  57.         if controller <=50:
  58.             output=controller*(2/5)
  59.         elif controller >50 and controller <=70:
  60.             output=controller*(32/70)
  61.         elif controller >70 and controller<=85:
  62.             output=controller*(65/85)
  63.         elif controller >85 and controller<=90:
  64.             output=controller*(75/90)
  65.         elif controller >90 and controller<=95:
  66.             output=controller*(90/95)
  67.         elif controller >95 and controller<=100:
  68.             output=controller
  69.         else:
  70.             pass
  71.        
  72.         #x1 is the output of x#
  73.        
  74.         if x <=50:
  75.             x1=x*(2/5)
  76.         elif x >50 and x <=70:
  77.             x1=x*(32/70)
  78.         elif x >70 and x<=85:
  79.             x1=x*(65/85)
  80.         elif x >85 and x<=90:
  81.             x1=x*(75/90)
  82.         elif x >90 and x<=95:
  83.             x1=x*(90/95)
  84.         elif x >95 and x<=100:
  85.             x1=x
  86.         else:
  87.             pass
  88.        
  89.        
  90.         #drives the robot#
  91.         # Might need this line later... self.robot_drive.aracadeDrive(output,x1)
  92.         self.robot_drive.arcadeDrive(output,x1)
  93.         #For slide motor#
  94.         #self.slide_motor.set(self.right_stickX.getX())
  95.  
  96.  
  97. #Runs the code#
  98. if __name__=="__main__":
  99.     wpilib.run(MyRobot)
RAW Paste Data
Top