def main():
args = handleArgs()
port = args['port']
s = ServoSerial(port=port)
# open serial port
try:
s.open()
except SerialException as e:
print('-'*20)
print(sys.argv[0], 'encountered an error')
print(e)
exit(1)
ids = range(1, 13)
resp = {}
for k in ids:
resp[k] = None
# get servo data
try:
for i in ids:
data = getSingle(i, s)
resp[i] = data
except Exception as e:
print(e)
exit(1)
cnt = 10
while cnt:
cnt = 0
for k, v in resp.items():
# search through and find servos w/o responses (i.e., None)
if v is None:
cnt += 1 # found a None
ans = getSingle(k, s)
resp[k] = ans
print('')
print('Servos: 1 - 12')
for i in range(1, 13):
printServo(resp[i])
print('-' * 50)
print('')
if args['json']:
print('Saving servo angle info to {}'.format(args['json']))
writeToFile(resp, args['json'])
s.close()
def main():
args = handleArgs()
port = args['port']
if args['all']:
ID = xl320.XL320_BROADCAST_ADDR
else:
ID = args['id']
if port.lower() == 'dummy':
ser = ServoSerial(port=port, fake=True)
else:
ser = ServoSerial(port=port)
ser.open()
pkt = Packet.makeRebootPacket(ID)
# print('pkt', pkt)
ser.write(pkt)
gramar = 'is'
if ID == xl320.XL320_BROADCAST_ADDR:
ID = 'all'
gramar = 'are'
print('Servo[{}] {} rebooting'.format(ID, gramar))
def main():
filename = 'test.json'
port = '/dev/tty.usbserial0'
data = JsonFile.read(filename)
print(data)
if port == 'dummy':
ser = ServoSerial(port=port, fake=True)
else:
ser = ServoSerial(port=port)
ser.open()
for servo in data:
ID = servo['id']
servo.pop('id')
for key, val in servo.items():
print('> setting servo[{}]: {} {}'.format(ID, key, val))
if key in xl320_func:
func = xl320_func[key][0] # grab correct function
pkt = func(ID, val) # build packet
# print('pkt', pkt)
ser.sendPkt(pkt) # send packet
else:
print('>> Error: {} not currently supported'.format(key))
def main():
args = handleArgs()
port = args['port']
s = ServoSerial(port=port)
s.open()
ids = range(1, 13)
resp = {}
for k in ids:
resp[k] = None
# as more servos add up, I might need to increase the cnt number???
for i in ids:
ID, angle = getSingle(i, s)
resp[i] = angle
cnt = 10
while cnt:
cnt = 0
for k, v in resp.items():
# search through and find servos w/o responses (i.e., None)
if v is None:
cnt += 1 # found a None
ID, angle = getSingle(k, s)
resp[k] = angle
print('')
print('Servos: 1 - 12')
print('All angles are in {}'.format('degrees'))
print(' {:>13} | {:>13} | {:>13} | {:>13} |'.format('Leg 1', 'Leg 2', 'Leg 3', 'Leg 4'))
print(' {:>4} | {:6} | {:>4} | {:6} | {:>4} | {:6} | {:>4} | {:6} |'.format('ID', 'Angle', 'ID', 'Angle', 'ID', 'Angle', 'ID', 'Angle'))
print('-' * 65)
# for k, v in resp.items():
# if v is None:
# print('{:4} | {}'.format(k, 'unknown'))
# else:
# print('{:4} | {:6.2f}'.format(k, v))
for i in range(1, 4):
print(' {:4} | {:6.2f} | {:4} | {:6.2f} | {:4} | {:6.2f} | {:4} | {:6.2f}'.format(i, resp[i], i+3, resp[i+3], i+6, resp[i+6], i+9, resp[i+9]))
print('-' * 65)
print('')
if args['json']:
print('Saving servo angle info to {}'.format(args['json']))
writeToFile(resp, args['json'])
s.close()
def __init__(self, data={}):
"""
Sets up all 4 legs and servos. Also setups limits for angles and servo
pulses.
"""
if 'serialPort' in data:
try:
ser = ServoSerial(data['serialPort'])
print('Using servo serial port: {}'.format(data['serialPort']))
except:
print('bye ...')
exit(1)
else:
print('Using dummy serial port!!!')
ser = ServoSerial('test_port', fake=True)
# raise Exception('No serial port given')
ser.open()
Servo.ser = ser # set static serial port, not sure I like this
if 'write' in data:
method = data['write']
if method == 'sync':
Servo.syncServoWrite = True # FIXME: this is broken
print('*** using sync write ***')
elif method == 'bulk':
Servo.bulkServoWrite = True
print('*** using bulk write ***')
self.legs = []
for i in range(0, 4): # 4 legs
channel = i*3 # 3 servos per leg
self.legs.append(
Leg([channel+1, channel+2, channel+3])
)
# better way?????
self.servoWrite = self.legs[0].servos[0].write
self.stand()
def main():
args = handleArgs()
port = args['port']
if port.lower() == 'dummy':
s = ServoSerial(port=port, fake=True)
else:
s = ServoSerial(port=port)
s.open()
ids = range(args['start'], args['end'] + 1)
resp = {}
for k in ids:
resp[k] = None
# as more servos add up, I might need to increase the cnt number???
for i in ids:
ID, angle = getSingle(i, s)
resp[i] = angle
for _ in range(3):
cnt = 0
for k, v in resp.items():
# search through and find servos w/o responses (i.e., None)
if v is None:
cnt += 1 # found a None
ID, angle = getSingle(k, s)
resp[k] = angle
if cnt == 0: # we must have gotten them all on the last run
break
print('')
print('Servos: {} - {}'.format(args['start'], args['end']))
print('{:>4} | {:6}'.format('#', 'Angle'))
print('-' * 40)
for k, v in resp.items():
if v is None:
print('{:4} | {}'.format(k, 'unknown'))
else:
print('{:4} | {:6.2f}'.format(k, v))
print('')
if args['json']:
print('Saving servo angle info to {}'.format(args['json']))
writeToFile(resp, args['json'])
s.close()
def main():
args = handleArgs()
port = args['port']
s = ServoSerial(port=port)
s.open()
ids = range(1, 13)
resp = {}
for k in ids:
resp[k] = None
# as more servos add up, I might need to increase the cnt number???
for i in ids:
angle = getSingle(i, s)
resp[i] = angle
cnt = 10
while cnt:
cnt = 0
for k, v in resp.items():
# search through and find servos w/o responses (i.e., None)
if v is None:
cnt += 1 # found a None
angle = getSingle(k, s)
resp[k] = angle
print('')
print('Servos: 1 - 12')
print('All angles are in {}'.format('degrees'))
print(' {:>13} | {:>13} | {:>13} | {:>13} |'.format(
'Leg 1', 'Leg 2', 'Leg 3', 'Leg 4'))
print(
' {:>4} | {:6} | {:>4} | {:6} | {:>4} | {:6} | {:>4} | {:6} |'.format(
'ID', 'Angle', 'ID', 'Angle', 'ID', 'Angle', 'ID', 'Angle'))
print('-' * 65)
# for k, v in resp.items():
# if v is None:
# print('{:4} | {}'.format(k, 'unknown'))
# else:
# print('{:4} | {:6.2f}'.format(k, v))
for i in range(1, 4):
print(
' {:4} | {:6.2f} | {:4} | {:6.2f} | {:4} | {:6.2f} | {:4} | {:6.2f}'
.format(i, resp[i], i + 3, resp[i + 3], i + 6, resp[i + 6], i + 9,
resp[i + 9]))
print('-' * 65)
print('')
if args['json']:
print('Saving servo angle info to {}'.format(args['json']))
writeToFile(resp, args['json'])
s.close()
def main(): args = handleArgs() port = args['port'] ID = args['id'] curr_rate = args['current_rate'] # new_id = args['newid'] # 0: 9600, 1:57600, 2:115200, 3:1Mbps new_rate = rates[args['new_rate']] if port == 'dummy': ser = ServoSerial(port=port, baud_rate=curr_rate, fake=True) else: ser = ServoSerial(port=port, baud_rate=curr_rate) ser.open() pkt = Packet.makeBaudRatePacket(ID, new_rate) ser.sendPkt(pkt)
def main():
args = handleArgs()
port = args['port']
s = ServoSerial(port=port)
# open serial port
try:
s.open()
except SerialException as e:
print('-' * 20)
print(sys.argv[0], 'encountered an error')
print(e)
exit(1)
ids = range(1, 13)
resp = {}
for k in ids:
resp[k] = None
# get servo data
try:
for i in ids:
data = getSingle(i, s)
resp[i] = data
except Exception as e:
print(e)
exit(1)
cnt = 10
while cnt:
cnt = 0
for k, v in resp.items():
# search through and find servos w/o responses (i.e., None)
if v is None:
cnt += 1 # found a None
ans = getSingle(k, s)
resp[k] = ans
print('')
print('Servos: 1 - 12')
for i in range(1, 13):
printServo(resp[i])
print('-' * 50)
print('')
if args['json']:
print('Saving servo angle info to {}'.format(args['json']))
writeToFile(resp, args['json'])
s.close()
def main():
args = handleArgs()
ID = args['id']
port = args['port'] # '/dev/tty.usbserial-A5004Flb'
angle = args['angle']
print('Setting servo[{}] to {:.2f} on port {}'.format(ID, angle, port))
if port.lower() == 'dummy':
serial = ServoSerial(port=port, fake=True)
else:
serial = ServoSerial(port=port)
serial.open()
pkt = Packet.makeServoPacket(ID, angle) # move servo 1 to 158.6 degrees
ans = serial.sendPkt(pkt) # send packet to servo
if ans:
print('status: {}'.format(ans))
def run():
# sport = '/dev/serial0'
sport = None
if sport:
ser = ServoSerial(sport)
else:
ser = DummySerial('test')
Servo.ser = ser # set static servo serial comm
leg = Leg([1, 2, 3])
foot0 = leg.foot0
print('foot0', foot0)
angles = [0, 0, 0]
pts = leg.fk(*angles) # allows angles out of range
print('pts', pts)
leg.moveFoot(*pts)
sit = Pose(leg.foot0)
sit.run()
time.sleep(2)
print('Bye')
def main():
args = handleArgs()
if args['interactive']:
port, curr_id, new_id = makeInterActive()
else:
port = args['port']
curr_id = args['current_id']
new_id = args['new_id']
if port == 'dummy':
ser = ServoSerial(port=port, fake=True)
else:
ser = ServoSerial(port=port)
ser.open()
pkt = makeServoIDPacket(curr_id, new_id)
print('pkt', pkt)
err, err_str = ser.sendPkt(pkt)
if err:
print('Error: {} {}'.format(err, err_str))
class XL320(object):
def __init__(self):
self.serial = ServoSerial('/dev/ttyAMA0')#tell it what port you want to use
#self.serial.close()
self.serial.open()
def setData(self,memAddress,ID,data):
"""
Set a memory address (memAddress) from servo ID to "data"
"""
numBytes = Packet.dataLength(memAddress)
if (numBytes == 2):
pkt = Packet.makeWritePacket(ID,memAddress,Packet.le(data))
else:
pkt = Packet.makeWritePacket(ID,memAddress,[data])
ans,err_num,err_str = self.serial.sendPkt(pkt)
if ans == [] or len(ans) < 9:#In case of an empty packet arrives
return -2
else:
return ans[8]#Byte of error status
def setNDataN(self,memAddress,listOf_IDsData):
"""
listOf_IDsData = [[id0,data0],[id1,data1],...,[idN,dataN]]
"""
pkt = Packet.makeSyncWritePacket(memAddress,listOf_IDsData)
ans,err_num,err_str = self.serial.sendPkt(pkt)
if ans == []:#In case of an empty packet arrives
return -2#This instruction doesn't return status packet
def getData(self,memAddress,*id_list):
"""
id_list = id0,id1,...,idN
"""
numBytes = Packet.dataLength(memAddress)
if(numBytes == 1):
ans = self.getByte(memAddress,id_list)
else:#numBytes==2
ans = self.getWord(memAddress,id_list)
return ans
def setByte(self,memAddress,ID,data):
#Already coded in setData()
pass
def getByte(self,memAddress,id_list):
"""
Gets a list of IDs and return its Model Numbers
inside a list (id0,data0,id1,data1,...idN,dataN)
"""
if id_list == ():#Empty list
return -1
elif len(id_list) == 1:#Just one ID.
pkt = Packet.makeReadPacket(id_list[0],memAddress)
else:
pkt = Packet.makeSyncReadPacket(memAddress,id_list)
ans,err_num,err_str = self.serial.sendPkt(pkt)
if ans == []:#In case of an empty packet arrives
return -2
else:
data = []
for index,val in enumerate(id_list):
data.append(val) #Append the ID value
data.append(ans[index*12+9])#Append the respective ID's data
return data
def setWord(self,memAddress,ID,data):
#Already coded in setData()
pass
# def setWord(self,memAddress,listOf_IDsData):
# """
# listOf_IDsData = [[id0,data0],[id1,data1],...,[idN,dataN]]
# """
# if listOf_IDsData == ():#Empty list
# return -1
# elif len(listOf_IDsData) == 2:#Just one ID and its respectively data
# pkt = Packet.makeWritePacket(listOf_IDsData[0],memAddress,listOf_IDsData[1])
# else:#more than one ID
# pkt = Packet.makeSyncWritePacket(memAddress,listOf_IDsData)
#
# ans,err_num,err_str = self.serial.sendPkt(pkt)
# if ans == []:#In case of an empty packet arrives
# return -2 #OBS.: There's no status packet for "makeSyncWritePacket()"
# else:
# return ans[8]#Byte of error status
def getWord(self,memAddress,id_list):
"""
Gets a list of IDs and return its Model Numbers
inside a list (id0,data0,id1,data1,...idN,dataN)
"""
if id_list == ():#Empty list
return -1
elif len(id_list) == 1:#Just one ID.
pkt = Packet.makeReadPacket(id_list[0],memAddress)
else:
pkt = Packet.makeSyncReadPacket(memAddress,id_list)
ans,err_num,err_str = self.serial.sendPkt(pkt)
if ans == []:#In case of an empty packet arrives
return -2
else:
data = []
for index,val in enumerate(id_list):
#print (index,val)
data.append(val) #Append the ID value
#print str(index*13+10)
#print str(index*13+9)
try:
data.append(ans[index*13+9]+(ans[index*13+10] << 8))#Append the respective ID's data
except IndexError:
print 'Index Error: '+str(len(ans))
return data
"""def getModelNumber(self, *id_list):
print (id_list)
print (type(id_list))
if id_list == ():
return -2
if len(id_list) == 1:#Just one ID.
pkt = Packet.makeReadPacket(id_list,xl320.XL320_MODEL_NUMBER)
ans,err_num,err_str = self.serial.sendPkt(pkt)
if ans == []:#In case of an empty packet arrives
return -1
else:
return (ans[9]+(ans[10] << 8))
else:
pkt = Packet.makeSyncReadPacket(xl320.XL320_MODEL_NUMBER,id_list)
ans,err_num,err_str = self.serial.sendPkt(pkt)
if ans == []:
return -1
else:
data = []
for index,val in enumerate(id_list):
#print (index,val)
data.append(val) #Append the ID value
data.append(ans[index*13+9]+(ans[index*13+10] << 8))#Append the respective ID's data
return data"""
def getModelNumber(self, *id_list):
"""
Gets a list of IDs and return its Model Numbers
inside a list (id0,data0,id1,data1,...idN,dataN)
"""
if id_list == ():#Empty list
return -1
elif len(id_list) == 1:#Just one ID.
pkt = Packet.makeReadPacket(id_list[0],xl320.XL320_MODEL_NUMBER)
else:
pkt = Packet.makeSyncReadPacket(xl320.XL320_MODEL_NUMBER,id_list)
ans,err_num,err_str = self.serial.sendPkt(pkt)
if ans == []:#In case of an empty packet arrives
return -2
else:
data = []
for index,val in enumerate(id_list):
#print (index,val)
data.append(val) #Append the ID value
data.append(ans[index*13+9]+(ans[index*13+10] << 8))#Append the respective ID's data
return data
def getFirmwareVersion(self, *id_list):
"""
Gets a list of IDs and return its Firmware Version
inside a list (id0,data0,id1,data1,...idN,dataN)
"""
if id_list == ():#Empty list
return -1
elif len(id_list) == 1:#Just one ID.
pkt = Packet.makeReadPacket(id_list[0],xl320.XL320_FIRMWARE_VERSION)
else:
pkt = Packet.makeSyncReadPacket(xl320.XL320_FIRMWARE_VERSION,id_list)
ans,err_num,err_str = self.serial.sendPkt(pkt)
if ans == []:#In case of an empty packet arrives
return -2
else:
data = []
for index,val in enumerate(id_list):
#print (index,val)
data.append(val) #Append the ID value
data.append(ans[index*12+9])#Append the respective ID's data
return data
def setID(self,curr_id,new_id):
pkt = Packet.makeWritePacket(curr_id,xl320.XL320_ID,[new_id])
ans,err_num,err_str = self.serial.sendPkt(pkt)
if ans == []:#In case of an empty packet arrives
return -2
else:
return ans[8]#Byte of error status
def getID(self):
pass
def setBaudRate(self,ID,new_baudrate):
pass
help='serial port',
type=str,
default='/dev/serial0')
# parser.add_argument('-g', '--gpio', help='Raspberry Pi GPIO pin number', type=int, default=17)
args = vars(parser.parse_args())
return args
args = handleArgs()
port = args['port']
angle = args['angle']
if port:
print('Opening: {}'.format(port))
serial = ServoSerial(port) # use this if you want to talk to real servos
else:
print('Using dummy serial port for testing')
serial = DummySerial(port) # use this for simulation
serial.open()
data = [(1, angle), (2, angle), (3, angle), (4, angle), (5, angle), (6, angle),
(7, angle), (8, angle), (9, angle), (10, angle), (11, angle),
(12, angle)]
pkt = makeSyncAnglePacket(data)
serial.sendPkt(pkt)
serial.close()
else:
ID = args['id']
level = args['level']
if level == 0:
level = xl320.XL320_RESET_ALL
elif level == 1:
level = xl320.XL320_RESET_ALL_BUT_ID
elif level == 2:
level = xl320.XL320_RESET_ALL_BUT_ID_BAUD_RATE
else:
print('Sorry you selected an invalid reset level')
exit()
if port.lower() == 'dummy':
ser = ServoSerial(port=port, fake=True)
else:
ser = ServoSerial(port=port)
ser.open()
pkt = Packet.makeResetPacket(ID, level)
if args['all']:
servo_str = 'all servos'
else:
servo_str = 'servo ID {}'.format(args['id'])
if level == xl320.XL320_RESET_ALL:
level_str = 'all parameters'
elif level == xl320.XL320_RESET_ALL_BUT_ID:
level_str = 'all parameters but ID'
help='serial port',
type=str,
default='/dev/serial0')
# parser.add_argument('-g', '--gpio', help='Raspberry Pi GPIO pin number', type=int, default=17)
args = vars(parser.parse_args())
return args
args = handleArgs()
port = args['port']
angle = args['angle']
if port:
print('Opening: {}'.format(port))
serial = ServoSerial(port) # use this if you want to talk to real servos
else:
print('Using dummy serial port for testing')
serial = DummySerial(port) # use this for simulation
serial.open()
data = [(1, angle), (2, angle), (3, angle), (4, angle), (5, angle), (6, angle),
(7, angle), (8, angle), (9, angle), (10, angle), (11, angle),
(12, angle)]
pkt = Packet.makeBulkAnglePacket(data)
serial.write(pkt) # there is no returned status packet, so just write
# serial.write(pkt)
print('Raw Packet:', pkt)
sleep(1)
# modify these for your system
ID = xl320.XL320_BROADCAST_ADDR # I seem to get better performance on braodcast
# port = '/dev/tty.usbserial-A5004Flb'
# port = '/dev/tty.usbserial-A700h2xE'
# port = '/dev/tty.usbserial-A5004Fnd'
# port = '/dev/tty.usbserial-A5004Flb'
port = '/dev/tty.usbserial-AL034G2K'
speed = 0.5 # 50% speed
if port is 'dummy':
serial = DummySerial(port) # use this for simulation
else:
serial = ServoSerial(port)
serial.open()
pkt = Packet.makeServoSpeedPacket(ID, speed) # set servo speed
serial.sendPkt(pkt)
try:
while True:
run(ID, serial, 300, 0, 10)
print('>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>')
run(ID, serial, 0, 300, -10)
print('<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<')
sleep(2)
except KeyboardInterrupt:
# e = sys.exc_info()[0]
def sweep(port, rate, ID, retry=3):
"""
Sends a ping packet to ID's from 0 to maximum and prints out any returned
messages.
Actually send a broadcast and will retry (resend) the ping 3 times ...
"""
if port == 'dummy':
s = ServoSerial(port, rate, fake=True)
else:
s = ServoSerial(port, rate)
if ID < 0:
ID = xl320.XL320_BROADCAST_ADDR
try:
s.open()
except SerialException as e:
# print('Error opening serial port:')
print('-' * 40)
print(sys.argv[0], ':')
print(e)
exit(1)
pkt = makePingPacket(ID)
# print('ping', pkt)
s.write(pkt)
# as more servos add up, I might need to increase the cnt number???
for cnt in range(retry):
ans = s.read()
if ans:
for pkt in ans:
servo = packetToDict(pkt)
utils.prettyPrintPacket(servo)
print('raw pkt: {}'.format(pkt))
else:
print('Try {}: no servos found'.format(cnt))
time.sleep(0.1)
s.close()
def __init__(self):
self.serial = ServoSerial('/dev/ttyAMA0')#tell it what port you want to use
#self.serial.close()
self.serial.open()