目录
13. 工作温度（Operating Temperature）
14. 安装方式和尺寸（Mounting and Dimensions）
视觉闭环部分代码与思路 [闭环控制] （只包含控制部分代码讲解不包含是视觉部分与数据传输方面的分析与讲解)与姿态分析相关代码与思路 （如何建立的直角坐标系与定位，不包含数据传输方面的分析与讲解）：
文章内所有代码都是提供解决思路，具体请查看购买舵机的数据手册与例程代码。
舵机：
舵机(Servo)是在程序的控制下，在一定范围内连续改变输出轴角度并保持的电机系统。即舵机只支持在一定角度内转动，无法像普通直流电机按圈转；其主要控制物体的转动并保持（机器人关节、转向机构）。适用于位置角度经常变化的场合。舵机种类大致分为两种控制逻辑类型：180°舵机与360°舵机，其中控制逻辑的区别将在下文阐述。
舵机基础知识：
舵机又称伺服马达。标准的舵机有3条导线，分别是：电源线（红色）、地线 （黑色或棕色）、控制线（黄色）
通讯模式：
-
UART
- UART的连接方式简单，通常只需要两根线（TXD和RXD）即可实现通信。
- 它广泛应用于计算机和外部设备之间的通信，如串口打印机、模块通信等。
- 在舵机控制中，UART可以用于连接单个舵机或少量舵机，实现点对点的控制。
- UART的传输速率通常较低，取决于波特率的设置和硬件的限制。
- 由于其简单的通信协议和连接方式，UART在可靠性和稳定性方面可能不如CAN总线。
-
CAN总线
- CAN总线的连接方式相对复杂，但允许多个节点同时连接在同一总线上。
- 它适用于需要多设备间协同工作的场景，如汽车中的引擎控制单元（ECU）、传感器和执行器之间的通信。
- 在舵机控制中，CAN总线可以用于连接多个舵机，实现多舵机的协同控制和分布式网络通信。
问题：
如果采用CAN总线，每个舵机都需要一个32，但如稚晖君制作的dummy机械臂就是采用的can总线，但使用的不是舵机而是步进电机。
【自制】我造了一台钢铁侠的迷你机械臂 ！【硬核】_哔哩哔哩_bilibili
- CAN总线具有较高的传输速率和更强的抗干扰能力。
- 它采用了多种技术手段来保证数据及时、准确地传输，如使用差分信号进行通信、采用CRC循环冗余码进行纠错等。
- 因此，CAN总线在可靠性和稳定性方面优于UART。
数字舵机与模拟舵机：
- 模拟舵机：以模拟电路作为控制芯片，需要持续发送PWM信号来维持舵机的位置和速度。由于模拟电路的差异，即使型号相同的舵机在性能上也可能存在差异。
- 数字舵机：结构上与模拟舵机相似，但只需发送一次PWM信号即可保持规定的位置。数字舵机在位置准确度、线性度等方面优于模拟舵机。
数字舵机与模拟舵机_控制方法与性能比较_数码舵机和模拟舵机区别-CSDN博客
3分钟搞懂数字舵机与模拟舵机的区别！_模拟舵机和数字舵机有什么区别-CSDN博客
180度舵机：
180°舵机是一种常见的伺服电机，其工作原理基于脉宽调制（PWM）技术。通过改变PWM信号的高电平时间，可以控制舵机旋转的角度。具体来说，舵机的控制信号以固定频率的方波形式发送，这个方波的周期通常为20ms，即50Hz的频率。在控制信号中，脉冲的高电平时间决定舵机旋转的角度。例如，对于180°舵机，高电平脉宽（us）与角度的对应关系如下：
• 0°对应高电平脉宽为500us
• 45°对应高电平脉宽为1000us
• 90°对应高电平脉宽为1500us
• 135°对应高电平脉宽为2000us
• 180°对应高电平脉宽为2500us
PWM脉冲宽度与角度的关系
对于180°舵机，舵机的角度控制通过PWM信号的脉冲宽度来实现。每个PWM周期为20ms（50Hz），而脉冲宽度在一定范围内变化，通常是1ms到2ms之间。
- 1ms脉冲：对应舵机转至0°（最小角度）。
- 1.5ms脉冲：对应舵机转至90°（中立位置）。
- 2ms脉冲：对应舵机转至180°（最大角度）。
简单来说：
- 脉冲宽度 = 1ms → 0°
- 脉冲宽度 = 1.5ms → 90°
- 脉冲宽度 = 2ms → 180°
通过改变脉冲宽度，可以让舵机在**0°到180°**之间平滑调整到目标位置。
舵机内部通常包含一个电动机（通常是直流电机）和一个反馈控制系统。该系统主要由以下几个部分组成：
- 电动机：用于驱动舵机旋转。
- 位置传感器（如电位计）：用于检测舵机当前的旋转角度。
- 控制电路：将接收到的PWM信号与舵机的实际角度进行比较，确保舵机旋转至目标位置。
工作流程：
- 接收PWM信号：舵机接收到来自控制器的PWM信号，信号中的脉冲宽度决定目标角度。
- 比较目标角度和当前位置：舵机内部的控制电路会将接收到的目标角度与实际的当前角度进行比较。
- 调整电动机运动：如果当前位置与目标角度不一致，电动机会驱动舵机调整旋转角度。
- 反馈机制：舵机的电位计会将当前位置反馈给控制电路，直到舵机转动到目标角度为止。
- 停止旋转：一旦舵机达到了目标角度，电动机停止工作，舵机保持在目标位置。
不同舵机的最小脉宽与最大脉宽范围不同，具体要看数据手册
在180°舵机中旋转角度和脉冲宽度呈现出线性关系
逻辑代码：
#include "stm32f1xx_hal.h"
TIM_HandleTypeDef TIM_Handle;
// 初始化PWM以控制舵机
void PWM_Init(uint16_t psc, uint16_t arr) {
__HAL_RCC_TIM2_CLK_ENABLE(); // 使能TIM2时钟
__HAL_RCC_GPIOA_CLK_ENABLE(); // 使能GPIOA时钟
TIM_Handle.Instance = TIM2;
TIM_Handle.Init.Prescaler = psc;
TIM_Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
TIM_Handle.Init.Period = arr;
TIM_Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
TIM_Handle.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
HAL_TIM_PWM_Init(&TIM_Handle);
TIM_OC_InitTypeDef TIM_OC_InitStruct = {0};
TIM_OC_InitStruct.OCMode = TIM_OCMODE_PWM1;
TIM_OC_InitStruct.Pulse = 0;
TIM_OC_InitStruct.OCPolarity = TIM_OCPOLARITY_HIGH;
TIM_OC_InitStruct.OCFastMode = TIM_OCFAST_DISABLE;
HAL_TIM_PWM_ConfigChannel(&TIM_Handle, &TIM_OC_InitStruct, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&TIM_Handle, TIM_CHANNEL_1);
}
// 设置舵机角度（0-180度）
void Servo_SetAngle(TIM_HandleTypeDef *htim, TIM_Channel_TypeDef channel, float angle) {
uint16_t pulse_width = (uint16_t)((angle / 180.0f) * 2000 + 500); // 将角度转换为脉冲宽度
__HAL_TIM_SET_COMPARE(htim, channel, pulse_width);
}
int main(void) {
HAL_Init(); // 初始化HAL库
PWM_Init(72 - 1, 20000 - 1); // 初始化PWM，设置20ms周期
while (1) {
float angle = 90.0f; // 设定目标角度，例如90度
Servo_SetAngle(&TIM_Handle, TIM_CHANNEL_1, angle); // 设置舵机角度
HAL_Delay(1000); // 延时1秒
// 可以根据需要更改angle的值来设置不同的角度
}
}360度舵机：
360度舵机的PWM信号不再表示角度，而是表示速度和方向。具体来说，脉冲宽度的变化决定舵机旋转的方向和速度。
-
舵机转动方向:(具体请查阅数据手册)
- 脉冲宽度为1.5ms时，舵机停止旋转（通常为中立位置）。
- 脉冲宽度小于1.5ms时，舵机会向一个方向旋转（如顺时针）。
- 脉冲宽度大于1.5ms时，舵机会向另一个方向旋转（如逆时针）。
-
舵机转速：
- 脉冲宽度离1.5ms越远，舵机的转速越快。接近1.0ms时，舵机转速较快且向一个方向旋转，接近2.0ms时，舵机转速较快且向另一个方向旋转。
- 脉冲宽度的变化影响旋转的速度。小于1.5ms时，舵机会以一定速度顺时针旋转，越接近1.0ms，速度越快；大于1.5ms时，则为逆时针旋转，越接近2.0ms，速度越快。
360度舵机的控制信号的原理和常规舵机类似，但在控制过程中，我们关注的不是角度而是旋转速度和旋转方向：
- 脉冲宽度（Pulse Width）：舵机的输入控制信号是一个周期性的脉冲信号，通常为20ms的周期。
- 停止信号（1.5ms）：脉冲宽度为1.5ms时，舵机停止旋转。
- 旋转方向：当脉冲宽度小于1.5ms时，舵机会顺时针旋转；当脉冲宽度大于1.5ms时，舵机会逆时针旋转。
- 旋转速度：脉冲宽度距离1.5ms的偏差越大，舵机的旋转速度就越快。
开环控制代码：
这个代码并不能直接让舵机旋转到指定的绝对位置，因为缺乏位置反馈。
代码只是提供思路，但整体来说可行性不高
#include "stm32f1xx_hal.h"
TIM_HandleTypeDef TIM_Handle;
// 初始化PWM以控制360°舵机
void PWM_Init(void) {
__HAL_RCC_TIM3_CLK_ENABLE(); // 根据你的硬件配置选择正确的定时器
__HAL_RCC_GPIOB_CLK_ENABLE(); // 根据你的硬件配置选择正确的GPIO端口
TIM_Handle.Instance = TIM3;
TIM_Handle.Init.Prescaler = 71; // 假设系统时钟为72MHz，则PWM频率为1kHz
TIM_Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
TIM_Handle.Init.Period = 999; // PWM周期
TIM_Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
TIM_Handle.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
HAL_TIM_PWM_Init(&TIM_Handle);
TIM_OC_InitTypeDef TIM_OC_InitStruct = {0};
TIM_OC_InitStruct.OCMode = TIM_OCMODE_PWM1;
TIM_OC_InitStruct.Pulse = 0; // 初始脉冲宽度为0
TIM_OC_InitStruct.OCPolarity = TIM_OCPOLARITY_HIGH;
TIM_OC_InitStruct.OCFastMode = TIM_OCFAST_DISABLE;
HAL_TIM_PWM_ConfigChannel(&TIM_Handle, &TIM_OC_InitStruct, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&TIM_Handle, TIM_CHANNEL_1);
}
// 设置舵机速度（通过PWM占空比）和方向
void Servo_SetSpeedAndDirection(TIM_HandleTypeDef *htim, TIM_Channel_TypeDef channel, uint16_t speed, uint8_t direction) {
// speed参数应该是一个介于0到htim->Init.Period之间的值
// direction参数为0表示正向，1表示反向（可能需要通过反转PWM信号的极性来实现）
if (direction == 0) {
// 正向
__HAL_TIM_SET_COMPARE(htim, channel, speed);
} else {
// 反向（假设通过设置一个不同的PWM通道或反转极性来实现，这里简化处理）
// 注意：这个示例没有实现反向功能，因为具体的实现方式取决于你的硬件和舵机
// 你可能需要查阅舵机手册或硬件文档来了解如何反转舵机的旋转方向
// 一种可能的方法是使用另一个PWM通道，并通过软件逻辑来反转信号
}
}
// 注意：以下函数是一个伪函数，用于说明如何通过开环控制来尝试达到指定位置
// 在实际应用中，你需要根据舵机的特性和应用场景来实现这个函数
void Servo_MoveToPosition(TIM_HandleTypeDef *htim, TIM_Channel_TypeDef channel, int32_t target_position, uint16_t speed) {
// 由于缺乏位置反馈，这个函数无法直接实现
// 你可能需要根据舵机的特性和应用场景来编写一个更复杂的控制算法
// 例如，你可以通过发送一系列PWM信号来控制舵机的速度和方向，并估计其移动的距离
// 这通常需要对舵机的动态特性有深入的了解，并可能需要一些外部传感器或逻辑来辅助
// 伪代码示例：
// 1. 发送PWM信号以启动舵机
// 2. 根据目标位置和当前速度估计所需的时间
// 3. 在指定时间后停止PWM信号（这通常不准确，因为缺乏位置反馈）
// 4. （可选）使用外部传感器或逻辑来检测舵机的实际位置，并进行必要的调整
// 在这个简化的示例中，我们只是简单地启动舵机并设置一个速度
Servo_SetSpeedAndDirection(htim, channel, speed, 0); // 假设我们想要正向旋转
// ... 缺少实际的位置控制逻辑 ...
}
int main(void) {
HAL_Init(); // 初始化HAL库
PWM_Init(); // 初始化PWM
while (1) {
// 尝试让舵机旋转到一个指定位置（但实际上无法精确控制）
Servo_MoveToPosition(&TIM_Handle, TIM_CHANNEL_1, 1000, 500); // 假设目标位置是1000（无单位），速度是50%
HAL_Delay(5000); // 延时5秒（这个延时是任意的，因为没有位置反馈来确定何时停止）
// 注意：上面的函数Servo_MoveToPosition在这个示例中并没有实际实现位置控制
// 你需要根据你的应用场景和舵机的特性来编写实际的控制逻辑
}
}闭环控制代码：
有的舵机会有位置反馈，这种能闭环控制
代码并不适配这款舵机，只是提供解决思路
智能总线舵机 串口金属单双轴20KG高精度带反馈大扭力 机器人舵机-tmall.com天猫
#include "stm32f1xx_hal.h"
// 假设你已经有了编码器读取的函数
// int32_t Read_Encoder(void); // 返回编码器的当前位置（单位：脉冲数或步数）
TIM_HandleTypeDef TIM_PWM_Handle; // 用于PWM输出的定时器
TIM_HandleTypeDef TIM_Encoder_Handle; // 用于编码器读取的定时器（如果编码器连接在定时器输入上）
// PWM初始化函数（与之前的示例类似）
void PWM_Init(void) {
// ... 初始化PWM定时器、通道和参数 ...
}
// 编码器初始化函数
void Encoder_Init(void) {
// ... 初始化编码器连接的定时器为编码器接口模式，配置GPIO，启动中断等 ...
}
// 闭环位置控制函数
void Servo_ClosedLoopControl(int32_t target_position, uint16_t speed_pwm) {
int32_t current_position = 0;
int32_t error = 0;
int32_t previous_error = 0;
int32_t derivative = 0;
uint16_t pwm_output = 0;
while (1) { // 通常这里会有一个条件来退出循环，比如达到目标位置或超时
current_position = Read_Encoder(); // 读取当前位置
error = target_position - current_position; // 计算位置误差
derivative = error - previous_error; // 计算误差的导数（可选，用于PID控制）
// 这里可以使用简单的比例控制（P控制），或者更复杂的PID控制
pwm_output = (uint16_t)((speed_pwm * (int32_t)abs(error)) / MAX_POSITION_ERROR + BASE_PWM_VALUE);
// 注意：MAX_POSITION_ERROR和BASE_PWM_VALUE是需要根据实际情况设定的常量
// pwm_output应该被限制在PWM定时器的有效范围内
// 根据误差的方向设置PWM的方向（如果需要的话）
if (error > 0) {
// 正向旋转
__HAL_TIM_SET_COMPARE(&TIM_PWM_Handle, TIM_CHANNEL_1, pwm_output);
// 可能还需要设置其他GPIO或定时器来控制方向，具体取决于你的硬件
} else {
// 反向旋转
__HAL_TIM_SET_COMPARE(&TIM_PWM_Handle, TIM_CHANNEL_1, MAX_PWM_VALUE - pwm_output);
// 注意：这里假设MAX_PWM_VALUE是PWM定时器的最大值，且反向旋转是通过减小PWM占空比来实现的
// 这可能不适用于所有舵机，具体取决于其控制方式和硬件接口
}
// 更新前一次误差
previous_error = error;
// 添加延时或等待中断来减少控制循环的频率（根据实际需要）
HAL_Delay(CONTROL_LOOP_DELAY); // CONTROL_LOOP_DELAY是一个根据控制需求设定的延时
// 检查是否达到目标位置（可能需要一个容差范围来判断）
if (abs(error) <= POSITION_TOLERANCE) {
break; // 退出循环
}
}
// 停止PWM输出（可选）
__HAL_TIM_SET_COMPARE(&TIM_PWM_Handle, TIM_CHANNEL_1, 0);
}
int main(void) {
HAL_Init(); // 初始化HAL库
PWM_Init(); // 初始化PWM
Encoder_Init(); // 初始化编码器
// 设置目标位置和PWM速度
int32_t target_position = 1000; // 假设的目标位置
uint16_t speed_pwm = 500; // 假设的PWM速度值
// 执行闭环位置控制
Servo_ClosedLoopControl(target_position, speed_pwm);
while (1) {
// 主循环可以执行其他任务或保持空闲
}
}舵机的数据手册：
1. 工作电压（Operating Voltage）
- 定义：舵机正常工作的电压范围。通常在数据手册中会给出最小值和最大值。
- 重要性：选择正确的电压非常关键，过高或过低的电压都会影响舵机的性能或损坏舵机。常见的工作电压有4.8V、5V、6V、12V等。
2. 空载扭矩（No-load Torque）
- 定义：舵机在空载（没有任何负载）的情况下，能够输出的最大扭矩，通常以牛顿·米（N·m）或千克·厘米（kg·cm）为单位表示。
- 重要性：扭矩表示舵机旋转时能够克服的力矩。高扭矩意味着舵机可以驱动更重的负载。
3. 负载扭矩（Load Torque）
- 定义：舵机在负载情况下的最大输出扭矩，通常也是以N·m或kg·cm为单位。这个值在有一定负载的情况下进行测量。
- 重要性：了解舵机的负载能力，可以判断它是否能驱动特定的负载。在实际应用中，选择舵机时要确保其扭矩大于或等于所需的负载。
4. 响应时间（Response Time）
- 定义：舵机从接收到控制信号到执行到目标位置所需的时间，通常以毫秒（ms）为单位。
- 重要性：响应时间决定了舵机对控制信号的响应速度，影响系统的实时性和动态性能。
5. 最大转角（Rotation Range）
- 定义：舵机可以旋转的最大角度范围，通常为0°到180°，或者0°到360°（在360度舵机中）。有些舵机是全旋转型的，可以无限旋转。
- 重要性：了解舵机的旋转角度范围，有助于判断是否能满足特定的应用需求（如机器人关节、舵面控制等）。
6. 控制信号（Control Signal）
- 定义：控制舵机的PWM（脉宽调制）信号的频率和脉冲宽度。常见的舵机控制信号是每20ms一个周期，其中脉冲宽度决定舵机的角度。
- 重要性：确保控制系统能输出适当的PWM信号，以便舵机能够正确地响应并调整角度。
7. 工作频率（Operating Frequency）
- 定义：舵机控制信号的工作频率，通常是50Hz（每20ms周期）左右。
- 重要性：此频率与PWM信号的周期有关，影响舵机的控制精度和响应能力。
8. 工作电流（Operating Current）
- 定义：舵机在正常工作时的电流消耗，通常分为空载电流和负载电流。
- 重要性：电流的大小决定了舵机的电源要求，较大的电流消耗可能需要更高功率的电池或电源。
9. 最大电流（Peak Current）
- 定义：舵机在启动、负载或突发情况下的最大电流，通常是短时的最大电流值。
- 重要性：这个值用于评估舵机在极端工作条件下对电源的要求，防止电源供电不足或者电路损坏。
10. 空载转速（No-load Speed）
- 定义：舵机在空载状态下的最大转速，通常以°/秒（角度/秒）表示。
- 重要性：空载转速可以帮助判断舵机的响应速度。需要根据应用场合判断是否满足动态控制要求。
11. 负载转速（Load Speed）
- 定义：舵机在一定负载下的转速，通常以°/秒表示。
- 重要性：负载转速比空载转速更能反映舵机在实际应用中的表现，尤其是在负载较大的情况下，转速可能会降低。
12. 寿命（Life Expectancy）
- 定义：舵机的预期使用寿命，通常以操作次数或工作小时数表示。
- 重要性：了解舵机的耐用性，有助于评估其在长期工作条件下的可靠性。
13. 工作温度（Operating Temperature）
- 定义：舵机能够稳定工作的温度范围。
- 重要性：根据舵机的工作环境，选择适合的工作温度范围，防止过热或过冷影响舵机的性能和寿命。
14. 安装方式和尺寸（Mounting and Dimensions）
- 定义：舵机的安装尺寸、接口以及如何固定到其它机械结构上。
- 重要性：确保舵机与其他部件的匹配，以便能够正确地安装和使用。
15. 重量（Weight）
- 定义：舵机的重量。
- 重要性：对于重量敏感的应用，如飞行器、机器人等，选择合适的舵机重量至关重要。
16. 电机类型（Motor Type）
- 定义：舵机使用的电机类型，通常为直流电机（DC Motor）。
- 重要性：电机类型决定了舵机的性能、效率以及控制方式。
17. 反馈信号（Feedback Signal）
- 定义：某些舵机（如伺服舵机）具有反馈信号输出，用于返回当前角度或位置。
- 重要性：这种反馈对于精确控制位置和进行闭环控制非常重要。
18. 扭矩/速度曲线
- 定义：通常舵机会提供一组不同电压、负载条件下的扭矩和速度曲线，说明在不同负载和电压下舵机的表现。
- 重要性：有助于用户在设计时评估舵机是否满足预期的负载和速度要求
机械臂：
理论：
六轴机械臂之所以称为“六轴”，是因为它包含六个旋转关节，这些关节可以在三个平面上移动（上下、前后、左右），同时每个关节还能绕自身的轴线旋转（绕X轴、Y轴、Z轴）。这种设计使得机械臂能够在三维空间中自由移动、旋转和扭曲，实现复杂的动作和精确的位置控制。这些关节通常由电机和传动装置控制，能够以精确的速度和力度进行移动和操作。
- 逆向运动学：逆向运动学是根据目标物体的位置和方向确定机械臂各个关节的位置和角度的过程。这需要利用数学计算和几何模型来解决。
- 正向运动学：正向运动学则是根据机械臂各个关节的位置和角度确定机械臂的末端位置和方向的过程。这可以通过几何关系和矩阵变换来计算。
- 动力学分析：机械臂在工作过程中会受到各种力的作用，如重力、摩擦力等。动力学分析通过求解机械臂的运动微分方程，可以确定机械臂在不同工作状态下的运动特性和受力情况，从而对机械臂的运动进行优化和控制。
代码：
以下代码非纯控制代码，内含ROS，Openmv视觉定位（反馈，做闭环控制），树莓派做主控
完全代码在：没有备注/记录代码
智能家庭分类机器人
调试上位机可问所购买的商家提供或使用Qt编写：
视觉闭环条件：机械臂操作全程，摄像头能够识别与定位
姿态解算条件：在随着机械臂移动过程中，摄像头无法完全识别与定位物体
部分参数讲解(方便看代码):
Angle_Buf[n]是目标脉冲值
Anglen(n为数字)是当前脉冲值
因为底盘舵机0度和360度处不能连续转换，所以在安装时给底盘舵机的118.8度安装在正前方
Buf_X：机械臂末端左右距离缓冲区
Buf_Y：机械臂末端前后距离缓冲区
Buf_H: 机械臂末端高度距离缓冲区
Hight: 机械臂末端当前高度
Distance：机械臂末端当前前后距离
Aim_Hight: 机械臂末端目标高度
Aim_Y : 机械臂末端目标前后距离Error_Y; Y轴的误差
Error_X; X轴的误差
Error_H; H轴的误差Runing_Mod： 运行模式
t_Angle ： 计时参数
Mod_Time： 单模式运行时间
ID ：物品种类
JXB_Y : 机械臂末端前后距离初始化
JXB_X: 机械臂末端左右距离初始化
Aim_X: 机械臂末端前后距离目标值
控制速度与冲突部分解决思路：
为了实现舵机的缓慢旋转，我们并不直接将目标值赋于舵机，而是将舵机的控制值按一定的速率逐渐增减到目标值，通过改变该速率便可改变机械臂的动作速度。但由于程序运行较快，可能舵机可能还未达到上一个设定的目标值就被赋予了新的目标值，这就会引起错误，因此还需要加入舵机赋值的闭锁，在机械臂尚未达到上一个设定的目标值时，将不会运行与机械臂相关的程序。
使用TIM13定时器，在每个定时器中断向移动目标值一个阶段，比如目前13000，现在15000，每个定时器中断移动1000值的大小，以1000值的大小来控制速度，比如定时器中断周期为5ms，每5ms转动1°。在jixiebi()控制函数中有if（__!=__）用来决定是否当前舵机转向到目标角度，在未到达目标角度时，不进入jixiebi()后段函数，也就是不进行位置判断与其他机械臂运行程序，知道移动到指定为止。
Flag_Uart是上位机相关参数，用于检测机器人数据，本文不赘述（后面有时间将会讲述上位机是如何编写的）
在jixiebi()中:先做了判断是否到达目标角度：
if((Angle0!=Angle_Buf[0]||Angle1!=Angle_Buf[1]||Angle2!=Angle_Buf[2]||Angle3!=Angle_Buf[3]||\
Angle4!=Angle_Buf[4]||Angle5!=Angle_Buf[5])) //判断所有舵机是否转到目标角度
{
if(++Angle_Slow_t>=1)//用于控制缓速改变舵机角度的频率
{
Angle_Slow(); //若某一舵机未转到目标角度，则缓速转动函数
Angle_Slow_t=0;
}
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if (htim->Instance == TIM13)
{
jixiebi();
Control_Sover();
if(Flag_Uart)
if(++val>=15)
{
val=0;
Flag_Uart=0;
USART2_RX_STA|=1<<15;
}
}
......
}视觉闭环部分代码与思路 [闭环控制] （只包含控制部分代码讲解不包含是视觉部分与数据传输方面的分析与讲解)与姿态分析相关代码与思路 （如何建立的直角坐标系与定位，不包含数据传输方面的分析与讲解）：
设计闭环控制环节前，首先分析机械臂的机械结构如图所示，根据各个臂之间的安装角度和舵机安装角度可以得到各个臂之间的实际角度和舵机角度的关系。即：
由于机器人主要整理堆放在地面上的杂物，因此我们将机械臂末端设计为垂直于地面，为满足此条件，机械臂小小臂舵机角度应满足
以底盘舵机的中心为原点，以机械臂前展方向为Y轴，竖直向上为Z轴建立空间坐标系，结合各个臂的长度即各个臂间夹角与舵机角度的关系，可以得到爪子末端垂直于地面时，爪子末端的（X,Y,Z）坐标与各个舵机角度的关系如下：
对以上公式分析发现，机械臂前后距离Y有大臂舵机主导，上下距离由小臂舵机主导，左右距离由底盘舵机主导。由此可以分别以目标X、Y、Z坐标为输入量，利用上述公式计算出机械臂末端当前的X、Y、Z坐标，相应的坐标值之差为误差量，大臂舵机角度、小臂舵机角度、底盘舵机角度的修正值为输出量，做增量式PID进行机械臂爪子末端位置的闭环控制，设置合适的PID参数，将输出量加在舵机当前的控制值上，便可以控制机械臂末端移动到设定的现实坐标处
保持摄像头与初始状态方向一致，手腕舵机与底盘舵机做绑定:
case 2: //模式2：用于在识别到物品后调节机械臂，使摄像头对准物品（后放弃了此方案）
if(++t_Angle>=5)
{
t_Angle=0;
Angle0+=Get_Angle0(); //增量式PID调节底盘，调节机械臂末端X坐标
Angle_Buf[0]=Angle0; //同步底盘舵机的缓冲区
Angle4=2570000-2*Angle0; //调节手腕，保证爪子与Y轴平行
Angle_Buf[4]=Angle4; //同步手腕舵机的缓冲区
Angle1+=Get_Angle1(); //增量式PID调节大臂，调节机械臂末端Y坐标
Angle_Buf[1]=Angle1; //同步大臂舵机的缓冲区
Angle2+=Get_Angle2(); //增量式PID调节小臂，调节机械臂末端Z坐标
Angle_Buf[2]=Angle2; //同步小臂舵机的缓冲区
Angle3=Angle_Buf[1]+Angle_Buf[2]-310444; //增量式PID调节小小臂，以确保爪子垂直地面
Angle_Buf[3]=Angle3; //同步小小臂舵机的缓冲区
if(Error_X>-10 && Error_X<10)//可省略
{
C_x=0;
C_y=0;
HAL_NVIC_EnableIRQ(USART3_IRQn); //打开串口3（与摄像头通讯）
if(++Mod_Time>20)
{
Mod_Time=0;
Send_Flag=1;//向摄像头发送数据
if(Cam_Mod==2)
Runing_Mod=3;
}
}
else
{
Mod_Time=0;
}
}
break;智能抓取的方案思路：
Angle4=2570000-2*Angle0; //调节手腕，保证爪子与Y轴平行
在物品识别中，可以获取被抓物品被抓取位置在摄像头画面中的坐标，当摄像头安装位置固定后，便可以将被抓取位置在摄像头画面中的坐标转换为现实坐标系下坐标。具体方法如下：
摄像头的像素是240*240，距离地面12cm时，240像素对应现实中的长度为16.5cm，且像素与现实中的长度之间为线性关系，因此一个像素在现实中对应的长度为16.5/240=0.06875cm，机械臂初始前后距离为16.5cm，初始左右距离为0，这意味着摄像头画面中心坐标（120，120）对应着现实坐标系的（0，16.5）,
故摄像头坐标(x,y)转换为(X,Y)的公式如下：
左右距离X=(x-120)/240.0*1650 （0.1mm）
前后距离Y=1650+(120-y)/240.0*1650 （0.1mm）
当STM32收到OpenMV发来的偏转角和坐标，先根据偏转角转动手腕，使爪子与被抓物品平行，再按上述坐标转换公式将目标物品中心坐标(x,y)转换为现实坐标系中的坐标(X,Y)，并将其作为目标值，通过上述PID闭环控制，控制机械臂爪子在地面上方12cm处对准被抓取位置，而后再将目标的Z设为0，机械臂末端便伸向地面，最后控制爪子闭合，完成物品的抓取。
舵机角度与关节角度的转化，以及建立直角坐标系代码：
第247行：Buf_Y=2150+(100-C_y)/200.0*1400; //根据摄像头数据，进行坐标系转换，设置机械臂末端的目标Y坐标
第310行：Buf_X=(C_x-100)/200.0*1400-100; //根据摄像头数据，进行坐标系转换，设置机械臂末端的目标X坐标
第696行：JXB_X=JXB_Y*sin((double)(2*(Get_Angle(Angle0)-118.8)/180*3.14159));
第719行：Hight=(int)Arm*cos((double)((Angle1_Fixed-Get_Angle(Angle1))/180*3.14159))+(int)Forearm*cos((double)((243-Get_Angle(Angle1)-Get_Angle(Angle2))/180*3.14159))-Fforearm+Car_Hight;
模式思路的讲法：
arm.h:
#ifndef __ROBOTIC_ARM_H_
#define __ROBOTIC_ARM_H_
#include "main.h"
//机械臂参数
#define Arm 1310 //大臂长度
#define Forearm 912 //小臂长度
#define Fforearm 1650 //小小臂长度
#define Car_Hight 1350 //小车高度（底盘到地面的距离）
#define Servo_Lenght 380 //底盘舵机长度
//六舵机初始值
#define Angle_Dat0 910000 //底盘舵机
#define Angle_Dat1 602108 //大臂舵机
#define Angle_Dat2 818094 //小臂舵机
#define Angle_Dat3 1100000 //小小臂舵机
#define Angle_Dat4 750000 //手腕舵机
#define Angle_Dat5 430000 //爪子舵机
#define Angle1_Fixed 86.4
#define Aim_Length_Dat 10 //摄像头到标签的目标距离
#define Distance_Dat 1500 //机械臂的前展距离
void jixiebi(void);
void Control_Sover(void);
typedef struct
{
int Angle;
uint8_t ID;
uint8_t Number;
uint8_t Aim;
} Books_TypeDef;
extern Books_TypeDef Books[4];
extern unsigned char Tx_Str[5];
extern uint8_t C_x;
extern uint8_t C_y;
extern double Length1;
extern uint8_t ID;
extern uint8_t Send_Flag;
extern uint8_t GenSui;
extern uint8_t Book;
extern uint8_t Shelf_X;
extern uint8_t Mod14;
extern int Angle0;
extern int Angle1;
extern int Angle2;
extern int Angle3;
extern int Angle4;
extern int Angle5;
extern int Angle_Buf[6];
extern uint8_t Flag_NoFound;
extern int Angle5_0;
extern uint16_t ADC_Value;
extern uint8_t Runing_Mod;
extern float Aim_Length;
extern uint8_t Book_Flag;
extern uint8_t jxb_start;
extern uint8_t GenSui;
extern uint32_t X_C;
extern uint32_t Y_C;
extern int Distance;
extern float Aim_Hight;
extern float Hight;
extern float JXB_Y;
extern float Aim_Y;
extern float JXB_X;
extern float Aim_X;
extern float Buf_X;
extern float Buf_Y;
extern float Buf_H;
extern int16_t Turn_Angle4;
extern uint8_t Solve_Flag;
extern uint8_t Cam_Mod;
extern int Error_Y;
extern int Error_X;
extern int Error_H;
extern int dis_x;
extern int dis_y;
extern float cam_disx;
extern float cam_disy;
float Get_Angle(uint32_t Ang);
int Get_Angle2(void);
int Get_Angle1(void);
int Get_Angle0(void);
#endif
arm.c
#include "Robotic_Arm.h"
#include "leidar.h"
#include "math.h"
#include "control.h"
#include "dcmotor.h"
void PaiXv(void);
int Get_Angle0(void);
void Angle_Slow();
void Get_Distance(void);
void Up_Down(void);
void Finding(void);
int Limite_Range(int Input,int Max,int Min);
int Get_Pulse(float angl);
//==========================机械臂相关参数==============================
char Servo=0; //舵机选择参数，范围0~5，分别对应六个舵机
//角度初始化
int Angle=750000;
int Angle0=Angle_Dat0;
int Angle1=Angle_Dat1;
int Angle2=Angle_Dat2;
int Angle3=Angle_Dat3;
int Angle4=Angle_Dat4;
int Angle5=Angle_Dat5;
int Angle_Buf[6]={Angle_Dat0,Angle_Dat1,Angle_Dat2,Angle_Dat3,Angle_Dat4,Angle_Dat5}; //机械臂缓慢移动控制参数
//空间坐标闭环控制参数
float Angle0_KI=20,Angle0_KP=10,Angle0_KD=10; //左右移动闭环控制参数
float Angle2_KI=25,Angle2_KP=15,Angle2_KD=35; //上下移动闭环控制参数
float Angle1_KI=29,Angle1_KP=11.5,Angle1_KD=40; //前后移动闭环控制参数
//末端视觉闭环
float Hight=1200; //机械臂末端当前高度
int Distance=1500; //机械臂末端当前前后距离
float Aim_Hight=1200; //机械臂末端目标高度
float Aim_Y=1650; //机械臂末端目标前后距离
double Length1=10;
//空间坐标闭环相关参数
float JXB_Y=0; //机械臂末端前后距离初始化
float JXB_X=0; //机械臂末端左右距离初始化
float Aim_X=0; //机械臂末端前后距离目标值
float Buf_X=0; //机械臂末端左右距离缓冲区（缓冲区用于逐渐赋值，以保证机械臂移动的稳定性）
float Buf_Y=1650; //机械臂末端前后距离缓冲区
float Buf_H=1200; //机械臂末端高度距离缓冲区
int Error_Y=0; //X、Y、H三轴的误差
int Error_X=0;
int Error_H=0;
//==========================书本相关参数==============================
unsigned char Tx_Str[5]={0xC2,0xCC,0xff,0xff,0x5b}; //要发送的数据
unsigned char Rx_Dat[1]={0}; //接收数据缓存区
uint8_t C_x=0,C_y=0; //摄像头接收到的标签中心坐标
uint32_t X_C=135,Y_C=110; //标签中心在摄像头中的目标坐标
uint8_t Flag_NoFound=1; //未发现目标标志位，1为未发现目标，0为发现目标
char Flag_LCD_Dis=1; //角度更新标志位，为1则更新数据和舵机控制面板背景，为0则只更新舵机数据(防止显示混乱)
uint8_t ID=0; //识别到的Ariltag的ID
uint8_t Send_Flag=0; //发送标志位（通过串口向摄像头发送相关数据，为1发送一次，为0不发送）
uint16_t ADC_Value=0; //ADC读取到的数据
float Aim_Length=8; //目标距离
uint8_t Solve_Flag=0;
uint8_t NumOfBooks=4; //计划运输的书本数量
Books_TypeDef Books[4]; //书的信息存储区 第一列：对应底座舵机角度 第二列：书本二维码信息 第三列：书架号 第四列：目标坐标
uint8_t Book=0; //第Book本书
uint8_t Find=0; //允许寻找书的标志位
uint8_t Runing_Mod=13; //系统运行模式 初始模式为13
uint32_t Mod_Time=0; //单模式运行时间
uint8_t Angle_Slow_t=0; //机械臂角度赋值减速参数
uint8_t Last_Mod=0; //
uint8_t Book_Flag=1; //
uint8_t Acce_Flag=1;
uint8_t t_Angle=0; //计时参数
uint8_t Shelf_X=0; //书架中心坐标
int16_t Turn_Angle4=0; //物品偏转角
uint8_t GenSui=0; //跟随标志位
uint8_t jxb_start=1; //机械臂运行标志位，为1正常执行运行模式
uint8_t Cam_Mod=0;
uint8_t First=0;
int dis_x=0;
int dis_y=0;
float cam_disx=0;
float cam_disy=0;
//函数名：机械臂控制函数
//入口参数：无
//返回值：无
//函数功能：控制机械臂
// 1.按照运行模式依次运行，当机械臂实际角度与目标值不同或jxb_start为0时，则停止模式运行
// 2.当机械臂实际角度与目标值不同时，缓速控制舵机角度变换，直到两者相等
// 3.该函数是TIM13产生的1ms中断(?)
//
void jixiebi()
{
if((Angle0!=Angle_Buf[0]||Angle1!=Angle_Buf[1]||Angle2!=Angle_Buf[2]||Angle3!=Angle_Buf[3]||\
Angle4!=Angle_Buf[4]||Angle5!=Angle_Buf[5])) //判断所有舵机是否转到目标角度
{
if(++Angle_Slow_t>=1)//用于控制缓速改变舵机角度的频率
{
Angle_Slow(); //若某一舵机未转到目标角度，则缓速转动函数
Angle_Slow_t=0;
}
}
else if(jxb_start) //所有舵机都转到目标角度
{
switch(Runing_Mod)
{
case 0: //模式0：机械臂初始化，重置摄像头
if(Mod_Time==0)
{
Buf_H=1200; //机械臂姿态初始化
Buf_X=0;
Buf_Y=1650;
set_target=0; //设定小车的目标位置
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_5, GPIO_PIN_RESET);//重置摄像头
HAL_NVIC_EnableIRQ(USART3_IRQn);
}
else
{
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_5, GPIO_PIN_SET);
}
if(++Mod_Time>=600) //延时，确保摄像头成功重置
{
Mod_Time=0;
Runing_Mod=1;
Send_Flag=1; //向摄像头发送数据
HAL_NVIC_DisableIRQ(UART5_IRQn);//关闭与树莓派的通讯
HAL_TIM_Base_Stop_IT(&htim14);
set_target=0;
__HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_1, 0);//初始化编码器数据
__HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_2, 0);
__HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_3, 0);
__HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_4, 0);
}
break;
case 1: //模式1：识别物品
if(C_x && C_y) //识别到物品
{
Angle_Buf[5]=Angle_Dat5;//松开爪子
t_Angle=0;
Runing_Mod=2;
Send_Flag=1; //向摄像头发送数据
}
if(++Mod_Time>=2000)//超时后返回模式0重置摄像头
{
Mod_Time=0;
Runing_Mod=0;
t_Angle=0;
}
break;
case 2: //模式2：用于在识别到物品后调节机械臂，使摄像头对准物品（后放弃了此方案）
if(++t_Angle>=5)
{
t_Angle=0;
Angle0+=Get_Angle0(); //增量式PID调节底盘，调节机械臂末端X坐标
Angle_Buf[0]=Angle0; //同步底盘舵机的缓冲区
Angle4=2570000-2*Angle0; //调节手腕，保证爪子与Y轴平行
Angle_Buf[4]=Angle4; //同步手腕舵机的缓冲区
Angle1+=Get_Angle1(); //增量式PID调节大臂，调节机械臂末端Y坐标
Angle_Buf[1]=Angle1; //同步大臂舵机的缓冲区
Angle2+=Get_Angle2(); //增量式PID调节小臂，调节机械臂末端Z坐标
Angle_Buf[2]=Angle2; //同步小臂舵机的缓冲区
Angle3=Angle_Buf[1]+Angle_Buf[2]-310444; //增量式PID调节小小臂，以确保爪子垂直地面
Angle_Buf[3]=Angle3; //同步小小臂舵机的缓冲区
if(Error_X>-10 && Error_X<10)//可省略
{
C_x=0;
C_y=0;
HAL_NVIC_EnableIRQ(USART3_IRQn); //打开串口3（与摄像头通讯）
if(++Mod_Time>20)
{
Mod_Time=0;
Send_Flag=1;//向摄像头发送数据
if(Cam_Mod==2)
Runing_Mod=3;
}
}
else
{
Mod_Time=0;
}
}
break;
case 3: //模式3：识别到物品的抓取位置后，设置新的目标高度坐标H和前后坐标Y。
if(++t_Angle>=5)
{
t_Angle=0;
Angle0+=Get_Angle0();
Angle_Buf[0]=Angle0;
Angle4=2570000-2*Angle0;
Angle_Buf[4]=Angle4;
Angle1+=Get_Angle1();
Angle_Buf[1]=Angle1;
Angle2+=Get_Angle2();
Angle_Buf[2]=Angle2;
Angle3=Angle_Buf[1]+Angle_Buf[2]-310444;
Angle_Buf[3]=Angle3;
}
if(C_x && C_y) //识别到物品的抓取位置
{
HAL_NVIC_DisableIRQ(USART3_IRQn); //关闭串口3，防止摄像头误识别
t_Angle=0;
Runing_Mod=4;
if(ID==3) //根据物品种类调节爪子下降不同的高度
Buf_H=800;
else
Buf_H=500;
Buf_Y=2150+(100-C_y)/200.0*1400; //根据摄像头数据，进行坐标系转换，设置机械臂末端的目标Y坐标
Send_Flag=1; //向摄像头发送数据
}
if(++Mod_Time>=2000)//超时后返回模式0重置摄像头
{
Mod_Time=0;
Runing_Mod=0;
t_Angle=0;
}
break;
case 4: //模式4：先使爪子下降一点距离，并在Y轴（前后方向）对准抓取位置(以防止机械臂姿态超过正常范围)
if(++t_Angle>=5)
{
t_Angle=0;
Angle0+=Get_Angle0();
Angle_Buf[0]=Angle0;
Angle4=2570000-2*Angle0-Turn_Angle4*100/18*1000;//根据摄像头数据，调节手腕角度，确保爪子与物品平行
Angle_Buf[4]=Angle4; //同步手腕舵机的缓冲区
Angle1+=Get_Angle1();
Angle_Buf[1]=Angle1;
Angle2+=Get_Angle2();
Angle_Buf[2]=Angle2;
Angle3=Angle_Buf[1]+Angle_Buf[2]-310444;
Angle_Buf[3]=Angle3;
if(Error_H>-5 && Error_H<5) //判断调节是否到位
{
if(++Mod_Time>20) //调节到位后，延时确保稳定
{
Runing_Mod=5;
Mod_Time=0;
}
}
else
{
Mod_Time=0;
}
}
break;
case 5: //模式5：设置新的目标左右坐标X。
if(++t_Angle>=5)
{
t_Angle=0;
Angle0+=Get_Angle0();
Angle_Buf[0]=Angle0;
Angle4=2570000-2*Angle0-Turn_Angle4*100/18*1000;
Angle_Buf[4]=Angle4;
Angle1+=Get_Angle1();
Angle_Buf[1]=Angle1;
Angle2+=Get_Angle2();
Angle_Buf[2]=Angle2;
Angle3=Angle_Buf[1]+Angle_Buf[2]-310444;
Angle_Buf[3]=Angle3;
if(++Mod_Time>20)
{
Mod_Time=0;
Buf_X=(C_x-100)/200.0*1400-100; //根据摄像头数据，进行坐标系转换，设置机械臂末端的目标X坐标
Runing_Mod=6;
C_x=0;
C_y=0;
}
}
break;
case 6: //模式6：在X轴（左右方向）对准抓取位置，然后再次设置新的目标高度坐标H，以便抓取
if(++t_Angle>=5)
{
t_Angle=0;
Angle0+=Get_Angle0();
Angle_Buf[0]=Angle0;
Angle4=2570000-2*Angle0-Turn_Angle4*100/18*1000;
Angle_Buf[4]=Angle4;
Angle1+=Get_Angle1();
Angle_Buf[1]=Angle1;
Angle2+=Get_Angle2();
Angle_Buf[2]=Angle2;
Angle3=Angle_Buf[1]+Angle_Buf[2]-310444;
Angle_Buf[3]=Angle3;
if(Error_X>-20 && Error_X<20) //确保爪子对准物品
{
switch(ID)
{
case 1:
case 5:
case 6:
case 9:
case 2:
case 4:
case 8:
case 7: Buf_H=50; break;
case 3: Buf_H=250; break;
}
if(++Mod_Time>20)
{
Runing_Mod=7;
Mod_Time=0;
}
}
else
{
Mod_Time=0;
}
}
break;
case 7: //模式7：调节爪子下降到指定位置
if(++t_Angle>=5)
{
t_Angle=0;
Angle0+=Get_Angle0();
Angle_Buf[0]=Angle0;
Angle4=2570000-2*Angle0-Turn_Angle4*100/18*1000;
Angle_Buf[4]=Angle4;
Angle1+=Get_Angle1();
Angle_Buf[1]=Angle1;
Angle2+=Get_Angle2();
Angle_Buf[2]=Angle2;
Angle3=Angle_Buf[1]+Angle_Buf[2]-310444;
Angle_Buf[3]=Angle3;
if(Error_H>-20 && Error_H<20) //确保爪子到达指定位置
{
if(++Mod_Time>=20) //延时确保机械臂稳定
{
switch(ID) //根据物品种类使爪子加紧相应物品
{
case 1: Angle_Buf[5]=560000; break;
case 2: Angle_Buf[5]=820000; break;
case 3: Angle_Buf[5]=900000; break;
case 4: Angle_Buf[5]=900000; break;
case 5: Angle_Buf[5]=830000; break;
case 6: Angle_Buf[5]=570000; break;
case 7: Angle_Buf[5]=1080000; break;
case 8: Angle_Buf[5]=800000; break;
case 9: Angle_Buf[5]=830000; break;
}
Mod_Time=0;
Runing_Mod=8;
}
}
}
break;
case 8: //模式8：设置新的目标高度坐标H、前后坐标Y、左右坐标X。
if(++t_Angle>=50)
{
t_Angle=0;
Buf_H=1200;
Buf_X=0;
Buf_Y=1650;
Runing_Mod=9;
HAL_NVIC_EnableIRQ(UART5_IRQn);
HAL_TIM_Base_Start_IT(&htim14);
}
break;
case 9: //模式9：根据目标值调节机械臂，使其抬起复位
if(++t_Angle>=5)
{
t_Angle=0;
Angle0+=Get_Angle0();
Angle_Buf[0]=Angle0;
Angle4=2570000-2*Angle0-Turn_Angle4*100/18*1000;
Angle_Buf[4]=Angle4;
Angle1+=Get_Angle1();
Angle_Buf[1]=Angle1;
Angle2+=Get_Angle2();
Angle_Buf[2]=Angle2;
Angle3=Angle_Buf[1]+Angle_Buf[2]-310444;
Angle_Buf[3]=Angle3;
if(Error_X>-20 && Error_X<20 && Error_H>-20 && Error_H<20) //确保机械臂调整到位
{
if(++Mod_Time>30)
{
Angle_Buf[1]=800000; //再次抬起机械臂，以防物品遮挡激光雷达
Mod_Time=0;
Runing_Mod=18; //转至control.c文件页，转向并导航至指定位置
}
}
else
{
Mod_Time=0;
}
}
break;
case 10: //模式10：设置新的目标高度坐标H、前后坐标Y，便于放置物品。
Buf_H=1300;
Buf_Y=1850;
Send_Flag=1; //向摄像头发送数据，更新相应状态
Runing_Mod=11;
break;
case 11: //模式11：根据目标值调节机械臂，并放置物品。
if(++t_Angle>=5)
{
t_Angle=0;
Angle0+=Get_Angle0();
Angle_Buf[0]=Angle0;
Angle4=2570000-2*Angle0;//-Turn_Angle4*100/18*1000;
Angle_Buf[4]=Angle4;
Angle1+=Get_Angle1();
Angle_Buf[1]=Angle1;
Angle2+=Get_Angle2();
Angle_Buf[2]=Angle2;
Angle3=Angle_Buf[1]+Angle_Buf[2]-310444;
Angle_Buf[3]=Angle3;
if(Error_Y>-20 && Error_Y<20) //确保机械臂调整到位
{
Angle_Buf[5]=Angle_Dat5; //松开机械爪
Runing_Mod=12;
Buf_H=1200; //设置新的目标高度坐标H、前后坐标Y，将机械臂复位
Buf_Y=1650;
Turn_Angle4=0; //重置手腕角度控制参数
}
}
break;
case 12: //模式12：放置完毕，根据目标值调节机械臂，复位机械臂。
if(++t_Angle>=5)
{
t_Angle=0;
Angle0+=Get_Angle0();
Angle_Buf[0]=Angle0;
Angle4=2570000-2*Angle0;
Angle_Buf[4]=Angle4;
Angle1+=Get_Angle1();
Angle_Buf[1]=Angle1;
Angle2+=Get_Angle2();
Angle_Buf[2]=Angle2;
Angle3=Angle_Buf[1]+Angle_Buf[2]-310444;
Angle_Buf[3]=Angle3;
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_5, GPIO_PIN_RESET); //重置摄像头
if(Error_H>-20 && Error_H<20) //确保机械臂调整到位
{
Runing_Mod=16; //转至control.c文件页
HAL_NVIC_EnableIRQ(UART5_IRQn);
HAL_TIM_Base_Start_IT(&htim14);
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_5, GPIO_PIN_SET); //重置摄像头
HAL_NVIC_EnableIRQ(USART3_IRQn);
}
}
break;
case 13: //模式13：空模式，重置摄像头，关闭机械臂中断
if(++t_Angle>=5)
{
t_Angle=0;
Angle0+=Get_Angle0();
Angle_Buf[0]=Angle0;
Angle4=2570000-2*Angle0;
Angle_Buf[4]=Angle4;
Angle1+=Get_Angle1();
Angle_Buf[1]=Angle1;
Angle2+=Get_Angle2();
Angle_Buf[2]=Angle2;
Angle3=Angle_Buf[1]+Angle_Buf[2]-310444;
Angle_Buf[3]=Angle3;
Send_Flag=1;
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_5, GPIO_PIN_SET);
HAL_TIM_Base_Stop_IT(&htim13);
}
break;
case 14: //在Y轴方向调节小车位置
int temp1=(abs(gtim_get_encode(1))+abs(gtim_get_encode(2))+abs(gtim_get_encode(3))+abs(gtim_get_encode(4)))*75*3.1415/4/60/52;
if(cam_disy>0) //正向调整
{
vy=20;
if(((temp1-dis_y)<=((cam_disy)+10))&&((temp1-dis_y)>=(cam_disy-10))) //确保小车调整到位
{
vx=0;
vy=0;
wz=0;
//==========重置编码器，确保精确调整小车位置================
g_timx_encode_count1=0;
g_timx_encode_count2=0;
g_timx_encode_count3=0;
g_timx_encode_count4=0;
__HAL_TIM_SET_COUNTER(&htim2,0);
__HAL_TIM_SET_COUNTER(&htim3,0);
__HAL_TIM_SET_COUNTER(&htim4,0);
__HAL_TIM_SET_COUNTER(&htim1,0);
MX_TIM2_Init();
MX_TIM3_Init();
MX_TIM4_Init();
MX_TIM1_Init();
HAL_TIM_Encoder_Start(&htim2,TIM_CHANNEL_1 | TIM_CHANNEL_2);
HAL_TIM_Encoder_Start(&htim3,TIM_CHANNEL_1 | TIM_CHANNEL_2);
HAL_TIM_Encoder_Start(&htim4,TIM_CHANNEL_1 | TIM_CHANNEL_2);
HAL_TIM_Encoder_Start(&htim1,TIM_CHANNEL_1 | TIM_CHANNEL_2);
__HAL_TIM_ENABLE_IT(&htim2,TIM_IT_UPDATE); /* 使能更新中断 */
__HAL_TIM_CLEAR_FLAG(&htim2,TIM_IT_UPDATE); /* 清除更新中断标志位 */
__HAL_TIM_ENABLE_IT(&htim3,TIM_IT_UPDATE); /* 使能更新中断 */
__HAL_TIM_CLEAR_FLAG(&htim3,TIM_IT_UPDATE); /* 清除更新中断标志位 */
__HAL_TIM_ENABLE_IT(&htim4,TIM_IT_UPDATE); /* 使能更新中断 */
__HAL_TIM_CLEAR_FLAG(&htim4,TIM_IT_UPDATE); /* 清除更新中断标志位 */
__HAL_TIM_ENABLE_IT(&htim1,TIM_IT_UPDATE); /* 使能更新中断 */
__HAL_TIM_CLEAR_FLAG(&htim1,TIM_IT_UPDATE); /* 清除更新中断标志位 */
dis_x=(abs(gtim_get_encode(1))+abs(gtim_get_encode(2))+abs(gtim_get_encode(3))+abs(gtim_get_encode(4)))*75*3.1415/4/60/52;
Runing_Mod=15;
}
}
else //反向调整
{
vy=-20;
if(((dis_y-temp1)<=((cam_disy)+10))&&((dis_y-temp1)>=(cam_disy-10))) //确保小车调整到位
{
vx=0;
vy=0;
wz=0;
//==========重置编码器，确保精确调整小车位置================
g_timx_encode_count1=0;
g_timx_encode_count2=0;
g_timx_encode_count3=0;
g_timx_encode_count4=0;
__HAL_TIM_SET_COUNTER(&htim2,0);
__HAL_TIM_SET_COUNTER(&htim3,0);
__HAL_TIM_SET_COUNTER(&htim4,0);
__HAL_TIM_SET_COUNTER(&htim1,0);
MX_TIM2_Init();
MX_TIM3_Init();
MX_TIM4_Init();
MX_TIM1_Init();
HAL_TIM_Encoder_Start(&htim2,TIM_CHANNEL_1 | TIM_CHANNEL_2);
HAL_TIM_Encoder_Start(&htim3,TIM_CHANNEL_1 | TIM_CHANNEL_2);
HAL_TIM_Encoder_Start(&htim4,TIM_CHANNEL_1 | TIM_CHANNEL_2);
HAL_TIM_Encoder_Start(&htim1,TIM_CHANNEL_1 | TIM_CHANNEL_2);
__HAL_TIM_ENABLE_IT(&htim2,TIM_IT_UPDATE); /* 使能更新中断 */
__HAL_TIM_CLEAR_FLAG(&htim2,TIM_IT_UPDATE); /* 清除更新中断标志位 */
__HAL_TIM_ENABLE_IT(&htim3,TIM_IT_UPDATE); /* 使能更新中断 */
__HAL_TIM_CLEAR_FLAG(&htim3,TIM_IT_UPDATE); /* 清除更新中断标志位 */
__HAL_TIM_ENABLE_IT(&htim4,TIM_IT_UPDATE); /* 使能更新中断 */
__HAL_TIM_CLEAR_FLAG(&htim4,TIM_IT_UPDATE); /* 清除更新中断标志位 */
__HAL_TIM_ENABLE_IT(&htim1,TIM_IT_UPDATE); /* 使能更新中断 */
__HAL_TIM_CLEAR_FLAG(&htim1,TIM_IT_UPDATE); /* 清除更新中断标志位 */
dis_x=(abs(gtim_get_encode(1))+abs(gtim_get_encode(2))+abs(gtim_get_encode(3))+abs(gtim_get_encode(4)))*75*3.1415/4/60/52/4;
Runing_Mod=15;
}
}
break;
case 15: //在X轴方向调节小车位置
int temp2=(abs(gtim_get_encode(1))+abs(gtim_get_encode(2))+abs(gtim_get_encode(3))+abs(gtim_get_encode(4)))*75*3.1415/4/60/52;
if(cam_disx>0) //正向调整
{
vx=20;
if(((temp2-dis_x)<=((cam_disx)+10))&&((temp2-dis_x)>=(cam_disx-10))) //确保小车调整到位
{
vx=0;
vy=0;
wz=0;
Runing_Mod=0;
}
}
else //反向调整
{
vx=-20;
if(((dis_x-temp2)<=((cam_disx)+10))&&((dis_x-temp2)>=(cam_disx-10))) //确保小车调整到位
{
vx=0;
vy=0;
wz=0;
Runing_Mod=0;
}
}
break;
case 16:
break;
case 17:
break;
}
}
}
//函数名：获取角度函数
//入口参数：无符整型舵机控制脉冲宽度，范围：250~1250
//返回值：浮点型舵机角度，范围：0~180
//函数功能：将入口参数的舵机控制脉冲宽度转换为舵机对应的角度
float Get_Angle(uint32_t Ang)
{
float angl=0;
angl=(Ang/1000-250)/1000.0*180;
return angl;
}
//函数名：舵机控制函数
//入口参数：无
//返回值：无
//函数功能：给舵机限幅并赋值
void Control_Sover(void)
{
Angle_Buf[0]=Limite_Range(Angle_Buf[0],1250000,250000);
Angle_Buf[1]=Limite_Range(Angle_Buf[1],1250000,250000);
Angle_Buf[2]=Limite_Range(Angle_Buf[2],1250000,250000);
Angle_Buf[3]=Limite_Range(Angle_Buf[3],1250000,250000);
Angle_Buf[4]=Limite_Range(Angle_Buf[4],1250000,250000);
Angle_Buf[5]=Limite_Range(Angle_Buf[5],1250000,250000);
//根据限幅后的舵机角度存储值，更新舵机角度控制值
Angle0=Limite_Range(Angle0,1250000,250000);
__HAL_TIM_SET_COMPARE(&htim5,TIM_CHANNEL_3,Angle0/1000);
Angle1=Limite_Range(Angle1,1250000,250000);
__HAL_TIM_SET_COMPARE(&htim5,TIM_CHANNEL_4,Angle1/1000);
Angle2=Limite_Range(Angle2,1250000,250000);
__HAL_TIM_SET_COMPARE(&htim12,TIM_CHANNEL_1,Angle2/1000);
Angle3=Limite_Range(Angle3,1250000,250000);
__HAL_TIM_SET_COMPARE(&htim12,TIM_CHANNEL_2,Angle3/1000);
Angle4=Limite_Range(Angle4,1250000,250000);
__HAL_TIM_SET_COMPARE(&htim15,TIM_CHANNEL_1,Angle4/1000);
Angle5=Limite_Range(Angle5,1250000,250000);
__HAL_TIM_SET_COMPARE(&htim15,TIM_CHANNEL_2,Angle5/1000);
}
//函数名：获取底盘舵机角度值函数
//入口参数：无
//返回值：底盘舵机的控制脉冲修正量
//函数功能：对底盘舵机的比例控制环节，根据摄像头反馈的横向误差获取控制脉冲修正量
int Get_Angle0(void)
{
int PWM=0;
static int Last_Error=0;
static int LLast_Error=0;
JXB_X=JXB_Y*sin((double)(2*(Get_Angle(Angle0)-118.8)/180*3.14159));
Error_X=(Aim_X-JXB_X);
// JXB_X=JXB_Y*sin((double)(2*(Get_Angle(Angle0)-118.8)/180*3.14159));
// Error_X=(Aim_X-JXB_X);
if(Error_X<=2&&Error_X>=-2)
{
Error_X=0;
Last_Error=0;
}
PWM=Angle0_KI*Error_X+Angle0_KP*(Error_X-Last_Error)+Angle0_KD*(Error_X-2*Last_Error+LLast_Error);
LLast_Error=Last_Error;
Last_Error=Error_H;
return PWM;
}
int Get_Angle2()
{
int PWM=0;
static int Last_Error=0;
static int LLast_Error=0;
Hight=(int)Arm*cos((double)((Angle1_Fixed-Get_Angle(Angle1))/180*3.14159))+(int)Forearm*cos((double)((243-Get_Angle(Angle1)-Get_Angle(Angle2))/180*3.14159))-Fforearm+Car_Hight;
Error_H=(int)(Aim_Hight-Hight);
if(Error_H<=2&&Error_H>=-2)
{
Error_H=0;
Last_Error=0;
}
PWM=Angle2_KI*Error_H+Angle2_KP*(Error_H-Last_Error)+Angle2_KD*(Error_H-2*Last_Error+LLast_Error);
LLast_Error=Last_Error;
Last_Error=Error_H;
return PWM;
}
int Get_Angle1()
{
int PWM=0;
static int Last_Error=0;
static int LLast_Error=0;
JXB_Y=(Arm*sin((double)((Angle1_Fixed-Get_Angle(Angle1))/180*3.14159))+Forearm*sin((double)((243-Get_Angle(Angle1)-Get_Angle(Angle2))/180*3.14159))\
+Servo_Lenght);
Error_Y=Aim_Y/cos((double)2*(Get_Angle(Angle0)-118.8)/180*3.14159)-JXB_Y+(int)130*cos((double)(90-Turn_Angle4)/180*3.14159)/cos((double)2*(Get_Angle(Angle0)-118.8)/180*3.14159);
// JXB_Y=(Arm*sin((double)((90-Get_Angle(Angle1))/180*3.14159))+Forearm*sin((double)((243-Get_Angle(Angle1)-Get_Angle(Angle2))/180*3.14159))\
// +Servo_Lenght);
// Error_Y=Aim_Y/cos((double)2*(Get_Angle(Angle0)-118.8)/180*3.14159)-JXB_Y;
if(Error_Y<=2&&Error_Y>=-2)
{
Error_Y=0;
Last_Error=0;
}
PWM=-Angle1_KI*Error_Y-Angle1_KP*(Error_Y-Last_Error)-Angle1_KD*(Error_Y-2*Last_Error+LLast_Error);
LLast_Error=Last_Error;
Last_Error=Error_Y;
return PWM;
}
//函数名：获取角度控制脉冲函数
//入口参数：浮点型舵机角度，范围：0~180
//返回值：无符整型舵机控制脉冲宽度，范围：250~1250
//函数功能：将舵机角度转换为舵机对应的控制脉冲宽度
int Get_Pulse(float angl)
{
angl=angl/180*1000+250;
return angl*1000;
}
//函数名：限幅函数
//入口参数：整型 输入值，限幅的上限，限幅的下限
//返回值：对输入值限幅后的结果
//函数功能：对输入值限幅后输出
int Limite_Range(int Input,int Max,int Min)
{
int Output;
Output=Input;
if(Output>Max)
Output=Max;
if(Output<Min)
Output=Min;
return Output;
}
//函数名：寻找函数
//入口参数：无
//返回值：无
//函数功能：摄像头未找到目标时，左右移动机械臂寻找目标（底盘舵机从当前角度递增至最大角度，再递减值最小角度并循环）
void Finding()
{
static uint8_t t_NoFound=0; //舵机角度变化减速控制变量
static uint8_t Angle0_Inc_Dec=0; //舵机角度增减变化标志位，0减 1增
t_NoFound++;
if(t_NoFound==20) //每100ms小幅改变一次舵机角度
{
t_NoFound=0;
if(Angle0>=900000) //舵机角度加至最大值开始减小角度
Angle0_Inc_Dec=0;
else if(Angle0<=600000)
Angle0_Inc_Dec=1; //舵机角度加至最小值开始增大角度
if(Angle0_Inc_Dec) //增大角度
{
Angle0 +=10000;
if(Angle0>900000)
Angle0=900000;
}
else //减小角度
{
Angle0 -=10000;
if(Angle0<600000)
Angle0=600000;
}
}
}
//函数名：机械臂上下移动控制函数
//入口参数：无
//返回值：无
//函数功能：在机械臂前展距离不变的情况下，根据上下移动修正量上下移动机械臂
void Up_Down()
{
double temp1,temp2;
// temp2=Angle2+Get_Angle2(); //更新小臂舵机修正量（上下移动修正量）
// x=Arm*cos(Get_Angle(Angle1)/180*3.14159)+Forearm*sin((202-Get_Angle(Angle2))/180*3.14159)+Fforearm;//计算当前机械臂前展长度
// if(temp2>Angle1+122) //更新后的舵机控制脉宽大于临界脉宽，大臂控制脉宽与小臂控制脉宽反向变化
Get_Distance();
temp1=180-(acos((double)(Distance-Forearm*sin((double)(30+Get_Angle(temp2))/180*3.14159)-Fforearm)/Arm))*180/3.14159; //根据当前机械臂前展长度和新小臂角度，计算新的大臂角度
// else //更新后的舵机控制脉宽大于临界脉宽，大臂控制脉宽与小臂控制脉宽同向变化
// temp1=(acos((1800-Forearm*sin((202-Get_Angle(temp2))/180*3.14159)-Fforearm)/Arm))*180/3.14159; //根据当前机械臂前展长度和新小臂角度，计算新的大臂角度
// if(temp1>=30&&temp1<=122&&temp2>=0) //判断新角度是否在范围内，若在则更新角度
// {
Angle2=temp2;
Angle1=Get_Pulse(temp1);
Angle_Buf[2]=Angle2;
Angle_Buf[1]=Angle1;
// }
}
//函数名：获取机械臂前展距离函数
//入口参数：无
//返回值：无
//函数功能：对小臂舵机的比例控制环节，根据摄像头反馈的前后误差获取控制脉冲修正量，并限幅
void Get_Distance()
{
// float Error;
// static float Last_Error=0;
// static float LLast_Error=0;
// Error=Aim_Length-Length1;
// Distance=Distance-Length_KI*Error-Length_KP*(Error-Last_Error)-Length_KD*(Error-2*Last_Error+LLast_Error);
// LLast_Error=Last_Error;
// Last_Error=Error;
// Limite_Range(Distance,2900,600);
}
//函数名：机械臂角度缓速赋值函数
//入口参数：无
//返回值：无
//函数功能：减缓舵机角度改变速度，以保证系统的稳定
void Angle_Slow()
{
if(Angle0!=Angle_Buf[0])
{
if(Angle0>Angle_Buf[0])
Angle0=Angle0-1000;
else
Angle0+=1000;
}
if(Angle1!=Angle_Buf[1])
{
if(Angle1>Angle_Buf[1])
Angle1-=1000;
else
Angle1+=1000;
}
if(Angle2!=Angle_Buf[2])
{
if(Angle2>Angle_Buf[2])
Angle2-=1000;
else
Angle2+=1000;
}
if(Angle3!=Angle_Buf[3])
{
if(Angle3>Angle_Buf[3])
Angle3-=1000;
else
Angle3+=1000;
}
if(Angle5!=Angle_Buf[5])
{
if(Angle5>Angle_Buf[5])
Angle5-=1000;
else
Angle5+=1000;
}
if(Angle0-Angle_Buf[0]<=1000&&Angle0-Angle_Buf[0]>=-1000)
Angle0=Angle_Buf[0];
if(Angle1-Angle_Buf[1]<=1000&&Angle1-Angle_Buf[1]>=-1000)
Angle1=Angle_Buf[1];
if(Angle2-Angle_Buf[2]<=1000&&Angle2-Angle_Buf[2]>=-1000)
Angle2=Angle_Buf[2];
if(Angle3-Angle_Buf[3]<=1000&&Angle3-Angle_Buf[3]>=-1000)
Angle3=Angle_Buf[3];
if(Angle4-Angle_Buf[4]<=1000&&Angle4-Angle_Buf[4]>=-1000)
Angle4=Angle_Buf[4];
if(Angle5-Angle_Buf[5]<=1000&&Angle5-Angle_Buf[5]>=-1000)
Angle5=Angle_Buf[5];
}
void PaiXv(void)
{
uint8_t temp1;
int temp2;
uint8_t i = 0;
uint8_t j = 0;
for (i = 1; i <= NumOfBooks-1; i++) /* 冒泡排序*/
{
for (j = 0; j <= (i - 1); j++)
{
if (Books[j].ID > Books[j+1].ID) /* 数值比较 */
{
temp1 = Books[j].ID; /* 数值换位 */
temp2 = Books[j].Angle;
Books[j].ID = Books[j+1].ID;
Books[j].Angle = Books[j+1].Angle;
Books[j+1].ID = temp1;
Books[j+1].Angle = temp2;
}
}
}
for(i=0;i<NumOfBooks;i++)
{
Books[i].Number=Books[i].ID/5+1;
switch(Books[i].Number)
{
case 1:
Books[i].Aim=0;
break;
case 2:
Books[i].Aim=2;
break;
case 3:
Books[i].Aim=3;
break;
case 4:
Books[i].Aim=5;
break;
case 5:
Books[i].Aim=6;
break;
case 6:
Books[i].Aim=8;
break;
}
}
}
整体设计思路（从头到尾）：
视频：
2023年嵌入式设计大赛国赛二等奖_哔哩哔哩_bilibili
扫一扫
5285
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
