6自由度智能识别机械臂项目总结

小白橘颂

已于 2024-08-21 15:20:26 修改

阅读量1.3k

点赞数 51

文章标签： stm32 物联网 OpenMV IIC 机械臂舵机步进电机

于 2024-08-21 10:50:52 首次发布

本文链接：https://blog.csdn.net/Jokerblank/article/details/141354277

版权

前言

最近做了一个机械臂的项目，主要是给学校教学使用的。本文主要记录一下本次项目的大致历程希望对后来者有所帮助。

编译环境：Kile5+FreeRTOS+7.11LVGL
界面制作：Gui-guider设计

一、硬件环境

主控芯片： STM32F103ZET6
核心和时钟

核心: ARM Cortex-M3
主频: 高达72 MHz
指令集: 支持Thumb-2指令集，提供高效的执行和代码压缩
内存

闪存: 512 KB Flash 存储器，用于程序存储和代码执行
SRAM: 64 KB SRAM，用于数据存储和操作
外设

GPIO: 多达37个通用输入/输出引脚
定时器: 3个16位定时器，1个高级控制定时器（TIM1），支持PWM输出和计时操作

ADC: 12位10通道模拟数字转换器（ADC），用于模拟信号采集
DAC: 12位数模转换器（DAC），用于模拟信号输出
USART/UART: 3个通用同步/异步收发器（USART1、USART2、USART3）
SPI: 2个串行外设接口（SPI1、SPI2）
I2C: 2个I2C接口（I2C1、I2C2）
CAN: 1个控制器局域网（CAN1），支持CAN 2.0B协议
USB: 1个全速USB 2.0接口
DMA: 7通道DMA控制器，支持数据传输的自动化和高速处理

时钟源: 内部和外部振荡器，提供灵活的时钟配置选项
复位: 支持软件和硬件复位

功耗
低功耗模式: 支持多种低功耗模式（如睡眠模式、待机模式），以降低功耗和延长电池寿命
封装类型: 64引脚LQFP（Low-profile Quad Flat Package）
电压范围: 2.0V至3.6V

机械臂：由5个舵机和一个步进电机（底座）组成，6个自由度。
采用两款舵机来驱动，其中一款是 LD-1501MG 数字舵机，另一款是 LD-20MG 数字舵机。
在这里插入图片描述
规格参数如下：

底座的步进电机规格参数如下：
在这里插入图片描述

规格	参数
型号	OC57HB54-401A
机身高	54MM
扭矩	1.3N.m
电流	2.5A/3.5A
轴径	Φ6.35/8MM
D 型轴轴长	20mm/25mm
引线长	50CM
相序	两相四线

步进电机驱动器参数：
在这里插入图片描述

本项目采用共阴极接法，分别将 PUL-，DIR-，EN-连接到控制系统的地端;脉冲输入信号通过PUL+接入，方向信号通过DIR+接入，该型号的固有步距角为1.8°，经过8细分后为0.225°即一个脉冲转动0.225°。

LCD电容触摸控屏：
采用的是AT070TN92 群创工业液晶屏

规格	参数
尺寸大小	7英寸(对角线)
分辨率	800 × 3(RGB) × 480
模块大小	164.9(W) ×100.0(H) ×5.7(D) mm
接口类型	采用Intel8080接口
LCD 驱动芯片	SSD1963
触摸屏	IIC
电容触摸芯片	GT911
触摸点数	最多5点同时触摸

其他辅助材料：
1 个 OpenMV 摄像头（通过视觉抓取）、1 个摄像头支架、3 个 RGB 积木、3 个 RGB 物料盒子、1 张机械臂定位纸。

二、步进电机、舵机控制

1.步进电机

初始化配置：

/**
  * @brief  初始化基座：步进电机
	*					PB5		DIR			PB6		PWM TIM4_CH1
  * @retval None
  */
void StepMonter_Init (void)
{
	/* GPIOB clock enable */
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO, ENABLE);
	GPIO_InitTypeDef GPIO_InitStructure;
	//PB5	DIR
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
	
	// PB6/TIM4_CH1		PUL
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);

	//TIM4_CH1
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);

	/* Time base configuration */
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	TIM_OCInitTypeDef  TIM_OCInitStructure;

	TIM_TimeBaseStructure.TIM_Period = 2000-1;//设置重装载值2ms，500hz
	TIM_TimeBaseStructure.TIM_Prescaler = 72-1;//将time4的计数速度分频至1M
	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;

	TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);

	/* PWM1 Mode configuration: Channel */
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;

	TIM_OC1Init(TIM4, &TIM_OCInitStructure);
	TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable);
	TIM_ARRPreloadConfig(TIM4, ENABLE);
	TIM_SetCompare1(TIM4, 1000-1);//设置比较值
	
	StepMotor_SetSpeed(100);//初始速度为60°/s
	StepMontor_Stop();
	
	//使能更新中断
	TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE);
	NVIC_EnableIRQ (TIM4_IRQn);
}

