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 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 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 setPID(self, kp, kd, ki):
# Do not execute if any constant is negative
if (kp < 0) or (kd < 0) or (ki < 0):
return False
# Convert all constants to bytes
data = list()
data.extend(su.FloatToBytes(kp))
data.extend(su.FloatToBytes(kd))
data.extend(su.FloatToBytes(ki))
# Assemble packet and write to USB, return true for success
packet = pu.getPacket(0x05, self.cid, data)
self.ser.write(packet)
return True
def setspeed(self, speed):
# Do not execute if speed is out of range or servo is not continuous
# On motor board, 0 = full reverse, 90 is stop, 180 is full forward
# Add 90 to produce this result given a -90 to 90 range
data = list([speed + 90])
# 0x11 for Servo 1, 0x13 for Servo 2
pack_sid = 0x11
if self.sid == 2:
pack_sid = 0x13
# Assemble packet and write to USB, return true for success
packet = pu.getPacket(pack_sid, self.cid, data)
self.ser.write(packet)
return True
def setSpeed(self, rpm):
# Do not execute if RPM is out of bounds or not in PID mode
if (rpm < -65000) or (rpm > 65000) or not self.isPID:
return False
# Update internal memory of direction
if rpm < 0:
self.cw = False
elif rpm > 0:
self.cw = True
# Create payload
rpm = abs(rpm)
data = list()
data.extend(su.ShortToBytes(rpm))
data.append(int(self.cw))
# Assemble packet and write to SPI, return true for success
packet = pu.getPacket(0x06, self.cid, data)
self.ser.write(packet)
return True
def rotateSpeed(self, rots, rpm):
# Do not execute if RPM is out of range, rotations < 0, or if not in PID mode
if (rpm < -65000) or (rpm > 65000) or (rots < 0) or not self.isPID:
return False
# Update internal memory of direction
if rpm < 0:
self.cw = False
elif rpm > 0:
self.cw = True
rpm = abs(rpm)
# Assemble payload -- pwm, then direction, then # rotations
data = list()
data.extend(su.ShortToBytes(rpm))
data.append(int(self.cw))
data.extend(su.FloatToBytes(rots))
# Assemble packet and write to USB, return true for success
packet = pu.getPacket(0x0A, self.cid, data)
self.ser.write(packet)
return True
def rotatePWM(self, rots, pwm):
# Do not execute if PWM is out of range, rotations < 0, or if in PID mode
if (pwm < -255) or (pwm > 255) or (rots < 0) or self.isPID:
return False
# Update internal memory of direction
if pwm < 0:
self.cw = False
elif pwm > 0:
self.cw = True
pwm = abs(pwm)
# Assemble payload -- pwm, then direction, then # rotations
data = list()
data.append(pwm)
data.append(int(self.cw))
data.extend(su.FloatToBytes(rots))
# Assemble packet and write to USB, return true for success
packet = pu.getPacket(0x09, self.cid, data)
self.ser.write(packet)
return True
def stop(self):
# Assemble packet and write to USB, return true for success
packet = pu.getPacket(0x0E, self.cid, list())
self.ser.write(packet)
return True
def resetEncoder(self):
# Assemble packet and write to USB, return true for success
packet = pu.getPacket(0x07, self.cid, list())
self.ser.write(packet)
return True
