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