【项目介绍】
本项目移植了TobundOS，并实现使用两个PAJ7620来实时控制机械臂，能实现实时上下，左右摆动，复位、以及点动等复杂的动作。
【硬件】
1、机械臂膀
2、Nucleo-H503RB核心板
3、PAJ7620*2
【软件】
1、STM32CubeMAX
2、MDK5.39
3、TobudOS国产开源操作系统
【硬件框图】
【移植TobundOS】
1、下载tobandOS源码，https://atomgit.com/OpenAtomFoundation/TobudOS
2、把源码的目录复制到工程的TobundOS目录下面：
3、打开keil，新建os/arch分组，官方没有雅特力方面的资料，网上也没有相关的帖子，我查看了atf425的数据手册，他是arm-v7m\cortex-m4下面的内核，因此把他下面的库加入到arch工程分组中。把r\arch\arm\arm-v7m\cortex-m4\armcc目录下面的prot_c.c、port_s.S下面添加进去，在添加时要注意下面选所有文件，才能看到.S的文件。同时把os\arch\arm\arm-v7m\common下面所有的C文件都添加进工程分组，新建kernel分组，把\kernel\core路径下所有的.c文件添加\os\kernel的工程分组中。
4、添加头文件的引用到工程中。
5、注释掉stm32h5xx_it.c中的PendSV_Handler,以免跟tos的系统中重复函数。同时在systick_Handelr中添回心跳包函数：
6、添加tobuandos任务函数：
/* Private user code ---------------------------------------------------------*/ /* USER CODE BEGIN 0 */ k_stack_t task_1_stack[512]; k_task_t task_1; k_stack_t task_2_stack[512]; k_task_t task_2;
7、创建任务初始化、开始调度：
tos_knl_init(); tos_task_create(&task_1,"button_task",test_task,NULL,2,task_1_stack,512,100); tos_task_create(&task_2,"led_task",led_task_entry,NULL,1,task_2_stack,512,100); tos_knl_start();
到此tobundos移植结束。两任务，一个为获取传感器数据，一个为驱动双舵机。
【舵机驱动】
舵机驱动，为初始化两个PWM驱动，产生20ms周期的pwm，然后产生0.5ms到2.5ms的高电平，来使舵机旋转不同的角度，来实现机械臂的控制。
如上图配置所示，主频为250M，所在配置250-1分频，再设置计数值为20000,即周期为50Hz（20ms)。
【手势传感器的驱动】
在前面的过程帖子有详细的讲解，但是这次需要用到两个I2c来驱动，所以修改后的驱动代码如下：
/*
******************************************************************************
* @file ( фаил ): PAJ7620U2.c
* @brief ( описание ):
******************************************************************************
* @attention ( внимание ): author: Golinskiy Konstantin e-mail: golinskiy.konstantin@gmail.com
******************************************************************************
*/
/* Includes ----------------------------------------------------------*/
#include "PAJ7620U2.h"
//Power up initialize array
const uint8_t Init_Register_Array[][2] = {
{0xEF,0x00},
{0x32,0x29},
{0x33,0x01},
{0x34,0x00},
{0x35,0x01},
{0x36,0x00},
{0x37,0x07},
{0x38,0x17},
{0x39,0x06},
{0x3A,0x12},
{0x3F,0x00},
{0x40,0x02},
{0x41,0xFF},
{0x42,0x01},
{0x46,0x2D},
{0x47,0x0F},
{0x48,0x3C},
{0x49,0x00},
{0x4A,0x1E},
{0x4B,0x00},
{0x4C,0x20},
{0x4D,0x00},
{0x4E,0x1A},
{0x4F,0x14},
{0x50,0x00},
{0x51,0x10},
{0x52,0x00},
{0x5C,0x02},
{0x5D,0x00},
{0x5E,0x10},
{0x5F,0x3F},
{0x60,0x27},
{0x61,0x28},
{0x62,0x00},
{0x63,0x03},
{0x64,0xF7},
{0x65,0x03},
{0x66,0xD9},
{0x67,0x03},
{0x68,0x01},
{0x69,0xC8},
{0x6A,0x40},
{0x6D,0x04},
{0x6E,0x00},
{0x6F,0x00},
{0x70,0x80},
{0x71,0x00},
{0x72,0x00},
{0x73,0x00},
{0x74,0xF0},
{0x75,0x00},
{0x80,0x42},
{0x81,0x44},
{0x82,0x04},
{0x83,0x20},
{0x84,0x20},
{0x85,0x00},
{0x86,0x10},
{0x87,0x00},
{0x88,0x05},
{0x89,0x18},
{0x8A,0x10},
{0x8B,0x01},
{0x8C,0x37},
{0x8D,0x00},
{0x8E,0xF0},
{0x8F,0x81},
{0x90,0x06},
{0x91,0x06},
{0x92,0x1E},
{0x93,0x0D},
{0x94,0x0A},
{0x95,0x0A},
{0x96,0x0C},
{0x97,0x05},
{0x98,0x0A},
{0x99,0x41},
{0x9A,0x14},
{0x9B,0x0A},
{0x9C,0x3F},
{0x9D,0x33},
{0x9E,0xAE},
{0x9F,0xF9},
{0xA0,0x48},
{0xA1,0x13},
{0xA2,0x10},
{0xA3,0x08},
{0xA4,0x30},
{0xA5,0x19},
{0xA6,0x10},
{0xA7,0x08},
{0xA8,0x24},
{0xA9,0x04},
{0xAA,0x1E},
{0xAB,0x1E},
{0xCC,0x19},
{0xCD,0x0B},
{0xCE,0x13},
{0xCF,0x64},
{0xD0,0x21},
{0xD1,0x0F},
{0xD2,0x88},
{0xE0,0x01},
{0xE1,0x04},
{0xE2,0x41},
{0xE3,0xD6},
{0xE4,0x00},
{0xE5,0x0C},
{0xE6,0x0A},
{0xE7,0x00},
{0xE8,0x00},
{0xE9,0x00},
{0xEE,0x07},
{0xEF,0x01},
{0x00,0x1E},
{0x01,0x1E},
{0x02,0x0F},
{0x03,0x10},
{0x04,0x02},
{0x05,0x00},
{0x06,0xB0},
{0x07,0x04},
{0x08,0x0D},
{0x09,0x0E},
{0x0A,0x9C},
{0x0B,0x04},
{0x0C,0x05},
{0x0D,0x0F},
{0x0E,0x02},
{0x0F,0x12},
{0x10,0x02},
{0x11,0x02},
{0x12,0x00},
{0x13,0x01},
{0x14,0x05},
{0x15,0x07},
{0x16,0x05},
{0x17,0x07},
{0x18,0x01},
{0x19,0x04},
{0x1A,0x05},
{0x1B,0x0C},
{0x1C,0x2A},
{0x1D,0x01},
{0x1E,0x00},
{0x21,0x00},
{0x22,0x00},
{0x23,0x00},
{0x25,0x01},
{0x26,0x00},
{0x27,0x39},
{0x28,0x7F},
{0x29,0x08},
{0x30,0x03},
{0x31,0x00},
{0x32,0x1A},
{0x33,0x1A},
{0x34,0x07},
{0x35,0x07},
{0x36,0x01},
{0x37,0xFF},
{0x38,0x36},
{0x39,0x07},
{0x3A,0x00},
{0x3E,0xFF},
{0x3F,0x00},
{0x40,0x77},
{0x41,0x40},
{0x42,0x00},
{0x43,0x30},
{0x44,0xA0},
{0x45,0x5C},
{0x46,0x00},
{0x47,0x00},
{0x48,0x58},
{0x4A,0x1E},
{0x4B,0x1E},
{0x4C,0x00},
{0x4D,0x00},
{0x4E,0xA0},
{0x4F,0x80},
{0x50,0x00},
{0x51,0x00},
{0x52,0x00},
{0x53,0x00},
{0x54,0x00},
{0x57,0x80},
{0x59,0x10},
{0x5A,0x08},
{0x5B,0x94},
{0x5C,0xE8},
{0x5D,0x08},
{0x5E,0x3D},
{0x5F,0x99},
{0x60,0x45},
{0x61,0x40},
{0x63,0x2D},
{0x64,0x02},
{0x65,0x96},
{0x66,0x00},
{0x67,0x97},
{0x68,0x01},
{0x69,0xCD},
{0x6A,0x01},
{0x6B,0xB0},
{0x6C,0x04},
{0x6D,0x2C},
{0x6E,0x01},
{0x6F,0x32},
{0x71,0x00},
{0x72,0x01},
{0x73,0x35},
{0x74,0x00},
{0x75,0x33},
{0x76,0x31},
{0x77,0x01},
{0x7C,0x84},
{0x7D,0x03},
{0x7E,0x01}
};
/******************************************************************************
function:
I2C Write and Read
******************************************************************************/
static void DEV_I2C_WriteByte(I2C_HandleTypeDef PAJ7620U2_I2C,uint8_t add_, uint8_t data_)
{
uint8_t Buf[1] = {0};
Buf[0] = data_;
HAL_I2C_Mem_Write(&PAJ7620U2_I2C, PAJ7620U2_I2C_ADDRESS, add_, I2C_MEMADD_SIZE_8BIT, Buf, 1, 500);
}
static void DEV_I2C_WriteWord(I2C_HandleTypeDef PAJ7620U2_I2C,uint8_t add_, uint16_t data_)
{
uint8_t Buf[2] = {0};
Buf[0] = data_ >> 8;
Buf[1] = data_;
HAL_I2C_Mem_Write(&PAJ7620U2_I2C, PAJ7620U2_I2C_ADDRESS, add_, I2C_MEMADD_SIZE_8BIT, Buf, 2, 500);
}
static uint8_t DEV_I2C_ReadByte(I2C_HandleTypeDef PAJ7620U2_I2C,uint8_t add_)
{
uint8_t Buf[1]={add_};
HAL_I2C_Mem_Read(&PAJ7620U2_I2C, PAJ7620U2_I2C_ADDRESS, add_, I2C_MEMADD_SIZE_8BIT, Buf, 1, 500);
return Buf[0];
}
static uint16_t DEV_I2C_ReadWord(I2C_HandleTypeDef PAJ7620U2_I2C,uint8_t add_)
{
uint8_t Buf[2]={0, 0};
HAL_I2C_Mem_Read(&PAJ7620U2_I2C, PAJ7620U2_I2C_ADDRESS, add_, I2C_MEMADD_SIZE_8BIT, Buf, 2, 500);
return ((Buf[1] << 8) | (Buf[0] & 0xff));
}
static uint16_t PAJ7620U2_GetPartID(I2C_HandleTypeDef PAJ7620U2_I2C)
{
uint8_t data[2] = {0, };
data[0] = DEV_I2C_ReadByte(PAJ7620U2_I2C,PAJ_PART_ID_L);
data[1] = DEV_I2C_ReadByte(PAJ7620U2_I2C,PAJ_PART_ID_H);
return ((uint16_t)data[1]<<8) | data[0];
}
//----------------------------------------------------------------------------------
// при инициализации возвращает ( если соединение с датчиком удачное ) значение 0x7620 либо 0 если датчик не увидел
uint16_t PAJ7620U2_init(I2C_HandleTypeDef PAJ7620U2_I2C)
{
uint16_t ID = PAJ7620U2_GetPartID(PAJ7620U2_I2C);
if( ID == PAJ_PARTID ){
DEV_I2C_WriteByte(PAJ7620U2_I2C,PAJ_BANK_SELECT, PAJ_BANK_0);
for (uint8_t i = 0; i < Init_Array; i++ )
{
DEV_I2C_WriteByte(PAJ7620U2_I2C,Init_Register_Array[i][0], Init_Register_Array[i][1]);//Power up initialize
}
DEV_I2C_WriteByte(PAJ7620U2_I2C,PAJ_BANK_SELECT, PAJ_BANK_0);
return ID;
}
return 0;
}
//----------------------------------------------------------------------------------
// запрашиваем номер движения ( если движения небыло вернет 0 ) если было то его номер
uint16_t PAJ7620U2_Gesture_ReadData(I2C_HandleTypeDef PAJ7620U2_I2C)
{
return DEV_I2C_ReadWord(PAJ7620U2_I2C,PAJ_INT_FLAG1);
}
//----------------------------------------------------------------------------------
// Object Brightness (Max. 255)
uint8_t PAJ7620U2_PS_ReadObjBrightness(I2C_HandleTypeDef PAJ7620U2_I2C)
{
return DEV_I2C_ReadByte(PAJ7620U2_I2C,PAJ_OBJ_BRIGHTNESS);
}
//----------------------------------------------------------------------------------
// Object Size (Max. 900)
uint16_t PAJ7620U2_PS_ReadObjSize(I2C_HandleTypeDef PAJ7620U2_I2C)
{
return DEV_I2C_ReadWord(PAJ7620U2_I2C,PAJ_OBJ_SIZE_L);
}
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
/************************ (C) COPYRIGHT GKP *****END OF FILE****/到此手势传感器的驱动就移完毕
【程序流程图】
控制代码分析：
在获取数据任务中，我们只需要周期获取两个传感器的数据就行了。
void test_task(void *Parameter)
{
uint16_t ID = PAJ7620U2_init(hi2c1);
ID = PAJ7620U2_init(hi2c2);
while (1)
{
OBJ_BRIGHTNESS = PAJ7620U2_PS_ReadObjBrightness(hi2c2); // Object Brightness (Max. 255)
OBJ_SIZE = PAJ7620U2_PS_ReadObjSize(hi2c2); // Object Size (Max. 900)
printf("Brightness:%d Size:%d \r\n",OBJ_BRIGHTNESS,OBJ_SIZE);
Gesture_Data = PAJ7620U2_Gesture_ReadData(hi2c1);
tos_task_delay(10);
}
}在控制任务中，我们分析两个传感器的数，根据是否按下的传感器1的状态，来分别对机械臂行点动，或者一步到底的操作。
void led_task_entry(void *Parameter)
{
while(1)
{
if( Gesture_Data != PAJ_NONE )
{
switch (Gesture_Data)
{
case PAJ_UP:
if(OBJ_SIZE>850)
{
up_Compare = 1*20000/40; //2.5ms
}
else{
up_Compare -= 10;
if(up_Compare<1*20000/40) //2.5ms
up_Compare = 1*20000/40; //2.5ms
}
break;
case PAJ_DOWN:
if(OBJ_SIZE>850)
{
up_Compare = 3*20000/40; //180
}
else{
up_Compare += 10;
if(up_Compare>3*20000/40)
up_Compare = 3*20000/40; //180
}
break; // вниз
case PAJ_LEFT:
if(OBJ_SIZE>850)
{
down_Compare = 1*20000/40; //0.5ms
}
else
{
down_Compare -= 10;
if(down_Compare < 1*20000/40)
down_Compare = 1*20000/40; //0.5ms
}
break; // влево
case PAJ_RIGHT:
if(OBJ_SIZE>850)
{
down_Compare = 5*20000/40; //180
}
else
{
down_Compare += 10;
if(down_Compare > 5*20000/40) //180)
down_Compare = 5*20000/40; //180; //0.5ms
}
break; // вправо
case PAJ_FORWARD:
up_Compare = 3*20000/40; //90
down_Compare = 3*20000/40; //90
break; // вперед
default: break;
}
}
Gesture_Data = 0;
__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_4,up_Compare);
__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_3,down_Compare);
tos_task_delay(50);
}
}【实验现象】
分别对两个感器进行操作，可以实现对机械臂的控制，详见视频讲解。
【作品视频介绍】
https://www.bilibili.com/video/BV1vdbxeaEA5/?pop_share=1
我要赚赏金
STM32
MCU
通讯及无线技术
物联网技术
电子DIY
板卡试用
基础知识
软件与操作系统
我爱生活
小e食堂
