pengsx
/
servoMotor
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

修复bug，现在水平垂直发送数据，解析数据都没有问题

This commit is contained in:
REASEARCHER\18383 2025-10-17 16:38:15 +08:00
parent 165565934a
commit 164ba15a43
8 changed files with 623 additions and 199 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

199
APP/Appcfg/app_cfg.h
View File

@ -55,88 +55,205 @@
/// CE
#define TASK_TESTQUEUE_PRIO 55u
///*****************************************************************/
//
//#define TASK_GET_ANGLE_PRIO 21u
//#define TASK_GET_ANGLE_STK_SIZE 120u
//
//#define POWER_OFF_PRIO 17u//原来为7，后导致ping包有延时，造成网络阻塞，UCOSII系统卡死，故优先级低于TCPIP
//#define POWER_OFF_STK_SIZE 200u
//
//#define TASK_PS_PRIO 22u
//#define TASK_PS_STK_SIZE 80u
//
//#define TASK_HORI_ROATE_MONITOR_PRIO 23u
//#define TASK_HORI_ROATE_MONITOR_STK_SIZE 120u
//
//#define TASK_VERT_ROATE_MONITOR_PRIO 24u
//#define TASK_VERT_ROATE_MONITOR_STK_SIZE 120u
//
//#define TASK_ELECTRIC_STABLE_PRIO 25u
//#define TASK_ELECTRIC_STABLE_STK_SIZE 150u
//
//#define TASK_VERT_ELECTRIC_STABLE_PWM_PRIO 26u
//#define TASK_VERT_ELECTRIC_STABLE_PWM_SIZE 150u
//
//#define TASK_PTZ_UPDATE_RECE_DATA_PRIO 27u//接收升级数据
//#define TASK_PTZ_UPDATE_RECE_DATA_STK_SIZE 170u
//
//#define TASK_VERT_SELF_CHECK_PRIO 28u
//#define TASK_VERT_SELF_CHECK_STK_SIZE 180u
//
//#define TASK_HORI_SELF_CHECK_PRIO 29u
//#define TASK_HORI_SELF_CHECK_STK_SIZE 180u
//
//#define TASK_HORI_PID_PRIO 30u
//#define TASK_HORI_PID_STK_SIZE 150u
//
//#define TASK_VERT_DIRECTOR_SPEED_PWM_PRIO 31u
//#define TASK_VERT_DIRECTOR_SPEED_PWM_STK_SIZE 150u
//
//#define TASK_VERT_PID_PRIO 32u
//#define TASK_VERT_PID_STK_SIZE 150u
//
//#define TASK_FAULT_DETECT_PRIO 34u
//#define TASK_FAULT_DETECT_STK_SIZE 180u
///**/
//#define TASK_AREA_SCAN_PRIO 35u
//#define TASK_AREA_SCAN_STK_SIZE 120u
//
//#define TASK_PRESET_BIT_SCAN_PRIO 36u
//#define TASK_PRESET_BIT_SCAN_STK_SIZE 100u
//
//#define TASK_LISTEN_PTZ_SERVER_PRIO 37u//云台接收指令分析
//#define TASK_LISTEN_PTZ_SERVER_STK_SIZE 600
//
//#define TASK_PTZ_UPDATE_DATA_PROCESS_PRIO 39u//处理升级数据
//#define TASK_PTZ_UPDATE_DATA_PROCESS_STK_SIZE 200u
//
//#define TASK_PTZ_UART_485_LASER_PROCESS_PRIO 40u//处理串口数据
//#define TASK_PTZ_UART_485_LASER_PROCESS_STK_SIZE 150u
//
//#define TASK_PTZ_UART_485_DATA_PROCESS_PRIO 41u//处理串口数据
//#define TASK_PTZ_UART_485_DATA_PROCESS_STK_SIZE 300u
//
//#define TASK_PTZ_UART_422_DATA_PROCESS_PRIO 42u//处理串口数据
//#define TASK_PTZ_UART_422_DATA_PROCESS_STK_SIZE 300u
//
//#define TASK_PTZ_AUTO_RETURN_PRIO 43u//角度回传任务
//#define TASK_PTZ_AUTO_RETURN_STK_SIZE 180u
//
//#define TASK_PTZ_SPEED_RETURN_PRIO 44u//角度回传任务
//#define TASK_PTZ_SPEED_RETURN_STK_SIZE 180u
//
//#define TASK_PTZ_DATA_COLLECT_PRIO 45u
//#define TASK_PTZ_DATA_COLLECT_STK_SIZE 200u
//
//#define TASK_PTZ_HEAT_RESISTOR_PRIO 46u
//#define TASK_PTZ_HEAT_RESISTOR_STK_SIZE 60u
//
//#define TASK_PTZ_RESTORE_PRIO 47u
//#define TASK_PTZ_RESTORE_STK_SIZE 60u
//
//
//#define TASK_PTZ_ERROR_COUNT_PRIO 48u
//#define TASK_PTZ_ERROR_COUNT_STK_SIZE 200u
//
//
////打印任务堆栈等信息
//#define TASK_PTZ_TASK_PRINTF_PRIO 55u
//#define TASK_PTZ_TASK_PRINTF_STK_SIZE 200u
//
//
////解析水平电机返回的数据
//#define TASK_RECV_HORI_SERVO_PRIO 56u
//#define TASK_RECV_HORI_SERVO_STK_SIZE 200u
//
////解析俯仰电机返回的数据
//#define TASK_RECV_VERT_SERVO_PRIO 57u
//#define TASK_RECV_VERT_SERVO_STK_SIZE 200u
//
//
//
////#define ????????????????????_PRIO 55u
////#define ????????????????????_STK_SIZE 150u
///*******************************************************************************/
/*****************************************************************/
#define TASK_GET_ANGLE_PRIO 21u
#define TASK_GET_ANGLE_PRIO 22u
#define TASK_GET_ANGLE_STK_SIZE 120u
#define POWER_OFF_PRIO 17u//原来为7，后导致ping包有延时，造成网络阻塞，UCOSII系统卡死，故优先级低于TCPIP
#define POWER_OFF_STK_SIZE 200u
#define TASK_PS_PRIO 22u
#define TASK_PS_STK_SIZE 80u
#define TASK_HORI_ROATE_MONITOR_PRIO 23u
#define TASK_HORI_ROATE_MONITOR_STK_SIZE 120u
#define TASK_VERT_ROATE_MONITOR_PRIO 24u
#define TASK_VERT_ROATE_MONITOR_STK_SIZE 120u
/*********电机通讯协议任务优先级在此**********/
//解析水平电机返回的数据
#define TASK_RECV_HORI_SERVO_PRIO 18u
#define TASK_RECV_HORI_SERVO_STK_SIZE 200u
#define TASK_ELECTRIC_STABLE_PRIO 25u
#define TASK_ELECTRIC_STABLE_STK_SIZE 150u
//解析俯仰电机返回的数据
#define TASK_RECV_VERT_SERVO_PRIO 19u
#define TASK_RECV_VERT_SERVO_STK_SIZE 200u
#define TASK_VERT_ELECTRIC_STABLE_PWM_PRIO 26u
#define TASK_VERT_ELECTRIC_STABLE_PWM_SIZE 150u
#define TASK_PTZ_UPDATE_RECE_DATA_PRIO 27u//接收升级数据
#define TASK_PTZ_UPDATE_RECE_DATA_STK_SIZE 170u
#define TASK_VERT_SELF_CHECK_PRIO 28u
#define TASK_VERT_SELF_CHECK_STK_SIZE 180u
#define TASK_HORI_SELF_CHECK_PRIO 29u
#define TASK_HORI_SELF_CHECK_STK_SIZE 180u
#define TASK_HORI_PID_PRIO 30u
#define TASK_HORI_PID_PRIO 20u
#define TASK_HORI_PID_STK_SIZE 150u
#define TASK_VERT_DIRECTOR_SPEED_PWM_PRIO 31u
#define TASK_VERT_DIRECTOR_SPEED_PWM_PRIO 21u
#define TASK_VERT_DIRECTOR_SPEED_PWM_STK_SIZE 150u
/***************/
#define TASK_VERT_PID_PRIO 32u
#define TASK_PS_PRIO 23u
#define TASK_PS_STK_SIZE 80u
#define TASK_HORI_ROATE_MONITOR_PRIO 24u
#define TASK_HORI_ROATE_MONITOR_STK_SIZE 120u
#define TASK_VERT_ROATE_MONITOR_PRIO 25u
#define TASK_VERT_ROATE_MONITOR_STK_SIZE 120u
#define TASK_ELECTRIC_STABLE_PRIO 26u
#define TASK_ELECTRIC_STABLE_STK_SIZE 150u
#define TASK_VERT_ELECTRIC_STABLE_PWM_PRIO 27u
#define TASK_VERT_ELECTRIC_STABLE_PWM_SIZE 150u
#define TASK_PTZ_UPDATE_RECE_DATA_PRIO 28u//接收升级数据
#define TASK_PTZ_UPDATE_RECE_DATA_STK_SIZE 170u
#define TASK_VERT_SELF_CHECK_PRIO 29u
#define TASK_VERT_SELF_CHECK_STK_SIZE 180u
#define TASK_HORI_SELF_CHECK_PRIO 32u
#define TASK_HORI_SELF_CHECK_STK_SIZE 180u
#define TASK_VERT_PID_PRIO 34u
#define TASK_VERT_PID_STK_SIZE 150u
#define TASK_FAULT_DETECT_PRIO 34u
#define TASK_FAULT_DETECT_PRIO 35u
#define TASK_FAULT_DETECT_STK_SIZE 180u
/**/
#define TASK_AREA_SCAN_PRIO 35u
#define TASK_AREA_SCAN_PRIO 36u
#define TASK_AREA_SCAN_STK_SIZE 120u
#define TASK_PRESET_BIT_SCAN_PRIO 36u
#define TASK_PRESET_BIT_SCAN_PRIO 37u
#define TASK_PRESET_BIT_SCAN_STK_SIZE 100u
#define TASK_LISTEN_PTZ_SERVER_PRIO 37u//云台接收指令分析
#define TASK_LISTEN_PTZ_SERVER_PRIO 39u//云台接收指令分析
#define TASK_LISTEN_PTZ_SERVER_STK_SIZE 600
#define TASK_PTZ_UPDATE_DATA_PROCESS_PRIO 39u//处理升级数据
#define TASK_PTZ_UPDATE_DATA_PROCESS_PRIO 40u//处理升级数据
#define TASK_PTZ_UPDATE_DATA_PROCESS_STK_SIZE 200u
#define TASK_PTZ_UART_485_LASER_PROCESS_PRIO 40u//处理串口数据
#define TASK_PTZ_UART_485_LASER_PROCESS_PRIO 41u//处理串口数据
#define TASK_PTZ_UART_485_LASER_PROCESS_STK_SIZE 150u
#define TASK_PTZ_UART_485_DATA_PROCESS_PRIO 41u//处理串口数据
#define TASK_PTZ_UART_485_DATA_PROCESS_PRIO 42u//处理串口数据
#define TASK_PTZ_UART_485_DATA_PROCESS_STK_SIZE 300u
#define TASK_PTZ_UART_422_DATA_PROCESS_PRIO 42u//处理串口数据
#define TASK_PTZ_UART_422_DATA_PROCESS_PRIO 43u//处理串口数据
#define TASK_PTZ_UART_422_DATA_PROCESS_STK_SIZE 300u
#define TASK_PTZ_AUTO_RETURN_PRIO 43u//角度回传任务
#define TASK_PTZ_AUTO_RETURN_PRIO 44u//角度回传任务
#define TASK_PTZ_AUTO_RETURN_STK_SIZE 180u
#define TASK_PTZ_SPEED_RETURN_PRIO 44u//角度回传任务
#define TASK_PTZ_SPEED_RETURN_PRIO 45u//角度回传任务
#define TASK_PTZ_SPEED_RETURN_STK_SIZE 180u
#define TASK_PTZ_DATA_COLLECT_PRIO 45u
#define TASK_PTZ_DATA_COLLECT_PRIO 46u
#define TASK_PTZ_DATA_COLLECT_STK_SIZE 200u
#define TASK_PTZ_HEAT_RESISTOR_PRIO 46u
#define TASK_PTZ_HEAT_RESISTOR_PRIO 47u
#define TASK_PTZ_HEAT_RESISTOR_STK_SIZE 60u
#define TASK_PTZ_RESTORE_PRIO 47u
#define TASK_PTZ_RESTORE_PRIO 48u
#define TASK_PTZ_RESTORE_STK_SIZE 60u
#define TASK_PTZ_ERROR_COUNT_PRIO 48u
#define TASK_PTZ_ERROR_COUNT_PRIO 49u
#define TASK_PTZ_ERROR_COUNT_STK_SIZE 200u
@ -145,13 +262,7 @@
#define TASK_PTZ_TASK_PRINTF_STK_SIZE 200u
//解析水平电机返回的数据
#define TASK_RECV_HORI_SERVO_PRIO 56u
#define TASK_RECV_HORI_SERVO_STK_SIZE 200u
//解析俯仰电机返回的数据
#define TASK_RECV_VERT_SERVO_PRIO 57u
#define TASK_RECV_VERT_SERVO_STK_SIZE 200u

