def init_vision(heading=0.0):
t = time.monotonic()
v = Vision()
v.chassis = FakeChassis()
# Inject some fake odometry
v.odometry.appendleft(Odometry(0, 0, heading, t - 0.3))
v.odometry.appendleft(Odometry(1, 1, heading, t - 0.2))
v.odometry.appendleft(Odometry(2, 2, heading, t - 0.1))
v.odometry.appendleft(Odometry(3, 3, heading, t))
return v, t
def test_odom_deque_order():
v = Vision()
v.chassis = FakeChassis()
for i in range(5):
time.sleep(0.05)
v.execute()
assert len(v.odometry) > 0
newer_odom = v.odometry.popleft()
for odom in v.odometry:
assert newer_odom.t > odom.t
newer_odom = odom
def test_sample_images():
variables = ['x', 'y', 'w', 'h']
vision = Vision()
with open('sample_img/tests.csv', 'r') as csvfile:
# filename, x, y, w, h
testreader = csv.reader(csvfile, delimiter=',')
for sample in testreader:
image = cv2.imread('sample_img/' + sample[0])
# Rescale if necessary
scaled = cv2.resize(image, (Vision.video_width, Vision.video_height))
results = vision._vision_process.findTarget(scaled)
yield find_target, sample[0], results[:-1], sample[1:], [0.05, 0.05] # Don't send the return image
vision.free()
def test_nt_update():
v = Vision()
nt = NetworkTable.getTable('vision')
nt.putNumber('x', 0.5)
assert v._values['x'] == 0.5
# Only updates to time should update the averaged values
assert v.pidGet() == 0.0
nt.putNumber('time', 1234)
# No change if width is zero
assert v.pidGet() == 0.0
assert v.no_vision_counter == 1
nt.putNumber('w', 0.7)
nt.putNumber('time', 1235)
assert v.pidGet() != 0.0
assert v.no_vision_counter == 0
def test_nt_update():
v = Vision()
nt = NetworkTable.getTable('vision')
nt.putNumber('x', 0.5)
assert v._values['x'] == 0.5
# Only updates to time should update the averaged values
assert v.pidGet() == 0.0
nt.putNumber('time', 1234)
# No change if width is zero
assert v.pidGet() == 0.0
assert v.no_vision_counter == 1
nt.putNumber('w', 0.7)
nt.putNumber('time', 1235)
assert v.pidGet() != 0.0
assert v.no_vision_counter == 0
def createObjects(self):
self.logger = logging.getLogger("robot")
self.sd = NetworkTable.getTable('SmartDashboard')
self.intake_motor = wpilib.CANTalon(14)
self.shooter_motor = wpilib.CANTalon(12)
self.defeater_motor = wpilib.CANTalon(1)
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.Joystick(1)
self.pressed_buttons_js = set()
self.pressed_buttons_gp = set()
# needs to be created here so we can pass it in to the PIDController
self.bno055 = BNO055()
self.vision = Vision()
self.range_finder = RangeFinder(0)
self.heading_hold_pid_output = BlankPIDOutput()
Tu = 1.6
Ku = 0.6
Kp = Ku * 0.3
self.heading_hold_pid = wpilib.PIDController(0.6,
2.0 * Kp / Tu * 0.1,
1.0 * Kp * Tu / 20.0 * 0,
self.bno055, self.heading_hold_pid_output)
self.heading_hold_pid.setTolerance(3.0*math.pi/180.0)
self.heading_hold_pid.setContinuous(True)
self.heading_hold_pid.setInputRange(-math.pi, math.pi)
self.heading_hold_pid.setOutputRange(-1.0, 1.0)
self.intake_motor.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.intake_motor.reverseSensor(False)
def createObjects(self):
"""Robot initialization function"""
self.chassis_left_front = rev.CANSparkMax(5, rev.MotorType.kBrushless)
self.chassis_left_rear = rev.CANSparkMax(4, rev.MotorType.kBrushless)
self.chassis_right_front = rev.CANSparkMax(7, rev.MotorType.kBrushless)
self.chassis_right_rear = rev.CANSparkMax(6, rev.MotorType.kBrushless)
self.indexer_motors = [ctre.WPI_TalonSRX(3)]
self.indexer_switches = [wpilib.DigitalInput(9)]
self.injector_slave_motor = ctre.WPI_TalonSRX(43)
self.injector_slave_motor.follow(self.indexer_motors[0])
self.injector_slave_motor.setInverted(True)
self.loading_piston = wpilib.Solenoid(0)
self.shooter_centre_motor = rev.CANSparkMax(3, rev.MotorType.kBrushless)
self.shooter_outer_motor = rev.CANSparkMax(2, rev.MotorType.kBrushless)
self.colour_sensor = rev.color.ColorSensorV3(wpilib.I2C.Port.kOnboard)
self.spinner_motor = wpilib.Spark(2)
self.spinner_solenoid = wpilib.DoubleSolenoid(2, 3)
self.turret_centre_index = wpilib.DigitalInput(1)
self.turret_motor = ctre.WPI_TalonSRX(10)
self.vision = Vision()
# operator interface
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.spinner_joystick = wpilib.Joystick(2)
self.turret_joystick = wpilib.Joystick(3)
def predict(self, timestep=1):
"""Predict what the measurement should be in the next timestep.
:param timestep: the number of timesteps in the past that we are predicting forward *from*"""
F = np.identity(self.state_vector_size)
F[0][1] = self.loop_dt
B = np.identity(self.state_vector_size)
self.imu_deque.append(self.get_heading_state())
u = Vision.rad_to_vision_units(self.imu_deque[-timestep] -
self.imu_deque[-timestep - 1])
self.filter.predict(F, u, B)
def createObjects(self):
self.logger = logging.getLogger("robot")
self.sd = NetworkTable.getTable('SmartDashboard')
self.intake_motor = wpilib.CANTalon(14)
self.shooter_motor = wpilib.CANTalon(12)
self.defeater_motor = wpilib.CANTalon(1)
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.Joystick(1)
self.pressed_buttons_js = set()
self.pressed_buttons_gp = set()
# needs to be created here so we can pass it in to the PIDController
self.bno055 = BNO055()
self.vision = Vision()
self.range_finder = RangeFinder(0)
self.heading_hold_pid_output = BlankPIDOutput()
Tu = 1.6
Ku = 0.6
Kp = Ku * 0.3
self.heading_hold_pid = wpilib.PIDController(
0.8,
0.0,
1.5, #2.0 * Kp / Tu * 0.1, 1.0 * Kp * Tu / 20.0 * 0,
self.bno055,
self.heading_hold_pid_output)
"""self.heading_hold_pid = wpilib.PIDController(0.6,
2.0 * Kp / Tu * 0.1,
1.0 * Kp * Tu / 20.0 * 0,
self.bno055, self.heading_hold_pid_output)"""
self.heading_hold_pid.setAbsoluteTolerance(3.0 * math.pi / 180.0)
self.heading_hold_pid.setContinuous(True)
self.heading_hold_pid.setInputRange(-math.pi, math.pi)
self.heading_hold_pid.setOutputRange(-0.2, 0.2)
#self.heading_hold_pid.setOutputRange(-1.0, 1.0)
self.intake_motor.setFeedbackDevice(
wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.intake_motor.reverseSensor(False)
self.joystick_rate = 0.3
def get_players(constants, entities):
players = []
for i in range(constants['player_count']):
# Building the players
fighter_component = Fighter(hp=30, defense=1, power=2)
inventory_component = Inventory(26)
level_component = Level()
equipment_component = Equipment()
vision_component = Vision(None, constants['fov_radius'])
players.append(
Entity(0,
0,
'@',
libtcod.blue,
'Player{0}'.format(i + 1),
blocks=True,
render_order=RenderOrder.ACTOR,
fighter=fighter_component,
inventory=inventory_component,
level=level_component,
equipment=equipment_component,
vision=vision_component))
# Give the player a dagger to start with
equippable_component = Equippable(EquipmentSlots.MAIN_HAND,
power_bonus=2)
dagger = Entity(0,
0,
'-',
libtcod.sky,
'Dagger',
equippable=equippable_component)
players[-1].inventory.add_item(dagger)
players[-1].equipment.toggle_equip(dagger)
entities.append(players[-1])
return players
class StrongholdRobot(magicbot.MagicRobot):
chassis = Chassis
intake = Intake
shooter = Shooter
defeater = Defeater
boulder_automation = BoulderAutomation
def createObjects(self):
self.logger = logging.getLogger("robot")
self.sd = NetworkTable.getTable('SmartDashboard')
self.intake_motor = wpilib.CANTalon(14)
self.shooter_motor = wpilib.CANTalon(12)
self.defeater_motor = wpilib.CANTalon(1)
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.Joystick(1)
self.pressed_buttons_js = set()
self.pressed_buttons_gp = set()
# needs to be created here so we can pass it in to the PIDController
self.bno055 = BNO055()
self.vision = Vision()
self.range_finder = RangeFinder(0)
self.heading_hold_pid_output = BlankPIDOutput()
Tu = 1.6
Ku = 0.6
Kp = Ku * 0.3
self.heading_hold_pid = wpilib.PIDController(
0.8,
0.0,
1.5, #2.0 * Kp / Tu * 0.1, 1.0 * Kp * Tu / 20.0 * 0,
self.bno055,
self.heading_hold_pid_output)
"""self.heading_hold_pid = wpilib.PIDController(0.6,
2.0 * Kp / Tu * 0.1,
1.0 * Kp * Tu / 20.0 * 0,
self.bno055, self.heading_hold_pid_output)"""
self.heading_hold_pid.setAbsoluteTolerance(3.0 * math.pi / 180.0)
self.heading_hold_pid.setContinuous(True)
self.heading_hold_pid.setInputRange(-math.pi, math.pi)
self.heading_hold_pid.setOutputRange(-0.2, 0.2)
#self.heading_hold_pid.setOutputRange(-1.0, 1.0)
self.intake_motor.setFeedbackDevice(
wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.intake_motor.reverseSensor(False)
self.joystick_rate = 0.3
def putData(self):
self.sd.putDouble("range_finder", self.range_finder.getDistance())
self.sd.putDouble("gyro", self.bno055.getHeading())
self.sd.putDouble("vision_pid_get", self.vision.pidGet())
self.sd.putDouble("vision_x", self.vision._values['x'])
self.sd.putDouble("vision_w", self.vision._values['w'])
self.sd.putDouble("vision_h", self.vision._values['h'])
self.sd.putDouble("vx", self.chassis.vx)
self.sd.putDouble("vy", self.chassis.vy)
self.sd.putDouble("vz", self.chassis.vz)
self.sd.putDouble("input_twist", self.chassis.inputs[2])
self.sd.putDouble("field_oriented", self.chassis.field_oriented)
self.sd.putDouble("raw_yaw", self.bno055.getRawHeading())
self.sd.putDouble("raw_pitch", self.bno055.getPitch())
self.sd.putDouble("raw_roll", self.bno055.getRoll())
self.sd.putDouble("shooter_speed",
-self.shooter.shooter_motor.getSetpoint()
) # minus sign here so +ve is shooting
self.sd.putDouble("heading_pid_output",
self.heading_hold_pid_output.output)
self.sd.putDouble("heading_hold_pid_setpoint",
self.heading_hold_pid.getSetpoint())
self.sd.putString("boulder_state",
self.boulder_automation.current_state)
self.sd.putDouble("intake_speed",
self.intake.intake_motor.getSetpoint())
self.sd.putDouble("distance_pid_output",
self.chassis.distance_pid_output.output)
self.sd.putBoolean("track_vision", self.chassis.track_vision)
self.sd.putDouble("pov", self.joystick.getPOV())
self.sd.putDouble("gyro_z_rate", self.bno055.getHeadingRate())
self.sd.putDouble(
"heading_hold_error",
self.heading_hold_pid.getSetpoint() - self.bno055.getAngle())
self.sd.putDouble("defeater_current",
self.defeater_motor.getOutputCurrent())
self.sd.putDouble("defeater_speed", self.defeater_motor.get())
self.sd.putDouble("joystick_throttle", self.joystick.getThrottle())
self.sd.putDouble("range_pid_get", self.range_finder.pidGet())
self.sd.putDouble("encoder_distance", self.chassis.distance)
distances = []
for module in self.chassis._modules.values():
distances.append(
abs(module.distance) / module.drive_counts_per_metre)
for key, distance in zip(self.chassis._modules.keys(), distances):
self.sd.putDouble("encoder_motor_" + key, distance)
def disabledInit(self):
self.boulder_automation.done()
def disabledPeriodic(self):
"""This function is called periodically when disabled."""
self.putData()
def teleopInit(self):
self.boulder_automation.done()
def autoInit(self):
self.boulder_automation.done()
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
try:
if self.debounce(6, gamepad=True):
self.boulder_automation.toggle_shoot_boulder()
except:
self.onException()
try:
if self.debounce(2) or self.debounce(1, gamepad=True):
self.boulder_automation.toggle_intake_boulder()
except:
self.onException()
try:
if self.debounce(7):
self.chassis.toggle_field_oriented()
except:
self.onException()
try:
if self.debounce(8):
enabled = self.heading_hold_pid.isEnable()
self.heading_hold_pid.disable()
self.bno055.resetHeading()
self.heading_hold_pid.setSetpoint(
constrain_angle(self.bno055.getAngle()))
self.heading_hold_pid.reset()
if enabled:
self.heading_hold_pid.enable()
except:
self.onException()
"""try:
if self.debounce(10):
self.chassis.toggle_vision_tracking()
except:
self.onException()"""
try:
if self.debounce(10):
self.chassis.toggle_range_holding(self.chassis.correct_range)
except:
self.onException()
try:
if self.debounce(1) or self.debounce(8, gamepad=True):
self.boulder_automation.toggle_shoot_boulder()
except:
self.onException()
try:
if self.debounce(9):
self.chassis.toggle_heading_hold()
except:
self.onException()
try:
if self.debounce(4):
self.defeater.up()
except:
self.onException()
try:
if self.debounce(5):
self.shooter.stop()
self.intake.stop()
except:
self.onException()
try:
if self.debounce(3):
self.chassis.track_vision = True
self.chassis.range_setpoint = self.chassis.correct_range
self.chassis.distance_pid.enable()
# self.shooter.start_shoot()
except:
self.onException()
try:
if self.debounce(6):
self.defeater.down()
except:
self.onException()
"""try:
if self.debounce(10):
self.shooter.backdrive()
self.intake.backdrive()
except:
self.onException()"""
try:
if self.joystick.getPOV() != -1:
self.chassis.heading_hold = True
direction = 0.0
if self.joystick.getPOV() == 0:
# shooter centre goal
direction = math.pi
elif self.joystick.getPOV() == 90:
# shooter right goal
direction = math.pi / 3.0 + math.pi
elif self.joystick.getPOV() == 270:
# shooter left goal
direction = -math.pi / 3.0 + math.pi
elif self.joystick.getPOV() == 180:
direction = 0.0
self.chassis.set_heading_setpoint(direction)
except:
self.onException()
try:
if self.joystick.getRawButton(11) or self.gamepad.getRawButton(2):
self.chassis.field_oriented = False
else:
self.chassis.field_oriented = True
except:
self.onException()
try:
if self.gamepad.getRawButton(3):
self.boulder_automation.engage("backdrive_manual")
elif self.boulder_automation.current_state == "backdrive_manual":
self.boulder_automation.done()
except:
self.onException()
"""try:
if self.debounce(1, gamepad=True):
self.chassis.zero_encoders()
self.chassis.distance_pid.setSetpoint(1.2)
self.chassis.distance_pid.enable()
except:
self.onException()"""
try:
if self.debounce(10, gamepad=True):
self.vision.write_image()
except:
self.onException()
try:
if self.joystick.getRawButton(12):
self.joystick_rate = 0.6
else:
self.joystick_rate = 0.4
except:
self.onException()
self.chassis.inputs = [
-rescale_js(self.joystick.getY(), deadzone=0.05, exponential=1.2),
-rescale_js(self.joystick.getX(), deadzone=0.05, exponential=1.2),
-rescale_js(self.joystick.getZ(),
deadzone=0.2,
exponential=15.0,
rate=self.joystick_rate),
(self.joystick.getThrottle() - 1.0) / -2.0
]
for input in self.chassis.inputs[0:3]:
if input != 0.0:
# Break out of auto if we move the stick
self.chassis.distance_pid.disable()
self.chassis.range_setpoint = None
self.chassis.track_vision = False
#self.chassis.field_oriented = True
self.putData()
def testPeriodic(self):
"""This function is called periodically during test mode."""
wpilib.LiveWindow.run()
def debounce(self, button, gamepad=False):
device = None
if gamepad:
pressed_buttons = self.pressed_buttons_gp
device = self.gamepad
else:
pressed_buttons = self.pressed_buttons_js
device = self.joystick
if device.getRawButton(button):
if button in pressed_buttons:
return False
else:
pressed_buttons.add(button)
return True
else:
pressed_buttons.discard(button)
return False
class StrongholdRobot(magicbot.MagicRobot):
chassis = Chassis
intake = Intake
shooter = Shooter
defeater = Defeater
def createObjects(self):
self.logger = logging.getLogger("robot")
self.sd = NetworkTable.getTable('SmartDashboard')
self.intake_motor = wpilib.CANTalon(14)
self.shooter_motor = wpilib.CANTalon(12)
self.defeater_motor = wpilib.CANTalon(1)
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.Joystick(1)
self.pressed_buttons_js = set()
self.pressed_buttons_gp = set()
# needs to be created here so we can pass it in to the PIDController
self.bno055 = BNO055()
self.vision = Vision()
self.range_finder = RangeFinder(0)
self.heading_hold_pid_output = BlankPIDOutput()
Tu = 1.6
Ku = 0.6
Kp = Ku * 0.3
self.heading_hold_pid = wpilib.PIDController(0.6,
2.0 * Kp / Tu * 0.1,
1.0 * Kp * Tu / 20.0 * 0,
self.bno055, self.heading_hold_pid_output)
self.heading_hold_pid.setTolerance(3.0*math.pi/180.0)
self.heading_hold_pid.setContinuous(True)
self.heading_hold_pid.setInputRange(-math.pi, math.pi)
self.heading_hold_pid.setOutputRange(-1.0, 1.0)
self.intake_motor.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.intake_motor.reverseSensor(False)
def putData(self):
self.sd.putDouble("range_finder", self.range_finder.getDistance())
self.sd.putDouble("gyro", self.bno055.getHeading())
vision_array = self.vision.get()
vision_x = None
vision_w = None
vision_h = None
if not vision_array:
vision_x = -2
vision_w = -2
vision_h = -2
else:
vision_x = vision_array[0]
vision_w = vision_array[2]
vision_h = vision_array[3]
self.sd.putDouble("vision_pid_get", self.vision.pidGet())
self.sd.putDouble("vision_x", vision_x)
self.sd.putDouble("vision_w", vision_w)
self.sd.putDouble("vision_h", vision_h)
self.sd.putDouble("vx", self.chassis.vx)
self.sd.putDouble("vy", self.chassis.vy)
self.sd.putDouble("vz", self.chassis.vz)
self.sd.putDouble("input_twist", self.chassis.inputs[2])
self.sd.putDouble("field_oriented", self.chassis.field_oriented)
self.sd.putDouble("raw_yaw", self.bno055.getRawHeading())
self.sd.putDouble("raw_pitch", self.bno055.getPitch())
self.sd.putDouble("raw_roll", self.bno055.getRoll())
self.sd.putDouble("shooter_speed", -self.shooter._speed) # minus sign here so +ve is shooting
self.sd.putDouble("heading_pid_output", self.heading_hold_pid_output.output)
self.sd.putDouble("heading_hold_pid_setpoint", self.heading_hold_pid.getSetpoint())
self.sd.putDouble("intake_state", self.intake.state)
self.sd.putDouble("intake_speed", self.intake._speed)
self.sd.putDouble("distance_pid_output", self.chassis.distance_pid_output.output)
self.sd.putBoolean("track_vision", self.chassis.track_vision)
self.sd.putDouble("pov", self.joystick.getPOV())
self.sd.putDouble("gyro_z_rate", self.bno055.getHeadingRate())
self.sd.putDouble("heading_hold_error", self.heading_hold_pid.getSetpoint()-self.bno055.getAngle())
self.sd.putDouble("defeater_current", self.defeater_motor.getOutputCurrent())
self.sd.putDouble("defeater_speed", self.defeater_motor.get())
self.sd.putDouble("joystick_throttle", self.joystick.getThrottle())
self.sd.putDouble("range_pid_get", self.range_finder.pidGet())
self.sd.putDouble("encoder_distance", self.chassis.distance)
def disabledInit(self):
pass
def disabledPeriodic(self):
"""This function is called periodically when disabled."""
self.putData()
def teleopInit(self):
pass
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
try:
if self.debounce(2) or self.debounce(1, gamepad=True):
self.intake.toggle()
except:
self.onException()
try:
if self.debounce(7):
self.chassis.toggle_field_oriented()
except:
self.onException()
try:
if self.debounce(8):
enabled = self.heading_hold_pid.isEnable()
self.heading_hold_pid.disable()
self.bno055.resetHeading()
self.heading_hold_pid.setSetpoint(constrain_angle(self.bno055.getAngle()))
self.heading_hold_pid.reset()
if enabled:
self.heading_hold_pid.enable()
except:
self.onException()
try:
if self.debounce(10):
self.chassis.toggle_vision_tracking()
except:
self.onException()
try:
if self.debounce(12):
self.chassis.toggle_range_holding(self.chassis.correct_range)
except:
self.onException()
try:
if self.debounce(1):
if self.shooter.state == shooter.States.off:
self.shooter.start_shoot()
else:
self.shooter.toggle()
self.intake.fire()
except:
self.onException()
try:
if self.debounce(9):
self.chassis.toggle_heading_hold()
except:
self.onException()
try:
if self.debounce(4):
self.defeater.up()
except:
self.onException()
try:
if self.debounce(5):
self.shooter.stop()
self.intake.stop()
except:
self.onException()
try:
if self.debounce(3):
self.chassis.track_vision = True
self.chassis.range_setpoint = self.chassis.correct_range
self.chassis.distance_pid.enable()
# self.shooter.start_shoot()
except:
self.onException()
try:
if self.debounce(6):
self.defeater.down()
except:
self.onException()
"""try:
if self.debounce(10):
self.shooter.backdrive()
self.intake.backdrive()
except:
self.onException()"""
try:
if self.joystick.getPOV() != -1:
self.chassis.heading_hold = True
direction = 0.0
if self.joystick.getPOV() == 0:
# shooter centre goal
direction = math.pi
elif self.joystick.getPOV() == 90:
# shooter right goal
direction = math.pi/3.0+math.pi
elif self.joystick.getPOV() == 270:
# shooter left goal
direction = -math.pi/3.0+math.pi
elif self.joystick.getPOV() == 180:
direction = 0.0
self.chassis.set_heading_setpoint(direction)
except:
self.onException()
try:
if self.joystick.getRawButton(11) or self.gamepad.getRawButton(2):
self.chassis.field_oriented = False
else:
self.chassis.field_oriented = True
except:
self.onException()
try:
if self.gamepad.getRawButton(3):
self.intake.backdrive_slow()
except:
self.onException()
try:
if self.debounce(1, gamepad=True):
self.chassis.zero_encoders()
self.chassis.distance_pid.setSetpoint(1.2)
self.chassis.distance_pid.enable()
except:
self.onException()
try:
if self.gamepad.getRawButton(4):
self.shooter.backdrive_recovery()
except:
self.onException()
self.chassis.inputs = [-rescale_js(self.joystick.getY(), deadzone=0.05, exponential=1.2),
- rescale_js(self.joystick.getX(), deadzone=0.05, exponential=1.2),
- rescale_js(self.joystick.getZ(), deadzone=0.2, exponential=15.0, rate=0.3),
(self.joystick.getThrottle() - 1.0) / -2.0
]
for input in self.chassis.inputs[0:3]:
if input != 0.0:
# Break out of auto if we move the stick
self.chassis.distance_pid.disable()
self.chassis.range_setpoint = None
self.chassis.track_vision = False
self.chassis.field_oriented = True
self.putData()
def testPeriodic(self):
"""This function is called periodically during test mode."""
wpilib.LiveWindow.run()
def debounce(self, button, gamepad = False):
device = None
if gamepad:
pressed_buttons = self.pressed_buttons_gp
device = self.gamepad
else:
pressed_buttons = self.pressed_buttons_js
device = self.joystick
if device.getRawButton(button):
if button in pressed_buttons:
return False
else:
pressed_buttons.add(button)
return True
else:
pressed_buttons.discard(button)
return False
