def __init__(self, devId, param): ParamBuildResponse.__init__(self, Button, devId, param.getSwitchPubHz(devId)) self._length = RES_LEN self._checkSum = 0 self._port = param.getSwitchPort(devId) self._checkSum = self.calCheckSum(self.dataTosend())
def __init__(self, param): ParamBuildResponse.__init__(self, Battery, 0, param.getBatteryPubHz()) self._length = MSG_LEN self._checkSum = 0 self._voltageDividerRatio = param.getBatteryVoltageDividerRatio() self._checkSum = self.calCheckSum(self.dataTosend())
def __init__(self, param): ParamBuildResponse.__init__(self, GPS, 0, param.getGpsPubHz()) self._length = MSG_LEN self._checkSum = 0 self._baudrate = param.getGpsBaudrate() self._checkSum = self.calCheckSum(self.dataTosend())
def __init__(self, devId, param, type): ParamBuildResponse.__init__(self, type, devId, param.getCloseLoopMotorPubHz(devId)) self._length = RES_LEN self._LPFHz = param.getCloseLoopMotorLPFHz(devId) self._LPFAlpha = param.getCloseLoopMotorLPFAlpha(devId) self._driverAddress = param.getCloseLoopMotorDriverAddress(devId) self._channel = param.getCloseLoopMotorChannel(devId) self._PIDHz = param.getCloseLoopMotorPIDHz(devId) self._KP = param.getCloseLoopMotorKp(devId) self._KI = param.getCloseLoopMotorKi(devId) self._KD = param.getCloseLoopMotorKd(devId) self._maxSpeed = param.getCloseLoopMotorMaxSpeed(devId) self._CPR = param.getCloseLoopMotorCpr(devId) self._timeout = param.getCloseLoopMotorTimeout(devId) self._timeout = param.getCloseLoopMotorTimeout(devId) self._limit = param.getCloseLoopMotorIntegralLimit(devId) port = param.getCloseLoopMotorEncoderPort(devId) if port == EN_PORT1: self._encoderPinA = 20 self._encoderPinB = 21 elif port == EN_PORT2: self._encoderPinA = 22 self._encoderPinB = 23 elif port == EN_PORT3: self._encoderPinA = 27 self._encoderPinB = 28 elif port == EN_PORT4: self._encoderPinA = 29 self._encoderPinB = 30 self._motorType = param.getCloseLoopMotorType(devId) self._motorDirection = param.getCloseLoopMotorDirection(devId) self._motorDirectionE = param.getCloseLoopMotorDirectionEncoder(devId) if param.getCloseLoopMotorEncoderType(devId) == 1: self._checkSum = 0 self._checkSum = self.calCheckSum(self.dataTosend())
def __init__(self, type, devId, param): ParamBuildResponse.__init__(self, type, devId, param.getURFPubHz(devId)) self._length = RES_LEN self._checkSum = 0 self._portNum = param.getURFPort(devId) self._checkSum = self.calCheckSum(self.dataTosend())
def __init__(self, devId , param): ParamBuildResponse.__init__(self, Rely, devId, 0) self._length = MSG_LEN self._checkSum = 0 self._port = param.getRelayPort(devId) self._checkSum = self.calCheckSum(self.dataTosend())
def __init__(self, devId, param): ParamBuildResponse.__init__(self, Rely, devId, 0) self._length = MSG_LEN self._checkSum = 0 self._port = param.getRelayPort(devId) self._checkSum = self.calCheckSum(self.dataTosend())
def __init__(self, param): ParamBuildResponse.__init__(self, IMU, 0, param.getIMUPubHz()) self._length = RES_LEN self._checkSum = 0 self._camp = param.getIMUCamp() self._fusionHz = param.getIMUFusionHz() self._checkSum = self.calCheckSum(self.dataTosend())
def __init__(self, switchNum, param): ParamBuildResponse.__init__(self, EmergencySwitch, switchNum, 0) self._length = MSG_LEN self._checkSum = 0 self._listenToPin = param.getEmergencyPin(switchNum) self._status = param.getEmergencyState(switchNum) self._checkSum = self.calCheckSum(self.dataTosend())
def __init__(self, devId, param): ParamBuildResponse.__init__(self, DiffDriverCL, devId, param.getCloseDiffPubHz()) self._length = MSG_LEN self._checkSum = 0 self._rWheel = param.getCloseDiffRWheel() self._width = param.getCloseDiffWidth() self._motorL = param.getCloseDiffMotorL() self._motorR = param.getCloseDiffMotorR() self._slip = param.getCloseDiffSlip() self._checkSum = self.calCheckSum(self.dataTosend())
def __init__(self, devId, param): ParamBuildResponse.__init__(self, EngineOL, devId, 0) self._length = MSG_LEN self._checkSum = 0 self._address = param.getOpenLoopAddress(devId) self._channel = param.getOpenLoopChannel(devId) self._timeout = param.getOpenLoopTimeout(devId) self._max = param.getOpenLoopMax(devId) self._checkSum = self.calCheckSum(self.dataTosend())
def __init__(self, param): ParamBuildResponse.__init__(self, IMU, 0, param.getIMUPubHz()) self._length = RES_LEN self._checkSum = 0 self._camp = param.getIMUCamp() self._fusionHz = param.getIMUFusionHz() self._enableFuseGyro = param.isIMUFuseGyro() self._checkSum = self.calCheckSum(self.dataTosend())
def __init__(self, devId, param): ParamBuildResponse.__init__(self, SERVO, devId, param.getServoPublishHz(devId)) self._length = SERVO_LEN_MSG self._port = param.getServoPort(devId) self._min = param.getServoMin(devId) self._max = param.getServoMax(devId) self._a = param.getServoAParameter(devId) self._b = param.getServoBParameter(devId) self._initPos = param.getServoInitMove(devId) self._checkSum = 0 self._checkSum = self.calCheckSum(self.dataTosend())
def dataTosend(self): return ParamBuildResponse.dataTosend(self) \ + struct.pack('<f', self._rWheel) \ + struct.pack('<f', self._width) \ + struct.pack('<i', self._motorL) \ + struct.pack('<i', self._motorR) \ + struct.pack('<f', self._slip)
def dataTosend(self): return ParamBuildResponse.dataTosend(self)\ + struct.pack('<i', self._port)\ + struct.pack('<f', self._min)\ + struct.pack('<f', self._max)\ + struct.pack('<f', self._a)\ + struct.pack('<f', self._b)\ + struct.pack('<f', self._initPos)
def dataTosend(self): return ( ParamBuildResponse.dataTosend(self) + struct.pack("<f", self._rWheel) + struct.pack("<f", self._width) + struct.pack("<i", self._motorFL) + struct.pack("<i", self._motorFR) + struct.pack("<i", self._motorBL) + struct.pack("<i", self._motorBR) + struct.pack("<f", self._slip) )
def dataTosend(self): return ParamBuildResponse.dataTosend(self) \ + struct.pack('<I', self._LPFHz) \ + struct.pack('<f', self._LPFAlpha) \ + struct.pack('<I', self._driverAddress) \ + struct.pack('<I', self._channel) \ + struct.pack('<I', self._PIDHz) \ + struct.pack('<f', self._KP) \ + struct.pack('<f', self._KI) \ + struct.pack('<f', self._KD) \ + struct.pack('<f', self._maxSpeed) \ + struct.pack('<I', self._CPR) \ + struct.pack('<I', self._timeout) \ + struct.pack('<I', self._encoderPinA) \ + struct.pack('<I', self._encoderPinB) \ + struct.pack('<I', self._motorType) \ + struct.pack('<i', self._motorDirection) \ + struct.pack('<i', self._motorDirectionE) \ + struct.pack('<f', self._limit)
def dataTosend(self): return ParamBuildResponse.dataTosend(self) + struct.pack('<i', self._port)
def dataTosend(self): return ParamBuildResponse.dataTosend(self) \ + struct.pack('<B', self._listenToPin) \ + struct.pack('<B', self._status)
def dataTosend(self): return ParamBuildResponse.dataTosend(self)\ + struct.pack('<f', self._camp) \ + struct.pack('<H', self._fusionHz)
def dataTosend(self): return ParamBuildResponse.dataTosend(self)\ + struct.pack('<f', self._camp) \ + struct.pack('<H', self._fusionHz)\ + struct.pack('<?', self._enableFuseGyro)
def __init__(self, param): ParamBuildResponse.__init__(self, PPM, 0, param.getPPMPubHz()) self._length = MSG_LEN self._checkSum = 0 self._checkSum = self.calCheckSum(self.dataTosend())
def dataTosend(self): return ParamBuildResponse.dataTosend(self) \ + struct.pack('<I', self._address) \ + struct.pack('<I', self._channel) \ + struct.pack('<I', self._timeout) \ + struct.pack('<i', self._max)
def dataTosend(self): return ParamBuildResponse.dataTosend(self) + struct.pack('<i', self._portNum)
def dataTosend(self): return ParamBuildResponse.dataTosend(self) + struct.pack( '<i', self._baudrate)
def dataTosend(self): return ParamBuildResponse.dataTosend(self) \ + struct.pack('<f', self._voltageDividerRatio)