【详细讲解 附全部代码】【openmv控制三自由度机械臂抓取物品】硬件+软件

前言：
这份代码很难得的是纯自己写的，虽然openmv梯子都搭成这样了也没什么大技术含量，只有一丢丢细小的逻辑。。
整体代码放在最后了，有需要的自取吧

实现功能

机械臂抓取一定范围内任意位置的物品，将其放到指定位置
物品以形状和颜色区分

硬件清单

1.openmv 以及数据线
2.电源，可以使用充电宝替代
3.机械臂一份，在淘宝十几块钱就可以买到
4.小舵机3个，长的没问题就可以不需要太纠结型号
5.杜邦线 之类的零件若干
6.抓取的物品若干，由于便宜的机械臂摩擦力不够，可能需要稍微处理一下
7.随便什么可以把openmv固定起来的杆，我使用的是硬纸壳粘在一起，使用起来没问题，你也可以尝试雪糕棒一类的东西

实物图

原谅我没好好拍图片。。只能在视频里面截取一下了

整体程序设计思路

识别部分

由于需要同时识别形状和颜色，我的第一反应当然是星瞳上面找例程啦！官方确实有一个同时识别形状和颜色的例程.
但是!
经过我自己试验发现，关于形状的判断十分不准确，尤其我使用的海绵块本身边界不是那么的平整。。就导致 根！本！用！不！了！
当然这种小问题难不倒我啦，官方的程序思路是先识别形状，然后使用“class Statistics – 统计数据对象”判断。

官方代码如下：

img = sensor.snapshot().lens_corr(1.8)
    for c in img.find_circles(threshold = 3500, x_margin = 10, y_margin = 10, r_margin = 10,
            r_min = 2, r_max = 100, r_step = 2):
        area = (c.x()-c.r(), c.y()-c.r(), 2*c.r(), 2*c.r())
        #area为识别到的圆的区域，即圆的外接矩形框
        statistics = img.get_statistics(roi=area)#像素颜色统计
        print(statistics)
        #(0,100,0,120,0,120)是红色的阈值，所以当区域内的众数（也就是最多的颜色），范围在这个阈值内，就说明是红色的圆。
        #l_mode()，a_mode()，b_mode()是L通道，A通道，B通道的众数。
        if 0<statistics.l_mode()<100 and 0<statistics.a_mode()<127 and 0<statistics.b_mode()<127:#if the circle is red
            img.draw_circle(c.x(), c.y(), c.r(), color = (255, 0, 0))#识别到的红色圆形用红色的圆框出来
        else:
            img.draw_rectangle(area, color = (255, 255, 255))
            #将非红色的圆用白色的矩形框出来

但是既然形状识别没有颜色识别靠谱。那咱们就改成先识别颜色再判断形状不就好啦？
另外在这里我加入了一个变量记录区域被识别为圆还是方的次数，如果你需要考虑更多形状可以改成数组的形式储存次数。

我的代码：

 blobs = img.find_blobs([yellow_threshold,green_threshold], roi=(0,0,160,80),pixels_threshold=100, area_threshold=100)# merge=True, margin=10
    if blobs:#如果找到了
        data=[]#初始化数据
        blob=find_max(blobs)#找最大
        color=blob.code()#记录颜色
        img.draw_rectangle(blob.x(),blob.y(),blob.w(),blob.h(), color = (255, 255, 255), thickness = 2, fill = False)
        #在色块周边一定范围内找圆
        circles= img.find_circles(threshold = 2600,r_min=10, x_margin = 10, y_margin = 10, r_margin = 10,
                    r_min = 2, r_max = 100, r_step = 2,roi=(blob.x(),blob.y(),blob.w(),blob.h()))
        #在色块周边一定范围内找方
        rects = img.find_rects(threshold = 10000,roi=(blob.x(),blob.y(),blob.w(),blob.h()))
        if circles:
            shape=shape+1
        if rects:
            shape=shape-1
        #上面两句会在一次抓取任务中对shape进行运算，shape表示该物品形状是圆还是方的概率，因此值理论上是-无穷到+无穷，实际不会太大，
        #正数表示更可能为圆，负数表示更可能为方，绝对值越大可能性越高，可以把中间值改成其他值以更好地提高准确率
        #由于openmv不能每一次都准确识别，因此采用这种方式显著提高了准确率。

硬件连接设计

我这里只用了3个舵机
openmv2有2个舵机控制引脚
openmv3有3个舵机控制引脚
可以通过连接pca9685连接更多舵机，不需要担心引脚不够用
顺便一提！openmv接屏幕 串口 舵机 等等外设多的时候特别容易引脚冲突！强烈建议买个pca9685备用，tb买单独的模块就行，比官方扩展板便宜很多，用法都一样两三句代码的事。

我的代码接引脚是这样的（没有使用扩展板）：

s1 = Servo(1) # P7
s2 = Servo(2) # P8
s3 = Servo(3) # P9 Only for OpenMV3 M7
#实例化3个舵机

运动控制

