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);
} }

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

@ -1,4 +1,7 @@
#include "stdint.h" #include "stdint.h"
#include "ptz_type_select.h"
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
static const uint8_t aucCRCHi[] = { 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,
@ -64,3 +67,4 @@ uint16_t ModbusCRC16(uint8_t *pucFrame, uint16_t usLen)
} }
return (uint16_t)(ucCRCHi << 8 | ucCRCLo); return (uint16_t)(ucCRCHi << 8 | ucCRCLo);
} }
#endif

5
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"
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
uint16_t ModbusCRC16(uint8_t *pucFrame, uint16_t usLen); 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

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

@ -4,6 +4,10 @@
#include "gd32f4xx.h" #include "gd32f4xx.h"
#include "stdbool.h" #include "stdbool.h"
#include "string.h" #include "string.h"
#include "ptz_type_select.h"
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
/* /*
******************************************************************************************************** ********************************************************************************************************
* dma缓冲区相关 * dma缓冲区相关
@ -65,3 +69,5 @@ 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

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

@ -1,5 +1,7 @@
#include "servoMotor.h" #include "servoMotor.h"
#include <ucos_ii.h> #include <ucos_ii.h>
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
/* /*
水平垂直电机使能开关引脚配置 水平垂直电机使能开关引脚配置
*/ */
@ -106,3 +108,4 @@ void servoMotorInit(void)
CommuDrvInit();//伺服电机RS485通讯初始化 CommuDrvInit();//伺服电机RS485通讯初始化
} }
#endif

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

@ -4,6 +4,9 @@
#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"
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
/* /*
******************************************************************************************************** ********************************************************************************************************
* 电机硬件相关 * 电机硬件相关
@ -84,5 +87,5 @@ uint8_t* ReadMotorOneReg(uint8_t motorNo, uint16_t regAddr);
* @return * @return
*/ */
void servoMotorInit(void); void servoMotorInit(void);
#endif
#endif #endif