def __init__(self, channel): """Allocate a PWM given a channel. :param channel: The PWM channel number. 0-9 are on-board, 10-19 are on the MXP port :type channel: int """ SensorBase.checkPWMChannel(channel) self.channel = channel self._port = hal.initializeDigitalPort(hal.getPort(channel)) self.freq = 1/self.kDefaultPwmPeriod * 1000 print('PWM::__init__ channel: {0}, freq: {1}'.format( channel, self.freq)) if not hal.allocatePWMChannel(self._port): raise IndexError("PWM channel %d is already allocated" % channel) #self.pwmObj = hal_data['pwm'][digital_port.pin]['pwmObj'] # Need this to free on unit test wpilib reset Resource._add_global_resource(self) hal.setPWM(self._port, 0) self.__finalizer = weakref.finalize(self, _freePWM, self._port) self.eliminateDeadband = True hal.HALReport(hal.HALUsageReporting.kResourceType_PWM, channel)
def __init__(self, channel): """Constructor. :param channel: The PWM channel that the SD540 is attached to. 0-9 are on-board, 10-19 are on the MXP port .. note :: Note that the SD540 uses the following bounds for PWM values. These values should work reasonably well for most controllers, but if users experience issues such as asymmetric behavior around the deadband or inability to saturate the controller in either direction, calibration is recommended. The calibration procedure can be found in the SD540 User Manual available from Mindsensors. - 2.05ms = full "forward" - 1.55ms = the "high end" of the deadband range - 1.50ms = center of the deadband range (off) - 1.44ms = the "low end" of the deadband range - .94ms = full "reverse" """ super().__init__(channel) self.setBounds(2.05, 1.55, 1.50, 1.44, .94) self.setPeriodMultiplier(self.PeriodMultiplier.k1X) self.setRaw(self.centerPwm) self.setZeroLatch() self.isInverted = False hal.HALReport(hal.HALUsageReporting.kResourceType_MindsensorsSD540, self.getChannel()) LiveWindow.addActuatorChannel("SD540", self.getChannel(), self)
def __init__(self): """Creates a preference class that will automatically read the file in a different thread. Any call to its methods will be blocked until the thread is finished reading. """ # The actual values (str->str) self.values = {} # The keys in the order they were read from the file self.keylist = [] # The comments that were in the file sorted by which key they appeared # over (str->str) self.comments = {} # The comment at the end of the file self.endComment = "" # The semaphore for beginning reads and writes to the file self.fileLock = threading.Condition() # The semaphore for reading from the table self.lock = threading.RLock() # We synchronized on fileLock and then wait # for it to know that the reading thread has started with self.fileLock: reader = threading.Thread(target=self._read, name="Preferences Read") reader.start() self.fileLock.wait() hal.HALReport(hal.HALUsageReporting.kResourceType_Preferences, 0)
def __init__(self, channel): """Construct an analog output on a specified MXP channel. :param channel: The channel number to represent. """ if not hal.checkAnalogOutputChannel(channel): raise IndexError( "Analog output channel %d cannot be allocated. Channel is not present." % channel) try: AnalogOutput.channels.allocate(self, channel) except IndexError as e: raise IndexError("Analog output channel %d is already allocated" % channel) from e self.channel = channel port = hal.getPort(channel) self._port = hal.initializeAnalogOutputPort(port) LiveWindow.addSensorChannel("AnalogOutput", channel, self) hal.HALReport(hal.HALUsageReporting.kResourceType_AnalogChannel, channel, 1) self.__finalizer = weakref.finalize(self, _freeAnalogOutput, self._port)
def __init__(self, channel): """Constructor for a Talon (original or Talon SR) :param channel: The PWM channel that the Talon is attached to. 0-9 are on-board, 10-19 are on the MXP port :type channel: int .. note :: The Talon uses the following bounds for PWM values. These values should work reasonably well for most controllers, but if users experience issues such as asymmetric behavior around the deadband or inability to saturate the controller in either direction, calibration is recommended. The calibration procedure can be found in the Talon User Manual available from CTRE. - 2.037ms = full "forward" - 1.539ms = the "high end" of the deadband range - 1.513ms = center of the deadband range (off) - 1.487ms = the "low end" of the deadband range - 0.989ms = full "reverse" """ super().__init__(channel) self.setBounds(2.037, 1.539, 1.513, 1.487, 0.989) self.setPeriodMultiplier(self.PeriodMultiplier.k1X) self.setRaw(self.centerPwm) self.setZeroLatch() self.isInverted = False LiveWindow.addActuatorChannel("Talon", self.getChannel(), self) hal.HALReport(hal.HALUsageReporting.kResourceType_Talon, self.getChannel())
def getTable(): if SmartDashboard.table is None: from networktables import NetworkTable SmartDashboard.table = NetworkTable.getTable("SmartDashboard") hal.HALReport(hal.HALUsageReporting.kResourceType_SmartDashboard, hal.HALUsageReporting.kSmartDashboard_Instance) return SmartDashboard.table
def __init__(self, port, range): """Constructor. Use this when the device is the first/only device on the bus :param port: The SPI port that the accelerometer is connected to :type port: :class:`.SPI.Port` :param range: The range (+ or -) that the accelerometer will measure. :type range: :class:`.ADXL345_SPI.Range` """ self.spi = SPI(port) self.spi.setClockRate(500000) self.spi.setMSBFirst() self.spi.setSampleDataOnFalling() self.spi.setClockActiveLow() self.spi.setChipSelectActiveHigh() # Turn on the measurements self.spi.write([self.kPowerCtlRegister, self.kPowerCtl_Measure]) self.setRange(range) hal.HALReport(hal.HALUsageReporting.kResourceType_ADXL345, hal.HALUsageReporting.kADXL345_SPI) LiveWindow.addSensor("ADXL345_SPI", port, self)
def __init__(self, channel): """Constructor. :param channel: The PWM channel that the VictorSP is attached to. 0-9 are on-board, 10-19 are on the MXP port. :type channel: int .. note :: The Talon uses the following bounds for PWM values. These values should work reasonably well for most controllers, but if users experience issues such as asymmetric behavior around the deadband or inability to saturate the controller in either direction, calibration is recommended. The calibration procedure can be found in the VictorSP User Manual. - 2.004ms = full "forward" - 1.520ms = the "high end" of the deadband range - 1.500ms = center of the deadband range (off) - 1.480ms = the "low end" of the deadband range - 0.997ms = full "reverse" """ super().__init__(channel) self.setBounds(2.004, 1.52, 1.50, 1.48, .997) self.setPeriodMultiplier(self.PeriodMultiplier.k1X) # never call the raw functions #self.setRaw(self.centerPwm) self.setZeroLatch() self.isInverted = False LiveWindow.addActuatorChannel("VictorSP", self.getChannel(), self) hal.HALReport(hal.HALUsageReporting.kResourceType_VictorSP, self.getChannel())
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, *args, **kwargs): """Constructor. Arguments can be supplied as positional or keyword. Acceptable positional argument combinations are: - channel - moduleNumber, channel Alternatively, the above names can be used as keyword arguments. :param moduleNumber: The CAN ID of the PCM the solenoid is attached to :type moduleNumber: int :param channel: The channel on the PCM to control (0..7) :type channel: int """ # keyword arguments channel = kwargs.pop("channel", None) moduleNumber = kwargs.pop("moduleNumber", None) if kwargs: warnings.warn("unknown keyword arguments: %s" % kwargs.keys(), RuntimeWarning) # positional arguments if len(args) == 1: channel = args[0] elif len(args) == 2: moduleNumber, channel = args elif len(args) != 0: raise ValueError( "don't know how to handle %d positional arguments" % len(args)) if moduleNumber is None: moduleNumber = SensorBase.getDefaultSolenoidModule() if channel is None: raise ValueError("must specify channel") SensorBase.checkSolenoidModule(moduleNumber) SensorBase.checkSolenoidChannel(channel) super().__init__(moduleNumber) self.channel = channel try: self.allocated.allocate(self, channel) except IndexError as e: raise IndexError( "Solenoid channel %d on module %d is already allocated" % (channel, moduleNumber)) from e self.port = self.ports[channel] LiveWindow.addActuatorModuleChannel("Solenoid", moduleNumber, channel, self) hal.HALReport(hal.HALUsageReporting.kResourceType_Solenoid, channel, moduleNumber)
def __init__(self, range=Accelerometer.Range.k8G): """Constructor. :param range: The range the accelerometer will measure. Defaults to +/-8g if unspecified. :type range: :class:`.Accelerometer.Range` """ self.setRange(range) hal.HALReport(hal.HALUsageReporting.kResourceType_Accelerometer, 0, 0, "Built-in accelerometer") LiveWindow.addSensor("BuiltInAccel", 0, self)
def __init__(self, channel): """Create an instance of a digital output. :param channel: the DIO channel for the digital output. 0-9 are on-board, 10-25 are on the MXP """ super().__init__(channel, False) self._pwmGenerator = None self._pwmGenerator_finalizer = None hal.HALReport(hal.HALUsageReporting.kResourceType_DigitalOutput, channel)
def __init__(self, port): ''' :param port: The port on the driver station that the controller is plugged into. :type port: int ''' self.ds = wpilib.DriverStation.getInstance() self.port = port hal.HALReport(hal.HALUsageReporting.kResourceType_Joystick, port)
def _initRelay(self): SensorBase.checkRelayChannel(self.channel) try: if (self.direction == self.Direction.kBoth or self.direction == self.Direction.kForward): Relay.relayChannels.allocate(self, self.channel * 2) hal.HALReport(hal.HALUsageReporting.kResourceType_Relay, self.channel) if (self.direction == self.Direction.kBoth or self.direction == self.Direction.kReverse): Relay.relayChannels.allocate(self, self.channel * 2 + 1) hal.HALReport(hal.HALUsageReporting.kResourceType_Relay, self.channel + 128) except IndexError as e: raise IndexError("Relay channel %d is already allocated" % self.channel) from e self._port = hal.initializeDigitalPort(hal.getPort(self.channel)) self._port_finalizer = weakref.finalize(self, _freeRelay, self._port) self.setSafetyEnabled(False)
def __init__(self, channel): """Create an instance of a Digital Input class. Creates a digital input given a channel. :param channel: the DIO channel for the digital input. 0-9 are on-board, 10-25 are on the MXP :type channel: int """ super().__init__(channel, True) hal.HALReport(hal.HALUsageReporting.kResourceType_DigitalInput, channel) LiveWindow.addSensorChannel("DigitalInput", channel, self)
def __init__(self, channel, directionSensitive=False): """Construct a GearTooth sensor. :param channel: The DIO channel index or DigitalSource that the sensor is connected to. :type channel: int :param directionSensitive: True to enable the pulse length decoding in hardware to specify count direction. Defaults to False. :type directionSensitive: bool """ super().__init__(channel) self.enableDirectionSensing(directionSensitive) if hasattr(self.upSource, "getChannel"): if directionSensitive: hal.HALReport(hal.HALUsageReporting.kResourceType_GearTooth, self.upSource.getChannel(), 0, "D") else: hal.HALReport(hal.HALUsageReporting.kResourceType_GearTooth, self.upSource.getChannel(), 0) LiveWindow.addSensorChannel("GearTooth", self.upSource.getChannel(), self)
def startCompetition(self): """Start a competition. This code tracks the order of the field starting to ensure that everything happens in the right order. Repeatedly run the correct method, either Autonomous or OperatorControl when the robot is enabled. After running the correct method, wait for some state to change, either the other mode starts or the robot is disabled. Then go back and wait for the robot to be enabled again. """ hal.HALReport(hal.HALUsageReporting.kResourceType_Framework, hal.HALUsageReporting.kFramework_Simple) self.robotMain() if hasattr(self, '_no_robot_main'): # first and one-time initialization LiveWindow.setEnabled(False) self.robotInit() #We call this now (not in prestart like default) so that the robot #won't enable until the initialization has finished. This is useful #because otherwise it's sometimes possible to enable the robot before #the code is ready. hal.HALNetworkCommunicationObserveUserProgramStarting() while True: if self.isDisabled(): self.ds.InDisabled(True) self.disabled() self.ds.InDisabled(False) while self.isDisabled(): Timer.delay(0.01) elif self.isAutonomous(): self.ds.InAutonomous(True) self.autonomous() self.ds.InAutonomous(False) while self.isAutonomous() and not self.isDisabled(): Timer.delay(0.01) elif self.isTest(): LiveWindow.setEnabled(True) self.ds.InTest(True) self.test() self.ds.InTest(False) while self.isTest() and self.isEnabled(): Timer.delay(0.01) LiveWindow.setEnabled(False) else: self.ds.InOperatorControl(True) self.operatorControl() self.ds.InOperatorControl(False) while self.isOperatorControl() and not self.isDisabled(): Timer.delay(0.01)
def __init__(self, port, deviceAddress): """Constructor. :param port: The I2C port the device is connected to. :param deviceAddress: The address of the device on the I2C bus. """ self.port = port self.deviceAddress = deviceAddress hal.i2CInitialize(self.port) self._i2c_finalizer = weakref.finalize(self, _freeI2C, self.port) hal.HALReport(hal.HALUsageReporting.kResourceType_I2C, deviceAddress)
def __init__(self): self.channelIndex = -1 with self.mutex: for i, v in enumerate(self.filterAllocated): if not v: self.channelIndex = i self.filterAllocated[i] = True hal.HALReport( hal.HALUsageReporting.kResourceType_DigitalFilter, self.channelIndex, 0) break else: raise ValueError("No more filters available")
def __init__(self, pingChannel, echoChannel, units=Unit.kInches): """Create an instance of the Ultrasonic Sensor. This is designed to supchannel the Daventech SRF04 and Vex ultrasonic sensors. :param pingChannel: The digital output channel that sends the pulse to initiate the sensor sending the ping. :param echoChannel: The digital input channel that receives the echo. The length of time that the echo is high represents the round trip time of the ping, and the distance. :param units: The units returned in either kInches or kMillimeters """ # Convert to DigitalInput and DigitalOutput if necessary self.pingAllocated = False self.echoAllocated = False if not hasattr(pingChannel, 'channel'): from .digitaloutput import DigitalOutput pingChannel = DigitalOutput(pingChannel) self.pingAllocated = True if not hasattr(echoChannel, 'channel'): from .digitalinput import DigitalInput echoChannel = DigitalInput(echoChannel) self.echoAllocated = True self.pingChannel = pingChannel self.echoChannel = echoChannel self.units = units self.pidSource = self.PIDSourceType.kDisplacement self.enabled = True # make it available for round robin scheduling # set up counter for this sensor self.counter = Counter(self.echoChannel) self.counter.setMaxPeriod(1.0) self.counter.setSemiPeriodMode(True) self.counter.reset() isAutomatic = Ultrasonic.isAutomaticMode() self.setAutomaticMode(False) Ultrasonic.sensors.add(self) if isAutomatic: self.setAutomaticMode(True) Resource._add_global_resource(self) Ultrasonic.instances += 1 hal.HALReport(hal.HALUsageReporting.kResourceType_Ultrasonic, Ultrasonic.instances) LiveWindow.addSensor("Ultrasonic", self.echoChannel.getChannel(), self)
def mecanumDrive_Cartesian(self, x, y, rotation, gyroAngle): """Drive method for Mecanum wheeled robots. A method for driving with Mecanum wheeled robots. There are 4 wheels on the robot, arranged so that the front and back wheels are toed in 45 degrees. When looking at the wheels from the top, the roller axles should form an X across the robot. This is designed to be directly driven by joystick axes. :param x: The speed that the robot should drive in the X direction. [-1.0..1.0] :param y: The speed that the robot should drive in the Y direction. This input is inverted to match the forward == -1.0 that joysticks produce. [-1.0..1.0] :param rotation: The rate of rotation for the robot that is completely independent of the translation. [-1.0..1.0] :param gyroAngle: The current angle reading from the gyro. Use this to implement field-oriented controls. """ if not RobotDrive.kMecanumCartesian_Reported: hal.HALReport(hal.HALUsageReporting.kResourceType_RobotDrive, self.getNumMotors(), hal.HALUsageReporting.kRobotDrive_MecanumCartesian) RobotDrive.kMecanumCartesian_Reported = True xIn = x yIn = y # Negate y for the joystick. yIn = -yIn # Compenstate for gyro angle. xIn, yIn = RobotDrive.rotateVector(xIn, yIn, gyroAngle) wheelSpeeds = [0]*self.kMaxNumberOfMotors wheelSpeeds[self.MotorType.kFrontLeft] = xIn + yIn + rotation wheelSpeeds[self.MotorType.kFrontRight] = -xIn + yIn - rotation wheelSpeeds[self.MotorType.kRearLeft] = -xIn + yIn + rotation wheelSpeeds[self.MotorType.kRearRight] = xIn + yIn - rotation RobotDrive.normalize(wheelSpeeds) self.frontLeftMotor.set(wheelSpeeds[self.MotorType.kFrontLeft] * self.maxOutput, self.syncGroup) self.frontRightMotor.set(wheelSpeeds[self.MotorType.kFrontRight] * self.maxOutput, self.syncGroup) self.rearLeftMotor.set(wheelSpeeds[self.MotorType.kRearLeft] * self.maxOutput, self.syncGroup) self.rearRightMotor.set(wheelSpeeds[self.MotorType.kRearRight] * self.maxOutput, self.syncGroup) if self.syncGroup != 0: CANJaguar.updateSyncGroup(self.syncGroup) self.feed()
def __init__(self, channel): """Constructor. :param channel: The PWM channel that the Victor is attached to. 0-9 are on-board, 10-19 are on the MXP port :type channel: int """ super().__init__(channel) self.setBounds(2.027, 1.525, 1.507, 1.49, 1.026) self.setPeriodMultiplier(self.PeriodMultiplier.k2X) self.setRaw(self.centerPwm) self.setZeroLatch() LiveWindow.addActuatorChannel("Victor", self.getChannel(), self) hal.HALReport(hal.HALUsageReporting.kResourceType_Victor, self.getChannel())
def __init__(self, channel): """Create a new instance of Accelerometer from either an existing AnalogChannel or from an analog channel port index. :param channel: port index or an already initialized :class:`.AnalogInput` """ if not hasattr(channel, "getAverageVoltage"): channel = AnalogInput(channel) self.analogChannel = channel self.voltsPerG = 1.0 self.zeroGVoltage = 2.5 hal.HALReport(hal.HALUsageReporting.kResourceType_Accelerometer, self.analogChannel.getChannel()) LiveWindow.addSensorChannel("Accelerometer", self.analogChannel.getChannel(), self)
def __init__(self, trigger, outputType): """Create an object that represents one of the four outputs from an analog trigger. Because this class derives from DigitalSource, it can be passed into routing functions for Counter, Encoder, etc. :param trigger: The trigger for which this is an output. :param outputType: An enum that specifies the output on the trigger to represent. """ self.trigger = trigger self.outputType = outputType hal.HALReport(hal.HALUsageReporting.kResourceType_AnalogTriggerOutput, trigger.index, outputType)
def __init__(self, port, range): """Constructor. :param port: The I2C port the accelerometer is attached to. :param range: The range (+ or -) that the accelerometer will measure. """ self.i2c = I2C(port, self.kAddress) # Turn on the measurements self.i2c.write(self.kPowerCtlRegister, self.kPowerCtl_Measure) self.setRange(range) hal.HALReport(hal.HALUsageReporting.kResourceType_ADXL345, hal.HALUsageReporting.kADXL345_I2C) LiveWindow.addSensor("ADXL345_I2C", port, self)
def drive(self, outputMagnitude, curve): """Drive the motors at "outputMagnitude" and "curve". Both outputMagnitude and curve are -1.0 to +1.0 values, where 0.0 represents stopped and not turning. ``curve < 0`` will turn left and ``curve > 0`` will turn right. The algorithm for steering provides a constant turn radius for any normal speed range, both forward and backward. Increasing m_sensitivity causes sharper turns for fixed values of curve. This function will most likely be used in an autonomous routine. :param outputMagnitude: The speed setting for the outside wheel in a turn, forward or backwards, +1 to -1. :param curve: The rate of turn, constant for different forward speeds. Set ``curve < 0`` for left turn or ``curve > 0`` for right turn. Set ``curve = e^(-r/w)`` to get a turn radius r for wheelbase w of your robot. Conversely, turn radius r = -ln(curve)*w for a given value of curve and wheelbase w. """ if not RobotDrive.kArcadeRatioCurve_Reported: hal.HALReport(hal.HALUsageReporting.kResourceType_RobotDrive, self.getNumMotors(), hal.HALUsageReporting.kRobotDrive_ArcadeRatioCurve) RobotDrive.kArcadeRatioCurve_Reported = True if curve < 0: value = math.log(-curve) ratio = (value - self.sensitivity) / (value + self.sensitivity) if ratio == 0: ratio = .0000000001 leftOutput = outputMagnitude / ratio rightOutput = outputMagnitude elif curve > 0: value = math.log(curve) ratio = (value - self.sensitivity) / (value + self.sensitivity) if ratio == 0: ratio = .0000000001 leftOutput = outputMagnitude rightOutput = outputMagnitude / ratio else: leftOutput = outputMagnitude rightOutput = outputMagnitude self.setLeftRightMotorOutputs(leftOutput, rightOutput)
def __init__(self, channel): """Constructor. * By default `kDefaultMaxServoPWM` ms is used as the maxPWM value * By default `kDefaultMinServoPWM` ms is used as the minPWM value :param channel: The PWM channel to which the servo is attached. 0-9 are on-board, 10-19 are on the MXP port. :type channel: int """ super().__init__(channel) self.setBounds(self.kDefaultMaxServoPWM, 0, 0, 0, self.kDefaultMinServoPWM) self.setPeriodMultiplier(self.PeriodMultiplier.k4X) LiveWindow.addActuatorChannel("Servo", self.getChannel(), self) hal.HALReport(hal.HALUsageReporting.kResourceType_Servo, self.getChannel())
def mecanumDrive_Polar(self, magnitude, direction, rotation): """Drive method for Mecanum wheeled robots. A method for driving with Mecanum wheeled robots. There are 4 wheels on the robot, arranged so that the front and back wheels are toed in 45 degrees. When looking at the wheels from the top, the roller axles should form an X across the robot. :param magnitude: The speed that the robot should drive in a given direction. :param direction: The direction the robot should drive in degrees. The direction and maginitute are independent of the rotation rate. :param rotation: The rate of rotation for the robot that is completely independent of the magnitute or direction. [-1.0..1.0] """ if not RobotDrive.kMecanumPolar_Reported: hal.HALReport(hal.HALUsageReporting.kResourceType_RobotDrive, self.getNumMotors(), hal.HALUsageReporting.kRobotDrive_MecanumPolar) RobotDrive.kMecanumPolar_Reported = True # Normalized for full power along the Cartesian axes. magnitude = RobotDrive.limit(magnitude) * math.sqrt(2.0) # The rollers are at 45 degree angles. dirInRad = math.radians(direction + 45.0) cosD = math.cos(dirInRad) sinD = math.sin(dirInRad) wheelSpeeds = [0]*self.kMaxNumberOfMotors wheelSpeeds[self.MotorType.kFrontLeft] = sinD * magnitude + rotation wheelSpeeds[self.MotorType.kFrontRight] = cosD * magnitude - rotation wheelSpeeds[self.MotorType.kRearLeft] = cosD * magnitude + rotation wheelSpeeds[self.MotorType.kRearRight] = sinD * magnitude - rotation RobotDrive.normalize(wheelSpeeds) self.frontLeftMotor.set(wheelSpeeds[self.MotorType.kFrontLeft] * self.invertedMotors[self.MotorType.kFrontLeft] * self.maxOutput, self.syncGroup) self.frontRightMotor.set(wheelSpeeds[self.MotorType.kFrontRight] * self.invertedMotors[self.MotorType.kFrontRight] * self.maxOutput, self.syncGroup) self.rearLeftMotor.set(wheelSpeeds[self.MotorType.kRearLeft] * self.invertedMotors[self.MotorType.kRearLeft] * self.maxOutput, self.syncGroup) self.rearRightMotor.set(wheelSpeeds[self.MotorType.kRearRight] * self.invertedMotors[self.MotorType.kRearRight] * self.maxOutput, self.syncGroup) if self.syncGroup != 0: CANJaguar.updateSyncGroup(self.syncGroup) self.feed()
def __init__(self): """Instantiates a Scheduler. """ hal.HALReport(hal.HALUsageReporting.kResourceType_Command, hal.HALUsageReporting.kCommand_Scheduler) # Active Commands self.commandTable = collections.OrderedDict() # The set of all Subsystems self.subsystems = set() # Whether or not we are currently adding a command self.adding = False # Whether or not we are currently disabled self.disabled = False # A list of all Commands which need to be added self.additions = [] # A list of all Buttons. It is created lazily. self.buttons = [] self.runningCommandsChanged = False
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, 'spiInitialize') assert hasattr(simPort, 'spiClose') self._port = (simPort, port) else: self._port = port self.bitOrder = 0 self.clockPolarity = 0 self.dataOnTrailing = 0 hal.spiInitialize(self._port) self.__finalizer = weakref.finalize(self, _freeSPI, self._port) SPI.devices += 1 hal.HALReport(hal.HALUsageReporting.kResourceType_SPI, SPI.devices)