class MyRobot(wpilib.IterativeRobot):
"""
This shows using the AutonomousModeSelector to automatically choose
autonomous modes.
If you find this useful, you may want to consider using the Magicbot
framework, as it already has this integrated into it.
"""
def robotInit(self):
# Simple two wheel drive
self.drive = wpilib.RobotDrive(0, 1)
# Items in this dictionary are available in your autonomous mode
# as attributes on your autonomous object
self.components = {"drive": self.drive}
# * The first argument is the name of the package that your autonomous
# modes are located in
# * The second argument is passed to each StatefulAutonomous when they
# start up
self.automodes = AutonomousModeSelector("autonomous", self.components)
def autonomousPeriodic(self):
self.automodes.run()
def teleopInit(self):
pass
def teleopPeriodic(self):
pass
def robotInit(self):
# Motor Init
self.motor1 = ctre.WPI_TalonSRX(1)
self.motor2 = ctre.WPI_TalonSRX(2)
self.motor3 = ctre.WPI_TalonSRX(3)
self.motor4 = ctre.WPI_TalonSRX(4)
# Arm Init
self.arm = ctre.WPI_TalonSRX(5)
# Speed control groups
self.left = wpilib.SpeedControllerGroup(self.motor1, self.motor2)
self.right = wpilib.SpeedControllerGroup(self.motor3, self.motor4)
# Drive Function Init
self.driveMeBoi = wpilib.drive.DifferentialDrive(self.left, self.right)
#Controller Init
self.controller = wpilib.XboxController(0)
# Sensor
self.intakeSensor = wpilib.DigitalInput(9)
#Color.py Init
self.color = color.PrintColor()
#Auto mode variables
self.components = {'drive': self.driveMeBoi}
self.automodes = AutonomousModeSelector('autonomous', self.components)
self.drive = drive.Drive(self.driveMeBoi)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.timer = wpilib.Timer()
self.gyro = ADXLGyro.ADXLGyro()
self.leftGearbox = Gearbox.Gearbox([0, 1, 2])
self.rightGearbox = Gearbox.Gearbox([3, 4, 5], inverted=True)
self.intake = Intake.Intake()
self.leftJoystick = wpilib.Joystick(0)
self.rightJoystick = wpilib.Joystick(1)
self.prefs = wpilib.Preferences.getInstance()
self.prefs.put("Robot", "CraigPy")
self.components = {
'left': self.leftGearbox,
'right': self.rightGearbox,
'intake': self.intake,
'gyro': self.gyro,
'prefs': self.prefs,
'isSim': self.isSimulation(),
'utils': Utils
}
self.autonomous = AutonomousModeSelector('Autonomous', self.components)
self.gyro.calibrate()
self.i = 0
class MyRobot(
wpilib.IterativeRobot
): # <---- IternativeRobot means that it loops over and over by itself like a while loop
def robotInit(self):
"""
Define all motor controllers, joysticks, Pneumatics, etc. here so you can use them in teleop/auton
"""
self.drive_motor1 = wpilib.Talon(
0) # <--- or whatever motor controller you are using
self.drive_motor2 = wpilib.Talon(1)
self.robot_drive = wpilib.RobotDrive(
self.drive_motor1, self.drive_motor2
) # <--- says to robot that these motors work together to drive robot
self.xboxController = wpilib.Joystick(
0) # <--- joystick, does not have to be an xbox controller
self.components = { # Add all the objects you are going to want in autonomous like sensors, the robot drive, etc.
'drive': self.
robot_drive #give it a nickname as well. In this case, we "nicknamed" self.robot_drive as 'drive' so in auto you will do self.drive
}
self.automodes = AutonomousModeSelector(
'auto-modes', self.components
) #pass in the folder with all your auto modes and the components you want in auto
def autonomousPeriodic(self):
self.automodes.run()
def robotInit(self):
"""
.. warning:: Internal API, don't override; use :meth:`createObjects` instead
"""
self._exclude_from_injection = ["logger", "members"]
self.__last_error_report = -10
self._components = []
self._feedbacks = []
self._reset_components = []
# Create the user's objects and stuff here
self.createObjects()
# Load autonomous modes
self._automodes = AutonomousModeSelector("autonomous")
# Next, create the robot components and wire them together
self._create_components()
self.__nt = NetworkTables.getTable("/robot")
self.__nt.putBoolean("is_simulation", self.isSimulation())
self.__nt.putBoolean("is_ds_attached", self.ds.isDSAttached())
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
# Simple two wheel drive
self.drive = wpilib.RobotDrive(0, 1)
# Items in this dictionary are available in your autonomous mode
# as attributes on your autonomous object
self.components = {
'drive': self.drive
}
# * The first argument is the name of the package that your autonomous
# modes are located in
# * The second argument is passed to each StatefulAutonomous when they
# start up
self.automodes = AutonomousModeSelector('autonomous',
self.components)
def autonomousPeriodic(self):
self.automodes.run()
def teleopInit(self):
pass
def teleopPeriodic(self):
pass
def robotInit(self):
#Motors
self.leftMotorInput = wpilib.Talon(1) # AEN
self.rightMotorInput = wpilib.Talon(2) # AEN
self.drive = wpilib.drive.DifferentialDrive(self.leftMotorInput, self.rightMotorInput)
#Inputs
self.xboxController = wpilib.Joystick(0)
self.xboxAbutton = wpilib.buttons.JoystickButton(self.xboxController, 1)
self.xboxBbutton = wpilib.buttons.JoystickButton(self.xboxController, 2)
self.xboxYbutton = wpilib.buttons.JoystickButton(self.xboxController, 4)
#Navigation and Logistics
#Defining Variables
self.dm = True
#Auto mode variables
self.components = {
'drive': self.drive
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
class Robot(wpilib.IterativeRobot):
def robotInit(self):
# Motor Init
self.motor1 = ctre.WPI_TalonSRX(1)
self.motor2 = ctre.WPI_TalonSRX(2)
self.motor3 = ctre.WPI_TalonSRX(3)
self.motor4 = ctre.WPI_TalonSRX(4)
# Arm Init
self.arm = ctre.WPI_TalonSRX(5)
# Speed control groups
self.left = wpilib.SpeedControllerGroup(self.motor1, self.motor2)
self.right = wpilib.SpeedControllerGroup(self.motor3, self.motor4)
# Drive Function Init
self.driveMeBoi = wpilib.drive.DifferentialDrive(self.left, self.right)
#Controller Init
self.controller = wpilib.XboxController(0)
# Sensor
self.intakeSensor = wpilib.DigitalInput(9)
#Color.py Init
self.color = color.PrintColor()
#Auto mode variables
self.components = {'drive': self.driveMeBoi}
self.automodes = AutonomousModeSelector('autonomous', self.components)
self.drive = drive.Drive(self.driveMeBoi)
def autonomousInit(self):
pass
def autonomousPeriodic(self):
self.arm.set(0)
self.automodes.run()
def teleopInit(self):
pass
def teleopPeriodic(self):
if self.controller.getAButton():
self.color.printRed("A Button Pressed")
elif self.controller.getBButton():
self.color.printGreen("B Button Pressed")
if not self.intakeSensor.get():
print("Sensor Hit")
else:
print("not hit")
self.arm.set(self.controller.getY(1))
self.drive.masterDriveMeBoi(self.controller.getX(0),
self.controller.getY(0))
def robotInit(self):
#Drive Motors
self.motor1 = ctre.WPI_TalonSRX(
1) # Initialize the TalonSRX on device 1.
self.motor2 = ctre.WPI_TalonSRX(2)
self.motor3 = ctre.WPI_TalonSRX(3)
self.motor4 = ctre.WPI_TalonSRX(4)
self.motor5 = ctre.WPI_TalonFX(5) #intake Motor
self.motor6 = ctre.WPI_TalonFX(6) #Shooter Motor
self.motor7 = ctre.WPI_VictorSPX(7) #Intake Arm
self.motor8 = ctre.WPI_VictorSPX(8) #Belt Drive
self.joy = wpilib.Joystick(0)
self.stick = wpilib.Joystick(
1) #this is a controller, also acceptable to use Joystick
self.intake = wpilib.DoubleSolenoid(0, 1)
self.balls = wpilib.DoubleSolenoid(2, 3)
self.left = wpilib.SpeedControllerGroup(self.motor1, self.motor2)
self.right = wpilib.SpeedControllerGroup(self.motor3, self.motor4)
#This is combining the Speed controls from above to make a left and right
#for the drive chassis. Note that self.motor1 and self.motor2 are combined to make self.left
#then self.motor3 and self.motor4 are combined to make self.right. This is done to Make
#the differential drive easier to setup
self.myRobot = wpilib.drive.DifferentialDrive(self.left, self.right)
#Here is our DifferentialDrive, Ultimately stating, Left side and Right side of chassis
#An Alternative to DifferentialDrive is this:
#self.robodrive = wpilib.RobotDrive(self.motor1, self.motor4, self.motor3, self.motor2)
#where motor 1 & 4 are left, and 2 & 3 are right
self.myRobot.setExpiration(0.1)
#These components here are for Autonomous Modes, and allows you to call parts and
#components here to be used IN automodes. For example- Our chassis above is labeled
#'self.myRobot', below in the self.components, If we want to use our chassis to drive
#in Autonomous, we need to call it in the fashion below. It is also encouraged to
#reuse the naming of the components from above to avoid confusion below. i.e.
#'Chassis': self.myRobot, In autonomous creation, I would then be using the variable
#self.chassis.set() instead of self.myRobot.set() when doing commands.
self.components = {
'myRobot': self.myRobot, #Chassis Driving
'motor5': self.motor5,
'motor6': self.motor6, #A speed control that is used for intake
'intake': self.intake,
'balls': self.balls #pneumatics arm that is not setup on bot yet
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
def robotInit(self):
# Simple two wheel drive
self.drive = wpilib.RobotDrive(0, 1)
# Items in this dictionary are available in your autonomous mode
# as attributes on your autonomous object
self.components = {"drive": self.drive}
# * The first argument is the name of the package that your autonomous
# modes are located in
# * The second argument is passed to each StatefulAutonomous when they
# start up
self.automodes = AutonomousModeSelector("autonomous", self.components)
def robotInit(self):
wpilib.CameraServer.launch('misc/vision.py:main')
if not wpilib.RobotBase.isSimulation(
): #This makes simulator show motor outputs for debugging
import ctre
self.RLC = ctre.CANTalon(self.rLeftChannel)
self.FLC = ctre.CANTalon(self.fLeftChannel)
self.FRC = ctre.CANTalon(self.fRightChannel)
self.RRC = ctre.CANTalon(self.rRightChannel)
else:
self.RLC = wpilib.Talon(self.rLeftChannel)
self.FLC = wpilib.Talon(self.fLeftChannel)
self.FRC = wpilib.Talon(self.fRightChannel)
self.RRC = wpilib.Talon(self.rRightChannel)
self.robotDrive = wpilib.RobotDrive(
self.RLC, self.RRC, self.FRC,
self.FLC) #Sets motors for robotDrive commands
#Controller Input Variables
self.controller = wpilib.Joystick(self.joystickChannel)
self.winch_backward = wpilib.buttons.JoystickButton(self.controller, 5)
self.paddleGet = wpilib.buttons.JoystickButton(self.controller, 1)
self.gearDrop = wpilib.buttons.JoystickButton(self.controller,
6) # Right Bumper
self.xboxMec = wpilib.buttons.JoystickButton(self.controller, 8)
self.xboxMec2 = wpilib.buttons.JoystickButton(self.controller, 7)
#CRio Output Variables
self.winch_motor1 = wpilib.Talon(7)
self.winch_motor2 = wpilib.Talon(8)
self.paddle = wpilib.Solenoid(1)
self.gear = wpilib.DoubleSolenoid(2, 3)
self.ultrasonic = wpilib.Ultrasonic(5, 4) #trigger to echo
self.ultrasonic.setAutomaticMode(True)
self.navx = navx.AHRS.create_spi()
#Auto mode variables
self.components = {
'drive': self.robotDrive,
'gearDrop': self.gear,
'ultrasonic': self.ultrasonic,
'navx': self.navx
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
def robotInit(self):
"""
.. warning:: Internal API, don't override; use :meth:`createObjects` instead
"""
self._exclude_from_injection = [
'logger', 'members'
]
self.__last_error_report = -10
self._components = []
self._feedbacks = []
self._reset_components = []
# Create the user's objects and stuff here
self.createObjects()
# Load autonomous modes
self._automodes = AutonomousModeSelector('autonomous')
# Next, create the robot components and wire them together
self._create_components()
self.__nt = NetworkTables.getTable('/robot')
self.__nt.putBoolean('is_simulation', self.isSimulation())
self.__nt.putBoolean('is_ds_attached', self.ds.isDSAttached())
def robotInit(self):
''' Robot initilization function '''
# initialize networktables
self.smart_dashboard = NetworkTables.getTable("SmartDashboard")
self.smart_dashboard.putNumber('shooter_speed', self.shooter_speed)
self.smart_dashboard.putNumber('gear_arm_opening_speed',
self.gear_arm_opening_speed)
self.smart_dashboard.putNumber('gear_arm_closing_speed',
self.gear_arm_closing_speed)
self.smart_dashboard.putNumber('loader_speed', self.loader_speed)
# initialize and launch the camera
wpilib.CameraServer.launch()
# object that handles basic drive operatives
self.drive_rf_motor = wpilib.Victor(portmap.motors.right_front)
self.drive_rr_motor = wpilib.Victor(portmap.motors.right_rear)
self.drive_lf_motor = wpilib.Victor(portmap.motors.left_front)
self.drive_lr_motor = wpilib.Victor(portmap.motors.left_rear)
self.shooter_motor = wpilib.Victor(portmap.motors.shooter)
self.gear_arm_motor = wpilib.Spark(portmap.motors.gear_arm)
self.loader_motor = wpilib.Spark(portmap.motors.loader)
# initialize drive
self.drive = wpilib.RobotDrive(self.drive_lf_motor,
self.drive_lr_motor,
self.drive_rf_motor,
self.drive_rr_motor)
self.drive.setExpiration(0.1)
# joysticks 1 & 2 on the driver station
self.left_stick = wpilib.Joystick(portmap.joysticks.left_joystick)
self.right_stick = wpilib.Joystick(portmap.joysticks.right_joystick)
# initialize gyro
self.gyro = wpilib.AnalogGyro(1)
# initialize autonomous components
self.components = {
'drive': self.drive,
'gear_arm_motor': self.gear_arm_motor
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
#Defining Constants
self.LeftTread = wpilib.Talon(0)
self.RightTread = wpilib.Talon(1)
self.robotDrive = wpilib.RobotDrive(self.LeftTread, self.RightTread)
self.xboxController = wpilib.Joystick(0)
self.xboxAbutton = wpilib.buttons.JoystickButton(self.xboxController, 1)
self.xboxBbutton = wpilib.buttons.JoystickButton(self.xboxController, 2)
self.xboxYbutton = wpilib.buttons.JoystickButton(self.xboxController, 4)
self.navx = navx.AHRS.create_spi()
self.drive = drive.Drive(self.robotDrive, self.xboxController, self.navx)
#Defining Variables
self.dm = True
#Auto mode variables
self.components = {
'drive': self.drive
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
def autonomousPeriodic(self):
self.automodes.run()
def teleopPeriodic(self):
if self.xboxYbutton.get():
self.drive.flipflip()
if self.xboxAbutton.get():
self.dm = False
if self.xboxBbutton.get():
self.dm = True
self.drive.customDrive(self.xboxController.getX(), self.xboxController.getY(), self.xboxController.getRawAxis(2), self.dm)
def robotInit(self):
#NetworkTables Init
NetworkTables.initialize()
self.table = NetworkTables.getTable("SmartDashboard")
#Navx
self.navx = navx.AHRS.create_spi()
#PowerDistributionPanel
self.power_board = wpilib.PowerDistributionPanel()
#Motors
self.leftMotor1 = wpilib.Spark(
0
) # <-- This is what links our PWM port on the CRIO to a physical ESC.
self.leftMotor2 = wpilib.Spark(
1
) # <-- This is what links our PWM port on the CRIO to a physical ESC.
self.left = wpilib.SpeedControllerGroup(self.leftMotor1,
self.leftMotor2)
self.rightMotor1 = wpilib.Spark(2)
self.rightMotor2 = wpilib.Spark(3)
self.right = wpilib.SpeedControllerGroup(self.rightMotor1,
self.rightMotor2)
self.liftMotor1 = wpilib.Talon(4)
self.liftMotor2 = wpilib.Talon(5)
self.liftMotor2.setInverted(True)
self.lift = wpilib.SpeedControllerGroup(self.liftMotor1,
self.liftMotor2)
#User Input
self.playerOne = wpilib.XboxController(
0) # <-- This is for using Xbox controllers
#Drive
self.robotDrive = wpilib.drive.DifferentialDrive(self.left, self.right)
#Drive.py init
self.drive = drive.Drive(self.robotDrive, self.navx, self.left,
self.right)
self.components = {'drive': self.drive, 'table': self.table}
self.automodes = AutonomousModeSelector('autonomous', self.components)
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
#Motors
self.leftMotorInput = wpilib.Talon(1) # AEN
self.rightMotorInput = wpilib.Talon(2) # AEN
self.drive = wpilib.drive.DifferentialDrive(self.leftMotorInput, self.rightMotorInput)
#Inputs
self.xboxController = wpilib.Joystick(0)
self.xboxAbutton = wpilib.buttons.JoystickButton(self.xboxController, 1)
self.xboxBbutton = wpilib.buttons.JoystickButton(self.xboxController, 2)
self.xboxYbutton = wpilib.buttons.JoystickButton(self.xboxController, 4)
#Navigation and Logistics
#Defining Variables
self.dm = True
#Auto mode variables
self.components = {
'drive': self.drive
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
def autonomousPeriodic(self):
self.automodes.run()
def teleopPeriodic(self):
self.drive.tankDrive(self.xboxController.getY(), self.xboxController.getRawAxis(5))
def robotInit(self):
hardware.init() # this makes everything not break
self.drive = drive.Drive()
self.pneumatics = pneumatics.Pneumatics()
self.intake = intake.Intake()
self.elevator = elevator.Elevator()
self.components = {
'drive': self.drive,
'pneumatics': self.pneumatics,
'intake': self.intake,
'elevator': self.elevator,
}
self.nt_timer = Timer(
) # timer for SmartDashboard update so we don't use all our bandwidth
self.nt_timer.start()
self.autonomous_modes = AutonomousModeSelector('autonomous',
self.components)
self.state = States.STACKING
def robotInit(self):
self.chandler = XboxController(0)
self.meet = XboxController(1)
self.drive = drive.Drive() # So redundant omg
self.pneumatics = pneumatics.Pneumatics()
self.intake = intake.Intake()
self.elevator = elevator.Elevator()
self.components = {
'drive': self.drive,
'pneumatics': self.pneumatics,
'intake': self.intake,
'elevator': self.elevator,
}
self.nt_timer = Timer() # timer for SmartDashboard update so we don't use all our bandwidth
self.nt_timer.start()
self.autonomous_modes = AutonomousModeSelector('autonomous', self.components)
quickdebug.add_printables(self, ('match_time', Timer.getMatchTime))
quickdebug.init()
def robotInit(self):
self.motor1 = ctre.WPI_TalonSRX(1)
self.motor2 = ctre.WPI_TalonSRX(2)
self.motor3 = ctre.WPI_TalonSRX(3)
self.motor4 = ctre.WPI_TalonSRX(4)
self.ultrasonic = wpilib.AnalogInput(0)
self.distanceSensor = navx.AHRS.create_i2c()
self.playerOne = wpilib.XboxController(0)
self.robotDrive = wpilib.RobotDrive(self.motor1, self.motor2,
self.motor3, self.motor4)
self.components = {
'robotDrive': self.robotDrive,
'ultrasonic': self.ultrasonic
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
self.navx = navx.AHRS.create_spi()
def robotInit(self):
#Defining Constants
self.LeftTread = wpilib.Talon(0)
self.RightTread = wpilib.Talon(1)
self.robotDrive = wpilib.RobotDrive(self.LeftTread, self.RightTread)
self.xboxController = wpilib.Joystick(0)
self.xboxAbutton = wpilib.buttons.JoystickButton(self.xboxController, 1)
self.xboxBbutton = wpilib.buttons.JoystickButton(self.xboxController, 2)
self.xboxYbutton = wpilib.buttons.JoystickButton(self.xboxController, 4)
self.navx = navx.AHRS.create_spi()
self.drive = drive.Drive(self.robotDrive, self.xboxController, self.navx)
#Defining Variables
self.dm = True
#Auto mode variables
self.components = {
'drive': self.drive
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
def robotInit(self):
"""
.. warning:: Internal API, don't override; use :meth:`createObjects` instead
"""
self._exclude_from_injection = ["logger"]
self.__last_error_report = -10
self._components = []
self._feedbacks = []
self._reset_components = []
# Create the user's objects and stuff here
self.createObjects()
# Load autonomous modes
self._automodes = AutonomousModeSelector("autonomous")
# Next, create the robot components and wire them together
self._create_components()
self.__nt = NetworkTables.getTable("/robot")
self.__nt_put_is_ds_attached = self.__nt.getEntry(
"is_ds_attached").setBoolean
self.__nt_put_mode = self.__nt.getEntry("mode").setString
self.__nt.putBoolean("is_simulation", self.isSimulation())
self.__nt_put_is_ds_attached(self.ds.isDSAttached())
self.__done = False
# cache these
self.__sd_update = wpilib.SmartDashboard.updateValues
self.__lv_update = wpilib.LiveWindow.getInstance().updateValues
# self.__sf_update = Shuffleboard.update
self.watchdog = SimpleWatchdog(self.control_loop_wait_time)
def robotInit(self):
"""
.. warning:: Internal API, don't override; use :meth:`createObjects` instead
"""
self._exclude_from_injection = [
'logger', 'members'
]
self.__last_error_report = -10
self._components = []
# Create the user's objects and stuff here
self.createObjects()
# Load autonomous modes
self._automodes = AutonomousModeSelector('autonomous')
# Next, create the robot components and wire them together
self._create_components()
class MyRobot(wpilib.SampleRobot):
def robotInit(self):
self.sd = NetworkTable.getTable('SmartDashboard')
# #INITIALIZE JOYSTICKS##
self.joystick1 = wpilib.Joystick(0)
self.joystick2 = wpilib.Joystick(1)
self.ui_joystick = wpilib.Joystick(2)
self.pinServo = wpilib.Servo(6)
# #INITIALIZE MOTORS##
self.lf_motor = wpilib.Talon(0)
self.lr_motor = wpilib.Talon(1)
self.rf_motor = wpilib.Talon(2)
self.rr_motor = wpilib.Talon(3)
# #ROBOT DRIVE##
self.robot_drive = wpilib.RobotDrive(self.lf_motor, self.lr_motor, self.rf_motor, self.rr_motor)
self.robot_drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kFrontRight, True)
self.robot_drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kRearRight, True)
# #INITIALIZE SENSORS#
self.gyro = wpilib.Gyro(0)
self.tote_motor = wpilib.CANTalon(5)
self.sensor = Sensor(self.tote_motor)
self.tote_forklift = ToteForklift(self.tote_motor,self.sensor,2)
self.calibrator = Calibrator(self.tote_forklift, self.sensor)
self.autoLifter = autolift.Autolift(self.sensor, self.tote_forklift)
self.backSensor = SharpIRGP2Y0A41SK0F(6)
self.drive = drive.Drive(self.robot_drive, self.gyro, self.backSensor)
self.align = alignment.Alignment(self.sensor, self.tote_forklift, self.drive, self.autoLifter)
# These must have a doit function
self.components = {
'tote_forklift': self.tote_forklift,
'drive': self.drive,
'autolift': self.autoLifter,
'align': self.align,
'sensors': self.sensor
}
# These do not, and get passed to autonomous mode
self.auton_components = {
'pin_servo': self.pinServo
}
self.auton_components.update(self.components)
# #Defining Buttons##
self.toteUp = Button(self.joystick2, 3)
self.toteDown = Button(self.joystick2, 2)
self.toteTop = Button(self.joystick2, 6)
self.toteBottom = Button(self.joystick2, 7)
self.reverseDirection = Button(self.joystick1, 1)
# Secondary driver's joystick
self.ui_joystick_tote_down = Button(self.ui_joystick, 4)
self.ui_joystick_tote_up = Button(self.ui_joystick, 6)
self.oldReverseRobot = False
self.toteTo = self.sd.getAutoUpdateValue('toteTo', -1)
self.reverseRobot = self.sd.getAutoUpdateValue('reverseRobot',False)
self.autoLift = self.sd.getAutoUpdateValue('autoLift', False)
# If set, this means we go forward and try to pickup the three totes
# that we've deposited in autonomous mode
self.autoPickup = self.sd.getAutoUpdateValue('autoPickup', False)
self.autoPickupSpeed = self.sd.getAutoUpdateValue('autoPickupSpeed', -0.2)
self.control_loop_wait_time = 0.025
self.automodes = AutonomousModeSelector('autonomous', self.auton_components)
def autonomous(self):
self.sd.putBoolean('autoPickup', False)
self.automodes.run(self.control_loop_wait_time, self.update)
def operatorControl(self):
self.tote_forklift.set_manual(0)
delay = PreciseDelay(self.control_loop_wait_time)
self.logger.info("Entering teleop mode")
while self.isOperatorControl() and self.isEnabled():
self.sensor.update()
#self.calibrator.calibrate()
try:
self.drive.move(self.joystick1.getY()*((1+self.joystick1.getZ())/2), self.joystick1.getX(), self.joystick2.getX(),True)
except:
if not self.isFMSAttached():
raise
#
# Can forklift controls
#
## Tote forklift controls##
try:
if self.joystick2.getRawButton(5):
self.tote_forklift.set_manual(1)
elif self.joystick2.getRawButton(4):
self.tote_forklift.set_manual(-1)
elif self.toteUp.get():
self.tote_forklift.raise_forklift()
elif self.toteDown.get():
self.tote_forklift.lower_forklift()
if self.toteTop.get():
self.tote_forklift.set_pos_top()
elif self.toteBottom.get():
self.tote_forklift.set_pos_bottom()
except:
if not self.isFMSAttached():
raise
if self.toteTo.value >= 0:
toteTo = int(self.toteTo.value)
if toteTo == 2048:
self.tote_forklift.set_pos_top()
else:
self.tote_forklift._set_position(toteTo)
self.sd.putDouble('toteTo', -1)
#
# Driver-enabled automatic alignment code
#
try:
if self.joystick2.getTrigger():
# self.drive.isTheRobotBackwards = False
# self.align.align()
#elif self.autoLift.value:
self.autoLifter.autolift()
except:
if not self.isFMSAttached():
raise
#
# Utilities
#
try:
if self.joystick2.getRawButton(11):
self.drive.reset_gyro_angle()
except:
if not self.isFMSAttached():
raise
try:
if self.joystick2.getRawButton(8):
self.drive.wall_strafe(-.7)
elif self.joystick2.getRawButton(9):
self.drive.wall_strafe(.7)
except:
if not self.isFMSAttached():
raise
#
# Reverse drive
#
try:
if self.reverseDirection.get():
self.drive.switch_direction()
except:
if not self.isFMSAttached():
raise
try:
if self.reverseRobot.value != self.oldReverseRobot:
if self.reverseRobot.value == 0:
self.drive.switch_direction()
self.oldReverseRobot = self.reverseRobot.value
except:
if not self.isFMSAttached():
raise
#
# At the end of autonomous mode, pick up the three bins in
# front of us if enabled
#
#if self.autoPickup.value:
# try:
# # End autopickup when the driver gives joystick input
# if abs(self.joystick1.getX())>.1 or abs(self.joystick1.getY())>.1 or abs(self.joystick2.getX())>.1:
# self.sd.putBoolean('autoPickup', False)
# else:
# if not self.sensor.is_against_tote():
# self.drive.move(self.autoPickupSpeed.value, 0, 0)
# else:
# # Move slow once we hit the totes
# self.drive.move(-0.1, 0, 0)
#
# self.autoLifter.autolift()
#
# except:
# self.sd.putBoolean('autoPickup', False)
# if not self.isFMSAttached():
# raise
try:
self.ui_joystick_buttons()
except:
if not self.isFMSAttached():
raise
try:
self.smartdashboard_update()
except:
if not self.isFMSAttached():
raise
try:
self.update()
except:
if not self.isFMSAttached():
raise
# Replaces wpilib.Timer.delay()
delay.wait()
def smartdashboard_update(self):
self.sensor.update_sd()
self.tote_forklift.update_sd('Tote Forklift')
self.sd.putNumber('backSensorValue', self.backSensor.getDistance())
self.sd.putNumber('gyroAngle', self.gyro.getAngle())
def ui_joystick_buttons(self):
if self.ui_joystick_tote_down.get():
self.tote_forklift.set_pos_bottom()
elif self.ui_joystick_tote_up.get():
self.tote_forklift.set_pos_top()
def update (self):
for component in self.components.values():
component.doit()
def disabled(self):
'''Called when the robot is in disabled mode'''
self.logger.info("Entering disabled mode")
while not self.isEnabled():
self.sensor.update()
self.smartdashboard_update()
wpilib.Timer.delay(.01)
def test(self):
'''Called when the robot is in test mode'''
while self.isTest() and self.isEnabled():
wpilib.LiveWindow.run()
wpilib.Timer.delay(.01)
def isFMSAttached(self):
return self.ds.isFMSAttached()
def robotInit(self):
self.sd = NetworkTable.getTable('SmartDashboard')
# #INITIALIZE JOYSTICKS##
self.joystick1 = wpilib.Joystick(0)
self.joystick2 = wpilib.Joystick(1)
self.ui_joystick = wpilib.Joystick(2)
self.pinServo = wpilib.Servo(6)
# #INITIALIZE MOTORS##
self.lf_motor = wpilib.Talon(0)
self.lr_motor = wpilib.Talon(1)
self.rf_motor = wpilib.Talon(2)
self.rr_motor = wpilib.Talon(3)
# #ROBOT DRIVE##
self.robot_drive = wpilib.RobotDrive(self.lf_motor, self.lr_motor, self.rf_motor, self.rr_motor)
self.robot_drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kFrontRight, True)
self.robot_drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kRearRight, True)
# #INITIALIZE SENSORS#
self.gyro = wpilib.Gyro(0)
self.tote_motor = wpilib.CANTalon(5)
self.sensor = Sensor(self.tote_motor)
self.tote_forklift = ToteForklift(self.tote_motor,self.sensor,2)
self.calibrator = Calibrator(self.tote_forklift, self.sensor)
self.autoLifter = autolift.Autolift(self.sensor, self.tote_forklift)
self.backSensor = SharpIRGP2Y0A41SK0F(6)
self.drive = drive.Drive(self.robot_drive, self.gyro, self.backSensor)
self.align = alignment.Alignment(self.sensor, self.tote_forklift, self.drive, self.autoLifter)
# These must have a doit function
self.components = {
'tote_forklift': self.tote_forklift,
'drive': self.drive,
'autolift': self.autoLifter,
'align': self.align,
'sensors': self.sensor
}
# These do not, and get passed to autonomous mode
self.auton_components = {
'pin_servo': self.pinServo
}
self.auton_components.update(self.components)
# #Defining Buttons##
self.toteUp = Button(self.joystick2, 3)
self.toteDown = Button(self.joystick2, 2)
self.toteTop = Button(self.joystick2, 6)
self.toteBottom = Button(self.joystick2, 7)
self.reverseDirection = Button(self.joystick1, 1)
# Secondary driver's joystick
self.ui_joystick_tote_down = Button(self.ui_joystick, 4)
self.ui_joystick_tote_up = Button(self.ui_joystick, 6)
self.oldReverseRobot = False
self.toteTo = self.sd.getAutoUpdateValue('toteTo', -1)
self.reverseRobot = self.sd.getAutoUpdateValue('reverseRobot',False)
self.autoLift = self.sd.getAutoUpdateValue('autoLift', False)
# If set, this means we go forward and try to pickup the three totes
# that we've deposited in autonomous mode
self.autoPickup = self.sd.getAutoUpdateValue('autoPickup', False)
self.autoPickupSpeed = self.sd.getAutoUpdateValue('autoPickupSpeed', -0.2)
self.control_loop_wait_time = 0.025
self.automodes = AutonomousModeSelector('autonomous', self.auton_components)
class MagicRobot(wpilib.SampleRobot,
metaclass=OrderedClass):
"""
.. warning:: This implementation is still being developed, and may
be changed during the course of the 2016 season.
Robots that use the MagicBot framework should use this as their
base robot class. If you use this as your base, you must
implement the following methods:
- :meth:`createObjects`
- :meth:`teleopPeriodic`
MagicRobot uses the :class:`.AutonomousModeSelector` to allow you
to define multiple autonomous modes and to select one of them via
the SmartDashboard/SFX.
"""
#: Amount of time each loop takes (default is 20ms)
control_loop_wait_time = 0.020
#: Error report interval: when an FMS is attached, how often should
#: uncaught exceptions be reported?
error_report_interval = 0.5
def robotInit(self):
"""
.. warning:: Internal API, don't override; use :meth:`createObjects` instead
"""
self._exclude_from_injection = [
'logger', 'members'
]
self.__last_error_report = -10
self._components = []
# Create the user's objects and stuff here
self.createObjects()
# Load autonomous modes
self._automodes = AutonomousModeSelector('autonomous')
# Next, create the robot components and wire them together
self._create_components()
def createObjects(self):
"""
You should override this and initialize all of your wpilib
objects here (and not in your components, for example). This
serves two purposes:
- It puts all of your motor/sensor initialization in the same
place, so that if you need to change a port/pin number it
makes it really easy to find it. Additionally, if you want
to create a simplified robot program to test a specific
thing, it makes it really easy to copy/paste it elsewhere
- It allows you to use the magic injection mechanism to share
variables between components
.. note:: Do not access your magic components in this function,
as their instances have not been created yet. Do not
create them either.
"""
raise NotImplementedError
def teleopInit(self):
"""
Initialization code for teleop control code may go here.
Users may override this method for initialization code which will be
called each time the robot enters teleop mode.
.. note:: The ``on_enable`` functions of all components are called
before this function is called.
"""
pass
def teleopPeriodic(self):
"""
Periodic code for teleop mode should go here.
Users should override this method for code which will be called
periodically at a regular rate while the robot is in teleop mode.
This code executes before the ``execute`` functions of all
components are called.
"""
func = self.teleopPeriodic.__func__
if not hasattr(func, "firstRun"):
self.logger.warn("Default MagicRobot.teleopPeriodic() method... Overload me!")
func.firstRun = False
def disabledInit(self):
"""
Initialization code for disabled mode may go here.
Users may override this method for initialization code which will be
called each time the robot enters disabled mode.
.. note:: The ``on_disable`` functions of all components are called
before this function is called.
"""
pass
def disabledPeriodic(self):
"""
Periodic code for disabled mode should go here.
Users should override this method for code which will be called
periodically at a regular rate while the robot is in disabled mode.
This code executes before the ``execute`` functions of all
components are called.
"""
func = self.disabledPeriodic.__func__
if not hasattr(func, "firstRun"):
self.logger.warn("Default MagicRobot.disabledPeriodic() method... Overload me!")
func.firstRun = False
def onException(self, forceReport=False):
'''
This function must *only* be called when an unexpected exception
has occurred that would otherwise crash the robot code. Use this
inside your :meth:`operatorActions` function.
If the FMS is attached (eg, during a real competition match),
this function will return without raising an error. However,
it will try to report one-off errors to the Driver Station so
that it will be recorded in the Driver Station Log Viewer.
Repeated errors may not get logged.
Example usage::
def teleopPeriodic(self):
try:
if self.joystick.getTrigger():
self.shooter.shoot()
except:
self.onException()
try:
if self.joystick.getRawButton(2):
self.ball_intake.run()
except:
self.onException()
# and so on...
:param forceReport: Always report the exception to the DS. Don't
set this to True
'''
# If the FMS is not attached, crash the robot program
if not self.ds.isFMSAttached():
raise
# Otherwise, if the FMS is attached then try to report the error via
# the driver station console. Maybe.
now = wpilib.Timer.getFPGATimestamp()
try:
if forceReport or (now - self.__last_error_report) > self.error_report_interval:
wpilib.DriverStation.reportError("Unexpected exception", True)
except:
pass # ok, can't do anything here
self.__last_error_report = now
#
# Internal API
#
def autonomous(self):
"""
MagicRobot will do The Right Thing and automatically load all
autonomous mode routines defined in the autonomous folder.
.. warning:: Internal API, don't override
"""
self._on_mode_enable_components()
self._automodes.run(self.control_loop_wait_time,
self._execute_components,
self.onException)
self._on_mode_disable_components()
def disabled(self):
"""
This function is called in disabled mode. You should not
override this function; rather, you should override the
:meth:`disabledPeriodics` function instead.
.. warning:: Internal API, don't override
"""
delay = PreciseDelay(self.control_loop_wait_time)
self._on_mode_disable_components()
try:
self.disabledInit()
except:
self.onException(forceReport=True)
while self.isDisabled():
try:
self.disabledPeriodic()
except:
self.onException()
delay.wait()
def operatorControl(self):
"""
This function is called in teleoperated mode. You should not
override this function; rather, you should override the
:meth:`teleopPeriodics` function instead.
.. warning:: Internal API, don't override
"""
delay = PreciseDelay(self.control_loop_wait_time)
# initialize things
self._on_mode_enable_components()
try:
self.teleopInit()
except:
self.onException(forceReport=True)
while self.isOperatorControl() and self.isEnabled():
try:
self.teleopPeriodic()
except:
self.onException()
self._execute_components()
delay.wait()
self._on_mode_disable_components()
def test(self):
'''Called when the robot is in test mode'''
while self.isTest() and self.isEnabled():
wpilib.LiveWindow.run()
wpilib.Timer.delay(.01)
def _on_mode_enable_components(self):
# initialize things
for component in self._components:
if hasattr(component, 'on_enable'):
try:
component.on_enable()
except:
self.onException(forceReport=True)
def _on_mode_disable_components(self):
# deinitialize things
for component in self._components:
if hasattr(component, 'on_disable'):
try:
component.on_disable()
except:
self.onException(forceReport=True)
def _create_components(self):
#
# TODO: Will need to inject into any autonomous mode component
# too, as they're a bit different
#
# TODO: Will need to order state machine components before
# other components just in case
components = []
# Identify all of the types, and create them
for m in self.members:
if m.startswith('_'):
continue
ctyp = getattr(self, m)
if not isinstance(ctyp, type):
continue
# Create instance, set it on self
component = ctyp()
setattr(self, m, component)
# Ensure that mandatory methods are there
if not hasattr(component, 'execute') or \
not callable(component.execute):
raise ValueError("Component %s (%s) must have a method named 'execute'" % (m, component))
# Automatically inject a logger object
component.logger = logging.getLogger(m)
# Store for later
components.append((m, component))
# Collect attributes of this robot that are injectable
self._injectables = {}
for n in dir(self):
if n.startswith('_') or n in self._exclude_from_injection:
continue
o = getattr(self, n)
# Don't inject methods
# TODO: This could actually be a cool capability..
if inspect.ismethod(o):
continue
self._injectables[n] = o
# For each new component, perform magic injection
for cname, component in components:
self._components.append(component)
self._inject_vars(cname, component)
# Do it for autonomous modes too
for mode in self._automodes.modes.values():
self._inject_vars(None, mode)
def _inject_vars(self, cname, component):
for n in dir(component):
if n.startswith('_'):
continue
inject_type = getattr(component, n)
if not isinstance(inject_type, type):
continue
injectable = self._injectables.get(n)
if injectable is None:
if cname is not None:
injectable = self._injectables.get('%s_%s' % (cname, n))
if injectable is None:
raise ValueError("Component %s has variable %s (type %s), which is not present in %s" %
(cname, n, inject_type, self))
if not isinstance(injectable, inject_type):
raise ValueError("Component %s variable %s in %s are not the same types! (Got %s, expected %s)" %
(cname, n, self, type(injectable), inject_type))
setattr(component, n, injectable)
def _execute_components(self):
for component in self._components:
try:
component.execute()
except:
self.onException()
class SteampedeRobot(wpilib.IterativeRobot):
''' robot code for steampede '''
def __init__(self):
super().__init__()
self.smart_dashboard = None
self.motor_speed_stop = 0
self.shooter_speed = 1.0
self.gear_arm_opening_speed = -1.0
self.gear_arm_closing_speed = 1.0
self.loader_speed = 1.0
self.shooter_enabled = False
self.loader_enabled = False
self.gear_arm_opened = False
self.gear_arm_closed = True
self.gear_arm_opening = False
self.gear_arm_closing = False
self.drive_rf_motor = None
self.drive_rr_motor = None
self.drive_lf_motor = None
self.drive_lr_motor = None
self.shooter_motor = None
self.gear_arm_motor = None
self.loader_motor = None
self.left_stick = None
self.right_stick = None
self.drive = None
def robotInit(self):
''' Robot initilization function '''
# initialize networktables
self.smart_dashboard = NetworkTables.getTable("SmartDashboard")
self.smart_dashboard.putNumber('shooter_speed', self.shooter_speed)
self.smart_dashboard.putNumber('gear_arm_opening_speed',
self.gear_arm_opening_speed)
self.smart_dashboard.putNumber('gear_arm_closing_speed',
self.gear_arm_closing_speed)
self.smart_dashboard.putNumber('loader_speed', self.loader_speed)
# initialize and launch the camera
wpilib.CameraServer.launch()
# object that handles basic drive operatives
self.drive_rf_motor = wpilib.Victor(portmap.motors.right_front)
self.drive_rr_motor = wpilib.Victor(portmap.motors.right_rear)
self.drive_lf_motor = wpilib.Victor(portmap.motors.left_front)
self.drive_lr_motor = wpilib.Victor(portmap.motors.left_rear)
self.shooter_motor = wpilib.Victor(portmap.motors.shooter)
self.gear_arm_motor = wpilib.Spark(portmap.motors.gear_arm)
self.loader_motor = wpilib.Spark(portmap.motors.loader)
# initialize drive
self.drive = wpilib.RobotDrive(self.drive_lf_motor,
self.drive_lr_motor,
self.drive_rf_motor,
self.drive_rr_motor)
self.drive.setExpiration(0.1)
# joysticks 1 & 2 on the driver station
self.left_stick = wpilib.Joystick(portmap.joysticks.left_joystick)
self.right_stick = wpilib.Joystick(portmap.joysticks.right_joystick)
# initialize gyro
self.gyro = wpilib.AnalogGyro(1)
# initialize autonomous components
self.components = {
'drive': self.drive,
'gear_arm_motor': self.gear_arm_motor
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
def teleopInit(self):
'''Executed at the start of teleop mode'''
self.drive.setSafetyEnabled(True)
def teleopPeriodic(self):
try:
if self.left_stick.getTrigger():
self.shooter_enabled = True
axis = self.left_stick.getAxis(wpilib.Joystick.AxisType.kZ)
if axis != self.shooter_speed:
self.shooter_speed = axis
self.smart_dashboard.putNumber('shooter_speed',
self.shooter_speed)
self.shooter_motor.set(self.shooter_speed)
else:
self.shooter_enabled = False
self.shooter_motor.set(self.motor_speed_stop)
except:
if not self.isFMSAttached():
raise
try:
if self.right_stick.getTrigger():
self.loader_enabled = True
axis = self.right_stick.getAxis(wpilib.Joystick.AxisType.kZ)
if axis != self.loader_speed:
self.loader_speed = axis
self.smart_dashboard.putNumber('loader_speed',
self.loader_speed)
self.loader_motor.set(self.loader_speed)
else:
self.loader_enabled = False
self.loader_motor.set(self.motor_speed_stop)
except:
if not self.isFMSAttached():
raise
try:
if self.right_stick.getRawButton(
portmap.joysticks.button_gear_arm_down
) or self.left_stick.getRawButton(
portmap.joysticks.button_gear_arm_down):
self.gear_arm_closing = True
self.gear_arm_motor.set(self.gear_arm_closing_speed)
else:
self.gear_arm_motor.set(self.motor_speed_stop)
self.gear_arm_closing = False
except:
if not self.isFMSAttached():
raise
try:
if self.right_stick.getRawButton(
portmap.joysticks.button_gear_arm_up
) or self.left_stick.getRawButton(
portmap.joysticks.button_gear_arm_up):
self.gear_arm_opening = True
self.gear_arm_motor.set(self.gear_arm_opening_speed)
else:
self.gear_arm_motor.set(self.motor_speed_stop)
self.gear_arm_opening = False
except:
if not self.isFMSAttached():
raise
try:
self.drive.tankDrive(self.left_stick, self.right_stick, True)
except:
if not self.isFMSAttached():
raise
def autonomousInit(self):
self.drive.setSafetyEnabled(True)
def autonomousPeriodic(self):
self.automodes.run()
def isFMSAttached(self):
return wpilib.DriverStation.getInstance().isFMSAttached()
class MyRobot(wpilib.TimedRobot):
def robotInit(self):
#Drive Motors
self.motor1 = ctre.WPI_TalonSRX(
1) # Initialize the TalonSRX on device 1.
self.motor2 = ctre.WPI_TalonSRX(2)
self.motor3 = ctre.WPI_TalonSRX(3)
self.motor4 = ctre.WPI_TalonSRX(4)
self.motor5 = ctre.WPI_TalonFX(5) #intake Motor
self.motor6 = ctre.WPI_TalonFX(6) #Shooter Motor
self.motor7 = ctre.WPI_VictorSPX(7) #Intake Arm
self.motor8 = ctre.WPI_VictorSPX(8) #Belt Drive
self.joy = wpilib.Joystick(0)
self.stick = wpilib.Joystick(
1) #this is a controller, also acceptable to use Joystick
self.intake = wpilib.DoubleSolenoid(0, 1)
self.balls = wpilib.DoubleSolenoid(2, 3)
self.left = wpilib.SpeedControllerGroup(self.motor1, self.motor2)
self.right = wpilib.SpeedControllerGroup(self.motor3, self.motor4)
#This is combining the Speed controls from above to make a left and right
#for the drive chassis. Note that self.motor1 and self.motor2 are combined to make self.left
#then self.motor3 and self.motor4 are combined to make self.right. This is done to Make
#the differential drive easier to setup
self.myRobot = wpilib.drive.DifferentialDrive(self.left, self.right)
#Here is our DifferentialDrive, Ultimately stating, Left side and Right side of chassis
#An Alternative to DifferentialDrive is this:
#self.robodrive = wpilib.RobotDrive(self.motor1, self.motor4, self.motor3, self.motor2)
#where motor 1 & 4 are left, and 2 & 3 are right
self.myRobot.setExpiration(0.1)
#These components here are for Autonomous Modes, and allows you to call parts and
#components here to be used IN automodes. For example- Our chassis above is labeled
#'self.myRobot', below in the self.components, If we want to use our chassis to drive
#in Autonomous, we need to call it in the fashion below. It is also encouraged to
#reuse the naming of the components from above to avoid confusion below. i.e.
#'Chassis': self.myRobot, In autonomous creation, I would then be using the variable
#self.chassis.set() instead of self.myRobot.set() when doing commands.
self.components = {
'myRobot': self.myRobot, #Chassis Driving
'motor5': self.motor5,
'motor6': self.motor6, #A speed control that is used for intake
'intake': self.intake,
'balls': self.balls #pneumatics arm that is not setup on bot yet
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
#This line is to label where our automodes folder is and what devices used,
#('Insert folder name here', What compenents used in your auto codes (See above)
def autonomousInit(self):
#This function is run once each time the robot enters autonomous mode.
pass
def autonomousPeriodic(self):
#This function is called periodically during autonomous.
self.automodes.run()
def teleopInit(self):
#Executed at the start of teleop mode
self.myRobot.setSafetyEnabled(True)
def teleopPeriodic(self):
#Runs Robot on Arcade Drive
if abs(self.stick.getRawAxis(1)) > 0.05 or abs(
self.stick.getRawAxis(5)) > 0.05:
self.myRobot.tankDrive(-.75 * self.stick.getRawAxis(1),
-.75 * self.stick.getRawAxis(5))
else:
self.myRobot.arcadeDrive(-1 * self.joy.getRawAxis(1),
self.joy.getRawAxis(0))
#Fine Tuning Driver Control, for lining up the shots
#Below is an example code to be used for when a button is pressed
#to do something
#Intake Commands
self.balls.set(wpilib.DoubleSolenoid.Value.kForward)
self.motor5.set(0)
self.motor6.set(0)
if self.joy.getRawButton(2): #Turn Intake motors on and intake Belt
self.motor5.set(.15)
self.motor6.set(.1)
elif self.joy.getRawButton(7): #Relax.... take a rest and stop motors
self.motor6.set(0)
self.motor5.set(0)
#Motor shooting Speeds Below
if self.stick.getRawButton(1): #Low Goal - Face On (Distance 0)
self.motor5.set(.60)
self.motor6.set(-.40)
elif self.stick.getRawButton(2): #7.5-12.5 Ft Shot
self.motor5.set(.85)
self.motor6.set(-.15)
elif self.stick.getRawButton(3): #12.5-17.5 ft Shot
self.motor5.set(.60)
self.motor6.set(-.40)
elif self.stick.getRawButton(4): #17.5 - 22.5 ft Shot
self.motor5.set(.7)
self.motor6.set(-.7)
elif self.stick.getRawButton(5): #Stop motors
self.motor5.set(0)
self.motor6.set(0)
#Arm out and Feed Balls
if self.joy.getRawButton(4): #Arm Out
self.intake.set(wpilib.DoubleSolenoid.Value.kForward)
elif self.joy.getRawButton(5): #Arm in
self.intake.set(wpilib.DoubleSolenoid.Value.kReverse)
if self.stick.getRawButton(6): #Feed Balls
self.balls.set(wpilib.DoubleSolenoid.Value.kReverse)
elif self.stick.getRawButton(5): #Load Balls
self.balls.set(wpilib.DoubleSolenoid.Value.kForward)
class Tachyon(SampleRobot):
# because robotInit is called straight from __init__
def robotInit(self):
hardware.init() # this makes everything not break
self.drive = drive.Drive()
self.pneumatics = pneumatics.Pneumatics()
self.intake = intake.Intake()
self.elevator = elevator.Elevator()
self.components = {
'drive': self.drive,
'pneumatics': self.pneumatics,
'intake': self.intake,
'elevator': self.elevator,
}
self.nt_timer = Timer(
) # timer for SmartDashboard update so we don't use all our bandwidth
self.nt_timer.start()
self.autonomous_modes = AutonomousModeSelector('autonomous',
self.components)
self.state = States.STACKING
def autonomous(self):
self.autonomous_modes.run(CONTROL_LOOP_WAIT_TIME,
iter_fn=self.update_all)
Timer.delay(CONTROL_LOOP_WAIT_TIME)
def update_all(self):
self.update()
def disabled(self):
while self.isDisabled():
Timer.delay(0.01)
def operatorControl(self):
precise_delay = delay.PreciseDelay(CONTROL_LOOP_WAIT_TIME)
while self.isOperatorControl() and self.isEnabled():
# States!
if hardware.driver.left_trigger():
self.state = States.DROPPING
elif hardware.operator.right_trigger():
self.state = States.CAPPING
else:
self.state = States.STACKING
if self.elevator.is_full():
self.intake.pause()
elif not self.elevator.has_bin and not self.elevator.tote_first:
self.intake.intake_bin()
else:
self.intake.intake_tote()
if hardware.driver.right_bumper():
self.intake.open()
else:
self.intake.close()
if self.state == States.STACKING:
if hardware.operator.left_bumper() or True:
self.elevator.stack_tote_first()
if hardware.driver.a():
self.elevator.manual_stack()
elif self.state == States.DROPPING:
self.elevator.drop_stack()
self.elevator.reset_stack()
self.intake.pause()
self.intake.open()
if hardware.driver.right_bumper():
self.intake.close()
elif self.state == States.CAPPING:
self.elevator.cap()
wheel = deadband(hardware.driver.right_x(), .2)
throttle = -deadband(hardware.driver.left_y(), .2)
quickturn = hardware.driver.left_bumper()
low_gear = hardware.driver.right_trigger()
self.drive.cheesy_drive(wheel, throttle, quickturn, low_gear)
driver_dpad = hardware.driver.dpad()
if driver_dpad == 180: # down on the dpad
self.drive.set_distance_goal(-2)
elif driver_dpad == 0:
self.drive.set_distance_goal(2)
elif driver_dpad == 90:
self.drive.set_distance_goal(-18)
operator_dpad = hardware.operator.dpad(
) # You can only call it once per loop, bcus dbouncing
if operator_dpad == 0 and self.elevator.tote_count < 6:
self.elevator.tote_count += 1
elif operator_dpad == 180 and self.elevator.tote_count > 0:
self.elevator.tote_count -= 1
elif operator_dpad == 90:
self.elevator.has_bin = not self.elevator.has_bin
if hardware.operator.start():
self.elevator.reset_stack()
if hardware.operator.b():
self.intake.set(0) # Pause?!
if hardware.operator.a() or hardware.driver.b():
self.intake.set(-1)
# Deadman switch. very important for safety (at competitions).
if not self.ds.isFMSAttached(
) and not hardware.operator.left_trigger(
) and False: # TODO re-enable at competitions
for component in self.components.values():
component.stop()
else:
self.update()
precise_delay.wait()
def test(self):
while self.isTest() and self.isEnabled():
LiveWindow.run()
for component in self.components.values():
component.stop()
if self.nt_timer.hasPeriodPassed(.5):
component.update_nt()
def update(self):
""" Calls the update functions for every component """
for component in self.components.values():
try:
component.update()
if self.nt_timer.hasPeriodPassed(.5):
component.update_nt()
except Exception as e:
if self.ds.isFMSAttached():
log.error("In subsystem %s: %s" % (component, e))
else:
raise
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
# joystick 1 on the driver station
self.stick = wpilib.XboxController(0)
self.driverStation = wpilib.DriverStation
self.frontRight = ctre.wpi_talonsrx.WPI_TalonSRX(3)
self.rearRight = ctre.wpi_talonsrx.WPI_TalonSRX(1)
self.right = wpilib.SpeedControllerGroup(self.frontRight,
self.rearRight)
self.frontLeft = ctre.wpi_talonsrx.WPI_TalonSRX(4)
self.rearLeft = ctre.wpi_talonsrx.WPI_TalonSRX(2)
self.left = wpilib.SpeedControllerGroup(self.frontLeft, self.rearLeft)
self.frontRight.setExpiration(0.2)
self.rearRight.setExpiration(0.2)
self.frontRight.setExpiration(0.2)
self.rearLeft.setExpiration(0.2)
self.drive = DifferentialDrive(self.left, self.right)
# initialize motors other than drive
self.intakeRight = wpilib.VictorSP(0)
self.elevator = ctre.wpi_talonsrx.WPI_TalonSRX(
5) # Talon SRX controller with CAN address 5
self.intakeLeft = wpilib.VictorSP(2)
self.battleAxe = wpilib.VictorSP(3)
self.actuator = wpilib.Spark(4)
self.axeExtender = wpilib.Spark(5)
######################################
self.encoderTicksPerInch = 1159
self.elevator.setQuadraturePosition(0, 0)
self.elevator.configForwardSoftLimitThreshold(
int(round(-0.1 * self.encoderTicksPerInch)), 10)
self.elevator.configReverseSoftLimitThreshold(
int(round(-39.5 * self.encoderTicksPerInch)), 10)
self.elevator.configForwardSoftLimitEnable(True, 10)
self.elevator.configReverseSoftLimitEnable(True, 10)
self.elevator.configPeakOutputForward(0.8, 10)
self.elevator.configPeakOutputReverse(-1, 10)
self.elevator.set(ctre.ControlMode.Position, 0)
self.elevator.selectProfileSlot(0, 0)
self.elevator.config_kF(0, 0, 10)
self.elevator.config_kP(0, 0.6, 10)
self.elevator.config_kI(0, 0.003, 10)
self.elevator.config_kD(0, 0, 10)
self.elevator.config_IntegralZone(0, 100, 10)
self.elevator.configAllowableClosedloopError(
0, int(round(0.01 * self.encoderTicksPerInch)), 10)
# initialize limit switches and hall-effect sensors
self.actuatorSwitchMin = wpilib.DigitalInput(0)
self.actuatorSwitchMax = wpilib.DigitalInput(1)
self.battleAxeSwitchUp = wpilib.DigitalInput(2)
self.battleAxeSwitchDown = wpilib.DigitalInput(3)
self.battleAxeExtenderSwitch = wpilib.DigitalInput(4)
self.elevatorZeroSensor = wpilib.DigitalInput(5)
self.powerDistributionPanel = wpilib.PowerDistributionPanel()
self.powerDistributionPanel.resetTotalEnergy()
#
# Communicate w/navX MXP via the MXP SPI Bus.
# - Alternatively, use the i2c bus.
# See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details
#
self.navxSensor = navx.AHRS.create_spi()
# self.navxSensor = navx.AHRS.create_i2c()
# Items in this dictionary are available in your autonomous mode
# as attributes on your autonomous object
self.components = {
'drive': self.drive,
'navxSensor': self.navxSensor,
'actuator': self.actuator,
'actuatorSwitchMin': self.actuatorSwitchMin,
'actuatorSwitchMax': self.actuatorSwitchMax,
'elevator': self.elevator,
'intakeRight': self.intakeRight,
'intakeLeft': self.intakeLeft,
'gameData': self.driverStation.getInstance()
}
# * The first argument is the name of the package that your autonomous
# modes are located in
# * The second argument is passed to each StatefulAutonomous when they
# start up
self.automodes = AutonomousModeSelector('autonomous', self.components)
NetworkTables.initialize()
self.sd = NetworkTables.getTable("SmartDashboard")
wpilib.CameraServer.launch('vision.py:main')
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.timer = wpilib.Timer()
self.gyro = ADXLGyro.ADXLGyro()
self.leftGearbox = Gearbox.Gearbox([0, 1, 2])
self.rightGearbox = Gearbox.Gearbox([3, 4, 5], inverted=True)
self.intake = Intake.Intake()
self.leftJoystick = wpilib.Joystick(0)
self.rightJoystick = wpilib.Joystick(1)
self.prefs = wpilib.Preferences.getInstance()
self.prefs.put("Robot", "CraigPy")
self.components = {
'left': self.leftGearbox,
'right': self.rightGearbox,
'intake': self.intake,
'gyro': self.gyro,
'prefs': self.prefs,
'isSim': self.isSimulation(),
'utils': Utils
}
self.autonomous = AutonomousModeSelector('Autonomous', self.components)
self.gyro.calibrate()
self.i = 0
def update_gyro(self):
self.gyro.update()
def autonomousInit(self):
pass
def autonomousPeriodic(self):
"""This function is called periodically during autonomous."""
self.autonomous.run(iter_fn=self.update_gyro)
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
self.gyro.update()
leftSide = self.leftJoystick.getRawAxis(1)
rightSide = self.rightJoystick.getRawAxis(1)
self.leftGearbox.set(leftSide * 0.635)
self.rightGearbox.set(rightSide * 1)
if self.leftJoystick.getRawButton(
3) or self.rightJoystick.getRawButton(3):
self.intake.set(-0.5)
elif self.leftJoystick.getRawButton(
1) or self.rightJoystick.getRawButton(1):
self.intake.set(1)
else:
self.intake.set(0)
wpilib.SmartDashboard.putNumber("Gyro Center",
self.gyro.gyro.getCenter())
wpilib.SmartDashboard.putNumber("Gyro Angle",
self.gyro.gyro.getAngle())
wpilib.SmartDashboard.putNumber("Gyro Y", self.gyro.y)
self.i += 1
if self.i > 100:
self.gyro.reset()
self.i = 0
def disabledInit(self):
self.gyro.calibrate()
def testPeriodic(self):
"""This function is called periodically during test mode."""
pass
def clamp(self, minV, maxV, value):
return max(minV, min(maxV, value))
class MagicRobot(wpilib.SampleRobot, metaclass=OrderedClass):
"""
Robots that use the MagicBot framework should use this as their
base robot class. If you use this as your base, you must
implement the following methods:
- :meth:`createObjects`
- :meth:`teleopPeriodic`
MagicRobot uses the :class:`.AutonomousModeSelector` to allow you
to define multiple autonomous modes and to select one of them via
the SmartDashboard/SFX.
MagicRobot will set the following NetworkTables variables
automatically:
- ``/robot/mode``: one of 'disabled', 'auto', 'teleop', or 'test'
- ``/robot/is_simulation``: True/False
- ``/robot/is_ds_attached``: True/False
"""
#: Amount of time each loop takes (default is 20ms)
control_loop_wait_time = 0.020
#: Error report interval: when an FMS is attached, how often should
#: uncaught exceptions be reported?
error_report_interval = 0.5
def robotInit(self):
"""
.. warning:: Internal API, don't override; use :meth:`createObjects` instead
"""
self._exclude_from_injection = ['logger', 'members']
self.__last_error_report = -10
self._components = []
# Create the user's objects and stuff here
self.createObjects()
# Load autonomous modes
self._automodes = AutonomousModeSelector('autonomous')
# Next, create the robot components and wire them together
self._create_components()
self.__nt = NetworkTable.getTable('/robot')
self.__nt.putBoolean('is_simulation', self.isSimulation())
self.__nt.putBoolean('is_ds_attached', self.ds.isDSAttached())
def createObjects(self):
"""
You should override this and initialize all of your wpilib
objects here (and not in your components, for example). This
serves two purposes:
- It puts all of your motor/sensor initialization in the same
place, so that if you need to change a port/pin number it
makes it really easy to find it. Additionally, if you want
to create a simplified robot program to test a specific
thing, it makes it really easy to copy/paste it elsewhere
- It allows you to use the magic injection mechanism to share
variables between components
.. note:: Do not access your magic components in this function,
as their instances have not been created yet. Do not
create them either.
"""
raise NotImplementedError
def teleopInit(self):
"""
Initialization code for teleop control code may go here.
Users may override this method for initialization code which will be
called each time the robot enters teleop mode.
.. note:: The ``on_enable`` functions of all components are called
before this function is called.
"""
pass
def teleopPeriodic(self):
"""
Periodic code for teleop mode should go here.
Users should override this method for code which will be called
periodically at a regular rate while the robot is in teleop mode.
This code executes before the ``execute`` functions of all
components are called.
"""
func = self.teleopPeriodic.__func__
if not hasattr(func, "firstRun"):
self.logger.warn(
"Default MagicRobot.teleopPeriodic() method... Overload me!")
func.firstRun = False
def disabledInit(self):
"""
Initialization code for disabled mode may go here.
Users may override this method for initialization code which will be
called each time the robot enters disabled mode.
.. note:: The ``on_disable`` functions of all components are called
before this function is called.
"""
pass
def disabledPeriodic(self):
"""
Periodic code for disabled mode should go here.
Users should override this method for code which will be called
periodically at a regular rate while the robot is in disabled mode.
This code executes before the ``execute`` functions of all
components are called.
"""
func = self.disabledPeriodic.__func__
if not hasattr(func, "firstRun"):
self.logger.warn(
"Default MagicRobot.disabledPeriodic() method... Overload me!")
func.firstRun = False
def onException(self, forceReport=False):
'''
This function must *only* be called when an unexpected exception
has occurred that would otherwise crash the robot code. Use this
inside your :meth:`operatorActions` function.
If the FMS is attached (eg, during a real competition match),
this function will return without raising an error. However,
it will try to report one-off errors to the Driver Station so
that it will be recorded in the Driver Station Log Viewer.
Repeated errors may not get logged.
Example usage::
def teleopPeriodic(self):
try:
if self.joystick.getTrigger():
self.shooter.shoot()
except:
self.onException()
try:
if self.joystick.getRawButton(2):
self.ball_intake.run()
except:
self.onException()
# and so on...
:param forceReport: Always report the exception to the DS. Don't
set this to True
'''
# If the FMS is not attached, crash the robot program
if not self.ds.isFMSAttached():
raise
# Otherwise, if the FMS is attached then try to report the error via
# the driver station console. Maybe.
now = wpilib.Timer.getFPGATimestamp()
try:
if forceReport or (now - self.__last_error_report
) > self.error_report_interval:
wpilib.DriverStation.reportError("Unexpected exception", True)
except:
pass # ok, can't do anything here
self.__last_error_report = now
@contextlib.contextmanager
def consumeExceptions(self, forceReport=False):
"""
This returns a context manager which will consume any uncaught
exceptions that might otherwise crash the robot.
Example usage::
def teleopPeriodic(self):
with self.consumeExceptions():
if self.joystick.getTrigger():
self.shooter.shoot()
with self.consumeExceptions():
if self.joystick.getRawButton(2):
self.ball_intake.run()
# and so on...
:param forceReport: Always report the exception to the DS. Don't
set this to True
.. seealso:: :meth:`onException` for more details
"""
try:
yield
except:
self.onException(forceReport=forceReport)
#
# Internal API
#
def autonomous(self):
"""
MagicRobot will do The Right Thing and automatically load all
autonomous mode routines defined in the autonomous folder.
.. warning:: Internal API, don't override
"""
self.__nt.putString('mode', 'auto')
self.__nt.putBoolean('is_ds_attached', self.ds.isDSAttached())
self._on_mode_enable_components()
self._automodes.run(self.control_loop_wait_time,
self._execute_components, self.onException)
self._on_mode_disable_components()
def disabled(self):
"""
This function is called in disabled mode. You should not
override this function; rather, you should override the
:meth:`disabledPeriodic` function instead.
.. warning:: Internal API, don't override
"""
self.__nt.putString('mode', 'disabled')
ds_attached = None
delay = PreciseDelay(self.control_loop_wait_time)
self._on_mode_disable_components()
try:
self.disabledInit()
except:
self.onException(forceReport=True)
while self.isDisabled():
if ds_attached != self.ds.isDSAttached():
ds_attached = not ds_attached
self.__nt.putBoolean('is_ds_attached', ds_attached)
try:
self.disabledPeriodic()
except:
self.onException()
delay.wait()
def operatorControl(self):
"""
This function is called in teleoperated mode. You should not
override this function; rather, you should override the
:meth:`teleopPeriodics` function instead.
.. warning:: Internal API, don't override
"""
self.__nt.putString('mode', 'teleop')
# don't need to update this during teleop -- presumably will switch
# modes when ds is no longer attached
self.__nt.putBoolean('is_ds_attached', self.ds.isDSAttached())
delay = PreciseDelay(self.control_loop_wait_time)
# initialize things
self._on_mode_enable_components()
try:
self.teleopInit()
except:
self.onException(forceReport=True)
while self.isOperatorControl() and self.isEnabled():
#self._update_autosend()
try:
self.teleopPeriodic()
except:
self.onException()
self._execute_components()
delay.wait()
self._on_mode_disable_components()
def test(self):
'''Called when the robot is in test mode'''
self.__nt.putString('mode', 'test')
self.__nt.putBoolean('is_ds_attached', self.ds.isDSAttached())
while self.isTest() and self.isEnabled():
wpilib.LiveWindow.run()
wpilib.Timer.delay(.01)
def _on_mode_enable_components(self):
# initialize things
for component in self._components:
if hasattr(component, 'on_enable'):
try:
component.on_enable()
except:
self.onException(forceReport=True)
def _on_mode_disable_components(self):
# deinitialize things
for component in self._components:
if hasattr(component, 'on_disable'):
try:
component.on_disable()
except:
self.onException(forceReport=True)
def _create_components(self):
#
# TODO: Will need to inject into any autonomous mode component
# too, as they're a bit different
#
# TODO: Will need to order state machine components before
# other components just in case
components = []
self.logger.info("Creating magic components")
# Identify all of the types, and create them
cls = self.__class__
for m in self.members:
if m.startswith('_') or isinstance(getattr(cls, m, None),
_TunableProperty):
continue
ctyp = getattr(self, m)
if not isinstance(ctyp, type):
continue
# Create instance, set it on self
component = ctyp()
setattr(self, m, component)
# Ensure that mandatory methods are there
if not hasattr(component, 'execute') or \
not callable(component.execute):
raise ValueError(
"Component %s (%s) must have a method named 'execute'" %
(m, component))
# Automatically inject a logger object
component.logger = logging.getLogger(m)
component._Magicbot__autosend = {}
# Store for later
components.append((m, component))
self.logger.info("-> %s (class: %s)", m, ctyp.__name__)
# Collect attributes of this robot that are injectable
self._injectables = {}
for n in dir(self):
if n.startswith('_') or n in self._exclude_from_injection or \
isinstance(getattr(cls, n, None), _TunableProperty):
continue
o = getattr(self, n)
# Don't inject methods
# TODO: This could actually be a cool capability..
if inspect.ismethod(o):
continue
self._injectables[n] = o
# For each new component, perform magic injection
for cname, component in components:
self._components.append(component)
setup_tunables(component, cname, 'components')
self._setup_vars(cname, component)
# Do it for autonomous modes too
for mode in self._automodes.modes.values():
mode.logger = logging.getLogger(mode.MODE_NAME)
setup_tunables(mode, mode.MODE_NAME, 'autonomous')
self._setup_vars(mode.MODE_NAME, mode)
# And for self too
setup_tunables(self, 'robot', None)
# Call setup functions for components
for cname, component in components:
if hasattr(component, 'setup'):
component.setup()
def _setup_vars(self, cname, component):
self.logger.debug("Injecting magic variables into %s", cname)
component_type = type(component)
# Iterate over variables with type annotations
for n, inject_type in getattr(component, '__annotations__',
{}).items():
# If the variable is private ignore it
if n.startswith('_'):
continue
if hasattr(component_type, n):
attr = getattr(component_type, n)
# If the value given to the variable is an instance of a type and isn't a property
# raise an error. No double declaring types, e.g foo: type = type
if isinstance(attr, type) and not isinstance(attr, property):
raise ValueError(
"Double Declaration: %s.%s has two type declarations" %
(component_type.__name__, n))
continue
self._inject(n, inject_type, cname, component_type)
# Iterate over static variables
for n in dir(component):
# If the variable is private or a proprty, don't inject
if n.startswith('_') or isinstance(
getattr(component_type, n, True), property):
continue
inject_type = getattr(component, n)
# If the value assigned isn't a type, don't inject
if not isinstance(inject_type, type):
continue
self._inject(n, inject_type, cname, component)
def _inject(self, n, inject_type, cname, component):
# Retrieve injectable object
injectable = self._injectables.get(n)
if injectable is None:
if cname is not None:
# Try for mangled names
injectable = self._injectables.get('%s_%s' % (cname, n))
# Raise error if injectable syntax used but no injectable was found.
if injectable is None:
raise ValueError(
"Component %s has variable %s (type %s), which is not present in %s"
% (cname, n, inject_type, self))
# Raise error if injectable declared with type different than the initial type
if not isinstance(injectable, inject_type):
raise ValueError(
"Component %s variable %s in %s are not the same types! (Got %s, expected %s)"
% (cname, n, self, type(injectable), inject_type))
# Perform injection
setattr(component, n, injectable)
self.logger.debug("%s -> %s as %s.%s", injectable, cname, n)
# XXX
#if is_autosend:
# where to store the nt key?
# component._Magicbot__autosend[prop.f] = None
#def _update_autosend(self):
# # seems like this should just be a giant list instead
# for component in self._components:
# d = component._Magicbot__autosend
# for f in d.keys():
# d[f] = f(component)
def _execute_components(self):
for component in self._components:
try:
component.execute()
except:
self.onException()
class MyRobot(wpilib.IterativeRobot):
rLeftChannel = 1
fLeftChannel = 2
fRightChannel = 3
rRightChannel = 4
joystickChannel = 0
def robotInit(self):
wpilib.CameraServer.launch('misc/vision.py:main')
if not wpilib.RobotBase.isSimulation(
): #This makes simulator show motor outputs for debugging
import ctre
self.RLC = ctre.CANTalon(self.rLeftChannel)
self.FLC = ctre.CANTalon(self.fLeftChannel)
self.FRC = ctre.CANTalon(self.fRightChannel)
self.RRC = ctre.CANTalon(self.rRightChannel)
else:
self.RLC = wpilib.Talon(self.rLeftChannel)
self.FLC = wpilib.Talon(self.fLeftChannel)
self.FRC = wpilib.Talon(self.fRightChannel)
self.RRC = wpilib.Talon(self.rRightChannel)
self.robotDrive = wpilib.RobotDrive(
self.RLC, self.RRC, self.FRC,
self.FLC) #Sets motors for robotDrive commands
#Controller Input Variables
self.controller = wpilib.Joystick(self.joystickChannel)
self.winch_backward = wpilib.buttons.JoystickButton(self.controller, 5)
self.paddleGet = wpilib.buttons.JoystickButton(self.controller, 1)
self.gearDrop = wpilib.buttons.JoystickButton(self.controller,
6) # Right Bumper
self.xboxMec = wpilib.buttons.JoystickButton(self.controller, 8)
self.xboxMec2 = wpilib.buttons.JoystickButton(self.controller, 7)
#CRio Output Variables
self.winch_motor1 = wpilib.Talon(7)
self.winch_motor2 = wpilib.Talon(8)
self.paddle = wpilib.Solenoid(1)
self.gear = wpilib.DoubleSolenoid(2, 3)
self.ultrasonic = wpilib.Ultrasonic(5, 4) #trigger to echo
self.ultrasonic.setAutomaticMode(True)
self.navx = navx.AHRS.create_spi()
#Auto mode variables
self.components = {
'drive': self.robotDrive,
'gearDrop': self.gear,
'ultrasonic': self.ultrasonic,
'navx': self.navx
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
def autonomousPeriodic(self):
self.automodes.run()
def teleopPeriodic(self):
### Climbing code
if (self.winch_backward.get()):
self.winch_motor1.set(-1 * self.controller.getRawAxis(2))
self.winch_motor2.set(-1 * self.controller.getRawAxis(2))
else:
if self.controller.getRawAxis(2) > 0.1:
self.winch_motor1.set(self.controller.getRawAxis(2))
self.winch_motor2.set(self.controller.getRawAxis(2))
else:
self.winch_motor1.set(0)
self.winch_motor2.set(0)
### Paddle/flipper assist code
if (self.paddleGet.get()):
self.paddle.set(True)
else:
self.paddle.set(False)
### Gear dropping code
if (self.gearDrop.get()):
self.gear.set(wpilib.DoubleSolenoid.Value.kForward)
else:
self.gear.set(wpilib.DoubleSolenoid.Value.kReverse)
#RobotDrive Code
self.robotDrive.mecanumDrive_Cartesian(
-1 * self.controller.getY(), -1 * self.controller.getRawAxis(4),
self.controller.getX(), 0)
def robotInit(self):
#Magic Numbers
self.rotateToAngleRate = 0
self.ControlSwitch = "GM"
#Sensors
self.LeftEncoder = wpilib.Encoder(0, 1)
self.LeftEncoder.reset()
self.RightEncoder = wpilib.Encoder(2, 3, True)
self.RightEncoder.reset()
self.LeftEncoder.setDistancePerPulse((4*math.pi)/256)
self.RightEncoder.setDistancePerPulse((4*math.pi)/256)
self.Gyro = AHRS.create_spi()
self.Gyro.reset()
wpilib.CameraServer.launch()
self.Sonar = wpilib.AnalogInput(0)
self.GS1 = wpilib.AnalogInput(1)
self.GS2 = wpilib.AnalogInput(2)
#wpilib.CameraServer.launch('vision.py:main')
#Motors
self.LDT = TripleMotorGearbox(5, 6, 7, self.LeftEncoder)
self.RDT = TripleMotorGearbox(0, 1, 2, self.RightEncoder)
self.RDT.setInverted(True)
self.LowIntake = wpilib.VictorSP(9)
self.Winch = wpilib.VictorSP(3)
self.Agitator = wpilib.VictorSP(10)
self.Conveyor = wpilib.VictorSP(4)
self.Shooter = ctre.CANTalon(25)
#Controllers
self.LeftJoystick = wpilib.Joystick(0)
self.RightJoystick = wpilib.Joystick(1)
self.XboxController = wpilib.Joystick(2)
#Pneumatics
self.Compressor = wpilib.Compressor(0)
self.Compressor.setClosedLoopControl(True)
self.PSV = self.Compressor.getPressureSwitchValue()
self.GearMech = wpilib.DoubleSolenoid(4, 3)
self.TopFlap = wpilib.DoubleSolenoid(1, 2)
self.Compressor.start()
#Smart Dashboard
self.nt = network_table.Network()
self.SD = NetworkTable.getTable("SmartDashboard")
self.SD.putBoolean(" Is Gear Mech Out?", False)
self.SD.putNumber("Gyro", self.Gyro.getAngle())
#Autonomous
self.components = {
'LDT': self.LDT,
'RDT': self.RDT,
'SD':self.SD,
'LowIntake': self.LowIntake,
'LeftEncoder': self.LeftEncoder,
'RightEncoder': self.RightEncoder,
'Sonar': self.Sonar,
'Gyro': self.Gyro,
'GearMech': self.GearMech,
'Top Flap': self.TopFlap,
'GS1': self.GS1,
'GS2': self.GS2}
self.Auton = AutonomousModeSelector('autonomous', self.components)
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
#Magic Numbers
self.rotateToAngleRate = 0
self.ControlSwitch = "GM"
#Sensors
self.LeftEncoder = wpilib.Encoder(0, 1)
self.LeftEncoder.reset()
self.RightEncoder = wpilib.Encoder(2, 3, True)
self.RightEncoder.reset()
self.LeftEncoder.setDistancePerPulse((4*math.pi)/256)
self.RightEncoder.setDistancePerPulse((4*math.pi)/256)
self.Gyro = AHRS.create_spi()
self.Gyro.reset()
wpilib.CameraServer.launch()
self.Sonar = wpilib.AnalogInput(0)
self.GS1 = wpilib.AnalogInput(1)
self.GS2 = wpilib.AnalogInput(2)
#wpilib.CameraServer.launch('vision.py:main')
#Motors
self.LDT = TripleMotorGearbox(5, 6, 7, self.LeftEncoder)
self.RDT = TripleMotorGearbox(0, 1, 2, self.RightEncoder)
self.RDT.setInverted(True)
self.LowIntake = wpilib.VictorSP(9)
self.Winch = wpilib.VictorSP(3)
self.Agitator = wpilib.VictorSP(10)
self.Conveyor = wpilib.VictorSP(4)
self.Shooter = ctre.CANTalon(25)
#Controllers
self.LeftJoystick = wpilib.Joystick(0)
self.RightJoystick = wpilib.Joystick(1)
self.XboxController = wpilib.Joystick(2)
#Pneumatics
self.Compressor = wpilib.Compressor(0)
self.Compressor.setClosedLoopControl(True)
self.PSV = self.Compressor.getPressureSwitchValue()
self.GearMech = wpilib.DoubleSolenoid(4, 3)
self.TopFlap = wpilib.DoubleSolenoid(1, 2)
self.Compressor.start()
#Smart Dashboard
self.nt = network_table.Network()
self.SD = NetworkTable.getTable("SmartDashboard")
self.SD.putBoolean(" Is Gear Mech Out?", False)
self.SD.putNumber("Gyro", self.Gyro.getAngle())
#Autonomous
self.components = {
'LDT': self.LDT,
'RDT': self.RDT,
'SD':self.SD,
'LowIntake': self.LowIntake,
'LeftEncoder': self.LeftEncoder,
'RightEncoder': self.RightEncoder,
'Sonar': self.Sonar,
'Gyro': self.Gyro,
'GearMech': self.GearMech,
'Top Flap': self.TopFlap,
'GS1': self.GS1,
'GS2': self.GS2}
self.Auton = AutonomousModeSelector('autonomous', self.components)
def autonomousPeriodic(self):
self.SD.putNumber("Gyro", self.Gyro.getYaw())
self.Auton.run()
def teleopInit(self):
self.motorUpdatePeriod = 0.005
self.networkUpdatePeriod = 0.25
self.LeftEncoder.reset()
self.RightEncoder.reset()
def teleopPeriodic(self):
self.Gyro.reset()
while self.isEnabled():
#self.SD.putNumber("Am i Alive?/Angle ", self.Gyro.getYaw())
self.SD.putNumber("Gyro", self.Gyro.getYaw())
self.SD.putNumber(" Sonar Voltage", self.Sonar.getVoltage())
self.SD.putString("Operator Stage", self.ControlSwitch)
self.SD.putNumber("Voltage 1", self.GS1.getAverageVoltage())
self.SD.putNumber("Voltage 2", self.GS2.getAverageVoltage())
#Drive Train
self.LJV = self.LeftJoystick.getY()
self.RJV = self.RightJoystick.getY()
# FORWARD
if(self.LJV > 0.03 and self.RJV > 0.03):
Lvalue = robot_utils.Efficiency(self, robot_utils.cookJoystickInputs2(self, self.LeftJoystick.getY()))
Rvalue = robot_utils.Efficiency(self, robot_utils.cookJoystickInputs2(self, self.RightJoystick.getY()))
self.LDT.set(-Lvalue)
self.RDT.set(-Rvalue)
# BACKWARDS
elif(self.LJV < -0.03 and self.RJV < -0.03):
Lvalue = robot_utils.Efficiency(self, robot_utils.cookJoystickInputs2(self, self.LeftJoystick.getY()))
Rvalue = robot_utils.Efficiency(self, robot_utils.cookJoystickInputs2(self, self.RightJoystick.getY()))
self.LDT.set(-Lvalue)
self.RDT.set(-Rvalue)
# Turning With Both Joysticks
elif((self.LJV > 0.03 and self.RJV < -0.03) or (self.LJV < -0.03 and self.RJV > 0.03)):
Lvalue = robot_utils.Efficiency(self, robot_utils.cookJoystickInputs2(self, self.LeftJoystick.getY()))
Rvalue = robot_utils.Efficiency(self, robot_utils.cookJoystickInputs2(self, self.RightJoystick.getY()))
self.LDT.set(-Lvalue/1.46)
self.RDT.set(-Rvalue/1.46)
# Spinning Only Left Joystick
elif((self.LJV > 0 or self.LJV < 0) and (self.RJV > -0.15 and self.RJV < 0.15)):
Lvalue = robot_utils.Efficiency(self, robot_utils.cookJoystickInputs2(self, self.LeftJoystick.getY()))
Rvalue = robot_utils.Efficiency(self, robot_utils.cookJoystickInputs2(self, self.RightJoystick.getY()))
self.LDT.set(-Lvalue/1.25)
self.RDT.set(-Rvalue/1.25)
# Spinning Only Right Joystick
elif((self.RJV > 0 or self.RJV < 0) and (self.LJV > -0.15 and self.LJV < 0.15)):
Lvalue = robot_utils.Efficiency(self, robot_utils.cookJoystickInputs2(self, self.LeftJoystick.getY()))
Rvalue = robot_utils.Efficiency(self, robot_utils.cookJoystickInputs2(self, self.RightJoystick.getY()))
self.LDT.set(-Lvalue/1.25)
self.RDT.set(-Rvalue/1.25)
else:
self.LDT.set(0)
self.RDT.set(0)
#MICRO_CONTROLS
while(self.LeftJoystick.getRawButton(5)):
try:
self.LDT.set(.17)
self.RDT.set(-.17)
except TypeError:
pass
while(self.LeftJoystick.getRawButton(4)):
try:
self.LDT.set(-.17)
self.RDT.set(.17)
except TypeError:
pass
while(self.LeftJoystick.getRawButton(2)):
try:
self.LDT.set(-.17)
self.RDT.set(-.17)
except TypeError:
pass
while(self.LeftJoystick.getRawButton(3)):
try:
self.LDT.set(.17)
self.RDT.set(.17)
except TypeError:
pass
#MACRO_CONTROLS
while(self.RightJoystick.getRawButton(5)):
try:
self.LDT.set(.3)
self.RDT.set(-.3)
except TypeError:
pass
while(self.RightJoystick.getRawButton(4)):
try:
self.LDT.set(-.3)
self.RDT.set(.3)
except TypeError:
pass
while(self.RightJoystick.getRawButton(3)):
try:
self.LDT.set(.3)
self.RDT.set(.3)
except TypeError:
pass
while(self.RightJoystick.getRawButton(2)):
try:
self.LDT.set(-.3)
self.RDT.set(-.3)
except TypeError:
pass
if(self.XboxController.getRawButton(5)):
self.LowIntake.set(0.875)
elif(self.XboxController.getRawButton(6)):
self.LowIntake.set(-0.875)
else:
self.LowIntake.set(0)
if(self.XboxController.getRawButton(7)):
self.ControlSwitch = "SH"
elif(self.XboxController.getRawButton(8)):
self.ControlSwitch = "GM"
if(self.ControlSwitch == "GM"):
if(self.XboxController.getRawButton(2)):
self.TopFlap.set(DoubleSolenoid.Value.kForward)
if(self.XboxController.getRawButton(4)):
self.TopFlap.set(DoubleSolenoid.Value.kReverse)
if(self.XboxController.getRawButton(1)):
self.GearMech.set(DoubleSolenoid.Value.kForward)
if(self.XboxController.getRawButton(3)):
self.GearMech.set(DoubleSolenoid.Value.kReverse)
#Winch
if(self.XboxController.getY()>.5):
self.Winch.set(.5)
elif(self.XboxController.getY()<-.5):
self.Winch.set(-.5)
else:
self.Winch.set(0)
elif(self.ControlSwitch == "SH"):
#Agitator
if(self.XboxController.getY() > 0.5):
self.Agitator.set(0.875)
elif(self.XboxController.getY() < -0.5):
self.Agitator.set(-0.875)
else:
self.Agitator.set(0)
#Conveyor
if(self.XboxController.getRawButton(1)):
self.Conveyor.set(0.875)
else:
self.Conveyor.set(0)
#Shooter
if(self.XboxController.getRawButton(3)):
self.Shooter.set(0.5)
else:
self.Shooter.set(0)
wpilib.Timer.delay(self.motorUpdatePeriod)
def disabledInit(self):
pass
def disabledPeriodic(self):
pass
def testInit(self):
pass
def testPeriodic(self):
pass
class MagicRobot(wpilib._wpilib.RobotBaseUser):
"""
Robots that use the MagicBot framework should use this as their
base robot class. If you use this as your base, you must
implement the following methods:
- :meth:`createObjects`
- :meth:`teleopPeriodic`
MagicRobot uses the :class:`.AutonomousModeSelector` to allow you
to define multiple autonomous modes and to select one of them via
the SmartDashboard/Shuffleboard.
MagicRobot will set the following NetworkTables variables
automatically:
- ``/robot/mode``: one of 'disabled', 'auto', 'teleop', or 'test'
- ``/robot/is_simulation``: True/False
- ``/robot/is_ds_attached``: True/False
"""
#: Amount of time each loop takes (default is 20ms)
control_loop_wait_time = 0.020
#: Error report interval: when an FMS is attached, how often should
#: uncaught exceptions be reported?
error_report_interval = 0.5
#: A Python logging object that you can use to send messages to the log.
#: It is recommended to use this instead of print statements.
logger = logging.getLogger("robot")
#: If True, teleopPeriodic will be called in autonomous mode
use_teleop_in_autonomous = False
def robotInit(self):
"""
.. warning:: Internal API, don't override; use :meth:`createObjects` instead
"""
self._exclude_from_injection = ["logger"]
self.__last_error_report = -10
self._components = []
self._feedbacks = []
self._reset_components = []
# Create the user's objects and stuff here
self.createObjects()
# Load autonomous modes
self._automodes = AutonomousModeSelector("autonomous")
# Next, create the robot components and wire them together
self._create_components()
self.__nt = NetworkTables.getTable("/robot")
self.__nt_put_is_ds_attached = self.__nt.getEntry(
"is_ds_attached").setBoolean
self.__nt_put_mode = self.__nt.getEntry("mode").setString
self.__nt.putBoolean("is_simulation", self.isSimulation())
self.__nt_put_is_ds_attached(self.ds.isDSAttached())
self.__done = False
# cache these
self.__sd_update = wpilib.SmartDashboard.updateValues
self.__lv_update = wpilib.LiveWindow.getInstance().updateValues
# self.__sf_update = Shuffleboard.update
self.watchdog = SimpleWatchdog(self.control_loop_wait_time)
def createObjects(self):
"""
You should override this and initialize all of your wpilib
objects here (and not in your components, for example). This
serves two purposes:
- It puts all of your motor/sensor initialization in the same
place, so that if you need to change a port/pin number it
makes it really easy to find it. Additionally, if you want
to create a simplified robot program to test a specific
thing, it makes it really easy to copy/paste it elsewhere
- It allows you to use the magic injection mechanism to share
variables between components
.. note:: Do not access your magic components in this function,
as their instances have not been created yet. Do not
create them either.
"""
raise NotImplementedError
def autonomousInit(self):
"""Initialization code for autonomous mode may go here.
Users may override this method for initialization code which
will be called each time the robot enters autonomous mode,
regardless of the selected autonomous mode.
This can be useful for code that must be run at the beginning of a match.
.. note:: The ``on_enable`` functions of all components are
called before this function is called.
"""
pass
def teleopInit(self):
"""
Initialization code for teleop control code may go here.
Users may override this method for initialization code which will be
called each time the robot enters teleop mode.
.. note:: The ``on_enable`` functions of all components are called
before this function is called.
"""
pass
def teleopPeriodic(self):
"""
Periodic code for teleop mode should go here.
Users should override this method for code which will be called
periodically at a regular rate while the robot is in teleop mode.
This code executes before the ``execute`` functions of all
components are called.
.. note:: If you want this function to be called in autonomous
mode, set ``use_teleop_in_autonomous`` to True in your
robot class.
"""
func = self.teleopPeriodic.__func__
if not hasattr(func, "firstRun"):
self.logger.warning(
"Default MagicRobot.teleopPeriodic() method... Override me!")
func.firstRun = False
def disabledInit(self):
"""
Initialization code for disabled mode may go here.
Users may override this method for initialization code which will be
called each time the robot enters disabled mode.
.. note:: The ``on_disable`` functions of all components are called
before this function is called.
"""
pass
def disabledPeriodic(self):
"""
Periodic code for disabled mode should go here.
Users should override this method for code which will be called
periodically at a regular rate while the robot is in disabled mode.
This code executes before the ``execute`` functions of all
components are called.
"""
func = self.disabledPeriodic.__func__
if not hasattr(func, "firstRun"):
self.logger.warning(
"Default MagicRobot.disabledPeriodic() method... Override me!")
func.firstRun = False
def testInit(self):
"""Initialization code for test mode should go here.
Users should override this method for initialization code which will be
called each time the robot enters disabled mode.
"""
pass
def testPeriodic(self):
"""Periodic code for test mode should go here."""
pass
def robotPeriodic(self):
"""
Periodic code for all modes should go here.
Users must override this method to utilize it
but it is not required.
This function gets called last in each mode.
You may use it for any code you need to run
during all modes of the robot (e.g NetworkTables updates)
The default implementation will update
SmartDashboard, LiveWindow and Shuffleboard.
"""
watchdog = self.watchdog
self.__sd_update()
watchdog.addEpoch("SmartDashboard")
self.__lv_update()
watchdog.addEpoch("LiveWindow")
# self.__sf_update()
# watchdog.addEpoch("Shuffleboard")
def onException(self, forceReport: bool = False) -> None:
"""
This function must *only* be called when an unexpected exception
has occurred that would otherwise crash the robot code. Use this
inside your :meth:`operatorActions` function.
If the FMS is attached (eg, during a real competition match),
this function will return without raising an error. However,
it will try to report one-off errors to the Driver Station so
that it will be recorded in the Driver Station Log Viewer.
Repeated errors may not get logged.
Example usage::
def teleopPeriodic(self):
try:
if self.joystick.getTrigger():
self.shooter.shoot()
except:
self.onException()
try:
if self.joystick.getRawButton(2):
self.ball_intake.run()
except:
self.onException()
# and so on...
:param forceReport: Always report the exception to the DS. Don't
set this to True
"""
# If the FMS is not attached, crash the robot program
if not self.ds.isFMSAttached():
raise
# Otherwise, if the FMS is attached then try to report the error via
# the driver station console. Maybe.
now = wpilib.Timer.getFPGATimestamp()
try:
if (forceReport or
(now - self.__last_error_report) > self.error_report_interval):
wpilib.DriverStation.reportError("Unexpected exception", True)
except:
pass # ok, can't do anything here
self.__last_error_report = now
@contextlib.contextmanager
def consumeExceptions(self, forceReport: bool = False):
"""
This returns a context manager which will consume any uncaught
exceptions that might otherwise crash the robot.
Example usage::
def teleopPeriodic(self):
with self.consumeExceptions():
if self.joystick.getTrigger():
self.shooter.shoot()
with self.consumeExceptions():
if self.joystick.getRawButton(2):
self.ball_intake.run()
# and so on...
:param forceReport: Always report the exception to the DS. Don't
set this to True
.. seealso:: :meth:`onException` for more details
"""
try:
yield
except:
self.onException(forceReport=forceReport)
#
# Internal API
#
def startCompetition(self) -> None:
"""
This runs the mode-switching loop.
.. warning:: Internal API, don't override
"""
# TODO: usage reporting?
self.robotInit()
# Tell the DS the robot is ready to be enabled
hal.observeUserProgramStarting()
while not self.__done:
isEnabled, isAutonomous, isTest = self.getControlState()
if not isEnabled:
self._disabled()
elif isAutonomous:
self.autonomous()
elif isTest:
self._test()
else:
self._operatorControl()
def endCompetition(self) -> None:
self.__done = True
def autonomous(self):
"""
MagicRobot will do The Right Thing and automatically load all
autonomous mode routines defined in the autonomous folder.
.. warning:: Internal API, don't override
"""
self.__nt_put_mode("auto")
self.__nt_put_is_ds_attached(self.ds.isDSAttached())
self._on_mode_enable_components()
try:
self.autonomousInit()
except:
self.onException(forceReport=True)
auto_functions = (
self._execute_components,
self._update_feedback,
self.robotPeriodic,
)
if self.use_teleop_in_autonomous:
auto_functions = (self.teleopPeriodic, ) + auto_functions
self._automodes.run(
self.control_loop_wait_time,
auto_functions,
self.onException,
watchdog=self.watchdog,
)
self._on_mode_disable_components()
def _disabled(self):
"""
This function is called in disabled mode. You should not
override this function; rather, you should override the
:meth:`disabledPeriodic` function instead.
.. warning:: Internal API, don't override
"""
watchdog = self.watchdog
watchdog.reset()
self.__nt_put_mode("disabled")
ds_attached = None
self._on_mode_disable_components()
try:
self.disabledInit()
except:
self.onException(forceReport=True)
watchdog.addEpoch("disabledInit()")
with NotifierDelay(self.control_loop_wait_time) as delay:
while not self.__done and self.isDisabled():
if ds_attached != self.ds.isDSAttached():
ds_attached = not ds_attached
self.__nt_put_is_ds_attached(ds_attached)
hal.observeUserProgramDisabled()
try:
self.disabledPeriodic()
except:
self.onException()
watchdog.addEpoch("disabledPeriodic()")
self._update_feedback()
self.robotPeriodic()
watchdog.addEpoch("robotPeriodic()")
# watchdog.disable()
watchdog.printIfExpired()
delay.wait()
watchdog.reset()
def _operatorControl(self):
"""
This function is called in teleoperated mode. You should not
override this function; rather, you should override the
:meth:`teleopPeriodics` function instead.
.. warning:: Internal API, don't override
"""
watchdog = self.watchdog
watchdog.reset()
self.__nt_put_mode("teleop")
# don't need to update this during teleop -- presumably will switch
# modes when ds is no longer attached
self.__nt_put_is_ds_attached(self.ds.isDSAttached())
# initialize things
self._on_mode_enable_components()
try:
self.teleopInit()
except:
self.onException(forceReport=True)
watchdog.addEpoch("teleopInit()")
observe = hal.observeUserProgramTeleop
with NotifierDelay(self.control_loop_wait_time) as delay:
while not self.__done and self.isOperatorControlEnabled():
observe()
try:
self.teleopPeriodic()
except:
self.onException()
watchdog.addEpoch("teleopPeriodic()")
self._execute_components()
self._update_feedback()
self.robotPeriodic()
watchdog.addEpoch("robotPeriodic()")
# watchdog.disable()
watchdog.printIfExpired()
delay.wait()
watchdog.reset()
self._on_mode_disable_components()
def _test(self):
"""Called when the robot is in test mode"""
watchdog = self.watchdog
watchdog.reset()
self.__nt_put_mode("test")
self.__nt_put_is_ds_attached(self.ds.isDSAttached())
lw = wpilib.LiveWindow.getInstance()
lw.setEnabled(True)
# Shuffleboard.enableActuatorWidgets()
try:
self.testInit()
except:
self.onException(forceReport=True)
watchdog.addEpoch("testInit()")
with NotifierDelay(self.control_loop_wait_time) as delay:
while not self.__done and self.isTest() and self.isEnabled():
hal.observeUserProgramTest()
try:
self.testPeriodic()
except:
self.onException()
watchdog.addEpoch("testPeriodic()")
self._update_feedback()
self.robotPeriodic()
watchdog.addEpoch("robotPeriodic()")
# watchdog.disable()
watchdog.printIfExpired()
delay.wait()
watchdog.reset()
lw.setEnabled(False)
# Shuffleboard.disableActuatorWidgets()
def _on_mode_enable_components(self):
# initialize things
for _, component in self._components:
on_enable = getattr(component, "on_enable", None)
if on_enable is not None:
try:
on_enable()
except:
self.onException(forceReport=True)
def _on_mode_disable_components(self):
# deinitialize things
for _, component in self._components:
on_disable = getattr(component, "on_disable", None)
if on_disable is not None:
try:
on_disable()
except:
self.onException(forceReport=True)
def _create_components(self):
#
# TODO: Will need to inject into any autonomous mode component
# too, as they're a bit different
#
# TODO: Will need to order state machine components before
# other components just in case
components = []
self.logger.info("Creating magic components")
# Identify all of the types, and create them
cls = type(self)
# - Iterate over class variables with type annotations
# .. this hack is necessary for pybind11 based modules
class FakeModule:
pass
import sys
sys.modules["pybind11_builtins"] = FakeModule()
for m, ctyp in typing.get_type_hints(cls).items():
# Ignore private variables
if m.startswith("_"):
continue
# If the variable has been set, skip it
if hasattr(self, m):
continue
# If the type is not actually a type, give a meaningful error
if not isinstance(ctyp, type):
raise TypeError(
"%s has a non-type annotation on %s (%r); lone non-injection variable annotations are disallowed, did you want to assign a static variable?"
% (cls.__name__, m, ctyp))
component = self._create_component(m, ctyp)
# Store for later
components.append((m, component))
# Collect attributes of this robot that are injectable
self._injectables = {}
for n in dir(self):
if (n.startswith("_") or n in self._exclude_from_injection
or isinstance(getattr(cls, n, None), tunable)):
continue
o = getattr(self, n)
# Don't inject methods
# TODO: This could actually be a cool capability..
if inspect.ismethod(o):
continue
self._injectables[n] = o
# For each new component, perform magic injection
for cname, component in components:
setup_tunables(component, cname, "components")
self._setup_vars(cname, component)
self._setup_reset_vars(component)
# Do it for autonomous modes too
for mode in self._automodes.modes.values():
mode.logger = logging.getLogger(mode.MODE_NAME)
setup_tunables(mode, mode.MODE_NAME, "autonomous")
self._setup_vars(mode.MODE_NAME, mode)
# And for self too
setup_tunables(self, "robot", None)
self._feedbacks += collect_feedbacks(self, "robot", None)
# Call setup functions for components
for cname, component in components:
setup = getattr(component, "setup", None)
if setup is not None:
setup()
# ... and grab all the feedback methods
self._feedbacks += collect_feedbacks(component, cname,
"components")
# Call setup functions for autonomous modes
for mode in self._automodes.modes.values():
if hasattr(mode, "setup"):
mode.setup()
self._components = components
def _create_component(self, name: str, ctyp: type):
# Create instance, set it on self
component = ctyp()
setattr(self, name, component)
# Ensure that mandatory methods are there
if not callable(getattr(component, "execute", None)):
raise ValueError(
"Component %s (%r) must have a method named 'execute'" %
(name, component))
# Automatically inject a logger object
component.logger = logging.getLogger(name)
self.logger.info("-> %s (class: %s)", name, ctyp.__name__)
return component
def _setup_vars(self, cname: str, component):
self.logger.debug("Injecting magic variables into %s", cname)
component_type = type(component)
# Iterate over variables with type annotations
for n, inject_type in typing.get_type_hints(component_type).items():
# If the variable is private ignore it
if n.startswith("_"):
continue
# If the variable has been set, skip it
if hasattr(component, n):
continue
# If the type is not actually a type, give a meaningful error
if not isinstance(inject_type, type):
raise TypeError(
"Component %s has a non-type annotation on %s (%r); lone non-injection variable annotations are disallowed, did you want to assign a static variable?"
% (cname, n, inject_type))
self._inject(n, inject_type, cname, component)
def _inject(self, n: str, inject_type: type, cname: str, component):
# Retrieve injectable object
injectable = self._injectables.get(n)
if injectable is None:
if cname is not None:
# Try for mangled names
injectable = self._injectables.get("%s_%s" % (cname, n))
# Raise error if injectable syntax used but no injectable was found.
if injectable is None:
raise MagicInjectError(
"Component %s has variable %s (type %s), which is not present in %s"
% (cname, n, inject_type, self))
# Raise error if injectable declared with type different than the initial type
if not isinstance(injectable, inject_type):
raise MagicInjectError(
"Component %s variable %s in %s are not the same types! (Got %s, expected %s)"
% (cname, n, self, type(injectable), inject_type))
# Perform injection
setattr(component, n, injectable)
self.logger.debug("-> %s as %s.%s", injectable, cname, n)
def _setup_reset_vars(self, component):
reset_dict = collect_resets(type(component))
if reset_dict:
component.__dict__.update(reset_dict)
self._reset_components.append((reset_dict, component))
def _update_feedback(self):
for method, entry in self._feedbacks:
try:
value = method()
except:
self.onException()
continue
entry.setValue(value)
self.watchdog.addEpoch("@magicbot.feedback")
def _execute_components(self):
for name, component in self._components:
try:
component.execute()
except:
self.onException()
self.watchdog.addEpoch(name)
for reset_dict, component in self._reset_components:
component.__dict__.update(reset_dict)
class MagicRobot(wpilib.SampleRobot,
metaclass=OrderedClass):
"""
Robots that use the MagicBot framework should use this as their
base robot class. If you use this as your base, you must
implement the following methods:
- :meth:`createObjects`
- :meth:`teleopPeriodic`
MagicRobot uses the :class:`.AutonomousModeSelector` to allow you
to define multiple autonomous modes and to select one of them via
the SmartDashboard/SFX.
MagicRobot will set the following NetworkTables variables
automatically:
- ``/robot/mode``: one of 'disabled', 'auto', 'teleop', or 'test'
- ``/robot/is_simulation``: True/False
- ``/robot/is_ds_attached``: True/False
"""
#: Amount of time each loop takes (default is 20ms)
control_loop_wait_time = 0.020
#: Error report interval: when an FMS is attached, how often should
#: uncaught exceptions be reported?
error_report_interval = 0.5
def robotInit(self):
"""
.. warning:: Internal API, don't override; use :meth:`createObjects` instead
"""
self._exclude_from_injection = [
'logger', 'members'
]
self.__last_error_report = -10
self._components = []
self._feedbacks = []
self._reset_components = []
# Create the user's objects and stuff here
self.createObjects()
# Load autonomous modes
self._automodes = AutonomousModeSelector('autonomous')
# Next, create the robot components and wire them together
self._create_components()
self.__nt = NetworkTables.getTable('/robot')
self.__nt.putBoolean('is_simulation', self.isSimulation())
self.__nt.putBoolean('is_ds_attached', self.ds.isDSAttached())
def createObjects(self):
"""
You should override this and initialize all of your wpilib
objects here (and not in your components, for example). This
serves two purposes:
- It puts all of your motor/sensor initialization in the same
place, so that if you need to change a port/pin number it
makes it really easy to find it. Additionally, if you want
to create a simplified robot program to test a specific
thing, it makes it really easy to copy/paste it elsewhere
- It allows you to use the magic injection mechanism to share
variables between components
.. note:: Do not access your magic components in this function,
as their instances have not been created yet. Do not
create them either.
"""
raise NotImplementedError
def teleopInit(self):
"""
Initialization code for teleop control code may go here.
Users may override this method for initialization code which will be
called each time the robot enters teleop mode.
.. note:: The ``on_enable`` functions of all components are called
before this function is called.
"""
pass
def teleopPeriodic(self):
"""
Periodic code for teleop mode should go here.
Users should override this method for code which will be called
periodically at a regular rate while the robot is in teleop mode.
This code executes before the ``execute`` functions of all
components are called.
"""
func = self.teleopPeriodic.__func__
if not hasattr(func, "firstRun"):
self.logger.warning("Default MagicRobot.teleopPeriodic() method... Overload me!")
func.firstRun = False
def disabledInit(self):
"""
Initialization code for disabled mode may go here.
Users may override this method for initialization code which will be
called each time the robot enters disabled mode.
.. note:: The ``on_disable`` functions of all components are called
before this function is called.
"""
pass
def disabledPeriodic(self):
"""
Periodic code for disabled mode should go here.
Users should override this method for code which will be called
periodically at a regular rate while the robot is in disabled mode.
This code executes before the ``execute`` functions of all
components are called.
"""
func = self.disabledPeriodic.__func__
if not hasattr(func, "firstRun"):
self.logger.warning("Default MagicRobot.disabledPeriodic() method... Overload me!")
func.firstRun = False
def robotPeriodic(self):
"""
Periodic code for all modes should go here.
` Users must override this method to utilize it
but it is not required.
This function gets called last in each mode.
You may use it for any code you need to run
during all modes of the robot (e.g NetworkTables updates)
"""
pass
def onException(self, forceReport=False):
'''
This function must *only* be called when an unexpected exception
has occurred that would otherwise crash the robot code. Use this
inside your :meth:`operatorActions` function.
If the FMS is attached (eg, during a real competition match),
this function will return without raising an error. However,
it will try to report one-off errors to the Driver Station so
that it will be recorded in the Driver Station Log Viewer.
Repeated errors may not get logged.
Example usage::
def teleopPeriodic(self):
try:
if self.joystick.getTrigger():
self.shooter.shoot()
except:
self.onException()
try:
if self.joystick.getRawButton(2):
self.ball_intake.run()
except:
self.onException()
# and so on...
:param forceReport: Always report the exception to the DS. Don't
set this to True
'''
# If the FMS is not attached, crash the robot program
if not self.ds.isFMSAttached():
raise
# Otherwise, if the FMS is attached then try to report the error via
# the driver station console. Maybe.
now = wpilib.Timer.getFPGATimestamp()
try:
if forceReport or (now - self.__last_error_report) > self.error_report_interval:
wpilib.DriverStation.reportError("Unexpected exception", True)
except:
pass # ok, can't do anything here
self.__last_error_report = now
@contextlib.contextmanager
def consumeExceptions(self, forceReport=False):
"""
This returns a context manager which will consume any uncaught
exceptions that might otherwise crash the robot.
Example usage::
def teleopPeriodic(self):
with self.consumeExceptions():
if self.joystick.getTrigger():
self.shooter.shoot()
with self.consumeExceptions():
if self.joystick.getRawButton(2):
self.ball_intake.run()
# and so on...
:param forceReport: Always report the exception to the DS. Don't
set this to True
.. seealso:: :meth:`onException` for more details
"""
try:
yield
except:
self.onException(forceReport=forceReport)
#
# Internal API
#
def autonomous(self):
"""
MagicRobot will do The Right Thing and automatically load all
autonomous mode routines defined in the autonomous folder.
.. warning:: Internal API, don't override
"""
self.__nt.putString('mode', 'auto')
self.__nt.putBoolean('is_ds_attached', self.ds.isDSAttached())
self._on_mode_enable_components()
self._automodes.run(self.control_loop_wait_time,
(self._execute_components, self._update_feedback, self.robotPeriodic),
self.onException)
self._on_mode_disable_components()
def disabled(self):
"""
This function is called in disabled mode. You should not
override this function; rather, you should override the
:meth:`disabledPeriodic` function instead.
.. warning:: Internal API, don't override
"""
self.__nt.putString('mode', 'disabled')
ds_attached = None
delay = PreciseDelay(self.control_loop_wait_time)
self._on_mode_disable_components()
try:
self.disabledInit()
except:
self.onException(forceReport=True)
while self.isDisabled():
if ds_attached != self.ds.isDSAttached():
ds_attached = not ds_attached
self.__nt.putBoolean('is_ds_attached', ds_attached)
try:
self.disabledPeriodic()
except:
self.onException()
self._update_feedback()
self.robotPeriodic()
delay.wait()
def operatorControl(self):
"""
This function is called in teleoperated mode. You should not
override this function; rather, you should override the
:meth:`teleopPeriodics` function instead.
.. warning:: Internal API, don't override
"""
self.__nt.putString('mode', 'teleop')
# don't need to update this during teleop -- presumably will switch
# modes when ds is no longer attached
self.__nt.putBoolean('is_ds_attached', self.ds.isDSAttached())
delay = PreciseDelay(self.control_loop_wait_time)
# initialize things
self._on_mode_enable_components()
try:
self.teleopInit()
except:
self.onException(forceReport=True)
while self.isOperatorControl() and self.isEnabled():
#self._update_autosend()
try:
self.teleopPeriodic()
except:
self.onException()
self._execute_components()
self._update_feedback()
self.robotPeriodic()
delay.wait()
self._on_mode_disable_components()
def test(self):
'''Called when the robot is in test mode'''
self.__nt.putString('mode', 'test')
self.__nt.putBoolean('is_ds_attached', self.ds.isDSAttached())
while self.isTest() and self.isEnabled():
wpilib.LiveWindow.run()
wpilib.Timer.delay(.01)
def _on_mode_enable_components(self):
# initialize things
for component in self._components:
if hasattr(component, 'on_enable'):
try:
component.on_enable()
except:
self.onException(forceReport=True)
def _on_mode_disable_components(self):
# deinitialize things
for component in self._components:
if hasattr(component, 'on_disable'):
try:
component.on_disable()
except:
self.onException(forceReport=True)
def _create_components(self):
#
# TODO: Will need to inject into any autonomous mode component
# too, as they're a bit different
#
# TODO: Will need to order state machine components before
# other components just in case
components = []
self.logger.info("Creating magic components")
# Identify all of the types, and create them
cls = self.__class__
# - Iterate over class variables with type annotations
for m, ctyp in get_class_annotations(cls).items():
# Ignore private variables
if m.startswith('_'):
continue
if hasattr(cls, m):
attr = getattr(cls, m)
# If the value given to the variable is an instance of a type and isn't a property
# raise an error. No double declaring types, e.g foo: type = type
if isinstance(attr, type) and not isinstance(attr, property):
raise ValueError("%s.%s has two type declarations" % (cls.__name__, m))
# Otherwise, skip this set class variable
continue
# If the variable has been assigned in __init__ or createObjects, skip it
if hasattr(self, m):
continue
# If the type is not actually a type, give a meaningful error
if not isinstance(ctyp, type):
raise TypeError('%s has a non-type annotation on %s (%r); lone non-injection variable annotations are disallowed, did you want to assign a static variable?'
% (cls.__name__, m, ctyp))
component = self._create_component(m, ctyp)
# Store for later
components.append((m, component))
# - Iterate over set class variables
for m in self.members:
if m.startswith('_') or isinstance(getattr(cls, m, None), _TunableProperty):
continue
ctyp = getattr(self, m)
if not isinstance(ctyp, type):
continue
component = self._create_component(m, ctyp)
# Store for later
components.append((m, component))
# Collect attributes of this robot that are injectable
self._injectables = {}
for n in dir(self):
if n.startswith('_') or n in self._exclude_from_injection or \
isinstance(getattr(cls, n, None), _TunableProperty):
continue
o = getattr(self, n)
# Don't inject methods
# TODO: This could actually be a cool capability..
if inspect.ismethod(o):
continue
self._injectables[n] = o
# For each new component, perform magic injection
for cname, component in components:
self._components.append(component)
setup_tunables(component, cname, 'components')
self._setup_vars(cname, component)
self._setup_reset_vars(component)
# Do it for autonomous modes too
for mode in self._automodes.modes.values():
mode.logger = logging.getLogger(mode.MODE_NAME)
setup_tunables(mode, mode.MODE_NAME, 'autonomous')
self._setup_vars(mode.MODE_NAME, mode)
# And for self too
setup_tunables(self, 'robot', None)
# Call setup functions for components
for cname, component in components:
if hasattr(component, 'setup'):
component.setup()
def _create_component(self, name, ctyp):
# Create instance, set it on self
component = ctyp()
setattr(self, name, component)
# Ensure that mandatory methods are there
if not callable(getattr(component, 'execute', None)):
raise ValueError("Component %s (%r) must have a method named 'execute'" % (name, component))
# Automatically inject a logger object
component.logger = logging.getLogger(name)
component._Magicbot__autosend = {}
self.logger.info("-> %s (class: %s)", name, ctyp.__name__)
return component
def _setup_vars(self, cname, component):
self.logger.debug("Injecting magic variables into %s", cname)
component_type = type(component)
# Iterate over variables with type annotations
for n, inject_type in get_class_annotations(component_type).items():
# If the variable is private ignore it
if n.startswith('_'):
continue
if hasattr(component_type, n):
attr = getattr(component_type, n)
# If the value given to the variable is an instance of a type and isn't a property
# raise an error. No double declaring types, e.g foo: type = type
if isinstance(attr, type) and not isinstance(attr, property):
raise ValueError("%s.%s has two type declarations" % (component_type.__name__, n))
# Otherwise, skip this set class variable
continue
# If the variable has been assigned in __init__, skip it
if hasattr(component, n):
continue
# If the type is not actually a type, give a meaningful error
if not isinstance(inject_type, type):
raise TypeError('Component %s has a non-type annotation on %s (%r); lone non-injection variable annotations are disallowed, did you want to assign a static variable?'
% (cname, n, inject_type))
self._inject(n, inject_type, cname, component)
# Iterate over static variables
for n in dir(component):
# If the variable is private or a proprty, don't inject
if n.startswith('_') or isinstance(getattr(component_type, n, True), property):
continue
inject_type = getattr(component, n)
# If the value assigned isn't a type, don't inject
if not isinstance(inject_type, type):
continue
self._inject(n, inject_type, cname, component)
for (name, method) in inspect.getmembers(component, predicate=inspect.ismethod):
if getattr(method, '__feedback__', False):
self._feedbacks.append((component, cname, name))
def _inject(self, n, inject_type, cname, component):
# Retrieve injectable object
injectable = self._injectables.get(n)
if injectable is None:
if cname is not None:
# Try for mangled names
injectable = self._injectables.get('%s_%s' % (cname, n))
# Raise error if injectable syntax used but no injectable was found.
if injectable is None:
raise ValueError("Component %s has variable %s (type %s), which is not present in %s" %
(cname, n, inject_type, self))
# Raise error if injectable declared with type different than the initial type
if not isinstance(injectable, inject_type):
raise ValueError("Component %s variable %s in %s are not the same types! (Got %s, expected %s)" %
(cname, n, self, type(injectable), inject_type))
# Perform injection
setattr(component, n, injectable)
self.logger.debug("-> %s as %s.%s", injectable, cname, n)
# XXX
#if is_autosend:
# where to store the nt key?
# component._Magicbot__autosend[prop.f] = None
def _setup_reset_vars(self, component):
reset_dict = {}
for n in dir(component):
if isinstance(getattr(type(component), n, True), property):
continue
a = getattr(component, n, None)
if isinstance(a, will_reset_to):
reset_dict[n] = a.default
if reset_dict:
component.__dict__.update(reset_dict)
self._reset_components.append((reset_dict, component))
#def _update_autosend(self):
# # seems like this should just be a giant list instead
# for component in self._components:
# d = component._Magicbot__autosend
# for f in d.keys():
# d[f] = f(component)
def _update_feedback(self):
for (component, cname, name) in self._feedbacks:
try:
func = getattr(component, name)
except:
continue
# Put ntvalue at /robot/components/component/key
self.__nt.putValue('/components/{0}/{1}'.format(cname, func.__key__), func())
def _execute_components(self):
for component in self._components:
try:
component.execute()
except:
self.onException()
for reset_dict, component in self._reset_components:
component.__dict__.update(reset_dict)
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
# joystick 1 on the driver station
self.stick = wpilib.XboxController(0)
self.driverStation = wpilib.DriverStation
self.frontRight = ctre.wpi_talonsrx.WPI_TalonSRX(3)
self.rearRight = ctre.wpi_talonsrx.WPI_TalonSRX(1)
self.right = wpilib.SpeedControllerGroup(self.frontRight,
self.rearRight)
self.frontLeft = ctre.wpi_talonsrx.WPI_TalonSRX(4)
self.rearLeft = ctre.wpi_talonsrx.WPI_TalonSRX(2)
self.left = wpilib.SpeedControllerGroup(self.frontLeft, self.rearLeft)
self.frontRight.setExpiration(0.2)
self.rearRight.setExpiration(0.2)
self.frontRight.setExpiration(0.2)
self.rearLeft.setExpiration(0.2)
self.drive = DifferentialDrive(self.left, self.right)
# initialize motors other than drive
self.intakeRight = wpilib.VictorSP(0)
self.elevator = ctre.wpi_talonsrx.WPI_TalonSRX(
5) # Talon SRX controller with CAN address 5
self.intakeLeft = wpilib.VictorSP(2)
self.battleAxe = wpilib.VictorSP(3)
self.actuator = wpilib.Spark(4)
self.axeExtender = wpilib.Spark(5)
######################################
self.encoderTicksPerInch = 1159
self.elevator.setQuadraturePosition(0, 0)
self.elevator.configForwardSoftLimitThreshold(
int(round(-0.1 * self.encoderTicksPerInch)), 10)
self.elevator.configReverseSoftLimitThreshold(
int(round(-39.5 * self.encoderTicksPerInch)), 10)
self.elevator.configForwardSoftLimitEnable(True, 10)
self.elevator.configReverseSoftLimitEnable(True, 10)
self.elevator.configPeakOutputForward(0.8, 10)
self.elevator.configPeakOutputReverse(-1, 10)
self.elevator.set(ctre.ControlMode.Position, 0)
self.elevator.selectProfileSlot(0, 0)
self.elevator.config_kF(0, 0, 10)
self.elevator.config_kP(0, 0.6, 10)
self.elevator.config_kI(0, 0.003, 10)
self.elevator.config_kD(0, 0, 10)
self.elevator.config_IntegralZone(0, 100, 10)
self.elevator.configAllowableClosedloopError(
0, int(round(0.01 * self.encoderTicksPerInch)), 10)
# initialize limit switches and hall-effect sensors
self.actuatorSwitchMin = wpilib.DigitalInput(0)
self.actuatorSwitchMax = wpilib.DigitalInput(1)
self.battleAxeSwitchUp = wpilib.DigitalInput(2)
self.battleAxeSwitchDown = wpilib.DigitalInput(3)
self.battleAxeExtenderSwitch = wpilib.DigitalInput(4)
self.elevatorZeroSensor = wpilib.DigitalInput(5)
self.powerDistributionPanel = wpilib.PowerDistributionPanel()
self.powerDistributionPanel.resetTotalEnergy()
#
# Communicate w/navX MXP via the MXP SPI Bus.
# - Alternatively, use the i2c bus.
# See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details
#
self.navxSensor = navx.AHRS.create_spi()
# self.navxSensor = navx.AHRS.create_i2c()
# Items in this dictionary are available in your autonomous mode
# as attributes on your autonomous object
self.components = {
'drive': self.drive,
'navxSensor': self.navxSensor,
'actuator': self.actuator,
'actuatorSwitchMin': self.actuatorSwitchMin,
'actuatorSwitchMax': self.actuatorSwitchMax,
'elevator': self.elevator,
'intakeRight': self.intakeRight,
'intakeLeft': self.intakeLeft,
'gameData': self.driverStation.getInstance()
}
# * The first argument is the name of the package that your autonomous
# modes are located in
# * The second argument is passed to each StatefulAutonomous when they
# start up
self.automodes = AutonomousModeSelector('autonomous', self.components)
NetworkTables.initialize()
self.sd = NetworkTables.getTable("SmartDashboard")
wpilib.CameraServer.launch('vision.py:main')
def autonomousInit(self):
"""This function is run once each time the robot enters autonomous mode."""
gameData = self.driverStation.getInstance().getGameSpecificMessage()
if gameData[0] == 'L':
self.sd.putString('gameData1', "Left")
elif gameData[0] == 'R':
self.sd.putString('gameData1', "Right")
if gameData[1] == 'L':
self.sd.putString('gameData2', "Left")
elif gameData[1] == 'R':
self.sd.putString('gameData2', "Right")
if gameData[2] == 'L':
self.sd.putString('gameData3', "Left")
elif gameData[2] == 'R':
self.sd.putString('gameData3', "Right")
def autonomousPeriodic(self):
"""This function is called periodically during autonomous."""
self.automodes.run()
def teleopInit(self):
wpilib.IterativeRobot.teleopInit(self)
self.setRamp = False
self.rampState = False
self.battleAxeUp = 0
self.battleAxeDown = 0
self.actuatorSpeedyIn = 0
self.actuatorSpeedyOut = 0.3
self.actuatorCount = 0
self.startClimb = False
self.climbMode = False
self.climbRobot = False
self.enableSequence1 = True
self.enableSequence2 = True
self.navxSensor.reset()
self.minPosition = 0
self.drivePosition = -11
self.climbPosition = -32
self.maxPosition = -39.5
self.positionSelector = 1
self.powerDistributionPanel.resetTotalEnergy()
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
leftXAxis = self.stick.getRawAxis(0) * 0.8
leftYAxis = self.stick.getRawAxis(1) * -0.8 # Get joystick value
# leftXAxis = 0
# leftYAxis = 0
# if leftYAxis >= 0.25 or leftYAxis <= -0.25:
# if leftYAxis <= -0.65:
# leftYAxis = -0.65
# self.setRamp = True
# else:
# self.setRamp = False
#
# if self.setRamp != self.rampState:
# if self.setRamp is True:
# self.frontRight.configOpenLoopRamp(1, 0)
# self.rearRight.configOpenLoopRamp(1, 0)
# self.frontRight.configOpenLoopRamp(1, 0)
# self.rearLeft.configOpenLoopRamp(1, 0)
# self.rampState = True
# else:
# self.frontRight.configOpenLoopRamp(0, 0)
# self.rearRight.configOpenLoopRamp(0, 0)
# self.frontRight.configOpenLoopRamp(0, 0)
# self.rearLeft.configOpenLoopRamp(0, 0)
# self.rampState = False
self.frontRight.configOpenLoopRamp(0, 0)
self.rearRight.configOpenLoopRamp(0, 0)
self.frontRight.configOpenLoopRamp(0, 0)
self.rearLeft.configOpenLoopRamp(0, 0)
self.drive.arcadeDrive(leftYAxis, leftXAxis, squaredInputs=True)
self.sdUpdate()
rightYAxis = self.stick.getRawAxis(5)
rightYAxis = self.normalize(rightYAxis, 0.15) # Set deadzone
if -0.15 < self.stick.getRawAxis(5) < 0.15 and self.climbMode is False:
if self.elevator.getQuadraturePosition() < (
self.drivePosition + 1) * self.encoderTicksPerInch:
self.enableSequence1 = False
self.positionSelector = 2
elif -0.15 < self.stick.getRawAxis(
5) < 0.15 and self.climbMode is True:
self.positionSelector = 3
self.battleAxeUp = self.stick.getRawAxis(2) * -0.35
self.battleAxeDown = self.stick.getRawAxis(3) * 0.50
if self.battleAxeSwitchUp.get() is True:
self.battleAxeUp = 0
if self.battleAxeSwitchDown.get() is True:
self.battleAxeDown = 0
self.battleAxe.set(self.battleAxeUp + self.battleAxeDown)
else:
self.positionSelector = 0
if self.stick.getRawAxis(2) > 0.1 and self.climbMode is False:
self.positionSelector = 5
self.elevator.set(ctre.ControlMode.PercentOutput,
self.stick.getRawAxis(2))
self.intakeRight.set(-1)
self.intakeLeft.set(-1)
else:
self.intakeRight.set(0)
self.intakeLeft.set(0)
if self.stick.getRawAxis(3) > 0.1 and self.climbMode is False:
self.intakeRight.set(self.stick.getRawAxis(3))
self.intakeLeft.set(self.stick.getRawAxis(3))
if self.climbRobot is True:
self.positionSelector = 1
if self.elevator.getQuadraturePosition(
) > -2 * self.encoderTicksPerInch:
self.elevator.configPeakOutputForward(0.2, 10)
self.elevator.configPeakOutputReverse(-0.5, 10)
elif self.elevator.getQuadraturePosition(
) < -37.5 * self.encoderTicksPerInch:
self.elevator.configPeakOutputForward(0.8, 10)
self.elevator.configPeakOutputReverse(-0.5, 10)
else:
self.elevator.configPeakOutputForward(0.8, 10)
self.elevator.configPeakOutputReverse(-1, 10)
if self.positionSelector is 0:
self.elevator.set(rightYAxis)
elif self.positionSelector is 1:
self.elevator.set(
ctre.ControlMode.Position,
int(round(self.minPosition * self.encoderTicksPerInch)))
elif self.positionSelector is 2:
self.elevator.set(
ctre.ControlMode.Position,
int(round(self.drivePosition * self.encoderTicksPerInch)))
elif self.positionSelector is 3:
self.elevator.set(
ctre.ControlMode.Position,
int(round(self.climbPosition * self.encoderTicksPerInch)))
elif self.positionSelector is 4:
self.elevator.set(
ctre.ControlMode.Position,
int(round(self.maxPosition * self.encoderTicksPerInch)))
else:
pass
if self.elevatorZeroSensor.get() is False:
self.elevator.setQuadraturePosition(0, 0)
if self.stick.getRawButton(1) is True and self.stick.getRawButton(
7) is True: # A and start button
self.startClimb = True
if self.stick.getRawButton(3) is True: # X button
if self.actuatorCount < 20 and self.elevator.getQuadraturePosition() < \
(self.climbPosition + 1) * self.encoderTicksPerInch:
self.actuatorSpeedyIn = -0.5
self.actuatorSpeedyOut = 0
else:
self.actuatorSpeedyIn = 0
self.actuatorSpeedyOut = 0
self.actuatorCount += 1
elif self.stick.getRawButton(
3) is False and self.actuatorCount is not 0:
self.actuatorSpeedyIn = 0
self.actuatorSpeedyOut = 0.3
self.actuatorCount = 0
if self.battleAxeSwitchUp.get() is True:
self.battleAxeUp = 0
if self.startClimb is True:
self.actuatorSpeedyIn = -0.45
if self.actuatorSwitchMin.get() is False:
self.actuatorSpeedyIn = 0
self.climbMode = True
if self.climbMode is False and self.enableSequence1 is False and self.enableSequence2 is False and\
self.startClimb is False:
self.battleAxeUp = -0.3
if self.actuatorSwitchMax.get() is False:
self.actuatorSpeedyOut = 0
if self.climbMode is False:
self.battleAxeDown = 0.2
self.actuator.set(self.actuatorSpeedyIn + self.actuatorSpeedyOut)
if self.stick.getRawButton(2) is True and self.stick.getRawButton(
7) is True and self.climbMode is True:
self.climbRobot = True
if self.startClimb is True:
self.battleAxeUp = -0.35
self.battleAxeDown = 0
if self.battleAxeSwitchDown.get() is True:
self.battleAxeDown = 0
if self.climbMode is False:
self.battleAxe.set(self.battleAxeUp + self.battleAxeDown)
if self.stick.getRawButton(6) is True:
axeOut = 1
axeIn = 0
elif self.stick.getRawButton(5) is True:
axeOut = 0
axeIn = -1
else:
axeOut = 0
axeIn = 0
# if self.enableSequence2 is True:
# axeIn = -1
# if self.battleAxeExtenderSwitch.get() is False:
# axeIn = 0
# self.enableSequence2 = False
self.axeExtender.set(axeOut + axeIn)
def testInit(self):
self.actuatorIn = 0
self.actuatorOut = 0
def testPeriodic(self):
"""This function is called periodically during test mode."""
self.actuatorIn = (self.stick.getRawAxis(2) * -0.5)
self.actuatorOut = (self.stick.getRawAxis(3) * 0.5)
# if self.actuatorSwitchMin.get() is False:
# self.actuatorIn = 0
# if self.actuatorSwitchMax.get() is False:
# self.actuatorOut = 0
self.axeExtender.set(self.actuatorIn + self.actuatorOut)
# self.battleAxe.set((self.stick.getRawAxis(2) * -0.5) + (self.stick.getRawAxis(3) * 0.5))
self.sdUpdate()
def normalize(self, joystickInput, deadzone):
"""joystickInput should be between -1 and 1, deadzone should be between 0 and 1."""
if joystickInput > 0:
if (joystickInput - deadzone) < 0:
return 0
else:
return (joystickInput - deadzone) / (1 - deadzone)
elif joystickInput < 0:
if (joystickInput + deadzone) > 0:
return 0
else:
return (joystickInput + deadzone) / (1 - deadzone)
else:
return 0
def sdUpdate(self):
self.sd.putNumber('robot/time', wpilib.Timer.getMatchTime())
self.sd.putNumber('drive/navx/yaw', self.navxSensor.getYaw())
self.sd.putNumber('robot/elevator/encoder',
self.elevator.getQuadraturePosition())
self.sd.putNumber('robot/elevator/motorPercent',
self.elevator.getMotorOutputPercent())
self.sd.putNumber('robot/totalCurrent',
self.powerDistributionPanel.getTotalCurrent())
self.sd.putNumber('robot/totalEnergy',
self.powerDistributionPanel.getTotalEnergy())
self.sd.putNumber('robot/totalPower',
self.powerDistributionPanel.getTotalPower())
if wpilib.DriverStation.getInstance().getAlliance(
) is wpilib.DriverStation.Alliance.Red:
theme = "red"
self.sd.putString('theme', theme)
elif wpilib.DriverStation.getInstance().getAlliance(
) is wpilib.DriverStation.Alliance.Blue:
theme = "blue"
self.sd.putString('theme', theme)
self.sd.putBoolean('example_variable', self.battleAxeSwitchUp.get())
class robot(wpilib.IterativeRobot):
def robotInit(self):
#NetworkTables Init
NetworkTables.initialize()
self.table = NetworkTables.getTable("SmartDashboard")
#Navx
self.navx = navx.AHRS.create_spi()
#PowerDistributionPanel
self.power_board = wpilib.PowerDistributionPanel()
#Motors
self.leftMotor1 = wpilib.Spark(
0
) # <-- This is what links our PWM port on the CRIO to a physical ESC.
self.leftMotor2 = wpilib.Spark(
1
) # <-- This is what links our PWM port on the CRIO to a physical ESC.
self.left = wpilib.SpeedControllerGroup(self.leftMotor1,
self.leftMotor2)
self.rightMotor1 = wpilib.Spark(2)
self.rightMotor2 = wpilib.Spark(3)
self.right = wpilib.SpeedControllerGroup(self.rightMotor1,
self.rightMotor2)
self.liftMotor1 = wpilib.Talon(4)
self.liftMotor2 = wpilib.Talon(5)
self.liftMotor2.setInverted(True)
self.lift = wpilib.SpeedControllerGroup(self.liftMotor1,
self.liftMotor2)
#User Input
self.playerOne = wpilib.XboxController(
0) # <-- This is for using Xbox controllers
#Drive
self.robotDrive = wpilib.drive.DifferentialDrive(self.left, self.right)
#Drive.py init
self.drive = drive.Drive(self.robotDrive, self.navx, self.left,
self.right)
self.components = {'drive': self.drive, 'table': self.table}
self.automodes = AutonomousModeSelector('autonomous', self.components)
def disabledInit(self):
self.table.putNumber('ctrlY', 0)
self.table.putNumber('ctrlX', 0)
def autonomousInit(self):
pass
def autonomousPeriodic(self):
self.table.putNumber('ctrlY', self.left.get())
self.table.putNumber('ctrlX', self.right.get())
self.automodes.run()
def teleopInit(self):
pass
def teleopPeriodic(self):
#NetworkTables Variables
self.table.putNumber('ctrlY', self.left.get())
self.table.putNumber('ctrlX', -1 * self.right.get())
#lift
self.lift.set(self.playerOne.getYButton() +
self.playerOne.getAButton() * -1)
#Shoot
#self.playerTwo.getTriggerAxis(1) + self.playerTwo.getTriggerAxis(0) * -1
#Drive
self.drive.masterDrive(self.playerOne.getY(0), self.playerOne.getY(1))
