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