pengsx
/
servoMotor
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
servoMotor/APP/Device/Device_rotate/rotate_servo.c

294 lines
9.1 KiB
C
Raw Normal View History

修改宏定义
2025-10-11 03:40:39 +00:00
#include "rotate_servo.h"
第一次合并后，水平电机收发测试通过（垂直没有测试）
2025-10-16 03:44:18 +00:00
#include "speed_to_servoMotor.h"
修改宏定义
2025-10-11 03:40:39 +00:00
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
static BSP_OS_SEM ptz_hori_stop_mutex;//共享资源锁
static BSP_OS_SEM ptz_vert_stop_mutex;
修复bug，现在水平垂直发送数据，解析数据都没有问题
2025-10-17 08:38:15 +00:00
static char ptz_hori_stop_count;//水平停止计数，防止多次停止多次延时
static char ptz_vert_stop_count;//垂直停止计数，防止多次停止多次延时
修改宏定义
2025-10-11 03:40:39 +00:00
float ptz_vert_break_angle()
{
//云台水平轴加速度(°/s²)
const float a = 1000.0f * 6.0f
/ ((float)(PTZ_VERT_MOTOR_DecelerationTimeConstant / 1000.0f))
/ PTZ_VERT_RATIO;
const float tmp = a / 2.0f;
//平稳运行角度，以最低转速运行1秒
const float s1 = PTZ_VERT_BREAK_SPEED * 1;
//当前云台水平轴转速(°/s)
float Vnow = g_ptz.hori_speed_actual * 6.0f;
return (Vnow - PTZ_VERT_BREAK_SPEED) * (Vnow + PTZ_VERT_BREAK_SPEED) / tmp + s1;
}
float ptz_hori_break_angle()
{
//云台水平轴加速度(°/s²)
const float a = 1000.0f * 6.0f
/ ((float)(PTZ_HORI_MOTOR_DecelerationTimeConstant / 1000.0f))
/ PTZ_HORI_RATIO;
const float tmp = a / 2.0f;
//平稳运行角度，以最低转速运行1秒
const float s1 = PTZ_HORI_BREAK_SPEED * 1;
//当前云台水平轴转速(°/s)
float Vnow = g_ptz.hori_speed_actual * 6.0f;
return (Vnow - PTZ_HORI_BREAK_SPEED) * (Vnow + PTZ_HORI_BREAK_SPEED) / tmp + s1;
}
void ptz_sem_post_stop_mutex()
{
BSP_OS_SemPost(&ptz_hori_stop_mutex);
BSP_OS_SemPost(&ptz_vert_stop_mutex);
}
修复bug，现在水平垂直发送数据，解析数据都没有问题
2025-10-17 08:38:15 +00:00
/*
///云台水平右转
#define PTZ_HORI_DIR_RIGHT 1
///云台水平左转
#define PTZ_HORI_DIR_LEFT 3//0
///云台处于停止状态
#define PTZ_HORI_DIR_STOP 2
*/
水平、垂直电机发送，接收，解析测试通过，准备写位置
2025-10-17 10:20:03 +00:00
void ptz_hori_start(char direction, float speed)//输入参数的speed是云台末端的r/min
修改宏定义
2025-10-11 03:40:39 +00:00
{
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
水平、垂直电机发送，接收，解析测试通过，准备写位置
2025-10-17 10:20:03 +00:00
switch ( direction )
{
case PTZ_HORI_DIR_RIGHT://伺服电机默认速度为正使云台右转
case PTZ_HORI_DIR_STOP:
break;
case PTZ_HORI_DIR_LEFT:
speed*=-1;
default:
break;
}
/*
-------------------------------------add speed change to here--------------------------------------
*/
//设定转动速度
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)//设置速度，电机的r/min范围-6000~6000
, 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_hori_stop_count = 0;
修改宏定义
2025-10-11 03:40:39 +00:00
BSP_OS_SemPost(&ptz_hori_stop_mutex);
}
void ptz_hori_stop(unsigned short int time)
{
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
水平、垂直电机发送，接收，解析测试通过，准备写位置
2025-10-17 10:20:03 +00:00
//停止电机
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
修复bug，现在水平垂直发送数据，解析数据都没有问题
2025-10-17 08:38:15 +00:00
修改宏定义
2025-10-11 03:40:39 +00:00
BSP_OS_SemPost(&ptz_hori_stop_mutex);
}
水平、垂直电机发送，接收，解析测试通过，准备写位置
2025-10-17 10:20:03 +00:00
void ptz_vert_start(char direction, float speed)//输入参数的speed是云台末端的r/min
修改宏定义
2025-10-11 03:40:39 +00:00
{
BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
水平、垂直电机发送，接收，解析测试通过，准备写位置
2025-10-17 10:20:03 +00:00
switch ( direction )
{
case PTZ_VERT_DIR_UP://伺服电机默认速度为正使云台向上
case PTZ_VERT_DIR_DOWN:
break;
case PTZ_VERT_DIR_STOP:
speed*=-1;
default:
break;
}
/*
-------------------------------------add speed change to here--------------------------------------
*/
//设定转动速度
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)//设置速度，电机的r/min范围-6000~6000
, 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;
修复bug，现在水平垂直发送数据，解析数据都没有问题
2025-10-17 08:38:15 +00:00
修改宏定义
2025-10-11 03:40:39 +00:00
BSP_OS_SemPost(&ptz_vert_stop_mutex);
}
void ptz_vert_stop(unsigned short int time)
{
BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
水平、垂直电机发送，接收，解析测试通过，准备写位置
2025-10-17 10:20:03 +00:00
//停止电机
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
修改宏定义
2025-10-11 03:40:39 +00:00
BSP_OS_SemPost(&ptz_vert_stop_mutex);
}
static void ptz_hori_rotate_monitor_task()
{
水平、垂直电机发送，接收，解析测试通过，准备写位置
2025-10-17 10:20:03 +00:00
修改宏定义
2025-10-11 03:40:39 +00:00
}
static void ptz_vert_rotate_monitor_task()
{
水平、垂直电机发送，接收，解析测试通过，准备写位置
2025-10-17 10:20:03 +00:00
修改宏定义
2025-10-11 03:40:39 +00:00
}
static void ptz_hori_rotate_task()
{
while(1)
{
ptz_hori_rotate_monitor_task();
修复bug，现在水平垂直发送数据，解析数据都没有问题
2025-10-17 08:38:15 +00:00
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
修改宏定义
2025-10-11 03:40:39 +00:00
}
}
static void ptz_vert_rotate_task()
{
while(1)
{
ptz_vert_rotate_monitor_task();
修复bug，现在水平垂直发送数据，解析数据都没有问题
2025-10-17 08:38:15 +00:00
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
修改宏定义
2025-10-11 03:40:39 +00:00
}
}
static OS_STK task_hori_rotate_stk[TASK_HORI_ROATE_MONITOR_STK_SIZE];
static void creat_task_hori_rotate(void)
{
CPU_INT08U task_err;
CPU_INT08U name_err;
task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_rotate_task,
(void *) 0,
(OS_STK *)&task_hori_rotate_stk[TASK_HORI_ROATE_MONITOR_STK_SIZE - 1],
(INT8U ) TASK_HORI_ROATE_MONITOR_PRIO,
(INT16U ) TASK_HORI_ROATE_MONITOR_PRIO,
(OS_STK *)&task_hori_rotate_stk[0],
(INT32U ) TASK_HORI_ROATE_MONITOR_STK_SIZE,
(void *) 0,
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
#if (OS_TASK_NAME_EN > 0)
OSTaskNameSet(TASK_HORI_ROATE_MONITOR_PRIO, "ptz_hori_rotate_task", &name_err);
#endif
if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
pdebug(DEBUG_LEVEL_INFO,"create ptz_hori_rotate_task success...\n\r");
} else {
pdebug(DEBUG_LEVEL_FATAL,"create ptz_hori_rotate_task failed...\n\r");
}
}
static OS_STK task_vert_rotate_stk[TASK_VERT_ROATE_MONITOR_STK_SIZE];
static void creat_task_vert_rotate(void)
{
CPU_INT08U task_err;
CPU_INT08U name_err;
task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_rotate_task,
(void *) 0,
(OS_STK *)&task_vert_rotate_stk[TASK_VERT_ROATE_MONITOR_STK_SIZE - 1],
(INT8U ) TASK_VERT_ROATE_MONITOR_PRIO,
(INT16U ) TASK_VERT_ROATE_MONITOR_PRIO,
(OS_STK *)&task_vert_rotate_stk[0],
(INT32U ) TASK_VERT_ROATE_MONITOR_STK_SIZE,
(void *) 0,
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
#if (OS_TASK_NAME_EN > 0)
OSTaskNameSet(TASK_VERT_ROATE_MONITOR_PRIO, "ptz_vert_rotate_task", &name_err);
#endif
if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
pdebug(DEBUG_LEVEL_INFO,"create ptz_vert_rotate_task success...\n\r");
} else {
pdebug(DEBUG_LEVEL_FATAL,"create ptz_vert_rotate_task failed...\n\r");
}
}
水平、垂直电机发送，接收，解析测试通过，准备写位置
2025-10-17 10:20:03 +00:00
修改宏定义
2025-10-11 03:40:39 +00:00
void init_rotate_monitor_module(void)
{
BSP_OS_SemCreate(&ptz_hori_stop_mutex,1u,"ptz_hori_stop_mutex");
BSP_OS_SemCreate(&ptz_vert_stop_mutex,1u,"ptz_vert_stop_mutex");
creat_task_hori_rotate();
creat_task_vert_rotate();
}
#endif