硬件
1.4自由度机械臂(3自由度也可以,少量改动即可)
2.pca9685舵机扩展(不使用的的话,只需改动所有舵机控制语句)
3.openmv
4.蓝牙hc-06
上一篇【详细讲解 附全部代码】【openmv控制三自由度机械臂抓取物品】硬件+软件
本篇在之前的基础上增加了:
1.蓝牙控制: 开始 结束 选择颜色 自动模式
2.放置位置可以不固定
3.一些优化
使用方式:
1.上电
2.运行代码
3.连接蓝牙
4.选择颜色-开始或自动模式
5.选择结束,摆放色块位置,重复步骤4
要更改的内容:
1.颜色阈值
red_threshold = (18, 60, 29, 127, -3, 124)#颜色阈值2 0010(14, 100, 8, 127, 9, 116)(14, 64, 17, 127, -3, 124)
blue_threshold = (10, 81, -128, 127, -128, -15)#颜色阈值4 0100
yellow_threshold = (53, 100, -12, 97, 19, 127)#颜色阈值1 0001
green_threshold = (25, 100, -128, -29, 6, 127)#颜色阈值
2.你的舵机限制角度和初始值
#以下是我的数据
#0号:0-150度
#1号:0-50度
#2号:0-130度,
#3号:0-90度,0度夹住
s0_now=95
s1_now=35
s2_now=75
s3_now=45
if s0_move > 140:#这4句限制舵机运动角度,防止卡死
s0_move = 140
if s0_move < 10:
s0_move = 10
3.接线
#以下是我的数据
#接线:一共4跟,正负,scl-p4,sda-p5
#舵机从下往上分别为0,1,2,3号,接在0 3 4 7
4.爪子位置补偿
cx=int(clap.x()+clap.w()/2+(clap.x()+clap.w()/2-180)*0.15)+5#改0.15和5这两个数字
完整代码
#接线:一共4跟,正负,scl-p4,sda-p5
#舵机从下往上分别为0,1,2,3号,接在0 3 4 7
#0号:0-150度
#1号:0-50度
#2号:0-130度,
#3号:0-90度,0度夹住
import sensor, image, time,pyb
from pyb import UART
from pyb import LED
from servo import Servos
from machine import I2C, Pin
uart = UART(1,38400)
uart.init(9600,bits=8,parity=None,stop=1,timeout_char=1000)#串口初始化
i2c = I2C(sda=Pin('P5'), scl=Pin('P4'))
servo = Servos(i2c, address=0x40, freq=50, min_us=650, max_us=2800, degrees=180)
red_threshold = (18, 60, 29, 127, -3, 124)#颜色阈值2 0010(14, 100, 8, 127, 9, 116)(14, 64, 17, 127, -3, 124)
blue_threshold = (10, 81, -128, 127, -128, -15)#颜色阈值4 0100
yellow_threshold = (53, 100, -12, 97, 19, 127)#颜色阈值1 0001
green_threshold = (25, 100, -128, -29, 6, 127)#颜色阈值
sensor.reset() # Reset and initialize the sensor.
sensor.set_pixformat(sensor.RGB565) # Set pixel format to RGB565 (or GRAYSCALE)
sensor.set_framesize(sensor.QVGA) # Set frame size to QVGA (320x240)
sensor.skip_frames(time = 2000) # Wait for settings take effect.
clock = time.clock() # Create a clock object to track