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