def calibrate(self): """Calibrate the gyro by running for a number of samples and computing the center value. Then use the center value as the Accumulator center value for subsequent measurements. It's important to make sure that the robot is not moving while the centering calculations are in progress, this is typically done when the robot is first turned on while it's sitting at rest before the competition starts. .. note:: Usually you don't need to call this, as it's called when the object is first created. If you do, it will freeze your robot for 5 seconds """ if self.spi is None: return if not hal.HALIsSimulation(): Timer.delay(0.1) self.spi.setAccumulatorCenter(0) self.spi.resetAccumulator() if not hal.HALIsSimulation(): Timer.delay(self.kCalibrationSampleTime) self.spi.setAccumulatorCenter(int(self.spi.getAccumulatorAverage())) self.spi.resetAccumulator()
def calibrate(self): if self.spi is None: return if not hal.HALIsSimulation(): Timer.delay(0.1) self.spi.setAccumulatorCenter(0) self.spi.resetAccumulator() if not hal.HALIsSimulation(): Timer.delay(self.kCalibrationSampleTime) self.spi.setAccumulatorCenter(int(self.spi.getAccumulatorAverage())) self.spi.resetAccumulator()
def __init__(self, port=None, address=None): super().__init__() self.address = address self.port = port if self.address is None: self.address = self.BNO055_ADDRESS_A if port is None: port = I2C.Port.kMXP self.logger = logging.getLogger("gyro") sim_port = None if hal.HALIsSimulation(): from .bno055_sim import BNO055Sim sim_port = BNO055Sim() self.i2c = I2C(port, self.address, sim_port) # set the units that we want self.offset = 0.0 try: current_units = self.i2c.read(self.BNO055_UNIT_SEL_ADDR, 1)[0] for unit_list in self.BNO055_UNIT_SEL_LIST: if unit_list[0] == 1: current_units = current_units | (1 << unit_list[1]) elif unit_list[0] == 0: current_units = current_units & ~(1 << unit_list[1]) self.i2c.write(self.BNO055_UNIT_SEL_ADDR, current_units) self.setOperationMode( self.OPERATION_MODE_IMUPLUS) # accelerometer and gyro self.reverse_axis(False, False, False) except: pass
def __init__(self, delay_period): """ :param delay_period: The amount of time (in seconds) to do a delay :type delay_period: float """ # The WPILib sleep/etc functions are slightly less stable as # they have more overhead, so only use them in simulation mode if hal.HALIsSimulation(): self.delay = wpilib.Timer.delay self.get_now = wpilib.Timer.getFPGATimestamp # Test to see if we're in a unit test, and switch the wait function # to run more efficiently -- otherwise full tests are dog slow try: import pyfrc.config if pyfrc.config.mode in ("test", "upload"): self.wait = self._wait_unit_tests except: pass else: self.delay = time.sleep self.get_now = time.monotonic self.delay_period = float(delay_period) if self.delay_period < 0.001: raise ValueError("You probably don't want to delay less than 1ms!") self.next_delay = self.get_now() + self.delay_period
def __init__(self, port): simPort = None if hal.HALIsSimulation(): from ._impl.sim_io import NavXI2CSim simPort = NavXI2CSim() self.port = wpilib.I2C(port, 0x32, simPort=simPort)
def main(robot_cls): """Starting point for the applications.""" RobotBase.initializeHardwareConfiguration() hal.HALReport(hal.HALUsageReporting.kResourceType_Language, hal.HALUsageReporting.kLanguage_Python) try: robot = robot_cls() robot.prestart() except: from .driverstation import DriverStation DriverStation.reportError("ERROR Could not instantiate robot", True) logger.exception("Robots don't quit!") logger.exception("Could not instantiate robot " + robot_cls.__name__ + "!") return False # Add a check to see if the user forgot to call super().__init__() if not hasattr(robot, '_RobotBase__initialized'): logger.error( "If your robot class has an __init__ function, it must call super().__init__()!" ) return False if not hal.HALIsSimulation(): try: import wpilib with open('/tmp/frc_versions/FRC_Lib_Version.ini', 'w') as fp: fp.write('RobotPy %s' % wpilib.__version__) except: logger.warning("Could not write FRC version file to disk") try: robot.startCompetition() except KeyboardInterrupt: logger.exception( "THIS IS NOT AN ERROR: The user hit CTRL-C to kill the robot") logger.info("Exiting because of keyboard interrupt") return True except: from .driverstation import DriverStation DriverStation.reportError("ERROR Unhandled exception", True) logger.warn("Robots don't quit!") logger.exception( "---> The startCompetition() method (or methods called by it) should have handled the exception." ) return False else: # startCompetition never returns unless exception occurs.... from .driverstation import DriverStation DriverStation.reportError("ERROR startCompetition() returned", False) logger.warn("Robots don't quit!") logger.error( "---> Unexpected return from startCompetition() method.") return False
def __init__(self, port=None): """ Constructor. :param port: The SPI port that the gyro is connected to :type port: :class:`.SPI.Port` """ super().__init__() if port is None: port = SPI.Port.kOnboardCS0 simPort = None if hal.HALIsSimulation(): from hal_impl.spi_helpers import ADXRS450_Gyro_Sim simPort = ADXRS450_Gyro_Sim(self) self.spi = SPI(port, simPort=simPort) self.spi.setClockRate(3000000) self.spi.setMSBFirst() self.spi.setSampleDataOnLeadingEdge() self.spi.setClockActiveHigh() self.spi.setChipSelectActiveLow() # Validate the part ID if (self._readRegister(self.kPIDRegister) & 0xFF00) != 0x5200: self.spi.close() self.spi = None DriverStation.reportError( "could not find ADXRS450 gyro on SPI port %s" % port, False) return # python-specific: make this easier to simulate if hal.isSimulation(): self.spi.initAccumulator(self.kSamplePeriod, 0x20000000, 4, 0x0, 0x0, 0, 32, True, True) else: self.spi.initAccumulator( self.kSamplePeriod, 0x20000000, 4, 0x0C00000E, 0x04000000, 10, 16, True, True, ) self.calibrate() hal.report(hal.UsageReporting.kResourceType_ADXRS450, port) self.setName("ADXRS450_Gyro", port)
def createObjects(self): # TEMP - PRACTICE BOT ONLY # Launch cameraserver # wpilib.CameraServer.launch() # Practice bot # On practice bot, DIO is shorted # self.is_practice_bot = wpilib.DigitalInput(30) if hal.HALIsSimulation(): self.is_practice_bot = wpilib.DigitalInput(20) else: self.is_practice_bot = wpilib.DigitalInput(30) # Drivetrain self.drivetrain_left_motor_master = ctre.WPI_TalonSRX(4) self.drivetrain_left_motor_slave = ctre.WPI_TalonSRX(7) # was 3 self.drivetrain_right_motor_master = ctre.WPI_TalonSRX(5) self.drivetrain_right_motor_slave = ctre.WPI_TalonSRX(6) self.drivetrain_shifter_solenoid = wpilib.Solenoid(0) # Elevator self.elevator_motor = ctre.WPI_TalonSRX(8) self.elevator_solenoid = wpilib.DoubleSolenoid(1, 2) self.elevator_reverse_limit = wpilib.DigitalInput(0) # Grabber self.grabber_left_motor = ctre.WPI_TalonSRX(1) self.grabber_right_motor = ctre.WPI_TalonSRX(2) # Ramp self.ramp_solenoid = wpilib.DoubleSolenoid(3, 4) self.ramp_motor = ctre.WPI_TalonSRX(3) # was 7 self.hold_start_time = None # Controllers self.drive_controller = wpilib.XboxController(0) self.operator_controller = wpilib.XboxController(1) # Compressor self.compressor = wpilib.Compressor() # LEDs self.led_driver = wpilib.Spark(0) # Navx self.navx = navx.AHRS.create_spi() # Macros / path recorder self._is_recording_macro = False self._is_playing_macro = False self._is_recording_path = False # Flippy toggle self._is_flippy = False
def resetAccumulator(self): """Resets the accumulator to the initial value. """ hal.resetAccumulator(self.port) # Wait until the next sample, so the next call to getAccumulator*() # won't have old values. sampleTime = 1.0 / AnalogInput.getGlobalSampleRate() overSamples = 1 << self.getOversampleBits() averageSamples = 1 << self.getAverageBits() if not hal.HALIsSimulation(): Timer.delay(sampleTime * overSamples * averageSamples)
def __init__(self): # mjpg-streamer isn't setting parameters properly yet, so do it here if not hal.HALIsSimulation(): # pragma: no cover from vision.vision import setCaptureParameters setCaptureParameters( "/dev/v4l/by-id/usb-046d_0825_96EBCE50-video-index0", "/etc/default/mjpg-streamer") self.nt = NetworkTable.getTable('vision') self._values = {'x': 0.0, 'y': 0.0, 'w': 0.0, 'h': 0.0, 'time': 0.0} self._smoothed_pidget = 0.0 self.no_vision_counter = 0 self.nt.addTableListener(self.valueChanged)
def __init__(self, port, bitrate=None): if bitrate is None: bitrate = DEFAULT_SPI_BITRATE_HZ simPort = None if hal.HALIsSimulation(): from ._impl.sim_io import NavXSPISim simPort = NavXSPISim() self.port = SPI(port, simPort=simPort) self.bitrate = bitrate
def __init__(self, port: wpilib.I2C.Port = 0): super().__init__() if port is None: port = wpilib.I2C.Port.kOnboard simPort = None if hal.HALIsSimulation(): simPort = REV_Color_Sim(self) self.i2c = wpilib.I2C(port, self.ADDRESS, simPort=simPort) self.clearChannel = 0 self.setName("REV_Robotics_Color_Sensor_V2", port)
def __init__(self): super().__init__(self.get_angle, 'angle_controller') self.last_angle = 0 self.angle_reset_factor = 0 if hal.HALIsSimulation(): self.set_abs_output_range(0.08, 0.25) else: # self.set_abs_output_range(0.13, 0.25) self.set_abs_output_range(0.13, 0.4) if hasattr(self, 'pid'): self.pid.setInputRange(-180.0, 180.0) self.pid.setOutputRange(-1.0, 1.0) self.pid.setContinuous(True) self.report = 0
def __init__(self, port, simPort=None): """Constructor :param port: the physical SPI port :type port: :class:`.SPI.Port` :param simPort: This must be an object that implements all of the spi* functions from hal_impl that you use. See ``test_spi.py`` for an example. """ if port not in [self.Port.kOnboardCS0, self.Port.kOnboardCS1, self.Port.kOnboardCS2, self.Port.kOnboardCS3, self.Port.kMXP]: raise ValueError("Invalid value '%s' for SPI port" % port) if hal.HALIsSimulation(): if simPort is None: # If you want more functionality, implement your own mock from hal_impl.spi_helpers import SPISimBase simPort = SPISimBase() msg = "Using stub simulator for SPI port %s" % port warnings.warn(msg) # Just check for basic functionality assert hasattr(simPort, 'initializeSPI') assert hasattr(simPort, 'closeSPI') self._port = (simPort, port) else: self._port = port self.bitOrder = False self.clockPolarity = False self.dataOnTrailing = False hal.initializeSPI(self._port) self.__finalizer = weakref.finalize(self, _freeSPI, self._port) SPI.devices += 1 hal.report(hal.UsageReporting.kResourceType_SPI, SPI.devices)
def __init__(self, port, simPort=None): """Constructor :param port: the physical SPI port :type port: :class:`.SPI.Port` :param simPort: This must be an object that implements all of the spi* functions from hal_impl that you use. See ``test_spi.py`` for an example. """ port = self.Port(port) # python-specific if hal.HALIsSimulation(): if simPort is None: # If you want more functionality, implement your own mock from hal_impl.spi_helpers import SPISimBase simPort = SPISimBase() msg = "Using stub simulator for SPI port %s" % port warnings.warn(msg) # Just check for basic functionality assert hasattr(simPort, "initializeSPI") assert hasattr(simPort, "closeSPI") self._port = (simPort, port) else: self._port = port # python-specific: these are bools instead of integers self.msbFirst = False self.clockIdleHigh = False self.sampleOnTrailing = False self.accum = None hal.initializeSPI(self._port) self.__finalizer = weakref.finalize(self, _freeSPI, self._port) SPI.devices += 1 hal.report(hal.UsageReporting.kResourceType_SPI, SPI.devices)
def __init__(self): self.running = True self.last_ts = None self.enabled = False self.lock = threading.Lock() self.cond = threading.Condition(self.lock) # limited buffer of data self.buffer = deque(maxlen=20) # WPILib sleep/etc functions have more overhead, so only use in simulation if hal.HALIsSimulation(): self.delay = wpilib.Timer.delay self.get_now = wpilib.Timer.getFPGATimestamp else: self.delay = time.sleep self.get_now = time.time self.thread = threading.Thread(target=self._run, daemon=True) self.thread.start() wpilib.Resource._add_global_resource(self)
def __init__(self, port=None): """ Constructor. :param port: The SPI port that the gyro is connected to :type port: :class:`.SPI.Port` """ if port is None: port = SPI.Port.kOnboardCS0 simPort = None if hal.HALIsSimulation(): from hal_impl.spi_helpers import ADXRS450_Gyro_Sim simPort = ADXRS450_Gyro_Sim(self) self.spi = SPI(port, simPort=simPort) self.spi.setClockRate(3000000) self.spi.setMSBFirst() self.spi.setSampleDataOnRising() self.spi.setClockActiveHigh() self.spi.setChipSelectActiveLow() # Validate the part ID if (self._readRegister(self.kPIDRegister) & 0xff00) != 0x5200: self.spi.free() self.spi = None DriverStation.reportError( "could not find ADXRS450 gyro on SPI port %s" % port, False) return self.spi.initAccumulator(self.kSamplePeriod, 0x20000000, 4, 0x0c00000e, 0x04000000, 10, 16, True, True) self.calibrate() hal.report(hal.UsageReporting.kResourceType_ADXRS450, port) LiveWindow.addSensor("ADXRS450_Gyro", port, self)
def __init__(self, port, deviceAddress, simPort=None): """Constructor. :param port: The I2C port the device is connected to. :type port: :class:`.I2C.Port` :param deviceAddress: The address of the device on the I2C bus. :param simPort: This must be an object that implements all of the i2c* functions from hal_impl that you use. See ``test_i2c.py`` for an example. """ if port not in [self.Port.kOnboard, self.Port.kMXP]: raise ValueError("Invalid value '%s' for I2C port" % port) if hal.HALIsSimulation(): if simPort is None: # If you want more functionality, implement your own mock from hal_impl.i2c_helpers import I2CSimBase simPort = I2CSimBase() msg = "Using stub simulator for I2C port %s" % port warnings.warn(msg) # Just check for basic functionality assert hasattr(simPort, 'initializeI2C') assert hasattr(simPort, 'closeI2C') self._port = (simPort, port) else: self._port = port self.deviceAddress = deviceAddress hal.initializeI2C(self._port) self.__finalizer = weakref.finalize(self, _freeI2C, self._port) hal.report(hal.UsageReporting.kResourceType_I2C, deviceAddress)
class AngleController(BasePIDComponent): """ When enabled, controls the angle of the robot. """ drivetrain = Drivetrain kP = tunable(0.01 if hal.HALIsSimulation() else 0.1) kI = tunable(0) kD = tunable(0) kF = tunable(0) kToleranceDegrees = tunable(0.5 if hal.HALIsSimulation() else 0.75) kIzone = tunable(0) navx = navx.AHRS def __init__(self): super().__init__(self.get_angle, 'angle_controller') self.last_angle = 0 self.angle_reset_factor = 0 if hal.HALIsSimulation(): self.set_abs_output_range(0.08, 0.25) else: # self.set_abs_output_range(0.13, 0.25) self.set_abs_output_range(0.13, 0.4) if hasattr(self, 'pid'): self.pid.setInputRange(-180.0, 180.0) self.pid.setOutputRange(-1.0, 1.0) self.pid.setContinuous(True) self.report = 0 def get_angle(self): """ Return the robot's current heading. """ # print('current_angle', self.navx.getYaw()) try: self.last_angle = self.navx.getYaw() return self.last_angle - self.angle_reset_factor except Exception as e: print('!!! gyro error, falling back', e) return self.last_angle - self.angle_reset_factor def align_to(self, angle): """ Move the robot and turns it to a specified absolute direction. :param angle: Angle value from 0 to 359 """ self.setpoint = angle def is_aligned(self): """ Return True if robot is pointing at specified angle. Note: Always returns False when move_at_angle is not being called. """ if not self.enabled: return False return self.is_aligned_to(self.setpoint) def is_aligned_to(self, setpoint): """ :param setpoint: Setpoint value to check alignment against :return: True if aligned False if not """ angle = self.get_angle() # compensate for wraparound (code from PIDController) error = setpoint - angle if abs(error) > 180.0: if error > 0: error -= 360.0 else: error += 360.0 return abs(error) < self.kToleranceDegrees def reset_angle(self): self.drivetrain.shift_low_gear() self.angle_reset_factor += self.get_angle() # if hal.HALIsSimulation(): # self.angle_reset_factor += self.get_angle() # else: # self.navx.reset() print('angle_controller#reset_angle', self.get_angle()) def compute_error(self, setpoint, pid_input): """ Compute the error between the setpoint and pid_input. :return: A value in degrees of error. (360 to -360) """ error = pid_input - setpoint if abs(error) > 180.0: # Used to find the closest path to the setpoint if error > 0: error -= 360.0 else: error += 360.0 return error def execute(self): super().execute() # rate will never be None when the component is not enabled if self.rate is not None: if self.is_aligned(): self.stop() else: self.drivetrain.turn(-self.rate, force=True) def stop(self): self.drivetrain.differential_drive(0) def on_disable(self): self.stop()
class PositionController(BasePIDComponent): drivetrain = Drivetrain kP = tunable(0.05) kI = tunable(0) kD = tunable(0.005) kF = tunable(0.0) kToleranceInches = tunable(0.2 if hal.HALIsSimulation() else 0.75) kIzone = tunable(0) angle_controller = AngleController kAngleP = 0.05 if hal.HALIsSimulation() else 0.1 kAngleI = 0 kAngleD = 0 kAngleF = 0 kAngleMax = 0.18 # Angle correction factor angle = 0 def __init__(self): super().__init__(self.get_position, 'position_controller') self.set_abs_output_range(0.18, 0.7) # Angle correction PID controller - used to maintain a straight # heading while the encoders track distance traveled. self._angle_offset = 0 self.angle_pid_controller = PIDController(Kp=self.kAngleP, Ki=self.kAngleI, Kd=self.kAngleD, Kf=self.kAngleF, source=self.get_angle, output=self.pidWriteAngle) self.angle_pid_controller.setInputRange(-180, 180) self.angle_pid_controller.setContinuous(True) self.angle_pid_controller.setOutputRange(-self.kAngleMax, self.kAngleMax) def get_position(self): return self.drivetrain.get_position() def get_angle(self): return self.angle_controller.get_angle() - self._angle_offset def reset_position_and_heading(self): self.drivetrain.shift_low_gear() self.drivetrain.reset_position() self._angle_offset = self.angle_controller.get_angle() self.angle_pid_controller.setSetpoint(0) def move_to(self, position): self.setpoint = position self.angle_pid_controller.enable() def is_at_location(self): return self.enabled and \ abs(self.get_position() - self.setpoint) < self.kToleranceInches def pidWrite(self, output): self.rate = -output def pidWriteAngle(self, angle): self.angle = angle def execute(self): super().execute() if self.rate is not None: if self.is_at_location(): self.stop() else: self.drivetrain.differential_drive(self.rate, self.angle, squared=False, force=True) def stop(self): self.drivetrain.differential_drive(0) self.angle_pid_controller.disable() def on_disable(self): self.stop()
def isSimulation(): """ :returns: If the robot is running in simulation. :rtype: bool """ return hal.HALIsSimulation()
import functools import hal import inspect import networktables from robotpy_ext.misc.orderedclass import OrderedClass from .magic_tunable import tunable if hal.HALIsSimulation(): from wpilib import Timer getTime = Timer.getFPGATimestamp else: import time getTime = time.monotonic class IllegalCallError(Exception): pass class NoFirstStateError(Exception): pass class MultipleFirstStatesError(Exception): pass class MultipleDefaultStatesError(Exception): pass
def isReal(): """ :returns: If the robot is running in the real world. :rtype: bool """ return not hal.HALIsSimulation()