//中断服务函数
//执行步进电机的旋转和停止
void TIM4_IRQHandler (void)
{
	TIM_ClearFlag(TIM4, TIM_FLAG_Update);
	if (STEPDIRGET())
		stepmotor.angle += stepmotor.pulseangle;
	else
		stepmotor.angle -= stepmotor.pulseangle;
	
	//检测当前角度与目标角度差值，误差在一个±步进角度之间则表示角度一致，关闭定时器
	float difvalue = stepmotor.angle - stepmotor.targetangel;
	if(difvalue < -stepmotor.pulseangle || difvalue > stepmotor.pulseangle)
		TIM_Cmd(TIM4, ENABLE);
	else
		TIM_Cmd(TIM4, DISABLE);
}

本次使用的步进电机为两相步进电机。一般情况下，想要驱动该二相步进电机，我们只需按照该步进电机的电机激励时序图给A+A-B+B-高低电平即可。此处为了更加方便精准的驱动该步进电机，我加入了步进电机驱动器。

步进电机接口	步进电机驱动器接口
A+，A-，B+，B-，GND，VCC	脉冲信号（PUL+）地端（PUL-）方向信号（DIR+）地端（DIR-）使能信号（EN+）地端（EN-）

由上可知，通过步进电机驱动器，我们只需通过驱动脉冲信号（PUL+），方向信号（DIR+）和使能信号（EN+）即可。
PB5: 配置为推挽输出模式 (GPIO_Mode_Out_PP)，用于步进电机的方向控制 (DIR)。
PB6: 配置为复用推挽输出模式 (GPIO_Mode_AF_PP)，用于定时器的通道输出 (TIM4_CH1)，用作步进电机的脉冲信号 (PUL)。

移动固定角度

typedef struct{
	u32		subdiv;					//细分值
	float speed;					//速度
	float angle;					//当前角度
	float targetangel;		//目标角度
	float pulseangle;			//单脉冲的角度
}STEPMOTOR_TypeDef;							
//							细分值
STEPMOTOR_TypeDef stepmotor = {1600,0,0,0,0};
/**
  * @brief  让步进电机旋转到指定角度
  * @param 	angle  被设置的角度值
  * @retval None
  */
  
void StepMontor_Move (float angle)
{
	stepmotor.targetangel = angle;//更新目标值
	float difvalue = stepmotor.angle - stepmotor.targetangel;
	if(difvalue < -stepmotor.pulseangle || difvalue > stepmotor.pulseangle)
	{
		if(angle > 0 && difvalue < 0)
			STEPDIR(DIR_RIGHT);
		if((angle > 0 || angle == 0) && difvalue > 0)
			STEPDIR(DIR_LEFT);
		if((angle < 0 || angle == 0) && difvalue < 0)
			STEPDIR(DIR_RIGHT);
		if(angle < 0 && difvalue > 0)
			STEPDIR(DIR_LEFT);
		TIM_Cmd(TIM4, ENABLE);
	}
	else
		TIM_Cmd(TIM4, DISABLE);
}

实现步骤：
1.更新电机的目标角度 stepmotor.targetangel 为传入的 angle。
2.计算当前角度与目标角度之间的差值 difvalue。
3.如果 difvalue 的绝对值大于 stepmotor.pulseangle（即差值超出容许的范围），则判断需要旋转的方向：
4.如果 angle 大于0且 difvalue 小于0，电机需要向右旋转。
5.如果 angle 大于或等于0且 difvalue 大于0，电机需要向左旋转。
6.如果 angle 小于或等于0且 difvalue 小于0，电机需要向右旋转。
7.如果 angle 小于0且 difvalue 大于0，电机需要向左旋转。
8.启用定时器 TIM4 以控制电机旋转。
9.如果角度差值小于或等于 pulseangle，则禁用定时器 TIM4，停止电机旋转。

设置步进电机的旋转速度

