From 0b481b7cb42b5bbb9dd3f17de8797fc209e7af3c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=A9=AC=E6=B1=9F=E6=B6=9B?= <444172940@qq.com> Date: Fri, 21 Jun 2024 10:48:59 +0000 Subject: [PATCH 1/9] 111 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: 马江涛 <444172940@qq.com> --- 1111.txt | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 1111.txt diff --git a/1111.txt b/1111.txt new file mode 100644 index 0000000..e69de29 -- Gitee From 2661111f4a1dcfee4f6fac4ab5370c27d14eaab8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=A9=AC=E6=B1=9F=E6=B6=9B?= <444172940@qq.com> Date: Fri, 21 Jun 2024 10:49:24 +0000 Subject: [PATCH 2/9] =?UTF-8?q?=E5=88=A0=E9=99=A4=E6=96=87=E4=BB=B6=201111?= =?UTF-8?q?.txt?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- 1111.txt | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 1111.txt diff --git a/1111.txt b/1111.txt deleted file mode 100644 index e69de29..0000000 -- Gitee From ea476e68dca9f6a7812c0054ff64c2d37ccf0cd0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=A9=AC=E6=B1=9F=E6=B6=9B?= <444172940@qq.com> Date: Fri, 21 Jun 2024 10:50:04 +0000 Subject: [PATCH 3/9] =?UTF-8?q?=E5=88=A0=E9=99=A4=E6=96=87=E4=BB=B6=20appl?= =?UTF-8?q?ication/user/generatecode/main.h?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/user/generatecode/main.h | 140 --------------------------- 1 file changed, 140 deletions(-) delete mode 100644 application/user/generatecode/main.h diff --git a/application/user/generatecode/main.h b/application/user/generatecode/main.h deleted file mode 100644 index 9127f56..0000000 --- a/application/user/generatecode/main.h +++ /dev/null @@ -1,140 +0,0 @@ -/** - * @copyright Copyright (c) 2022, HiSilicon (Shanghai) Technologies Co., Ltd. All rights reserved. - * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the - * following conditions are met: - * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following - * disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the - * following disclaimer in the documentation and/or other materials provided with the distribution. - * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote - * products derived from this software without specific prior written permission. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * @file main.h - * @author MCU Driver Team - * @brief This file contains driver init functions. - */ - -/* Define to prevent recursive inclusion ------------------------------------- */ -#ifndef McuMagicTag_SYSTEM_INIT_H -#define McuMagicTag_SYSTEM_INIT_H - -#include "adc.h" -#include "adc_ex.h" -#include "apt.h" -#include "uart.h" -#include "uart_ex.h" -#include "i2c.h" -#include "i2c_ex.h" -#include "can.h" -#include "qdm.h" -#include "gpio.h" -#include "gpt.h" -#include "gpt_ex.h" -#include "timer.h" -#include "timer_ex.h" -#include "crg.h" -#include "iocmg.h" - -#define IO_SPEED_FAST 0x00U -#define IO_SPEED_SLOW 0x01U - -#define IO_DRV_LEVEL4 0x00U -#define IO_DRV_LEVEL3 0x01U -#define IO_DRV_LEVEL2 0x02U -#define IO_DRV_LEVEL1 0x03U - -#define XTAL_DRV_LEVEL4 0x03U -#define XTAL_DRV_LEVEL3 0x02U -#define XTAL_DRV_LEVEL2 0x01U -#define XTAL_DRV_LEVEL1 0x00U - -#define MOTOR_TYPE_GEAR_HALL_45 0 -#define MOTOR_TYPE_GEAR_GMR_45 1 -#define MOTOR_TYPE_GEAR_HALL_90 2 -#define MOTOR_TYPE_GEAR_GMR_90 3 - -#define MOTOR_LINE_NUM_HALL_45 585 -#define MOTOR_LINE_NUM_GMR_45 22500 -#define MOTOR_LINE_NUM_HALL_90 1170 -#define MOTOR_LINE_NUM_GMR_90 45000 - -#define MOTOR_TIRE_DIAMETER 67.0 //轮胎直径67mm - -#define MOTOR_MAX_ANGULAR_SPEED 90.0 //最大角速度60度/s -#define MOTOR_MAX_SPEED_GEAR_90 650.0 //空载情况下减速比为1:90的电机，带直径为67mm的轮胎，最高速度650mm/s -#define MOTOR_MAX_SPEED_GEAR_45 1280.0 //空载情况下减速比为1:45的电机，带直径为67mm的轮胎，最高速度1280mm/s - -extern GPT_Handle g_gpt0; -extern GPT_Handle g_gpt1; -extern GPT_Handle g_gpt2; -extern GPT_Handle g_gpt3; -extern CAN_Handle g_can; -extern QDM_Handle g_qdm0; -extern QDM_Handle g_qdm1; -extern TIMER_Handle g_timer0; -extern UART_Handle g_uart0; -extern UART_Handle g_uart2; -extern UART_Handle g_uart3; -extern I2C_Handle g_i2c0; -extern I2C_Handle g_i2c1; -extern APT_Handle g_apt0; -extern APT_Handle g_apt1; -extern ADC_Handle g_adc0; - -extern GPIO_Handle g_gpio1; -extern GPIO_Handle g_gpio2; -extern GPIO_Handle g_gpio5; - -extern float g_motorMaxSpeed; //电机最大速度 -extern unsigned int g_curMotorType; //电机类型 -extern unsigned int g_motorLineNum; //电机编码器线数 - -extern unsigned int g_dataSendPeriod; //向上位机发送数据的周期 - -extern float g_KP; -extern float g_KI; -extern float g_KD; - -BASE_StatusType CRG_Config(CRG_CoreClkSelect *coreClkSelect); -void SystemInit(void); - -void UART2WriteInterruptCallback(void *handle); -void UART2ReadInterruptCallback(void *handle); -void UART3WriteInterruptCallback(void *handle); -void UART3ReadInterruptCallback(void *handle); - -void UART2InterruptErrorCallback(void *handle); -void UART3InterruptErrorCallback(void *handle); -void QDM1PTUCycleCallback(void *handle); -void QDM1SpeedLoseCallback(void *handle); -void QDM1ZIndexLockedCallback(void *handle); -void QDM1PositionCompareMatchCallback(void *handle); -void QDM1PositionCompareReadyCallback(void *handle); -void QDM1PositionCounterOverflowCallback(void *handle); -void QDM1PositionCounterUnderflowCallback(void *handle); -void QDM1QuadratureDirectionChangeCallback(void *handle); -void QDM1QuadraturePhaseErrorCallback(void *handle); -void QDM1PositionCounterErrorCallback(void *handle); -void TIMER0_InterruptProcess(void *handle); -void TIMER0_DMAOverFlow_InterruptProcess(void *handle); -void QDM0PTUCycleCallback(void *handle); -void QDM0SpeedLoseCallback(void *handle); -void QDM0ZIndexLockedCallback(void *handle); -void QDM0PositionCompareMatchCallback(void *handle); -void QDM0PositionCompareReadyCallback(void *handle); -void QDM0PositionCounterOverflowCallback(void *handle); -void QDM0PositionCounterUnderflowCallback(void *handle); -void QDM0QuadratureDirectionChangeCallback(void *handle); -void QDM0QuadraturePhaseErrorCallback(void *handle); -void QDM0PositionCounterErrorCallback(void *handle); - -void GPIO1_0_CallbackFunc(void *param); -void GPIO1_1_CallbackFunc(void *param); - -#endif /* McuMagicTag_SYSTEM_INIT_H */ \ No newline at end of file -- Gitee From 9034439f4b95097e6ba3b08581e31d0255beeb44 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=A9=AC=E6=B1=9F=E6=B6=9B?= <444172940@qq.com> Date: Fri, 21 Jun 2024 10:50:51 +0000 Subject: [PATCH 4/9] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E9=80=9F=E7=8E=87?= =?UTF-8?q?=E4=B8=BA=E6=AF=AB=E7=B1=B3=E6=AF=8F=E7=A7=92=E6=AF=AB=E8=A7=92?= =?UTF-8?q?=E9=80=9F=E5=BA=A6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: 马江涛 <444172940@qq.com> --- application/user/generatecode/main.h | 140 +++++++++++++++++++++++++++ 1 file changed, 140 insertions(+) create mode 100644 application/user/generatecode/main.h diff --git a/application/user/generatecode/main.h b/application/user/generatecode/main.h new file mode 100644 index 0000000..5fddda2 --- /dev/null +++ b/application/user/generatecode/main.h @@ -0,0 +1,140 @@ +/** + * @copyright Copyright (c) 2022, HiSilicon (Shanghai) Technologies Co., Ltd. All rights reserved. + * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + * following conditions are met: + * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + * disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + * following disclaimer in the documentation and/or other materials provided with the distribution. + * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + * products derived from this software without specific prior written permission. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE + * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * @file main.h + * @author MCU Driver Team + * @brief This file contains driver init functions. + */ + +/* Define to prevent recursive inclusion ------------------------------------- */ +#ifndef McuMagicTag_SYSTEM_INIT_H +#define McuMagicTag_SYSTEM_INIT_H + +#include "adc.h" +#include "adc_ex.h" +#include "apt.h" +#include "uart.h" +#include "uart_ex.h" +#include "i2c.h" +#include "i2c_ex.h" +#include "can.h" +#include "qdm.h" +#include "gpio.h" +#include "gpt.h" +#include "gpt_ex.h" +#include "timer.h" +#include "timer_ex.h" +#include "crg.h" +#include "iocmg.h" + +#define IO_SPEED_FAST 0x00U +#define IO_SPEED_SLOW 0x01U + +#define IO_DRV_LEVEL4 0x00U +#define IO_DRV_LEVEL3 0x01U +#define IO_DRV_LEVEL2 0x02U +#define IO_DRV_LEVEL1 0x03U + +#define XTAL_DRV_LEVEL4 0x03U +#define XTAL_DRV_LEVEL3 0x02U +#define XTAL_DRV_LEVEL2 0x01U +#define XTAL_DRV_LEVEL1 0x00U + +#define MOTOR_TYPE_GEAR_HALL_45 0 +#define MOTOR_TYPE_GEAR_GMR_45 1 +#define MOTOR_TYPE_GEAR_HALL_90 2 +#define MOTOR_TYPE_GEAR_GMR_90 3 + +#define MOTOR_LINE_NUM_HALL_45 585 +#define MOTOR_LINE_NUM_GMR_45 22500 +#define MOTOR_LINE_NUM_HALL_90 1170 +#define MOTOR_LINE_NUM_GMR_90 45000 + +#define MOTOR_TIRE_DIAMETER 67.0 //轮胎直径67mm + +#define MOTOR_MAX_ANGULAR_SPEED 100000.0 //最大角速度，单位mrad/s +#define MOTOR_MAX_SPEED_GEAR_90 650.0 //空载情况下减速比为1:90的电机，带直径为67mm的轮胎，最高速度650mm/s +#define MOTOR_MAX_SPEED_GEAR_45 1280.0 //空载情况下减速比为1:45的电机，带直径为67mm的轮胎，最高速度1280mm/s + +extern GPT_Handle g_gpt0; +extern GPT_Handle g_gpt1; +extern GPT_Handle g_gpt2; +extern GPT_Handle g_gpt3; +extern CAN_Handle g_can; +extern QDM_Handle g_qdm0; +extern QDM_Handle g_qdm1; +extern TIMER_Handle g_timer0; +extern UART_Handle g_uart0; +extern UART_Handle g_uart2; +extern UART_Handle g_uart3; +extern I2C_Handle g_i2c0; +extern I2C_Handle g_i2c1; +extern APT_Handle g_apt0; +extern APT_Handle g_apt1; +extern ADC_Handle g_adc0; + +extern GPIO_Handle g_gpio1; +extern GPIO_Handle g_gpio2; +extern GPIO_Handle g_gpio5; + +extern float g_motorMaxSpeed; //电机最大速度 +extern unsigned int g_curMotorType; //电机类型 +extern unsigned int g_motorLineNum; //电机编码器线数 + +extern unsigned int g_dataSendPeriod; //向上位机发送数据的周期 + +extern float g_KP; +extern float g_KI; +extern float g_KD; + +BASE_StatusType CRG_Config(CRG_CoreClkSelect *coreClkSelect); +void SystemInit(void); + +void UART2WriteInterruptCallback(void *handle); +void UART2ReadInterruptCallback(void *handle); +void UART3WriteInterruptCallback(void *handle); +void UART3ReadInterruptCallback(void *handle); + +void UART2InterruptErrorCallback(void *handle); +void UART3InterruptErrorCallback(void *handle); +void QDM1PTUCycleCallback(void *handle); +void QDM1SpeedLoseCallback(void *handle); +void QDM1ZIndexLockedCallback(void *handle); +void QDM1PositionCompareMatchCallback(void *handle); +void QDM1PositionCompareReadyCallback(void *handle); +void QDM1PositionCounterOverflowCallback(void *handle); +void QDM1PositionCounterUnderflowCallback(void *handle); +void QDM1QuadratureDirectionChangeCallback(void *handle); +void QDM1QuadraturePhaseErrorCallback(void *handle); +void QDM1PositionCounterErrorCallback(void *handle); +void TIMER0_InterruptProcess(void *handle); +void TIMER0_DMAOverFlow_InterruptProcess(void *handle); +void QDM0PTUCycleCallback(void *handle); +void QDM0SpeedLoseCallback(void *handle); +void QDM0ZIndexLockedCallback(void *handle); +void QDM0PositionCompareMatchCallback(void *handle); +void QDM0PositionCompareReadyCallback(void *handle); +void QDM0PositionCounterOverflowCallback(void *handle); +void QDM0PositionCounterUnderflowCallback(void *handle); +void QDM0QuadratureDirectionChangeCallback(void *handle); +void QDM0QuadraturePhaseErrorCallback(void *handle); +void QDM0PositionCounterErrorCallback(void *handle); + +void GPIO1_0_CallbackFunc(void *param); +void GPIO1_1_CallbackFunc(void *param); + +#endif /* McuMagicTag_SYSTEM_INIT_H */ \ No newline at end of file -- Gitee From f59ab9874f6249dd7e6b2d466355fa2eb813595c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=A9=AC=E6=B1=9F=E6=B6=9B?= <444172940@qq.com> Date: Fri, 21 Jun 2024 10:51:57 +0000 Subject: [PATCH 5/9] =?UTF-8?q?=E5=88=A0=E9=99=A4=E6=96=87=E4=BB=B6=20appl?= =?UTF-8?q?ication/user/eulercar=5Fcontrol.c?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/user/eulercar_control.c | 445 ---------------------------- 1 file changed, 445 deletions(-) delete mode 100644 application/user/eulercar_control.c diff --git a/application/user/eulercar_control.c b/application/user/eulercar_control.c deleted file mode 100644 index 87b02eb..0000000 --- a/application/user/eulercar_control.c +++ /dev/null @@ -1,445 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include "user_motor_control.h" -#include "eulercar_control.h" -#include "uart.h" -#include "pid.h" -#include "button.h" -#include "encoder.h" - -unsigned char g_MotorState; //电机状态 - -EulerCarSendData g_SendData; //串口发送数据 -EulerCarRecvData g_ReceiveData; //串口接收数据 -unsigned int g_dataSendPeriod = 20; //向上位机发送数据的周期 -static unsigned char g_ReceiveDataCount = 0; - -extern unsigned int g_pid_count; -extern PidEulerCar g_pidEulerCarRight; -extern PidEulerCar g_pidEulerCarLeft; - -//按键状态 -extern int g_button1State; -extern int g_button2State; - -//按键滤波计数器 -extern int g_button1_count; -extern int g_button2_count; - -//通过按键调整PID参数的步进参数 -extern float g_KP_Step; -extern float g_KI_Step; -extern float g_KD_Step; - -/* 接收完成标志 */ -static volatile bool g_RxInterruptflag = true; -/* 发送完成标志 */ -static volatile bool g_TxInterruptFlag = true; - -/* 定时器中断计数器 */ -unsigned int g_TimerInterruptCount = 0; - -/* 上报数据给上位机计数器 */ -static unsigned int g_SendCount = 0; - -/* 上位机下发命令计数器 */ -static unsigned int g_RecvCount = 0; - -//小车三轴目标运动速度，单位：m/s -float Move_X, Move_Y, Move_Z; - -#define EULERCAR_WHEEL_TRACK (0.12) //EulerCar小车两个轮子之间的轮距0.12m -float modelCalLeftSpeed = 0.0; //根据两轮差速小车模型计算出的左轮速度 -float modelCalRightSpeed = 0.0; //根据两轮差速小车模型计算出的右轮速度 - -#define IS_SUPPORT_PID 1 - -/* 串口发送的数据进行赋值 */ -static void data_transition(void) -{ - g_SendData.SensorStr.Frame_Header = FRAME_HEADER; //帧头 - g_SendData.SensorStr.Frame_Tail = FRAME_TAIL; //帧尾 - - //从各车轮当前速度求出三轴当前速度 - g_SendData.SensorStr.X_speed = 13; //小车x轴速度 - g_SendData.SensorStr.Y_speed = 0; - g_SendData.SensorStr.Z_speed = 15; //小车z轴速度 - - //加速度计三轴加速度 - g_SendData.SensorStr.Accelerometer.X_data = 1; //加速度计Y轴转换到ROS坐标X轴 - g_SendData.SensorStr.Accelerometer.Y_data = 1; //加速度计X轴转换到ROS坐标Y轴 - g_SendData.SensorStr.Accelerometer.Z_data = 1; //加速度计Z轴转换到ROS坐标Z轴 - - //角速度计三轴角速度 - g_SendData.SensorStr.Gyroscope.X_data = 2; //角速度计Y轴转换到ROS坐标X轴 - g_SendData.SensorStr.Gyroscope.Y_data = 2; //角速度计X轴转换到ROS坐标Y轴 - if (g_MotorState == 0) - //如果电机控制位使能状态，那么正常发送Z轴角速度 - g_SendData.SensorStr.Gyroscope.Z_data = 1; - else - //如果机器人是静止的（电机控制位失能），那么发送的Z轴角速度为0 - g_SendData.SensorStr.Gyroscope.Z_data = 0; - - //电池电压(这里将浮点数放大一千倍传输，相应的在接收端在接收到数据后也会缩小一千倍) - g_SendData.SensorStr.Power_Voltage = 90; - - g_SendData.buffer[0] = g_SendData.SensorStr.Frame_Header; // 帧头 - g_SendData.buffer[1] = g_MotorState; // 小车软件失能标志位 - - //小车三轴速度,各轴都拆分为两个8位数据再发送 - g_SendData.buffer[2] = g_SendData.SensorStr.X_speed >> 8; - g_SendData.buffer[3] = g_SendData.SensorStr.X_speed; - g_SendData.buffer[4] = g_SendData.SensorStr.Y_speed >> 8; - g_SendData.buffer[5] = g_SendData.SensorStr.Y_speed; - g_SendData.buffer[6] = g_SendData.SensorStr.Z_speed >> 8; - g_SendData.buffer[7] = g_SendData.SensorStr.Z_speed; - - //IMU加速度计三轴加速度,各轴都拆分为两个8位数据再发送 - g_SendData.buffer[8] = g_SendData.SensorStr.Accelerometer.X_data >> 8; - g_SendData.buffer[9] = g_SendData.SensorStr.Accelerometer.X_data; - g_SendData.buffer[10] = g_SendData.SensorStr.Accelerometer.Y_data >> 8; - g_SendData.buffer[11] = g_SendData.SensorStr.Accelerometer.Y_data; - g_SendData.buffer[12] = g_SendData.SensorStr.Accelerometer.Z_data >> 8; - g_SendData.buffer[13] = g_SendData.SensorStr.Accelerometer.Z_data; - - //IMU角速度计三轴角速度,各轴都拆分为两个8位数据再发送 - g_SendData.buffer[14] = g_SendData.SensorStr.Gyroscope.X_data >> 8; - g_SendData.buffer[15] = g_SendData.SensorStr.Gyroscope.X_data; - g_SendData.buffer[16] = g_SendData.SensorStr.Gyroscope.Y_data >> 8; - g_SendData.buffer[17] = g_SendData.SensorStr.Gyroscope.Y_data; - g_SendData.buffer[18] = g_SendData.SensorStr.Gyroscope.Z_data >> 8; - g_SendData.buffer[19] = g_SendData.SensorStr.Gyroscope.Z_data; - - //电池电压,拆分为两个8位数据发送 - g_SendData.buffer[20] = g_SendData.SensorStr.Power_Voltage >> 8; - g_SendData.buffer[21] = g_SendData.SensorStr.Power_Voltage; - - //数据校验位计算 - unsigned char check_sum = 0, k; - for (k = 0; k < 22; k++) { - check_sum = check_sum ^ g_SendData.buffer[k]; - } - g_SendData.buffer[22] = check_sum; - g_SendData.buffer[23] = g_SendData.SensorStr.Frame_Tail; //帧尾 -} - -void debug_send_buffer(unsigned int count) -{ - DBG_PRINTF("-----------------send_count=%u-----------------\r\n", count); - for (size_t i = 0; i < SEND_DATA_SIZE; i++) { - DBG_PRINTF("0x%x ", g_SendData.buffer[i]); - } - DBG_PRINTF(" \r\n"); -} - -void UART3WriteInterruptCallback(void *handle) -{ - BASE_FUNC_UNUSED(handle); - //DBG_PRINTF("\r\nUART Write Finish\r\n"); - g_TxInterruptFlag = true; - return; -} - -void UART3ReadInterruptCallback(void *handle) -{ - BASE_FUNC_UNUSED(handle); - g_RxInterruptflag = 1; - return; -} - -// 定时器10ms产生一个中断 -void TIMER0_InterruptProcess(void *handle) -{ - BASE_StatusType ret; - /* USER CODE BEGIN TIMER0_InterruptProcess */ - TIMER_Handle *timerHandle = (TIMER_Handle *)handle; - BASE_FUNC_UNUSED(timerHandle); - g_TimerInterruptCount++; - - //按键滤波，500毫秒内算一次按键 - if(g_button1_count > 0) - { - g_button1_count ++; - if(g_button1_count > 50){ - g_button1_count = 0; - //DBG_PRINTF("Button1 Count Reset 0\r\n"); - } - } - - if(g_button2_count > 0) - { - g_button2_count ++; - if(g_button2_count > 50){ - g_button2_count = 0; - //DBG_PRINTF("Button2 Count Reset 0\r\n"); - } - } - - //按设置周期上报底盘数据 - if ((g_TimerInterruptCount % g_dataSendPeriod) == 0) { - if (g_TxInterruptFlag) { - g_TxInterruptFlag = false; - //获取小车要发送的数据 - data_transition(); - ret = HAL_UART_WriteIT(&g_uart3, g_SendData.buffer, SEND_DATA_SIZE); - - if(ret == BASE_STATUS_OK) { - //DBG_PRINTF("Data send OK, send count = %d\r\n",g_SendCount); - }else{ - DBG_PRINTF("Data send error, code = %d\r\n",ret); - } - g_SendCount++; - } - } - - //每间隔10毫秒进行一次PID电机控制 - if (g_TimerInterruptCount % 1 == 0) { - //获取小车运行实际速度 - QDM_SampleM(&g_pidEulerCarRight.ActualSpeed, &g_pidEulerCarLeft.ActualSpeed); - - //如果上位机下发速度大于0.1mm/s，则启动PID控制 - if ((fabsf(modelCalLeftSpeed) > 0.1) || (fabsf(modelCalRightSpeed) > 0.1)) { - Pid_Process(); - } - } - -} - -static float XYZ_transition(unsigned char High, unsigned char Low) -{ - short transition = ((High << 8) + Low); //将高8位和低8位整合成一个16位的short型数据 - return transition / 1000 + (transition % 1000) * 0.001; //单位转换, mm/s->m/s -} - -int EulerCarSpeedCtrlLeft(float LeftSpeed) -{ - - unsigned int duty = (abs)((int)(LeftSpeed/g_motorMaxSpeed * 100)); - if (duty > 99) { - duty = 35; - } - //DBG_PRINTF("left wheel duty:%d\r\n", duty); - HAL_APT_SetPWMDutyByNumber(&g_apt1, duty); - - //如果电机小于0.1mm/s，则停止电机转动 - if (fabsf(LeftSpeed) < 0.1) { - HAL_APT_StopModule(RUN_APT1); - return 0; - } - if (LeftSpeed > 0) { - User_APTPwmARecovery(g_apt1.baseAddress); - User_APTForcePWMBOutputLow(g_apt1.baseAddress); - } else { - User_APTPwmBRecovery(g_apt1.baseAddress); - User_APTForcePWMAOutputLow(g_apt1.baseAddress); - } - HAL_APT_StartModule(RUN_APT1); - //DBG_PRINTF("Left wheel set duty:%d, run ok!\r\n",duty); - - return 0; -} - -int EulerCarSpeedCtrlRight(float rightSpeed) -{ - unsigned int duty = (abs)((int)(rightSpeed/g_motorMaxSpeed * 100)); - if (duty > 99) { - duty = 35; - } - HAL_APT_SetPWMDutyByNumber(&g_apt0, duty); - - //如果电机小于0.1mm/s，则停止电机转动 - if (fabsf(rightSpeed) < 0.1) { - HAL_APT_StopModule(RUN_APT0); - return 0; - } - if (rightSpeed > 0) { - User_APTPwmARecovery(g_apt0.baseAddress); - User_APTForcePWMBOutputLow(g_apt0.baseAddress); - } else { - User_APTPwmBRecovery(g_apt0.baseAddress); - User_APTForcePWMAOutputLow(g_apt0.baseAddress); - } - HAL_APT_StartModule(RUN_APT0); - //DBG_PRINTF("Right wheel set duty:%d, run ok!\r\n",duty); - - return 0; -} - -void Pid_Process(void) -{ - float pidTargetSpeed; - g_pid_count++; - - /* 通过PID算法计算右轮目标速度 */ - pidTargetSpeed = Pid_Ctrl(&g_pidEulerCarRight); - EulerCarSpeedCtrlRight(pidTargetSpeed); - - /* 通过PID算法计算左轮目标速度 */ - pidTargetSpeed = Pid_Ctrl(&g_pidEulerCarLeft); - EulerCarSpeedCtrlLeft(pidTargetSpeed); - - //左右轮同时打印 - if((g_TimerInterruptCount % 10) == 0) { - DBG_PRINTF("N=%d,P=%.02f,I=%.02f,D=%.02f|L-SS:%.1f,AS:%.1f,err:%.1f,nr:%.1f,lr:%.1f,in:%.1f,TS:%.1f,dt:%d|R-SS:%.1f,AS:%.1f,err:%.1f,nr:%.1f,lr:%.1f,in:%.f,TS:%.1f,dt:%d\r\n", - g_pid_count,g_KP,g_KI,g_KD,g_pidEulerCarRight.SetSpeed,g_pidEulerCarRight.ActualSpeed, - g_pidEulerCarRight.err,g_pidEulerCarRight.err_next,g_pidEulerCarRight.err_last,g_pidEulerCarRight.IncSpeed, - g_pidEulerCarRight.TargetIncSpeed,g_pidEulerCarRight.duty, - g_pidEulerCarLeft.SetSpeed,g_pidEulerCarLeft.ActualSpeed, - g_pidEulerCarLeft.err,g_pidEulerCarLeft.err_next,g_pidEulerCarLeft.err_last,g_pidEulerCarLeft.IncSpeed, - g_pidEulerCarLeft.TargetIncSpeed,g_pidEulerCarLeft.duty); - } - - /* - //打印电机运行情况 - if((g_TimerInterruptCount % 10) == 0) { - DBG_PRINTF("N=%d|L-SS:%.1f,AS:%.1f,err:%.1f,nr:%.1f,in:%.1f,TS:%.1f,dt:%d|R-SS:%.1f,AS:%.1f,err:%.1f,nr:%.1f,in:%.f,TS:%.1f,dt:%d\r\n", - g_pid_count,g_pidEulerCarRight.SetSpeed,g_pidEulerCarRight.ActualSpeed, - g_pidEulerCarRight.err,g_pidEulerCarRight.err_next,g_pidEulerCarRight.IncSpeed, - g_pidEulerCarRight.TargetIncSpeed,g_pidEulerCarRight.duty, - g_pidEulerCarLeft.SetSpeed,g_pidEulerCarLeft.ActualSpeed, - g_pidEulerCarLeft.err,g_pidEulerCarLeft.err_next,g_pidEulerCarLeft.IncSpeed, - g_pidEulerCarLeft.TargetIncSpeed,g_pidEulerCarLeft.duty); - } - - - //左右轮同时打印 - if((g_TimerInterruptCount % 10) == 0) { - DBG_PRINTF("N=%d,P=%.02f,I=%.02f,D=%.02f|L-SS:%.1f,AS:%.1f,err:%.1f,nr:%.1f,lr:%.1f,in:%.1f,TS:%.1f,dt:%d|R-SS:%.1f,AS:%.1f,err:%.1f,nr:%.1f,lr:%.1f,in:%.f,TS:%.1f,dt:%d\r\n", - g_pid_count,g_KP,g_KI,g_KD,g_pidEulerCarRight.SetSpeed,g_pidEulerCarRight.ActualSpeed, - g_pidEulerCarRight.err,g_pidEulerCarRight.err_next,g_pidEulerCarRight.err_last,g_pidEulerCarRight.IncSpeed, - g_pidEulerCarRight.TargetIncSpeed,g_pidEulerCarRight.duty, - g_pidEulerCarLeft.SetSpeed,g_pidEulerCarLeft.ActualSpeed, - g_pidEulerCarLeft.err,g_pidEulerCarLeft.err_next,g_pidEulerCarLeft.err_last,g_pidEulerCarLeft.IncSpeed, - g_pidEulerCarLeft.TargetIncSpeed,g_pidEulerCarLeft.duty); - } - - if((g_TimerInterruptCount % 10) == 0) { - DBG_PRINTF("N=%d,P=%.02f,I=%.02f,D=%.02f|L-SS:%.1f,RS:%.1f,err:%.1f,nr:%.1f,lr:%.1f,in:%.1f,TS:%.1f,dt:%d|LS:%.1f,err:%.1f,nr:%.1f,ir:%.1f%,in:%.f,TS:%.1f,dt:%d\r\n", - g_pid_count,g_KP,g_KI,g_KD,g_pidEulerCarRight.SetSpeed, - g_pidEulerCarRight.ActualSpeed,g_pidEulerCarRight.err,g_pidEulerCarRight.err_next,g_pidEulerCarRight.err_last,, - g_pidEulerCarRight.IncSpeed,g_pidEulerCarRight.TargetIncSpeed,g_pidEulerCarRight.duty, - g_pidEulerCarLeft.ActualSpeed,g_pidEulerCarLeft.err,g_pidEulerCarLeft.err_next,g_pidEulerCarLeft.i_err, - g_pidEulerCarLeft.IncrementSpeed,g_pidEulerCarLeft.TargetSpeed,g_pidEulerCarLeft.duty); - } - - //只打印右轮 - if((g_TimerInterruptCount % 10) == 0) { - DBG_PRINTF("N=%d,P=%.02f,I=%.02f,D=%.02f|R-SS:%.1f,AS:%.1f,err:%.1f,nr:%.1f,lr:%.1f,in:%.1f,TS:%.1f,dt:%d\r\n", - g_pid_count,g_KP,g_KI,g_KD,g_pidEulerCarRight.SetSpeed, - g_pidEulerCarRight.ActualSpeed,g_pidEulerCarRight.err,g_pidEulerCarRight.err_next,g_pidEulerCarRight.err_last, - g_pidEulerCarRight.IncSpeed,g_pidEulerCarRight.TargetIncSpeed,g_pidEulerCarRight.duty); - } - - //只打印左轮 - if((g_TimerInterruptCount % 100) == 0) { - DBG_PRINTF("N=%d,P=%.02f,I=%.02f,D=%.02f|L-SS:%.1f,AS:%.1f,err:%.1f,nr:%.1f,lr:%.1f,in:%.1f,TS:%.1f,dt:%d\r\n", - g_pid_count,g_KP,g_KI,g_KD,g_pidEulerCarLeft.SetSpeed, - g_pidEulerCarLeft.ActualSpeed,g_pidEulerCarLeft.err,g_pidEulerCarLeft.err_next,g_pidEulerCarLeft.err_last, - g_pidEulerCarLeft.IncSpeed,g_pidEulerCarLeft.TargetIncSpeed,g_pidEulerCarLeft.duty); - } */ -} - -void recv_data_cal(void) -{ - float x_linear,z_angular; - - g_ReceiveData.ControlStr.X_speed = Move_X; - g_ReceiveData.ControlStr.Y_speed = Move_Y; - g_ReceiveData.ControlStr.Z_speed = Move_Z; - x_linear = g_ReceiveData.ControlStr.X_speed; - z_angular = g_ReceiveData.ControlStr.Z_speed; - //DBG_PRINTF("x_speed:%.02f, Z_speed:%.02f\r\n", g_ReceiveData.ControlStr.X_speed, g_ReceiveData.ControlStr.Z_speed); - //差分轮运动学模型求解 - modelCalLeftSpeed = (x_linear - z_angular * EULERCAR_WHEEL_TRACK / 2.0) * 1000; - modelCalRightSpeed = (x_linear + z_angular * EULERCAR_WHEEL_TRACK / 2.0) * 1000; - DBG_PRINTF("modelCalLeftSpeed:%.1fmm/s, modelCalRightSpeed:%.1fmm/s\r\n", modelCalLeftSpeed, modelCalRightSpeed); - - //将上位机设置轮速存到PID控制结构体 - g_pidEulerCarLeft.SetSpeed = modelCalLeftSpeed; - g_pidEulerCarRight.SetSpeed = modelCalRightSpeed; - - //如果上位机下发速度小于0.1mm/s，小车停止运动 - if((fabsf(modelCalLeftSpeed) < 0.1) && (fabsf(modelCalRightSpeed) < 0.1)) - { - EulerCarSpeedCtrlRight(0); - EulerCarSpeedCtrlLeft(0); - DBG_PRINTF("EulerCar stoped!!!\r\n"); - } -} - -void UART3_INTRxSimultaneously(void) -{ - float tmp_X,tmp_Y,tmp_Z,maxLinearSpeed; - unsigned char k,Usart_Receive,check_sum; - - while (1) { - if (g_RxInterruptflag) { - g_RxInterruptflag = false; - HAL_UART_ReadIT(&g_uart3, &Usart_Receive, 1); // 读取数据 - g_ReceiveData.buffer[g_ReceiveDataCount] = Usart_Receive; - //确保数组第一个数据为FRAME_HEADER - if (Usart_Receive == FRAME_HEADER || g_ReceiveDataCount > 0) { - g_ReceiveDataCount++; - } - else { - g_ReceiveDataCount = 0; - } - // 验证数据包的长度 - if (g_ReceiveDataCount == RECEIVE_DATA_SIZE) { - g_ReceiveDataCount = 0; //为串口数据重新填入数组做准备 - //验证数据包的帧尾 - if (g_ReceiveData.buffer[10] == FRAME_TAIL) { - check_sum = 0; - for (k = 0; k < 9; k++) { - check_sum = check_sum ^ g_ReceiveData.buffer[k]; - } - //数据异或位校验计算，模式0是发送数据校验 - if (g_ReceiveData.buffer[9] == check_sum) { - g_RecvCount++; - //从串口数据求三轴目标速度， 单位m/s - tmp_X = XYZ_transition(g_ReceiveData.buffer[3], g_ReceiveData.buffer[4]); - tmp_Y = XYZ_transition(g_ReceiveData.buffer[5], g_ReceiveData.buffer[6]); - tmp_Z = XYZ_transition(g_ReceiveData.buffer[7], g_ReceiveData.buffer[8]); - maxLinearSpeed = g_motorMaxSpeed/1000.0; - //合法性检查，设置速度必须小于当前电机支持的最高速度 - if( (fabsf(tmp_X) < maxLinearSpeed) && (fabsf(tmp_Y) < maxLinearSpeed) && (fabsf(tmp_Z) < MOTOR_MAX_ANGULAR_SPEED) ) { - Move_X = tmp_X; - Move_Y = tmp_Y; - Move_Z = tmp_Z; - DBG_PRINTF("\r\ng_TimerCount=%d g_RecvCount=%d, Move_X=%.2fm/s Move_Y=%.2fm/s Move_Z=%.2frad/s\r\n", - g_TimerInterruptCount,g_RecvCount, Move_X, Move_Y, Move_Z); - recv_data_cal(); - } - else{ - DBG_PRINTF("\r\ng_TimerCount=%d g_RecvCount=%d",g_TimerInterruptCount,g_RecvCount); - if((fabsf(tmp_X) > maxLinearSpeed) || (fabsf(tmp_Y) > maxLinearSpeed)) - DBG_PRINTF("\r\nLinear speed set error: X=%.2fm/s Y=%.2fm/s, must less %.2fm/s\r\n", tmp_X, tmp_Y,maxLinearSpeed); - if((fabsf(tmp_Z) > MOTOR_MAX_ANGULAR_SPEED)) - DBG_PRINTF("\r\nAngular speed set error: Z=%.2frad/s, must less %.1frad/s\r\n", tmp_Z, tmp_Y,MOTOR_MAX_ANGULAR_SPEED); - } - } - } - } - } - - //检测按键是否按下，修改PID参数 - if (g_button1State == 1) { - g_button1State = 0; - g_KP = g_KP + g_KP_Step; - DBG_PRINTF("Button1 Evnet, g_KP + %f\r\n",g_KP_Step); - DBG_PRINTF("g_KP=%f g_KI=%f g_KD=%f\r\n", g_KP, g_KI, g_KD); - } - - if (g_button2State == 1) { - g_button2State = 0; - g_KI = g_KI + g_KI_Step; - DBG_PRINTF("Button2 Evnet, g_KI + %f\r\n",g_KI_Step); - DBG_PRINTF("g_KP=%f g_KI=%f g_KD=%f\r\n", g_KP, g_KI, g_KD); - } - } -} \ No newline at end of file -- Gitee From d31d3504895db1b481657b0d965ed1dcb3e12165 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=A9=AC=E6=B1=9F=E6=B6=9B?= <444172940@qq.com> Date: Fri, 21 Jun 2024 10:52:13 +0000 Subject: [PATCH 6/9] =?UTF-8?q?=E5=88=A0=E9=99=A4=E6=96=87=E4=BB=B6=20appl?= =?UTF-8?q?ication/user/encoder.c?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/user/encoder.c | 122 ------------------------------------- 1 file changed, 122 deletions(-) delete mode 100644 application/user/encoder.c diff --git a/application/user/encoder.c b/application/user/encoder.c deleted file mode 100644 index 1713ef6..0000000 --- a/application/user/encoder.c +++ /dev/null @@ -1,122 +0,0 @@ -#include "qdm.h" -#include "debug.h" -#include "encoder.h" -#include "main.h" -#include "eulercar_control.h" - - -//系统10ms中断计数器 -extern unsigned int g_TimerInterruptCount; - -/** - * @brief QDM sample use M method - * @param none. - * @retval none. - */ -Gear_Motor_handle RightMotor; -Gear_Motor_handle LeftMotor; - -//编码器读取周期内最大差值 -unsigned int g_maxEncodeDeltaValue; - -void InitGearMotor(void) -{ - RightMotor.curNumber = 0; /* 当前计数器 */ - RightMotor.lastNumber = 0; /* 上一次计数器 */ - RightMotor.speedRps = 0; /* 电机转速，单位每秒多少圈 */ - RightMotor.speed = 0; /* 电机速度，单位mm/s，由转速和轮胎直径计算 */ - - LeftMotor.curNumber = 0; /* 当前计数器 */ - LeftMotor.lastNumber = 0; /* 上一次计数器 */ - LeftMotor.speedRps = 0; /* 电机转速，单位每秒多少圈 */ - LeftMotor.speed = 0; /* 电机速度，单位mm/s，由转速和轮胎直径计算 */ - - //编码器10ms周期内最大差值，电机转速的2倍，主要用于容错处理 - g_maxEncodeDeltaValue = (int)(g_motorMaxSpeed / MOTOR_TIRE_DIAMETER / PI * g_motorLineNum / 100.0 * 2.0); - //DBG_PRINTF("g_maxEncodeDeltaValue = %d\r\n", g_maxEncodeDeltaValue); - -} - -void QDM_SampleM(float *rightSpeed, float *leftSpeed) -{ - unsigned int qdm0_cnt, qdm0_dir; - unsigned int qdm1_cnt, qdm1_dir; - unsigned int deltaValue; - HAL_QDM_ReadPosCountAndDir(&g_qdm0, &qdm0_cnt, &qdm0_dir); - HAL_QDM_ReadPosCountAndDir(&g_qdm1, &qdm1_cnt, &qdm1_dir); - - /* 计算右轮速度 */ - RightMotor.curNumber = qdm0_cnt; - if(qdm0_dir == 1){ //电机正转 - if(RightMotor.curNumber >= RightMotor.lastNumber) { - deltaValue = RightMotor.curNumber - RightMotor.lastNumber; - }else{ //过零点后的计算编码器差值 - deltaValue = (RightMotor.curNumber +g_motorLineNum) - RightMotor.lastNumber; - } - }else{ //电机反转 - if(RightMotor.curNumber <= RightMotor.lastNumber) { - deltaValue = RightMotor.lastNumber - RightMotor.curNumber; - }else{ //过零点后的计算编码器差值 - deltaValue = (RightMotor.lastNumber + g_motorLineNum) - RightMotor.curNumber ; - } - } - - /* 电机正反转切换时，会出现编码器差值计算错误，设置编码器差值为0 */ - if(deltaValue > g_maxEncodeDeltaValue) { - DBG_PRINTF("===Right Motor Encode Delta Value Error = %d, set to zero.===\r\n", deltaValue); - deltaValue = 0; - } - - /* 计算右轮电机每秒转速 */ - RightMotor.speedRps = ((float)deltaValue * (1000 / MOTOR_PID_CONTROL_PERIOD))/g_motorLineNum; - RightMotor.speed = RightMotor.speedRps * MOTOR_TIRE_DIAMETER * PI; - RightMotor.lastNumber = RightMotor.curNumber; - RightMotor.deltaValue = deltaValue; - if(qdm0_dir == 0){ //电机反转 - RightMotor.speed = -RightMotor.speed; - } - - /* 计算左轮速度 */ - LeftMotor.curNumber = qdm1_cnt; - if(qdm1_dir == 1){ //电机正转 - if(LeftMotor.curNumber >= LeftMotor.lastNumber) { - deltaValue = LeftMotor.curNumber - LeftMotor.lastNumber; - }else{ //过零点后的计算编码器差值 - deltaValue = (LeftMotor.curNumber + g_motorLineNum) - LeftMotor.lastNumber; - } - }else{ //电机反转 - if(LeftMotor.curNumber <= LeftMotor.lastNumber) { - deltaValue = LeftMotor.lastNumber - LeftMotor.curNumber; - }else{ //过零点后的计算编码器差值 - deltaValue = (LeftMotor.lastNumber + g_motorLineNum) - LeftMotor.curNumber ; - } - } - - /* 电机正反转切换时，会出现编码器差值计算错误，设置编码器差值为0 */ - if(deltaValue > g_maxEncodeDeltaValue) { - DBG_PRINTF("===Lift Motor Encode Delta Value Error = %d, set to zero===\r\n", deltaValue); - deltaValue = 0; - } - - LeftMotor.speedRps = ((float)deltaValue * (1000 / MOTOR_PID_CONTROL_PERIOD))/g_motorLineNum; - LeftMotor.speed = LeftMotor.speedRps * MOTOR_TIRE_DIAMETER * PI; - LeftMotor.lastNumber = LeftMotor.curNumber; - LeftMotor.deltaValue = deltaValue; - if(qdm1_dir == 0){ //电机反向 - LeftMotor.speed = -LeftMotor.speed; - } - - *rightSpeed = RightMotor.speed; - *leftSpeed = LeftMotor.speed; - - /* 每10ms打印一次编码器实际数值和计算出来的速度 - if((g_TimerInterruptCount % 1) == 0) { - if((tmpd0 != 0) || (tmpd1 != 0) ) { //编码器数值有变化才进行打印 - DBG_PRINTF("T:%d|q0c:%d,d0:%d,dr:%d,crs:%.02f,rs:%.01f|q1d:%d,d1:%d,dr:%d,cls:%.02f,ls:%.01f\r\n", - g_TimerInterruptCount, - qdm0_cnt,RightMotor.deltaValue,qdm0_dir,RightMotor.speedRps,*rightSpeed, - qdm1_cnt,LeftMotor.deltaValue,qdm1_dir,LeftMotor.speedRps,*leftSpeed); - } - }*/ - -} \ No newline at end of file -- Gitee From 787fda0942c7d0d785ffea0c178a2bd19f5adcd6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=A9=AC=E6=B1=9F=E6=B6=9B?= <444172940@qq.com> Date: Fri, 21 Jun 2024 10:52:27 +0000 Subject: [PATCH 7/9] =?UTF-8?q?=E5=88=A0=E9=99=A4=E6=96=87=E4=BB=B6=20appl?= =?UTF-8?q?ication/user/encoder.h?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/user/encoder.h | 20 -------------------- 1 file changed, 20 deletions(-) delete mode 100644 application/user/encoder.h diff --git a/application/user/encoder.h b/application/user/encoder.h deleted file mode 100644 index 02b367e..0000000 --- a/application/user/encoder.h +++ /dev/null @@ -1,20 +0,0 @@ -#ifndef ENCODER_H -#define ENCODER_H - -#define PI 3.1415927 - -#define MOTOR_PID_CONTROL_PERIOD 10 //编码器读取周期，单位ms - -typedef struct _Gear_Motor_handle { - unsigned int curNumber; /* 当前计数器 */ - unsigned int lastNumber; /* 上一次计数器 */ - float speedRps; /* 电机转速，单位每秒多少圈 */ - float speed; /* 电机速度，单位mm/s，由转速和轮胎直径计算 */ - unsigned int deltaValue; /* 编码器差值 */ -} Gear_Motor_handle; - - -void InitGearMotor(void); -void QDM_SampleM(float *rightSpeed, float *leftSpeed); - -#endif \ No newline at end of file -- Gitee From 6142982a3dbe3a3c55dcad92a7f01ee7b7f704eb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=A9=AC=E6=B1=9F=E6=B6=9B?= <444172940@qq.com> Date: Fri, 21 Jun 2024 10:52:46 +0000 Subject: [PATCH 8/9] =?UTF-8?q?=E5=88=A0=E9=99=A4=E6=96=87=E4=BB=B6=20appl?= =?UTF-8?q?ication/user/main.c?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/user/main.c | 129 ---------------------------------------- 1 file changed, 129 deletions(-) delete mode 100644 application/user/main.c diff --git a/application/user/main.c b/application/user/main.c deleted file mode 100644 index 95e22c9..0000000 --- a/application/user/main.c +++ /dev/null @@ -1,129 +0,0 @@ -/** - * @copyright Copyright (c) 2022, HiSilicon (Shanghai) Technologies Co., Ltd. All rights reserved. - * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the - * following conditions are met: - * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following - * disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the - * following disclaimer in the documentation and/or other materials provided with the distribution. - * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote - * products derived from this software without specific prior written permission. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * @file main.c - * @author MCU Driver Team - * @brief Main program body. - */ - -#include "typedefs.h" -#include "feature.h" -#include "main.h" -/* USER CODE BEGIN 0 */ -/* USER CODE 区域内代码不会被覆盖，区域外会被生成的默认代码覆盖（其余USER CODE 区域同理） */ -#include "debug.h" -#include "ssd1306.h" -#include "ssd1306_fonts.h" -#include "uart.h" -#include "user_motor_control.h" -#include "button.h" -#include "adc_demo.h" -#include "beep.h" -#include "gyro.h" -#include "encoder.h" -#include "eulercar_control.h" -#include "debug.h" -#include "pid.h" -/* 建议用户放置头文件 */ -/* USER CODE END 0 */ - -/* USER CODE BEGIN 1 */ -CAN_Handle g_can; -QDM_Handle g_qdm0; -QDM_Handle g_qdm1; -GPT_Handle g_gpt0; -GPT_Handle g_gpt1; -GPT_Handle g_gpt2; -GPT_Handle g_gpt3; -TIMER_Handle g_timer0; -UART_Handle g_uart0; -UART_Handle g_uart2; -UART_Handle g_uart3; -I2C_Handle g_i2c0; -I2C_Handle g_i2c1; -APT_Handle g_apt0; -APT_Handle g_apt1; -ADC_Handle g_adc0; -GPIO_Handle g_gpio1; -GPIO_Handle g_gpio2; -GPIO_Handle g_gpio5; - -char g_motorTypeStr[4][20] = { - "1:45 HALL", - "1:45 GMR", - "1:90 HALL", - "1:90 GMR", -}; - -/* -//减速比为1:90的霍尔传感器电机 -float g_motorMaxSpeed = MOTOR_MAX_SPEED_GEAR_90; -unsigned int g_curMotorType = MOTOR_TYPE_GEAR_HALL_90; -unsigned int g_motorLineNum = MOTOR_LINE_NUM_HALL_90; - -//减速比为1:90的GMR传感器电机 -float g_motorMaxSpeed = MOTOR_MAX_SPEED_GEAR_90; -unsigned int g_curMotorType = MOTOR_TYPE_GEAR_GMR_90; -unsigned int g_motorLineNum = MOTOR_LINE_NUM_GMR_90; - -//减速比为1:45的霍尔传感器电机 -float g_motorMaxSpeed = MOTOR_MAX_SPEED_GEAR_45; -unsigned int g_curMotorType = MOTOR_TYPE_GEAR_HALL_45; -unsigned int g_motorLineNum = MOTOR_LINE_NUM_HALL_45; - -//减速比为1:45的GMR传感器电机 -float g_motorMaxSpeed = MOTOR_MAX_SPEED_GEAR_45; -unsigned int g_curMotorType = MOTOR_TYPE_GEAR_GMR_45; -unsigned int g_motorLineNum = MOTOR_LINE_NUM_GMR_45; -*/ - -//减速比为1:90的GMR传感器电机 -float g_motorMaxSpeed = MOTOR_MAX_SPEED_GEAR_45; -unsigned int g_curMotorType = MOTOR_TYPE_GEAR_GMR_45; -unsigned int g_motorLineNum = MOTOR_LINE_NUM_GMR_45; - -/* 建议用户定义全局变量、结构体、宏定义或函数声明等 */ -/* USER CODE END 1 */ - -int main(void) -{ - SystemInit(); - InitGearMotor(); - Pid_Init(); - DBG_PRINTF("==============================================================\r\n"); - DBG_PRINTF(" • EulerCar Controller 1.0 • \r\n"); - DBG_PRINTF(" \r\n"); - DBG_PRINTF(" ➤ System Information: \r\n"); - DBG_PRINTF(" • Motor Type:%s\r\n",g_motorTypeStr[g_curMotorType]); - DBG_PRINTF(" • Motor Encode Line Number:%05d\r\n",g_motorLineNum); - DBG_PRINTF(" • Motor Max Speed:%.02fmm/s\r\n", g_motorMaxSpeed); - DBG_PRINTF(" • EulerCar Data Send Period:%dms,%dHZ\r\n", g_dataSendPeriod*10, 100/g_dataSendPeriod); - DBG_PRINTF(" \r\n"); - DBG_PRINTF(" ➤ PID Information: \r\n"); - DBG_PRINTF(" • KP:%.02f • KI:%.02f • KD:%.02f\r\n",g_KP,g_KI,g_KD); - DBG_PRINTF(" • PID Control Period:%dms\r\n",MOTOR_PID_CONTROL_PERIOD); - DBG_PRINTF("==============================================================\r\n"); - DBG_PRINTF("EulerCar MCU init success!!!\r\n"); - HAL_TIMER_Start(&g_timer0); - DBG_PRINTF("TIMER start\r\n"); - InitButtonFunction(); - - UART3_INTRxSimultaneously(); - - /* USER CODE END 5 */ - return BASE_STATUS_OK; -} \ No newline at end of file -- Gitee From 1ddd73d0a187f87af6b24480654f49e054003f01 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=A9=AC=E6=B1=9F=E6=B6=9B?= <444172940@qq.com> Date: Fri, 21 Jun 2024 10:53:01 +0000 Subject: [PATCH 9/9] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E9=80=9F=E7=8E=87?= =?UTF-8?q?=E4=B8=BA=E6=AF=AB=E7=B1=B3=E6=AF=8F=E7=A7=92=E6=AF=AB=E8=A7=92?= =?UTF-8?q?=E9=80=9F=E5=BA=A6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: 马江涛 <444172940@qq.com> --- encoder.c | 129 +++++++++++++ encoder.h | 26 +++ eulercar_control.c | 455 +++++++++++++++++++++++++++++++++++++++++++++ main.c | 129 +++++++++++++ 4 files changed, 739 insertions(+) create mode 100644 encoder.c create mode 100644 encoder.h create mode 100644 eulercar_control.c create mode 100644 main.c diff --git a/encoder.c b/encoder.c new file mode 100644 index 0000000..2ff7eac --- /dev/null +++ b/encoder.c @@ -0,0 +1,129 @@ +#include "qdm.h" +#include "debug.h" +#include "encoder.h" +#include "main.h" +#include "eulercar_control.h" + + +//系统10ms中断计数器 +extern unsigned int g_TimerInterruptCount; + +/** + * @brief QDM sample use M method + * @param none. + * @retval none. + */ + +//用于pid算法的速度，即MOTOR_PID_CONTROL_PERIOD周期内的速度 +Gear_Motor_handle pidRightMotor; +Gear_Motor_handle pidLeftMotor; + +//用于ODOM里程计的平均速度，即EULER_CAR_DATA_SEND_PERIOD周期内的速度 +Gear_Motor_handle aveRightMotor; +Gear_Motor_handle aveLeftMotor; + +//编码器读取周期内最大差值 +unsigned int g_maxEncodePidDeltaValue; +unsigned int g_maxEncodeAveDeltaValue; + +void InitGearMotor(void) +{ + pidRightMotor.motorSide = MOTOR_RIGHT; + pidRightMotor.curNumber = 0; /* 当前计数器 */ + pidRightMotor.lastNumber = 0; /* 上一次计数器 */ + pidRightMotor.speedRps = 0; /* 电机转速，单位每秒多少圈 */ + pidRightMotor.speed = 0; /* 电机速度，单位mm/s，由转速和轮胎直径计算 */ + pidRightMotor.calPeriod = MOTOR_PID_CONTROL_PERIOD; + + pidLeftMotor.motorSide = MOTOR_LEFT; + pidLeftMotor.curNumber = 0; /* 当前计数器 */ + pidLeftMotor.lastNumber = 0; /* 上一次计数器 */ + pidLeftMotor.speedRps = 0; /* 电机转速，单位每秒多少圈 */ + pidLeftMotor.speed = 0; /* 电机速度，单位mm/s，由转速和轮胎直径计算 */ + pidLeftMotor.calPeriod = MOTOR_PID_CONTROL_PERIOD; + + + aveRightMotor.motorSide = MOTOR_RIGHT; + aveRightMotor.curNumber = 0; /* 当前计数器 */ + aveRightMotor.lastNumber = 0; /* 上一次计数器 */ + aveRightMotor.speedRps = 0; /* 电机转速，单位每秒多少圈 */ + aveRightMotor.speed = 0; /* 电机速度，单位mm/s，由转速和轮胎直径计算 */ + aveRightMotor.calPeriod = EULER_CAR_DATA_SEND_PERIOD; + + aveLeftMotor.motorSide = MOTOR_LEFT; + aveLeftMotor.curNumber = 0; /* 当前计数器 */ + aveLeftMotor.lastNumber = 0; /* 上一次计数器 */ + aveLeftMotor.speedRps = 0; /* 电机转速，单位每秒多少圈 */ + aveLeftMotor.speed = 0; /* 电机速度，单位mm/s，由转速和轮胎直径计算 */ + aveLeftMotor.calPeriod = EULER_CAR_DATA_SEND_PERIOD; + + + //编码器在一个PID周期内最大差值，电机转速的2倍，主要用于容错处理 + g_maxEncodePidDeltaValue = (int)(g_motorMaxSpeed / MOTOR_TIRE_DIAMETER / PI * g_motorLineNum / (1000.0/MOTOR_PID_CONTROL_PERIOD) * 2.0); + //编码器在一个底盘数据上报周期内最大差值，电机转速的2倍，主要用于容错处理 + g_maxEncodeAveDeltaValue = (int)(g_motorMaxSpeed / MOTOR_TIRE_DIAMETER / PI * g_motorLineNum / (1000.0/EULER_CAR_DATA_SEND_PERIOD) * 2.0); + + //DBG_PRINTF("g_maxEncodeDeltaValue = %d\r\n", g_maxEncodeDeltaValue); +} + + + +void QDM_CalMotorSpeed(Gear_Motor_handle *pMotor) +{ + unsigned int qdm_cnt, qdm_dir; + unsigned int deltaValue; + if(pMotor->motorSide == MOTOR_RIGHT ) + HAL_QDM_ReadPosCountAndDir(&g_qdm0, &qdm_cnt, &qdm_dir); + else + HAL_QDM_ReadPosCountAndDir(&g_qdm1, &qdm_cnt, &qdm_dir); + + /* 计算车轮速度 */ + pMotor->curNumber = qdm_cnt; + if(qdm_dir == 1){ //电机正转 + if(pMotor->curNumber >= pMotor->lastNumber) { + deltaValue = pMotor->curNumber - pMotor->lastNumber; + }else{ //过零点后的计算编码器差值 + deltaValue = (pMotor->curNumber +g_motorLineNum) - pMotor->lastNumber; + } + }else{ //电机反转 + if(pMotor->curNumber <= pMotor->lastNumber) { + deltaValue = pMotor->lastNumber - pMotor->curNumber; + }else{ //过零点后的计算编码器差值 + deltaValue = (pMotor->lastNumber + g_motorLineNum) - pMotor->curNumber ; + } + } + + /* 电机正反转切换时，会出现编码器差值计算错误，设置编码器差值为0 */ + if(pMotor->calPeriod == MOTOR_PID_CONTROL_PERIOD){ + if(deltaValue > g_maxEncodePidDeltaValue) { + DBG_PRINTF("===Motor Encode Error = %d, motor side:%d, calculate period %dms===\r\n", deltaValue,pMotor->motorSide,pMotor->calPeriod); + deltaValue = 0; + } + }else{ + if(deltaValue > g_maxEncodeAveDeltaValue) { + DBG_PRINTF("===Motor Encode Error = %d, motor side:%d, calculate period %dms===\r\n", deltaValue,pMotor->motorSide,pMotor->calPeriod); + deltaValue = 0; + } + } + + + /* 计算右轮电机每秒转速 */ + pMotor->speedRps = ((float)deltaValue * (1000.0 / (float)pMotor->calPeriod))/g_motorLineNum; + pMotor->speed = pMotor->speedRps * MOTOR_TIRE_DIAMETER * PI; + pMotor->lastNumber = pMotor->curNumber; + pMotor->deltaValue = deltaValue; + if(qdm_dir == 0){ //电机反转 + pMotor->speed = -pMotor->speed; + } + + /*每10ms打印一次编码器实际数值和计算出来的速度 + if(pMotor->motorSide == MOTOR_LEFT){ + if((g_TimerInterruptCount % 100) == 0) { + if((deltaValue != 0) ) { //编码器数值有变化才进行打印 + DBG_PRINTF("T:%d|qc:%d,d:%d,dr:%d,crs:%.02f,rs:%.01f\r\n", + g_TimerInterruptCount,qdm_cnt,pMotor->deltaValue,qdm_dir,pMotor->speedRps,pMotor->speed); + } + } + }*/ + +} diff --git a/encoder.h b/encoder.h new file mode 100644 index 0000000..0ee1864 --- /dev/null +++ b/encoder.h @@ -0,0 +1,26 @@ +#ifndef ENCODER_H +#define ENCODER_H + +#define PI 3.1415927 + +#define MOTOR_LEFT 0 +#define MOTOR_RIGHT 1 + +#define MOTOR_PID_CONTROL_PERIOD 10 //编码器读取周期，单位ms +#define EULER_CAR_DATA_SEND_PERIOD 200 //底盘数据上报上位机的周期，单位ms + +typedef struct _Gear_Motor_handle { + unsigned int motorSide; /* 当前是那个轮子*/ + unsigned int curNumber; /* 当前计数器 */ + unsigned int lastNumber; /* 上一次计数器 */ + float speedRps; /* 电机转速，单位每秒多少圈 */ + float speed; /* 电机速度，单位mm/s，由转速和轮胎直径计算 */ + unsigned int calPeriod; /* 轮速计算周期, 单位ms */ + unsigned int deltaValue; /* 编码器差值 */ +} Gear_Motor_handle; + + +void InitGearMotor(void); +void QDM_CalMotorSpeed(Gear_Motor_handle *pMotor); + +#endif \ No newline at end of file diff --git a/eulercar_control.c b/eulercar_control.c new file mode 100644 index 0000000..fac2b70 --- /dev/null +++ b/eulercar_control.c @@ -0,0 +1,455 @@ +#include +#include +#include +#include +#include +#include +#include +#include "user_motor_control.h" +#include "eulercar_control.h" +#include "uart.h" +#include "pid.h" +#include "button.h" +#include "encoder.h" + +unsigned char g_MotorState; //电机状态 + +EulerCarSendData g_SendData; //串口发送数据 +EulerCarRecvData g_ReceiveData; //串口接收数据 +static unsigned char g_ReceiveDataCount = 0; + +extern unsigned int g_pid_count; +extern PidEulerCar g_pidEulerCarRight; +extern PidEulerCar g_pidEulerCarLeft; +extern Gear_Motor_handle pidRightMotor; +extern Gear_Motor_handle pidLeftMotor; +extern Gear_Motor_handle aveRightMotor; +extern Gear_Motor_handle aveLeftMotor; + + +//按键状态 +extern int g_button1State; +extern int g_button2State; + +//按键滤波计数器 +extern int g_button1_count; +extern int g_button2_count; + +//通过按键调整PID参数的步进参数 +extern float g_KP_Step; +extern float g_KI_Step; +extern float g_KD_Step; + +/* 接收完成标志 */ +static volatile bool g_RxInterruptflag = true; +/* 发送完成标志 */ +static volatile bool g_TxInterruptFlag = true; + +/* 定时器中断计数器 */ +unsigned int g_TimerInterruptCount = 0; + +/* 上报数据给上位机计数器 */ +static unsigned int g_SendCount = 0; + +/* 上位机下发命令计数器 */ +static unsigned int g_RecvCount = 0; + +//小车三轴目标运动速度，单位：m/s +float Move_X, Move_Y, Move_Z; + +#define EULERCAR_WHEEL_TRACK 120.0 //EulerCar小车两个轮子之间的轮距，单位mm +float modelCalLeftSpeed = 0.0; //根据两轮差速小车模型计算出的左轮速度 +float modelCalRightSpeed = 0.0; //根据两轮差速小车模型计算出的右轮速度 + +#define IS_SUPPORT_PID 1 + +/* 串口发送的数据进行赋值 */ +static void data_transition(void) +{ + float A_Move_X,A_Move_Z; + + g_SendData.SensorStr.Frame_Header = FRAME_HEADER; //帧头 + g_SendData.SensorStr.Frame_Tail = FRAME_TAIL; //帧尾 + + //从各车轮当前速度求出三轴当前速度 + A_Move_X = (aveRightMotor.speed + aveLeftMotor.speed) / 2.0; //小车x轴速度，单位mm/s + A_Move_Z = (aveRightMotor.speed - aveLeftMotor.speed) / (EULERCAR_WHEEL_TRACK * 2.0) * 1000.0; //小车角速度, 单位mrad/s + + g_SendData.SensorStr.X_speed = (short)(round(A_Move_X)); //小车x轴速度，单位mm/s + g_SendData.SensorStr.Y_speed = 0; + g_SendData.SensorStr.Z_speed = (short)(round(A_Move_Z)); //小车角速度，单位mm/s + + DBG_PRINTF("ActualeftSpeed:%.2fmm/s,ActualRightSpeed:%.2fmm/s,A_Move_X=%.2fmm/s,A_Move_Z=%.2fmrad/s\r\n", + aveLeftMotor.speed,aveRightMotor.speed,A_Move_X,A_Move_Z); + + //加速度计三轴加速度 + g_SendData.SensorStr.Accelerometer.X_data = 1; //加速度计Y轴转换到ROS坐标X轴 + g_SendData.SensorStr.Accelerometer.Y_data = 1; //加速度计X轴转换到ROS坐标Y轴 + g_SendData.SensorStr.Accelerometer.Z_data = 1; //加速度计Z轴转换到ROS坐标Z轴 + + //角速度计三轴角速度 + g_SendData.SensorStr.Gyroscope.X_data = 2; //角速度计Y轴转换到ROS坐标X轴 + g_SendData.SensorStr.Gyroscope.Y_data = 2; //角速度计X轴转换到ROS坐标Y轴 + if (g_MotorState == 0) + //如果电机控制位使能状态，那么正常发送Z轴角速度 + g_SendData.SensorStr.Gyroscope.Z_data = 1; + else + //如果机器人是静止的（电机控制位失能），那么发送的Z轴角速度为0 + g_SendData.SensorStr.Gyroscope.Z_data = 0; + + //电池电压(这里将浮点数放大一千倍传输，相应的在接收端在接收到数据后也会缩小一千倍) + g_SendData.SensorStr.Power_Voltage = 90; + + g_SendData.buffer[0] = g_SendData.SensorStr.Frame_Header; // 帧头 + g_SendData.buffer[1] = g_MotorState; // 小车软件失能标志位 + + //小车三轴速度,各轴都拆分为两个8位数据再发送 + g_SendData.buffer[2] = g_SendData.SensorStr.X_speed >> 8; + g_SendData.buffer[3] = g_SendData.SensorStr.X_speed; + g_SendData.buffer[4] = g_SendData.SensorStr.Y_speed >> 8; + g_SendData.buffer[5] = g_SendData.SensorStr.Y_speed; + g_SendData.buffer[6] = g_SendData.SensorStr.Z_speed >> 8; + g_SendData.buffer[7] = g_SendData.SensorStr.Z_speed; + + //IMU加速度计三轴加速度,各轴都拆分为两个8位数据再发送 + g_SendData.buffer[8] = g_SendData.SensorStr.Accelerometer.X_data >> 8; + g_SendData.buffer[9] = g_SendData.SensorStr.Accelerometer.X_data; + g_SendData.buffer[10] = g_SendData.SensorStr.Accelerometer.Y_data >> 8; + g_SendData.buffer[11] = g_SendData.SensorStr.Accelerometer.Y_data; + g_SendData.buffer[12] = g_SendData.SensorStr.Accelerometer.Z_data >> 8; + g_SendData.buffer[13] = g_SendData.SensorStr.Accelerometer.Z_data; + + //IMU角速度计三轴角速度,各轴都拆分为两个8位数据再发送 + g_SendData.buffer[14] = g_SendData.SensorStr.Gyroscope.X_data >> 8; + g_SendData.buffer[15] = g_SendData.SensorStr.Gyroscope.X_data; + g_SendData.buffer[16] = g_SendData.SensorStr.Gyroscope.Y_data >> 8; + g_SendData.buffer[17] = g_SendData.SensorStr.Gyroscope.Y_data; + g_SendData.buffer[18] = g_SendData.SensorStr.Gyroscope.Z_data >> 8; + g_SendData.buffer[19] = g_SendData.SensorStr.Gyroscope.Z_data; + + //电池电压,拆分为两个8位数据发送 + g_SendData.buffer[20] = g_SendData.SensorStr.Power_Voltage >> 8; + g_SendData.buffer[21] = g_SendData.SensorStr.Power_Voltage; + + //数据校验位计算 + unsigned char check_sum = 0, k; + for (k = 0; k < 22; k++) { + check_sum = check_sum ^ g_SendData.buffer[k]; + } + g_SendData.buffer[22] = check_sum; + g_SendData.buffer[23] = g_SendData.SensorStr.Frame_Tail; //帧尾 +} + +void debug_send_buffer(unsigned int count) +{ + DBG_PRINTF("-----------------send_count=%u-----------------\r\n", count); + for (size_t i = 0; i < SEND_DATA_SIZE; i++) { + DBG_PRINTF("0x%x ", g_SendData.buffer[i]); + } + DBG_PRINTF(" \r\n"); +} + +void UART3WriteInterruptCallback(void *handle) +{ + BASE_FUNC_UNUSED(handle); + //DBG_PRINTF("\r\nUART Write Finish\r\n"); + g_TxInterruptFlag = true; + return; +} + +void UART3ReadInterruptCallback(void *handle) +{ + BASE_FUNC_UNUSED(handle); + g_RxInterruptflag = 1; + return; +} + +// 定时器10ms产生一个中断 +void TIMER0_InterruptProcess(void *handle) +{ + BASE_StatusType ret; + /* USER CODE BEGIN TIMER0_InterruptProcess */ + TIMER_Handle *timerHandle = (TIMER_Handle *)handle; + BASE_FUNC_UNUSED(timerHandle); + g_TimerInterruptCount++; + + //按键滤波，500毫秒内算一次按键 + if(g_button1_count > 0) + { + g_button1_count ++; + if(g_button1_count > 50){ + g_button1_count = 0; + //DBG_PRINTF("Button1 Count Reset 0\r\n"); + } + } + + if(g_button2_count > 0) + { + g_button2_count ++; + if(g_button2_count > 50){ + g_button2_count = 0; + //DBG_PRINTF("Button2 Count Reset 0\r\n"); + } + } + + //按设置周期上报底盘数据 + if ((g_TimerInterruptCount % (EULER_CAR_DATA_SEND_PERIOD/10)) == 0) { + if (g_TxInterruptFlag) { + g_TxInterruptFlag = false; + //获取小车要发送的数据 + QDM_CalMotorSpeed(&aveLeftMotor); //计算左轮平均速度 + QDM_CalMotorSpeed(&aveRightMotor); //计算右轮平均速度 + data_transition(); + + ret = HAL_UART_WriteIT(&g_uart3, g_SendData.buffer, SEND_DATA_SIZE); + + if(ret == BASE_STATUS_OK) { + //DBG_PRINTF("Data send OK, send count = %d\r\n",g_SendCount); + }else{ + DBG_PRINTF("Data send error, code = %d\r\n",ret); + } + g_SendCount++; + } + } + + //每间隔10毫秒进行一次PID电机控制 + if (g_TimerInterruptCount % (MOTOR_PID_CONTROL_PERIOD / 10) == 0) { + //获取小车运行实际速度 + QDM_CalMotorSpeed(&pidLeftMotor); //计算一个PID周期内左轮速度 + QDM_CalMotorSpeed(&pidRightMotor); //计算一个PID周期内右轮速度 + + g_pidEulerCarLeft.ActualSpeed = pidLeftMotor.speed; + g_pidEulerCarRight.ActualSpeed = pidRightMotor.speed; + + //如果上位机下发速度大于0.1mm/s，则启动PID控制 + if ((fabsf(modelCalLeftSpeed) > 0.1) || (fabsf(modelCalRightSpeed) > 0.1)) { + Pid_Process(); + } + } + +} + +static float XYZ_transition(unsigned char High, unsigned char Low) +{ + short transition = ((High << 8) + Low); //将高8位和低8位整合成一个16位的short型数据 + //return transition / 1000 + (transition % 1000) * 0.001; //单位转换, mm/s->m/s + return (float)transition; +} + +int EulerCarSpeedCtrlLeft(float LeftSpeed) +{ + + unsigned int duty = (abs)((int)(LeftSpeed/g_motorMaxSpeed * 100)); + if (duty > 99) { + duty = 35; + } + //DBG_PRINTF("left wheel duty:%d\r\n", duty); + HAL_APT_SetPWMDutyByNumber(&g_apt1, duty); + + //如果电机小于0.1mm/s，则停止电机转动 + if (fabsf(LeftSpeed) < 0.1) { + HAL_APT_StopModule(RUN_APT1); + return 0; + } + if (LeftSpeed > 0) { + User_APTPwmARecovery(g_apt1.baseAddress); + User_APTForcePWMBOutputLow(g_apt1.baseAddress); + } else { + User_APTPwmBRecovery(g_apt1.baseAddress); + User_APTForcePWMAOutputLow(g_apt1.baseAddress); + } + HAL_APT_StartModule(RUN_APT1); + //DBG_PRINTF("Left wheel set duty:%d, run ok!\r\n",duty); + + return 0; +} + +int EulerCarSpeedCtrlRight(float rightSpeed) +{ + unsigned int duty = (abs)((int)(rightSpeed/g_motorMaxSpeed * 100)); + if (duty > 99) { + duty = 35; + } + HAL_APT_SetPWMDutyByNumber(&g_apt0, duty); + + //如果电机小于0.1mm/s，则停止电机转动 + if (fabsf(rightSpeed) < 0.1) { + HAL_APT_StopModule(RUN_APT0); + return 0; + } + if (rightSpeed > 0) { + User_APTPwmARecovery(g_apt0.baseAddress); + User_APTForcePWMBOutputLow(g_apt0.baseAddress); + } else { + User_APTPwmBRecovery(g_apt0.baseAddress); + User_APTForcePWMAOutputLow(g_apt0.baseAddress); + } + HAL_APT_StartModule(RUN_APT0); + //DBG_PRINTF("Right wheel set duty:%d, run ok!\r\n",duty); + + return 0; +} + +void Pid_Process(void) +{ + float pidTargetSpeed; + g_pid_count++; + + /* 通过PID算法计算右轮目标速度 */ + pidTargetSpeed = Pid_Ctrl(&g_pidEulerCarRight); + EulerCarSpeedCtrlRight(pidTargetSpeed); + + /* 通过PID算法计算左轮目标速度 */ + pidTargetSpeed = Pid_Ctrl(&g_pidEulerCarLeft); + EulerCarSpeedCtrlLeft(pidTargetSpeed); + + /* + //左右轮同时打印 + if((g_TimerInterruptCount % 10) == 0) { + DBG_PRINTF("N=%d,P=%.02f,I=%.02f,D=%.02f|L-SS:%.1f,AS:%.1f,err:%.1f,nr:%.1f,lr:%.1f,in:%.1f,TS:%.1f,dt:%d|R-SS:%.1f,AS:%.1f,err:%.1f,nr:%.1f,lr:%.1f,in:%.f,TS:%.1f,dt:%d\r\n", + g_pid_count,g_KP,g_KI,g_KD,g_pidEulerCarRight.SetSpeed,g_pidEulerCarRight.ActualSpeed, + g_pidEulerCarRight.err,g_pidEulerCarRight.err_next,g_pidEulerCarRight.err_last,g_pidEulerCarRight.IncSpeed, + g_pidEulerCarRight.TargetIncSpeed,g_pidEulerCarRight.duty, + g_pidEulerCarLeft.SetSpeed,g_pidEulerCarLeft.ActualSpeed, + g_pidEulerCarLeft.err,g_pidEulerCarLeft.err_next,g_pidEulerCarLeft.err_last,g_pidEulerCarLeft.IncSpeed, + g_pidEulerCarLeft.TargetIncSpeed,g_pidEulerCarLeft.duty); + } + + //打印电机运行情况 + if((g_TimerInterruptCount % 10) == 0) { + DBG_PRINTF("N=%d|L-SS:%.1f,AS:%.1f,err:%.1f,nr:%.1f,in:%.1f,TS:%.1f,dt:%d|R-SS:%.1f,AS:%.1f,err:%.1f,nr:%.1f,in:%.f,TS:%.1f,dt:%d\r\n", + g_pid_count,g_pidEulerCarRight.SetSpeed,g_pidEulerCarRight.ActualSpeed, + g_pidEulerCarRight.err,g_pidEulerCarRight.err_next,g_pidEulerCarRight.IncSpeed, + g_pidEulerCarRight.TargetIncSpeed,g_pidEulerCarRight.duty, + g_pidEulerCarLeft.SetSpeed,g_pidEulerCarLeft.ActualSpeed, + g_pidEulerCarLeft.err,g_pidEulerCarLeft.err_next,g_pidEulerCarLeft.IncSpeed, + g_pidEulerCarLeft.TargetIncSpeed,g_pidEulerCarLeft.duty); + } + + + //左右轮同时打印 + if((g_TimerInterruptCount % 10) == 0) { + DBG_PRINTF("N=%d,P=%.02f,I=%.02f,D=%.02f|L-SS:%.1f,AS:%.1f,err:%.1f,nr:%.1f,lr:%.1f,in:%.1f,TS:%.1f,dt:%d|R-SS:%.1f,AS:%.1f,err:%.1f,nr:%.1f,lr:%.1f,in:%.f,TS:%.1f,dt:%d\r\n", + g_pid_count,g_KP,g_KI,g_KD,g_pidEulerCarRight.SetSpeed,g_pidEulerCarRight.ActualSpeed, + g_pidEulerCarRight.err,g_pidEulerCarRight.err_next,g_pidEulerCarRight.err_last,g_pidEulerCarRight.IncSpeed, + g_pidEulerCarRight.TargetIncSpeed,g_pidEulerCarRight.duty, + g_pidEulerCarLeft.SetSpeed,g_pidEulerCarLeft.ActualSpeed, + g_pidEulerCarLeft.err,g_pidEulerCarLeft.err_next,g_pidEulerCarLeft.err_last,g_pidEulerCarLeft.IncSpeed, + g_pidEulerCarLeft.TargetIncSpeed,g_pidEulerCarLeft.duty); + } + + //只打印右轮 + if((g_TimerInterruptCount % 10) == 0) { + DBG_PRINTF("N=%d,P=%.02f,I=%.02f,D=%.02f|R-SS:%.1f,AS:%.1f,err:%.1f,nr:%.1f,lr:%.1f,in:%.1f,TS:%.1f,dt:%d\r\n", + g_pid_count,g_KP,g_KI,g_KD,g_pidEulerCarRight.SetSpeed, + g_pidEulerCarRight.ActualSpeed,g_pidEulerCarRight.err,g_pidEulerCarRight.err_next,g_pidEulerCarRight.err_last, + g_pidEulerCarRight.IncSpeed,g_pidEulerCarRight.TargetIncSpeed,g_pidEulerCarRight.duty); + } + + //只打印左轮 + if((g_TimerInterruptCount % 100) == 0) { + DBG_PRINTF("N=%d,P=%.02f,I=%.02f,D=%.02f|L-SS:%.1f,AS:%.1f,err:%.1f,nr:%.1f,lr:%.1f,in:%.1f,TS:%.1f,dt:%d\r\n", + g_pid_count,g_KP,g_KI,g_KD,g_pidEulerCarLeft.SetSpeed, + g_pidEulerCarLeft.ActualSpeed,g_pidEulerCarLeft.err,g_pidEulerCarLeft.err_next,g_pidEulerCarLeft.err_last, + g_pidEulerCarLeft.IncSpeed,g_pidEulerCarLeft.TargetIncSpeed,g_pidEulerCarLeft.duty); + } */ +} + +void recv_data_cal(void) +{ + float x_linear,z_angular; + + g_ReceiveData.ControlStr.X_speed = Move_X; + g_ReceiveData.ControlStr.Y_speed = Move_Y; + g_ReceiveData.ControlStr.Z_speed = Move_Z; + x_linear = g_ReceiveData.ControlStr.X_speed; + z_angular = g_ReceiveData.ControlStr.Z_speed; + //DBG_PRINTF("x_speed:%.02f, Z_speed:%.02f\r\n", g_ReceiveData.ControlStr.X_speed, g_ReceiveData.ControlStr.Z_speed); + //差分轮运动学模型求解 + modelCalLeftSpeed = x_linear - z_angular * EULERCAR_WHEEL_TRACK / 2.0 / 1000.0; //左轮速度，单位mm/s + modelCalRightSpeed = x_linear + z_angular * EULERCAR_WHEEL_TRACK / 2.0 / 1000.0; //右轮速度，单位mm/s + DBG_PRINTF("modelCalLeftSpeed:%.2fmm/s, modelCalRightSpeed:%.2fmm/s\r\n", modelCalLeftSpeed, modelCalRightSpeed); + + //将上位机设置轮速存到PID控制结构体 + g_pidEulerCarLeft.SetSpeed = modelCalLeftSpeed; + g_pidEulerCarRight.SetSpeed = modelCalRightSpeed; + + //如果上位机下发速度小于0.1mm/s，小车停止运动 + if((fabsf(modelCalLeftSpeed) < 0.1) && (fabsf(modelCalRightSpeed) < 0.1)) + { + EulerCarSpeedCtrlRight(0); + EulerCarSpeedCtrlLeft(0); + //DBG_PRINTF("EulerCar stoped!!!\r\n"); + } +} + +void UART3_INTRxSimultaneously(void) +{ + float tmp_X,tmp_Y,tmp_Z; + unsigned char k,Usart_Receive,check_sum; + + while (1) { + if (g_RxInterruptflag) { + g_RxInterruptflag = false; + HAL_UART_ReadIT(&g_uart3, &Usart_Receive, 1); // 读取数据 + g_ReceiveData.buffer[g_ReceiveDataCount] = Usart_Receive; + //确保数组第一个数据为FRAME_HEADER + if (Usart_Receive == FRAME_HEADER || g_ReceiveDataCount > 0) { + g_ReceiveDataCount++; + } + else { + g_ReceiveDataCount = 0; + } + // 验证数据包的长度 + if (g_ReceiveDataCount == RECEIVE_DATA_SIZE) { + g_ReceiveDataCount = 0; //为串口数据重新填入数组做准备 + //验证数据包的帧尾 + if (g_ReceiveData.buffer[10] == FRAME_TAIL) { + check_sum = 0; + for (k = 0; k < 9; k++) { + check_sum = check_sum ^ g_ReceiveData.buffer[k]; + } + //数据异或位校验计算，模式0是发送数据校验 + if (g_ReceiveData.buffer[9] == check_sum) { + g_RecvCount++; + //从串口数据求三轴目标速度， 单位mm/s + tmp_X = XYZ_transition(g_ReceiveData.buffer[3], g_ReceiveData.buffer[4]); + tmp_Y = XYZ_transition(g_ReceiveData.buffer[5], g_ReceiveData.buffer[6]); + tmp_Z = XYZ_transition(g_ReceiveData.buffer[7], g_ReceiveData.buffer[8]); + //合法性检查，设置速度必须小于当前电机支持的最高速度 + if( (fabsf(tmp_X) < g_motorMaxSpeed) && (fabsf(tmp_Y) < g_motorMaxSpeed) && (fabsf(tmp_Z) < MOTOR_MAX_ANGULAR_SPEED) ) { + Move_X = tmp_X; + Move_Y = tmp_Y; + Move_Z = tmp_Z; + DBG_PRINTF("\r\ng_TimerCount=%d,g_RecvCount=%d,Move_X=%.2fmm/s,Move_Y=%.2fmm/s,Move_Z=%.2fmrad/s\r\n", + g_TimerInterruptCount,g_RecvCount, Move_X, Move_Y, Move_Z); + recv_data_cal(); + } + else{ + DBG_PRINTF("\r\ng_TimerCount=%d,g_RecvCount=%d",g_TimerInterruptCount,g_RecvCount); + if((fabsf(tmp_X) > g_motorMaxSpeed) || (fabsf(tmp_Y) > g_motorMaxSpeed)) + DBG_PRINTF("\r\nLinear speed set error: X=%.2fmm/s Y=%.2fmm/s, must less %.2fmm/s\r\n", tmp_X, tmp_Y,g_motorMaxSpeed); + if((fabsf(tmp_Z) > MOTOR_MAX_ANGULAR_SPEED)) + DBG_PRINTF("\r\nAngular speed set error: Z=%.2fmrad/s, must less %.1fmrad/s\r\n", tmp_Z, MOTOR_MAX_ANGULAR_SPEED); + } + } + } + } + } + + //检测按键是否按下，修改PID参数 + if (g_button1State == 1) { + g_button1State = 0; + g_KP = g_KP + g_KP_Step; + DBG_PRINTF("Button1 Evnet, g_KP + %f\r\n",g_KP_Step); + DBG_PRINTF("g_KP=%f g_KI=%f g_KD=%f\r\n", g_KP, g_KI, g_KD); + } + + if (g_button2State == 1) { + g_button2State = 0; + g_KI = g_KI + g_KI_Step; + DBG_PRINTF("Button2 Evnet, g_KI + %f\r\n",g_KI_Step); + DBG_PRINTF("g_KP=%f g_KI=%f g_KD=%f\r\n", g_KP, g_KI, g_KD); + } + } +} \ No newline at end of file diff --git a/main.c b/main.c new file mode 100644 index 0000000..a77cad1 --- /dev/null +++ b/main.c @@ -0,0 +1,129 @@ +/** + * @copyright Copyright (c) 2022, HiSilicon (Shanghai) Technologies Co., Ltd. All rights reserved. + * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + * following conditions are met: + * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + * disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + * following disclaimer in the documentation and/or other materials provided with the distribution. + * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + * products derived from this software without specific prior written permission. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE + * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * @file main.c + * @author MCU Driver Team + * @brief Main program body. + */ + +#include "typedefs.h" +#include "feature.h" +#include "main.h" +/* USER CODE BEGIN 0 */ +/* USER CODE 区域内代码不会被覆盖，区域外会被生成的默认代码覆盖（其余USER CODE 区域同理） */ +#include "debug.h" +#include "ssd1306.h" +#include "ssd1306_fonts.h" +#include "uart.h" +#include "user_motor_control.h" +#include "button.h" +#include "adc_demo.h" +#include "beep.h" +#include "gyro.h" +#include "encoder.h" +#include "eulercar_control.h" +#include "debug.h" +#include "pid.h" +/* 建议用户放置头文件 */ +/* USER CODE END 0 */ + +/* USER CODE BEGIN 1 */ +CAN_Handle g_can; +QDM_Handle g_qdm0; +QDM_Handle g_qdm1; +GPT_Handle g_gpt0; +GPT_Handle g_gpt1; +GPT_Handle g_gpt2; +GPT_Handle g_gpt3; +TIMER_Handle g_timer0; +UART_Handle g_uart0; +UART_Handle g_uart2; +UART_Handle g_uart3; +I2C_Handle g_i2c0; +I2C_Handle g_i2c1; +APT_Handle g_apt0; +APT_Handle g_apt1; +ADC_Handle g_adc0; +GPIO_Handle g_gpio1; +GPIO_Handle g_gpio2; +GPIO_Handle g_gpio5; + +char g_motorTypeStr[4][20] = { + "1:45 HALL", + "1:45 GMR", + "1:90 HALL", + "1:90 GMR", +}; + +/* +//减速比为1:90的霍尔传感器电机 +float g_motorMaxSpeed = MOTOR_MAX_SPEED_GEAR_90; +unsigned int g_curMotorType = MOTOR_TYPE_GEAR_HALL_90; +unsigned int g_motorLineNum = MOTOR_LINE_NUM_HALL_90; + +//减速比为1:90的GMR传感器电机 +float g_motorMaxSpeed = MOTOR_MAX_SPEED_GEAR_90; +unsigned int g_curMotorType = MOTOR_TYPE_GEAR_GMR_90; +unsigned int g_motorLineNum = MOTOR_LINE_NUM_GMR_90; + +//减速比为1:45的霍尔传感器电机 +float g_motorMaxSpeed = MOTOR_MAX_SPEED_GEAR_45; +unsigned int g_curMotorType = MOTOR_TYPE_GEAR_HALL_45; +unsigned int g_motorLineNum = MOTOR_LINE_NUM_HALL_45; + +//减速比为1:45的GMR传感器电机 +float g_motorMaxSpeed = MOTOR_MAX_SPEED_GEAR_45; +unsigned int g_curMotorType = MOTOR_TYPE_GEAR_GMR_45; +unsigned int g_motorLineNum = MOTOR_LINE_NUM_GMR_45; +*/ + +//减速比为1:90的GMR传感器电机 +float g_motorMaxSpeed = MOTOR_MAX_SPEED_GEAR_90; +unsigned int g_curMotorType = MOTOR_TYPE_GEAR_GMR_90; +unsigned int g_motorLineNum = MOTOR_LINE_NUM_GMR_90; + +/* 建议用户定义全局变量、结构体、宏定义或函数声明等 */ +/* USER CODE END 1 */ + +int main(void) +{ + SystemInit(); + InitGearMotor(); + Pid_Init(); + DBG_PRINTF("==============================================================\r\n"); + DBG_PRINTF(" • EulerCar Controller 1.0 • \r\n"); + DBG_PRINTF(" \r\n"); + DBG_PRINTF(" ➤ System Information: \r\n"); + DBG_PRINTF(" • Motor Type:%s\r\n",g_motorTypeStr[g_curMotorType]); + DBG_PRINTF(" • Motor Encode Line Number:%05d\r\n",g_motorLineNum); + DBG_PRINTF(" • Motor Max Speed:%.02fmm/s\r\n", g_motorMaxSpeed); + DBG_PRINTF(" • EulerCar Data Send Period:%dms,%dHZ\r\n", EULER_CAR_DATA_SEND_PERIOD, 1000/EULER_CAR_DATA_SEND_PERIOD); + DBG_PRINTF(" \r\n"); + DBG_PRINTF(" ➤ PID Information: \r\n"); + DBG_PRINTF(" • KP:%.02f • KI:%.02f • KD:%.02f\r\n",g_KP,g_KI,g_KD); + DBG_PRINTF(" • PID Control Period:%dms\r\n",MOTOR_PID_CONTROL_PERIOD); + DBG_PRINTF("==============================================================\r\n"); + DBG_PRINTF("EulerCar MCU init success!!!\r\n"); + HAL_TIMER_Start(&g_timer0); + DBG_PRINTF("TIMER start\r\n"); + InitButtonFunction(); + + UART3_INTRxSimultaneously(); + + /* USER CODE END 5 */ + return BASE_STATUS_OK; +} \ No newline at end of file -- Gitee