def robotInit(self):
self.C = Component(
) # Components inits all connected motors, sensors, and joysticks. See inits.py.
# Setup subsystems
self.driverStation = wpilib.DriverStation.getInstance()
self.drive = Chassis(self.C.driveTrain, self.C.gyroS,
self.C.driveYEncoderS)
self.lights = Lights()
self.metabox = MetaBox(self.C.elevatorEncoderS, self.C.elevatorLimitS,
self.C.jawsLimitS, self.C.metaboxLimitS,
self.C.jawsM, self.C.elevatorM, self.C.intakeM,
self.C.jawsSol)
self.winch = Winch(self.C.winchM)
self.power = Power()
# Joysticks
self.joystick = wpilib.XboxController(0)
self.leftJ = wpilib.Joystick(1)
# default to rainbow effect
self.lights.run({'effect': 'rainbow'})
self.sd = NetworkTables.getTable('SmartDashboard')
self.sd.putNumber('station', 2)
class MyRobot(wpilib.SampleRobot):
def robotInit(self):
self.C = Component() # Components inits all connected motors, sensors, and joysticks. See components.py.
# Setup subsystems
self.drive = Chassis(self.C.driveTrain, self.C.gyroS)
self.belt = Belt(self.C.beltM)
self.beltAxis = BeltAxis(self.C.beltAxisM)
self.shoot = Shooter(self.C.flipM, self.C.shootM)
self.lift = Lift(self.C.lift1M, self.C.lift2M)
self.sonic = Sonic(self.C.sonic)
self.arduino = Arduino(self.C.arduino)
self.guide = Guiding(self.sonic, self.drive)
self.sd = SmartDashboard
self.sd.putBoolean("autoMove", False)
self.autoLoop = 0
def operatorControl(self):
self.C.driveTrain.setSafetyEnabled(True) # keeps robot from going crazy if connection with DS is lost
while self.isOperatorControl() and self.isEnabled():
distance = self.sonic.getAverage()
print(distance)
self.sd.putDouble('distance', distance)
if (5.25 < distance and distance < 5.75):
self.sd.putBoolean("Ready", True)
else:
self.sd.putBoolean("Ready", False)
if (self.C.middleJ.getRawButton(1) == True):
self.guide.guideSonic()
# Drive
self.drive.run(self.C.leftJ.getRawButton(1), self.C.leftJ.getY(), self.C.middleJ.getY())
# Components
self.belt.run(self.C.rightJ.getRawButton(4), self.C.rightJ.getRawButton(5))
self.beltAxis.run(self.C.rightJ.getRawButton(3), self.C.rightJ.getY(), self.C.beltAxisTS.get(), self.C.beltAxisBS.get())
self.lift.run(self.C.leftJ.getRawButton(1), self.C.leftJ.getY(), self.C.middleJ.getY())
self.shoot.run(self.C.rightJ.getRawButton(2), self.C.beltAxisTS.get(), self.C.rightJ.getRawButton(1), self.C.flipS.get()) # weird argument in the middle makes sure shooter doesn't turn on when belt it up
wpilib.Timer.delay(0.005) # wait for a motor update time
def autonomous(self):
"""This function is called periodically during autonomous."""
if (self.sd.getBoolean("autoMove") == True):
distance = self.sonic.getAverage()
while (self.autoLoop < 2000 and distance > 5):
self.drive.set(-1, 0)
wpilib.Timer.delay(0.005)
self.autoLoop = self.autoLoop + 1
def test(self):
"""This function is called periodically during test mode."""
def test_toggle_range_holding():
chassis = Chassis()
chassis.distance_pid = MagicMock()
chassis.range_setpoint = 0.0
chassis.toggle_range_holding(2.0)
assert chassis.range_setpoint == 2.0
chassis.toggle_range_holding(2.0)
assert chassis.range_setpoint == 0.0
def test_toggle_track_vision():
chassis = Chassis()
chassis.distance_pid = MagicMock()
vision = chassis.track_vision
chassis.toggle_vision_tracking()
assert chassis.track_vision is not vision
chassis.toggle_vision_tracking()
assert chassis.track_vision is vision
def test_toggle_field_oriented():
chassis = Chassis()
field_o = chassis.field_oriented
chassis.toggle_field_oriented()
assert chassis.field_oriented is not field_o
chassis.toggle_field_oriented()
assert chassis.field_oriented is field_o
def create_component(component):
"""
Return the desired component.
"""
# Weapon components.
if component == WeaponComponent.LASER:
return Weapon(name='pulse laser',
damage=5,
min_targets=1,
max_targets=5,
color=libtcod.green,
range=10,
cost=2,
rate_of_fire=10,
projectile=ProjectileType.LASER_PROJECTILE)
elif component == WeaponComponent.GUN:
return Weapon(name='gattling gun',
damage=5,
min_targets=1,
max_targets=3,
color=libtcod.red,
range=10,
cost=1,
rate_of_fire=4,
projectile=ProjectileType.BASIC_PROJECTILE)
# Chassis components.
elif component == ChassisComponent.BASIC_CHASSIS:
return Chassis(max_hp=30)
elif component == ChassisComponent.WEAK_CHASSIS:
return Chassis(max_hp=20)
# Propulsion components.
elif component == PropulsionComponent.BASIC_PROPULSION:
return Propulsion(max_speed=12, max_impulse=2)
elif component == PropulsionComponent.WEAK_PROPULSION:
return Propulsion(max_speed=4, max_impulse=1)
# AI components.
elif component == AIComponent.DEBUG:
return DoNothing()
elif component == AIComponent.PROJECTILE:
return ProjectileAI()
return None
def test_absolute(robot):
epsilon = 0.01 # Tolerance for angular floating point errors (~0.05 degrees)
chassis = Chassis()
reset_chassis(chassis)
# Absolute argument should point the modules in the absolute direction
# for diagnostic purposes
chassis.drive(-1.0, -1.0, 0.0, absolute=True)
for module in chassis._modules.values():
assert abs(constrain_angle(module.direction + 3.0 / 4.0 * math.pi)) < epsilon
reset_chassis(chassis)
def test_retain_wheel_direction():
# When the joystick is returned to the centre, keep the last direction that the wheels were pointing
chassis = Chassis()
for name, module in chassis._modules.items():
module.steer(math.pi / 4.0)
chassis.drive(0.0, 0.0, 0.0)
for name, module in chassis._modules.items():
assert abs(constrain_angle(module.direction - math.pi / 4.0)) < epsilon
# Should not matter what the throttle is, even if it is zero
chassis.drive(0.0, 0.0, 0.0)
for name, module in chassis._modules.items():
assert abs(constrain_angle(module.direction - math.pi / 4.0)) < epsilon
def robotInit(self):
self.C = Component() # Components inits all connected motors, sensors, and joysticks. See components.py.
# Setup subsystems
self.drive = Chassis(self.C.driveTrain, self.C.gyroS)
self.belt = Belt(self.C.beltM)
self.beltAxis = BeltAxis(self.C.beltAxisM)
self.shoot = Shooter(self.C.flipM, self.C.shootM)
self.lift = Lift(self.C.lift1M, self.C.lift2M)
self.sonic = Sonic(self.C.sonic)
self.arduino = Arduino(self.C.arduino)
self.guide = Guiding(self.sonic, self.drive)
self.sd = SmartDashboard
self.sd.putBoolean("autoMove", False)
self.autoLoop = 0
def test_chassis(robot, wpilib):
epsilon = 0.01 # Tolerance for angular floating point errors (~0.05 degrees)
chassis = Chassis()
# vX is out the left side of the robot, vY is out of the front, vZ is upwards, so a +ve rotation is counter-clockwise
# vX vY vZ throttle
reset_chassis(chassis)
# test x axis
chassis.drive(1.0, 0.0, 0.0, 1.0)
for name, module in chassis._modules.items():
assert abs(constrain_angle(module.direction)) % math.pi < epsilon
reset_chassis(chassis)
# test y axis
chassis.drive(0.0, 1.0, 0.0)
for name, module in chassis._modules.items():
# test weather each module is facing in the right direction
assert abs(constrain_angle(math.pi / 2.0 - module.direction)) < epsilon
reset_chassis(chassis)
vz = {'a': math.atan2(-Chassis.length, Chassis.width), # the angle that module a will go to if we spin on spot
'b': math.atan2(Chassis.length, Chassis.width),
'c': math.atan2(-Chassis.length, Chassis.width),
'd': math.atan2(Chassis.length, Chassis.width)
}
chassis.drive(0.0, 0.0, 1.0)
for name, module in chassis._modules.items():
assert abs(constrain_angle(module.direction - vz[name])) < epsilon
reset_chassis(chassis)
chassis.drive(1.0, 1.0, 0.0)
for module in chassis._modules.values():
assert abs(constrain_angle(module.direction - math.pi / 4.0)) < epsilon
reset_chassis(chassis)
class Randy(wpilib.TimedRobot):
def robotInit(self):
self.C = Component(
) # Components inits all connected motors, sensors, and joysticks. See inits.py.
# Setup subsystems
self.driverStation = wpilib.DriverStation.getInstance()
self.drive = Chassis(self.C.driveTrain, self.C.gyroS,
self.C.driveYEncoderS)
self.lights = Lights()
self.metabox = MetaBox(self.C.elevatorEncoderS, self.C.elevatorLimitS,
self.C.jawsLimitS, self.C.metaboxLimitS,
self.C.jawsM, self.C.elevatorM, self.C.intakeM,
self.C.jawsSol)
self.winch = Winch(self.C.winchM)
self.power = Power()
# Joysticks
self.joystick = wpilib.XboxController(0)
self.leftJ = wpilib.Joystick(1)
# default to rainbow effect
self.lights.run({'effect': 'rainbow'})
self.sd = NetworkTables.getTable('SmartDashboard')
self.sd.putNumber('station', 2)
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
'''Components'''
# Rumble
averageDriveCurrent = self.power.getAverageCurrent([0, 1, 14, 15])
if (averageDriveCurrent > 8):
self.joystick.setRumble(0, 1)
else:
self.joystick.setRumble(0, 0)
print(self.metabox.getEncoder())
'''
TODO: calibrate sparks
'''
# Drive
self.drive.run(self.joystick.getRawAxis(0),
self.joystick.getRawAxis(1),
self.joystick.getRawAxis(4))
# MetaBox
self.metabox.run(
self.leftJ.getY(), # elevator rate of change
self.leftJ.getRawButton(1), # run intake wheels in
self.leftJ.getRawButton(3), # open jaws
self.leftJ.getRawButton(2), # run intake wheels out
self.leftJ.getRawButton(4), # go to bottom
self.leftJ.getRawAxis(2), # set angle of jaws
self.leftJ.getRawButton(8)) # calibrate elevator
# Lights
self.lights.setColor(self.driverStation.getAlliance())
if (self.driverStation.getMatchTime() < 30
and self.driverStation.getMatchTime() != -1):
self.lights.run({'effect': 'flash', 'fade': True, 'speed': 200})
elif (helpers.deadband(self.leftJ.getY(), 0.1) != 0):
self.lights.run({'effect': 'stagger'})
elif (self.leftJ.getRawButton(1) or self.leftJ.getRawButton(2)):
self.lights.run({'effect': 'flash', 'fade': False, 'speed': 20})
else:
self.lights.run({'effect': 'rainbow'})
def teleopInit(self):
"""This function is run once each time the robot enters teleop mode."""
# reset gyro
self.C.gyroS.reset()
# reset encoder
self.C.driveYEncoderS.reset()
def autonomousInit(self):
"""This function is run once each time the robot enters autonomous mode."""
self.lights.run({'effect': 'flash', 'fade': True, 'speed': 400})
# reset gyro
self.C.gyroS.reset()
# reset encoder
self.C.driveYEncoderS.reset()
# Init autonomous
self.autonomousRoutine = Autonomous(self.drive, self.C.driveYEncoderS,
self.C.gyroS, self.metabox,
self.driverStation)
# reset state
self.autonomousRoutine.state = 0
def autonomousPeriodic(self):
self.autonomousRoutine.run() # see autonomous.py
def test(self):
# reset gyro
self.C.gyroS.reset()
# reset encoder
self.C.driveYEncoderS.reset()
