diff --git a/APP/Appcfg/app_cfg.h b/APP/Appcfg/app_cfg.h index 5613da8..f79f873 100644 --- a/APP/Appcfg/app_cfg.h +++ b/APP/Appcfg/app_cfg.h @@ -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 diff --git a/APP/Device/Device_rotate/rotate_servo.c b/APP/Device/Device_rotate/rotate_servo.c index 4f54edf..2b9ee53 100644 --- a/APP/Device/Device_rotate/rotate_servo.c +++ b/APP/Device/Device_rotate/rotate_servo.c @@ -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); } } diff --git a/APP/Device/Device_speed/servoMotor_recv.c b/APP/Device/Device_speed/servoMotor_recv.c index 19e6f14..cd05e5a 100644 --- a/APP/Device/Device_speed/servoMotor_recv.c +++ b/APP/Device/Device_speed/servoMotor_recv.c @@ -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); } } diff --git a/APP/Device/Device_speed/speed_to_servoMotor.c b/APP/Device/Device_speed/speed_to_servoMotor.c index 392a6fb..d854cfb 100644 --- a/APP/Device/Device_speed/speed_to_servoMotor.c +++ b/APP/Device/Device_speed/speed_to_servoMotor.c @@ -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 diff --git a/APP/Device/Device_speed/speed_to_servoMotor.h b/APP/Device/Device_speed/speed_to_servoMotor.h index 075b724..063d74c 100644 --- a/APP/Device/Device_speed/speed_to_servoMotor.h +++ b/APP/Device/Device_speed/speed_to_servoMotor.h @@ -33,6 +33,8 @@ typedef struct _linkList { uint8_t data[sendDataBufLen]; //发送的数据长度 uint16_t length; +// uint8_t funcCode; +// uint16_t regAddr; //下一个节点 struct _linkList *next; } linkList; diff --git a/BSP/Driver/servoMotor/motorCommu.c b/BSP/Driver/servoMotor/motorCommu.c index e0d799b..bf1efaa 100644 --- a/BSP/Driver/servoMotor/motorCommu.c +++ b/BSP/Driver/servoMotor/motorCommu.c @@ -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; diff --git a/BSP/Driver/servoMotor/servoMotor.c b/BSP/Driver/servoMotor/servoMotor.c index 59050db..9076cd1 100644 --- a/BSP/Driver/servoMotor/servoMotor.c +++ b/BSP/Driver/servoMotor/servoMotor.c @@ -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 diff --git a/BSP/Driver/servoMotor/servoMotor.h b/BSP/Driver/servoMotor/servoMotor.h index 006e48f..9d61ee3 100644 --- a/BSP/Driver/servoMotor/servoMotor.h +++ b/BSP/Driver/servoMotor/servoMotor.h @@ -52,6 +52,9 @@ /*RS485ͨѶ빦ܲH0C*/ #define H0C_COMMU_PARAM_EEPR_UPDATE 0X0C13//MODBUSͨѶдǷµ EEPROM1Ϊд +/*ֻ*/ +#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