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)