diff --git a/application/user/encoder.c b/application/user/encoder.c deleted file mode 100644 index 1713ef6a2a979706451027bad1bf2c1396b40fc0..0000000000000000000000000000000000000000 --- 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 diff --git a/application/user/generatecode/main.h b/application/user/generatecode/main.h index 9127f56189c7bc4e70cf93024349375cd8f95624..5fddda212f9fcb30931cac43a22cc674fb1f7aa0 100644 --- a/application/user/generatecode/main.h +++ b/application/user/generatecode/main.h @@ -66,7 +66,7 @@ #define MOTOR_TIRE_DIAMETER 67.0 //轮胎直径67mm -#define MOTOR_MAX_ANGULAR_SPEED 90.0 //最大角速度60度/s +#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 diff --git a/encoder.c b/encoder.c new file mode 100644 index 0000000000000000000000000000000000000000..2ff7eaca28c1cabd80b73f34e616dc7965487c53 --- /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/application/user/encoder.h b/encoder.h similarity index 56% rename from application/user/encoder.h rename to encoder.h index 02b367e26299a0b40c756e197af4f2d4014ada71..0ee1864fe38014b86825e0fcb41b893c24f32f45 100644 --- a/application/user/encoder.h +++ b/encoder.h @@ -3,18 +3,24 @@ #define PI 3.1415927 -#define MOTOR_PID_CONTROL_PERIOD 10 //编码器读取周期，单位ms +#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_SampleM(float *rightSpeed, float *leftSpeed); +void QDM_CalMotorSpeed(Gear_Motor_handle *pMotor); #endif \ No newline at end of file diff --git a/application/user/eulercar_control.c b/eulercar_control.c similarity index 84% rename from application/user/eulercar_control.c rename to eulercar_control.c index 87b02eb96d93e59b3b046a779806ff6bec837fb7..fac2b70962c4f86302c5e812b883b4b49a0b0779 100644 --- a/application/user/eulercar_control.c +++ b/eulercar_control.c @@ -12,16 +12,20 @@ #include "button.h" #include "encoder.h" -unsigned char g_MotorState; //电机状态 +unsigned char g_MotorState; //电机状态 -EulerCarSendData g_SendData; //串口发送数据 -EulerCarRecvData g_ReceiveData; //串口接收数据 -unsigned int g_dataSendPeriod = 20; //向上位机发送数据的周期 +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; @@ -53,7 +57,7 @@ static unsigned int g_RecvCount = 0; //小车三轴目标运动速度，单位：m/s float Move_X, Move_Y, Move_Z; -#define EULERCAR_WHEEL_TRACK (0.12) //EulerCar小车两个轮子之间的轮距0.12m +#define EULERCAR_WHEEL_TRACK 120.0 //EulerCar小车两个轮子之间的轮距，单位mm float modelCalLeftSpeed = 0.0; //根据两轮差速小车模型计算出的左轮速度 float modelCalRightSpeed = 0.0; //根据两轮差速小车模型计算出的右轮速度 @@ -62,13 +66,21 @@ float modelCalRightSpeed = 0.0; //根据两轮差速小车模型计算出 /* 串口发送的数据进行赋值 */ 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; //帧尾 //从各车轮当前速度求出三轴当前速度 - g_SendData.SensorStr.X_speed = 13; //小车x轴速度 + 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 = 15; //小车z轴速度 + 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轴 @@ -181,11 +193,14 @@ void TIMER0_InterruptProcess(void *handle) } //按设置周期上报底盘数据 - if ((g_TimerInterruptCount % g_dataSendPeriod) == 0) { + 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) { @@ -198,9 +213,13 @@ void TIMER0_InterruptProcess(void *handle) } //每间隔10毫秒进行一次PID电机控制 - if (g_TimerInterruptCount % 1 == 0) { + if (g_TimerInterruptCount % (MOTOR_PID_CONTROL_PERIOD / 10) == 0) { //获取小车运行实际速度 - QDM_SampleM(&g_pidEulerCarRight.ActualSpeed, &g_pidEulerCarLeft.ActualSpeed); + 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)) { @@ -212,8 +231,9 @@ void TIMER0_InterruptProcess(void *handle) 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 + 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) @@ -282,7 +302,8 @@ void Pid_Process(void) /* 通过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", @@ -294,7 +315,6 @@ void Pid_Process(void) 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", @@ -318,15 +338,6 @@ void Pid_Process(void) 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", @@ -355,9 +366,9 @@ void recv_data_cal(void) 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); + 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; @@ -368,13 +379,13 @@ void recv_data_cal(void) { EulerCarSpeedCtrlRight(0); EulerCarSpeedCtrlLeft(0); - DBG_PRINTF("EulerCar stoped!!!\r\n"); + //DBG_PRINTF("EulerCar stoped!!!\r\n"); } } void UART3_INTRxSimultaneously(void) { - float tmp_X,tmp_Y,tmp_Z,maxLinearSpeed; + float tmp_X,tmp_Y,tmp_Z; unsigned char k,Usart_Receive,check_sum; while (1) { @@ -401,26 +412,25 @@ void UART3_INTRxSimultaneously(void) //数据异或位校验计算，模式0是发送数据校验 if (g_ReceiveData.buffer[9] == check_sum) { g_RecvCount++; - //从串口数据求三轴目标速度， 单位m/s + //从串口数据求三轴目标速度， 单位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]); - maxLinearSpeed = g_motorMaxSpeed/1000.0; //合法性检查，设置速度必须小于当前电机支持的最高速度 - if( (fabsf(tmp_X) < maxLinearSpeed) && (fabsf(tmp_Y) < maxLinearSpeed) && (fabsf(tmp_Z) < MOTOR_MAX_ANGULAR_SPEED) ) { + 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=%.2fm/s Move_Y=%.2fm/s Move_Z=%.2frad/s\r\n", + 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) > 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); + 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=%.2frad/s, must less %.1frad/s\r\n", tmp_Z, tmp_Y,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); } } } diff --git a/application/user/main.c b/main.c similarity index 95% rename from application/user/main.c rename to main.c index 95e22c93ad5d319c3df4628f63d5eb61bae2b388..a77cad1513681a37373189a418e60e3e93426cc1 100644 --- a/application/user/main.c +++ b/main.c @@ -92,9 +92,9 @@ 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; +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 */ @@ -111,7 +111,7 @@ int main(void) 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(" • 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);