/********************************************
		设置步进电机的旋转速度	
		传参：angle		angle°/s
*********************************************/
void StepMotor_SetSpeed(float angle)
{
	stepmotor.pulseangle = 360.0 / stepmotor.subdiv;
	stepmotor.speed = angle;
	
	//根据速度求每秒钟的脉冲数 = 每秒钟的角度 / 单脉冲角度
	float pulsenum = angle / stepmotor.pulseangle + 0.5;
	if(pulsenum < 0.16)
	{
		//如果每秒钟脉冲数小于0.16个，则设置速度为每秒0.16个脉冲
		TIM4->PSC = 7200 - 1;
		TIM_SetAutoreload(TIM4, 65535);
		TIM_SetCompare1(TIM4, 32768);		
	}
	else if(pulsenum < 16)
	{
		//如果脉冲给的速度比 65.536ms 慢,那么我们可以重新修改分频值
		TIM4->PSC = 7200 - 1;
		TIM_SetAutoreload(TIM4, 10000.0f/pulsenum);
		TIM_SetCompare1(TIM4, 10000.0f/pulsenum/2);
	}
	else
	{
		//在1us一次的计数速率下，1s内的脉冲个数最快1us一次，最慢65.536ms一次
		TIM4->PSC = 72 - 1;//72分频，1us计数一下
		TIM_SetAutoreload(TIM4, 1000000.0f/pulsenum);
		TIM_SetCompare1(TIM4, 1000000.0f/pulsenum/2);	
	}
	
}

该函数通过调整定时器的参数来实现对步进电机速度的精确控制。根据需要的速度，选择适当的分频器和定时器参数，以确保电机能够按预期的速度旋转。
步进电机的每转一圈的角度取决于电机的步距角(1.8°)。

2.舵机

初始化

/**************************************************
	机械臂 		舵机初始化函数
***************************************************/
void ArmCtrl_Init(void)
{
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);

	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_AFIO, ENABLE);

	GPIO_InitTypeDef GPIO_InitStructure;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_6 | GPIO_Pin_7;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

	GPIO_Init(GPIOA, &GPIO_InitStructure);

	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	TIM_OCInitTypeDef  TIM_OCInitStructure;

	TIM_TimeBaseStructure.TIM_Period = 20000-1;//重装载值，20ms，50hz
	TIM_TimeBaseStructure.TIM_Prescaler = 72-1;
	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;

	TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
	TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure);

	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;

	TIM_OC1Init(TIM3, &TIM_OCInitStructure);
	TIM_OC2Init(TIM3, &TIM_OCInitStructure);
	TIM_OC1Init(TIM5, &TIM_OCInitStructure);
	TIM_OC2Init(TIM5, &TIM_OCInitStructure);
	TIM_OC3Init(TIM5, &TIM_OCInitStructure);
	TIM_OC4Init(TIM5, &TIM_OCInitStructure);

	TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);
	TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
	
	TIM_OC1PreloadConfig(TIM5, TIM_OCPreload_Enable);
	TIM_OC2PreloadConfig(TIM5, TIM_OCPreload_Enable);
	TIM_OC3PreloadConfig(TIM5, TIM_OCPreload_Enable);
	TIM_OC4PreloadConfig(TIM5, TIM_OCPreload_Enable);
	
	TIM_ARRPreloadConfig(TIM3, ENABLE);
	TIM_ARRPreloadConfig(TIM5, ENABLE);

	TIM_Cmd(TIM3, ENABLE);
	TIM_Cmd(TIM5, ENABLE);
	Arm_UpdateAngle(motor_pos[1]*1.8,motor_pos[2]*1.8,motor_pos[3]*1.8,motor_pos[4]*1.8,motor_pos[5]*1.8);	

}

该函数配置了定时器和 GPIO ，以生成 PWM 信号来控制电机。配置了两组定时器（TIM3 和 TIM5），每个定时器都有多个 PWM 输出通道，且都设置为 20ms 的周期（50Hz），初始占空比为 0。最后，通过 Arm_UpdateAngle 函数更新电机的目标位置。

具体为什么配置那个引脚和定时器，这个是由原理图得出的。
在这里插入图片描述

PID控制
完成了以上配置，我们就可以直接改变PWM的占空比来控制舵机。但是为了使舵机控制系统能够实现更高的精度、稳定性和适应性，我引入了PID控制器来控制舵机。
PID 控制舵机的原理基于比例（P）、积分（I）和微分（D）控制的组合，通过不断调整控制信号来使舵机达到并保持在目标位置。

