【仅供参考】手把手教你制作捡球小车 2018年9月12日93万+纪念

【程序仅供参考，夭折文，哪天抽空写了】

首先了解模块，它的引脚作用，即接线方式

L298N直流减速电机驱动板，控制小车轮子

输出A 输出B 分别接两个直流电机。

12V供电 这个是外部电源为驱动板供电的接口，一般驱动电压实际可以接受的输入范围是7~12V，此时可以使能板载的5V逻辑供电。当使用大于12V的驱动电压时，为了避免稳压芯片损坏，首先要拔掉板载5V输出使能的跳线帽，然后在5V输出端口外部接入5V电压对L298N内部逻辑供电。

5V供电 （引出5V电压接到Arduino板，给Arduino板供电（驱动电压7~12V）连接到UNO板的5V）同时Arduino板也要引出一条GND线，连接到L298N的GND与外部电源共地。

ENA ENB通道A使能，通道B使能，当你不用PWM时不需要拔掉跳线帽，当你需要PWM时，将需要拔掉跳线帽，接到arduino上的模拟输入输出接口，我的只需要外侧接上,下面回介绍。

IN1 IN2 IN3 IN4逻辑输出口，其中IN1 IN2 控制一个电机的转动，IN3 IN4 控制另一个电机的转动，只要一个置高 一个置低，就可以让电机转动起来。

 

PCA9685模块连接多个舵机——控制机械臂

 

import sensor, image, time, math
#from math import pi
from servo import Servos #舵机控制
from machine import I2C, Pin  #这个是16路PCA9685模块的
import car


#16路PCA9685模块的I2C通信，不需要懂
i2c = I2C(sda=Pin('P5'), scl=Pin('P4'))  #定义P4和P5引脚
servo = Servos(i2c, address=0x40, freq=50, min_us=650, max_us=2800, degrees=180)


#设置颜色阈值，如果是rgb图像，六个数字分别为(minL, maxL, minA, maxA, minB, maxB)；
green_threshold = (0,100,   -128,0,   0,127) # L A B
blue_threshold = (0,100,   -128,127,   -128,0) # L A B
red_threshold   = (42, 72, 53, 99, -14, 67)
judge_threshold = 1200   #是颜色像素的参考大小,如果计算的大于它，说明小球近了


#这下面是常规操作
sensor.reset() # 初始化摄像头
sensor.set_pixformat(sensor.RGB565) # 格式为 RGB565.
sensor.set_framesize(sensor.QQVGA) # 使用 QQVGA 速度快一些 QQVGA的整个画面像素是x*y=160*120，x中间就是x=80
sensor.skip_frames(time = 2000) # 跳过2000s，使新设置生效,并自动调节白平衡
#sensor.skip_frames(10) # Let new settings take affect.
sensor.set_auto_gain(False) # 关闭自动自动增益。默认开启的，在颜色识别中，一定要关闭白平衡。
sensor.set_auto_whitebal(False)#关闭白平衡。白平衡是默认开启的，在颜色识别中，一定要关闭白平衡。
clock = time.clock() # 追踪帧率


K=5000    #测距常数K, 距离=K/球的直径，得到的是mm单位
l_camera=0
x_camera=0           #小球对于摄像头x坐标
y_camera=0           #小球对于摄像头y坐标
x_chassis=0          #小球对于底盘x坐标
y_chassis=0          #小球对于底盘y坐标
i=0
arm_ball=0             #臂舵机到球距离
chassis_high=70        #底座高度mm 90-20=70 20是球的高度
arm_long=130           #手臂长度
elbow_hand_long=165    #手爪到肘长
chassis_ball=0         #底座到球距离
arm_ball=0             #臂到球距离
chassis_ball=0         #底座中心到球的距离
arm_angle_tump=0       #分析过程数值
chassis_angle_tump=0   #分析过程数值


#极限角度，我们的舵机能转180度，注意舵机如果在远离安装孔另一侧的方向看，顺时针旋转是角度是增大的
max_chassis_angle=180     #底盘舵机复位角度
min_chassis_angle=0       #底盘舵机有效角度
max_arm_angle=60          #臂舵机复位角度
min_arm_angle=0           #臂舵机有效角度
max_elbow_angle=180       #肘舵机有效角度
min_elbow_angle=70        #肘舵机复位角度
max_hand_angle=100        #手爪舵复位机角度
min_hand_angle=75         #手爪舵机有效角度

#机械臂从下到上舵机命名为底盘-chassis 臂-arm 肘-elbow  爪-hand
#计算过程的目标角度
chassis_angle_goal=0             #底盘舵机目标角度
arm_angle_goal=0                 #手臂舵机目标角度
elbow_angle_goal=0               #肘部舵机目标角度
hand_angle_goal=min_hand_angle   #手爪舵机目标角度

#复位角度
chassis_angle=90                 #底盘舵机角度
arm_angle=max_arm_angle          #臂舵机角度
elbow_angle=min_elbow_angle      #肘舵机角度
hand_angle=max_hand_angle        #手爪舵机角度


