def main():
port = "/dev/tty.usbserial-AL034G1T"
serial = ServoSerial(port=port)
serial.open()
servo = Packet(pyservos.AX12)
# servo = Packet(pyservos.XL320)
# build
# 59 rpm max
data = []
ids = [1, 2, 3]
pos = [150, 200, 90]
vel = [200, 100, 300] # 0.111 rpm
for i, p, v in zip(ids, pos, vel):
data.append([i] + angle2int(p) + le(v))
pkt = servo.makeSyncWritePacket(pyservos.AX12.GOAL_POSITION, data)
print(pkt)
ans = serial.sendPkt(pkt) # send packet to servo
ans = servo.processStatusPacket(ans)
if ans:
print('status: {}'.format(ans))
def __init__(self, data, servoType):
"""
data: serial port to use, if none, then use dummy port
servoType: AX12 or XL320 or other servo type
curr_pos: current leg position
bcm_pin: which gpio pin is used for the comm
{
0: [(t0,t1,t2,t3,speed), ...]
1: [...]
...
3: [...]
}
"""
if 'bcm_pin' in data:
bcm_pin = data['bcm_pin']
else:
bcm_pin = None
# self.wait = wait
# determine serial port
# default to fake serial port
if 'serialPort' in data:
try:
self.serial = ServoSerial(data['serialPort'], pi_pin=bcm_pin)
print('Using servo serial port: {}'.format(data['serialPort']))
self.serial.open()
except Exception as e:
print(e)
print('Engine::init(): bye ...')
exit(1)
else:
print('*** Using dummy serial port!!! ***')
self.serial = ServoSerial('dummy')
# raise Exception('No serial port given')
# handle servos ... how does it know how many servos???
self.servos_up = {} # check servos are operating
self.servos = []
for ID, seg in enumerate(['coxa', 'femur', 'tibia', 'tarsus']):
length, offset = data[seg]
self.servos.append(Servo(ID, offset))
# resp = self.pingServo(ID) # return: (T/F, servo_angle)
# self.servos[ID] = resp[0]
# curr_angle[ID] =
# for s, val in self.servos.items():
# if val is False:
# print("*** Engine.__init__(): servo[{}] has failed".format(s))
self.packet = Packet(servoType)
# keep track of last location, this is servo angle space
# self.last_move = {
# 0: curr_pos[0][0],
# 1: curr_pos[1][0],
# 2: curr_pos[2][0],
# 3: curr_pos[3][0]
# }
self.last_move = self.getCurrentAngles()
def get(port, IDs, bcm_pin, 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 ...
"""
valid_return = False
s = ServoSerial(port, pi_pin=bcm_pin)
try:
s.open()
except Exception as e:
print('-' * 40)
print(sys.argv[0], ':')
print(e)
exit(1)
servo = Packet(pyservos.AX12)
found_servos = {}
# print("read:")
for id in IDs:
# print(id, end=' ')
pkt = makeAngleReadPacket(id)
s.write(pkt)
valid_return = False
# as more servos add up, I might need to increase the cnt number???
for cnt in range(retry):
ans = s.read()
if ans:
valid_return = True
pkts = servo.decodePacket(ans)
# print(pkts)
for pkt in pkts:
if pkt[:5] == [0xff, 0xff, id, 4, 0]:
angle = ((pkt[6] << 8) + pkt[5]) * 0.29 # cnts to deg
found_servos[id] = angle
# print('Found')
break
if not valid_return:
print(id, 'ERROR: Could not read servo angle')
time.sleep(0.1)
s.close()
return valid_return, found_servos
def makeAngleReadPacket(id):
servo = Packet(pyservos.AX12) # remove
pkt = servo.makeReadPacket(id, 36, 2) # self.base.makeReadPacket()
return pkt
# you will need to CHANGE this to the correct port
port = "/dev/ttyS0"
ID = 1
angle = int(sys.argv[1])
print('Setting servo[{}] to {:.2f} on port {}'.format(ID, angle, port))
try:
serial = ServoSerial(port=port)
serial.open()
except Exception as e:
print(e)
print('Oops, wrong port')
print('bye ....')
exit(1)
if True:
servo = Packet(pyservos.AX12)
print('We are talking to an AX12 servo')
else:
servo = Packet(pyservos.XL320)
print('We are talking to an XL320 servo')
pkt = servo.makeServoMovePacket(ID, angle)
ans = serial.sendPkt(pkt) # send packet to servo
ans = servo.processStatusPacket(ans)
if ans:
print('status: {}'.format(ans))
speed = 1024 + abs(speed)
pkt = servo.makeWritePacket(ID, servo.base.GOAL_VELOCITY, le(speed))
return pkt
ID = 1
port = "/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A904MISU-if00-port0"
# port = "/dev/serial/by-id/usb-1a86_USB2.0-Serial-if00-port0"
serial = ServoSerial(port=port)
serial.open()
print(">> baudrate:", serial.serial.baudrate)
servo = Packet(pyservos.AX12)
pkt = set_wheel_mode(ID)
ans = serial.sendPkt(pkt) # send packet to servo
for speed in range(-1023, 1023, 10):
print(">>", speed)
pkt = set_wheel_speed(ID, speed)
ans = serial.sendPkt(pkt) # send packet to servo
time.sleep(0.1)
speed = 0
# pkt = servo.makeWritePacket(ID, servo.base.GOAL_VELOCITY, le(speed))
pkt = set_wheel_speed(ID, speed)
ans = serial.sendPkt(pkt) # send packet to servo
from pyservos import Packet
from pyservos.servoserial import ServoSerial
import pyservos
from pyservos.utils import angle2int, le
import time
servoType = pyservos.XL320
servoStr = 'XL-320'
port = "/dev/ttyS0"
offset = 5
speed = 312
serial = ServoSerial(port=port)
serial.open()
servo = Packet(servoType)
def set_angle(id, angle):
val = angle2int(angle, degrees=True) + le(speed)
pkt = servo.makeWritePacket(id, servo.base.GOAL_POSITION, val)
serial.sendPkt(pkt) # send packet to servo
while True:
set_angle(2, 0)
set_angle(3, 300 - offset)
time.sleep(4)
set_angle(2, 300 - offset)
set_angle(3, 0)
time.sleep(4)