211 lines
6.1 KiB
C
211 lines
6.1 KiB
C
|
||
#include "rotate_servo.h"
|
||
#include "speed_to_servoMotor.h"
|
||
|
||
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
|
||
|
||
static BSP_OS_SEM ptz_hori_stop_mutex;//共享资源锁
|
||
static BSP_OS_SEM ptz_vert_stop_mutex;
|
||
|
||
|
||
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);
|
||
}
|
||
|
||
void ptz_hori_start(char direction, float speed)
|
||
{
|
||
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
|
||
|
||
BSP_OS_SemPost(&ptz_hori_stop_mutex);
|
||
}
|
||
|
||
void ptz_hori_stop(unsigned short int time)
|
||
{
|
||
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
|
||
|
||
BSP_OS_SemPost(&ptz_hori_stop_mutex);
|
||
}
|
||
|
||
void ptz_vert_start(char direction, float speed)
|
||
{
|
||
BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
|
||
|
||
BSP_OS_SemPost(&ptz_vert_stop_mutex);
|
||
}
|
||
|
||
void ptz_vert_stop(unsigned short int time)
|
||
{
|
||
BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
|
||
|
||
BSP_OS_SemPost(&ptz_vert_stop_mutex);
|
||
}
|
||
|
||
static void ptz_hori_rotate_monitor_task()
|
||
{
|
||
static uint8_t status;
|
||
static uint16_t speed = 100;
|
||
switch( status )
|
||
{
|
||
case 0://切换为速度控制模式
|
||
|
||
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H02_CONTR_MODE_SELEC, 0)
|
||
, WRITE_ONE_REG_FRAME_NUM, lowPriority);
|
||
status = 1;
|
||
|
||
break;
|
||
|
||
case 1://设置加速度时间常数
|
||
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_UP_SLOPE_VALUE, 3000)
|
||
, WRITE_ONE_REG_FRAME_NUM, lowPriority);
|
||
status = 2;
|
||
break;
|
||
|
||
case 2://设置减速度时间常数
|
||
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_DOWN_SLOPE_VALUE, 2000)
|
||
, WRITE_ONE_REG_FRAME_NUM, lowPriority);
|
||
status = 3;
|
||
break;
|
||
|
||
case 3://设置运行速度
|
||
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)
|
||
, WRITE_ONE_REG_FRAME_NUM, lowPriority);
|
||
speed += 20;
|
||
speed %= 500;
|
||
status = 4;
|
||
break;
|
||
|
||
case 4://保存设定的参数并运行
|
||
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
|
||
, WRITE_ONE_REG_FRAME_NUM, lowPriority);
|
||
status = 3;
|
||
break;
|
||
}
|
||
}
|
||
|
||
static void ptz_vert_rotate_monitor_task()
|
||
{
|
||
|
||
}
|
||
|
||
|
||
static void ptz_hori_rotate_task()
|
||
{
|
||
while(1)
|
||
{
|
||
ptz_hori_rotate_monitor_task();
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 100u);
|
||
}
|
||
}
|
||
|
||
|
||
static void ptz_vert_rotate_task()
|
||
{
|
||
while(1)
|
||
{
|
||
ptz_vert_rotate_monitor_task();
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
|
||
}
|
||
}
|
||
|
||
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");
|
||
}
|
||
}
|
||
|
||
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 |