def makePacket(self, ID, instr, params=None):
"""
This makes a generic packet.
TODO: look a struct ... does that add value using it?
0xFF, 0xFF, 0xFD, 0x00, ID, LEN_L, LEN_H, INST, PARAM 1, PARAM 2, ..., PARAM N, CRC_L, CRC_H]
in:
ID - servo id
instr - instruction
params - [register, instruction parameter values]
out: packet
"""
pkt = []
# [header, reserved, 0x00, ID, len low, len high, instruction]
pkt += [0xFF, 0xFF, 0xFD, 0x00, ID, 0x00, 0x00, instr] # header
# pkt += [0x00] # reserved byte
# pkt += [ID]
# pkt += [0x00, 0x00] # length placeholder
# pkt += [instr] # instruction
# if reg:
# pkt += le(reg) # not everything has a register
if params:
pkt += params # not everything has parameters
length = le(len(pkt) - 5) # length = len(packet) - (header(3), reserve(1), id(1))
pkt[5] = length[0] # L
pkt[6] = length[1] # H
crc = self.check_sum(pkt)
pkt += le(crc)
return pkt
def test_angle2int():
for angle in [0, 90, 180, 270, 300]:
a = int(angle/300.0*1023.0)
assert angle2int(angle, True) == le(a)
for angle in [0, pi/4, pi/2, 3*pi/2]:
a = int(180*angle/300.0/pi*1023.0)
assert angle2int(angle, False) == le(a)
def makeWritePacket(self, ID, reg, values=None):
"""
Creates a packet that writes a value(s) to servo ID at location reg. Make
sure the values are in little endian (use Packet.le() if necessary) for 16 b
(word size) values.
"""
if values:
params = le(reg) + values
else:
params = le(reg)
pkt = self.makePacket(ID, self.WRITE, params)
return pkt
def set_joint_mode(ID):
"""
This will over write CW_ANGLE_LIMIT and CCW_ANGLE_LIMIT
"""
pkt = servo.makeWritePacket(ID, servo.base.CW_ANGLE_LIMIT,
[0, 0] + le(1023))
return pkt
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 makeServoMoveCountsPacket(self, ID, angle):
"""
Commands the servo to an angle in counts (0-1023)
"""
val = le(int(angle))
pkt = self.makeWritePacket(ID, self.GOAL_POSITION, val)
return pkt
def test_xl320_crc16():
xl = XL320()
pkt = [
0xff, 0xff, 0xfd, 0x00, 0x01, 0x07, 0x00, 0x02, 0x63, 0x02, 0x04, 0x00,
0x1b, 0xf9
]
ans = xl.check_sum(pkt[:-2]) # you don't count the crc
b = le(ans)
assert b == [0x1b, 0xf9]
def makeSpeedPacket(self, speed):
"""
Set max speed for all servos
speed - [0-1023] in units of 0.111 rpm. If speed = 0, then max motor
speed is used. You cannot exceed max servo speed.
"""
speed = speed if (speed <= self.MAX_RPM) else self.MAX_RPM
pkt = self.makeWritePacket(self.BROADCAST_ADDR, self.GOAL_VELOCITY,
le(speed))
return pkt
def set_wheel_speed(ID, speed):
"""
Sets speed of wheel, all values are integers:
positive [0 to 1023]: CW
negative [-1023 to 0]: CCW
For the servo, the values are:
0, 1024: Stop
1-1023: CW
1025-2047: CCW
Bit 10 determines direction, 0:CW, 1:CCW
"""
speed = 1023 if speed > 1023 else speed
if speed < 0:
speed = -1023 if speed < -1023 else speed
speed = 1024 + abs(speed)
pkt = servo.makeWritePacket(ID, servo.base.GOAL_VELOCITY, le(speed))
return pkt
def find_packets(self, pkt):
"""
Search through a string of binary for a valid xl320 package.
in: buffer to search through
out: a list of valid data packet
"""
# print('findpkt', pkt)
# print('-----------------------')
ret = []
while len(pkt)-10 >= 0:
if pkt[0:4] != [0xFF, 0xFF, 0xFD, 0x00]:
pkt.pop(0) # get rid of the first index
# print(' - pop:', pkt)
continue
# print(' > good packet')
length = (pkt[6] << 8) + pkt[5]
# print(' > length', length)
crc_pos = 5 + length
pkt_crc = pkt[crc_pos:crc_pos + 2]
crc = le(self.check_sum(pkt[:crc_pos]))
# if len(pkt) < (crc_pos + 1):
# print('<<< need more data for findPkt >>>')
# print(' > calc crc', crc)
# print(' > pkt crc', pkt_crc)
if pkt_crc == crc:
pkt_end = crc_pos+2
ret.append(pkt[:pkt_end])
# print(' > found:', pkt[:pkt_end])
# print(' > pkt size', pkt_end)
del pkt[:pkt_end]
# print(' > remaining:', pkt)
else:
pkt_end = crc_pos+2
# print(' - crap:', pkt[:pkt_end])
del pkt[:pkt_end]
# print('findpkt ret:', ret)
return ret
def makeSyncMoveSpeedPacket(self, info, degrees=True):
"""
Write sync angle information to servos.
info = [[ID, angle, speed], [ID, angle, speed], ...]
ID: servo ID
angle: 0-300 degrees or in radians
speed: 0-1023
"""
data = []
for cmd in info:
dataN = [] # data for specific actuator
dataN.append(cmd[0]) # ID
angle = angle2int(cmd[1], degrees)
dataN.append(angle[0]) # LSB
dataN.append(angle[1]) # MSB
speed = le(cmd[2])
dataN.append(speed[0]) # LSB
dataN.append(speed[1]) # MSB
data.append(dataN)
pkt = self.makeSyncWritePacket(self.GOAL_POSITION, data)
return pkt
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 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
