def createCLMotors(self):
closeMotorsAmount = self._param.getCloseLoopMotorSize()
for motorNum in xrange(closeMotorsAmount):
rospy.loginfo("Configuring motor: %s", self._param.getCloseLoopMotorName(motorNum))
motor = RiCCloseLoopMotor(motorNum, self._param, self._output)
self._allDevs['motorsCl'].append(motor)
if self._param.getCloseLoopMotorEncoderType(motorNum) == 1:
toSend = CloseLoopMotorBuildResponse(motorNum, self._param, EngineCL)
else:
toSend = CloseLoopMotorTwoEncBuildResponse(motorNum, self._param, EngineCL2)
self._output.writeAndWaitForAck(toSend.dataTosend(), motorNum)
rospy.loginfo("Motor: %s is ready", self._param.getCloseLoopMotorName(motorNum))
def createCLMotors(self):
closeMotorsAmount = self._param.getCloseLoopMotorSize()
for motorNum in xrange(closeMotorsAmount):
rospy.loginfo("Configuring motor: %s",
self._param.getCloseLoopMotorName(motorNum))
motor = RiCCloseLoopMotor(motorNum, self._param, self._output)
self._allDevs['motorsCl'].append(motor)
if self._param.getCloseLoopMotorEncoderType(motorNum) == 1:
toSend = CloseLoopMotorBuildResponse(motorNum, self._param,
EngineCL)
else:
toSend = CloseLoopMotorTwoEncBuildResponse(
motorNum, self._param, EngineCL2)
self._output.writeAndWaitForAck(toSend.dataTosend(), motorNum)
rospy.loginfo("Motor: %s is ready",
self._param.getCloseLoopMotorName(motorNum))
def __init__(self, devId, param, type):
CloseLoopMotorBuildResponse.__init__(self, devId, param, type)
self._length = LEN_MSG
port = param.getCloseLoopMotorPort2(devId)
if port == EN_PORT1:
self._encoderPin2A = 20
self._encoderPin2B = 21
elif port == EN_PORT2:
self._encoderPin2A = 22
self._encoderPin2B = 23
elif port == EN_PORT3:
self._encoderPin2A = 27
self._encoderPin2B = 28
else:
self._encoderPin2A = 29
self._encoderPin2B = 30
self._checkSum = 0
self._checkSum = self.calCheckSum(self.dataTosend())
def dataTosend(self):
return CloseLoopMotorBuildResponse.dataTosend(self) \
+ struct.pack('<I', self._encoderPin2A) \
+ struct.pack('<I', self._encoderPin2B)
