def setPWM(self, pwm):
# Do not execute if PWM is out of range or if in PID mode
if (pwm < -255) or (pwm > 255) or self.isPID:
return False
# Update internal memory of direction
if pwm < 0:
self.cw = False
elif pwm > 0:
self.cw = True
# Calculate absolute value
pwm = abs(pwm)
# Assemble packet and write to USB, return true for success
#packet = pu.getPacket(0x00, self.cid, list([pwm, int(self.cw)]))
#print list(packet)
#print packet
#print self.calc_checksum(list(packet))
#self.ser.write(packet)
self.send_cmd(0x00, self.cid, [pwm, int(self.cw)])
#data = []
data = []
for i in xrange(9):
data.append(ord(su.readBytes(self.ser, 1)[0]))
print("Recieved: ")
print data
return True
def getFault(self):
# Assemble packet and write to SPI
packet = pu.getPacket(0x0F, self.cid, list())
self.ser.write(packet)
# Read one byte
resp = su.readBytes(1)
# Check if the read failed, if yes, return -1
if pu.isEmpty(resp):
return -1
# If read succeeded, return first element, the fault state
return resp[0]
def getCurrent(self):
# Assemble packet and write to USB
packet = pu.getPacket(0x04, self.cid, list())
self.ser.write(packet)
# Read two bytes
resp = su.readBytes(2)
# Check if the read failed, if yes, return -1
if pu.isEmpty(resp):
return -1
# If read succeeded, return current in mA as short
return su.BytesToShort(resp)
def getEncoder(self):
# Assemble packet and write to USB
packet = pu.getPacket(0x08, self.cid, list())
self.ser.write(packet)
# Read four bytes
resp = su.readBytes(4)
# Check if the read failed, if yes, return -1
if pu.isEmpty(resp):
return -1
# Get Integer
return su.BytesToInt(resp[0:4])
def setangle(self, angle):
# Do not execute if angle is out of range or servo is continuous
# Convert to proper angle range (0 to 180)
#newangle = int(round(angle * (180.0/float(self.angle))))
LSB = 0x00ff & angle
MSB = (0xff00 & angle) >> 8
# 0x10 for Servo 1, 0x12 for Servo 2
pack_sid = 0x10
if self.sid == 2:
pack_sid = 0x12
# Assemble packet and write to USB, return true for success
#print("settign servo")
self.send_cmd(pack_sid, self.cid, list([LSB, MSB]))
data = []
for i in xrange(9):
data.append(ord(su.readBytes(self.ser, 1)[0]))
print("Recieved: ")
print data
return True