167
APP/Device/Device_rotate/rotate_servo.c
View File

@ -7,6 +7,8 @@
static BSP_OS_SEM ptz_hori_stop_mutex;//共享资源锁
static BSP_OS_SEM ptz_vert_stop_mutex;
static char ptz_hori_stop_count;//水平停止计数，防止多次停止多次延时
static char ptz_vert_stop_count;//垂直停止计数，防止多次停止多次延时
float ptz_vert_break_angle()
{
@ -49,10 +51,42 @@ void ptz_sem_post_stop_mutex()
BSP_OS_SemPost(&ptz_vert_stop_mutex);
}
/*
///云台水平右转
#define PTZ_HORI_DIR_RIGHT 1
///云台水平左转
#define PTZ_HORI_DIR_LEFT 3//0
///云台处于停止状态
#define PTZ_HORI_DIR_STOP 2
*/
void ptz_hori_start(char direction, float speed)
{
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
// switch ( direction )
// {
// case PTZ_HORI_DIR_RIGHT://伺服电机默认速度为正使云台右转
// case PTZ_HORI_DIR_STOP:
// break;
// case PTZ_HORI_DIR_LEFT:
// speed*=-1;
// default:
// break;
// }
// //设定转动速度
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)//设置速度
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机
// g_ptz.hori_speed_set = speed;
// //设定转动方向
// g_ptz.hori_direction_last = g_ptz.hori_direction_set;
// g_ptz.hori_direction_set = direction;
// g_ptz.hori_direction_actual = g_ptz.hori_direction_set;
// //启动电机
// g_ptz.hori_start_stop_set = PTZ_HORI_START;
// g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
//
// ptz_vert_stop_count = 0;
BSP_OS_SemPost(&ptz_hori_stop_mutex);
}
@ -60,13 +94,73 @@ void ptz_hori_stop(unsigned short int time)
{
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
// //停止电机
// g_ptz.hori_start_stop_set = PTZ_HORI_STOP;
// g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
//
// g_ptz.hori_direction_actual = PTZ_HORI_DIR_STOP;
//
//
// //设定转动速度
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, 0)//设置速度
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority); //失能电机
// g_ptz.hori_speed_set = 0;
// g_ptz.hori_speed_actual = 0;
//
// if(ptz_hori_stop_count <= 0)
// {
// OSTimeDlyHMSM(0u, 0u, 0u, time);
// ptz_hori_stop_count = 0;
// }
// ptz_hori_stop_count ++;
// //电子稳定
//#ifdef PTZ_ELECTRIC_STABLE_L6235D
// ptz_hori_electric_stable_init();
//#endif
BSP_OS_SemPost(&ptz_hori_stop_mutex);
}
/*
///云台垂直向上
#define PTZ_VERT_DIR_UP 3//0
///云台垂直向下
#define PTZ_VERT_DIR_DOWN 1
///云台处于停止状态
#define PTZ_VERT_DIR_STOP 2
*/
void ptz_vert_start(char direction, float speed)
{
BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
// switch ( direction )
// {
// case PTZ_HORI_DIR_RIGHT://伺服电机默认速度为正使云台右转
// case PTZ_HORI_DIR_STOP:
// break;
// case PTZ_HORI_DIR_LEFT:
// speed*=-1;
// default:
// break;
// }
// //设定转动速度
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)//设置速度
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机
// g_ptz.hori_speed_set = speed;
// //设定转动方向
// g_ptz.hori_direction_last = g_ptz.hori_direction_set;
// g_ptz.hori_direction_set = direction;
// g_ptz.hori_direction_actual = g_ptz.hori_direction_set;
// //启动电机
// g_ptz.hori_start_stop_set = PTZ_HORI_START;
// g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
//
// ptz_vert_stop_count = 0;
BSP_OS_SemPost(&ptz_vert_stop_mutex);
}
@ -80,7 +174,7 @@ void ptz_vert_stop(unsigned short int time)
static void ptz_hori_rotate_monitor_task()
{
static uint8_t status;
static int16_t speed = 20;
switch( status )
{
case 0://切换为速度控制模式
@ -103,17 +197,25 @@ static void ptz_hori_rotate_monitor_task()
status = 3;
break;
case 3://设置运行速度
if ( servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, 20)
case 3://保存设定的参数并运行
if ( servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
, WRITE_ONE_REG_FRAME_NUM, lowPriority) )
status = 4;
break;
case 4://保存设定的参数并运行
if ( servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
case 4://读取速度
if ( servoSendData(horiMotorType, ReadMotorOneReg(H_MOTOR, READ_MOTOR_SPEED_NOW)
, READ_ONE_REG_FRAME_NUM, lowPriority) )
status = 5;
break;
case 5://设置运行速度
if ( servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)
, WRITE_ONE_REG_FRAME_NUM, lowPriority) )
status = 3;
speed+=10, speed%=100,
status = 4;
break;
default:
break;
}
@ -121,7 +223,52 @@ static void ptz_hori_rotate_monitor_task()
static void ptz_vert_rotate_monitor_task()
{
static uint8_t status;
static int16_t speed = 20;
switch( status )
{
case 0://切换为速度控制模式
if (servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H02_CONTR_MODE_SELEC, 0)
, WRITE_ONE_REG_FRAME_NUM, lowPriority) )
status = 1;
break;
case 1://设置加速度时间常数
if ( servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_UP_SLOPE_VALUE, 3000)
, WRITE_ONE_REG_FRAME_NUM, lowPriority) )
status = 2;
break;
case 2://设置减速度时间常数
if ( servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_DOWN_SLOPE_VALUE, 2000)
, WRITE_ONE_REG_FRAME_NUM, lowPriority) )
status = 3;
break;
case 3://保存设定的参数并运行
if ( servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
, WRITE_ONE_REG_FRAME_NUM, lowPriority) )
status = 4;
break;
case 4://读速度
if ( servoSendData(vertMotorType, ReadMotorOneReg(V_MOTOR, READ_MOTOR_SPEED_NOW)
, READ_ONE_REG_FRAME_NUM, lowPriority) )
status = 5;
break;
case 5://设置运行速度
if ( servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)
, WRITE_ONE_REG_FRAME_NUM, lowPriority) )
speed+=10, speed%=100,
status = 4;
break;
default:
break;
}
}
@ -130,7 +277,7 @@ static void ptz_hori_rotate_task()
while(1)
{
ptz_hori_rotate_monitor_task();
OSTimeDlyHMSM(0u, 0u, 0u, 3000u);
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
}
}
@ -140,7 +287,7 @@ static void ptz_vert_rotate_task()
while(1)
{
ptz_vert_rotate_monitor_task();
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
}
}

