def resetPosition(self, reset_gripper=True): # legacy:initMove
""" Move servo to neutral, initial position """
if reset_gripper:
Board.setBusServoPulse(1, self.neutralAngle, 300)
self.AK.setPitchRangeMoving((0, 10, 10), -30, -30, -90, 1500)
time.sleep(1.5)
def close(self, closeAngle=None):
""" Close the gripper paw, optionally set a custom close angle int (eg 500) """
if closeAngle == None:
closeAngle = self.closeAngle
Board.setBusServoPulse(1, closeAngle, 500)
time.sleep(0.5)
def UnloadBusServo(args):
ret = (True, ())
if args != 'servoPowerDown':
return (False, __RPC_E01)
try:
for i in range(1, 7):
Board.unloadBusServo(i)
except Exception as e:
print(e)
ret = (False, __RPC_E03)
def SetBusServoDeviation(*args):
ret = (True, ())
arglen = len(args)
if arglen != 2:
return (False, __RPC_E01)
try:
servo = args[0]
deviation = args[1]
Board.setBusServoDeviation(servo, deviation)
except Exception as e:
print(e)
ret = (False, __RPC_E03)
def update(t, x):
leftv = 0
rightv = 0
if (x < -0.3):
leftv = 50
elif (x > 0.3):
rightv = 54
else:
leftv = 50
rightv = 54
print(t, leftv, rightv)
Board.setMotor(1, leftv)
Board.setMotor(2, rightv)
return x
def GetBatteryVoltage():
ret = (True, 0)
try:
ret = (True, Board.getBattery())
except Exception as e:
print(e)
ret = (False, __RPC_E03)
return ret
def SetBusServoPulse(*args, **kwargs):
ret = (True, ())
arglen = len(args)
if (args[1] * 2 + 2) != arglen or arglen < 4:
return (False, __RPC_E01)
try:
servos = args[2:arglen:2]
pulses = args[3:arglen:2]
use_times = args[0]
for s in servos:
if s < 1 or s > 6:
return (False, __RPC_E02)
dat = zip(servos, pulses)
for (s, p) in dat:
Board.setBusServoPulse(s, p, use_times)
except Exception as e:
print(e)
ret = (False, __RPC_E03)
def SetBrushMotor(*args, **kwargs):
ret = (True, ())
arglen = len(args)
if 0 != (arglen % 2):
return (False, __RPC_E01)
try:
motors = args[0:arglen:2]
speeds = args[1:arglen:2]
for m in motors:
if m < 1 or m > 4:
return (False, __RPC_E02)
dat = zip(motors, speeds)
for m, s in dat:
Board.setMotor(m, s)
except:
ret = (False, __RPC_E03)
return ret
def SetPWMServo(*args, **kwargs):
ret = (True, ())
arglen = len(args)
if 0 != (arglen % 3):
return (False, __RPC_E01)
try:
servos = args[0:arglen:3]
pulses = args[1:arglen:3]
use_times = args[2:arglen:3]
for s in servos:
if s < 1 or s > 6:
return (False, __RPC_E02)
dat = zip(servos, pulses, use_times)
for (s, p, t) in dat:
Board.setPWMServoPulse(s, p, t)
except Exception as e:
print(e)
ret = (False, __RPC_E03)
return ret
def update(t, x):
print(x)
leftv = 0
rightv = 0
if (x < -0.3):
leftv = 50
elif (x > 0.3):
rightv = 54
else:
leftv = 50
rightv = 54
if int(time.time() * 10) % 2 == 1:
leftv = 0
rightv = 0
Board.setMotor(1, leftv)
Board.setMotor(2, rightv)
return x
def SaveBusServosDeviation(args):
ret = (True, ())
if args != "downloadDeviation":
return (False, __RPC_E01)
try:
for i in range(1, 7):
dev = Board.saveBusServoDeviation(i)
except Exception as e:
print(e)
ret = (False, __RPC_E03)
return ret
def __place(self, x, y, z, rotation=-90):
if not self.__check_reachable(x, y, z + 6):
print("could not reach the position!")
return
servo2_angle = getAngle(x, y, rotation)
Board.setBusServoPulse(2, servo2_angle, 500)
time.sleep(0.5)
self.AK.setPitchRangeMoving((x, y, z + 3), -90, -90, 0, 500)
time.sleep(0.5)
self.AK.setPitchRangeMoving((x, y, z), -90, -90, 0, 1000)
time.sleep(0.8)
Board.setBusServoPulse(1, self.servo1 - 200, 500)
time.sleep(0.8)
self.AK.setPitchRangeMoving((x, y, 12), -90, -90, 0, 800)
time.sleep(0.8)
def setBuzzer(self, timer):
""" Activate buzzer to sound for 'timer' seconds """
Board.setBuzzer(0)
Board.setBuzzer(1)
time.sleep(timer)
Board.setBuzzer(0)
def __initMove(self):
Board.setBusServoPulse(1, self.servo1 - 250, 300)
time.sleep(0.5)
Board.setBusServoPulse(1, self.servo1 - 50, 300)
time.sleep(0.5)
Board.setBusServoPulse(2, 500, 500)
self.AK.setPitchRangeMoving((0, 10, 10), -30, -30, -90, 1500)
time.sleep(1.5)
def set_rgb(color):
if color == "red":
Board.RGB.setPixelColor(0, Board.PixelColor(255, 0, 0))
Board.RGB.setPixelColor(1, Board.PixelColor(255, 0, 0))
Board.RGB.show()
elif color == "green":
Board.RGB.setPixelColor(0, Board.PixelColor(0, 255, 0))
Board.RGB.setPixelColor(1, Board.PixelColor(0, 255, 0))
Board.RGB.show()
elif color == "blue":
Board.RGB.setPixelColor(0, Board.PixelColor(0, 0, 255))
Board.RGB.setPixelColor(1, Board.PixelColor(0, 0, 255))
Board.RGB.show()
else:
Board.RGB.setPixelColor(0, Board.PixelColor(0, 0, 0))
Board.RGB.setPixelColor(1, Board.PixelColor(0, 0, 0))
Board.RGB.show()
def __pick(self, x, y, z, rotation):
servo2_angle = getAngle(x, y, rotation)
Board.setBusServoPulse(1, self.servo1 - 280, 500)
Board.setBusServoPulse(2, servo2_angle, 500)
time.sleep(0.5)
if not self.__check_reachable(x, y, z):
print("could not pick the target!")
return
Board.setBusServoPulse(1, self.servo1, 500)
time.sleep(0.8)
Board.setBusServoPulse(2, 500, 500)
self.AK.setPitchRangeMoving((x, y, 12), -90, -90, 0, 1000)
time.sleep(1)
def partyTime(self):
""" spin 'n wave yo arm like you just dont care - """
self.resetPosition(False)
Board.setBusServoPulse(6, 100, 1000)
time.sleep(1)
Board.setBusServoPulse(6, 800, 1000)
time.sleep(1)
Board.setBusServoPulse(6, 100, 1000)
time.sleep(1)
def GetBusServosDeviation(args):
ret = (True, ())
data = []
if args != "readDeviation":
return (False, __RPC_E01)
try:
for i in range(1, 7):
dev = Board.getBusServoDeviation(i)
if dev is None:
dev = 999
data.append(dev)
ret = (True, data)
except Exception as e:
print(e)
ret = (False, __RPC_E03)
return ret
def palm_controller(self, world_X=None, world_Y=None, rotation_angle=None):
# Control the palm
if world_X is None and world_Y is None and rotation_angle is None:
Board.setBusServoPulse(2, self.servo2, 500)
elif world_X is None and world_Y is None and rotation_angle is not None:
Board.setBusServoPulse(2, rotation_angle)
else:
# calculate the angle that gripper need to rotate
servo2_angle = AT.getAngle(world_X, world_Y, rotation_angle)
Board.setBusServoPulse(2, servo2_angle, 500)
time.sleep(1)
def GetBusServosPulse(args):
ret = (True, ())
data = []
if args != 'angularReadback':
return (False, __RPC_E01)
try:
for i in range(1, 7):
pulse = Board.getBusServoPulse(i)
if pulse is None:
ret = (False, __RPC_E04)
return ret
else:
data.append(pulse)
ret = (True, data)
except Exception as e:
print(e)
ret = (False, __RPC_E03)
return ret
def set_rgb(self, color):
""" Set the RGB light color of the expansion board to make it consistent with the color to be tracked """
if color == "red":
Board.RGB.setPixelColor(0, Board.PixelColor(255, 0, 0))
Board.RGB.setPixelColor(1, Board.PixelColor(255, 0, 0))
Board.RGB.show()
elif color == "green":
Board.RGB.setPixelColor(0, Board.PixelColor(0, 255, 0))
Board.RGB.setPixelColor(1, Board.PixelColor(0, 255, 0))
Board.RGB.show()
elif color == "blue":
Board.RGB.setPixelColor(0, Board.PixelColor(0, 0, 255))
Board.RGB.setPixelColor(1, Board.PixelColor(0, 0, 255))
Board.RGB.show()
else:
print('set_rgb color not found')
Board.RGB.setPixelColor(0, Board.PixelColor(0, 0, 0))
Board.RGB.setPixelColor(1, Board.PixelColor(0, 0, 0))
Board.RGB.show()
def setBuzzer(timer):#我觉得这个函数有点大病
Board.setBuzzer(0)
Board.setBuzzer(1)
time.sleep(timer)
Board.setBuzzer(0)
def initMove():
Board.setBusServoPulse(1, servo1 - 50, 300)#驱动转到指定位置，舵机id，位置何转动需要时间
Board.setBusServoPulse(2, 500, 500)#
AK.setPitchRangeMoving((0, 10, 10), -30, -30, -90, 1500)#转过去或者返回false，俯仰角范围可以调整
def move():
global rect
global _stop
global get_roi
global get_layer
global unreachable
global __isRunning
global detect_color
global detect_layer
global detect_direct
global start_pick_up
global rotation_angle
global world_X, world_Y
#放置坐标,等会测试一下
'''coordinate = {
'red': (-15 + 0.5, 12 - 0.5, 1.5),
'green': (-15 + 0.5, 6 - 0.5, 1.5),
'blue': (-15 + 0.5, 0 - 0.5, 1.5),
}'''
coordinate = {
'one': {'left':(-28 + 0.5,15 - 0.5,5), 'mid':(-28 + 0.5,15 - 0.5,5), 'right':(-28 + 0.5,15 - 0.5,5)},
'two': {'left':(-28 + 0.5,10 - 0.5,10), 'mid':(-28 + 0.5,10 - 0.5,10), 'right':(-28 + 0.5,10 - 0.5,10)},
'three': {'left':(-28 + 0.5,5 - 0.5,15), 'mid':(-28 + 0.5,5 - 0.5,15), 'right':(-28 + 0.5,5 - 0.5,15)},
}
while True:
if __isRunning:
if detect_color != 'None' and start_pick_up and detect_layer != "None": #如果检测到方块没有移动一段时间后，开始夹取
print('1')
#移到目标位置，高度6cm, 通过返回的结果判断是否能到达指定位置
#如果不给出运行时间参数，则自动计算，并通过结果返回
set_rgb(detect_color)#设置显示灯
setBuzzer(0.1)#有点病的程序
result = AK.setPitchRangeMoving((world_X, world_Y, 7), -90, -90, 0) #移到目标位置返回角度俯仰角等
if result == False:
unreachable = True
else:
unreachable = False
time.sleep(result[2]/1000) #如果可以到达指定位置，则获取运行时间
if not __isRunning:
continue
servo2_angle = getAngle(world_X, world_Y, rotation_angle) #计算夹持器需要旋转的角度
Board.setBusServoPulse(1, servo1 - 500, 500) #1号动，角度固定
Board.setBusServoPulse(2, servo2_angle, 500)#2号动，角度计算
time.sleep(0.5)#休息
if not __isRunning:
continue
AK.setPitchRangeMoving((world_X, world_Y, 1.5), -90, -90, 0, 1000)#转到A点(world_X, world_Y, 1.5)
time.sleep(1.5)
if not __isRunning:
continue
Board.setBusServoPulse(1, servo1-100, 500) #1号动，夹持器闭合，角度固定
time.sleep(0.8)
Board.setBusServoPulse(2, 500, 500)#2号动，角度固定腕部转动
time.sleep(0.8)
if not __isRunning:
continue
AK.setPitchRangeMoving((world_X, world_Y, 12), -90, -90, 0, 1000) #机械臂抬起，转到B点(world_X, world_Y, 12)
time.sleep(1)
if not __isRunning:
continue
result = AK.setPitchRangeMoving((coordinate[detect_layer][detect_direct][0], coordinate[detect_layer][detect_direct][1], 12), -90, -90, 0) #计算并转到目标1点(coordinate[detect_color][0], coordinate[detect_color][1], 12)
time.sleep(result[2]/1000)#如果可以到达指定位置，则获取运行时间
if not __isRunning:
continue
servo2_angle = getAngle(coordinate[detect_layer][detect_direct][0], coordinate[detect_layer][detect_direct][1], -90)#计算夹持器需要旋转的角度
time.sleep(0.5)
if not __isRunning:
continue
AK.setPitchRangeMoving((coordinate[detect_layer][detect_direct][0]+6, coordinate[detect_layer][detect_direct][1], coordinate[detect_layer][detect_direct][2] + 3), 0, -10, 10, 1000)#计算并转到目标2点上方(coordinate[detect_color][0], coordinate[detect_color][1], coordinate[detect_color][2] + 3)
time.sleep(0.5)
if not __isRunning:
continue
AK.setPitchRangeMoving((coordinate[detect_layer][detect_direct][0]+6, coordinate[detect_layer][detect_direct][1], coordinate[detect_layer][detect_direct][2]), 0, -10, 10, 1000)#转到目标2点前方(coordinate[detect_color])
time.sleep(0.8)
if not __isRunning:
continue
AK.setPitchRangeMoving((coordinate[detect_layer][detect_direct]), 0, -10, 10, 1000)#转到目标2点(coordinate[detect_color])
time.sleep(0.8)
if not __isRunning:
continue
Board.setBusServoPulse(1, servo1 - 500, 500) # 1号动，固定角度,打开
time.sleep(0.8)
if not __isRunning:
continue
AK.setPitchRangeMoving((coordinate[detect_layer][detect_direct][0], coordinate[detect_layer][detect_direct][1], 12), -90, -90, 0, 800)#到了3点有点奇怪，(coordinate[detect_color][0], coordinate[detect_color][1], 12)
time.sleep(0.8)
initMove() # 回到初始位置
time.sleep(1.5)
#全部初始化
detect_color = 'None'
detect_layer = 'None'
detect_direct = 'None'
get_roi = False
get_layer = False
start_pick_up = False
set_rgb(detect_color)
else:
if _stop:#停止了
_stop = False
Board.setBusServoPulse(1, servo1 - 70, 300)#1号动，固定角度
time.sleep(0.5)#休息
Board.setBusServoPulse(2, 500, 500)#2号动，固定角度
AK.setPitchRangeMoving((0, 10, 10), -30, -30, -90, 1500)#夹持角度
time.sleep(1.5)
time.sleep(0.01)
def setBuzzer(timer):
Board.setBuzzer(0)
Board.setBuzzer(1)
time.sleep(timer)
Board.setBuzzer(0)
def initMove():
Board.setBusServoPulse(1, servo1 - 50, 300)
Board.setBusServoPulse(2, 500, 500)
AK.setPitchRangeMoving((0, 10, 10), -30, -30, -90, 1500)
def move():
global rect
global _stop
global get_roi
global move_square
global __isRunning
global unreachable
global detect_color
global start_pick_up
global rotation_angle
global world_X, world_Y
global z_r, z_g, z_b, z
dz = 2.5
while True:
if __isRunning:
if detect_color != 'None' and start_pick_up: # 如果检测到方块没有移动一段时间后，开始夹取
set_rgb(detect_color)
setBuzzer(0.1)
# 高度累加
z = z_r
z_r += dz
if z == 2 * dz + coordinate['red'][2]:
z_r = coordinate['red'][2]
if z == coordinate['red'][2]:
move_square = True
time.sleep(3)
move_square = False
result = AK.setPitchRangeMoving((world_X, world_Y, 7), -90,
-90, 0) # 移到目标位置，高度5cm
if result == False:
unreachable = True
else:
unreachable = False
time.sleep(result[2] / 1000)
if not __isRunning:
continue
# 计算夹持器需要旋转的角度
servo2_angle = getAngle(world_X, world_Y, rotation_angle)
Board.setBusServoPulse(1, servo1 - 280, 500) # 爪子张开
Board.setBusServoPulse(2, servo2_angle, 500)
time.sleep(0.5)
if not __isRunning:
continue
AK.setPitchRangeMoving((world_X, world_Y, 2), -90, -90, 0,
1000) # 降低高度到2cm
time.sleep(1.5)
if not __isRunning:
continue
Board.setBusServoPulse(1, servo1, 500) # 夹持器闭合
time.sleep(0.8)
if not __isRunning:
continue
Board.setBusServoPulse(2, 500, 500)
AK.setPitchRangeMoving((world_X, world_Y, 12), -90, -90, 0,
1000) # 机械臂抬起
time.sleep(1)
if not __isRunning:
continue
AK.setPitchRangeMoving((coordinate[detect_color][0],
coordinate[detect_color][1], 12),
-90, -90, 0, 1500)
time.sleep(1.5)
if not __isRunning:
continue
servo2_angle = getAngle(coordinate[detect_color][0],
coordinate[detect_color][1], -90)
Board.setBusServoPulse(2, servo2_angle, 500)
time.sleep(0.5)
if not __isRunning:
continue
AK.setPitchRangeMoving(
(coordinate[detect_color][0],
coordinate[detect_color][1], z + 3), -90, -90, 0, 500)
time.sleep(0.5)
if not __isRunning:
continue
AK.setPitchRangeMoving((coordinate[detect_color][0],
coordinate[detect_color][1], z),
-90, -90, 0, 1000)
time.sleep(0.8)
if not __isRunning:
continue
Board.setBusServoPulse(1, servo1 - 200, 500) # 爪子张开 ，放下物体
time.sleep(1)
if not __isRunning:
continue
AK.setPitchRangeMoving((coordinate[detect_color][0],
coordinate[detect_color][1], 12),
-90, -90, 0, 800)
time.sleep(0.8)
initMove() # 回到初始位置
time.sleep(1.5)
detect_color = 'None'
get_roi = False
start_pick_up = False
set_rgb(detect_color)
else:
if _stop:
_stop = False
Board.setBusServoPulse(1, servo1 - 70, 300)
time.sleep(0.5)
Board.setBusServoPulse(2, 500, 500)
AK.setPitchRangeMoving((0, 10, 10), -30, -30, -90, 1500)
time.sleep(1.5)
time.sleep(0.01)
def servo_set_angle(servo, angle, speed=10):
pulse = 500 + int(sign_flip[servo] * 375 / 90 * angle +
375 / 90 * angle_offset[servo])
Board.setBusServoPulse(servo_name[servo], pulse, speed)
def move():
global rect
global track
global _stop
global get_roi
global unreachable
global __isRunning
global detect_color
global action_finish
global rotation_angle
global world_X, world_Y
global world_x, world_y
global center_list, count
global start_pick_up, first_move
# 不同颜色木快放置坐标(x, y, z)
coordinate = {
'red': (-15 + 0.5, 12 - 0.5, 1.5),
'green': (-15 + 0.5, 6 - 0.5, 1.5),
'blue': (-15 + 0.5, 0 - 0.5, 1.5),
}
while True:
if __isRunning:
if first_move and start_pick_up: # 当首次检测到物体时
action_finish = False
set_rgb(detect_color)
setBuzzer(0.1)
result = AK.setPitchRangeMoving((world_X, world_Y - 2, 5), -90,
-90, 0) # 不填运行时间参数，自适应运行时间
if result == False:
unreachable = True
else:
unreachable = False
time.sleep(result[2] / 1000) # 返回参数的第三项为时间
start_pick_up = False
first_move = False
action_finish = True
elif not first_move and not unreachable: # 不是第一次检测到物体
set_rgb(detect_color)
if track: # 如果是跟踪阶段
if not __isRunning: # 停止以及退出标志位检测
continue
AK.setPitchRangeMoving((world_x, world_y - 2, 5), -90, -90,
0, 20)
time.sleep(0.02)
track = False
if start_pick_up: #如果物体没有移动一段时间，开始夹取
action_finish = False
if not __isRunning: # 停止以及退出标志位检测
continue
Board.setBusServoPulse(1, servo1 - 280, 500) # 爪子张开
# 计算夹持器需要旋转的角度
servo2_angle = getAngle(world_X, world_Y, rotation_angle)
Board.setBusServoPulse(2, servo2_angle, 500)
time.sleep(0.8)
if not __isRunning:
continue
AK.setPitchRangeMoving((world_X, world_Y, 2), -90, -90, 0,
1000) # 降低高度
time.sleep(2)
if not __isRunning:
continue
Board.setBusServoPulse(1, servo1, 500) # 夹持器闭合
time.sleep(1)
if not __isRunning:
continue
Board.setBusServoPulse(2, 500, 500)
AK.setPitchRangeMoving((world_X, world_Y, 12), -90, -90, 0,
1000) # 机械臂抬起
time.sleep(1)
if not __isRunning:
continue
# 对不同颜色方块进行分类放置
result = AK.setPitchRangeMoving(
(coordinate[detect_color][0],
coordinate[detect_color][1], 12), -90, -90, 0)
time.sleep(result[2] / 1000)
if not __isRunning:
continue
servo2_angle = getAngle(coordinate[detect_color][0],
coordinate[detect_color][1], -90)
Board.setBusServoPulse(2, servo2_angle, 500)
time.sleep(0.5)
if not __isRunning:
continue
AK.setPitchRangeMoving((coordinate[detect_color][0],
coordinate[detect_color][1],
coordinate[detect_color][2] + 3),
-90, -90, 0, 500)
time.sleep(0.5)
if not __isRunning:
continue
AK.setPitchRangeMoving((coordinate[detect_color]), -90,
-90, 0, 1000)
time.sleep(0.8)
if not __isRunning:
continue
Board.setBusServoPulse(1, servo1 - 200, 500) # 爪子张开，放下物体
time.sleep(0.8)
if not __isRunning:
continue
AK.setPitchRangeMoving((coordinate[detect_color][0],
coordinate[detect_color][1], 12),
-90, -90, 0, 800)
time.sleep(0.8)
initMove() # 回到初始位置
time.sleep(1.5)
detect_color = 'None'
first_move = True
get_roi = False
action_finish = True
start_pick_up = False
set_rgb(detect_color)
else:
time.sleep(0.01)
else:
if _stop:
_stop = False
Board.setBusServoPulse(1, servo1 - 70, 300)
time.sleep(0.5)
Board.setBusServoPulse(2, 500, 500)
AK.setPitchRangeMoving((0, 10, 10), -30, -30, -90, 1500)
time.sleep(1.5)
time.sleep(0.01)
def move():
global rect
global _stop
global get_roi
global unreachable
global __isRunning
global detect_color
global start_pick_up
global rotation_angle
global world_X, world_Y
#放置坐标
coordinate = {
'red': (-15 + 0.5, 12 - 0.5, 1.5),
'green': (-15 + 0.5, 6 - 0.5, 1.5),
'blue': (-15 + 0.5, 0 - 0.5, 1.5),
}
while True:
if __isRunning:
if detect_color != 'None' and start_pick_up: #如果检测到方块没有移动一段时间后，开始夹取
#移到目标位置，高度6cm, 通过返回的结果判断是否能到达指定位置
#如果不给出运行时间参数，则自动计算，并通过结果返回
set_rgb(detect_color)
setBuzzer(0.1)
result = AK.setPitchRangeMoving((world_X, world_Y, 7), -90,
-90, 0)
if result == False:
unreachable = True
else:
unreachable = False
time.sleep(result[2] / 1000) #如果可以到达指定位置，则获取运行时间
if not __isRunning:
continue
servo2_angle = getAngle(world_X, world_Y,
rotation_angle) #计算夹持器需要旋转的角度
Board.setBusServoPulse(1, servo1 - 280, 500) # 爪子张开
Board.setBusServoPulse(2, servo2_angle, 500)
time.sleep(0.5)
if not __isRunning:
continue
AK.setPitchRangeMoving((world_X, world_Y, 1.5), -90, -90,
0, 1000)
time.sleep(1.5)
if not __isRunning:
continue
Board.setBusServoPulse(1, servo1, 500) #夹持器闭合
time.sleep(0.8)
if not __isRunning:
continue
Board.setBusServoPulse(2, 500, 500)
AK.setPitchRangeMoving((world_X, world_Y, 12), -90, -90, 0,
1000) #机械臂抬起
time.sleep(1)
if not __isRunning:
continue
result = AK.setPitchRangeMoving(
(coordinate[detect_color][0],
coordinate[detect_color][1], 12), -90, -90, 0)
time.sleep(result[2] / 1000)
if not __isRunning:
continue
servo2_angle = getAngle(coordinate[detect_color][0],
coordinate[detect_color][1], -90)
Board.setBusServoPulse(2, servo2_angle, 500)
time.sleep(0.5)
if not __isRunning:
continue
AK.setPitchRangeMoving((coordinate[detect_color][0],
coordinate[detect_color][1],
coordinate[detect_color][2] + 3),
-90, -90, 0, 500)
time.sleep(0.5)
if not __isRunning:
continue
AK.setPitchRangeMoving((coordinate[detect_color]), -90,
-90, 0, 1000)
time.sleep(0.8)
if not __isRunning:
continue
Board.setBusServoPulse(1, servo1 - 200, 500) # 爪子张开 ，放下物体
time.sleep(0.8)
if not __isRunning:
continue
AK.setPitchRangeMoving((coordinate[detect_color][0],
coordinate[detect_color][1], 12),
-90, -90, 0, 800)
time.sleep(0.8)
initMove() # 回到初始位置
time.sleep(1.5)
detect_color = 'None'
get_roi = False
start_pick_up = False
set_rgb(detect_color)
else:
if _stop:
_stop = False
Board.setBusServoPulse(1, servo1 - 70, 300)
time.sleep(0.5)
Board.setBusServoPulse(2, 500, 500)
AK.setPitchRangeMoving((0, 10, 10), -30, -30, -90, 1500)
time.sleep(1.5)
time.sleep(0.01)