万能pid，我觉得不用多说了，我只用了一个p
注意的是我把运动拆解成了拿取之前的运动和拿取之后的运动，因为拿取之后的运动基本固定的，只要判断几个参数就行。

以下是拿取之前的运动：

def move(s3_error,s2_error):#定义一个函数，作用是运动到目标物体位置
    kp=0.2
    global s2_now
    global s3_now #global关键字 用于全局变量
    s3_move = s3_error*kp+s3_now#pid计算
    if s3_move > 90:#这4句限制舵机运动角度，防止卡死
        s3_move = 90
    if s3_move < -90:
        s3_move = -90
    s3.angle(s3_move)#这一句是输出pwm控制舵机，采用openmv控制舵机时需要，采用arduino时不需要
    s3_now = s3_move#更新当前舵机角度存档
    if abs(s3_error)<10:
        s2_move = s2_error*kp+s2_now#pid计算
        if s2_move > -20:#这4句限制舵机运动角度，防止卡死
            s2_move = -20
        if s2_move < -90:
            s2_move = -90
            s2.angle(s2_move)#这一句是输出pwm控制舵机，采用openmv控制舵机时需要，采用arduino时不需要
            s2_now = s2_move

拿取之后的运动我基本就是for循环加延时，

判断应该放到哪里的代码如下：

 angle = 0#这个值表示目的角度
    if color == 1:#根据颜色判断
        angle = 90
    else :
        angle = -90
    if shape > 0:#根据形状判断
        if angle >0:
            angle = angle - 30
        else:
            angle = angle + 30

需要注意的是：
for循环设计上要考虑到目标位置与循环条件的关系！
解决办法是写2个循环就可以了，每次只会执行符合的哪一个

不多说，直接看代码就理解了：

for i in range(s2_now,-20,-1):
        s2.angle(i)
        time.sleep(20)
    for i in range(s2_now,-20):
        s2.angle(i)
        time.sleep(20)

关于机械臂硬件

我在淘宝买的4自由度机械臂，强行改成3自由度
如果你是4自由度，单独控制一下第四个舵机就行，差别不大，方法都一样的。

如何get同款硬件？

1.购买一个特别便宜的长得差不多就对了的机械臂 什么颜色都可以
2.其他部分正常组装，只有控制高度的舵机不装
3.使用一根剩下来的杆子固定高度
4.差不多就完工了！具体堆栈我的图片就可以，这东西只要运动没问题就行。

关于整体代码

# 设计：沈阳航空航天大学 berry


# 博客：https://blog.csdn.net/qq_45037925/article/details/105901893


# 交流方式：私信我的博客即可






import time


from pyb import Servo
import sensor, image, time


from pyb import UART


#导入需要的模块




s1 = Servo(1) # P7


s2 = Servo(2) # P8


s3 = Servo(3) # P9 Only for OpenMV3 M7
#实例化3个舵机


uart = UART(3, 115200)#初始化串口


s2_now = -90
s3_now = 0




red_threshold  = (21, 100, 36, 127, -9, 34)#颜色阈值1   0001(14, 100, 8, 127, 9, 116)
blue_threshold  = ((20, 67, -52, -13, -36, -1))#颜色阈值2   0010
yellow_threshold  = (14, 100, 22, 127, 42, 113)#颜色阈值4    0100
green_threshold  = (20, 100, -128, -24, 17, 127)#颜色阈值8   1000










sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.RGB565) # use RGB565.
sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed.
sensor.skip_frames(10) # Let new settings take affect.
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # turn this off.
clock = time.clock() # Tracks FPS.




#以上部分是每个程序差不多都有的初始化设定






def move(s3_error,s2_error):#定义一个函数，作用是运动到目标物体位置
    kp=0.2
    global s2_now
    global s3_now #global关键字 用于全局变量


    s3_move = s3_error*kp+s3_now#pid计算


    if s3_move > 90:#这4句限制舵机运动角度，防止卡死
        s3_move = 90
    if s3_move < -90:
        s3_move = -90
    s3.angle(s3_move)#这一句是输出pwm控制舵机，采用openmv控制舵机时需要，采用arduino时不需要
    s3_now = s3_move#更新当前舵机角度存档


    if abs(s3_error)<10:
        s2_move = s2_error*kp+s2_now#pid计算
        if s2_move > -20:#这4句限制舵机运动角度，防止卡死
            s2_move = -20
        if s2_move < -90:
            s2_move = -90
            s2.angle(s2_move)#这一句是输出pwm控制舵机，采用openmv控制舵机时需要，采用arduino时不需要
            s2_now = s2_move
    #data.append((s2_now,s3_now))#这三句时串口输出舵机应转的角度，采用openmv控制舵机时不需要，采用arduino时需要
    #data_out = json.dumps(set(data))
    #uart.write(data_out +'\n')




def lay(shape,color):#定义一个函数，用于把抓取的物品放到指定位置
    global s2_now#global关键字 用于全局变量
    global s3_now
    angle = 0#这个值表示目的角度
    if color == 1:#根据颜色判断
        angle = 90
    else :
        angle = -90
    if shape > 0:#根据形状判断
        if angle >0:
            angle = angle - 30
        else:
            angle = angle + 30