//舵机的位置式PID参数							Pi	Ki	Di	Ek	Ek1	Ek2	LocSum
PID_LocTypeDef position_pid[6] = {{6,	0,	3,	0,	0,	0,	0},
																	{6,	0,	3,	0,	0,	0,	0},
																	{6,	0,	3,	0,	0,	0,	0},
																	{6,	0,	3,	0,	0,	0,	0},
																	{6,	0,	3,	0,	0,	0,	0},
																	{6,	0,	3,	0,	0,	0,	0}};


/*************************************************************************
函数名称 ： PID_Location
功    能 ： PID位置(Location)计算
参    数 ： SetValue ------ 设置值(期望值)
            ActualValue --- 实际值(反馈值)
            PID ----------- PID数据结构
返 回 值 ： PIDLoc -------- PID位置
*************************************************************************/
float PID_Location(float SetValue, float ActualValue, PID_LocTypeDef * PID)
{
	float PIDLoc;
	PID->Ek = SetValue - ActualValue;
	PID->LocSum += PID->Ek;
	PIDLoc = PID->Kp*PID->Ek/100 + PID->Ki*PID->LocSum/100 + PID->Kd*(PID->Ek1-PID->Ek)/100;
	PID->Ek1 = PID->Ek;
	return PIDLoc;
}

void Arm_Update_Position(void)
{
	for(u8 i=0; i<6; i++)
	{
		Arm_Position[i] += PID_Location(Arm_SetValue[i], Arm_Position[i], &position_pid[i]);
		Arm_SetDJ_PWM(i, Arm_Position[i]);
	}
}

角度转换PWM控制：

void Arm_SetAngle(u8 djnum, float angle)
{
	Arm_TargetAngle[djnum] = angle;
	switch(djnum)
	{
		case 0:;break;//空
		case 1://4号	范围 -90 -- 90			传递参数时传递物块与x轴的偏角angle	500方向为	angle 值为正		
			Arm_SetValue[djnum] = Arm_TargetAngle[djnum]/180.0*2000+500;
		break;		
		case 2://3号	范围 -180° -- 0°		500 - 2500
			Arm_SetValue[djnum] = (Arm_TargetAngle[djnum])/180.0*2000+500;
		break;
		case 3://2号	范围 -90 -- 90			500 - 2500
			Arm_SetValue[djnum] = Arm_TargetAngle[djnum]/180.0*2000+1500;
		break;
		case 4://1号	范围 180 -- 0				500 - 2500
			Arm_SetValue[djnum] = (Arm_TargetAngle[djnum])/180.0*2000+500;
		break;
		case 5://爪		夹距 0 -- 5.5cm			2500 - 500	180 - 0
			//爪子在夹取过程需要等待其他舵机移动完成才能抓取
			Arm_SetValue[djnum] = (Arm_TargetAngle[djnum])/180.0*2000+500;
			break;
	}
}

原理：每种舵机类型有其特定的角度范围和 PWM 范围。通过将角度值进行线性转换，代码计算出适合该舵机的 PWM 值。该 PWM 值可以控制舵机到达和保持目标角度。

三、GT911触摸驱动（IIC）

GT911电容触摸驱动

1.IIC

IIC知识点

2.个人理解

连接方式：
在这里插入图片描述

触摸芯片的硬件接口

接口	介绍
VCC	电源
GND	地
SCL	与GT911进行IIC通信的时钟线
SDA	与GT911进行IIC通信的数据线
INT	当发生触摸时，GT911的INT脚会输出上升/下降沿（内部寄存器可以配置），主机就可以通过外部中断处理去读取触点信息。
RST	复位引脚，拉低100us以上，即可复位。正常工作时，应该保持拉高

相关屏幕触摸驱动和IIC配置文件官方给的都有，只需要修改接口即可。

从LCD屏到LVGL到屏幕触摸，配置步骤如下：
1.首先将该LCD屏官方写好的bsp_lcd和font文件移植到工程中。
2.将LVGL源码复制添加到工程中。
3.修改lv_conf.h配置文件和屏幕接口。屏幕的大小和填充方式等等。
4.配置lv_port_indev_templ.ctp_dev.init();输入设备文件。触摸屏初始化，按压检测，按压坐标获取等。

GT911 触摸屏控制器通过 I2C 接口与主机芯片通信，从而提供触摸坐标的数据。

