diff --git a/RBcore/BHBF.c b/RBcore/BHBF.c index 679663f..f6ae3c1 100644 --- a/RBcore/BHBF.c +++ b/RBcore/BHBF.c @@ -19,6 +19,8 @@ #include "BHBF.h" #include "common.h" #include "msg_center.h" +#include "bsp_EEPROM.h" +#include "bsp_CV.pb.h" /*----------------------------------------------* * 外部变量说明 * @@ -89,6 +91,7 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg) void RBcore_Task(void *argument) { + GF_BSP_EEPROM_Init(); while(1) { // 处理消息 diff --git a/RBcore/drv_interface.c b/RBcore/drv_interface.c index 92c98b2..1577cbe 100644 --- a/RBcore/drv_interface.c +++ b/RBcore/drv_interface.c @@ -275,7 +275,7 @@ void Drv_InterfaceInit(void) }; (void)osThreadNew(MotorTask, NULL, &motor_task_attributes); -// RBcore_Init(); + RBcore_Init(); TUartUserData *ptSbus = UART_userdata_init(5, -1, 512); g_ptSbus = rd_ComCreate(check_MK32, decode_MK32, E28_SBUS_Send, ptSbus->m_buf_size, ptSbus); diff --git a/bspMCU/bsp_EEPROM.c b/bspMCU/bsp_EEPROM.c new file mode 100644 index 0000000..2021ed3 --- /dev/null +++ b/bspMCU/bsp_EEPROM.c @@ -0,0 +1,526 @@ +/* + * bsp_EEPROM.c + * + * Created on: Oct 26, 2023 + * Author: shiya + */ +#include "bsp_EEPROM.h" +#include "bsp_pin.h" + +void bsp_InitI2C(void); +void i2c_Start(void); +void i2c_Stop(void); +void i2c_SendByte(uint8_t _ucByte); +uint8_t i2c_ReadByte(void); +uint8_t i2c_WaitAck(void); +void i2c_Ack(void); +void i2c_NAck(void); +uint8_t i2c_CheckDevice(uint8_t _Address); + +uint16_t App_Download_EEPROM_Addr= 1024; +/* 定义读写SCL和SDA的宏 */ +#define I2C_SCL_GPIO EEPROM_SCL_GPIO_Port +#define I2C_SDA_GPIO EEPROM_SDA_GPIO_Port +#define I2C_SCL_PIN EEPROM_SCL_Pin +#define I2C_SDA_PIN EEPROM_SDA_Pin + +#define I2C_SCL_1() I2C_SCL_GPIO->BSRR = I2C_SCL_PIN /* SCL = 1 */ +#define I2C_SCL_0() I2C_SCL_GPIO->BSRR = ((uint32_t)I2C_SCL_PIN << 16U) /* SCL = 0 */ + +#define I2C_SDA_1() I2C_SDA_GPIO->BSRR = I2C_SDA_PIN /* SDA = 1 */ +#define I2C_SDA_0() I2C_SDA_GPIO->BSRR = ((uint32_t)I2C_SDA_PIN << 16U) /* SDA = 0 */ + + +#define I2C_SDA_READ() ((I2C_SDA_GPIO->IDR & I2C_SDA_PIN) != 0) /* 读SDA口线状态 */ +#define I2C_SCL_READ() ((I2C_SCL_GPIO->IDR & I2C_SCL_PIN) != 0) /* 读SCL口线状态 */ + + + +uint8_t GF_BSP_EEPROM_Init(void) +{ + bsp_InitI2C(); + return(GF_BSP_EEPROM_CheckOK()); +} + +/* +********************************************************************************************************* +* 函 数 名: bsp_InitI2C +* 功能说明: 配置I2C总线的GPIO，采用模拟IO的方式实现 +* 形 参: 无 +* 返 回 值: 无 +********************************************************************************************************* +*/ +void bsp_InitI2C(void) +{ + GPIO_InitTypeDef gpio_init; + + + gpio_init.Mode = GPIO_MODE_OUTPUT_OD; /* 设置开漏输出 */ + gpio_init.Pull = GPIO_NOPULL; /* 上下拉电阻不使能 */ + gpio_init.Speed = GPIO_SPEED_FREQ_LOW; // GPIO_SPEED_FREQ_HIGH; /* GPIO速度等级 */ + + gpio_init.Pin = I2C_SCL_PIN; + HAL_GPIO_Init(I2C_SCL_GPIO, &gpio_init); + + gpio_init.Pin = I2C_SDA_PIN; + HAL_GPIO_Init(I2C_SDA_GPIO, &gpio_init); + + /* 给一个停止信号, 复位I2C总线上的所有设备到待机模式 */ + i2c_Stop(); +} + +/* +********************************************************************************************************* +* 函 数 名: i2c_Delay +* 功能说明: I2C总线位延迟，最快400KHz +* 形 参: 无 +* 返 回 值: 无 +********************************************************************************************************* +*/ +static void i2c_Delay(void) +{ + delay_us(2); +} + +/* +********************************************************************************************************* +* 函 数 名: i2c_Start +* 功能说明: CPU发起I2C总线启动信号 +* 形 参: 无 +* 返 回 值: 无 +********************************************************************************************************* +*/ +void i2c_Start(void) +{ + /* 当SCL高电平时，SDA出现一个下跳沿表示I2C总线启动信号 */ + I2C_SDA_1(); + I2C_SCL_1(); + i2c_Delay(); + I2C_SDA_0(); + i2c_Delay(); + + I2C_SCL_0(); + i2c_Delay(); +} + +/* +********************************************************************************************************* +* 函 数 名: i2c_Start +* 功能说明: CPU发起I2C总线停止信号 +* 形 参: 无 +* 返 回 值: 无 +********************************************************************************************************* +*/ +void i2c_Stop(void) +{ + /* 当SCL高电平时，SDA出现一个上跳沿表示I2C总线停止信号 */ + I2C_SDA_0(); + i2c_Delay(); + I2C_SCL_1(); + i2c_Delay(); + I2C_SDA_1(); + i2c_Delay(); +} + +/* +********************************************************************************************************* +* 函 数 名: i2c_SendByte +* 功能说明: CPU向I2C总线设备发送8bit数据 +* 形 参: _ucByte ： 等待发送的字节 +* 返 回 值: 无 +********************************************************************************************************* +*/ +void i2c_SendByte(uint8_t _ucByte) +{ + uint8_t i; + + /* 先发送字节的高位bit7 */ + for (i = 0; i < 8; i++) + { + if (_ucByte & 0x80) + { + I2C_SDA_1(); + } + else + { + I2C_SDA_0(); + } + i2c_Delay(); + I2C_SCL_1(); + i2c_Delay(); + I2C_SCL_0(); + I2C_SCL_0(); /* 2019-03-14 针对GT811电容触摸，添加一行，相当于延迟几十ns */ + if (i == 7) + { + I2C_SDA_1(); // 释放总线 + } + _ucByte <<= 1; /* 左移一个bit */ + } +} + +/* +********************************************************************************************************* +* 函 数 名: i2c_ReadByte +* 功能说明: CPU从I2C总线设备读取8bit数据 +* 形 参: 无 +* 返 回 值: 读到的数据 +********************************************************************************************************* +*/ +uint8_t i2c_ReadByte(void) +{ + uint8_t i; + uint8_t value; + + /* 读到第1个bit为数据的bit7 */ + value = 0; + for (i = 0; i < 8; i++) + { + value <<= 1; + I2C_SCL_1(); + i2c_Delay(); + if (I2C_SDA_READ()) + { + value++; + } + I2C_SCL_0(); + i2c_Delay(); + } + return value; +} + +/* +********************************************************************************************************* +* 函 数 名: i2c_WaitAck +* 功能说明: CPU产生一个时钟，并读取器件的ACK应答信号 +* 形 参: 无 +* 返 回 值: 返回0表示正确应答，1表示无器件响应 +********************************************************************************************************* +*/ +uint8_t i2c_WaitAck(void) +{ + uint8_t re; + + I2C_SDA_1(); /* CPU释放SDA总线 */ + i2c_Delay(); + I2C_SCL_1(); /* CPU驱动SCL = 1, 此时器件会返回ACK应答 */ + i2c_Delay(); + if (I2C_SDA_READ()) /* CPU读取SDA口线状态 */ + { + re = 1; + } + else + { + re = 0; + } + I2C_SCL_0(); + i2c_Delay(); + return re; +} + +/* +********************************************************************************************************* +* 函 数 名: i2c_Ack +* 功能说明: CPU产生一个ACK信号 +* 形 参: 无 +* 返 回 值: 无 +********************************************************************************************************* +*/ +void i2c_Ack(void) +{ + I2C_SDA_0(); /* CPU驱动SDA = 0 */ + i2c_Delay(); + I2C_SCL_1(); /* CPU产生1个时钟 */ + i2c_Delay(); + I2C_SCL_0(); + i2c_Delay(); + I2C_SDA_1(); /* CPU释放SDA总线 */ + + i2c_Delay(); +} + +/* +********************************************************************************************************* +* 函 数 名: i2c_NAck +* 功能说明: CPU产生1个NACK信号 +* 形 参: 无 +* 返 回 值: 无 +********************************************************************************************************* +*/ +void i2c_NAck(void) +{ + I2C_SDA_1(); /* CPU驱动SDA = 1 */ + i2c_Delay(); + I2C_SCL_1(); /* CPU产生1个时钟 */ + i2c_Delay(); + I2C_SCL_0(); + i2c_Delay(); +} + +/* +********************************************************************************************************* +* 函 数 名: i2c_CheckDevice +* 功能说明: 检测I2C总线设备，CPU向发送设备地址，然后读取设备应答来判断该设备是否存在 +* 形 参: _Address：设备的I2C总线地址 +* 返 回 值: 返回值 0 表示正确， 返回1表示未探测到 +********************************************************************************************************* +*/ +uint8_t i2c_CheckDevice(uint8_t _Address) +{ + uint8_t ucAck; + + if (I2C_SDA_READ() && I2C_SCL_READ()) + { + i2c_Start(); /* 发送启动信号 */ + + /* 发送设备地址+读写控制bit（0 = w， 1 = r) bit7 先传 */ + i2c_SendByte(_Address | I2C_WR); + ucAck = i2c_WaitAck(); /* 检测设备的ACK应答 */ + + i2c_Stop(); /* 发送停止信号 */ + + return ucAck; + } + return 1; /* I2C总线异常 */ +} + +/* +********************************************************************************************************* +* 函 数 名: ee_CheckOk +* 功能说明: 判断串行EERPOM是否正常 +* 形 参: 无 +* 返 回 值: 1 表示正常， 0 表示不正常 +********************************************************************************************************* +*/ +uint8_t GF_BSP_EEPROM_CheckOK(void) +{ + if (i2c_CheckDevice(EE_DEV_ADDR) == 0) + { + return 1; + } + else + { + /* 失败后，切记发送I2C总线停止信号 */ + i2c_Stop(); + return 0; + } +} + + +/* +********************************************************************************************************* +* 函 数 名: GF_BSP_EEPROM_ReadBytes +* 功能说明: 从串行EEPROM指定地址处开始读取若干数据 +* 形 参: _usAddress : 起始地址 +* _usSize : 数据长度，单位为字节 +* _pReadBuf : 存放读到的数据的缓冲区指针 +* 返 回 值: 0 表示失败，1表示成功 +********************************************************************************************************* +*/ +uint8_t GF_BSP_EEPROM_ReadBytes(uint8_t *_pReadBuf, uint16_t _usAddress, uint16_t _usSize) +{ + uint16_t i; + + /* 采用串行EEPROM随即读取指令序列，连续读取若干字节 */ + + /* 第1步：发起I2C总线启动信号 */ + i2c_Start(); + + /* 第2步：发起控制字节，高7bit是地址，bit0是读写控制位，0表示写，1表示读 */ + i2c_SendByte(EE_DEV_ADDR | I2C_WR); /* 此处是写指令 */ + + /* 第3步：发送ACK */ + if (i2c_WaitAck() != 0) + { + goto cmd_fail; /* EEPROM器件无应答 */ + } + + /* 第4步：发送字节地址，24C02只有256字节，因此1个字节就够了，如果是24C04以上，那么此处需要连发多个地址 */ + if (EE_ADDR_BYTES == 1) + { + i2c_SendByte((uint8_t)_usAddress); + if (i2c_WaitAck() != 0) + { + goto cmd_fail; /* EEPROM器件无应答 */ + } + } + else + { + i2c_SendByte(_usAddress >> 8); + if (i2c_WaitAck() != 0) + { + goto cmd_fail; /* EEPROM器件无应答 */ + } + + i2c_SendByte(_usAddress&0xff); + if (i2c_WaitAck() != 0) + { + goto cmd_fail; /* EEPROM器件无应答 */ + } + } + + /* 第6步：重新启动I2C总线。下面开始读取数据 */ + i2c_Start(); + + /* 第7步：发起控制字节，高7bit是地址，bit0是读写控制位，0表示写，1表示读 */ + i2c_SendByte(EE_DEV_ADDR | I2C_RD); /* 此处是读指令 */ + + /* 第8步：发送ACK */ + if (i2c_WaitAck() != 0) + { + goto cmd_fail; /* EEPROM器件无应答 */ + } + + /* 第9步：循环读取数据 */ + for (i = 0; i < _usSize; i++) + { + _pReadBuf[i] = i2c_ReadByte(); /* 读1个字节 */ + + /* 每读完1个字节后，需要发送Ack， 最后一个字节不需要Ack，发Nack */ + if (i != _usSize - 1) + { + i2c_Ack(); /* 中间字节读完后，CPU产生ACK信号(驱动SDA = 0) */ + } + else + { + i2c_NAck(); /* 最后1个字节读完后，CPU产生NACK信号(驱动SDA = 1) */ + } + } + /* 发送I2C总线停止信号 */ + i2c_Stop(); + return 1; /* 执行成功 */ + +cmd_fail: /* 命令执行失败后，切记发送停止信号，避免影响I2C总线上其他设备 */ + /* 发送I2C总线停止信号 */ + i2c_Stop(); + return 0; +} + +/* +********************************************************************************************************* +* 函 数 名: GF_BSP_EEPROM_WriteBytes +* 功能说明: 向串行EEPROM指定地址写入若干数据，采用页写操作提高写入效率 +* 形 参: _usAddress : 起始地址 +* _usSize : 数据长度，单位为字节 +* _pWriteBuf : 存放读到的数据的缓冲区指针 +* 返 回 值: 0 表示失败，1表示成功 +********************************************************************************************************* +*/ +uint8_t GF_BSP_EEPROM_WriteBytes(uint8_t *_pWriteBuf, uint16_t _usAddress, uint16_t _usSize) +{ + uint16_t i,m; + uint16_t usAddr; + + /* + 写串行EEPROM不像读操作可以连续读取很多字节，每次写操作只能在同一个page。 + 对于24xx02，page size = 8 + 简单的处理方法为：按字节写操作模式，每写1个字节，都发送地址 + 为了提高连续写的效率: 本函数采用page wirte操作。 + */ + + usAddr = _usAddress; + for (i = 0; i < _usSize; i++) + { + /* 当发送第1个字节或是页面首地址时，需要重新发起启动信号和地址 */ + if ((i == 0) || (usAddr & (EE_PAGE_SIZE - 1)) == 0) + { + /*　第０步：发停止信号，启动内部写操作　*/ + i2c_Stop(); + + /* 通过检查器件应答的方式，判断内部写操作是否完成, 一般小于 10ms + CLK频率为200KHz时，查询次数为30次左右 + */ + for (m = 0; m < 1000; m++) + { + /* 第1步：发起I2C总线启动信号 */ + i2c_Start(); + + /* 第2步：发起控制字节，高7bit是地址，bit0是读写控制位，0表示写，1表示读 */ + i2c_SendByte(EE_DEV_ADDR | I2C_WR); /* 此处是写指令 */ + + /* 第3步：发送一个时钟，判断器件是否正确应答 */ + if (i2c_WaitAck() == 0) + { + break; + } + } + if (m == 1000) + { + goto cmd_fail; /* EEPROM器件写超时 */ + } + + /* 第4步：发送字节地址，24C02只有256字节，因此1个字节就够了，如果是24C04以上，那么此处需要连发多个地址 */ + if (EE_ADDR_BYTES == 1) + { + i2c_SendByte((uint8_t)usAddr); + if (i2c_WaitAck() != 0) + { + goto cmd_fail; /* EEPROM器件无应答 */ + } + } + else + { + i2c_SendByte(usAddr >> 8); + if (i2c_WaitAck() != 0) + { + goto cmd_fail; /* EEPROM器件无应答 */ + } + + i2c_SendByte(usAddr&0xff); + if (i2c_WaitAck() != 0) + { + goto cmd_fail; /* EEPROM器件无应答 */ + } + } + } + + /* 第6步：开始写入数据 */ + i2c_SendByte(_pWriteBuf[i]); + + /* 第7步：发送ACK */ + if (i2c_WaitAck() != 0) + { + goto cmd_fail; /* EEPROM器件无应答 */ + } + + usAddr++; /* 地址增1 */ + } + + /* 命令执行成功，发送I2C总线停止信号 */ + i2c_Stop(); + + /* 通过检查器件应答的方式，判断内部写操作是否完成, 一般小于 10ms + CLK频率为200KHz时，查询次数为30次左右 + */ + for (m = 0; m < 1000; m++) + { + /* 第1步：发起I2C总线启动信号 */ + i2c_Start(); + + /* 第2步：发起控制字节，高7bit是地址，bit0是读写控制位，0表示写，1表示读 */ + #if EE_ADDR_A8 == 1 + i2c_SendByte(EE_DEV_ADDR | I2C_WR | ((_usAddress >> 7) & 0x0E)); /* 此处是写指令 */ + #else + i2c_SendByte(EE_DEV_ADDR | I2C_WR); /* 此处是写指令 */ + #endif + + /* 第3步：发送一个时钟，判断器件是否正确应答 */ + if (i2c_WaitAck() == 0) + { + break; + } + } + if (m == 1000) + { + goto cmd_fail; /* EEPROM器件写超时 */ + } + + /* 命令执行成功，发送I2C总线停止信号 */ + i2c_Stop(); + return 1; + +cmd_fail: /* 命令执行失败后，切记发送停止信号，避免影响I2C总线上其他设备 */ + /* 发送I2C总线停止信号 */ + i2c_Stop(); + return 0; +} + diff --git a/bspMCU/include/bsp_EEPROM.h b/bspMCU/include/bsp_EEPROM.h new file mode 100644 index 0000000..07e434c --- /dev/null +++ b/bspMCU/include/bsp_EEPROM.h @@ -0,0 +1,52 @@ +/* + * bsp_EEPROM.h + * + * Created on: Oct 26, 2023 + * Author: shiya + */ + +#ifndef INC_BSP_EEPROM_H_ +#define INC_BSP_EEPROM_H_ + +#include "common.h" +#include "main.h" + +#define EEPROM_WP_Pin GPIO_PIN_11 +#define EEPROM_WP_GPIO_Port GPIOE +#define EEPROM_SCL_Pin GPIO_PIN_12 +#define EEPROM_SCL_GPIO_Port GPIOE +#define EEPROM_SDA_Pin GPIO_PIN_13 +#define EEPROM_SDA_GPIO_Port GPIOE + +#define AT24C512 +#ifdef AT24C512 +#define EE_MODEL_NAME "AT24C512" +#define EE_DEV_ADDR 0xA0 /* 设备地址 */ +#define EE_PAGE_SIZE 128 /* 页面大小(字节) */ +#define EE_SIZE (512*128) /* 总容量(字节) */ // not used in this progrm +#define EE_ADDR_BYTES 2 /* 地址字节个数 */ +#endif + +#define I2C_WR 0 /* 写控制bit */ +#define I2C_RD 1 /* 读控制bit */ + +////////////////////////////////////////////////////////////////// +//there are 12M eeprom in total, and the code download Address is App_Download_EEPROM_Addr +//512 Pages x 128 Bytes = 65536 Bytes = 512 kbits + + +#define GF_BSP_EEPROM_CV_struct_define_Start_Address 0 +#define IAP_struct_define_Start_Address 512 + +extern uint16_t App_Download_EEPROM_Addr; + +//返回值：：1-正常；0-错误 +uint8_t GF_BSP_EEPROM_Init(void); + +uint8_t GF_BSP_EEPROM_CheckOK(void); +uint8_t GF_BSP_EEPROM_ReadBytes(uint8_t *_pReadBuf, uint16_t _usAddress, + uint16_t _usSize); +uint8_t GF_BSP_EEPROM_WriteBytes(uint8_t *_pWriteBuf, uint16_t _usAddress, + uint16_t _usSize); + +#endif /* INC_BSP_EEPROM_H_ */ diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index aae4cd6..1c15eed 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/project/paint_robot_new/paint_robot_new.c @@ -52,7 +52,9 @@ PV_struct_define g_stPV; /*----------------------------------------------* * 宏定义 * *----------------------------------------------*/ - +#define JOY_STICKER_VALUE_ALLOWANCE 600 //摇杆行程死区 +#define JOY_STICKER_ANGLE_ALLOWANCE 30 //摇杆角度死区 + static void Custom_ModuleHandler(const Msg_t *pstMsg) { if (NULL == pstMsg) @@ -76,6 +78,35 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) { RD_MEMCPY(&g_stMK32, pstMsg->m_aucData, sizeof(g_stMK32)); } + + if (abs(g_stMK32.CH2_LY_V) <= JOY_STICKER_VALUE_ALLOWANCE && abs(g_stMK32.CH3_LY_H) <= JOY_STICKER_VALUE_ALLOWANCE) + { + MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0); + break; + } + + int angle = atan2(g_stMK32.CH2_LY_V, g_stMK32.CH3_LY_H) * 180 / M_PI; + + if (abs(angle - 90) <= JOY_STICKER_ANGLE_ALLOWANCE) + { + MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0); + } + else if (abs(angle - (-90)) <= JOY_STICKER_ANGLE_ALLOWANCE) + { + MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0); + } + else if (abs(angle - 0) <= JOY_STICKER_ANGLE_ALLOWANCE) + { + MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNRIGHT, NULL, 0); + } + else if (abs(angle - 180) <= JOY_STICKER_ANGLE_ALLOWANCE) + { + MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNLEFT, NULL, 0); + } + else + { + MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0); + } break; } case LUA_SHOW_INFO: @@ -98,8 +129,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) lua_print("CH14_LT \t%d\n", g_stMK32.CH14_LT); lua_print("CH15_RT \t%d\n", g_stMK32.CH15_RT); lua_print("PV Info\n"); - lua_print("RunMode = %d\nRobotSpeed = %d\nVertical_Calibration = %d\nIV_IsRestart_Notified = %d\nTimeStamp = %d\n", - g_stPV.RunMode, g_stPV.RobotSpeed, g_stPV.Vertical_Calibration, g_stPV.IV_IsRestart_Notified, g_stPV.TimeStamp); + lua_print("RunMode = %d\nRobotSpeed = %d\nLaneChangeDistance = %d\nVertical_Calibration = %d\nIV_IsRestart_Notified = %d\nTimeStamp = %d\n", + g_stPV.RunMode, g_stPV.RobotSpeed, g_stPV.LaneChangeDistance, g_stPV.Vertical_Calibration, g_stPV.IV_IsRestart_Notified, g_stPV.TimeStamp); break; } default: diff --git a/project/paint_robot_new/paint_robot_new_MK32.c b/project/paint_robot_new/paint_robot_new_MK32.c index 1d47d6f..263bcc9 100644 --- a/project/paint_robot_new/paint_robot_new_MK32.c +++ b/project/paint_robot_new/paint_robot_new_MK32.c @@ -101,42 +101,4 @@ void decode_MK32(const char *buf, uint32_t _iSize) RB_MK32.RxIndex++; MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_GET_MK32, (void *)&RB_MK32, sizeof(RB_MK32)); - - int angle; - angle = atan2(RB_MK32.CH1_RY_V, RB_MK32.CH0_RY_H) * 180 / M_PI; - - if(RB_MK32.CH5_SB == 0) - { - // 前进 - if((angle >= 45) && (angle <= 135)) - { - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0); - } - // 后退 - else if((angle >= -135) && (angle < -45)) - { - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0); - } - else - { - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0); - } - } - else - { - // 前进 - if((angle >= 45) && (angle <= 135)) - { - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0); - } - // 后退 - else if((angle >= -135) && (angle < -45)) - { - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0); - } - else - { - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0); - } - } }