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))
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)