获取触摸坐标的流程：
1.当手指按下时，GT911的INT脚会输出上升/下降沿。
2.此时GT911 控制器会将触摸坐标存储在特定的寄存器中
3.主机通过INT脚输出上升/下降沿进入外部中断，从控制器的寄存器中读取触摸数据。
4.解析从寄存器中读取的触摸数据。

3.界面进度条控制

1.打开Gui-guider，在界面中放置一个进度条。
在这里插入图片描述
2.事件添加

4.勾选用户代码

5.移植到工程中后打开事件文件

6.找到对应的事件

根据需求更改case以及想要触发的事情即可。

四、OpenMV自动抓取

OpenMV是一个专门为嵌入式视觉应用设计的硬件平台，提供了易于使用的编程接口（Python），主要用于简单的视觉任务，如物体识别和跟踪。
OpenCV是一个强大的计算机视觉库，适用于各种平台（包括PC和嵌入式系统），支持更复杂的视觉处理和机器学习任务。
总结：OpenMV更适合需要嵌入式解决方案的项目，而OpenCV则适合需要全面计算机视觉功能的应用。

OpenMV模块主控芯片详情
OpenMV

更多了解模块官网

1.OpenMV物块数据获取

# AprilTags Example
#
# This example shows the power of the OpenMV Cam to detect April Tags
# on the OpenMV Cam M7. The M4 versions cannot detect April Tags.

import sensor, image, time, math, pyb
from pyb import LED, UART

#led灯的定义
LedRed = LED(1)
LedGreen = LED(2)
LedBlue = LED(3)

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA) # we run out of memory if the resolution is much bigger...
sensor.skip_frames(100)
sensor.set_auto_gain(False)  # must turn this off to prevent image washout...
sensor.set_auto_whitebal(False)  # must turn this off to prevent image washout...
clock = time.clock()
uart = UART(3,9600,timeout_char=1000)
uart.init(9600, bits=8, parity=None, stop=1, timeout_char=1000)

#apriltag 坐标信息存储列表
apriltags = [[0,0],[0,0],[0,0],[0,0]]

#坐标系的原点和宽高 x y w h
coordinate = [1,1,1,1]

#定义各个可能需要被识别的色块
red_threshold = (88, 29, 1, 127, 10, 57)
green_threshold = (100, 23, -128, -23, 9, 84)
blue_threshold = (17, 38, -5, 34, -50, -22)

#查询所有apritag，并将有用的apriltag保存到列表内
#img:相机拍摄照片后的image对象
def search_apritags(img):
    for tag in img.find_apriltags(families=image.TAG16H5):
        img.draw_rectangle(tag.rect(), color = (255, 0, 0))     #用红色框出apriltag
        img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0)) #用绿色十字标出apriltag的中心
        #将apriltag信息存放到apriltags列表内
        if tag.id() >= 0 and tag.id() <= 3:
            apriltags[tag.id()] = [tag.cx(), tag.cy()]


#检测apriltags内是否保存有四个坐标点
def check_apriltags():
    for i in range(0,4):
        if apriltags[i][0] == 0 and apriltags[i][1] == 0:
            return 0
    return 1

ROI=(10,10,20,20)

#确定原点坐标以及背景区域的宽高像素值
#num:原点坐标apritag的ID号
#    目前使用2号apritag为原点坐标
#       2       0
#
#       3       1
def make_coordinate(num=2):
    if check_apriltags() == 1:
        coordinate[0] = apriltags[num][0]
        coordinate[1] = apriltags[num][1]
        coordinate[2] = (int)((apriltags[0][0] - apriltags[2][0] + apriltags[1][0] - apriltags[3][0])/2 + 0.5)
        coordinate[3] = (int)((apriltags[3][1] - apriltags[2][1] + apriltags[1][1] - apriltags[0][1])/2 + 0.5)
        print(coordinate)

#寻找色块  red  1   blue  2   green  4
#发送数据  一次只发送一个目标点的信息
def find_color(img, roi):
    blobs = img.find_blobs([red_threshold, blue_threshold, green_threshold],roi=roi)
    if blobs:
        for b in blobs:
            if b.w() > 10 and b.h() > 10:
                img.draw_rectangle(b[0:4])
                img.draw_cross(b[5],b[6])
        for b in blobs:
            if b.w() > 10 and b.h() > 10:
                com_sendbyte(b.cx(),b.cy(),b.code())
                break

