Go to the documentation of this file.00001 import struct
00002
00003 __author__ = 'tom1231'
00004 from BAL.Header.Response.ParamBuildResponse import ParamBuildResponse, EngineCL
00005
00006 RES_LEN = 82
00007 EN_PORT1 = 1
00008 EN_PORT2 = 2
00009 EN_PORT3 = 3
00010 EN_PORT4 = 4
00011
00012 class CloseLoopMotorBuildResponse(ParamBuildResponse):
00013 def dataTosend(self):
00014 return ParamBuildResponse.dataTosend(self) \
00015 + struct.pack('<I', self._LPFHz) \
00016 + struct.pack('<f', self._LPFAlpha) \
00017 + struct.pack('<I', self._driverAddress) \
00018 + struct.pack('<I', self._channel) \
00019 + struct.pack('<I', self._PIDHz) \
00020 + struct.pack('<f', self._KP) \
00021 + struct.pack('<f', self._KI) \
00022 + struct.pack('<f', self._KD) \
00023 + struct.pack('<f', self._maxSpeed) \
00024 + struct.pack('<I', self._CPR) \
00025 + struct.pack('<I', self._timeout) \
00026 + struct.pack('<I', self._encoderPinA) \
00027 + struct.pack('<I', self._encoderPinB) \
00028 + struct.pack('<I', self._motorType) \
00029 + struct.pack('<i', self._motorDirection) \
00030 + struct.pack('<i', self._motorDirectionE) \
00031 + struct.pack('<f', self._limit)
00032
00033 def __init__(self, devId, param, type):
00034 ParamBuildResponse.__init__(self, type, devId, param.getCloseLoopMotorPubHz(devId))
00035 self._length = RES_LEN
00036 self._LPFHz = param.getCloseLoopMotorLPFHz(devId)
00037 self._LPFAlpha = param.getCloseLoopMotorLPFAlpha(devId)
00038 self._driverAddress = param.getCloseLoopMotorDriverAddress(devId)
00039 self._channel = param.getCloseLoopMotorChannel(devId)
00040 self._PIDHz = param.getCloseLoopMotorPIDHz(devId)
00041 self._KP = param.getCloseLoopMotorKp(devId)
00042 self._KI = param.getCloseLoopMotorKi(devId)
00043 self._KD = param.getCloseLoopMotorKd(devId)
00044 self._maxSpeed = param.getCloseLoopMotorMaxSpeed(devId)
00045 self._CPR = param.getCloseLoopMotorCpr(devId)
00046 self._timeout = param.getCloseLoopMotorTimeout(devId)
00047 self._timeout = param.getCloseLoopMotorTimeout(devId)
00048 self._limit = param.getCloseLoopMotorIntegralLimit(devId)
00049 port = param.getCloseLoopMotorEncoderPort(devId)
00050 if port == EN_PORT1:
00051 self._encoderPinA = 20
00052 self._encoderPinB = 21
00053 elif port == EN_PORT2:
00054 self._encoderPinA = 22
00055 self._encoderPinB = 23
00056 elif port == EN_PORT3:
00057 self._encoderPinA = 27
00058 self._encoderPinB = 28
00059 elif port == EN_PORT4:
00060 self._encoderPinA = 29
00061 self._encoderPinB = 30
00062 self._motorType = param.getCloseLoopMotorType(devId)
00063 self._motorDirection = param.getCloseLoopMotorDirection(devId)
00064 self._motorDirectionE = param.getCloseLoopMotorDirectionEncoder(devId)
00065 if param.getCloseLoopMotorEncoderType(devId) == 1:
00066 self._checkSum = 0
00067 self._checkSum = self.calCheckSum(self.dataTosend())
00068