def loadServos(self):
model = QStandardItemModel()
devs = il.devices(il.NET_PROT.EUSB)
for dev in devs:
try:
net = il.Network(il.NET_PROT.EUSB, dev)
except il.exceptions.ILCreationError:
continue
found = net.servos()
for servo_id in found:
try:
servo = il.Servo(net, servo_id)
except il.exceptions.ILCreationError:
continue
item = QStandardItem('0x{:02x} ({})'.format(servo_id, dev))
item.setData(servo, Qt.UserRole)
image = QImage(join(_RESOURCES, 'images', 'triton-core.png'))
item.setData(QPixmap.fromImage(image), Qt.DecorationRole)
model.appendRow([item])
self.cboxServos.setModel(model)
import matplotlib.pyplot as plt
import rospy
from std_msgs.msg import Float64
import math
if __name__ == '__main__':
#Setup the network and connection to the motor
#print(il.devices(il.NET_PROT.EUSB))
net = il.Network(il.NET_PROT.EUSB,
'/dev/ttyACM1') #to find lsusb(1) and dmesg(2)
servo_ids = net.servos()
first_id = servo_ids[0]
#print('Will use servo with id: {}'.format(first_id))
servo = il.Servo(net, first_id)
#print(servo.info)
# we want to work in degrees and RPM
servo.units_pos = il.SERVO_UNITS_POS.DEG
#servo.units_vel = il.SERVO_UNITS_VEL.RPM
#Homing
servo.mode = il.SERVO_MODE.HOMING
servo.enable()
servo.homing_start()
servo.homing_wait(timeout=10.)
servo.disable()
print("Home position reached. Elbow ready to juggle.")
# Nb, Mc = brake.initNebula()
# brake.setTorque(Mc, 500)
# time.sleep(2)
# print(brake.readCurrent(Mc))
# brake.setTorque(Mc, 0)
# print('brake command sent')
# brake.close(Nb, Mc)
# print('brake closed')
# print(il.devices())
# rc = RoboClaw('COM7', 0x80)
print('connected to roboclaw')
net = il.Network('COM6', timeout=1000)
servo_ids = net.servos()
first_id = servo_ids[0]
servo = il.Servo(net, first_id, timeout=1000)
servo.mode = il.MODE_PT
servo.units_torque = il.UNITS_TORQUE_NATIVE
print(servo.read(regs.TORQUE_MAX))
print('Connected to Servo')
servo.enable(timeout=2000)
print('Servo Enabled')
time.sleep(10)
# CMF.setMotorSpeed(rc, CMF.compPWM(45, 0))
# time.sleep(.02)
servo.torque = 1000
# servo.wait_reached(timeout=1000)
print("Torque Set")
start = time.time()
print('Start Sleep')