Permalink

Comparing changes

Choose two branches to see what’s changed or to start a new pull request. If you need to, you can also .

Open a pull request

Create a new pull request by comparing changes across two branches. If you need to, you can also .
...
  • 19 commits
  • 38 files changed
  • 0 commit comments
  • 3 contributors
Showing with 813 additions and 199 deletions.
  1. +3 −0 .gitignore
  2. +17 −1 docs/guide/sim_and_tests.rst
  3. +3 −1 docs/support.rst
  4. +64 −0 examples/examples/Gyro/robot.py
  5. +37 −34 hal-base/hal/functions.py
  6. +4 −0 hal-roborio/hal_impl/fndef.py
  7. +1 −1 hal-roborio/setup.py
  8. +21 −3 hal-sim/hal_impl/data.py
  9. +24 −9 hal-sim/hal_impl/fndef.py
  10. +19 −3 hal-sim/hal_impl/functions.py
  11. +40 −0 hal-sim/hal_impl/i2c_helpers.py
  12. +135 −0 hal-sim/hal_impl/spi_helpers.py
  13. +93 −104 installer/installer.py
  14. +1 −1 wpilib/setup.py
  15. +1 −1 wpilib/tests/run_tests.sh
  16. +69 −0 wpilib/tests/test_i2c.py
  17. +129 −0 wpilib/tests/test_spi.py
  18. +3 −3 wpilib/wpilib/adxl362.py
  19. +16 −7 wpilib/wpilib/adxrs450_gyro.py
  20. +7 −6 wpilib/wpilib/canjaguar.py
  21. +1 −0 wpilib/wpilib/command/command.py
  22. +1 −0 wpilib/wpilib/command/commandgroup.py
  23. +1 −0 wpilib/wpilib/command/pidcommand.py
  24. +1 −0 wpilib/wpilib/command/pidsubsystem.py
  25. +1 −0 wpilib/wpilib/command/printcommand.py
  26. +1 −0 wpilib/wpilib/command/scheduler.py
  27. +1 −0 wpilib/wpilib/command/startcommand.py
  28. +1 −0 wpilib/wpilib/command/subsystem.py
  29. +1 −0 wpilib/wpilib/command/waitcommand.py
  30. +1 −0 wpilib/wpilib/command/waitforchildren.py
  31. +1 −0 wpilib/wpilib/command/waituntilcommand.py
  32. +2 −8 wpilib/wpilib/counter.py
  33. +9 −1 wpilib/wpilib/driverstation.py
  34. +33 −12 wpilib/wpilib/i2c.py
  35. +1 −0 wpilib/wpilib/interfaces/__init__.py
  36. +45 −0 wpilib/wpilib/interfaces/pidinterface.py
  37. +2 −2 wpilib/wpilib/robotbase.py
  38. +23 −2 wpilib/wpilib/spi.py