#16路PCA9685模块的，进行机械臂的复位，0，1，2，3开始的四路
servo.position(0,90)                  #底盘舵机复位
servo.position(1,max_arm_angle)
servo.position(2,min_elbow_angle)
servo.position(3,max_hand_angle)
print('复位成功')

#寻找最大色块函数
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



#主函数部分
while(True):

    #驱动小车接近小球功能，通过计算误差，判断，驱动小车，如果视野中没球，就逆时针转动
    while(True):
        clock.tick() # Track elapsed milliseconds between snapshots().
        img = sensor.snapshot() #创建一个对象img,相当于截一张图
        blobs = img.find_blobs([red_threshold])  #找颜色函数
        if blobs:   #如果找到了
            max_blob = find_max(blobs)    #调用上面定义的找最大色块函数

            #误差计算 x_error角度误差，h_error是距离误差
            x_error = max_blob[5]-img.width()/2   #x方向的误差=x-屏幕中央 img.width()=160 大于的话是在车子左边，右轮速度加快
            h_error = max_blob[2]*max_blob[3]-judge_threshold #y方向误差=最大色块面积-预定的面积 +的话过近
            print("x error:", x_error,"h_error:",h_error)

            #小车驱动判断，不使用pid
            #机械爪有效范围，或者小车目标停车范围
            #-20<=x_error<=20
            #-50<=h_error<=800
            #是调试出来的数值
            if(x_error<-20):
                if(h_error>800):
                    pass  #pass是占位置的，不执行什么的
                    print("在左边很近")
                    car.run(-80,-40) #小车后退，左轮退得快，这个调用了小车驱动函数，（左轮速度，右轮速度），起步速度在30左右
                elif(h_error<-50):  #elif就是else if
                    pass
                    print("在左边很远")
                    car.run(40,80)
            elif(x_error>20):
                if(h_error>800):
                    print("在右边很近")
                    car.run(-40,-80)
                elif(h_error<-50):
                    print("在右边很远")

                    car.run(80,40)

            elif(-20<=x_error<=20):
                if(-50<=h_error<=800):
                    print("角度和距离在范围内")
                    car.run(0,0)
                    break    #小车已经跑到目标范围内，break退出找小球循环，进入抓球循环
                elif(h_error>800):
                    print("在中间很近")
                    car.run(-60,-60)
                elif(h_error<-50):
                    print("在中间很远")
                    car.run(60,60)
        else:#如果没有找到小球，就旋转
            car.run(-100,100)  #调试发现小车起步很难，给足电压延时20ms切换慢一点速度
            time.sleep(20)
            car.run(-60,60) #左轮，右轮
            pass


    print("车开到了")
    #time.sleep(500)



    #机械臂抓取小球部分


    print('.........Loading..........')


    #获取小球距离和x_chassis,y_chassis
    i=0
    while(i<10):

        clock.tick() # Track elapsed milliseconds between snapshots().
        img = sensor.snapshot() # 从感光芯片获得一张图像
        blobs = img.find_blobs([red_threshold])
        if blobs:           #如果找到了目标颜色
                i=i+1           #让他运行10次退出
                b = blobs[0]
                img.draw_rectangle(b[0:4]) # rect
                img.draw_cross(b[5], b[6]) # cx, cy
                Lm = (b[2]+b[3])/2
                length = K/Lm
                img.draw_rectangle(b[0:4]) # rect#用矩形标记出目标颜色区域
                img.draw_cross(b[5], b[6]) # cx, cy#在目标颜色区域的中心画十字形标记

                x_camera=b[5]   #小球离摄像头中轴线距离
                l_camera=length #距离
                print(i,'正在获取小球对于球的坐标……球到摄像头距离:{}mm,  拍摄x坐标：{}mm'.format(int(length),b[5]))

    #115是广角，整个视野范围角度，平均到x方向每个像素就是115/160=0.7168（度每像素）
    x_camera=(img.width()/2-x_camera)*115/160/180*math.pi    #转换成弧度，img.width()/2=80，再-x_camera 左边为负，右边为正
    x_chassis=math.sin(x_camera)*l_camera    #x方向真实的偏移距离，纵向球到摄像头和到底座距离是是一样的
    y_chassis=math.cos(x_camera)*l_camera
    y_chassis+=114   #y方向真实的偏移距离,114是摄像头到底座的距离 , +=的写法就相当于，y_chassis=y_chassis+114
    print('得到数据:球到摄像头{}mm，拍摄x坐标{}mm(范围0-160)，x球到底座{}mm，y球到底座{}mm，角度{}°（负数为左，正数为右）'.format(int(l_camera),int(x_camera),int(x_chassis),int(y_chassis),int((x_camera-80)*115/160)))
    print('球到摄像头距离:{}cm,x球到底座：{}cm，y球到底座{}cm'.format(int(l_camera/10),int(x_chassis/10),int(y_chassis/10)))
    pass
    #获取小球距离和x_chassis,y_chassis结束


    #下面是各个舵机角度的分析
    #底座舵机如下:
    chassis_angle_tump=math.atan(x_chassis/y_chassis) #得到相对弧度  #分析过程数值
    chassis_angle_tump=chassis_angle_tump*180/math.pi   #转换成角度
    chassis_angle_goal=90-chassis_angle_tump/0.34   #0.34=50/17，这个是齿轮比  转换成舵机绝对角度
    print('底座目标角度:',chassis_angle_goal)
    #得到底座目标角度

    #手臂和肘部舵机如下:
    #arm_angle_goal和elbow_angle_goal是目标角度
    chassis_ball=math.sqrt(x_chassis**2+y_chassis**2)#底盘到球的距离，贴着地面
    arm_angle_goal=math.atan(chassis_high/chassis_ball)   #得到弧度 #角度:底座——球 & ——水平线
    arm_ball=math.sqrt(chassis_ball**2+chassis_high**2) #臂舵机到球距离
    #手臂和水平线角度余弦定理c*c = a*a + b*b - 2*a*b*cosC求反余弦减去臂舵机到球距离
    arm_angle_tump = (arm_long**2 + arm_ball**2 - elbow_hand_long**2 )/2/arm_long/arm_ball
    arm_angle_goal= arm_angle_goal*180/math.pi #手臂和水平线角度
    print('手臂算术角度：',arm_angle_goal)
    #得到手臂目标角度

    elbow_angle_goal = (elbow_hand_long**2 + arm_long**2 - arm_ball**2 )/(2 * elbow_hand_long * arm_long)
    if -1<=elbow_angle_goal<=1:
        elbow_angle_goal = math.acos(elbow_angle_goal)*180/math.pi#臂肘之间的角度
    elbow_angle_goal = elbow_angle_goal-90+arm_angle_goal  #肘舵机于竖线的角度
    #得到肘舵目标角度

    #修正角度，因为安装的时候，算出来的和安装的不一样
    arm_angle_goal-=5                     #越大越低
    elbow_angle_goal=elbow_angle_goal+70  #越大越低
    #得到臂肘舵绝对目标角度
    print('手臂目标角度：',arm_angle_goal,'肘部目标度:',elbow_angle_goal)

    #下面是驱动舵机
    #先进行角度范围的判断，这里还不完善，应该如果不在范围，就要驱动车子怎么修正，或者车子运作得很好，这里只是确保一下而已
    if chassis_angle_goal<0 or chassis_angle_goal>180 or arm_angle_goal<min_arm_angle or arm_angle_goal>max_arm_angle or elbow_angle_goal<min_elbow_angle or elbow_angle_goal>max_elbow_angle:
        print('已不在机械臂范围')
        #break

    #实现四个舵机机同步运行
    while(chassis_angle < chassis_angle_goal or arm_angle>arm_angle_goal or elbow_angle<elbow_angle_goal or hand_angle>min_hand_angle ):
        i=4
        print('在机械臂范围,抓球中……')

        while(i>=0):
            i=i-1
            if   i==3 and hand_angle>=hand_angle_goal:
                 servo.position(i,hand_angle)
                 hand_angle-=5
            elif i==2 and elbow_angle <= elbow_angle_goal:
                 pass
                 servo.position(i,elbow_angle)
                 elbow_angle+=4

            elif i==1 and arm_angle >= arm_angle_goal:
                 pass
                 servo.position(i,arm_angle)
                 arm_angle-=1
            elif i==0 and chassis_angle != chassis_angle_goal:
                    if chassis_angle > chassis_angle_goal:
                        servo.position(i,chassis_angle)
                        chassis_angle-=4
                    else:
                        servo.position(i,chassis_angle)
                        chassis_angle+=5
            time.sleep(5)     #这个控制快慢

    #伸展到位，手爪开始握紧
    time.sleep(200)
    while(hand_angle_goal< max_hand_angle):
        servo.position(3,hand_angle_goal)
        hand_angle_goal+=1
        time.sleep(10)
        print(hand_angle_goal)
        print(max_hand_angle)
        print('抓球完毕，开始回收')
    time.sleep(300)
    print('抓球完毕，开始回收!!!!!!!!!!!!!!!!!!!!')

    #抓球回来，也是同时工作
    while(elbow_angle_goal>min_elbow_angle or arm_angle_goal < max_arm_angle ):
        pass
        i=3
        while(i>=0):
        #底座，手臂，肘，手爪，对应0，1，2，3
            i=i-1
            if   i==2 and elbow_angle_goal>=min_elbow_angle:
                servo.position(i,elbow_angle_goal)
                elbow_angle_goal-=1
            elif i==1 and arm_angle_goal<=max_arm_angle:
                servo.position(i,arm_angle_goal)
                arm_angle_goal+=8
            elif i==0 and chassis_angle_goal<=90:
                servo.position(i,chassis_angle_goal)
                chassis_angle_goal+=5
            elif i==0 and chassis_angle_goal>=90:
                servo.position(i,chassis_angle_goal)
                chassis_angle_goal-=5
            time.sleep(3)

    print('回收成功了！！！！！！！！！！！！！！！！！！')
    break