Permalink
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
1043 lines (898 sloc) 26.2 KB
"""
This file was created by Ion Motion Control as an API for their RoboClaw motor
controller. It has been released into the public domain as per
http://forums.ionmc.com/viewtopic.php?t=14
"""
import random
import serial
import struct
import time
class Roboclaw:
'Roboclaw Interface Class'
def __init__(self, comport, rate, timeout=0.01, retries=3):
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=1, interCharTimeout=self.timeout)
except:
return 0
return 1