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()