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 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_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 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
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
