text: import random
  import serial
  import struct
  import time
  
  class Roboclaw:
  	'Roboclaw Interface Class'
  	
  	def __init__(self, comport, rate, timeout=0.1, retries=1):
  		self.comport = comport
  		self.rate = rate
  		self.timeout = timeout;
  		self._trystimeout = retries
  		self._crc = 0;
  
  	#Command Enums
  	class Cmd():
  		M1FORWARD = 0
  		M1BACKWARD = 1
  		SETMINMB = 2
  		SETMAXMB = 3
  		M2FORWARD = 4
  		M2BACKWARD = 5
  		M17BIT = 6
  		M27BIT = 7
  		MIXEDFORWARD = 8
  		MIXEDBACKWARD = 9
  		MIXEDRIGHT = 10
  		MIXEDLEFT = 11
  		MIXEDFB = 12
  		MIXEDLR = 13
  		GETM1ENC = 16
  		GETM2ENC = 17
  		GETM1SPEED = 18
  		GETM2SPEED = 19
  		RESETENC = 20
  		GETVERSION = 21
  		SETM1ENCCOUNT = 22
  		SETM2ENCCOUNT = 23
  		GETMBATT = 24
  		GETLBATT = 25
  		SETMINLB = 26
  		SETMAXLB = 27
  		SETM1PID = 28
  		SETM2PID = 29
  		GETM1ISPEED = 30
  		GETM2ISPEED = 31
  		M1DUTY = 32
  		M2DUTY = 33
  		MIXEDDUTY = 34
  		M1SPEED = 35
  		M2SPEED = 36
  		MIXEDSPEED = 37
  		M1SPEEDACCEL = 38
  		M2SPEEDACCEL = 39
  		MIXEDSPEEDACCEL = 40
  		M1SPEEDDIST = 41
  		M2SPEEDDIST = 42
  		MIXEDSPEEDDIST = 43
  		M1SPEEDACCELDIST = 44
  		M2SPEEDACCELDIST = 45
  		MIXEDSPEEDACCELDIST = 46
  		GETBUFFERS = 47
  		GETPWMS = 48
  		GETCURRENTS = 49
  		MIXEDSPEED2ACCEL = 50
  		MIXEDSPEED2ACCELDIST = 51
  		M1DUTYACCEL = 52
  		M2DUTYACCEL = 53
  		MIXEDDUTYACCEL = 54
  		READM1PID = 55
  		READM2PID = 56
  		SETMAINVOLTAGES = 57
  		SETLOGICVOLTAGES = 58
  		GETMINMAXMAINVOLTAGES = 59
  		GETMINMAXLOGICVOLTAGES = 60
  		SETM1POSPID = 61
  		SETM2POSPID = 62
  		READM1POSPID = 63
  		READM2POSPID = 64
  		M1SPEEDACCELDECCELPOS = 65
  		M2SPEEDACCELDECCELPOS = 66
  		MIXEDSPEEDACCELDECCELPOS = 67
  		SETM1DEFAULTACCEL = 68
  		SETM2DEFAULTACCEL = 69
  		SETPINFUNCTIONS = 74
  		GETPINFUNCTIONS = 75
  		SETDEADBAND = 76
  		GETDEADBAND = 77
  		RESTOREDEFAULTS = 80
  		GETTEMP = 82
  		GETTEMP2 = 83
  		GETERROR = 90
  		GETENCODERMODE = 91
  		SETM1ENCODERMODE = 92
  		SETM2ENCODERMODE = 93
  		WRITENVM = 94
  		READNVM = 95
  		SETCONFIG = 98
  		GETCONFIG = 99
  		SETM1MAXCURRENT = 133
  		SETM2MAXCURRENT = 134
  		GETM1MAXCURRENT = 135
  		GETM2MAXCURRENT = 136
  		SETPWMMODE = 148
  		GETPWMMODE = 149
  		FLAGBOOTLOADER = 255
  			
  	#Private Functions
  	def crc_clear(self):
  		self._crc = 0
  		return
  		
  	def crc_update(self,data):
  		self._crc = self._crc ^ (data << 8)
  		for bit in range(0, 8):
  			if (self._crc&0x8000)  == 0x8000:
  				self._crc = ((self._crc << 1) ^ 0x1021)
  			else:
  				self._crc = self._crc << 1
  		return
  
  	def _sendcommand(self,address,command):
  		self.crc_clear()
  		self.crc_update(address)
  		self._port.write(chr(address))
  		self.crc_update(command)
  		self._port.write(chr(command))
  		return
  
  	def _readchecksumword(self):
  		data = self._port.read(2)
  		if len(data)==2:
  			crc = (ord(data[0])<<8) | ord(data[1])
  			return (1,crc)	
  		return (0,0)
  		
  	def _readbyte(self):
  		data = self._port.read(1)
  		if len(data):
  			val = ord(data)
  			self.crc_update(val)
  			return (1,val)	
  		return (0,0)
  		
  	def _readword(self):
  		val1 = self._readbyte()
  		if val1[0]:
  			val2 = self._readbyte()
  			if val2[0]:
  				return (1,val1[1]<<8|val2[1])
  		return (0,0)
  
  	def _readlong(self):
  		val1 = self._readbyte()
  		if val1[0]:
  			val2 = self._readbyte()
  			if val2[0]:
  				val3 = self._readbyte()
  				if val3[0]:
  					val4 = self._readbyte()
  					if val4[0]:
  						return (1,val1[1]<<24|val2[1]<<16|val3[1]<<8|val4[1])
  		return (0,0)	
  
  	def _readslong(self):
  		val = self._readlong()
  		if val[0]:
  			if val[1]&0x80000000:
  				return (val[0],val[1]-0x100000000)
  			return (val[0],val[1])
  		return (0,0)
  
  	def _writebyte(self,val):
  		self.crc_update(val&0xFF)
  		self._port.write(chr(val&0xFF))
  
  	def _writesbyte(self,val):
  		self._writebyte(val)
  
  	def _writeword(self,val):
  		self._writebyte((val>>8)&0xFF)
  		self._writebyte(val&0xFF)
  		
  	def _writesword(self,val):
  		self._writeword(val)
  
  	def _writelong(self,val):
  		self._writebyte((val>>24)&0xFF)
  		self._writebyte((val>>16)&0xFF)
  		self._writebyte((val>>8)&0xFF)
  		self._writebyte(val&0xFF)
  
  	def _writeslong(self,val):
  		self._writelong(val)
  
  	def _read1(self,address,cmd):
  		trys = self._trystimeout
  		while 1:
  			self._port.flushInput()
  			self._sendcommand(address,cmd)
  			val1 = self._readbyte()
  			if val1[0]:
  				crc = self._readchecksumword()
  				if crc[0]:
  					if self._crc&0xFFFF!=crc[1]&0xFFFF:
  						return (0,0)
  					return (1,val1[1])
  			trys-=1
  			if trys==0:
  				break
  		return (0,0)
  
  	def _read2(self,address,cmd):
  		trys = self._trystimeout
  		while 1:
  			self._port.flushInput()
  			self._sendcommand(address,cmd)
  			val1 = self._readword()
  			if val1[0]:
  				crc = self._readchecksumword()
  				if crc[0]:
  					if self._crc&0xFFFF!=crc[1]&0xFFFF:
  						return (0,0)
  					return (1,val1[1])
  			trys-=1
  			if trys==0:
  				break
  		return (0,0)
  
  	def _read4(self,address,cmd):
  		trys = self._trystimeout
  		while 1:
  			self._port.flushInput()
  			self._sendcommand(address,cmd)
  			val1 = self._readlong()
  			if val1[0]:
  				crc = self._readchecksumword()
  				if crc[0]:
  					if self._crc&0xFFFF!=crc[1]&0xFFFF:
  						return (0,0)
  					return (1,val1[1])
  			trys-=1
  			if trys==0:
  				break
  		return (0,0)
  
  	def _read4_1(self,address,cmd):
  		trys = self._trystimeout
  		while 1:
  			self._port.flushInput()
  			self._sendcommand(address,cmd)
  			val1 = self._readslong()
  			if val1[0]:
  				val2 = self._readbyte()
  				if val2[0]:
  					crc = self._readchecksumword()
  					if crc[0]:
  						if self._crc&0xFFFF!=crc[1]&0xFFFF:
  							return (0,0)
  						return (1,val1[1],val2[1])
  			trys-=1
  			if trys==0:
  				break
  		return (0,0)
  
  	def _read_n(self,address,cmd,args):
  		trys = self._trystimeout
  		while 1:
  			self._port.flushInput()
  			trys-=1
  			if trys==0:
  				break
  			failed=False
  			self._sendcommand(address,cmd)
  			data = [1,]
  			for i in range(0,args):
  				val = self._readlong()
  				if val[0]==0:
  					failed=True
  					break
  				data.append(val[1])
  			if failed:
  				continue
  			crc = self._readchecksumword()
  			if crc[0]:
  				if self._crc&0xFFFF==crc[1]&0xFFFF:
  					return (data);
  		return (0,0,0,0,0)
  
  	def _writechecksum(self):
  		self._writeword(self._crc&0xFFFF)
  		val = self._readbyte()
  		if(len(val)>0):
  			if val[0]:
  				return True
  		return False
  
  	def _write0(self,address,cmd):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _write1(self,address,cmd,val):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writebyte(val)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _write11(self,address,cmd,val1,val2):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writebyte(val1)
  			self._writebyte(val2)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _write111(self,address,cmd,val1,val2,val3):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writebyte(val1)
  			self._writebyte(val2)
  			self._writebyte(val3)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _write2(self,address,cmd,val):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writeword(val)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _writeS2(self,address,cmd,val):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writesword(val)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _write22(self,address,cmd,val1,val2):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writeword(val1)
  			self._writeword(val2)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _writeS22(self,address,cmd,val1,val2):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writesword(val1)
  			self._writeword(val2)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _writeS2S2(self,address,cmd,val1,val2):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writesword(val1)
  			self._writesword(val2)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _writeS24(self,address,cmd,val1,val2):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writesword(val1)
  			self._writelong(val2)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _writeS24S24(self,address,cmd,val1,val2,val3,val4):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writesword(val1)
  			self._writelong(val2)
  			self._writesword(val3)
  			self._writelong(val4)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _write4(self,address,cmd,val):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writelong(val)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _writeS4(self,address,cmd,val):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writeslong(val)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _write44(self,address,cmd,val1,val2):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writelong(val1)
  			self._writelong(val2)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _write4S4(self,address,cmd,val1,val2):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writelong(val1)
  			self._writeslong(val2)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _writeS4S4(self,address,cmd,val1,val2):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writeslong(val1)
  			self._writeslong(val2)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _write441(self,address,cmd,val1,val2,val3):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writelong(val1)
  			self._writelong(val2)
  			self._writebyte(val3)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _writeS441(self,address,cmd,val1,val2,val3):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writeslong(val1)
  			self._writelong(val2)
  			self._writebyte(val3)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _write4S4S4(self,address,cmd,val1,val2,val3):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writelong(val1)
  			self._writeslong(val2)
  			self._writeslong(val3)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _write4S441(self,address,cmd,val1,val2,val3,val4):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writelong(val1)
  			self._writeslong(val2)
  			self._writelong(val3)
  			self._writebyte(val4)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _write4444(self,address,cmd,val1,val2,val3,val4):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writelong(val1)
  			self._writelong(val2)
  			self._writelong(val3)
  			self._writelong(val4)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _write4S44S4(self,address,cmd,val1,val2,val3,val4):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writelong(val1)
  			self._writeslong(val2)
  			self._writelong(val3)
  			self._writeslong(val4)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _write44441(self,address,cmd,val1,val2,val3,val4,val5):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writelong(val1)
  			self._writelong(val2)
  			self._writelong(val3)
  			self._writelong(val4)
  			self._writebyte(val5)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _writeS44S441(self,address,cmd,val1,val2,val3,val4,val5):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writeslong(val1)
  			self._writelong(val2)
  			self._writeslong(val3)
  			self._writelong(val4)
  			self._writebyte(val5)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _write4S44S441(self,address,cmd,val1,val2,val3,val4,val5,val6):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writelong(val1)
  			self._writeslong(val2)
  			self._writelong(val3)
  			self._writeslong(val4)
  			self._writelong(val5)
  			self._writebyte(val6)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _write4S444S441(self,address,cmd,val1,val2,val3,val4,val5,val6,val7):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(self,address,cmd)
  			self._writelong(val1)
  			self._writeslong(val2)
  			self._writelong(val3)
  			self._writelong(val4)
  			self._writeslong(val5)
  			self._writelong(val6)
  			self._writebyte(val7)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _write4444444(self,address,cmd,val1,val2,val3,val4,val5,val6,val7):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writelong(val1)
  			self._writelong(val2)
  			self._writelong(val3)
  			self._writelong(val4)
  			self._writelong(val5)
  			self._writelong(val6)
  			self._writelong(val7)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	def _write444444441(self,address,cmd,val1,val2,val3,val4,val5,val6,val7,val8,val9):
  		trys=self._trystimeout
  		while trys:
  			self._sendcommand(address,cmd)
  			self._writelong(val1)
  			self._writelong(val2)
  			self._writelong(val3)
  			self._writelong(val4)
  			self._writelong(val5)
  			self._writelong(val6)
  			self._writelong(val7)
  			self._writelong(val8)
  			self._writebyte(val9)
  			if self._writechecksum():
  				return True
  			trys=trys-1
  		return False
  
  	#User accessible functions
  	def SendRandomData(self,cnt):
  		for i in range(0,cnt):
  			byte = random.getrandbits(8)
  			self._port.write(chr(byte))
  		return
  
  	def ForwardM1(self,address,val):
  		return self._write1(address,self.Cmd.M1FORWARD,val)
  
  	def BackwardM1(self,address,val):
  		return self._write1(address,self.Cmd.M1BACKWARD,val)
  
  	def SetMinVoltageMainBattery(self,address,val):
  		return self._write1(address,self.Cmd.SETMINMB,val)
  
  	def SetMaxVoltageMainBattery(self,address,val):
  		return self._write1(address,self.Cmd.SETMAXMB,val)
  
  	def ForwardM2(self,address,val):
  		return self._write1(address,self.Cmd.M2FORWARD,val)
  
  	def BackwardM2(self,address,val):
  		return self._write1(address,self.Cmd.M2BACKWARD,val)
  
  	def ForwardBackwardM1(self,address,val):
  		return self._write1(address,self.Cmd.M17BIT,val)
  
  	def ForwardBackwardM2(self,address,val):
  		return self._write1(address,self.Cmd.M27BIT,val)
  
  	def ForwardMixed(self,address,val):
  		return self._write1(address,self.Cmd.MIXEDFORWARD,val)
  
  	def BackwardMixed(self,address,val):
  		return self._write1(address,self.Cmd.MIXEDBACKWARD,val)
  
  	def TurnRightMixed(self,address,val):
  		return self._write1(address,self.Cmd.MIXEDRIGHT,val)
  
  	def TurnLeftMixed(self,address,val):
  		return self._write1(address,self.Cmd.MIXEDLEFT,val)
  
  	def ForwardBackwardMixed(self,address,val):
  		return self._write1(address,self.Cmd.MIXEDFB,val)
  
  	def LeftRightMixed(self,address,val):
  		return self._write1(address,self.Cmd.MIXEDLR,val)
  
  	def ReadEncM1(self,address):
  		return self._read4_1(address,self.Cmd.GETM1ENC)
  
  	def ReadEncM2(self,address):
  		return self._read4_1(address,self.Cmd.GETM2ENC)
  
  	def ReadSpeedM1(self,address):
  		return self._read4_1(address,self.Cmd.GETM1SPEED)
  
  	def ReadSpeedM2(self,address):
  		return self._read4_1(address,self.Cmd.GETM2SPEED)
  
  	def ResetEncoders(self,address):
  		return self._write0(address,self.Cmd.RESETENC)
  
  	def ReadVersion(self,address):
  		trys=self._trystimeout
  		while 1:
  			self._port.flushInput()
  			self._sendcommand(address,self.Cmd.GETVERSION)
  			str = ""
  			passed = True
  			for i in range(0,48):
  				data = self._port.read(1)
  				if len(data):
  					val = ord(data)
  					self.crc_update(val)
  					if(val==0):
  						break
  					str+=data[0]
  				else:
  					passed = False
  					break
  			if passed:
  				crc = self._readchecksumword()
  				if crc[0]:
  					if self._crc&0xFFFF==crc[1]&0xFFFF:
  						return (1,str)
  					else:
  						time.sleep(0.01)
  			trys-=1
  			if trys==0:
  				break
  		return (0,0)
  
  	def SetEncM1(self,address,cnt):
  		return self._write4(address,self.Cmd.SETM1ENCCOUNT,cnt)
  
  	def SetEncM2(self,address,cnt):
  		return self._write4(address,self.Cmd.SETM2ENCCOUNT,cnt)
  
  	def ReadMainBatteryVoltage(self,address):
  		return self._read2(address,self.Cmd.GETMBATT)
  
  	def ReadLogicBatteryVoltage(self,address,):
  		return self._read2(address,self.Cmd.GETLBATT)
  
  	def SetMinVoltageLogicBattery(self,address,val):
  		return self._write1(address,self.Cmd.SETMINLB,val)
  
  	def SetMaxVoltageLogicBattery(self,address,val):
  		return self._write1(address,self.Cmd.SETMAXLB,val)
  
  	def SetM1VelocityPID(self,address,p,i,d,qpps):
  		return self._write4444(address,self.Cmd.SETM1PID,long(d*65536),long(p*65536),long(i*65536),qpps)
  
  	def SetM2VelocityPID(self,address,p,i,d,qpps):
  		return self._write4444(address,self.Cmd.SETM2PID,long(d*65536),long(p*65536),long(i*65536),qpps)
  
  	def ReadISpeedM1(self,address):
  		return self._read4_1(address,self.Cmd.GETM1ISPEED)
  
  	def ReadISpeedM2(self,address):
  		return self._read4_1(address,self.Cmd.GETM2ISPEED)
  
  	def DutyM1(self,address,val):
  		return self._simplFunctionS2(address,self.Cmd.M1DUTY,val)
  
  	def DutyM2(self,address,val):
  		return self._simplFunctionS2(address,self.Cmd.M2DUTY,val)
  
  	def DutyM1M2(self,address,m1,m2):
  		return self._writeS2S2(address,self.Cmd.MIXEDDUTY,m1,m2)
  
  	def SpeedM1(self,address,val):
  		return self._writeS4(address,self.Cmd.M1SPEED,val)
  
  	def SpeedM2(self,address,val):
  		return self._writeS4(address,self.Cmd.M2SPEED,val)
  
  	def SpeedM1M2(self,address,m1,m2):
  		return self._writeS4S4(address,self.Cmd.MIXEDSPEED,m1,m2)
  
  	def SpeedAccelM1(self,address,accel,speed):
  		return self._write4S4(address,self.Cmd.M1SPEEDACCEL,accel,speed)
  
  	def SpeedAccelM2(self,address,accel,speed):
  		return self._write4S4(address,self.Cmd.M2SPEEDACCEL,accel,speed)
  
  	def SpeedAccelM1M2(self,address,accel,speed1,speed2):
  		return self._write4S4S4(address,self.Cmd.MIXEDSPEEDACCEL,accel,speed1,speed2)
  
  	def SpeedDistanceM1(self,address,speed,distance,buffer):
  		return self._writeS441(address,self.Cmd.M1SPEEDDIST,speed,distance,buffer)
  
  	def SpeedDistanceM2(self,address,speed,distance,buffer):
  		return self._writeS441(address,self.Cmd.M2SPEEDDIST,speed,distance,buffer)
  
  	def SpeedDistanceM1M2(self,address,speed1,distance1,speed2,distance2,buffer):
  		return self._writeS44S441(address,self.Cmd.MIXEDSPEEDDIST,speed1,distance1,speed2,distance2,buffer)
  
  	def SpeedAccelDistanceM1(self,address,accel,speed,distance,buffer):
  		return self._write4S441(address,self.Cmd.M1SPEEDACCELDIST,accel,speed,distance,buffer)
  
  	def SpeedAccelDistanceM2(self,address,accel,speed,distance,buffer):
  		return self._write4S441(address,self.Cmd.M2SPEEDACCELDIST,accel,speed,distance,buffer)
  
  	def SpeedAccelDistanceM1M2(self,address,accel,speed1,distance1,speed2,distance2,buffer):
  		return self._write4S44S441(address,self.Cmd.MIXEDSPEEDACCELDIST,accel,speed1,distance1,speed2,distance2,buffer)
  
  	def ReadBuffers(self,address):
  		val = self._read2(address,self.Cmd.GETBUFFERS)
  		if val[0]:
  			return (1,val[1]>>8,val[1]&0xFF)
  		return (0,0,0)
  
  	def ReadPWMs(self,address):
  		val = self._read4(address,self.Cmd.GETPWMS)
  		if val[0]:
  			pwm1 = val[1]>>16
  			pwm2 = val[1]&0xFFFF
  			if pwm1&0x8000:
  				pwm1-=0x10000
  			if pwm2&0x8000:
  				pwm2-=0x10000
  			return (1,pwm1,pwm2)
  		return (0,0,0)
  
  	def ReadCurrents(self,address):
  		val = self._read4(address,self.Cmd.GETCURRENTS)
  		if val[0]:
  			cur1 = val[1]>>16
  			cur2 = val[1]&0xFFFF
  			if cur1&0x8000:
  				cur1-=0x10000
  			if cur2&0x8000:
  				cur2-=0x10000
  			return (1,cur1,cur2)
  		return (0,0,0)
  
  	def SpeedAccelM1M2_2(self,address,accel1,speed1,accel2,speed2):
  		return self._write4S44S4(address,self.Cmd.MIXEDSPEED2ACCEL,accel,speed1,accel2,speed2)
  
  	def SpeedAccelDistanceM1M2_2(self,address,accel1,speed1,distance1,accel2,speed2,distance2,buffer):
  		return self._write4S444S441(address,self.Cmd.MIXEDSPEED2ACCELDIST,accel1,speed1,distance1,accel2,speed2,distance2,buffer)
  
  	def DutyAccelM1(self,address,accel,duty):
  		return self._writeS24(address,self.Cmd.M1DUTYACCEL,duty,accel)
  
  	def DutyAccelM2(self,address,accel,duty):
  		return self._writeS24(address,self.Cmd.M2DUTYACCEL,duty,accel)
  
  	def DutyAccelM1M2(self,address,accel1,duty1,accel2,duty2):
  		return self._writeS24S24(self.Cmd.MIXEDDUTYACCEL,duty1,accel1,duty2,accel2)
  		
  	def ReadM1VelocityPID(self,address):
  		data = self._read_n(address,self.Cmd.READM1PID,4)
  		if data[0]:
  			data[1]/=65536.0
  			data[2]/=65536.0
  			data[3]/=65536.0
  			return data
  		return (0,0,0,0,0)
  
  	def ReadM2VelocityPID(self,address):
  		data = self._read_n(address,self.Cmd.READM2PID,4)
  		if data[0]:
  			data[1]/=65536.0
  			data[2]/=65536.0
  			data[3]/=65536.0
  			return data
  		return (0,0,0,0,0)
  
  	def SetMainVoltages(self,address,min, max):
  		return self._write22(address,self.Cmd.SETMAINVOLTAGES,min,max)
  		
  	def SetLogicVoltages(self,address,min, max):
  		return self._write22(address,self.Cmd.SETLOGICVOLTAGES,min,max)
  		
  	def ReadMinMaxMainVoltages(self,address):
  		val = self._read4(address,self.Cmd.GETMINMAXMAINVOLTAGES)
  		if val[0]:
  			min = val[1]>>16
  			max = val[1]&0xFFFF
  			return (1,min,max)
  		return (0,0,0)
  
  	def ReadMinMaxLogicVoltages(self,address):
  		val = self._read4(address,self.Cmd.GETMINMAXLOGICVOLTAGES)
  		if val[0]:
  			min = val[1]>>16
  			max = val[1]&0xFFFF
  			return (1,min,max)
  		return (0,0,0)
  
  	def SetM1PositionPID(self,address,kp,ki,kd,kimax,deadzone,min,max):
  		return self._write4444444(address,self.Cmd.SETM1POSPID,long(kd*1024),long(kp*1024),long(ki*1024),kimax,deadzone,min,max)
  
  	def SetM2PositionPID(self,address,kp,ki,kd,kimax,deadzone,min,max):
  		return self._write4444444(address,self.Cmd.SETM2POSPID,long(kd*1024),long(kp*1024),long(ki*1024),kimax,deadzone,min,max)
  
  	def ReadM1PositionPID(self,address):
  		data = self._read_n(address,self.Cmd.READM1POSPID,7)
  		if data[0]:
  			data[1]/=1024.0
  			data[2]/=1024.0
  			data[3]/=1024.0
  			return data
  		return (0,0,0,0,0,0,0,0)
  		
  	def ReadM2PositionPID(self,address):
  		data = self._read_n(address,self.Cmd.READM2POSPID,7)
  		if data[0]:
  			data[1]/=1024.0
  			data[2]/=1024.0
  			data[3]/=1024.0
  			return data
  		return (0,0,0,0,0,0,0,0)
  
  	def SpeedAccelDeccelPositionM1(self,address,accel,speed,deccel,position,buffer):
  		return self._write44441(address,self.Cmd.M1SPEEDACCELDECCELPOS,accel,speed,deccel,position,buffer)
  
  	def SpeedAccelDeccelPositionM2(self,address,accel,speed,deccel,position,buffer):
  		return self._write44441(address,self.Cmd.M2SPEEDACCELDECCELPOS,accel,speed,deccel,position,buffer)
  
  	def SpeedAccelDeccelPositionM1M2(self,address,accel1,speed1,deccel1,position1,accel2,speed2,deccel2,position2,buffer):
  		return self._write444444441(address,self.Cmd.MIXEDSPEEDACCELDECCELPOS,accel1,speed1,deccel1,position1,accel2,speed2,deccel2,position2,buffer)
  
  	def SetM1DefaultAccel(self,address,accel):
  		return self._write4(address,self.Cmd.SETM1DEFAULTACCEL,accel)
  
  	def SetM2DefaultAccel(self,address,accel):
  		return self._write4(address,self.Cmd.SETM2DEFAULTACCEL,accel)
  
  	def SetPinFunctions(self,address,S3mode,S4mode,S5mode):
  		return self._write111(address,self.Cmd.SETPINFUNCTIONS,S3mode,S4mode,S5mode)
  
  	def ReadPinFunctions(self,address):
  		trys = self._trystimeout
  		while 1:
  			self._sendcommand(address,self.Cmd.GETPINFUNCTIONS)
  			val1 = self._readbyte()
  			if val1[0]:
  				val2 = self._readbyte()
  				if val1[0]:
  					val3 = self._readbyte()
  					if val1[0]:
  						crc = self._readchecksumword()
  						if crc[0]:
  							if self._crc&0xFFFF!=crc[1]&0xFFFF:
  								return (0,0)
  							return (1,val1[1],val2[1],val3[1])
  			trys-=1
  			if trys==0:
  				break
  		return (0,0)
  
  	def SetDeadBand(self,address,min,max):
  		return self._write11(address,self.Cmd.SETDEADBAND,min,max)
  
  	def GetDeadBand(self,address):
  		val = self._read2(address,self.Cmd.GETDEADBAND)
  		if val[0]:
  			return (1,val[1]>>8,val[1]&0xFF)
  		return (0,0,0)
  		
  	#Warning(TTL Serial): Baudrate will change if not already set to 38400.  Communications will be lost
  	def RestoreDefaults(self,address):
  		return self._write0(address,self.Cmd.RESTOREDEFAULTS)
  
  	def ReadTemp(self,address):
  		return self._read2(address,self.Cmd.GETTEMP)
  
  	def ReadTemp2(self,address):
  		return self._read2(address,self.Cmd.GETTEMP2)
  
  	def ReadError(self,address):
  		return self._read2(address,self.Cmd.GETERROR)
  
  	def ReadEncoderModes(self,address):
  		val = self._read2(address,self.Cmd.GETENCODERMODE)
  		if val[0]:
  			return (1,val[1]>>8,val[1]&0xFF)
  		return (0,0,0)
  		
  	def SetM1EncoderMode(self,address,mode):
  		return self._write1(address,self.Cmd.SETM1ENCODERMODE,mode)
  
  	def SetM2EncoderMode(self,address,mode):
  		return self._write1(address,self.Cmd.SETM2ENCODERMODE,mode)
  
  	#saves active settings to NVM
  	def WriteNVM(self,address):
  		return self._write4(address,self.Cmd.WRITENVM,0xE22EAB7A)
  
  	#restores settings from NVM
  	#Warning(TTL Serial): If baudrate changes or the control mode changes communications will be lost
  	def ReadNVM(self,address):
  		return self._write0(address,self.Cmd.READNVM)
  
  	#Warning(TTL Serial): If control mode is changed from packet serial mode when setting config communications will be lost!
  	#Warning(TTL Serial): If baudrate of packet serial mode is changed communications will be lost!
  	def SetConfig(self,address,config):
  		return self._write2(address,self.Cmd.SETCONFIG,config)
  
  	def GetConfig(self,address):
  		return self._read2(address,self.Cmd.GETCONFIG)
  
  	def SetM1MaxCurrent(self,address,max):
  		return self._write44(address,self.Cmd.SETM1MAXCURRENT,max,0)
  
  	def SetM2MaxCurrent(self,address,max):
  		return self._write44(address,self.Cmd.SETM2MAXCURRENT,max,0)
  
  	def ReadM1MaxCurrent(self,address):
  		data = self._read_n(address,self.Cmd.GETM1MAXCURRENT,2)
  		if data[0]:
  			return (1,data[1])
  		return (0,0)
  
  	def ReadM2MaxCurrent(self,address):
  		data = self._read_n(address,self.Cmd.GETM2MAXCURRENT,2)
  		if data[0]:
  			return (1,data[1])
  		return (0,0)
  
  	def SetPWMMode(self,address,mode):
  		return self._write1(address,self.Cmd.SETPWMMODE,mode)
  
  	def ReadPWMMode(self,address):
  		return self._read1(address,self.Cmd.GETPWMMODE)
  
  	def Open(self):
  		try:
  			self._port = serial.Serial(port=self.comport, baudrate=self.rate, timeout=0.05, interCharTimeout=self.timeout)
  		except:
  			return 0
  		return 1
  
  