#整合数据、串口发送
#帧头 背景宽高位  背景宽低位  背景高高位  背景高低位  颜色  y高位 y低位 x高位 x低位 校验和
def com_sendbyte(x, y, color):
    bwh = (coordinate[2] & 0xFF00) >> 8
    bwl = coordinate[2] & 0x00FF
    bhh = (coordinate[3] & 0xFF00) >> 8
    bhl = coordinate[3] & 0x00FF

    target_x = x - coordinate[0]
    target_y = y - coordinate[1]
    txh = (target_x & 0xFF00) >> 8
    txl = target_x & 0x00FF
    tyh = (target_y & 0xFF00) >> 8
    tyl = target_y & 0x00FF

    check = txh + txl + tyh + tyl + bwh + bwl + bhh + bhl + color

    print(bwh, bwl, bhh, bhl, color, txh, txl, tyh, tyl, check)
    arr = bytearray([bwh, bwl, bhh, bhl, color, txh, txl, tyh, tyl, check])
    uart.write(arr)


find_aprtime = pyb.millis()
find_blobtime = pyb.millis()
while(True):
    img = sensor.snapshot().lens_corr(strength = 1.2, zoom = 1.2)
    ROI = tuple(coordinate)
    img.draw_rectangle(ROI)
    if pyb.millis() - find_aprtime > 1000:
        find_aprtime = pyb.millis()
        search_apritags(img)    #寻找坐标点
        make_coordinate()       #建立坐标系
    if pyb.millis() - find_blobtime > 100:
        find_blobtime = pyb.millis()
        find_color(img,ROI)

坐标获取：
上面是一个用python实现的代码，通过识别AprilTag来建立坐标系，从而获取物块的x,y坐标。
颜色获取：
通过识别小物块的颜色阈值来识别物块。不同颜色的颜色阈值不同，颜色阈值包含了该颜色的色调、饱和度和亮度。
最终，将检测到的颜色块，坐标位置等信息通过该模块的串口3传输给主控芯片的串口1。

2.坐标解析转换

获取OpenMV传过来的数据

void OpenMV_Analysis(void)
{
	u8 check = 0;
	u16 bgw = 0, bgh = 0, tx = 0, ty = 0;
	float x = 0, y = 0;
	if(openmv.openmv_rxover == 0)	return;		//保证接收完成才解析
	
	for(u8 i=0; i<openmv.openmv_rxcount-1; i++)	//计算校验值
		check += openmv.openmv_rxbuff[i];
	
	if(check != openmv.openmv_rxbuff[openmv.openmv_rxcount-1])	//校验失败
	{
		memset(&openmv,0,sizeof(openmv));
		return;
	}
	
//	for(u8 i=0; i<openmv.openmv_rxcount; i++)	//计算校验值
//		printf("%02X\t", openmv.openmv_rxbuff[i]);
//	printf("\r\n");
	
	
	bgw = (openmv.openmv_rxbuff[0]<<8) | openmv.openmv_rxbuff[1];		//背景宽度像素大小  y
	bgh = (openmv.openmv_rxbuff[2]<<8) | openmv.openmv_rxbuff[3];		//背景高度像素大小	x
	taget.color = openmv.openmv_rxbuff[4];
	ty = (openmv.openmv_rxbuff[5]<<8) | openmv.openmv_rxbuff[6];		//目标像素 y
	tx = (openmv.openmv_rxbuff[7]<<8) | openmv.openmv_rxbuff[8];		//目标像素 x
	
	x = ((float)tx/bgh) * BG_HIGH + LENGTH;
	y = (bgw/2.0f)-ty;
	y = y/bgw * BG_WIDTH;


	taget.x = sqrt(x*x+y*y);
	taget.alpha = Radian_2_Angle(atan2(y,x));
	taget.y = 0;	//目前不做处理，针对于小木块，距离基座高度位置固定
	printf("taget.x = %0.2f\t taget.alpha = %0.2f,taget.color:%d\t\n",taget.x, taget.alpha,taget.color);
	
  memset(&openmv,0,sizeof(openmv));//执行完一次之后再清理
}

其中比较重要的数据就是x轴坐标，针对木块的偏移角度，以及识别物块的颜色等。红：1 蓝：2 绿：4

