def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
# Comments begin with a "#". I will be making liberal use of comments.
#
# These initializations won't do anything on our robot at the moment, but won't
# hinder a simulation.
# Sparks are motor controllers. The Spark class in the library contains code to talk to them.
# "self" refers to the object being defined by this class, or in other words our robot.
# This is saying that the variable "left_motor" is part of the class's definition and
# is accessible from any function in the class.
self.left_motor = wpilib.Spark(
0
) # Create an object of the Spark class on PWM channel 0 and call it left_motor.
self.right_motor = wpilib.Spark(
1) # Create another object of the Spark class.
# An object of the class DifferentialDrive helps us to control a drive train with all left wheels connected
# and all right wheels connected, similar to a Bobcat skid-steer or a bulldozer.
self.drive = wpilib.drive.DifferentialDrive(self.left_motor,
self.right_motor)
self.stick = wpilib.Joystick(
1
) # Joystick is a class that talks to the joysticks connected to the driver station computer.
# Timer to control periodic logging of messages.
self.print_timer = wpilib.Timer()
self.print_timer.start()
# Another timer, this one to help us measure time during play modes.
self.play_timer = wpilib.Timer()
self.play_timer.start()
def robotInit(self):
self.driveMethods = MethodsRobot.Drive()
self.shooterMethods = MethodsRobot.Shooter()
self.controllerMethods = MethodsRobot.Controller()
self.controllerMotor = ctre.WPI_TalonSRX(
ports.talonPorts.get("controllerMotor"))
self.timer = wpi.Timer()
self.resetStates()
self.driverController = wpi.XboxController(
ports.controllerPorts.get("driverController"))
self.codriverController = wpi.XboxController(
ports.controllerPorts.get("codriverController"))
wpi.CameraServer.launch()
#wpi.CameraServer.
self.printTimer = wpi.Timer()
self.printTimer.reset()
self.printTimer.start()
self.table = NetworkTables.getTable("limelight")
self.tx = self.table.getNumber('tx', None)
self.ty = self.table.getNumber('ty', None)
self.ta = self.table.getNumber('ta', None)
self.ts = self.table.getNumber('ts', None)
def __init__(self, talon_id, follower_id):
"""
Create a new instance of the claw subsystem.
This function constructs and configures the CANTalon instance and the
touch sensor. It also sets the state machine to the neutral starting
state.
"""
self.talon = TalonSRX(talon_id)
self.follower = TalonSRX(follower_id)
self.follower.set(
TalonSRX.ControlMode.Follower,
talon_id
)
self.talon.setInverted(True)
self.follower.setInverted(False)
# Forward limit switch = claw opening limit switch
# self.talon.configForwardLimitSwitchSource(
# TalonSRX.LimitSwitchSource.FeedbackConnector,
# TalonSRX.LimitSwitchNormal.NormallyOpen,
# 0
# )
self.state = 'neutral'
self.closeAdjustTimer = wpilib.Timer()
self.movementTimer = wpilib.Timer()
def OperatorControl(self):
dog = self.GetWatchdog()
dog.SetEnabled(True)
dog.SetExpiration(0.25)
shiftTime = wpilib.Timer()
haveTime = wpilib.Timer()
shiftTime.Start()
haveTime.Start()
intakeVelocity = -0.75
while self.IsOperatorControl() and self.IsEnabled():
dog.Feed()
CheckRestart()
if not HaveBall():
haveTime.Reset()
# Reset pneumatics
if shiftTime.Get() > 0.3:
shifter1.Set(False)
shifter2.Set(False)
# Kicker control
self.kicker.OperatorControl()
# Drive control
drive.TankDrive(lstick, rstick)
# Shifter control
if rstick.GetTrigger():
shifter1.Set(True)
shifter2.Set(False)
shiftTime.Reset()
elif lstick.GetTrigger():
shifter1.Set(False)
shifter2.Set(True)
shiftTime.Reset()
# Intake motor
if stick3.GetRawButton(6):
intakeVelocity = 1.0
elif stick3.GetRawButton(7):
intakeVelocity = -0.75
elif stick3.GetRawButton(8):
intakeVelocity = 0.0
#elif rstick.GetRawButton(11):
# intakeVelocity = 0.75
# The motors are stopped when kicking; start them up again after
# timeout
if self.kicker.kickTime.Get() > 0.1:
if (haveTime.Get() > 2.0 and intakeVelocity < 0.0
and lstick.GetY() <= 0.0 and rstick.GetY() <= 0.0):
intakeMotor.Set(-0.35)
else:
intakeMotor.Set(intakeVelocity)
wpilib.Wait(0.01)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.driver_stick = wpilib.Joystick(0)
self.codriver_stick = wpilib.Joystick(1)
# Drivetrain Motors
self.left_drivetrain_motor = ctre.WPI_TalonSRX(4)
self.left_drivetrain_motor_2 = ctre.WPI_TalonSRX(3)
self.right_drivetrain_motor = ctre.WPI_TalonSRX(6)
self.right_drivetrain_motor_2 = ctre.WPI_TalonSRX(5)
#Encoders
self.left_drivetrain_encoder = self.left_drivetrain_motor.getSelectedSensorPosition()
self.right_drivetrain_encoder = self.right_drivetrain_motor.getSelectedSensorPosition()
# Box-mechanism Motors
self.intake_motor = wpilib.Spark(8)
self.box_lift_motor = ctre.WPI_TalonSRX(2)
# Lift mechanism Motors
self.elevator_motor = wpilib.Spark(9)
self.main_lift = rev.CANSparkMax(7, rev.MotorType.kBrushless)
#ControlPanel objects
self.arm = ctre.WPI_TalonSRX(1)
self.arm.configSelectedFeedbackSensor(ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)
self.arm.config_kP(0, (0.3 * 1023) / 4161 , 0)
self.arm.config_kI(0, (0.5 * 1023) / (4161*30), 0)
self.arm.config_kD(0, 30*(0.5 * 1023) / 4161, 0)
self.arm.config_kF(0, 0, 0)
self.timer = wpilib.Timer()
self.duration = 20
self.drivetrain = DriveTrain(self.left_drivetrain_motor, self.left_drivetrain_motor_2, self.right_drivetrain_motor, self.right_drivetrain_motor_2)
self.box = Box(self.intake_motor,self.box_lift_motor)
self.robotlift = RLift(self.elevator_motor, self.main_lift)
self.Panel = Arm(self.arm)
wpilib.SmartDashboard.putString(keyName="Robot_Station", value= "1")
wpilib.CameraServer.launch()
self.auto_timer = wpilib.Timer()
self.navx = navx.AHRS.create_spi()
self.auto = 0
def __init__(self, components):
'''Assume that any components needed will be passed in as a parameter. Store them so you can use them'''
self.drive = components['drive']
self.intake = components['intake']
self.catapult = components['catapult']
self.timer = wpilib.Timer()
self.timer2 = wpilib.Timer()
self.goalHot = 0 #-1,not active,1 active, 0 maybe
wpilib.SmartDashboard.PutNumber("position", 0)
wpilib.SmartDashboard.PutNumber("Goal Hot", 0)
def __init__(self):
self.armed = False
self.retraction = 0
self.winchTarget = 0
self.kickTime = wpilib.Timer()
self.pneumaticTime = wpilib.Timer()
self.kickTime.Start()
self.pneumaticTime.Start()
self.prevFrontLimitSwitch = frontLimitSwitch.Get()
self.raw10Previous = False
self.raw11Previous = False
def __init__(self, robot):
#Motor Setup
self.RakeMotor = ctre.WPI_TalonSRX(7)
self.ConveyorMotor1 = ctre.WPI_TalonSRX(1)
self.ConveyorMotor2 = ctre.WPI_TalonSRX(6)
self.Flywheel = ctre.WPI_TalonSRX(10)
#Timer Setup
self.ShootTimer = wpilib.Timer()
self.PrintTimer = wpilib.Timer()
self.PrintTimer.start()
self.RakeTimer = wpilib.Timer()
self.IndexTimer = wpilib.Timer()
#Encoder Setup
#Fix these
#self.Conveyor1Encoder = wpilib.Encoder(6, 7, False, wpilib.Encoder.EncodingType.k1X)
#self.Conveyor2Encoder = wpilib.Encoder(8, 9, False, wpilib.Encoder.EncodingType.k1X)
#self.FlywheelEncoder = wpilib.Encoder(10, 11, False, wpilib.Encoder.EncodingType.k1X)
#self.ConveyorMotor1.get
#Misc Setup:
self.IndexSensor1 = wpilib.DigitalInput(5)
self.IndexSensor2 = wpilib.DigitalInput(6)
self.IndexSensor3 = wpilib.DigitalInput(7)
#self.BumpSwitch = wpilib.DigitalInput(1)
self.RakeRelease1 = wpilib.PWM(0)
self.RakeRelease2 = wpilib.PWM(1)
self.RakeDropping = True
self.AutoShoot = False
self.AutoShootLow = False
self.TimerBegin = False
self.TimerRunning = False
self.IndexRunning = False
self.WaitTime = .5
self.ConveyorIndex = 1536 #1.5 rotations * 1024 encoder counts, Placeholder
#---------------------------------------------------------------------------------
#PID loop for velocity
#keeps the flywheel going at a constant RPM at all times
#Corrects for errors
self.Flywheel.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder,0,0)
self.Flywheel.config_kF(0, 0.01, 0)
self.Flywheel.config_kP(0, 0.01, 0)
self.Flywheel.config_kI(0, 0.00001, 0)
self.Flywheel.config_kD(0, 0.0, 0)
self.fleshWound = wpilib.Timer()
self.fleshWound.start()
self.Errorzone = 0
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.driver_stick = wpilib.Joystick(0)
self.codriver_stick = wpilib.Joystick(1)
self.left_shooter_motor = ctre.WPI_TalonSRX(0)
self.left_drivetrain_motor = ctre.WPI_TalonSRX(1)
self.right_drivetrain_motor = ctre.WPI_TalonSRX(2)
self.right_shooter_motor = ctre.WPI_TalonSRX(3)
self.intake_motor = wpilib.Spark(8)
self.arm_pivot_motor = wpilib.Spark(6)
self.arm_lock_motor = wpilib.Spark(5)
self.timer = wpilib.Timer()
#self.navx = navx.AHRS.create_spi()
self.duration = 20
self.drivetrain = DriveTrain(self.left_drivetrain_motor,
self.right_drivetrain_motor)
self.shooter = Shooter(self.intake_motor, self.left_shooter_motor,
self.right_shooter_motor)
self.arm = Arm(self.arm_pivot_motor, self.arm_lock_motor)
def __init__(self, winch, activateSolenoid, passSolenoid, potentiometer,
analog_channel, timer):
'''initialize'''
self.ballSensor = analog_channel
self.passSolenoid = passSolenoid
self.arrayOfMotorValues = [0] * 119
self.sendArray = False
self.potentiometer = potentiometer
self.winch = winch
self.activateSolenoid = activateSolenoid
self.timer = timer
self.i = 0
#wpilib.SmartDashboard.PutValue('CatapultValues', self.arrayOfMotorValues)
# timer is always running, but we reset it before using it so we're
# guaranteed that the time is zero when we use it
self.launchTimer = wpilib.Timer()
self.launchTimer.Start()
self.cState = NOTHING
self.do_autowinch = False
# 100 is max, 0 is min
self.winchLocation = 100
def __init__(self, elevatorEncoder, elevatorLimitS, jawsLimitS,
metaboxLimitS, jawsM, elevatorM, intakeM, jawsSol):
self.elevatorEncoder = elevatorEncoder
self.elevatorLimit = elevatorLimitS
self.jawsLimitS = jawsLimitS
self.metaboxLimitS = metaboxLimitS
self.jawsM = jawsM
self.elevatorM = elevatorM
self.intakeM = intakeM
self.jawsSol = jawsSol
self.elevatorTravel = 26.4
self.isCalibrated = False
self.sd = NetworkTables.getTable('SmartDashboard')
self.pidDefault = {'p': 0.8, 'i': 0.2, 'd': 0.5}
self.pid = wpilib.PIDController(self.pidDefault['p'],
self.pidDefault['i'],
self.pidDefault['d'],
lambda: self.getEncoder(),
self.setElevator)
self.pid.setAbsoluteTolerance(0.1)
self.sd.putNumber('elevatorP', self.pidDefault['p'])
self.sd.putNumber('elevatorI', self.pidDefault['i'])
self.sd.putNumber('elevatorD', self.pidDefault['d'])
self.timer = wpilib.Timer()
self.autoActionStarted = False
def robotInit(self):
self.driveMotor = rev.CANSparkMax(1, rev.MotorType.kBrushless)
self.turnMotor = rev.CANSparkMax(2, rev.MotorType.kBrushless)
#self.turnEncoder = wpilib.AnalogInput(0)
self.turnEncoder = self.turnMotor.getEncoder()
self.turnEncoder.setPositionConversionFactor(20)
# PID coefficients
self.kP = 5e-5
self.kI = 0
self.kD = 0
self.PIDTolerance = 0
#PID controllers for the turn motors
self.turnController = wpilib.controller.PIDController(
self.kP, self.kI, self.kD)
self.turnController.setTolerance(self.PIDTolerance)
self.turnController.enableContinuousInput(0, 360)
self.joystick = wpilib.Joystick(0)
self.joystickDeadband = .05
self.timer = wpilib.Timer(
) #used to use it while testing stuff, don't need it now, but oh well
# Push PID Coefficients to SmartDashboard
wpilib.SmartDashboard.putNumber("P Gain", self.kP)
wpilib.SmartDashboard.putNumber("I Gain", self.kI)
wpilib.SmartDashboard.putNumber("D Gain", self.kD)
wpilib.SmartDashboard.putNumber("Set Rotations", 0)
wpilib.SmartDashboard.putNumber("Tolerance", self.PIDTolerance)
wpilib.SmartDashboard.putBoolean("Manual Control", True)
wpilib.SmartDashboard.putNumber("Manual Speed", 0)
def robotInit(self):
self.pegChooser = wpilib.SendableChooser()
self.pegChooser.addObject('Left', 'left')
self.pegChooser.addObject('Right', 'right')
self.pegChooser.addObject('Middle', 'middle')
wpilib.SmartDashboard.putData('ChooseYourPeg', self.pegChooser)
#test = sd.getNumber("someNumber")
#self.logger.info("Test = " + str(test))
self.logger.info("Robot Starting up...")
self.logger.info("Camera started")
self.mctype = wpilib.Spark
self.logger.info("Defined motor controller type")
self.cam_horizontal = wpilib.Servo(6)
self.cam_vertical = wpilib.Servo(7)
self.cam_vertical_value = 0.2
self.cam_horizontal_value = 0.5
self.logger.info("Defined Camera Servos and Respective Values")
self.cam_horizontal.set(self.cam_horizontal_value)
self.cam_vertical.set(self.cam_vertical_value)
self.logger.info("Set Camera Servos to Halfway")
self.pdp = wpilib.PowerDistributionPanel()
self.logger.info("defined PowerDistributionPanel")
self.left = self.mctype(LEFT_MOTOR)
self.right = self.mctype(RIGHT_MOTOR)
self.winchMotor = self.mctype(WINCH_MOTOR)
self.logger.info("Defined FL, BL, FR, BR motors")
self.controls = controlstemplate.Controls(
wpilib.Joystick(JOYSTICK_PORT), self.isTest)
self.logger.info("Defined Control scheme")
self.timer = wpilib.Timer()
self.logger.info("Defined Timer")
self.logger.info("Robot On")
def __init__(self, my_robot, time, rpm):
super().__init__(my_robot)
self.shooter = my_robot.shooter
self.timer = wpilib.Timer()
self.time = time
self.rpm = rpm
self.fueltank = my_robot.fueltank
def init(self):
self.logger.info("DeepSpaceDrive::init()")
self.timer = wpilib.Timer()
self.timer.start()
self.current_state = DriveState.OPERATOR_CONTROL
self.pigeon = PigeonIMU(robotmap.PIGEON_IMU_CAN_ID)
self.leftTalonMaster = ctre.WPI_TalonSRX(robotmap.DRIVE_LEFT_MASTER_CAN_ID)
self.leftTalonSlave = ctre.WPI_TalonSRX(robotmap.DRIVE_LEFT_SLAVE_CAN_ID)
self.rightTalonMaster = ctre.WPI_TalonSRX(robotmap.DRIVE_RIGHT_MASTER_CAN_ID)
self.rightTalonSlave = ctre.WPI_TalonSRX(robotmap.DRIVE_RIGHT_SLAVE_CAN_ID)
self.dummyTalon = ctre.TalonSRX(robotmap.CLAW_LEFT_WHEELS_CAN_ID)
self.talons = [self.leftTalonMaster, self.leftTalonSlave, self.rightTalonMaster, self.rightTalonSlave]
self.masterTalons = [self.leftTalonMaster, self.rightTalonMaster]
self.drive = wpilib.RobotDrive(self.leftTalonMaster, self.rightTalonMaster)
self.drive.setSafetyEnabled(False)
self.drive_front_extend = wpilib.Solenoid(robotmap.PCM1_CANID, robotmap.DRIVE_FRONT_EXTEND_SOLENOID)
self.drive_front_retract = wpilib.Solenoid(robotmap.PCM1_CANID, robotmap.DRIVE_FRONT_RETRACT_SOLENOID)
self.drive_back_extend = wpilib.Solenoid(robotmap.PCM1_CANID, robotmap.DRIVE_REAR_EXTEND_SOLENOID)
self.drive_back_retract = wpilib.Solenoid(robotmap.PCM1_CANID, robotmap.DRIVE_REAR_RETRACT_SOLENOID)
def __init__(self):
# initialize wpilib stuff
self.sd = wpilib.SmartDashboard
self.timer = wpilib.Timer()
# initialize motors
self.motorLeftFront = robot_map.motorLeftFront
self.motorLeftMiddle = robot_map.motorLeftMiddle
self.motorLeftBack = robot_map.motorLeftBack
self.leftSideMotors = wpilib.SpeedControllerGroup(
robot_map.motorLeftFront, robot_map.motorLeftMiddle,
robot_map.motorLeftBack)
self.motorRightFront = robot_map.motorRightFront
self.motorRightMiddle = robot_map.motorRightMiddle
self.motorRightBack = robot_map.motorRightBack
self.rightSideMotors = wpilib.SpeedControllerGroup(
robot_map.motorRightFront, robot_map.motorRightMiddle,
robot_map.motorRightBack)
# initialize drive sticks
self.leftStick = robot_map.xboxControllerLeftStickY
self.rightStick = robot_map.xboxControllerRightStickY
# initialize gyro
self.navx = navx.AHRS.create_spi()
self.analog = wpilib.AnalogInput(
navx.getNavxAnalogInChannel(robot_map.gyro))
def __init__(self,
robot,
holdpos=config.articulateAngleHigh,
power=-.4,
pastPower=-.6,
delay=0,
dist=100):
super().__init__()
subsystems = robot.subsystems
self.requires(subsystems['drive'])
self.requires(subsystems['shooter'])
self.requires(subsystems['tomahawk'])
self.driveSubsystem = subsystems['drive']
self.shooterSubsystem = subsystems['shooter']
self.tomahawkSubsystem = subsystems['tomahawk']
self.drivePower = power
self.pastPower = pastPower
self.dist = dist
self.holdpos = holdpos
self.delay = delay
self.delayCount = 0
self.timer = wpilib.Timer()
self.timerStarted = False
self.currentState = self.State.NOT_HIT_PLATFORM
def __init__(self, intakeMotor, time, speed=.8):
self.time = time
self.speed = speed
self.intakeMotor = intakeMotor
self.tm = wpilib.Timer()
self.tm.start()
self.firstTime = True
def autonomousInit(self):
self.gameData = ""
self.autonTimer = wpilib.Timer()
self.shooterTimer = wpilib.Timer()
self.autonTimer.start()
self.Aimer.reset()
#self.Aimer.setaim(self.Aimer.getAngle())
self.turned180 = False
self.setTarget = False
#
self.Aimer.setaim(self.Aimer.getAngle())
def teleop():
dog = wpilib.GetWatchdog()
dog.SetEnabled(True)
dog.SetExpiration(0.25)
shiftTime = wpilib.Timer()
shiftTime.Start()
while wpilib.IsOperatorControl() and wpilib.IsEnabled():
dog.Feed()
checkRestart()
if shiftTime.Get() > 0.3:
shifter1.Set(False)
shifter2.Set(False)
# Shifter control
if rstick.GetTrigger():
shifter1.Set(True)
shifter2.Set(False)
shiftTime.Reset()
highGear = True
elif lstick.GetTrigger():
shifter1.Set(False)
shifter2.Set(True)
shiftTime.Reset()
highGear = False
# Drive control
drive.TankDrive(lstick, rstick)
wpilib.Wait(0.04)
def operatorControl(self):
'''Called when operation control mode is enabled'''
timer = wpilib.Timer()
timer.start()
while self.isOperatorControl() and self.isEnabled():
if timer.hasPeriodPassed(1.0):
print("Gyro:", self.gyro.getAngle())
# This is where the data ends up..
#from hal_impl.data import hal_data
#print(hal_data['robot'])
self.robot_drive.arcadeDrive(self.lstick)
# Move a motor with a Joystick
y = self.rstick.getY()
# stop movement backwards when 1 is on
if self.limit1.get():
y = max(0, y)
# stop movement forwards when 2 is on
if self.limit2.get():
y = min(0, y)
self.motor.set(y)
wpilib.Timer.delay(0.04)
def createObjects(self):
with open("../ports.json" if os.getcwd()[-5:-1] == "test" else "ports.json") as f:
self.ports = json.load(f)
with open("../buttons.json" if os.getcwd()[-5:-1] == "test" else "buttons.json") as f:
self.buttons = json.load(f)
# Arm
arm_ports = self.ports["arm"]
self.arm_left = wpilib.Victor(arm_ports["arm_left"])
self.arm_right = ctre.WPI_TalonSRX(arm_ports["arm_right"])
self.wrist = ctre.WPI_TalonSRX(arm_ports["wrist"])
self.intake = wpilib.Spark(arm_ports["intake"])
self.hatch = wpilib.DoubleSolenoid(arm_ports["hatch_in"], arm_ports["hatch_out"])
# DriveTrain
drive_ports = self.ports["drive"]
self.front_left = wpilib.Victor(drive_ports["front_left"])
self.front_right = wpilib.Victor(drive_ports["front_right"])
self.back_left = wpilib.Victor(drive_ports["back_left"])
self.back_right = wpilib.Victor(drive_ports["back_right"])
self.joystick = wpilib.Joystick(0)
self.print_timer = wpilib.Timer()
self.print_timer.start()
self.logger = logging.getLogger("Robot")
self.test_tab = Shuffleboard.getTab("Test")
wpilib.CameraServer.launch()
def robotInit(self):
"""Robot-wide initialization code should go here."""
self.lstick = wpilib.Joystick(0)
self.motor = wpilib.Talon(3)
self.timer = wpilib.Timer()
self.loops = 0
def __init__(self, drive, gyro, encoderY):
self.drive = drive
self.gyro = gyro
self.encoderY = encoderY
self.jDeadband = 0.06
self.sd = NetworkTables.getTable('SmartDashboard')
# PID loop for angle
self.pidAngleDefault = {'p': 0.01, 'i': 0, 'd': 0.004}
self.sd.putNumber('pidAngleP', self.pidAngleDefault['p'])
self.sd.putNumber('pidAngleI', self.pidAngleDefault['i'])
self.sd.putNumber('pidAngleD', self.pidAngleDefault['d'])
self.pidAngle = wpilib.PIDController(self.pidAngleDefault['p'], self.pidAngleDefault['i'], self.pidAngleDefault['d'], self.gyro, self.updateAnglePID)
self.pidAngle.setAbsoluteTolerance(2)
self.pidRotateRate = 0
self.wasRotating = False
# PID loop for Cartesian Y direction
self.pidYDefault = {'p': 0.15, 'i': 0, 'd': 0.05}
self.sd.putNumber('pidYP', self.pidYDefault['p'])
self.sd.putNumber('pidYI', self.pidYDefault['i'])
self.sd.putNumber('pidYD', self.pidYDefault['d'])
self.pidY = wpilib.PIDController(self.pidYDefault['p'], self.pidYDefault['i'], self.pidYDefault['d'], self.encoderY.getDistance, self.updateYPID)
self.pidYRate = 0
self.toDistanceFirstCall = True
self.toAngleFirstCall = True
self.toTimeFirstCall = True
self.lastAngle = 0
self.timer = wpilib.Timer()
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 robotInit(self):
super().__init__()
# Instances of classes
# Instantiate Subsystems
#XXX DEBUGGING
self.drivetrain = Drivetrain(self)
self.shooter = Shooter(self)
self.carrier = Carrier(self)
self.feeder = Feeder(self)
self.intake = Intake(self)
#self.winch = Winch(self)
#self.climber = Climber(self)
#self.climber_big = Climber_Big(self)
#self.climber_little = Climber_Little(self)
# Instantiate Joysticks
self.left_joy = wpilib.Joystick(1)
self.right_joy = wpilib.Joystick(2)
# Instantiate Xbox
self.xbox = wpilib.Joystick(3)
# Instantiate OI; must be AFTER joysticks are inited
self.oi = OI(self)
self.timer = wpilib.Timer()
def teleopInit(self):
#starting out the state at neutral motors
self.state = 4
#timer for the fire function
self.timer = wpilib.Timer()
self.timer.start()
def __init__(self):
super().__init__()
self.leds = LEDSubsystem.getInstance()
self.requires(self.leds)
self.timer = wpilib.Timer()
self.state = False
self.counter = 0
def __init__(self):
super().__init__('Drivetrain')
#The set motor controllers for this years robot and how motors are coded
self.motor_rb = ctre.WPI_TalonSRX(1)
self.motor_rf = ctre.WPI_VictorSPX(17)
self.motor_lb = ctre.WPI_TalonSRX(13)
self.motor_lf = ctre.WPI_VictorSPX(15)
self.motor_rf.follow(self.motor_rb)
self.motor_lf.follow(self.motor_lb)
self.motors = [self.motor_rb, self.motor_lb, self.motor_rf, self.motor_lf]
self.drive = wpilib.drive.DifferentialDrive(self.motor_rb, self.motor_lb)
self.navx = navx.AHRS.create_spi()
self.motor_lb.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder,0,0)
self.motor_rb.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder, 0, 0)
self.motor_rb.selectProfileSlot(0, 0)
self.motor_lb.selectProfileSlot(0, 0)
self.navx_table = networktables.NetworkTables.getTable('/Sensor/Navx')
self.leftEncoder_table = networktables.NetworkTables.getTable("/Encoder/Left")
self.rightEncoder_table = networktables.NetworkTables.getTable("/Encoder/Right")
self.leftError = networktables.NetworkTables.getTable("/TalonL/Error")
self.rightError = networktables.NetworkTables.getTable("/TalonR/Error")
self.motor_lb.setSensorPhase(True)
self.motor_rb.setSensorPhase(True)
self.timer = wpilib.Timer()
self.timer.start()
self.mode = ""
self.logger = None
def robotInit(self):
'''Robot initialization function'''
# object that handles basic drive operations
self.leftMotor = wpilib.Victor(0)
self.rightMotor = wpilib.Victor(1)
#self.myRobot = wpilib.RobotDrive(0, 1)
self.myRobot = DifferentialDrive(self.leftMotor, self.rightMotor)
# joyStick 0
self.joyStick = wpilib.Joystick(0)
self.myRobot.setExpiration(0.1)
self.myRobot.setSafetyEnabled(True)
# encoders
self.rightEncoder = wpilib.Encoder(0, 1, False, wpilib.Encoder.EncodingType.k4X)
self.leftEncoder = wpilib.Encoder(2,3, False, wpilib.Encoder.EncodingType.k4X)
# set up encoder
self.drivePulsePerRotation = 1024
self.driveWheelRadius = 3.0
self. driveGearRatio = 1.0/1.0
self.driveEncoderPulsePerRot = self.drivePulsePerRotation * self.driveGearRatio
self.driveEncoderDistPerPulse = (math.pi*2*self.driveWheelRadius)/self.driveEncoderPulsePerRot;
self.leftEncoder.setDistancePerPulse(self.driveEncoderDistPerPulse)
self.leftEncoder.setReverseDirection(False)
self.rightEncoder.setDistancePerPulse(self.driveEncoderDistPerPulse)
self.rightEncoder.setReverseDirection(False)
self.timer = wpilib.Timer()