View
@@ -49,3 +49,6 @@ parsetab.py
# Downloaded libHALAthena
/hal-roborio/hal_impl/libHALAthena.so
/wpilib/tests/__hal__
+
+# deploy files
+.deploy_cfg
@@ -9,7 +9,23 @@ test it! Because we feel strongly about testing and simulation, the RobotPy
project provides tools to make those types of things easier through the
`pyfrc <https://github.com/robotpy/pyfrc>`_ project.
-To get started, check out the `pyfrc documentation <http://pyfrc.readthedocs.org>`_.
+Adding tests to your robot
+--------------------------
+
+pyfrc comes with builtin tests that you can add to your robot code that will
+test basic functionality of most robots. As of pyfrc 2016.1.1, you can add
+these tests to your robot by executing this:
+
+.. code-block:: sh
+
+ Windows: py robot.py add-tests
+
+ Linux/OSX: python3 robot.py add-tests
+
+Customized tests
+----------------
+
+For more detailed information, check out the `pyfrc documentation <http://pyfrc.readthedocs.org>`_.
Next Steps
----------
View
@@ -51,8 +51,10 @@ IRC
During the FRC Build Season, some RobotPy developers may be able to be reached on
**#robotpy** channel on `Freenode <http://freenode.net/irc_servers.shtml>`_.
-.. note:: the channel is not very active, but if you stick around long enough
+.. note:: the channel is not very active, but if you stick around for a day or two
someone will probably answer your question -- think in terms of email
response time
+
+ The channel tends to be most active between 11pm and 1am EST.
@@ -0,0 +1,64 @@
+#!/usr/bin/env python3
+
+import wpilib
+
+class MyRobot(wpilib.SampleRobot):
+ '''This is a demo program showing how to use Gyro control with the
+ RobotDrive class.'''
+
+ def robotInit(self):
+ '''Robot initialization function'''
+ gyroChannel = 0 #analog input
+
+ self.joystickChannel = 0 #usb number in DriverStation
+
+ #channels for motors
+ self.leftMotorChannel = 1
+ self.rightMotorChannel = 0
+ self.leftRearMotorChannel = 3
+ self.rightRearMotorChannel = 2
+
+ self.angleSetpoint = 0.0
+ self.pGain = 1 #propotional turning constant
+
+ #gyro calibration constant, may need to be adjusted
+ #gyro value of 360 is set to correspond to one full revolution
+ self.voltsPerDegreePerSecond = .0128
+
+ self.myRobot = wpilib.RobotDrive(wpilib.CANTalon(self.leftMotorChannel),
+ wpilib.CANTalon(self.leftRearMotorChannel),
+ wpilib.CANTalon(self.rightMotorChannel),
+ wpilib.CANTalon(self.rightRearMotorChannel))
+
+ self.gyro = wpilib.AnalogGyro(gyroChannel)
+ self.joystick = wpilib.Joystick(self.joystickChannel)
+
+ def autonomous(self):
+ '''Runs during Autonomous'''
+ pass
+
+
+ def operatorControl(self):
+ '''
+ Sets the gyro sensitivity and drives the robot when the joystick is pushed. The
+ motor speed is set from the joystick while the RobotDrive turning value is assigned
+ from the error between the setpoint and the gyro angle.
+ '''
+
+ self.gyro.setSensitivity(self.voltsPerDegreePerSecond) #calibrates gyro values to equal degrees
+ while self.isOperatorControl() and self.isEnabled():
+ turningValue = (self.angleSetpoint - self.gyro.getAngle())*self.pGain
+ if self.joystick.getY() <= 0:
+ #forwards
+ self.myRobot.drive(self.joystick.getY(), turningValue)
+ elif self.joystick.getY() > 0:
+ #backwards
+ self.myRobot.drive(self.joystick.getY(), -turningValue)
+ wpilib.Timer.delay(0.005)
+
+ def test(self):
+ '''Runs during test mode'''
+ pass
+
+if __name__ == "__main__":
+ wpilib.run(MyRobot)
@@ -6,7 +6,7 @@
from .constants import *
from hal_impl.types import *
-from hal_impl.fndef import _RETFUNC, _VAR, _dll
+from hal_impl.fndef import _RETFUNC, _THUNKFUNC, _VAR, _dll
from hal_impl import __hal_simulation__
def hal_wrapper(f):
@@ -20,7 +20,7 @@ def hal_wrapper(f):
return f
def _STATUSFUNC(name, restype, *params, out=None, library=_dll,
- handle_missing=False):
+ handle_missing=False, _inner_func=_RETFUNC):
realparams = list(params)
realparams.append(("status", C.POINTER(C.c_int32)))
if restype is not None and out is not None:
@@ -31,8 +31,8 @@ def errcheck(rv, f, args):
return tuple(out)
else:
errcheck = None
- _inner = _RETFUNC(name, restype, *realparams, out=out, library=library,
- errcheck=errcheck, handle_missing=handle_missing)
+ _inner = _inner_func(name, restype, *realparams, out=out, library=library,
+ errcheck=errcheck, handle_missing=handle_missing)
def outer(*args, **kwargs):
status = C.c_int32(0)
rv = _inner(*args, status=status, **kwargs)
@@ -49,6 +49,9 @@ def outer(*args, **kwargs):
outer.fndata = _inner.fndata
return outer
+def _TSTATUSFUNC(*a, **k):
+ return _STATUSFUNC(_inner_func=_THUNKFUNC, *a, **k)
+
def _CTRFUNC_errcheck(result, func, args):
if result != 0:
warnings.warn(getHALErrorMessage(result))
@@ -331,9 +334,9 @@ def HALIsSimulation():
getLoopTiming = _STATUSFUNC("getLoopTiming", C.c_uint16)
-spiInitialize = _STATUSFUNC("spiInitialize", None, ("port", C.c_uint8))
+spiInitialize = _TSTATUSFUNC("spiInitialize", None, ("port", C.c_uint8))
-_spiTransaction = _RETFUNC("spiTransaction", C.c_int32, ("port", C.c_uint8),
+_spiTransaction = _THUNKFUNC("spiTransaction", C.c_int32, ("port", C.c_uint8),
("data_to_send", C.POINTER(C.c_uint8)), ("data_received", C.POINTER(C.c_uint8)), ("size", C.c_uint8))
@hal_wrapper
def spiTransaction(port, data_to_send):
@@ -345,7 +348,7 @@ def spiTransaction(port, data_to_send):
raise IOError(_os.strerror(C.get_errno()))
return recv_buffer[:rv]
-_spiWrite = _RETFUNC("spiWrite", C.c_int32, ("port", C.c_uint8), ("data_to_send", C.POINTER(C.c_uint8)), ("send_size", C.c_uint8))
+_spiWrite = _THUNKFUNC("spiWrite", C.c_int32, ("port", C.c_uint8), ("data_to_send", C.POINTER(C.c_uint8)), ("send_size", C.c_uint8))
@hal_wrapper
def spiWrite(port, data_to_send):
send_size = len(data_to_send)
@@ -355,56 +358,56 @@ def spiWrite(port, data_to_send):
raise IOError(_os.strerror(C.get_errno()))
return rv
-_spiRead = _RETFUNC("spiRead", C.c_int32, ("port", C.c_uint8), ("buffer", C.POINTER(C.c_uint8)), ("count", C.c_uint8))
+_spiRead = _THUNKFUNC("spiRead", C.c_int32, ("port", C.c_uint8), ("buffer", C.POINTER(C.c_uint8)), ("count", C.c_uint8))
@hal_wrapper
def spiRead(port, count):
- buffer = C.c_uint8 * count
+ buffer = (C.c_uint8 * count)()
rv = _spiRead(port, buffer, count)
if rv < 0:
raise IOError(_os.strerror(C.get_errno()))
return [x for x in buffer]
-spiClose = _RETFUNC("spiClose", None, ("port", C.c_uint8))
-spiSetSpeed = _RETFUNC("spiSetSpeed", None, ("port", C.c_uint8), ("speed", C.c_uint32))
-#spiSetBitsPerWord = _RETFUNC("spiSetBitsPerWord", None, ("port", C.c_uint8), ("bpw", C.c_uint8))
-spiSetOpts = _RETFUNC("spiSetOpts", None, ("port", C.c_uint8), ("msb_first", C.c_int), ("sample_on_trailing", C.c_int), ("clk_idle_high", C.c_int))
-spiSetChipSelectActiveHigh = _STATUSFUNC("spiSetChipSelectActiveHigh", None, ("port", C.c_uint8))
-spiSetChipSelectActiveLow = _STATUSFUNC("spiSetChipSelectActiveLow", None, ("port", C.c_uint8))
-spiGetHandle = _RETFUNC("spiGetHandle", C.c_int32, ("port", C.c_uint8));
-spiSetHandle = _RETFUNC("spiSetHandle", None, ("port", C.c_uint8), ("handle", C.c_int32))
+spiClose = _THUNKFUNC("spiClose", None, ("port", C.c_uint8))
+spiSetSpeed = _THUNKFUNC("spiSetSpeed", None, ("port", C.c_uint8), ("speed", C.c_uint32))
+#spiSetBitsPerWord = _THUNKFUNC("spiSetBitsPerWord", None, ("port", C.c_uint8), ("bpw", C.c_uint8))
+spiSetOpts = _THUNKFUNC("spiSetOpts", None, ("port", C.c_uint8), ("msb_first", C.c_int), ("sample_on_trailing", C.c_int), ("clk_idle_high", C.c_int))
+spiSetChipSelectActiveHigh = _TSTATUSFUNC("spiSetChipSelectActiveHigh", None, ("port", C.c_uint8))
+spiSetChipSelectActiveLow = _TSTATUSFUNC("spiSetChipSelectActiveLow", None, ("port", C.c_uint8))
+spiGetHandle = _THUNKFUNC("spiGetHandle", C.c_int32, ("port", C.c_uint8));
+spiSetHandle = _THUNKFUNC("spiSetHandle", None, ("port", C.c_uint8), ("handle", C.c_int32))
-spiInitAccumulator = _STATUSFUNC('spiInitAccumulator', None, ("port", C.c_uint8),
+spiInitAccumulator = _TSTATUSFUNC('spiInitAccumulator', None, ("port", C.c_uint8),
('period', C.c_uint32), ('cmd', C.c_uint32), ('xfer_size', C.c_uint8),
('valid_mask', C.c_uint32), ('valid_value', C.c_uint32), ('data_shift', C.c_uint8),
('data_size', C.c_uint8), ('is_signed', C.c_bool), ('big_endian', C.c_bool))
-spiFreeAccumulator = _STATUSFUNC('spiFreeAccumulator', None, ("port", C.c_uint8))
-spiResetAccumulator = _STATUSFUNC('spiResetAccumulator', None, ("port", C.c_uint8))
-spiSetAccumulatorCenter = _STATUSFUNC('spiSetAccumulatorCenter', None, ("port", C.c_uint8), ('center', C.c_int32))
-spiSetAccumulatorDeadband = _STATUSFUNC('spiSetAccumulatorDeadband', None, ("port", C.c_uint8), ('deadband', C.c_int32))
-spiGetAccumulatorLastValue = _STATUSFUNC('spiGetAccumulatorLastValue', C.c_int32, ("port", C.c_uint8))
-spiGetAccumulatorValue = _STATUSFUNC('spiGetAccumulatorValue', C.c_int64, ("port", C.c_uint8))
-spiGetAccumulatorCount = _STATUSFUNC('spiGetAccumulatorCount', C.c_uint32, ("port", C.c_uint8))
-spiGetAccumulatorAverage = _STATUSFUNC('spiGetAccumulatorAverage', C.c_double, ("port", C.c_uint8))
-spiGetAccumulatorOutput = _STATUSFUNC('spiGetAccumulatorOutput', None, ("port", C.c_uint8), ('value', C.POINTER(C.c_int64)), ('count', C.POINTER(C.c_uint32)), out=['value', 'count'])
+spiFreeAccumulator = _TSTATUSFUNC('spiFreeAccumulator', None, ("port", C.c_uint8))
+spiResetAccumulator = _TSTATUSFUNC('spiResetAccumulator', None, ("port", C.c_uint8))
+spiSetAccumulatorCenter = _TSTATUSFUNC('spiSetAccumulatorCenter', None, ("port", C.c_uint8), ('center', C.c_int32))
+spiSetAccumulatorDeadband = _TSTATUSFUNC('spiSetAccumulatorDeadband', None, ("port", C.c_uint8), ('deadband', C.c_int32))
+spiGetAccumulatorLastValue = _TSTATUSFUNC('spiGetAccumulatorLastValue', C.c_int32, ("port", C.c_uint8))
+spiGetAccumulatorValue = _TSTATUSFUNC('spiGetAccumulatorValue', C.c_int64, ("port", C.c_uint8))
+spiGetAccumulatorCount = _TSTATUSFUNC('spiGetAccumulatorCount', C.c_uint32, ("port", C.c_uint8))
+spiGetAccumulatorAverage = _TSTATUSFUNC('spiGetAccumulatorAverage', C.c_double, ("port", C.c_uint8))
+spiGetAccumulatorOutput = _TSTATUSFUNC('spiGetAccumulatorOutput', None, ("port", C.c_uint8), ('value', C.POINTER(C.c_int64)), ('count', C.POINTER(C.c_uint32)), out=['value', 'count'])
-i2CInitialize = _STATUSFUNC("i2CInitialize", None, ("port", C.c_uint8))
+i2CInitialize = _TSTATUSFUNC("i2CInitialize", None, ("port", C.c_uint8))
-_i2CTransaction = _RETFUNC("i2CTransaction", C.c_int32, ("port", C.c_uint8), ("device_address", C.c_uint8),
+_i2CTransaction = _THUNKFUNC("i2CTransaction", C.c_int32, ("port", C.c_uint8), ("device_address", C.c_uint8),
("data_to_send", C.POINTER(C.c_uint8)), ("send_size", C.c_uint8),
("data_received", C.POINTER(C.c_uint8)), ("receive_size", C.c_uint8))
@hal_wrapper
def i2CTransaction(port, device_address, data_to_send, receive_size):
send_size = len(data_to_send)
send_buffer = (C.c_uint8 * send_size)(*data_to_send)
- recv_buffer = C.c_uint8 * receive_size
+ recv_buffer = (C.c_uint8 * receive_size)()
rv = _i2CTransaction(port, device_address, send_buffer, send_size, recv_buffer, receive_size)
if rv < 0:
raise IOError(_os.strerror(C.get_errno()))
return [x for x in recv_buffer]
-_i2CWrite = _RETFUNC("i2CWrite", C.c_int32, ("port", C.c_uint8), ("device_address", C.c_uint8), ("data_to_send", C.POINTER(C.c_uint8)), ("send_size", C.c_uint8))
+_i2CWrite = _THUNKFUNC("i2CWrite", C.c_int32, ("port", C.c_uint8), ("device_address", C.c_uint8), ("data_to_send", C.POINTER(C.c_uint8)), ("send_size", C.c_uint8))
@hal_wrapper
def i2CWrite(port, device_address, data_to_send):
send_size = len(data_to_send)
@@ -413,16 +416,16 @@ def i2CWrite(port, device_address, data_to_send):
if rv < 0:
raise IOError(_os.strerror(C.get_errno()))
-_i2CRead = _RETFUNC("i2CRead", C.c_int32, ("port", C.c_uint8), ("device_address", C.c_uint8), ("buffer", C.POINTER(C.c_uint8)), ("count", C.c_uint8))
+_i2CRead = _THUNKFUNC("i2CRead", C.c_int32, ("port", C.c_uint8), ("device_address", C.c_uint8), ("buffer", C.POINTER(C.c_uint8)), ("count", C.c_uint8))
@hal_wrapper
def i2CRead(port, device_address, count):
- buffer = C.c_uint8 * count
+ buffer = (C.c_uint8 * count)()
rv = _i2CRead(port, device_address, buffer, count)
if rv < 0:
raise IOError(_os.strerror(C.get_errno()))
return [x for x in buffer]
-i2CClose = _RETFUNC("i2CClose", None, ("port", C.c_uint8))
+i2CClose = _THUNKFUNC("i2CClose", None, ("port", C.c_uint8))
#############################################################################
# Interrupts
@@ -30,5 +30,9 @@ def func(*args, **kwargs):
raise NotImplementedError
return func
+# Thunkfunc is specialized in simulation, but the same as _RETFUNC
+# outside of simulation
+_THUNKFUNC = _RETFUNC
+
def _VAR(name, type, library=_dll):
return type.in_dll(library, name)
@@ -9,7 +9,7 @@
git_dir = join(setup_dir, '..', '.git')
base_package = 'hal_impl'
version_file = join(setup_dir, base_package, 'version.py')
-hal_version = 'jenkins-release-2016.405'
+hal_version = 'jenkins-release-2016.413'
hal_site = 'http://www.tortall.net/~robotpy/hal'
hal_base_file = 'libHALAthena.so'
hal_file = join(setup_dir, base_package, hal_base_file)
@@ -168,13 +168,31 @@ def _reset_hal_data(hooks):
# built-in accelerometer on roboRIO
'accelerometer': {
- 'has_source': IN(False),
- 'active': OUT(False),
- 'range': OUT(0),
+ 'has_source': IN(False), # built-in accelerometer only
+ 'active': OUT(False), # built-in accelerometer only
+ 'range': OUT(0), # built-in accelerometer only
+
+ # These should be used by other simulation components when
+ # appropriate
'x': IN(0),
'y': IN(0),
'z': IN(0),
},
+
+ # Generic robot information to be used by sim components when
+ # appropriate... some components can add custom data here.
+ #
+ # The way this works is that each device will define its own key(s)
+ # here, and that you set the values that it's expecting. Refer to the
+ # sim device's documentation to figure out the right key.
+ #
+ # By convention, it should be devicename_bus_port_thing, such as
+ # "adxrs450_i2c_0_angle"
+ #
+ # Sim devices should read these values with defaults of 0, as they may
+ # not exist.
+ 'robot': {
+ },
# global for all
'analog_sample_rate': OUT(1024.0), # need a better default
Oops, something went wrong.

No commit comments for this range