''' 设置机械臂关节的角度, 单位° ''' from mirobot import Mirobot import time arm = Mirobot(portname='COM7', debug=False) arm.home_simultaneous() # 设置单个关节的角度 print("测试设置单个关节的角度") arm.set_joint_angle({1: 100.0}, wait=True) print("动作执行完毕") # 状态查询 print(f"状态查询: {arm.get_status()}") # 停顿2s time.sleep(2) # 设置多个关节的角度 print("设置多个关节的角度") target_angles = {1: 90.0, 2: 30.0, 3: -20.0, 4: 10.0, 5: 0.0, 6: 90.0} arm.set_joint_angle(target_angles, wait=True) print("动作执行完毕") # 状态查询 print(f"状态查询: {arm.get_status()}") # 停顿2s time.sleep(2)
from mirobot import Mirobot from time import sleep api = Mirobot(portname='COM5', debug=False) api.home_simultaneous() sleep(1) # above block target_angles = {1: 49.0, 2: 9.5, 3: -3.0, 4: 0.0, 5: 0.0, 6: 0.0} api.set_joint_angle(target_angles, wait=True) # attach block target_angles = {1: 49.0, 2: 54.5, 3: -3.0, 4: 0.0, 5: -51.0, 6: 0.0} api.set_joint_angle(target_angles, wait=True) # suction ON api.pump_on() # above block target_angles = {1: 49.0, 2: 9.5, 3: -3.0, 4: 0.0, 5: 0.0, 6: 0.0} api.set_joint_angle(target_angles, wait=True) # back to zero pos api.go_to_zero() # over chuck target_angles = {1: 0.0, 2: 35.0, 3: 0.0, 4: 0.0, 5: -40.0, 6: 0.0} api.set_joint_angle(target_angles, wait=True) # suction Blow #api.pump_blow() api.pump_off() # back to zero pos ( fin ) api.go_to_zero() #api.pump_off()
class RobotController: def __init__(self, player: str): self.player = player if self.player == "b": self.robot = Mirobot(portname='/dev/ttyUSB0', debug=False) self.chessPickupZ = 37 else: # self.player == "r" self.robot = Mirobot(portname='/dev/ttyUSB1', debug=False) self.chessPickupZ = 31 self.mapTable = [[[0, 0, self.chessPickupZ] for i in range(1001)] for j in range(1101)] self.CalculateMapTable() self.RobotInit() # regression function # Y regression def RegressionY(self, A, B, index): [row1, col1] = A [row2, col2] = B [x1, y1, z1] = self.mapTable[row1][col1] [x2, y2, z2] = self.mapTable[row2][col2] distance = index - col1 step = col2 - col1 x = x1 + (x2 - x1) / step * distance y = y1 + (y2 - y1) / step * distance z = z1 + (z2 - z1) / step * distance P = [round(x, 2), round(y, 2), round(z, 2)] return P # X regression def RegressionX(self, A, B, index): [row1, col1] = A [row2, col2] = B [x1, y1, z1] = self.mapTable[row1][col1] [x2, y2, z2] = self.mapTable[row2][col2] distance = index - row1 step = row2 - row1 x = x1 + (x2 - x1) / step * distance y = y1 + (y2 - y1) / step * distance z = z1 + (z2 - z1) / step * distance P = [round(x, 2), round(y, 2), round(z, 2)] return P # calculating mapping table def CalculateMapTable(self): if self.player == "r": # red # line 1, x = 0, y = [0,2,4,6,8,10] self.mapTable[0][0] = [86.5, 99, 36.7] self.mapTable[0][200] = [124.6, 99, 36.7] self.mapTable[0][400] = [156.6, 101, 36.7] self.mapTable[0][600] = [193.1, 102.5, 36.2] self.mapTable[0][800] = [229.6, 102.5, 36.2] self.mapTable[0][1000] = [264.6, 102.5, 35.7] # line 2, x = 1, y = [0,2,4,6,8,10] self.mapTable[100][0] = [86.5, 84, 36.7] self.mapTable[100][200] = [124.6, 84, 36.7] self.mapTable[100][400] = [156.6, 84, 36.7] self.mapTable[100][600] = [193.1, 85.5, 36.2] self.mapTable[100][800] = [229.6, 85.5, 36.2] self.mapTable[100][1000] = [264.6, 85.5, 35.7] # line 3, x = 3, y = [0,2,4,6,8,10] self.mapTable[300][0] = [87.5, 47, 36.7] self.mapTable[300][200] = [122.6, 47, 36.7] self.mapTable[300][400] = [157.1, 48, 36.7] self.mapTable[300][600] = [194.6, 49.5, 36.2] self.mapTable[300][800] = [230.1, 49, 36.2] self.mapTable[300][1000] = [265.1, 49, 36.2] # line 4, x = 5, y = [0,2,4,6,8,10] self.mapTable[500][0] = [91.6, 9.5, 37.2] self.mapTable[500][200] = [126.6, 9.5, 37.2] self.mapTable[500][400] = [159.6, 13, 36.7] self.mapTable[500][600] = [193.6, 13.5, 36.2] self.mapTable[500][800] = [232.6, 15, 35.7] self.mapTable[500][1000] = [267.6, 15, 35.7] # line 5, x = 6, y = [0,2,4,6,8,10] self.mapTable[600][0] = [91.6, -8, 36.7] self.mapTable[600][200] = [126.6, -8, 36.7] self.mapTable[600][400] = [158.6, -6, 36.7] self.mapTable[600][600] = [194.6, -4, 36.7] self.mapTable[600][800] = [232.6, -3.5, 35.7] self.mapTable[600][1000] = [267.6, -3.5, 35.7] # line 6, x = 8, y = [0,2,4,6,8,10] self.mapTable[800][0] = [93.6, -43, 36.7] self.mapTable[800][200] = [128.6, -43, 36.7] self.mapTable[800][400] = [159.6, -42, 36.7] self.mapTable[800][600] = [196.6, -40, 35.7] self.mapTable[800][800] = [231.6, -39, 35.7] self.mapTable[800][1000] = [266.6, -39, 35.7] # line 7, x = 10, y = [0,2,4,6,8,10] self.mapTable[1000][0] = [90.6, -79, 36.7] self.mapTable[1000][200] = [125.6, -79, 36.7] self.mapTable[1000][400] = [162.6, -76.5, 36.2] self.mapTable[1000][600] = [197.6, -75, 35.7] self.mapTable[1000][800] = [234.1, -75.5, 35.2] self.mapTable[1000][1000] = [269.1, -75.5, 35.2] # line 8, x = 11, y = [0,2,4,6,8,10] self.mapTable[1100][0] = [90.1, -95.5, 36.2] self.mapTable[1100][200] = [125.1, -95.5, 36.2] self.mapTable[1100][400] = [162.6, -92.5, 36.2] self.mapTable[1100][600] = [197.6, -91.5, 35.7] self.mapTable[1100][800] = [234.1, -92, 35.2] self.mapTable[1100][1000] = [269.1, -92, 35.2] else: # black # line 1, x = 0, y = [0,2,4,6,8,10] self.mapTable[0][0] = [269.6, -98, 37.7] self.mapTable[0][200] = [234.6, -98, 37.7] self.mapTable[0][400] = [199.7, -98.5, 37.7] self.mapTable[0][600] = [163.2, -99, 37.7] self.mapTable[0][800] = [128.2, -100, 37.7] self.mapTable[0][1000] = [92.7, -101, 37.7] # line 2, x = 1, y = [0,2,4,6,8,10] self.mapTable[100][0] = [269.6, -81, 37.7] self.mapTable[100][200] = [234.6, -82, 37.7] self.mapTable[100][400] = [198.7, -82, 37.7] self.mapTable[100][600] = [162.2, -83, 37.7] self.mapTable[100][800] = [127.2, -84, 37.7] self.mapTable[100][1000] = [92.2, -85, 37.7] # line 3, x = 3, y = [0,2,4,6,8,10] self.mapTable[300][0] = [265.2, -45, 37.7] self.mapTable[300][200] = [230.7, -44.5, 37.7] self.mapTable[300][400] = [196.2, -44.5, 37.7] self.mapTable[300][600] = [159.2, -44.5, 38.7] self.mapTable[300][800] = [127.7, -47, 37.7] self.mapTable[300][1000] = [96.2, -47, 37.7] # line 4, x = 5, y = [0,2,4,6,8,10] self.mapTable[500][0] = [264.2, -9.5, 37.7] self.mapTable[500][200] = [231.2, -9.5, 37.7] self.mapTable[500][400] = [198.7, -9.5, 37.7] self.mapTable[500][600] = [162.7, -10, 37.7] self.mapTable[500][800] = [127.7, -10, 38.2] self.mapTable[500][1000] = [92.2, -10, 38.2] # line 5, x = 6, y = [0,2,4,6,8,10] self.mapTable[600][0] = [265.7, 10, 37.7] self.mapTable[600][200] = [230.7, 10, 37.7] self.mapTable[600][400] = [195.7, 10, 37.7] self.mapTable[600][600] = [159.7, 9, 37.7] self.mapTable[600][800] = [127.7, 9, 38.2] self.mapTable[600][1000] = [95.7, 9, 38.2] # line 6, x = 8, y = [0,2,4,6,8,10] self.mapTable[800][0] = [263.1, 45.5, 37.7] self.mapTable[800][200] = [230.6, 46, 37.7] self.mapTable[800][400] = [198.6, 46, 37.7] self.mapTable[800][600] = [163.6, 46, 37.7] self.mapTable[800][800] = [127.6, 47, 37.7] self.mapTable[800][1000] = [91.6, 47.5, 37.7] # line 7, x = 10, y = [0,2,4,6,8,10] self.mapTable[1000][0] = [267.1, 80.5, 37.7] self.mapTable[1000][200] = [231.1, 80.5, 37.7] self.mapTable[1000][400] = [195.1, 81, 37.7] self.mapTable[1000][600] = [159.6, 81, 37.7] self.mapTable[1000][800] = [124.6, 82.5, 37.7] self.mapTable[1000][1000] = [89.6, 83, 37.7] # line 8, x = 11, y = [0,2,4,6,8,10] self.mapTable[1100][0] = [266.1, 115.5, 37.7] self.mapTable[1100][200] = [230.1, 115.5, 37.7] self.mapTable[1100][400] = [195.6, 116, 37.7] self.mapTable[1100][600] = [160.1, 116, 37.7] self.mapTable[1100][800] = [123.1, 117, 37.7] self.mapTable[1100][1000] = [90.1, 118, 37.7] # row: 0 1 3 5 6 8 10 for row in [0, 1, 3, 5, 6, 8, 10, 11]: for col in range(1000): row_A = row * 100 col_A = 200 * (col // 200) row_B = row * 100 col_B = 200 * (col // 200 + 1) self.mapTable[100 * row][col] = self.RegressionY( A=[row_A, col_A], B=[row_B, col_B], index=col) # 0-1 for row in range(0, 100): for col in range(1001): self.mapTable[row][col] = self.RegressionX(A=[0, col], B=[100, col], index=row) # 1-3 for row in range(100, 300): for col in range(1001): self.mapTable[row][col] = self.RegressionX(A=[100, col], B=[300, col], index=row) # 3-5 for row in range(300, 500): for col in range(1001): self.mapTable[row][col] = self.RegressionX(A=[300, col], B=[500, col], index=row) # 5-6 for row in range(500, 600): for col in range(1001): self.mapTable[row][col] = self.RegressionX(A=[500, col], B=[600, col], index=row) # 6-8 for row in range(600, 800): for col in range(1001): self.mapTable[row][col] = self.RegressionX(A=[600, col], B=[800, col], index=row) # 8-10 for row in range(800, 1000): for col in range(1001): self.mapTable[row][col] = self.RegressionX(A=[800, col], B=[1000, col], index=row) # 10-11 for row in range(1000, 1100): for col in range(1001): self.mapTable[row][col] = self.RegressionX(A=[1000, col], B=[1100, col], index=row) # convert coordinate def Convert2RobotCoordinate(self, ChessPos): # be careful of the margin +100 robotCoordinate = (self.mapTable[ChessPos[0] + 100][ChessPos[1] + 100][0], self.mapTable[ChessPos[0] + 100][ChessPos[1] + 100][1]) return robotCoordinate # Chess at ChessAPos eat chess at ChessAPos def Eat(self, ChessAPos, ChessBPos): self.Drop(ChessBPos) self.Move(ChessAPos, ChessBPos) print("Chess eat operation complete") # Move chess at ChessAPos to ChessBPos def Move(self, ChessAPos, ChessBPos): # Pick up chessRealPosA = self.Convert2RobotCoordinate(ChessAPos) self.robot.set_wrist_pose(chessRealPosA[0], chessRealPosA[1], self.chessPickupZ + 10) self.robot.set_wrist_pose(chessRealPosA[0], chessRealPosA[1], self.chessPickupZ) self.robot.pump_on() self.robot.set_wrist_pose(chessRealPosA[0], chessRealPosA[1], self.chessPickupZ + 10) # Put down chessRealPosB = self.Convert2RobotCoordinate(ChessBPos) self.robot.set_wrist_pose(chessRealPosB[0], chessRealPosB[1], self.chessPickupZ + 10) self.robot.set_wrist_pose(chessRealPosB[0], chessRealPosB[1], self.chessPickupZ + 1) self.robot.pump_off() # Lift the robotic arm self.robot.set_wrist_pose(chessRealPosB[0], chessRealPosB[1], self.chessPickupZ + 15) # Go to standby position self.robot.go_to_zero() self.robot.set_joint_angle({1: 90.0}, wait=True) print("Chess move operation complete") # drop a chess def Drop(self, ChessAPos): # Pick up chessRealPosA = self.Convert2RobotCoordinate(ChessAPos) self.robot.set_wrist_pose(chessRealPosA[0], chessRealPosA[1], self.chessPickupZ + 20) self.robot.set_wrist_pose(chessRealPosA[0], chessRealPosA[1], self.chessPickupZ) self.robot.pump_on() # Lift the robotic arm self.robot.set_wrist_pose(chessRealPosA[0], chessRealPosA[1], self.chessPickupZ + 10) # Put down chessDropPos = (100, -180, 100) self.robot.set_wrist_pose(chessDropPos[0], chessDropPos[1], chessDropPos[2]) self.robot.pump_off() print("Chess drop operation complete") # initialize def RobotInit(self): self.robot.home_simultaneous() self.robot.set_joint_angle({1: 90.0}, wait=True) print("Robot-Black initialization complete")