241
APP/Device/Device_speed/servoMotor_recv.c
View File

@ -9,6 +9,220 @@
BSP_OS_SEM g_horiMotorMutex;
BSP_OS_SEM g_vertMotorMutex;
/**
* @brief 写单个寄存器成功后，电机会返回一帧与写格式完全一致的数据
* @param motorNo：写垂直电机还是水平电机
* @return false:错误，读出的内容与写入的内容不一致
*/
static bool MotorReplyForWrite(uint8_t motorNo)
{
static uint8_t motorReplybuff[WRITE_ONE_REG_FRAME_NUM];
if (motorNo == horiMotorType )
{
if ( highPriority == g_servoMotorLinkList.horiMotor.linkListPriority )
{
if ( g_servoMotorLinkList.horiMotor.LinkListHead_H->data[1] == WRITE_ONE_REG )//为写单个寄存器
{
/*提取数据失败*/
if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
{
return false;
}
if ( 0 != memcmp( motorReplybuff, g_servoMotorLinkList.horiMotor.LinkListHead_H->data, WRITE_ONE_REG_FRAME_NUM ) )
{
return false;
}
servoLinkListMemPut(horiMotorType);
}
else
{
return false;
}
}
else if ( lowPriority == g_servoMotorLinkList.horiMotor.linkListPriority )
{
if ( g_servoMotorLinkList.horiMotor.LinkListHead_L->data[1] == WRITE_ONE_REG )//为写单个寄存器
{
/*提取数据失败*/
if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
{
return false;
}
if ( 0 != memcmp( motorReplybuff, g_servoMotorLinkList.horiMotor.LinkListHead_L->data, WRITE_ONE_REG_FRAME_NUM ) )
{
return false;
}
servoLinkListMemPut(horiMotorType);
}
else
{
return false;
}
}
// servoLinkListMemPut(horiMotorType);
}
else if ( motorNo == vertMotorType )
{
if ( highPriority == g_servoMotorLinkList.vertMotor.linkListPriority )
{
if ( g_servoMotorLinkList.vertMotor.LinkListHead_H->data[1] == WRITE_ONE_REG )//为写单个寄存器
{
/*提取数据失败*/
if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
{
return false;
}
if ( 0 != memcmp( motorReplybuff, g_servoMotorLinkList.vertMotor.LinkListHead_H->data, WRITE_ONE_REG_FRAME_NUM ) )
{
return false;
}
servoLinkListMemPut(vertMotorType);
}
else
{
return false;
}
}
else if ( lowPriority == g_servoMotorLinkList.vertMotor.linkListPriority )
{
if ( g_servoMotorLinkList.vertMotor.LinkListHead_L->data[1] == WRITE_ONE_REG )//为写单个寄存器
{
/*提取数据失败*/
if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
{
return false;
}
if ( 0 != memcmp( motorReplybuff, g_servoMotorLinkList.vertMotor.LinkListHead_L->data, WRITE_ONE_REG_FRAME_NUM ) )
{
return false;
}
servoLinkListMemPut(vertMotorType);
}
else
{
return false;
}
}
// servoLinkListMemPut(vertMotorType);
}
return true;
}
/**
* @brief 读单个寄存器成功后，电机会返回一帧7字节数据
* @param motorNo：写垂直电机还是水平电机
* @param speed：获取读取命令后，电机返回的数据内容
* @return false:读取返回错误
*/
static bool MotorReplyForRead(uint8_t motorNo, int16_t* speed)
{
static uint8_t motorReplybuff[READ_ONE_REG_FRAME_NUM];
if (motorNo == horiMotorType )
{
if ( highPriority == g_servoMotorLinkList.horiMotor.linkListPriority )
{
if ( g_servoMotorLinkList.horiMotor.LinkListHead_H->data[1] == READ_ONE_REG )//为读单个寄存器
{
/*提取数据失败*/
if ( CommuRsvData(motorNo, motorReplybuff, READ_ONE_REG_FRAME_NUM-1) == false )
{
return false;
}
if ( motorReplybuff[0] != H_MOTOR_ADDR )return false;//从机地址不对
if ( motorReplybuff[1] != READ_ONE_REG )return false;//功能码不对
if ( motorReplybuff[2] != 0x02 )return false;//返回数据的长度不对
*speed = motorReplybuff[3];
*speed = ( (*speed) << 8 ) | motorReplybuff[4];
servoLinkListMemPut(horiMotorType);
}
else
{
return false;
}
}
else if ( lowPriority == g_servoMotorLinkList.horiMotor.linkListPriority )
{
if ( g_servoMotorLinkList.horiMotor.LinkListHead_L->data[1] == READ_ONE_REG )//为读单个寄存器
{
/*提取数据失败*/
if ( CommuRsvData(motorNo, motorReplybuff, READ_ONE_REG_FRAME_NUM-1) == false )
{
return false;
}
if ( motorReplybuff[0] != H_MOTOR_ADDR )return false;//从机地址不对
if ( motorReplybuff[1] != READ_ONE_REG )return false;//功能码不对
if ( motorReplybuff[2] != 0x02 )return false;//返回数据的长度不对
*speed = motorReplybuff[3];
*speed = ( (*speed) << 8 ) | motorReplybuff[4];
servoLinkListMemPut(horiMotorType);
}
else
{
return false;
}
}
// servoLinkListMemPut(horiMotorType);
}
else if ( motorNo == vertMotorType )
{
if ( highPriority == g_servoMotorLinkList.vertMotor.linkListPriority )
{
if ( g_servoMotorLinkList.vertMotor.LinkListHead_H->data[1] == READ_ONE_REG )//为读单个寄存器
{
/*提取数据失败*/
if ( CommuRsvData(motorNo, motorReplybuff, READ_ONE_REG_FRAME_NUM-1) == false )
{
return false;
}
if ( motorReplybuff[0] != V_MOTOR_ADDR )return false;//从机地址不对
if ( motorReplybuff[1] != READ_ONE_REG )return false;//功能码不对
if ( motorReplybuff[2] != 0x02 )return false;//返回数据的长度不对
*speed = motorReplybuff[3];
*speed = ( (*speed) << 8 ) | motorReplybuff[4];
servoLinkListMemPut(vertMotorType);
}
else
{
return false;
}
}
else if ( lowPriority == g_servoMotorLinkList.vertMotor.linkListPriority )
{
if ( g_servoMotorLinkList.vertMotor.LinkListHead_L->data[1] == READ_ONE_REG )//为读单个寄存器
{
/*提取数据失败*/
if ( CommuRsvData(motorNo, motorReplybuff, READ_ONE_REG_FRAME_NUM-1) == false )
{
return false;
}
if ( motorReplybuff[0] != V_MOTOR_ADDR )return false;//从机地址不对
if ( motorReplybuff[1] != READ_ONE_REG )return false;//功能码不对
if ( motorReplybuff[2] != 0x02 )return false;//返回数据的长度不对
*speed = motorReplybuff[3];
*speed = ( (*speed) << 8 ) | motorReplybuff[4];
servoLinkListMemPut(vertMotorType);
}
else
{
return false;
}
}
// servoLinkListMemPut(vertMotorType);
}
return true;
}
/*!
\brief 水平电机接收任务
\param[in] none
@ -18,19 +232,17 @@ BSP_OS_SEM g_vertMotorMutex;
static void ptz_recv_hori_servo_task()
{
CPU_INT08U err;
static int16_t s_horiMotorSpeed;
while(1) {
OSSemPend(g_horiMotorMutex, 0, &err);
if ( MotorReplyForWrite(H_MOTOR) == true )
{
}
else
stopTimeOut(H_MOTOR);
if ( ( MotorReplyForWrite(H_MOTOR) | MotorReplyForRead(H_MOTOR, &s_horiMotorSpeed) ) == false)
{
beep_enable();
H_MOTOR_STOP;
}
//释放信号量，通知能发送一次
OSSemPost(g_horiSpeedMutex);
}
}
@ -43,12 +255,17 @@ static void ptz_recv_hori_servo_task()
static void ptz_recv_vert_servo_task()
{
CPU_INT08U err;
static int16_t s_vertMotorSpeed;
while(1) {
OSSemPend(g_vertMotorMutex, 0, &err);
stopTimeOut(vertMotorType);
stopTimeOut(V_MOTOR);
if ( ( MotorReplyForWrite(V_MOTOR) | MotorReplyForRead(V_MOTOR, &s_vertMotorSpeed) ) == false)
{
beep_enable();
V_MOTOR_STOP;
}
//释放信号量，通知能发送一次
OSSemPost(g_vertSpeedMutex);
}
}

48
APP/Device/Device_speed/speed_to_servoMotor.c
View File

@ -64,7 +64,7 @@ void servoLinkListMemPut(uint8_t motorType)
//水平电机
if (motorType == horiMotorType) {
//当前发送完成的数据为高优先级链表中的数据
if (g_servoMotorLinkList.horiMotor.linkListNum == highPriority) {
if (g_servoMotorLinkList.horiMotor.linkListPriority == highPriority) {
if (g_servoMotorLinkList.horiMotor.LinkListHead_H != NULL) {
linkList *ptr;
ptr = g_servoMotorLinkList.horiMotor.LinkListHead_H;
@ -74,12 +74,13 @@ void servoLinkListMemPut(uint8_t motorType)
else {
g_servoMotorLinkList.horiMotor.LinkListHead_H = NULL;
g_servoMotorLinkList.horiMotor.LinkListTail_H = NULL;
}
}
g_servoMotorLinkList.horiMotor.linkListNum--;
OSMemPut(g_memPtr, (uint8_t *)ptr);
}
}
//当前发送完成的数据为低优先级链表中的数据
else if (g_servoMotorLinkList.horiMotor.linkListNum == lowPriority) {
else if (g_servoMotorLinkList.horiMotor.linkListPriority == lowPriority) {
if (g_servoMotorLinkList.horiMotor.LinkListHead_L != NULL) {
linkList *ptr;
ptr = g_servoMotorLinkList.horiMotor.LinkListHead_L;
@ -89,15 +90,15 @@ void servoLinkListMemPut(uint8_t motorType)
else {
g_servoMotorLinkList.horiMotor.LinkListHead_L = NULL;
g_servoMotorLinkList.horiMotor.LinkListTail_L = NULL;
}
}
g_servoMotorLinkList.horiMotor.linkListNum--;
OSMemPut(g_memPtr, (uint8_t *)ptr);
}
}
//释放信号量，通知能发送一次
// OSSemPost(g_horiSpeedMutex);
// g_servoMotorLinkList.horiMotor.linkListNum--;
}
else if (motorType == vertMotorType) {
if (g_servoMotorLinkList.vertMotor.linkListNum == highPriority) {
if (g_servoMotorLinkList.vertMotor.linkListPriority == highPriority) {
if (g_servoMotorLinkList.vertMotor.LinkListHead_H != NULL) {
linkList *ptr;
ptr = g_servoMotorLinkList.vertMotor.LinkListHead_H;
@ -108,10 +109,12 @@ void servoLinkListMemPut(uint8_t motorType)
g_servoMotorLinkList.vertMotor.LinkListHead_H = NULL;
g_servoMotorLinkList.vertMotor.LinkListTail_H = NULL;
}
g_servoMotorLinkList.vertMotor.linkListNum--;
OSMemPut(g_memPtr, (uint8_t *)ptr);
}
}
else if (g_servoMotorLinkList.vertMotor.linkListNum == lowPriority) {
else if (g_servoMotorLinkList.vertMotor.linkListPriority == lowPriority) {
if (g_servoMotorLinkList.vertMotor.LinkListHead_L != NULL) {
linkList *ptr;
ptr = g_servoMotorLinkList.vertMotor.LinkListHead_L;
@ -122,11 +125,12 @@ void servoLinkListMemPut(uint8_t motorType)
g_servoMotorLinkList.vertMotor.LinkListHead_L = NULL;
g_servoMotorLinkList.vertMotor.LinkListTail_L = NULL;
}
g_servoMotorLinkList.vertMotor.linkListNum--;
OSMemPut(g_memPtr, (uint8_t *)ptr);
}
}
//释放信号量，通知能发送一次
// OSSemPost(g_vertSpeedMutex);
// g_servoMotorLinkList.vertMotor.linkListNum--;
}
@ -189,6 +193,7 @@ bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
g_servoMotorLinkList.horiMotor.LinkListHead_H
= g_servoMotorLinkList.horiMotor.LinkListTail_H;
}
g_servoMotorLinkList.horiMotor.linkListNum++;
}
else {
if (g_servoMotorLinkList.horiMotor.LinkListTail_L == NULL) {
@ -203,6 +208,7 @@ bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
g_servoMotorLinkList.horiMotor.LinkListHead_L
= g_servoMotorLinkList.horiMotor.LinkListTail_L;
}
g_servoMotorLinkList.horiMotor.linkListNum++;
}
//释放信号量，通知能发送一次
@ -227,6 +233,7 @@ bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
g_servoMotorLinkList.vertMotor.LinkListHead_H
= g_servoMotorLinkList.vertMotor.LinkListTail_H;
}
g_servoMotorLinkList.vertMotor.linkListNum++;
}
else {
if (g_servoMotorLinkList.vertMotor.LinkListTail_L == NULL) {
@ -240,7 +247,8 @@ bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
if (g_servoMotorLinkList.vertMotor.LinkListHead_L == NULL) {
g_servoMotorLinkList.vertMotor.LinkListHead_L
= g_servoMotorLinkList.vertMotor.LinkListTail_L;
}
}
g_servoMotorLinkList.vertMotor.linkListNum++;
}
//释放信号量，通知能发送一次
@ -269,7 +277,7 @@ static void ptz_hori_servo_task()
CommuTransData(H_MOTOR, g_servoMotorLinkList.horiMotor.LinkListHead_H->data
, g_servoMotorLinkList.horiMotor.LinkListHead_H->length);
g_servoMotorLinkList.horiMotor.linkListNum = highPriority;
g_servoMotorLinkList.horiMotor.linkListPriority = highPriority;
startTimeOut(horiMotorType);
continue;
}
@ -280,7 +288,7 @@ static void ptz_hori_servo_task()
CommuTransData(H_MOTOR, g_servoMotorLinkList.horiMotor.LinkListHead_L->data
, g_servoMotorLinkList.horiMotor.LinkListHead_L->length);
g_servoMotorLinkList.horiMotor.linkListNum = lowPriority;
g_servoMotorLinkList.horiMotor.linkListPriority = lowPriority;
startTimeOut(horiMotorType);
}
else {
@ -305,7 +313,7 @@ static void ptz_vert_servo_task()
// 高优先级链表中数据先发送
if (g_servoMotorLinkList.vertMotor.LinkListHead_H != NULL) {
g_servoMotorLinkList.vertMotor.linkListNum = highPriority;
g_servoMotorLinkList.vertMotor.linkListPriority = highPriority;
CommuTransData(V_MOTOR, g_servoMotorLinkList.vertMotor.LinkListHead_H->data
, g_servoMotorLinkList.vertMotor.LinkListHead_H->length);
startTimeOut(V_MOTOR);
@ -314,7 +322,7 @@ static void ptz_vert_servo_task()
// 高优先级链表中无数据时，发送低优先级中的数据
if (g_servoMotorLinkList.vertMotor.LinkListHead_L != NULL) {
g_servoMotorLinkList.vertMotor.linkListNum = highPriority;
g_servoMotorLinkList.vertMotor.linkListPriority = lowPriority;
CommuTransData(V_MOTOR, g_servoMotorLinkList.vertMotor.LinkListHead_L->data
, g_servoMotorLinkList.vertMotor.LinkListHead_L->length);
startTimeOut(V_MOTOR);
@ -381,11 +389,13 @@ static void creat_task_vert_servo_task(void)
void horiServoTimeOut()
{
OSSemPost(g_horiSpeedMutex);
servoLinkListMemPut(horiMotorType);
OSSemPost(g_horiSpeedMutex);
}
void vertServoTimeOut()
{
servoLinkListMemPut(vertMotorType);
OSSemPost(g_vertSpeedMutex);
}
@ -394,7 +404,7 @@ BSP_OS_TMR vertServoSoftWareTim;
void servoCommSoftWareTimInit()
{
CPU_INT08U ServoSoftWareTimErr;
horiServoSoftWareTim = OSTmrCreate(1
horiServoSoftWareTim = OSTmrCreate(100
, 100//1*200ms
, OS_TMR_OPT_ONE_SHOT
, (OS_TMR_CALLBACK)horiServoTimeOut
@ -408,8 +418,8 @@ void servoCommSoftWareTimInit()
pdebug(DEBUG_LEVEL_FATAL,"create horiServoSoftWareTim failed...\n\r");
}
vertServoSoftWareTim = OSTmrCreate(1
, 1
vertServoSoftWareTim = OSTmrCreate(100
, 100
, OS_TMR_OPT_ONE_SHOT
, (OS_TMR_CALLBACK)vertServoTimeOut
, (void *)0

2
APP/Device/Device_speed/speed_to_servoMotor.h
View File

@ -33,6 +33,8 @@ typedef struct _linkList {
uint8_t data[sendDataBufLen];
//发送的数据长度
uint16_t length;
// uint8_t funcCode;
// uint16_t regAddr;
//下一个节点
struct _linkList *next;
} linkList;

26
BSP/Driver/servoMotor/motorCommu.c
View File

@ -303,7 +303,6 @@ void DMA0_Channel3_IRQHandler(void)
while(usart_flag_get(g_motorCommuBuff[H_MOTOR].uartNo, USART_FLAG_TC) == RESET); // 等待串口发送完成
H_COMMU_RS485_RX; //485切换为接收
pCommuDeal->dmaTranFlag = DMA_TRANS_IDLE;
servoLinkListMemPut(horiMotorType);
}
}
@ -330,8 +329,6 @@ void USART2_IRQHandler(void)
g_MotorDmaBuff[H_MOTOR].dmaRxch);//函数获取的是还有多少个没传输，而不是已经传输了多少
//释放信号量，通知能处理数据
OSSemPost(g_horiMotorMutex);
//释放信号量，通知能发送一次
OSSemPost(g_horiSpeedMutex);
}
}
@ -374,7 +371,6 @@ void DMA1_Channel7_IRQHandler(void)
CommuInfo_t *pCommuDeal = g_commuInfoBuff[V_MOTOR].pCommuInfo;
while(usart_flag_get(g_motorCommuBuff[V_MOTOR].uartNo, USART_FLAG_TC) == RESET); // 等待串口发送完成
servoLinkListMemPut(vertMotorType);
V_COMMU_RS485_RX; //485切换为接收
pCommuDeal->dmaTranFlag = DMA_TRANS_IDLE;
}
@ -399,12 +395,10 @@ void USART5_IRQHandler(void)
usart_data_receive(g_motorCommuBuff[V_MOTOR].uartNo); //第二步，读取数据寄存器，清除IDLE标志位
g_vertLastPos = g_vertNowPos;
g_vertNowPos = DMA_BUFF_SIZE - dma_transfer_number_get(g_MotorDmaBuff[H_MOTOR].dmaNo,
g_MotorDmaBuff[H_MOTOR].dmaRxch);
g_vertNowPos = DMA_BUFF_SIZE - dma_transfer_number_get(g_MotorDmaBuff[V_MOTOR].dmaNo,
g_MotorDmaBuff[V_MOTOR].dmaRxch);
//释放信号量，通知接收任务处理
OSSemPost(g_vertMotorMutex);
//释放信号量，通知能发送一次
OSSemPost(g_vertSpeedMutex);
}
}
@ -483,36 +477,38 @@ bool CommuTransData(uint8_t motorNo, uint8_t* buffer, int32_t len)
*/
bool CommuRsvData(uint8_t motorNo, uint8_t* userBuff, uint32_t len)
{
CommuInfo_t *pCommuDeal = g_commuInfoBuff[motorNo].pCommuInfo;
/*简易判断*/
if( userBuff == NULL || len == 0 )
{
return false;
}
uint16_t horiPosTemp = g_horiLastPos;
uint16_t vertPosTemp = g_vertLastPos;
/*接收DMA缓冲区数据*/
if ( motorNo == H_MOTOR )
{
if ( ( (g_horiLastPos + len) % DMA_BUFF_SIZE ) != g_horiNowPos )//假如是写寄存器，电机返回消息正常是8字节，但如果是写错误，则返回非8字节错误码
if ( ( (horiPosTemp + len) % DMA_BUFF_SIZE ) != g_horiNowPos )//假如是写寄存器，电机返回消息正常是8字节，但如果是写错误，则返回非8字节错误码
{
return false;
}
for(uint8_t i = 0; i < len; i++)
{
*(userBuff+i) = pCommuDeal->pDmaRsvBuff[g_horiLastPos];
g_horiLastPos = (g_horiLastPos + 1) % DMA_BUFF_SIZE;
*(userBuff+i) = pCommuDeal->pDmaRsvBuff[horiPosTemp];
horiPosTemp = (horiPosTemp + 1) % DMA_BUFF_SIZE;
}
}
else
{
if ( ( (g_vertLastPos + len) % DMA_BUFF_SIZE ) != g_vertNowPos )//假如是写寄存器，电机返回消息正常是8字节，但如果是写错误，则返回非8字节错误码
if ( ( (vertPosTemp + len) % DMA_BUFF_SIZE ) != g_vertNowPos )//假如是写寄存器，电机返回消息正常是8字节，但如果是写错误，则返回非8字节错误码
{
return false;
}
for(uint8_t i = 0; i < len; i++)
{
*(userBuff+i) = pCommuDeal->pDmaRsvBuff[g_vertLastPos];
g_horiLastPos = (g_vertLastPos + 1) % DMA_BUFF_SIZE;
*(userBuff+i) = pCommuDeal->pDmaRsvBuff[vertPosTemp];
vertPosTemp = (vertPosTemp + 1) % DMA_BUFF_SIZE;
}
}
return true;

121
BSP/Driver/servoMotor/servoMotor.c
View File

@ -28,56 +28,32 @@ static void MotorSwitchGpioCofig(void)
* @param data：要向寄存器写入的数据
* @return 返回数组地址
*/
static uint8_t g_writeOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
static uint8_t g_HwriteOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
static uint8_t g_VwriteOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, uint16_t data)
{
uint16_t crc;
if ( motorNo == H_MOTOR )
{
g_writeOneRegBuff[0] = H_MOTOR_ADDR;//由于采用一主一从模式，所以水平电机垂直电机从机地址都是0x01，云台后期也不会扩展
}
else
{
g_writeOneRegBuff[0] = V_MOTOR_ADDR;
}
g_writeOneRegBuff[1] = WRITE_ONE_REG;
g_writeOneRegBuff[2] = regAddr >> 8;
g_writeOneRegBuff[3] = (uint8_t)(regAddr & 0xff);
g_writeOneRegBuff[4] = data >> 8;
g_writeOneRegBuff[5] = data & 0xff;
crc = ModbusCRC16(g_writeOneRegBuff, WRITE_ONE_REG_FRAME_NUM - 2);
g_writeOneRegBuff[6] = (uint8_t)(crc & 0xff);
g_writeOneRegBuff[7] = crc >> 8;
return g_writeOneRegBuff;
static uint8_t g_writeOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
uint16_t crc;
g_writeOneRegBuff[0] = 0x01;
g_writeOneRegBuff[1] = WRITE_ONE_REG;
g_writeOneRegBuff[2] = regAddr >> 8;
g_writeOneRegBuff[3] = (uint8_t)(regAddr & 0xff);
g_writeOneRegBuff[4] = data >> 8;
g_writeOneRegBuff[5] = data & 0xff;
crc = ModbusCRC16(g_writeOneRegBuff, WRITE_ONE_REG_FRAME_NUM - 2);
g_writeOneRegBuff[6] = (uint8_t)(crc & 0xff);
g_writeOneRegBuff[7] = crc >> 8;
if ( motorNo == H_MOTOR )
{
memcpy(g_HwriteOneRegBuff, g_writeOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
return g_HwriteOneRegBuff;
}
memcpy(g_VwriteOneRegBuff, g_writeOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
return g_VwriteOneRegBuff;
}
/**
* @brief 写单个寄存器成功后，电机会返回一帧与写格式完全一致的数据
* @param motorNo：写垂直电机还是水平电机
* @return false:错误，读出的内容与写入的内容不一致
*/
static uint8_t motorReplybuff[WRITE_ONE_REG_FRAME_NUM];
bool MotorReplyForWrite(uint8_t motorNo)
{
/*提取数据失败*/
if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
{
return false;
}
else
{
for( uint8_t i = 0; i < WRITE_ONE_REG_FRAME_NUM; i++ )
{
/*提取的数据不对*/
if ( motorReplybuff[i] != g_writeOneRegBuff[i] )
{
return false;
}
}
}
return true;
}
/*
从机地址 功能码 寄存器地址高 寄存器地址低 reg数量高位 数量低位 crc校验高位 crc校验低位
@ -89,18 +65,13 @@ bool MotorReplyForWrite(uint8_t motorNo)
* @param regAddr：要读的寄存器
* @return 返回数组地址
*/
static uint8_t g_readOneRegBuff[READ_ONE_REG_FRAME_NUM];
static uint8_t g_HreadOneRegBuff[READ_ONE_REG_FRAME_NUM];
static uint8_t g_VreadOneRegBuff[READ_ONE_REG_FRAME_NUM];
uint8_t* ReadMotorOneReg(uint8_t motorNo, uint16_t regAddr)
{
static uint8_t g_readOneRegBuff[READ_ONE_REG_FRAME_NUM];
uint16_t crc;
if ( motorNo == H_MOTOR )
{
g_readOneRegBuff[0] = H_MOTOR_ADDR;//由于采用一主一从模式，所以水平电机垂直电机从机地址都是0x01，云台后期也不会扩展
}
else
{
g_readOneRegBuff[0] = V_MOTOR_ADDR;
}
g_readOneRegBuff[0] = 0x01;
g_readOneRegBuff[1] = READ_ONE_REG;
g_readOneRegBuff[2] = regAddr >> 8;
g_readOneRegBuff[3] = regAddr & 0xff;
@ -109,40 +80,18 @@ uint8_t* ReadMotorOneReg(uint8_t motorNo, uint16_t regAddr)
crc = ModbusCRC16(g_readOneRegBuff, READ_ONE_REG_FRAME_NUM - 2);
g_readOneRegBuff[6] = crc & 0xff;
g_readOneRegBuff[7] = crc >> 8;
return g_readOneRegBuff;
}
/*
从机地址 功能码 返回数据字节数 返回数据高位 返回数据低位 crc校验高位 crc校验低位
01H 03H 02H 00H 00H B8H 44H
*/
/**
* @brief 读单个寄存器命令发送成功后，电机会返回一帧7字节的数据
* @param motorNo：写垂直电机还是水平电机
* @param data：写垂直电机还是水平电机
* @return false:错误，读出的内容与写入的内容不一致
*/
bool MotorReplyForRead(uint8_t motorNo, uint16_t* data)
{
/*简易判断*/
if ( data == NULL )
{
return false;
}
static uint8_t motorReplybuff[WRITE_ONE_REG_FRAME_NUM - 1];
/*提取数据失败*/
if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM - 1) == false )
{
return false;
}
else
{
*data = motorReplybuff[3];
*data = (*data << 8) | motorReplybuff[4];
}
return true;
if ( motorNo == H_MOTOR )
{
memcpy(g_HreadOneRegBuff, g_readOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
return g_HreadOneRegBuff;
}
memcpy(g_VreadOneRegBuff, g_readOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
return g_VreadOneRegBuff;
}
/**
* @brief 伺服电机驱动初始化
* @param

18
BSP/Driver/servoMotor/servoMotor.h
View File

@ -52,6 +52,9 @@
/*RS485通讯与功能参数H0C*/
#define H0C_COMMU_PARAM_EEPR_UPDATE 0X0C13//MODBUS通讯写入是否更新到 EEPROM，设置1为写入
/*监视只读参数*/
#define READ_MOTOR_SPEED_NOW 0X0B00//读取电机滤波后的实时转速
@ -64,12 +67,6 @@
*/
uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, uint16_t data);
/**
* @brief 写单个寄存器成功后，电机会返回一帧与写格式完全一致的数据
* @param motorNo：写垂直电机还是水平电机
* @return false:错误，读出的内容与写入的内容不一致
*/
bool MotorReplyForWrite(uint8_t motorNo);
/**
* @brief 伺服电机速度模式读单个寄存器
@ -79,13 +76,8 @@ bool MotorReplyForWrite(uint8_t motorNo);
*/
uint8_t* ReadMotorOneReg(uint8_t motorNo, uint16_t regAddr);
/**
* @brief 读单个寄存器命令发送成功后，电机会返回一帧7字节的数据
* @param motorNo：写垂直电机还是水平电机
* @param data：写垂直电机还是水平电机
* @return false:错误，读出的内容与写入的内容不一致
*/
bool MotorReplyForRead(uint8_t motorNo, uint16_t* data);
/**
* @brief 伺服电机驱动初始化
* @param