/*****************************************************************
 *						机械臂 几何解析法 函数
 *	功能：根据末端执行器的轴坐标 以及末端执行器与x轴之间的夹角
 *		  来求出各个关节的角度
 *	传参：x,y末端执行器关节节点的坐标（轴的坐标）
 *	传参：phi,φ末端执行器与x轴之间的夹角
 *	整体思路：先求θ2：有几何关系可得：
 *					x^2 + y^2 = l1^2 + l2^2 - 2*l1*l2*cos(180-θ2)
 *			  再根据余弦定理求中间变量ψ  ，再求出θ1，继而求出θ3
 *****************************************************************
 */
Arm_Angle_TypeDef Arm_Angle_Analysis(float x, float y, float phi)
{
	Arm_Angle_TypeDef arm_angle = {0};
	float psi = 0;		//ψ	中间变量
	float l1 = arm.l1;
	float l2 = arm.l2;
	float l1_2 = l1 * l1;
	float l2_2 = l2 * l2;
	float x_2 = x * x;
	float y_2 = y * y;
	float theta1, theta2, theta3;

	phi = Angle_2_Radian(phi);

	//先求theta2	
	theta2 = PI - acos( (l1_2 + l2_2 - x_2 - y_2) / (2 * l1 * l2) );

	//求中间变量ψ
	psi = acos( (l2_2 - x_2 - y_2 - l1_2) / (-2 * l1 * sqrt(x_2 + y_2)) );

	//求theta1
	if (theta2 >= 0)
		theta1 = atan2(y, x) - psi;	
	else
		theta1 = atan2(y, x) + psi;

//	if (theta1 > -0.0524 && theta1 < 0.0524)
//		theta1 = 0;

	//我们的手臂考虑不到theta1小于零的情况
	if (theta1 <= 0)
	{
		theta2 = -theta2;
		theta1 = atan2(y, x) + psi;
	}
	
	//求theta3
	theta3 = phi - theta1 - theta2;

	arm_angle.theta1 = Radian_2_Angle(theta1);
	arm_angle.theta2 = Radian_2_Angle(theta2);
	arm_angle.theta3 = Radian_2_Angle(theta3);
	return arm_angle;
}

以上是一个机械臂角度解析 (Arm_Angle_Analysis函数)
该函数根据串口1收到的小木块坐标（x 和 y）及其与 X 轴之间的夹角（phi），计算机械臂各个关节的角度 (theta1, theta2, theta3)。
主要用到了逆运动学算法，通过已知物体位置反推关轴角度。
在这里插入图片描述

详细计算过程如下：
1.利用几何关系和余弦定理来求解 theta2，公式如下：
在这里插入图片描述
L1，L2为机械臂两个舵机之间的关节长度。

2.计算中间变量 psi
使用余弦定理来求解中间变量 psi，公式如下：
在这里插入图片描述

3.计算 theta1
根据 theta2 和 psi 的值计算 theta1，公式如下：
在这里插入图片描述
对于 theta1 小于零的情况进行特殊处理，以确保 theta1 的值在合理范围内。

4.计算 theta3
利用小木块偏移的夹角 phi 计算 theta3：
在这里插入图片描述
5.返回角度
将计算得到的角度转换为角度制并返回结果。


/***************************************************************
 *
 *   运动
 *	 根据物块实际坐标 计算并更新机械臂目标角度
 *	 传参：x 物块距离基座的轴垂线距离
 *				 y 物块距离基座的高度   针对于小木块目前不做处理
 *
 ***************************************************************
 */
 
void Arm_Kinematic_2_Angle(float x, float y)
{
	if(x < 23)
		x = x-1;
	if(x < 12)
		arm.angle = Arm_Angle_Analysis(x, 3, -110);
	else if(x < 13)
		arm.angle = Arm_Angle_Analysis(x, 3, -100);
	else if(x <= 20)
		arm.angle = Arm_Angle_Analysis(x, 3, -90);
	else if(x <21)
		arm.angle = Arm_Angle_Analysis(x-10, -1, -60);
	else if(x <= 23)
		arm.angle = Arm_Angle_Analysis(x-10, -1, -45);
	else if(x <= 25)
		arm.angle = Arm_Angle_Analysis(x-10, -1, -45);
	else
		arm.angle = Arm_Angle_Analysis(x-10, -1, -45);	
	//将解析后得到的角度赋值到目标角度
	Arm_TargetAngle[4] = arm.angle.theta1;
	Arm_TargetAngle[3] = arm.angle.theta2;
	Arm_TargetAngle[2] = arm.angle.theta3;
	Auto_TargetAngle[4] = arm.angle.theta1;
	Auto_TargetAngle[3] = arm.angle.theta2;
	Auto_TargetAngle[2] = arm.angle.theta3;
}

