STM32 HAL库快速实战【七】《机械臂控制》--基于黑龙江科技大学机电工业机器人实训

系列目录

点击查看

接线

image

总共六个舵机，使用PB3-PB9引脚。
从下面支撑舵机到上面夹子舵机依次接DJ0到DJ5接口。

配置CubeMX

可以使用上节配置好的demo复制后继续配置，也可以重新生成，这里不多赘述。
勾选PB3-PB8，打开定时器。由于涉及到多个舵机，归零可能会造成干涉，所以所有舵机一定要调整到中位后在安装。
调整中位只需要将所有定时器通道的Pulse设置为1500即可。
image

配置方法同STM32 HAL库快速实战【三】《pwm控制舵机》--基于黑龙江科技大学机电工业机器人实训 - USTHzhanglu - 博客园 (cnblogs.com)

记住每个舵机对应的通道。

编写代码

生成代码后，新建robot-arm.c和robot-arm.h,放入对应文件夹以及工程中。

初始化舵机

在.h文件中添加依赖，并声明初始化函数

#include "tim.h"
#include "motor.h"
void arm_init(void);

在.c文件中定义初始化函数

#include "robot-arm.h"

/**
 * @brief  ：初始化舵机0到舵机5，均初始化到中间位置。
 * @param  ：void
 * @retval None
 **/
void arm_init(void)
{
	//__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,500);修改DJ0的初始化角度为0度。
	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_2);//DJ0,PB3
	HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_3);//DJ1,PB8
	HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_4);//DJ2,PB9
	HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_1);//DJ3,PB6
	HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_2);//DJ4,PB7
	HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_1);//DJ5,PB4
	return;
}
}

由于设置的Pulse为1500，所以每个舵机的初始位置都在中间位。如果需要初始时位置不同，可以修改配置工程，或者在Start之前使用__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,rad);来修改初始化位置。

控制舵机

由于我们需要一次控制六个舵机，每个舵机单独控制属实有够麻烦。我们可以定义一个数组，里面储存每次每个舵机需要运动到的角度，这样每次我们只需要修改数组，就能很方便的控制六个舵机。

/**
 * @brief  ：控制舵机0到舵机5，左上转为正，右下转为负，DJ0-5 +-135°，DJ6+-90°
 * @param  ：rad[6]，分别储存每个舵机转动角度
 * @retval None
 **/
void arm_set(const int rad[6])
{

需要注意的是，__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,rad);传入的参数是占空比，但是我们需要控制的是角度，因此需要进行一个简单的运算(假设舵机处在中间位时为0°):1500+rad[x]*1000/135。同时，由于安装原因，有些舵机可能装反了，拆卸安装有够恶心的，我们可以定义一个数组声明舵机运动状态，装反的舵机只需要声明该舵机运动状态相反即可。
在robot-arm.c顶部中添加

#include "robot-arm.h"

/* 定义一个数组，说明舵机运动方向，如果实际转向相反与输入相反，则修改对应位为-1
*/
const int arm_toggle[6]={1,1,1,1,1,1};

然后我们就可以定义舵机控制函数

/**
 * @brief  ：控制舵机0到舵机5，左上转为正，右下转为负，DJ0-5 +-135°，DJ6+-90°
 * @param  ：rad[6]，分别储存每个舵机转动角度
 * @retval None
 **/
void arm_set(const int rad[6])
{
	int compare[6]={
		1500+rad[0]*arm_toggle[0]*1000/135,
		1500+rad[1]*arm_toggle[1]*1000/135,
		1500+rad[2]*arm_toggle[2]*1000/135,
		1500+rad[3]*arm_toggle[3]*1000/135,
		1500+rad[4]*arm_toggle[4]*1000/135,
		1500+rad[5]*arm_toggle[5]*1000/90,
		};
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,compare[0]);
__HAL_TIM_SetCompare(&htim4,TIM_CHANNEL_3,compare[1]);
__HAL_TIM_SetCompare(&htim4,TIM_CHANNEL_4,compare[2]);
__HAL_TIM_SetCompare(&htim4,TIM_CHANNEL_1,compare[3]);
__HAL_TIM_SetCompare(&htim4,TIM_CHANNEL_2,compare[4]);
__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_1,compare[5]);
}

