基于野火F407骄阳开发板的苹果采摘机器人机械臂的采摘轨迹与夹持器的采摘动作的设计（1）
- 苹果采摘机器人
- 1、采摘流程与硬件设计
- 2、机械臂驱动以及采摘轨迹设计
- 2.1、台达A2电机驱动实现
- 2.2、机械臂寻找苹果巡逻轨迹
苹果采摘机器人
1、采摘流程与硬件设计
苹果采摘机器人的流程框图和硬件图，如下图所示。简单介绍下采摘流程，摄像头采集环境画面，如果画面中没有苹果，那么机械臂将以设定的运动轨迹运动，直至画面中出现苹果。一旦画面出现苹果，F04骄阳开发板将会驱动机械臂电机对准苹果与夹持器电机实现对苹果的抓取。完成采摘之后，机械臂将继续以设定的运动轨迹运动。
首先，由夹持器内侧的摄像头采集外部环境图片，将图片信息传输到nano开发板之后，由nano开发板进行深度学习图像处理，可以得到目标苹果与画面正中心位置的偏差量。将偏差量和是否存在苹果等信息量以串口的形式发送到野火F407骄阳开发板。F407会根据得到的信息量，进行处理，不存在苹果则机械臂以指定的轨迹动作。否则进行采摘苹果动作。
（注：本文将下图中机械臂和电动推杆合称机械臂。末端执行器部分将在后续文章中详细讲解）
2、机械臂驱动以及采摘轨迹设计
三轴机械臂的伺服驱动器皆为台达的asda-a2系列。设置了驱动器的驱动模式（默认为：脉冲+方向）、电子齿轮比（指令为：P1-44、P1-45。功能就是根据输入的PWM数量，决定电机的旋转圈数。具体如何设置可以查询A2的使用手册）、SON（以指令设置的方式强制使能伺服驱动器）。通过查询A2使用手册中的CN1部分信号接线，发现台达官方只提供了5V和24V的电源系统连接方式。但是通过实际测试，发现F407以5V系统的接线方式，发送PWM和方向信号，伺服驱动器是完全能够接收到脉冲的。下面给出了5V系统的接线方式和电子齿轮比的设置，具体内容可以查看A2的使用手册。
2.1、台达A2电机驱动实现
由于机械臂上限位开关的损坏，那个推杆就没有限位开关。于是决定使用主从定时器的方式实现软件限位。主定时器：产生PWM波，从定时器：监视主定时器产生的PWM波，并产生中断，中断内部根据当前电机方向信号的正负决定限制角度的加减，实现软件限位。为什么不用主定时器开启中断，一定使用从定时器产生中断呢？因为担心主定时器开启中断，会导致中断次数太频繁会影响到主函数的执行。这里就相当于主定时器对从定时器起分频作用。
首先就是脉冲引脚和方向引脚的初始化
void TIMx_GPIO_Config(void)
{
/* 定义一个 GPIO_InitTypeDef 类型的结构体 */
GPIO_InitTypeDef GPIO_InitStructure;
/* 开启定时器相关的 GPIO 外设时钟 */
TIMER_1_Master_CLK_ENABLE();
TIMER_2_Master_CLK_ENABLE();
TIMER_3_Master_CLK_ENABLE();
Motor_A_CLK_ENABLE();
Motor_B_CLK_ENABLE();
Motor_C_CLK_ENABLE();
/* 定时器PWM脉冲复用引脚初始化 */
GPIO_InitStructure.Pin = TIMER_1_Master_PIN;
GPIO_InitStructure.Mode = GPIO_MODE_AF_PP;
GPIO_InitStructure.Pull = GPIO_NOPULL;
GPIO_InitStructure.Speed = GPIO_SPEED_HIGH;
GPIO_InitStructure.Alternate = TIMER_1_Master_AF;
HAL_GPIO_Init(TIMER_1_Master_GPIO_PORT, &GPIO_InitStructure);
GPIO_InitStructure.Pin = TIMER_2_Master_PIN;
GPIO_InitStructure.Alternate = TIMER_2_Master_AF;
HAL_GPIO_Init(TIMER_2_Master_GPIO_PORT, &GPIO_InitStructure);
GPIO_InitStructure.Pin = TIMER_3_Master_PIN;
GPIO_InitStructure.Alternate = TIMER_3_Master_AF;
HAL_GPIO_Init(TIMER_3_Master_GPIO_PORT, &GPIO_InitStructure);
GPIO_InitTypeDef GPIO_InitStructureDR;
/* 伺服电机方向引脚初始化 */
GPIO_InitStructureDR.Pin = Motor_A_DIR_PIN;
GPIO_InitStructureDR.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStructureDR.Pull = GPIO_NOPULL;
GPIO_InitStructureDR.Speed = GPIO_SPEED_HIGH;
HAL_GPIO_Init(Motor_A_DIR_GPIO_PORT, &GPIO_InitStructureDR);
GPIO_InitStructureDR.Pin = Motor_B_DIR_PIN;
HAL_GPIO_Init(Motor_B_DIR_GPIO_PORT, &GPIO_InitStructureDR);
GPIO_InitStructureDR.Pin = Motor_C_DIR_PIN;
HAL_GPIO_Init(Motor_C_DIR_GPIO_PORT, &GPIO_InitStructureDR);
}
然后是主从定时器的设置，设置TIM1为主定时器，TIM2为从定时器。主从定时器的设置是有限制要求的。触发源只能是ITR0~ITR4,每组主从定时器只能用一个，且不能重复。本文一共是三轴，三组主从定时器。具体细节可以查询其他人的文章，这里不具体详解。PWM波的周期越小，电机转速就越快。而想要实现电机转速变快，只要修改主定时器的period周期和pulse变量就行，占空比尽可能在50%。Error_Handler();可以自己设置，查看是否主从定时器配置出了问题。
在从定时器的最后，开启了从定时器中断，来记录产生的PWM波的数量
/**********************X轴PWM定时器配置:主TIM1、从TIM2***********************************/
/*******************************主定时器配置***********************************/
void MX_Master_TIM1_Init(uint16_t period,uint16_t prescaler,uint16_t pulse)
{
TIM_MasterConfigTypeDef sMasterConfig = {0};
TIM_OC_InitTypeDef sConfigOC = {0};
/* TIM1 clock enable */
__HAL_RCC_TIM1_CLK_ENABLE();
htim1.Instance = TIM1; //设置主定时器为TIM1
htim1.Init.Prescaler = prescaler; //设置PWM频率
htim1.Init.CounterMode = TIM_COUNTERMODE_UP; //设置计数模式为向上计数
htim1.Init.Period = period; //设置占空比-周期
htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; //设置为无分频
if (HAL_TIM_PWM_Init(&htim1) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_UPDATE; //更新事件被选为触发输入
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_ENABLE; //开启主从模式
if (HAL_TIMEx_MasterConfigSynchronization(&htim1 , &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
sConfigOC.OCMode = TIM_OCMODE_PWM1; //设置PWM模式为PWM1
sConfigOC.Pulse = pulse; //设置PWM占空比 = pulse / period
sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW; //设置PWM空闲状态引脚拉低
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
{
Error_Handler();
}
HAL_TIM_Base_Stop(&htim1);
}
/**********************从TIM2定时器配置***********************************/
void MX_Slave_TIM2_Init(uint16_t period)
{
TIM_ClockConfigTypeDef sClockSourceConfig = {0};
TIM_SlaveConfigTypeDef sSlaveConfig = {0};
TIM_MasterConfigTypeDef sMasterConfig = {0};
/* TIM2 clock enable */
__HAL_RCC_TIM2_CLK_ENABLE();
htim2.Instance = TIM2; //设置从定时器为TIM2
htim2.Init.Prescaler = 0; //设置从定时器频率为0
htim2.Init.CounterMode = TIM_COUNTERMODE_UP; //设置计数模式为向上计数
htim2.Init.Period =period; //这个大于0就行
htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; //设置为无分频
htim2.Init.RepetitionCounter = 0;
htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim2) != HAL_OK)
{
Error_Handler();
}
sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_ITR0; //设置为内部时钟触发，即为TIM1
if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK)
{
Error_Handler();
}
sSlaveConfig.SlaveMode = TIM_SLAVEMODE_EXTERNAL1; //设置为外部触发
sSlaveConfig.InputTrigger = TIM_TS_ITR0; //设置ITR0(tim1)为输入源
sSlaveConfig.TriggerPolarity = TIM_TRIGGERPOLARITY_RISING; //设置触发模式为上升沿
sSlaveConfig.TriggerPrescaler = TIM_TRIGGERPRESCALER_DIV1; //设置无预分频
sSlaveConfig.TriggerFilter = 0x0; //设置无滤波
if (HAL_TIM_SlaveConfigSynchronization(&htim2, &sSlaveConfig) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_UPDATE;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
/* TIM2 interrupt Init */
HAL_NVIC_SetPriority(TIM2_IRQn, 2, 2);
HAL_NVIC_EnableIRQ(TIM2_IRQn);
}
/****************************************************************************/
下面是中断部分。根据Motor_A_DIR_PIN方向引脚的正负来决定限制角度Limit_angle_A的加减，以此来记录当前电机相对初始状态的旋转位置。
void TIM2_IRQHandler(void)
{
HAL_TIM_IRQHandler(&htim2);
}
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
/************************从定时器5中断*********************************/
if(htim==(&htim2))
{
// 更新中断，计数器向上溢出/向下溢出，计数器初始化(通过软件或者内部/外部触发)
if (__HAL_TIM_GET_FLAG(&htim2, TIM_FLAG_TRIGGER) != RESET)
{
__HAL_TIM_CLEAR_FLAG(&htim2, TIM_FLAG_TRIGGER);
if(HAL_GPIO_ReadPin(Motor_A_DIR_GPIO_PORT, Motor_A_DIR_PIN))
{
Limit_angle_A += 2;
}
else
{
Limit_angle_A -= 2;
}
}
}
}
主函数部分，PWM的产生与停止由HAL_TIM_PWM_Start()与HAL_TIM_PWM_Stop()函数控制。
/*PWM输出引脚、方向引脚初始化(机械臂电机控制)*/
TIMx_GPIO_Config();
//主定时器1周期、分频、占空比配置
MX_Master_TIM1_Init(400,71,200);
//从定时器2周期配置
MX_Slave_TIM2_Init(20);
/******************X轴PWM发生、限位启动项*******************/
//主定时器1时基使能
HAL_TIM_Base_Start(&htim1);
//主定时器2开始产生PWM波、停止产生PWM波
// HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
// HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1);
//开启从定时器2中断模式
HAL_TIM_Base_Start_IT(&htim2);
以上是单轴电机控制的代码，需要多个轴运动的话，复制修改即可。
2.2、机械臂寻找苹果巡逻轨迹
巡逻轨迹就是机械臂来回往复寻找苹果的过程。代码实现就是机械臂对准的方位由采摘机器人主视图左上角移动到右上角，然后控制机械臂下降一定距离。再由右到左以此往复，直至巡逻完成一整个苹果树。
void Picking_auto_posture()
{
//最初状态：将机械臂回归到摄像头视野最左上方
if ((Save_locus_A == 8000 && Save_locus_B == 4000) || Save_locus_Flag == K)
{
Motor_A_CW;
//X轴电机左转
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
while(Limit_angle_A < 12000);
HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1);
Motor_B_CCW; //Y轴上升
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
while(Limit_angle_B > 1000);
HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_1);
Save_locus_A = Limit_angle_A;
Save_locus_B = Limit_angle_B;
Save_locus_Flag = 0; //机械臂初始状态已经完成 flag = 0
}
//状态：x最左侧，y轴第一行运动，由左->右
if (Save_locus_Flag == 0)
{
Motor_A_CCW; //X轴电机右转
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
if (Limit_angle_A < 4000)
{
Save_locus_Flag = 1;
HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1); //X轴电机停止
}
}
//状态：y轴下降，x轴最右侧
if (Save_locus_Flag == 1)
{
Motor_B_CW; //Y轴电机下降
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
if (Limit_angle_B > 1500)
{
Save_locus_Flag = 2;
HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_1); //Y轴电机停止
}
}
...
