OpenHarmony 项目实战：基于全志 XR806 实现的简易四足行走机器狗
本文整理自：https://bbs.aw-ol.com/topic/932/ 作者 @ 堇花还没开吗 小米在2021年发布的自研四足仿生机器人——CyberDog，使得“仿生机器人”这个话题 再次站在风口之上 ，伴随芯片、舵机、减速器等核心部件的发展，也期待更多更“生动”的“仿生机器人”出现 ，全志XR806是一款支持WiFi和BLE的芯片可满足智能机器人等领域的无线连接需求，今天就先带大家看一款基于全志XR806开源鸿蒙开发板做的四足行走机器狗。 一、机器狗外观图 二、PCA9685开发流程 狗子身上有12个舵机，一条腿接3个舵机，连接到一个pca9685舵机驱动上，舵机驱动通过I2C与XR806通信，XR806有两个I2C接口，我主要是用第1个(B14, B15)，通过控制PCA9685上的寄存器来简介控制舵机。 pca9685参数： I2C接口，支持高达16路PWM输出，每路12位分辨率(4096级); 内置25MHz晶振，可不连接外部晶振，也可以连接外部晶振，最大50MHz； 支持2.3V-5.5V电压，最大耐压值5.5V,逻辑电平3.3V； 具有上电复位，以及软件复位等功能。 引脚图示： 1. pca初始化 首先要进行i2c初始化: i2c_init(i2c_id); 导入相关库: #include "driver/chip/hal_i2c.h" #define i2c_id 1 # 使用第1个i2c i2c初始化函数: void i2c_init(unsigned int id){ I2C_InitParam initParam; initParam.addrMode = I2C_ADDR_MODE_7BIT; initParam.clockFreq = 40000; if (HAL_I2C_Init(id, &initParam) != HAL_OK) { printf("i2c init fail!\n"); while(1); } else { printf("i2c init success!\n"); } } 主要是调用HAL_I2C_Init函数对第1个i2c接口进行初始化。 初始化pca9685： pca9685_init(60); pca9685_init函数： void pca9685_init(float hz) { pca_write(pca_mode1, 0x0); pca_setfreq(hz); OS_MSleep(500); } 2. pca操作 1. pca9685写寄存器 void pca_write(uint8_t reg_addr, uint8_t data) { uint8_t buf[1]; buf[0] = data; HAL_I2C_Master_Transmit_Mem_IT(i2c_id, pca_adrr, reg_addr, I2C_MEMADDR_SIZE_8BIT, buf, 1); } 2. pca9685读寄存器 uint8_t pca_read(uint8_t reg_addr) { uint8_t buf[1]; HAL_I2C_Master_Receive_Mem_IT(i2c_id, pca_adrr, reg_addr, I2C_MEMADDR_SIZE_8BIT, buf, 1); return buf[0]; } 3. pca9685设置频率 void pca_setfreq(float freq) { uint8_t prescale,old_mode,new_mode; double prescaleval; freq *= 0.92; prescaleval = 25000000; prescaleval /= 4096; prescaleval /= freq; prescaleval -= 1; prescale =(uint8_t)(prescaleval + 0.5f); old_mode = pca_read(pca_mode1); new_mode = (old_mode & 0x7F) | 0x10; pca_write(pca_mode1, new_mode); pca_write(pca_pre, prescale); pca_write(pca_mode1, old_mode); OS_MSleep(2); pca_write(pca_mode1, old_mode | 0xa1); } 这里的代码不像读函数和写函数一样好理解，主要是一般情况下，在用pca9685内置晶振，为25MHZ，通过配置PRE_SCALE寄存器进行配置。如果在舵机控制中，采用内置晶振，取osc_clock=25000000，update_rate=50(舵机控制频率50Hz)。 而且在写PRESCALE寄存器的时候，要先设置为Sleep模式，也就是将mode1寄存器的SLEEP标志位设置为1，具体可参考我上面写的代码。 3. pca设置pwm void pca_setpwm(uint8_t num, uint32_t on, uint32_t off) { pca_write(LED0_ON_L+4*num,on); pca_write(LED0_ON_H+4*num,on>>8); pca_write(LED0_OFF_L+4*num,off); pca_write(LED0_OFF_H+4*num,off>>8); } pwm通道寄存器如下图： 由图可知，对于每一个通道，有4个寄存器，在设置PWM占空比的时候，首先配置舵机的示例如下图所示(ON < OFF的情况): 狗子运行图： 核心代码： #include <stdio.h> #include "ohos_init.h" #include "kernel/os/os.h" // #include "iot_i2c.h" #include "driver/chip/hal_i2c.h" #include "iot_errno.h" #include "math.h" #define i2c_id 1 #define pca_adrr 0x40 // pca9685设备地址 #define pca_mode1 0x0 #define pca_pre 0xFE #define LED0_ON_L 0x6 #define LED0_ON_H 0x7 #define LED0_OFF_L 0x8 #define LED0_OFF_H 0x9 static OS_Thread_t g_main_thread; void pca_write(uint8_t reg_addr, uint8_t data) { uint8_t buf[1]; buf[0] = data; HAL_I2C_Master_Transmit_Mem_IT(i2c_id, pca_adrr, reg_addr, I2C_MEMADDR_SIZE_8BIT, buf, 1); } uint8_t pca_read(uint8_t reg_addr) { uint8_t buf[1]; HAL_I2C_Master_Receive_Mem_IT(i2c_id, pca_adrr, reg_addr, I2C_MEMADDR_SIZE_8BIT, buf, 1); return buf[0]; } void pca_setfreq(float freq) { uint8_t prescale,old_mode,new_mode; double prescaleval; freq *= 0.92; prescaleval = 25000000; prescaleval /= 4096; prescaleval /= freq; prescaleval -= 1; prescale =(uint8_t)(prescaleval + 0.5f); old_mode = pca_read(pca_mode1); new_mode = (old_mode & 0x7F) | 0x10; pca_write(pca_mode1, new_mode); pca_write(pca_pre, prescale); pca_write(pca_mode1, old_mode); OS_MSleep(2); pca_write(pca_mode1, old_mode | 0xa1); } void pca_setpwm(uint8_t num, uint32_t on, uint32_t off) { pca_write(LED0_ON_L+4*num,on); pca_write(LED0_ON_H+4*num,on>>8); pca_write(LED0_OFF_L+4*num,off); pca_write(LED0_OFF_H+4*num,off>>8); } void pca9685_init(float hz) { pca_write(pca_mode1, 0x0); pca_setfreq(hz); OS_MSleep(500); } void pca_rotate(uint8_t num,uint8_t angle) { uint32_t off=0; off=floor(angle * 2 + angle / 5 + 158); pca_setpwm(num, 0, off); } void i2c_init(unsigned int id){ I2C_InitParam initParam; initParam.addrMode = I2C_ADDR_MODE_7BIT; initParam.clockFreq = 40000; if (HAL_I2C_Init(id, &initParam) != HAL_OK) { printf("i2c init fail!\n"); while(1); } else { printf("i2c init success!\n"); } } static void MainThread(void *arg) { uint8_t i = 0; i2c_init(i2c_id); pca9685_init(60); printf("i2c and pca9685 init done.\n"); while(1){ pca_rotate(0, 0); OS_MSleep(1000); pca_rotate(0, 180); OS_MSleep(1000); } } void PCAMain(void) { printf("PCA9685 Motor Start.\n"); if (OS_ThreadCreate(&g_main_thread, "MainThread", MainThread, NULL, OS_THREAD_PRIO_APP, 10 * 1024) != OS_OK) { printf("[ERR] Create MainThread Failed\n"); } } SYS_RUN(PCAMain); 狗子的相关步态算法还在调试，使用的舵机是MG90S，走得还不是很流畅，狗腿子会抖，后面需要继续调试，而且供电也是个大问题，不过已经焊了个供电模块稍微解决了，后面还要给狗子加个壳。
