import sensor, image, time,math,pyb
from pyb import Pin,Timer,UART
import kinematics,robotMoveCmd
class Apriltag():
uart = UART(3,115200) #设置串口波特率，与stm32一致
uart.init(115200, bits=8, parity=None, stop=1 )
tim = Timer(4, freq=1000) # Frequency in Hz
led_dac = tim.channel(1, Timer.PWM, pin=Pin("P7"), pulse_width_percent=50)
led_dac.pulse_width_percent(50)
#机器人运动
robot_move_cmd = robotMoveCmd.RobotMoveCmd()
kinematic = kinematics.Kinematics()
def init(self,cx=80.5,cy=60.5):#初始化巡线配置，传入两个参数调整中位值
sensor.reset() #初始化摄像头
sensor.set_pixformat(sensor.RGB565) #图像格式为 RGB565 灰度 GRAYSCALE
sensor.set_framesize(sensor.QQVGA) #QQVGA: 160x120
sensor.skip_frames(n=2000) #在更改设置后，跳过n张照片，等待感光元件变稳定
sensor.set_auto_gain(True) #使用颜色识别时需要关闭自动自动增益
sensor.set_auto_whitebal(True)#使用颜色识别时需要关闭自动自动白平衡
self.uart.init(115200, bits=8, parity=None, stop=1 )
self.led_dac.pulse_width_percent(50)
self.place_block_ID=0#二维码ID
self.mid_block_cx=cx
self.mid_block_cy=cy
self.mid_block_cnt=0#用来记录机械臂已对准物块计数，防止误差
self.move_status=0#机械臂移动的方式
#用来记录已经抓取到标签
self.apriltag_succeed_flag=0
self.place_succeed_flag=0
self.block_degress=0#机械爪旋转角度
#机械臂移动位置
self.move_x=0
self.move_y=160
robot_move_cmd = robotMoveCmd.RobotMoveCmd()
kinematic = kinematics.Kinematics()
self.kinematic.send_str("{#000P1460T1500!}")
time.sleep_ms(100)
self.kinematic.send_str("{#001P1750T1500!}")
time.sleep_ms(100)
self.kinematic.send_str("{#002P1800T1500!}")
time.sleep_ms(100)
self.kinematic.send_str("{#003P500T1500!}")
self.kinematic.send_str("{#005P500T1500!}")
time.sleep_ms(100)
self.kinematic.send_str("{#004P1500T1500!}")
time.sleep_ms(2000)
def flag_def(self):
self.oled_show_flag1=1
self.oled_show_flag2=1
self.oled_show_flag3=1
self.oled_show_flag4=1
def run_sort(self):#分拣
self.mid_cx=80.5
self.mid_cy=60.5
#物块中心点
self.block_cx=0
self.block_cy=0
color_read_succed=0#是否识别到颜色
# 获取图像
img = sensor.snapshot()
if self.apriltag_succeed_flag==0:#识别抓取标签
for tag in img.find_apriltags(families=image.TAG36H11 | image.TAG25H9): # defaults to TAG36H11 without "families".
img.draw_rectangle(tag.rect(), color = (255, 0, 0)) #画框
img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0)) #画十字
img.draw_string(tag[0], (tag[1]-10), "{}".format(tag.id()), color=(255,0,0))
self.block_cx=tag.cx()
self.block_cy=tag.cy()
print("block_cx",self.block_cx)
print("block_cy",self.block_cy)
self.block_degress = 180 * tag.rotation() / math.pi#求April Tags旋转的角度
self.place_block_ID=tag.id()
print("self.place_block_ID", self.place_block_ID)
print("self.move_status",self.move_status)
#************************************************ 运动机械臂*************************************************************************************
color_read_succed=1
B
bzug
@bzug
0
声望
2
楼层
537
资料浏览
0
粉丝
0
关注
bzug 发布的帖子
-
openmv的Apriltag能自己布局吗？发布在 OpenMV Cam
-
如果我想识别比赛方提供的标签怎么办，有什么方法录入？发布在 OpenMV Cam
import sensor, image, time,math,pyb
from pyb import Pin,Timer,UART
import kinematics,robotMoveCmdclass Apriltag():
uart = UART(3,115200) #设置串口波特率，与stm32一致 uart.init(115200, bits=8, parity=None, stop=1 ) tim = Timer(4, freq=1000) # Frequency in Hz led_dac = tim.channel(1, Timer.PWM, pin=Pin("P7"), pulse_width_percent=50) led_dac.pulse_width_percent(50) #机器人运动 robot_move_cmd = robotMoveCmd.RobotMoveCmd() kinematic = kinematics.Kinematics() def init(self,cx=80.5,cy=60.5):#初始化巡线配置，传入两个参数调整中位值 sensor.reset() #初始化摄像头 sensor.set_pixformat(sensor.RGB565) #图像格式为 RGB565 灰度 GRAYSCALE sensor.set_framesize(sensor.QQVGA) #QQVGA: 160x120 sensor.skip_frames(n=2000) #在更改设置后，跳过n张照片，等待感光元件变稳定 sensor.set_auto_gain(True) #使用颜色识别时需要关闭自动自动增益 sensor.set_auto_whitebal(True)#使用颜色识别时需要关闭自动自动白平衡 self.uart.init(115200, bits=8, parity=None, stop=1 ) self.led_dac.pulse_width_percent(50) self.place_block_ID=0#二维码ID self.mid_block_cx=cx self.mid_block_cy=cy self.mid_block_cnt=0#用来记录机械臂已对准物块计数，防止误差 self.move_status=1#机械臂移动的方式 #用来记录已经抓取到标签 self.apriltag_succeed_flag=0 self.place_succeed_flag=0 self.block_degress=0#机械爪旋转角度 #机械臂移动位置 self.move_x=0 self.move_y=160 robot_move_cmd = robotMoveCmd.RobotMoveCmd() kinematic = kinematics.Kinematics() self.kinematic.send_str("{#000P1460T1500!}") time.sleep_ms(100) self.kinematic.send_str("{#001P1750T1500!}") time.sleep_ms(100) self.kinematic.send_str("{#002P1800T1500!}") time.sleep_ms(100) self.kinematic.send_str("{#003P500T1500!}") self.kinematic.send_str("{#005P500T1500!}") time.sleep_ms(100) self.kinematic.send_str("{#004P1500T1500!}") time.sleep_ms(2000) def flag_def(self): self.oled_show_flag1=1 self.oled_show_flag2=1 self.oled_show_flag3=1 self.oled_show_flag4=1 def run_sort(self):#分拣 self.mid_cx=80.5 self.mid_cy=60.5 #物块中心点 self.block_cx=0 self.block_cy=0 color_read_succed=0#是否识别到颜色 # 获取图像 img = sensor.snapshot() if self.apriltag_succeed_flag==0:#识别抓取标签 for tag in img.find_apriltags(): # defaults to TAG36H11 without "families". img.draw_rectangle(tag.rect(), color = (255, 0, 0)) #画框 img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0)) #画十字 img.draw_string(tag[0], (tag[1]-10), "{}".format(tag.id()), color=(255,0,0)) self.block_cx=tag.cx() self.block_cy=tag.cy() print("block_cx",self.block_cx) print("block_cy",self.block_cy) self.block_degress = 180 * tag.rotation() / math.pi#求April Tags旋转的角度 self.place_block_ID=tag.id() print("self.place_block_ID", self.place_block_ID) print("self.move_status",self.move_status) #************************************************ 运动机械臂************************************************************************************* color_read_succed=1