#下面是机械臂把物品放到指定angle对应位置，并回到初始位置
    for i in range(s2_now,-20,-1):




        s2.angle(i)


        time.sleep(20)


    for i in range(s2_now,-20):




        s2.angle(i)


        time.sleep(20)


    for i in range(5,-19,-1):


        s1.angle(i)
        time.sleep(10)


    for i in range(-20,-90,-1):




        s2.angle(i)


        time.sleep(20)


    for i in range(s3_now,angle,-1):




        s3.angle(i)


        time.sleep(20)


    for i in range(s3_now,angle):




        s3.angle(i)


        time.sleep(20)






    for i in range(-90,-40):




        s2.angle(i)


        time.sleep(20)




    for i in range(-10,5):


        s1.angle(i)
        time.sleep(10)


    time.sleep(200)
    #data.append((s2_now,s3_now))
    #data_out = json.dumps(set(data))
    #uart.write(data_out +'\n')




    for i in range(-20,-90,-1):




        s2.angle(i)


        time.sleep(20)




    for i in range(angle,0,-1):




        s3.angle(i)


        time.sleep(20)


    for i in range(angle,0):




        s3.angle(i)


        time.sleep(20)
    time.sleep(2000)


    s2.angle(-90)
    s2_now = -90
    s3_now = angle
    #data.append((s2_now,s3_now))
    #data_out = json.dumps(set(data))
    #uart.write(data_out +'\n')



def find_max(blobs):#定义一个找最大色块的函数，输入一个色块对象列表，输出一个色块对象
    max_size=0
    for blob in blobs:
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob


def find_max_c(blobs):#定义一个找最大圆的函数，输入一个圆对象列表，输出一个圆对象
    max_size=0
    for blob in blobs:
        if blob[2] > max_size:
            max_blob=blob
            max_size = blob[2]
    return max_blob



s3.angle(s3_now)#初始化舵机
s2.angle(s2_now)
shape=0#记录形状和颜色
color=0
time.sleep(2000)
#下面是循环执行的部分
while(True):
    clock.tick() # Track elapsed milliseconds between snapshots().
    img = sensor.snapshot().lens_corr(1.8)#拍摄一张照片，并畸变矫正
    claps = img.find_blobs([red_threshold])#寻找色块




    if claps:
        clap=find_max(claps)
        img.draw_cross(clap.x(), clap.y(), color = (0, 255, 0), size = 10, thickness = 2)#画一个十字


    #寻找色块
    blobs = img.find_blobs([yellow_threshold,green_threshold], roi=(0,0,160,80),pixels_threshold=100, area_threshold=100)# merge=True, margin=10
    if blobs:#如果找到了
        data=[]#初始化数据
        blob=find_max(blobs)#找最大
        color=blob.code()#记录颜色
        img.draw_rectangle(blob.x(),blob.y(),blob.w(),blob.h(), color = (255, 255, 255), thickness = 2, fill = False)
        #在色块周边一定范围内找圆
        circles= img.find_circles(threshold = 2600,r_min=10, x_margin = 10, y_margin = 10, r_margin = 10,
                    r_min = 2, r_max = 100, r_step = 2,roi=(blob.x(),blob.y(),blob.w(),blob.h()))
        #在色块周边一定范围内找方
        rects = img.find_rects(threshold = 10000,roi=(blob.x(),blob.y(),blob.w(),blob.h()))
        if circles:
            shape=shape+1
        if rects:
            shape=shape-1
        #上面两句会在一次抓取任务中对shape进行运算，shape表示该物品形状是圆还是方的概率，因此值理论上是-无穷到+无穷，实际不会太大，
        #正数表示更可能为圆，负数表示更可能为方，绝对值越大可能性越高，可以把中间值改成其他值以更好地提高准确率
        #由于openmv不能每一次都准确识别，因此采用这种方式显著提高了准确率。
        tx=blob.cx()
        ty=blob.cy()
        #s3_error = clap.cx()-tx#计算error
        #s2_error = clap.cy()-ty
        s3_error = 78-tx#计算error
        s2_error = 44-ty
        move(s3_error,s2_error)#使用前面定义的函数
        if abs(s3_error)<10:
            lay(shape,color)#使用前面定义的函数
            shape=0#清除标志
            color=0



        #s1.angle(5)打开#s1.angle(-10)闭合
        #s2.angle(-90)#前s2.angle(-20)后
        #s3.angle(-90)#左 s3.angle(0)#中s3.angle(90)#右

后话

这个程序其实有一个不完美的地方，就是。。它不会主动判断夹取是否成功，没写的原因是我懒。。
简单写一下我的思路吧，

思路：
1.在原来程序基础上，在夹取完成之后每xx时间（可以使用定时器也可以穿插在之后的程序中）检测一下屏幕中间一定区域的颜色，如果不符合预期就重新寻找一次色块，如果发现色块就重新回去取色块
2.使用帧差分啥的不知道行不行