在main.c中初始化arm

 /* USER CODE BEGIN 2 */
motor_init();
arm_init();
HAL_Delay(2000);
HAL_GPIO_WritePin(led_GPIO_Port,led_Pin,GPIO_PIN_RESET);
  /* USER CODE END 2 */

烧录程序后，机械臂会直立起来。如果角度不对，请重新安装各关节舵机。

调节各关节角度

有了舵机控制代码，我们可以方便的调节舵机角度，但是手动控制过于繁琐，实际使用时更偏向于自动化控制。最好的方法是建立机械臂运动模型，但是难度较高。在这里我们选择一种简单类似于示教机器人的方法，通过记录每次动作的角度状态，在不同动作间直接切换即可。
首先，需要实时获取当前关节角度。为了方便使用，这里封装成一个函数：

/**
 * @brief  打印当前关节角度
 * @param  ：arm_rad[6]，分别储存每个舵机转动角度
 * @retval None
 **/
void printf_rad(const int arm_rad[6])
{  char cmd_return[128];
	sprintf(cmd_return, "{rad:%d,%d,%d,%d,%d,%d}",arm_rad[0],arm_rad[1],arm_rad[2],arm_rad[3],arm_rad[4],arm_rad[5]);
	usart_send_str(&huart3,(unsigned char *)cmd_return);
	return;
}

然后定义一个get_rad函数，用于获取输入的命令，调节舵机角度

点击查看代码 
/**
 * @brief  ：控制舵机0到舵机5，并打印当前角度
 * @param  ：None
 * @retval None
 **/
void get_rad(void)
{
	char menu;
	int arm_status=1;
	int rad[6]={0,0,0,0,0,0};
	while( arm_status)
	{menu=get_cmd();
		switch(menu)
		{
			  case 'A':rad[0]++;break;
			  case 'B':rad[0]--;break;
			  case 'C':rad[1]++;break;
			  case 'D':rad[1]--;break;
			  case 'E':rad[2]++;break;
			  case 'F':rad[2]--;break;
			  case 'G':rad[3]++;break;
			  case 'H':rad[3]--;break;
			  case 'I':rad[4]++;break;
			  case 'J':rad[4]--;break;
			  case 'K':rad[5]++;break;
			  case 'L':rad[5]--;break;
			  case 'Q':arm_status=0;break;
		}
	printf_rad(rad);
	arm_set(rad);
	menu=NULL;
	}
return;
}

同时在.h文件中声明函数

#include "tim.h"
#include "motor.h"
void arm_init(void);
void arm_set(const int rad[6]);
void printf_rad(const int arm_rad[6]);
void get_rad(void);

在main.c的switch中新增一个选项

switch(input)
		  {	  
			  case '0':get_rad();break;

通过蓝牙控制，输入0进入舵机调节菜单，然后输入A-L调节关节，输入Q退出调节菜单。
可以在BlueSPP中切换到键盘界面，快速调节
image
然后在CHAT界面查看当前角度。
image
记录不同动作的角度，例如待机时为{0,85,-135,-54,0,0}，在.c文件顶部定义：

/* 关节初始状态*/
const int arm_begin[6]={0,85,-135,-54,0,0};

然后就可以直接执行关节动作。例如初始化舵机时直接初始化到待机位置

void arm_init(void)
{
	arm_set(arm_begin);
	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_2);//DJ0,PB3

工程源码

国内用户请使用gitee克隆或是使用代理访问Github
https://github.com/USTHzhanglu/stm32-hal/tree/main/robot-arm

posted @ 2021-12-08 17:16  USTHzhanglu  阅读(1955)  评论(0)  编辑  收藏  举报