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

水平、垂直电机发送，接收，解析测试通过，准备写位置

This commit is contained in:
REASEARCHER\18383 2025-10-17 18:20:03 +08:00
parent 164ba15a43
commit e082c9f795
10 changed files with 437 additions and 458 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -59,34 +59,37 @@ void ptz_sem_post_stop_mutex()
///云台处于停止状态 ///云台处于停止状态
#define PTZ_HORI_DIR_STOP 2 #define PTZ_HORI_DIR_STOP 2
*/ */
void ptz_hori_start(char direction, float speed) void ptz_hori_start(char direction, float speed)//输入参数的speed是云台末端的r/min
{ {
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u); BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
// switch ( direction ) switch ( direction )
// { {
// case PTZ_HORI_DIR_RIGHT://伺服电机默认速度为正使云台右转 case PTZ_HORI_DIR_RIGHT://伺服电机默认速度为正使云台右转
// case PTZ_HORI_DIR_STOP: case PTZ_HORI_DIR_STOP:
// break; break;
// case PTZ_HORI_DIR_LEFT: case PTZ_HORI_DIR_LEFT:
// speed*=-1; speed*=-1;
// default: default:
// break; break;
// } }
// //设定转动速度 /*
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)//设置速度 -------------------------------------add speed change to here--------------------------------------
// , WRITE_ONE_REG_FRAME_NUM, lowPriority); */
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1) //设定转动速度
// , WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机 servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)//设置速度，电机的r/min范围-6000~6000
// g_ptz.hori_speed_set = speed; , WRITE_ONE_REG_FRAME_NUM, lowPriority);
// //设定转动方向 servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
// g_ptz.hori_direction_last = g_ptz.hori_direction_set; , WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机
// g_ptz.hori_direction_set = direction; g_ptz.hori_speed_set = speed;
// g_ptz.hori_direction_actual = g_ptz.hori_direction_set; //设定转动方向
// //启动电机 g_ptz.hori_direction_last = g_ptz.hori_direction_set;
// g_ptz.hori_start_stop_set = PTZ_HORI_START; g_ptz.hori_direction_set = direction;
// g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set; g_ptz.hori_direction_actual = g_ptz.hori_direction_set;
// //启动电机
// ptz_vert_stop_count = 0; g_ptz.hori_start_stop_set = PTZ_HORI_START;
g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
ptz_hori_stop_count = 0;
BSP_OS_SemPost(&ptz_hori_stop_mutex); BSP_OS_SemPost(&ptz_hori_stop_mutex);
} }
@ -94,72 +97,68 @@ void ptz_hori_stop(unsigned short int time)
{ {
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u); BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
// //停止电机 //停止电机
// g_ptz.hori_start_stop_set = PTZ_HORI_STOP; g_ptz.hori_start_stop_set = PTZ_HORI_STOP;
// g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set; g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
//
// g_ptz.hori_direction_actual = PTZ_HORI_DIR_STOP; g_ptz.hori_direction_actual = PTZ_HORI_DIR_STOP;
//
//
// //设定转动速度 //设定转动速度
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, 0)//设置速度 servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, 0)//设置速度
// , WRITE_ONE_REG_FRAME_NUM, lowPriority); , WRITE_ONE_REG_FRAME_NUM, lowPriority);
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 0) servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority); //失能电机 , WRITE_ONE_REG_FRAME_NUM, lowPriority); //失能电机
// g_ptz.hori_speed_set = 0; g_ptz.hori_speed_set = 0;
// g_ptz.hori_speed_actual = 0; g_ptz.hori_speed_actual = 0;
//
// if(ptz_hori_stop_count <= 0) if(ptz_hori_stop_count <= 0)
// { {
// OSTimeDlyHMSM(0u, 0u, 0u, time); OSTimeDlyHMSM(0u, 0u, 0u, time);
// ptz_hori_stop_count = 0; ptz_hori_stop_count = 0;
// } }
// ptz_hori_stop_count ++; ptz_hori_stop_count ++;
// //电子稳定 //电子稳定
//#ifdef PTZ_ELECTRIC_STABLE_L6235D #ifdef PTZ_ELECTRIC_STABLE_L6235D
// ptz_hori_electric_stable_init(); ptz_hori_electric_stable_init();
//#endif #endif
BSP_OS_SemPost(&ptz_hori_stop_mutex); BSP_OS_SemPost(&ptz_hori_stop_mutex);
} }
/*
///云台垂直向上 void ptz_vert_start(char direction, float speed)//输入参数的speed是云台末端的r/min
#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); BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
// switch ( direction ) switch ( direction )
// { {
// case PTZ_HORI_DIR_RIGHT://伺服电机默认速度为正使云台右转 case PTZ_VERT_DIR_UP://伺服电机默认速度为正使云台向上
// case PTZ_HORI_DIR_STOP: case PTZ_VERT_DIR_DOWN:
// break; break;
// case PTZ_HORI_DIR_LEFT: case PTZ_VERT_DIR_STOP:
// speed*=-1; speed*=-1;
// default: default:
// break; break;
// } }
// //设定转动速度 /*
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)//设置速度 -------------------------------------add speed change to here--------------------------------------
// , WRITE_ONE_REG_FRAME_NUM, lowPriority); */
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1) //设定转动速度
// , WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机 servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)//设置速度，电机的r/min范围-6000~6000
// g_ptz.hori_speed_set = speed; , WRITE_ONE_REG_FRAME_NUM, lowPriority);
// //设定转动方向 servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
// g_ptz.hori_direction_last = g_ptz.hori_direction_set; , WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机
// g_ptz.hori_direction_set = direction; g_ptz.hori_speed_set = speed;
// g_ptz.hori_direction_actual = g_ptz.hori_direction_set; //设定转动方向
// //启动电机 g_ptz.hori_direction_last = g_ptz.hori_direction_set;
// g_ptz.hori_start_stop_set = PTZ_HORI_START; g_ptz.hori_direction_set = direction;
// g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set; g_ptz.hori_direction_actual = g_ptz.hori_direction_set;
// //启动电机
// ptz_vert_stop_count = 0; 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); BSP_OS_SemPost(&ptz_vert_stop_mutex);
} }
@ -168,107 +167,42 @@ void ptz_vert_stop(unsigned short int time)
{ {
BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u); BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
//停止电机
g_ptz.vert_start_stop_set = PTZ_VERT_STOP;
g_ptz.vert_start_stop_actual = g_ptz.vert_start_stop_set;
g_ptz.vert_direction_actual = PTZ_VERT_DIR_STOP;
//设定转动速度
servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, 0)//设置速度
, WRITE_ONE_REG_FRAME_NUM, lowPriority);
servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
, WRITE_ONE_REG_FRAME_NUM, lowPriority); //失能电机
g_ptz.vert_speed_set = 0;
g_ptz.vert_speed_actual = 0;
if(ptz_vert_stop_count <= 0)
{
OSTimeDlyHMSM(0u, 0u, 0u, time);
ptz_vert_stop_count = 0;
}
ptz_vert_stop_count ++;
//电子稳定
#ifdef PTZ_ELECTRIC_STABLE_L6235D
ptz_hori_electric_stable_init();
#endif
BSP_OS_SemPost(&ptz_vert_stop_mutex); BSP_OS_SemPost(&ptz_vert_stop_mutex);
} }
static void ptz_hori_rotate_monitor_task() static void ptz_hori_rotate_monitor_task()
{ {
static uint8_t status;
static int16_t speed = 20;
switch( status )
{
case 0://切换为速度控制模式
if (servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H02_CONTR_MODE_SELEC, 0)
, WRITE_ONE_REG_FRAME_NUM, lowPriority) )
status = 1;
break;
case 1://设置加速度时间常数
if ( servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_UP_SLOPE_VALUE, 3000)
, WRITE_ONE_REG_FRAME_NUM, lowPriority) )
status = 2;
break;
case 2://设置减速度时间常数
if ( servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_DOWN_SLOPE_VALUE, 2000)
, WRITE_ONE_REG_FRAME_NUM, lowPriority) )
status = 3;
break;
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, 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) )
speed+=10, speed%=100,
status = 4;
break;
default:
break;
}
} }
static void ptz_vert_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;
}
} }
@ -345,6 +279,8 @@ static void creat_task_vert_rotate(void)
} }
} }
void init_rotate_monitor_module(void) void init_rotate_monitor_module(void)
{ {
BSP_OS_SemCreate(&ptz_hori_stop_mutex,1u,"ptz_hori_stop_mutex"); BSP_OS_SemCreate(&ptz_hori_stop_mutex,1u,"ptz_hori_stop_mutex");

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

@ -238,7 +238,6 @@ static void ptz_recv_hori_servo_task()
stopTimeOut(H_MOTOR); stopTimeOut(H_MOTOR);
if ( ( MotorReplyForWrite(H_MOTOR) | MotorReplyForRead(H_MOTOR, &s_horiMotorSpeed) ) == false) if ( ( MotorReplyForWrite(H_MOTOR) | MotorReplyForRead(H_MOTOR, &s_horiMotorSpeed) ) == false)
{ {
beep_enable();
H_MOTOR_STOP; H_MOTOR_STOP;
} }
//释放信号量，通知能发送一次 //释放信号量，通知能发送一次
@ -261,7 +260,6 @@ static void ptz_recv_vert_servo_task()
stopTimeOut(V_MOTOR); stopTimeOut(V_MOTOR);
if ( ( MotorReplyForWrite(V_MOTOR) | MotorReplyForRead(V_MOTOR, &s_vertMotorSpeed) ) == false) if ( ( MotorReplyForWrite(V_MOTOR) | MotorReplyForRead(V_MOTOR, &s_vertMotorSpeed) ) == false)
{ {
beep_enable();
V_MOTOR_STOP; V_MOTOR_STOP;
} }
//释放信号量，通知能发送一次 //释放信号量，通知能发送一次

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

@ -526,9 +526,30 @@ void init_speed_module(void)
OSTimeDlyHMSM(0u, 0u, 0u, 500u);//等待硬件初始化成功 OSTimeDlyHMSM(0u, 0u, 0u, 500u);//等待硬件初始化成功
for( uint8_t i = 0; i < 2; i++)
{
servoSendData(i, WriteMotorOneReg(i, H02_CONTR_MODE_SELEC, 0),
WRITE_ONE_REG_FRAME_NUM, lowPriority);//切换为速度控制模式
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
servoSendData(i, WriteMotorOneReg(i, H06_SPEED_UP_SLOPE_VALUE, 3000)
, WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置加速时间常数
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
servoSendData(i, WriteMotorOneReg(i, H06_SPEED_DOWN_SLOPE_VALUE, 2000)
, WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置减速时间常数
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
servoSendData(i, WriteMotorOneReg(i, H06_SPEED_COMMU_SET_VALUE, 60)
, WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置电机转速为60r/min
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
servoSendData(i, WriteMotorOneReg(i, H03_DI1_LOGICAL_SELEC, 1)
, WRITE_ONE_REG_FRAME_NUM, lowPriority);//电机运行使能
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
}
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H02_CONTR_MODE_SELEC, 0),
// WRITE_ONE_REG_FRAME_NUM, lowPriority);
} }

18
BSP/Driver/servoMotor/modbus_crc.c
View File

@ -1,6 +1,9 @@
#include "stdint.h" #include "stdint.h"
#include "ptz_type_select.h"
static const uint8_t aucCRCHi[] = { #ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
static const uint8_t aucCRCHi[] = {
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
@ -23,9 +26,9 @@ static const uint8_t aucCRCHi[] = {
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40 0x00, 0xC1, 0x81, 0x40
}; };
static const uint8_t aucCRCLo[] = { static const uint8_t aucCRCLo[] = {
0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7,
0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E,
0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9,
@ -48,10 +51,10 @@ static const uint8_t aucCRCLo[] = {
0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83,
0x41, 0x81, 0x80, 0x40 0x41, 0x81, 0x80, 0x40
}; };
uint16_t ModbusCRC16(uint8_t *pucFrame, uint16_t usLen) uint16_t ModbusCRC16(uint8_t *pucFrame, uint16_t usLen)
{ {
uint8_t ucCRCHi = 0xFF; uint8_t ucCRCHi = 0xFF;
uint8_t ucCRCLo = 0xFF; uint8_t ucCRCLo = 0xFF;
int iIndex; int iIndex;
@ -63,4 +66,5 @@ uint16_t ModbusCRC16(uint8_t *pucFrame, uint16_t usLen)
ucCRCHi = aucCRCLo[iIndex]; ucCRCHi = aucCRCLo[iIndex];
} }
return (uint16_t)(ucCRCHi << 8 | ucCRCLo); return (uint16_t)(ucCRCHi << 8 | ucCRCLo);
} }
#endif

7
BSP/Driver/servoMotor/modbus_crc.h
View File

@ -1,7 +1,12 @@
#ifndef _MODBUS_CRC_H_ #ifndef _MODBUS_CRC_H_
#define _MODBUS_CRC_H_ #define _MODBUS_CRC_H_
#include "ptz_type_select.h"
uint16_t ModbusCRC16(uint8_t *pucFrame, uint16_t usLen); #ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
uint16_t ModbusCRC16(uint8_t *pucFrame, uint16_t usLen);
#endif
#endif #endif

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

@ -1,6 +1,8 @@
#include "motorCommu.h" #include "motorCommu.h"
#include "servoMotor_recv.h" #include "servoMotor_recv.h"
#include "speed_to_servoMotor.h" #include "speed_to_servoMotor.h"
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
/* /*
******************************************************************************************************** ********************************************************************************************************
* 电机串口通讯驱动初始化相关结构体 * 电机串口通讯驱动初始化相关结构体
@ -513,3 +515,4 @@ bool CommuRsvData(uint8_t motorNo, uint8_t* userBuff, uint32_t len)
} }
return true; return true;
} }
#endif

102
BSP/Driver/servoMotor/motorCommu.h
View File

@ -4,64 +4,70 @@
#include "gd32f4xx.h" #include "gd32f4xx.h"
#include "stdbool.h" #include "stdbool.h"
#include "string.h" #include "string.h"
/* #include "ptz_type_select.h"
********************************************************************************************************
* dma缓冲区相关 #ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
********************************************************************************************************
*/ /*
typedef struct ********************************************************************************************************
{ * dma缓冲区相关
********************************************************************************************************
*/
typedef struct
{
int16_t dmaTranFlag; /*dma发送是否在工作的标志位*/ int16_t dmaTranFlag; /*dma发送是否在工作的标志位*/
int32_t dmaSize; /*DMA接收缓冲区的大小*/ int32_t dmaSize; /*DMA接收缓冲区的大小*/
uint8_t *pDmaRsvBuff; /*指向接收DMA缓冲区的首地址*/ uint8_t *pDmaRsvBuff; /*指向接收DMA缓冲区的首地址*/
}CommuInfo_t; }CommuInfo_t;
#define DMA_TRANS_IDLE 0//dma当前未在发送数据 #define DMA_TRANS_IDLE 0//dma当前未在发送数据
#define DMA_TRANS_BUSY 1//dma当前正在发送数据 #define DMA_TRANS_BUSY 1//dma当前正在发送数据
#define DMA_BUFF_SIZE 64//dma缓冲区大小 #define DMA_BUFF_SIZE 64//dma缓冲区大小
//extern CommuInfo_t g_commuDeal;//来自motorCommu.c //extern CommuInfo_t g_commuDeal;//来自motorCommu.c
/* /*
******************************************************************************************************** ********************************************************************************************************
* 串口设备驱动初始化相关 * 串口设备驱动初始化相关
******************************************************************************************************** ********************************************************************************************************
*/ */
#define H_MOTOR 0//数组g_motorCommuInitBuff[MOTOR_NUM]位号 #define H_MOTOR 0//数组g_motorCommuInitBuff[MOTOR_NUM]位号
#define V_MOTOR 1 #define V_MOTOR 1
/*-------------485接收发送宏开关----------------------*/ /*-------------485接收发送宏开关----------------------*/
#define H_COMMU_RS485_TX gpio_bit_set(GPIOD, GPIO_PIN_10)//水平电机485发送 #define H_COMMU_RS485_TX gpio_bit_set(GPIOD, GPIO_PIN_10)//水平电机485发送
#define H_COMMU_RS485_RX gpio_bit_reset(GPIOD, GPIO_PIN_10)//水平电机485接收 #define H_COMMU_RS485_RX gpio_bit_reset(GPIOD, GPIO_PIN_10)//水平电机485接收
#define V_COMMU_RS485_TX gpio_bit_set(GPIOC, GPIO_PIN_8)//垂直电机485发送 #define V_COMMU_RS485_TX gpio_bit_set(GPIOC, GPIO_PIN_8)//垂直电机485发送
#define V_COMMU_RS485_RX gpio_bit_reset(GPIOC, GPIO_PIN_8)//垂直电机485接收 #define V_COMMU_RS485_RX gpio_bit_reset(GPIOC, GPIO_PIN_8)//垂直电机485接收
/** /**
* @brief 串口设备通讯驱动初始化 * @brief 串口设备通讯驱动初始化
* @param * @param
* @return * @return
*/ */
void CommuDrvInit(void); void CommuDrvInit(void);
/** /**
* @brief 数据的发送 * @brief 数据的发送
* @param motorNo：电机号，H_MOTOR：水平电机，V_MOTOR：垂直电机 * @param motorNo：电机号，H_MOTOR：水平电机，V_MOTOR：垂直电机
* @param buffer：需要发送的数据 * @param buffer：需要发送的数据
* @param len：发送数据的长度 * @param len：发送数据的长度
* @return ture:DMA空闲当前数据可以发送；false：DMA正在发送数据，当前数据不能发送 * @return ture:DMA空闲当前数据可以发送；false：DMA正在发送数据，当前数据不能发送
*/ */
bool CommuTransData(uint8_t motorNo, uint8_t* buffer, int32_t len); bool CommuTransData(uint8_t motorNo, uint8_t* buffer, int32_t len);
/** /**
* @brief 从接收循环缓冲区中读取指定长度的数据到用户数组 * @brief 从接收循环缓冲区中读取指定长度的数据到用户数组
* @param motorNo：电机号，H_MOTOR：水平电机，V_MOTOR：垂直电机 * @param motorNo：电机号，H_MOTOR：水平电机，V_MOTOR：垂直电机
* @param userBuff：从接收循环缓冲区中接收数据的数组 * @param userBuff：从接收循环缓冲区中接收数据的数组
* @param len：接收数据的长度 * @param len：接收数据的长度
* @return none * @return none
*/ */
bool CommuRsvData(uint8_t motorNo, uint8_t* userBuff, uint32_t len); bool CommuRsvData(uint8_t motorNo, uint8_t* userBuff, uint32_t len);
///*用于结构体数组赋值，方便外部使用此结构体数组*/ ///*用于结构体数组赋值，方便外部使用此结构体数组*/
//CommuHwInfo_t GetMotorCommuBuffStr(uint8_t motorNo); //CommuHwInfo_t GetMotorCommuBuffStr(uint8_t motorNo);
#endif
#endif #endif

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

@ -1,10 +1,12 @@
#include "servoMotor.h" #include "servoMotor.h"
#include <ucos_ii.h> #include <ucos_ii.h>
/*
水平垂直电机使能开关引脚配置 #ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
*/ /*
static void MotorSwitchGpioCofig(void) 水平垂直电机使能开关引脚配置
{ */
static void MotorSwitchGpioCofig(void)
{
/*GPIO时钟初始化*/ /*GPIO时钟初始化*/
rcu_periph_clock_enable(RCU_GPIOE); rcu_periph_clock_enable(RCU_GPIOE);
/*水平电机打开引脚*/ /*水平电机打开引脚*/
@ -14,24 +16,24 @@ static void MotorSwitchGpioCofig(void)
gpio_mode_set(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_1); gpio_mode_set(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_1);
gpio_output_options_set(GPIOE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_1); gpio_output_options_set(GPIOE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_1);
} }
/* /*
从机地址 功能码 寄存器地址高 寄存器地址低 数据高位 数据低位 crc校验高位 crc校验低位 从机地址 功能码 寄存器地址高 寄存器地址低 数据高位 数据低位 crc校验高位 crc校验低位
01H 06H 02H 00H 00H 01H 49H B2H 01H 06H 02H 00H 00H 01H 49H B2H
*/ */
/** /**
* @brief 伺服电机速度模式写单个寄存器 * @brief 伺服电机速度模式写单个寄存器
* @param motorNo：写垂直电机还是水平电机 * @param motorNo：写垂直电机还是水平电机
* @param regAddr：要写的寄存器 * @param regAddr：要写的寄存器
* @param data：要向寄存器写入的数据 * @param data：要向寄存器写入的数据
* @return 返回数组地址 * @return 返回数组地址
*/ */
static uint8_t g_HwriteOneRegBuff[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]; static uint8_t g_VwriteOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, uint16_t data) uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, uint16_t data)
{ {
static uint8_t g_writeOneRegBuff[WRITE_ONE_REG_FRAME_NUM]; static uint8_t g_writeOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
uint16_t crc; uint16_t crc;
g_writeOneRegBuff[0] = 0x01; g_writeOneRegBuff[0] = 0x01;
@ -51,24 +53,24 @@ uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, uint16_t data)
memcpy(g_VwriteOneRegBuff, g_writeOneRegBuff, WRITE_ONE_REG_FRAME_NUM); memcpy(g_VwriteOneRegBuff, g_writeOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
return g_VwriteOneRegBuff; return g_VwriteOneRegBuff;
} }
/* /*
从机地址 功能码 寄存器地址高 寄存器地址低 reg数量高位 数量低位 crc校验高位 crc校验低位 从机地址 功能码 寄存器地址高 寄存器地址低 reg数量高位 数量低位 crc校验高位 crc校验低位
01H 03H 0BH 00H 00H 01H 86H 2EH 01H 03H 0BH 00H 00H 01H 86H 2EH
*/ */
/** /**
* @brief 伺服电机速度模式读单个寄存器 * @brief 伺服电机速度模式读单个寄存器
* @param motorNo：读垂直电机还是水平电机 * @param motorNo：读垂直电机还是水平电机
* @param regAddr：要读的寄存器 * @param regAddr：要读的寄存器
* @return 返回数组地址 * @return 返回数组地址
*/ */
static uint8_t g_HreadOneRegBuff[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]; static uint8_t g_VreadOneRegBuff[READ_ONE_REG_FRAME_NUM];
uint8_t* ReadMotorOneReg(uint8_t motorNo, uint16_t regAddr) uint8_t* ReadMotorOneReg(uint8_t motorNo, uint16_t regAddr)
{ {
static uint8_t g_readOneRegBuff[READ_ONE_REG_FRAME_NUM]; static uint8_t g_readOneRegBuff[READ_ONE_REG_FRAME_NUM];
uint16_t crc; uint16_t crc;
g_readOneRegBuff[0] = 0x01; g_readOneRegBuff[0] = 0x01;
@ -88,21 +90,22 @@ uint8_t* ReadMotorOneReg(uint8_t motorNo, uint16_t regAddr)
memcpy(g_VreadOneRegBuff, g_readOneRegBuff, WRITE_ONE_REG_FRAME_NUM); memcpy(g_VreadOneRegBuff, g_readOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
return g_VreadOneRegBuff; return g_VreadOneRegBuff;
} }
/** /**
* @brief 伺服电机驱动初始化 * @brief 伺服电机驱动初始化
* @param * @param
* @return * @return
*/ */
void servoMotorInit(void) void servoMotorInit(void)
{ {
MotorSwitchGpioCofig();//两个电机电源的开关PE0,1引脚，如果其它地方实现了，可以不需要 MotorSwitchGpioCofig();//两个电机电源的开关PE0,1引脚，如果其它地方实现了，可以不需要
H_MOTOR_OPEN; H_MOTOR_OPEN;
OSTimeDlyHMSM(0u, 0u, 0u, 1000u); OSTimeDlyHMSM(0u, 0u, 0u, 1000u);
V_MOTOR_OPEN; V_MOTOR_OPEN;
CommuDrvInit();//伺服电机RS485通讯初始化 CommuDrvInit();//伺服电机RS485通讯初始化
} }
#endif

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

@ -4,85 +4,88 @@
#include "motorCommu.h" #include "motorCommu.h"
#include "modbus_crc.h" #include "modbus_crc.h"
#include "stdbool.h" #include "stdbool.h"
/* #include "ptz_type_select.h"
********************************************************************************************************
* 电机硬件相关
********************************************************************************************************
*/
#define H_MOTOR_OPEN gpio_bit_set(GPIOE,GPIO_PIN_0) //水平电机电源打开
#define H_MOTOR_STOP gpio_bit_reset(GPIOE,GPIO_PIN_0) //水平电机电源关闭
#define V_MOTOR_OPEN gpio_bit_set(GPIOE,GPIO_PIN_1) //垂直电机电源打开
#define V_MOTOR_STOP gpio_bit_reset(GPIOE,GPIO_PIN_1) //垂直电机电源关闭
#define H_MOTOR_ADDR 0x01//水平电机地址 #ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
#define V_MOTOR_ADDR 0x01//垂直电机地址 /*
/* ********************************************************************************************************
******************************************************************************************************** * 电机硬件相关
* 功能码命令 ********************************************************************************************************
******************************************************************************************************** */
*/ #define H_MOTOR_OPEN gpio_bit_set(GPIOE,GPIO_PIN_0) //水平电机电源打开
#define READ_ONE_REG 0X03//读单个寄存器 #define H_MOTOR_STOP gpio_bit_reset(GPIOE,GPIO_PIN_0) //水平电机电源关闭
#define READ_MULT_CONSE_REG 0X03//读多个连续的寄存器Read multiple consecutive registers #define V_MOTOR_OPEN gpio_bit_set(GPIOE,GPIO_PIN_1) //垂直电机电源打开
#define WRITE_ONE_REG 0X06//写单个寄存器 #define V_MOTOR_STOP gpio_bit_reset(GPIOE,GPIO_PIN_1) //垂直电机电源关闭
#define WRITE_MULT_CONSE_REG 0x10//写多个连续的寄存器
#define WRITE_ONE_REG_FRAME_NUM 8//写单个寄存器，数据帧的字节个数 #define H_MOTOR_ADDR 0x01//水平电机地址
#define READ_ONE_REG_FRAME_NUM 8//读单个寄存器，数据帧的字节个数 #define V_MOTOR_ADDR 0x01//垂直电机地址
/* /*
******************************************************************************************************** ********************************************************************************************************
* 寄存器参数 * 功能码命令
******************************************************************************************************** ********************************************************************************************************
*/ */
/*基本控制参数H02*/ #define READ_ONE_REG 0X03//读单个寄存器
#define H02_CONTR_MODE_SELEC 0X0200//（Control mode selection）控制模式选择0：速度模式，1：位置模式，2：转矩模式 #define READ_MULT_CONSE_REG 0X03//读多个连续的寄存器Read multiple consecutive registers
#define WRITE_ONE_REG 0X06//写单个寄存器
#define WRITE_MULT_CONSE_REG 0x10//写多个连续的寄存器
/*DI/DO参数H03~H04*/ #define WRITE_ONE_REG_FRAME_NUM 8//写单个寄存器，数据帧的字节个数
#define H03_DI1_FUNC_SELEC 0X0302//DI1端子功能选择,一个 DI 功能选项只能关联一个 DI 端子，不可重复分配 #define READ_ONE_REG_FRAME_NUM 8//读单个寄存器，数据帧的字节个数
#define H03_DI1_LOGICAL_SELEC 0X0303//DI1端子逻辑选择 /*
#define H04_DO1_FUNC_SELEC 0X0400//DO1端子功能选择 ********************************************************************************************************
#define H04_DO1_LOGICAL_SELEC 0X0401//DO1端子逻辑选择 * 寄存器参数
********************************************************************************************************
*/
/*基本控制参数H02*/
#define H02_CONTR_MODE_SELEC 0X0200//（Control mode selection）控制模式选择0：速度模式，1：位置模式，2：转矩模式
/*速度控制参数H06*/ /*DI/DO参数H03~H04*/
#define H06_SPEED_COMMAND_SELEC 0X0602//速度指令选择 #define H03_DI1_FUNC_SELEC 0X0302//DI1端子功能选择,一个 DI 功能选项只能关联一个 DI 端子，不可重复分配
#define H06_SPEED_COMMU_SET_VALUE 0X0603//速度指令通讯设置值,当 H06_02=0 时，通过此参数设定电机运行转速 #define H03_DI1_LOGICAL_SELEC 0X0303//DI1端子逻辑选择
#define H06_SPEED_UP_SLOPE_VALUE 0X0605//速度指令加速斜坡时间常数 #define H04_DO1_FUNC_SELEC 0X0400//DO1端子功能选择
#define H06_SPEED_DOWN_SLOPE_VALUE 0X0606//速度指令减速斜坡时间常数 #define H04_DO1_LOGICAL_SELEC 0X0401//DO1端子逻辑选择
#define H06_SPEED_REACH_MAX 0X0618//速度到达信号阈值
/*RS485通讯与功能参数H0C*/ /*速度控制参数H06*/
#define H0C_COMMU_PARAM_EEPR_UPDATE 0X0C13//MODBUS通讯写入是否更新到 EEPROM，设置1为写入 #define H06_SPEED_COMMAND_SELEC 0X0602//速度指令选择
#define H06_SPEED_COMMU_SET_VALUE 0X0603//速度指令通讯设置值,当 H06_02=0 时，通过此参数设定电机运行转速
#define H06_SPEED_UP_SLOPE_VALUE 0X0605//速度指令加速斜坡时间常数
#define H06_SPEED_DOWN_SLOPE_VALUE 0X0606//速度指令减速斜坡时间常数
#define H06_SPEED_REACH_MAX 0X0618//速度到达信号阈值
/*监视只读参数*/ /*RS485通讯与功能参数H0C*/
#define READ_MOTOR_SPEED_NOW 0X0B00//读取电机滤波后的实时转速 #define H0C_COMMU_PARAM_EEPR_UPDATE 0X0C13//MODBUS通讯写入是否更新到 EEPROM，设置1为写入
/*监视只读参数*/
#define READ_MOTOR_SPEED_NOW 0X0B00//读取电机滤波后的实时转速
/** /**
* @brief 伺服电机速度模式写单个寄存器 * @brief 伺服电机速度模式写单个寄存器
* @param motorNo：写垂直电机还是水平电机 * @param motorNo：写垂直电机还是水平电机
* @param regAddr：要写的寄存器 * @param regAddr：要写的寄存器
* @param data：要向寄存器写入的数据 * @param data：要向寄存器写入的数据
* @return false:写失败，当前DMA正在发送数据 * @return false:写失败，当前DMA正在发送数据
*/ */
uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, uint16_t data); uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, uint16_t data);
/** /**
* @brief 伺服电机速度模式读单个寄存器 * @brief 伺服电机速度模式读单个寄存器
* @param motorNo：读垂直电机还是水平电机 * @param motorNo：读垂直电机还是水平电机
* @param regAddr：要读的寄存器 * @param regAddr：要读的寄存器
* @return 返回数组地址 * @return 返回数组地址
*/ */
uint8_t* ReadMotorOneReg(uint8_t motorNo, uint16_t regAddr); uint8_t* ReadMotorOneReg(uint8_t motorNo, uint16_t regAddr);
/** /**
* @brief 伺服电机驱动初始化 * @brief 伺服电机驱动初始化
* @param * @param
* @return * @return
*/ */
void servoMotorInit(void); void servoMotorInit(void);
#endif
#endif #endif