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
class Engine(object):
"""
This class holds the serial port and talks to the hardware. Other classes (
e.g., gait, legs and servos) do the calculations for walking.
"""
last_move = None
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 getCurrentAngles(self):
"""
Returns the current angles for all servos in DH space as a dictionary.
angles = {
0: [0.0, 130.26, -115.73, -104.52],
1: [0.0, 130.26, -115.73, -104.52],
2: [0.0, 130.26, -115.73, -104.52],
3: [0.0, 130.26, -115.73, -104.52],
}
FIXME: actually query the servos and get there angles in DH space
"""
angles = {
0: [0.0, 130.26, -115.73, -104.52],
1: [0.0, 130.26, -115.73, -104.52],
2: [0.0, 130.26, -115.73, -104.52],
3: [0.0, 130.26, -115.73, -104.52],
}
return angles
def DH2Servo(self, angle, num):
return self.servos[num].DH2Servo(angle)
def moveLegsGait4(self, legs):
"""
gait or sequence?
speed = 1 - 1023 (scalar, all servos move at same rate)
{ step 0 step 1 ...
0: [(t1,t2,t3,t4,speed), (t1,t2,t3,t4,speed), ...] # leg0
2: [(t1,t2,t3,t4,speed), (t1,t2,t3,t4,speed), ...] # leg2
...
} where t=theta
NOTE: each leg needs the same number of steps and servos per leg
WARNING: these angles are in servo space [0-300 deg]
[Join Mode]
0 ~ 1,023(0x3FF) can be used, and the unit is about 0.111rpm.
If it is set to 0, it means the maximum rpm of the motor is used
without controlling the speed. If it is 1023, it is about 114rpm.
For example, if it is set to 300, it is about 33.3 rpm.
AX12 max rmp is 59 (max speed 532)
rev min 360 deg deg
--- ------- ------- = 6 ----
min 60 sec rev sec
sleep time = | (new_angle - old_angle) /(0.111 * min_speed * 6) |
"""
# get the keys and figure out some stuff
keys = list(legs.keys()) # which legs are we moving
numSteps = len(legs[keys[0]]) # how many steps in the cycle
numServos = len(legs[keys[0]][0])-1 # how many servos per leg, -1 because speed there
# if self.last_move is None:
# # assume we just turned on and was in the sit position
# # need a better solution
# self.last_move = {
# 0: legs[0][0],
# 1: legs[1][0],
# 2: legs[2][0],
# 3: legs[3][0]
# }
# curr_move = {}
# for each step in legs
for step in range(numSteps):
# dprint("\nStep[{}]===============================================".format(step))
data = []
# find max time we have to wait for all 4 legs to reach their end
# point.
max_wait = 0
for legNum in keys:
angles = legs[legNum][step][:4]
speed = legs[legNum][step][4]
# print(" speed", speed)
for a, oa in zip(angles, self.last_move[legNum][:4]):
da = abs(a-oa)
w = calc_wait(da, speed)
# print(" calc_wait: {:.3f}".format(w))
# print("calc_wait: {}".format(w))
max_wait = w if w > max_wait else max_wait
# print(">> found wait", max_wait, " speed:", speed)
servo_speeds = [100,125,150,200]
# for legNum in [0,3,1,2]:
for legNum in keys:
# dprint(" leg[{}]--------------".format(legNum))
leg_angles_speed = legs[legNum][step]
# print(leg_angles_speed)
angles = leg_angles_speed[:4] # 4 servo angles
speed = leg_angles_speed[4]
# print("Speed:", speed, "wait", max_wait)
for i, DH_angle in enumerate(angles):
# oldangle = self.last_move[legNum][i]
# due to rounding errors, to ensure the other servers finish
# BEFORE time.sleep(max_wait) ends, the the function it
# has less time
# spd = calc_rpm((angle - oldangle), 0.9*max_wait)
# now that i am scaling the spd parameter above, I sometimes
# exceed the original speed number, so saturate it if
# necessary
# spd = spd if spd <= speed else speed
# sl, sh = le(spd)
sl, sh = le(servo_speeds[i])
servo_angle = self.DH2Servo(DH_angle, i)
al, ah = angle2int(servo_angle) # angle
data.append([legNum*numServos + i+1, al, ah, sl, sh]) # ID, low angle, high angle, low speed, high speed
# data.append([legNum*numServos + i+1, al, ah]) # ID, low angle, high angle, low speed, high speed
self.last_move[legNum] = leg_angles_speed
pkt = self.packet.makeSyncWritePacket(self.packet.base.GOAL_POSITION, data)
self.serial.write(pkt)
dprint("sent serial packet leg: {}".format(legNum))
data = []
print('max_wait', max_wait)
time.sleep(max_wait)
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)