机械臂运动
功能：
根据物块的实际坐标 x 更新机械臂目标角度。根据不同的 x 范围选择不同的 phi 值，并调用 Arm_Angle_Analysis 函数计算角度。

处理步骤：
调整 x 值：
如果 x 小于 23，x 减去 1。这可能是为了处理特定的工作区域。

根据 x 范围选择 phi 值：
根据 x 的值选择不同的 phi 值，并调用 Arm_Angle_Analysis 函数来计算机械臂的角度。具体的 phi 值取决于 x 的范围。

更新目标角度：
将解析得到的角度（theta1, theta2, theta3）赋值给目标角度数组 Arm_TargetAngle 和 Auto_TargetAngle。

关键代码解释：
Angle_2_Radian 和 Radian_2_Angle 函数：用于角度和弧度的转换。
atan2(y, x)：计算 x 和 y 的反正切值，考虑了坐标的象限。
PI：表示圆周率。
这段代码的主要目的是通过解析末端执行器的坐标和夹角来计算机械臂的各个关节角度，并根据物块的实际坐标更新目标角度。

3.木块识别以及自动抓取

	//函数：Arm_AutoMove
//功能：机械臂自动控制
//传参：x，y坐标
//			alpha  坐标转角
//红：1	蓝：2	绿：4

void Arm_AutoMove111(float x, float y, float alpha)
{
	static u8 movestate = 0;
	if(x > 30)
	{
		memset(&openmv,0,sizeof(openmv));
		return;
	}
	switch(movestate)
	{
		case 0://初始化动作  丢掉物块动作
			Arm_AllAngle_Update(90, 0, -90, 0, 170, 0);
			if(Difference_auto_Angle(6))
			{
				movestate = 1;
				Delay_nms(500);
			}
			break;
		case 1://根据目标位置 找到目标
			Arm_Kinematic_2_Angle(x,y);
			//更新目标角度值
			Arm_AllAngle_Update(Auto_TargetAngle[4],Auto_TargetAngle[3],Auto_TargetAngle[2],Auto_TargetAngle[1],170,alpha);	
			if(Difference_auto_Angle(6))	
			{
				movestate = 2;
				Delay_nms(500);
			}
			break;
		case 2://夹取
			Arm_AllAngle_Update(Auto_TargetAngle[4],Auto_TargetAngle[3],Auto_TargetAngle[2],Auto_TargetAngle[1],50,alpha);
			if(Difference_auto_Angle(6))	
			{
				movestate = 3;
				Delay_nms(500);
			}
			break;
		case 3://拿起
			Arm_AllAngle_Update(90,0,-90,0,50,0);
			if(Difference_auto_Angle(6))	
			{
				movestate = 4;
			}
			break;
		case 4://松开
			Arm_AllAngle_Update(45,0,-90,0,50,-65);
			if(Difference_auto_Angle(6))
			{					
					TIM_SetCompare2(TIM3, 500);
					Delay_nms(1000);
					movestate = 5;
					
//					StepMontor_Move(0);
			}
			break;
		case 5:
			Arm_AllAngle_Update(90, 0, -90, 0, 170, 0);
			if(Difference_auto_Angle(6))
			{
				auto_flag=0;
				memset(&openmv,0,sizeof(openmv));//清空openmv的标志，用于接收下一次数据
				memset(&taget,0,sizeof(taget));
				movestate = 0;
				Delay_nms(500);
			}
			break;
	}
	Arm_Update_Auto_Position();//更新舵机位置
	if(movestate == 3)
	{
		memset(Auto_TargetAngle,0,sizeof(Auto_TargetAngle));
	}
}

void Arm_AllAngle_Update(float angle1, float angle2, float angle3, float angle4, float angle5, float angle6)
{
	Arm_UpdateAngle111(angle1, angle2, angle3, angle4, angle5);
	StepMontor_Move(angle6);	
}

给该函数传入小物块的坐标的偏移角度，然后将该函数需放在while（1）里执行即可。
自动抓取流程如下：
1.初始化动作：将机械臂手动调整到未抓取时的初始位置。
2.找到目标：通过解析坐标和偏移角度，更新目标角度。
3.夹取物块：调整夹子的舵机角度以夹取物块。
4.拿起物块：将机械臂调整为拿起状态。
5.松开物块：调整夹子的舵机角度以松开物块，最后重置状态准备下一次操作。