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

修复电机通信bug

This commit is contained in:
起床就犯困 2025-10-18 13:35:20 +08:00
parent e082c9f795
commit 79b49fcc0b
17 changed files with 401 additions and 224 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
APP/Appcfg/app_cfg.h
View File

@ -181,6 +181,9 @@
#define TASK_HORI_PID_PRIO 20u
#define TASK_HORI_PID_STK_SIZE 150u
#define TASK_VERT_PID_PRIO 21u
#define TASK_VERT_PID_STK_SIZE 150u
#define TASK_VERT_DIRECTOR_SPEED_PWM_PRIO 21u
#define TASK_VERT_DIRECTOR_SPEED_PWM_STK_SIZE 150u
/***************/
@ -210,8 +213,8 @@
#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_VERT_PID_PRIO 34u
// #define TASK_VERT_PID_STK_SIZE 150u
#define TASK_FAULT_DETECT_PRIO 35u
#define TASK_FAULT_DETECT_STK_SIZE 180u

2
APP/Common/ptz_default_value.h
View File

@ -537,7 +537,7 @@
#define PTZ_HORI_MOTOR_DIRECTION 1
///水平电机加速度时间常数 单位ms (0-->>1000rpm)
#define PTZ_HORI_MOTOR_AccelerationTimeConstant 3000
///水平电机加速度时间常数 单位ms (1000-->>0rpm)
///水平电机减速度时间常数 单位ms (1000-->>0rpm)
#define PTZ_HORI_MOTOR_DecelerationTimeConstant 2000

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

@ -51,6 +51,43 @@ void ptz_sem_post_stop_mutex()
BSP_OS_SemPost(&ptz_vert_stop_mutex);
}
/*!
\brief 设置伺服电机速度
\param[in] data：储存发送数据的链表
\param[out] none
\retval none
*/
void set_speed_to_servoMotor(uint8_t motorType, float speed)
{
//转换为电机端的r/min
speed = speed * PTZ_HORI_RATIO + 0.5;
if (motorType == horiMotorType) {
if (speed > PTZ_HORI_MOTOR_MAX_SPEED) {
speed = PTZ_HORI_MOTOR_MAX_SPEED;
}
else if (speed < PTZ_HORI_MIN_SPEED) {
speed = PTZ_HORI_MOTOR_MIN_SPEED;
}
//设置速度，电机的r/min范围-6000~6000
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, (uint16_t)speed)
, WRITE_ONE_REG_FRAME_NUM, lowPriority);
}
else if (motorType == vertMotorType) {
if (speed > PTZ_VERT_MOTOR_MAX_SPEED) {
speed = PTZ_VERT_MOTOR_MAX_SPEED;
}
else if (speed < PTZ_VERT_MOTOR_MIN_SPEED) {
speed = PTZ_VERT_MOTOR_MIN_SPEED;
}
//设置速度，电机的r/min范围-6000~6000
servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, (uint16_t)speed)
, WRITE_ONE_REG_FRAME_NUM, lowPriority);
}
}
/*
///云台水平右转
#define PTZ_HORI_DIR_RIGHT 1
@ -68,7 +105,7 @@ void ptz_hori_start(char direction, float speed)//输入参数的speed是云台
case PTZ_HORI_DIR_STOP:
break;
case PTZ_HORI_DIR_LEFT:
speed*=-1;
speed = -speed;
default:
break;
}
@ -76,8 +113,9 @@ void ptz_hori_start(char direction, float speed)//输入参数的speed是云台
-------------------------------------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, H06_SPEED_COMMU_SET_VALUE, speed)//设置速度，电机的r/min范围-6000~6000
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);
set_speed_to_servoMotor(horiMotorType, speed);
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
, WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机
g_ptz.hori_speed_set = speed;
@ -105,10 +143,9 @@ void ptz_hori_stop(unsigned short int time)
//设定转动速度
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); //失能电机
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, 0)//设置速度
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);
set_speed_to_servoMotor(horiMotorType, 0);
g_ptz.hori_speed_set = 0;
g_ptz.hori_speed_actual = 0;
@ -118,6 +155,8 @@ void ptz_hori_stop(unsigned short int time)
ptz_hori_stop_count = 0;
}
ptz_hori_stop_count ++;
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
, WRITE_ONE_REG_FRAME_NUM, highPriority); //失能电机
//电子稳定
#ifdef PTZ_ELECTRIC_STABLE_L6235D
ptz_hori_electric_stable_init();
@ -126,7 +165,6 @@ void ptz_hori_stop(unsigned short int time)
BSP_OS_SemPost(&ptz_hori_stop_mutex);
}
void ptz_vert_start(char direction, float speed)//输入参数的speed是云台末端的r/min
{
BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
@ -137,7 +175,7 @@ void ptz_vert_start(char direction, float speed)//输入参数的speed是云台
case PTZ_VERT_DIR_DOWN:
break;
case PTZ_VERT_DIR_STOP:
speed*=-1;
speed = -speed;
default:
break;
}
@ -145,9 +183,8 @@ void ptz_vert_start(char direction, float speed)//输入参数的speed是云台
-------------------------------------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)
set_speed_to_servoMotor(vertMotorType, speed);
servoSendData(vertMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
, WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机
g_ptz.hori_speed_set = speed;
//设定转动方向
@ -175,10 +212,9 @@ void ptz_vert_stop(unsigned short int time)
//设定转动速度
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); //失能电机
// servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, 0)//设置速度
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);
set_speed_to_servoMotor(vertMotorType, 0);
g_ptz.vert_speed_set = 0;
g_ptz.vert_speed_actual = 0;
@ -187,6 +223,8 @@ void ptz_vert_stop(unsigned short int time)
OSTimeDlyHMSM(0u, 0u, 0u, time);
ptz_vert_stop_count = 0;
}
servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
, WRITE_ONE_REG_FRAME_NUM, highPriority); //失能电机
ptz_vert_stop_count ++;
//电子稳定
#ifdef PTZ_ELECTRIC_STABLE_L6235D

2
APP/Device/Device_rotate/rotate_servo.h
View File

@ -48,6 +48,8 @@ void ptz_hori_stop(unsigned short int time);
void ptz_vert_start(char direction, float speed);
void ptz_vert_stop(unsigned short int time);
void set_speed_to_servoMotor(uint8_t motorType, float speed);
void init_rotate_monitor_module(void);
#endif

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

@ -120,8 +120,9 @@ static bool MotorReplyForWrite(uint8_t motorNo)
* @return false:读取返回错误
*/
static bool MotorReplyForRead(uint8_t motorNo, int16_t* speed)
static bool MotorReplyForRead(uint8_t motorNo)
{
int16_t speed = 0;
static uint8_t motorReplybuff[READ_ONE_REG_FRAME_NUM];
if (motorNo == horiMotorType )
{
@ -138,8 +139,12 @@ static bool MotorReplyForRead(uint8_t motorNo, int16_t* speed)
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];
speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
if (speed < 0)
{
speed = -speed;
}
g_ptz.hori_speed_actual = (float)speed / PTZ_HORI_RATIO;
servoLinkListMemPut(horiMotorType);
}
else
@ -160,8 +165,12 @@ static bool MotorReplyForRead(uint8_t motorNo, int16_t* speed)
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];
speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
if (speed < 0)
{
speed = -speed;
}
g_ptz.hori_speed_actual = (float)speed / PTZ_HORI_RATIO;
servoLinkListMemPut(horiMotorType);
}
else
@ -186,8 +195,12 @@ static bool MotorReplyForRead(uint8_t motorNo, int16_t* speed)
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];
speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
if (speed < 0)
{
speed = -speed;
}
g_ptz.vert_speed_actual = (float)speed / PTZ_VERT_RATIO;
servoLinkListMemPut(vertMotorType);
}
else
@ -208,8 +221,12 @@ static bool MotorReplyForRead(uint8_t motorNo, int16_t* speed)
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];
speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
if (speed < 0)
{
speed = -speed;
}
g_ptz.vert_speed_actual = (float)speed / PTZ_VERT_RATIO;
servoLinkListMemPut(vertMotorType);
}
else
@ -232,14 +249,15 @@ static bool MotorReplyForRead(uint8_t motorNo, int16_t* speed)
static void ptz_recv_hori_servo_task()
{
CPU_INT08U err;
static int16_t s_horiMotorSpeed;
while(1) {
OSSemPend(g_horiMotorMutex, 0, &err);
stopTimeOut(H_MOTOR);
if ( ( MotorReplyForWrite(H_MOTOR) | MotorReplyForRead(H_MOTOR, &s_horiMotorSpeed) ) == false)
if ( ( MotorReplyForWrite(H_MOTOR) | MotorReplyForRead(H_MOTOR) ) == false)
{
H_MOTOR_STOP;
// H_MOTOR_STOP;
continue;
}
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
//释放信号量，通知能发送一次
OSSemPost(g_horiSpeedMutex);
}
@ -254,14 +272,15 @@ 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(V_MOTOR);
if ( ( MotorReplyForWrite(V_MOTOR) | MotorReplyForRead(V_MOTOR, &s_vertMotorSpeed) ) == false)
if ( ( MotorReplyForWrite(V_MOTOR) | MotorReplyForRead(V_MOTOR) ) == false)
{
V_MOTOR_STOP;
// V_MOTOR_STOP;
continue;
}
//释放信号量，通知能发送一次
OSSemPost(g_vertSpeedMutex);
}

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

@ -175,10 +175,18 @@ bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
//将节点添加进入链表中
if (motor == horiMotorType) {
if (g_servoMotorLinkList.horiMotor.linkListNum > sendDataBufNumber) {
OSMemPut(g_memPtr, ptr);
return false;
if (priority == highPriority) {
if (g_servoMotorLinkList.horiMotor.linkListNum >= sendDataBufNumber) {
OSMemPut(g_memPtr, ptr);
return false;
}
}
else {
if (g_servoMotorLinkList.horiMotor.linkListNum >= sendDataBufNumber - 1) {
OSMemPut(g_memPtr, ptr);
return false;
}
}
if (priority == highPriority) {
if (g_servoMotorLinkList.horiMotor.LinkListTail_H == NULL) {
@ -215,9 +223,17 @@ bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
OSSemPost(g_horiSpeedSem);
}
else {
if (g_servoMotorLinkList.vertMotor.linkListNum > sendDataBufNumber) {
OSMemPut(g_memPtr, ptr);
return false;
if (priority == highPriority) {
if (g_servoMotorLinkList.vertMotor.linkListNum >= sendDataBufNumber) {
OSMemPut(g_memPtr, ptr);
return false;
}
}
else {
if (g_servoMotorLinkList.vertMotor.linkListNum >= sendDataBufNumber - 1) {
OSMemPut(g_memPtr, ptr);
return false;
}
}
if (priority == highPriority) {
@ -390,7 +406,7 @@ static void creat_task_vert_servo_task(void)
void horiServoTimeOut()
{
servoLinkListMemPut(horiMotorType);
OSSemPost(g_horiSpeedMutex);
OSSemPost(g_horiSpeedMutex);
}
void vertServoTimeOut()
@ -404,7 +420,7 @@ BSP_OS_TMR vertServoSoftWareTim;
void servoCommSoftWareTimInit()
{
CPU_INT08U ServoSoftWareTimErr;
horiServoSoftWareTim = OSTmrCreate(100
horiServoSoftWareTim = OSTmrCreate(5
, 100//1*200ms
, OS_TMR_OPT_ONE_SHOT
, (OS_TMR_CALLBACK)horiServoTimeOut
@ -418,7 +434,7 @@ void servoCommSoftWareTimInit()
pdebug(DEBUG_LEVEL_FATAL,"create horiServoSoftWareTim failed...\n\r");
}
vertServoSoftWareTim = OSTmrCreate(100
vertServoSoftWareTim = OSTmrCreate(5
, 100
, OS_TMR_OPT_ONE_SHOT
, (OS_TMR_CALLBACK)vertServoTimeOut
@ -526,30 +542,42 @@ void init_speed_module(void)
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(horiMotorType, H02_CONTR_MODE_SELEC, 0),
WRITE_ONE_REG_FRAME_NUM, lowPriority);//切换为速度控制模式
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H02_CONTR_MODE_SELEC, 0),
WRITE_ONE_REG_FRAME_NUM, lowPriority);//切换为速度控制模式
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
// servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H02_MOTOR_DIR_SELEC, PTZ_HORI_MOTOR_DIRECTION)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置水平电机方向
// servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H02_MOTOR_DIR_SELEC, PTZ_VERT_MOTOR_DIRECTION)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置垂直电机方向
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
// servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H06_SPEED_UP_SLOPE_VALUE, PTZ_HORI_MOTOR_AccelerationTimeConstant)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置加速时间常数
// servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H06_SPEED_UP_SLOPE_VALUE, PTZ_VERT_MOTOR_AccelerationTimeConstant)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置加速时间常数
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
// servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H06_SPEED_DOWN_SLOPE_VALUE, PTZ_HORI_MOTOR_DecelerationTimeConstant)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置减速时间常数
// servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H06_SPEED_DOWN_SLOPE_VALUE, PTZ_VERT_MOTOR_DecelerationTimeConstant)
// , 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(horiMotorType, WriteMotorOneReg(horiMotorType, H03_DI1_LOGICAL_SELEC, 1)
, WRITE_ONE_REG_FRAME_NUM, lowPriority);//电机运行使能
servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H03_DI1_LOGICAL_SELEC, 1)
, WRITE_ONE_REG_FRAME_NUM, lowPriority);//电机运行使能
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
}

31
APP/Device/device_Other/device_adc_collect.c
View File

@ -1,5 +1,7 @@
#include "device_adc_collect.h"
#include "rotate_servo.h"
#include "speed_to_servoMotor.h"
// ADC_Phase_current H_ADC_Collect;
// ADC_Phase_current V_ADC_Collect;
@ -487,21 +489,26 @@ static char ptz_data_collect_task()
{
int i=0,j=0;
while(1)
{
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
{//电机处于启动状态
// H_ADC2_Phase_current();
}
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
{//电机处于启动状态
// V_ADC0_Phase_current();
}
{
// if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
// {//电机处于启动状态
// // H_ADC2_Phase_current();
// }
// if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
// {//电机处于启动状态
// // V_ADC0_Phase_current();
// }
if(j >= 100)
{
// //读取水平电机实时速度
// servoSendData(horiMotorType, ReadMotorOneReg(horiMotorType, READ_MOTOR_SPEED_NOW)
// , READ_ONE_REG_FRAME_NUM, lowPriority);
// //读取俯仰电机实时速度
// servoSendData(vertMotorType, ReadMotorOneReg(vertMotorType, READ_MOTOR_SPEED_NOW)
// , READ_ONE_REG_FRAME_NUM, lowPriority);
j=0;
//云台不自检关闭，打开采集任务
#ifndef PTZ_NO_SELF_CHECK

325
APP/Device/device_Other/device_interrupt.c
View File

@ -17,101 +17,7 @@
/// @note 修改日志
/// LH于2022-05-25
void EXTI_IRQ_init()
{
#ifdef L6235D
/*HALL中断，水平——PD13-H1,PD14-H2,PD15-H3，，垂直PC6-H1,PC7-H2,PC8-H3*/
nvic_irq_enable(EXTI10_15_IRQn, 3U, 3U);//水平，一个中断函数，中断线10-15
nvic_irq_enable(EXTI5_9_IRQn, 3U, 2U);//垂直，一个中断函数，中断线5-9
//对应引脚配置外部中断线
syscfg_exti_line_config(EXTI_SOURCE_GPIOD, EXTI_SOURCE_PIN13);
syscfg_exti_line_config(EXTI_SOURCE_GPIOD, EXTI_SOURCE_PIN14);
syscfg_exti_line_config(EXTI_SOURCE_GPIOD, EXTI_SOURCE_PIN15);
syscfg_exti_line_config(EXTI_SOURCE_GPIOC, EXTI_SOURCE_PIN6);
syscfg_exti_line_config(EXTI_SOURCE_GPIOC, EXTI_SOURCE_PIN7);
syscfg_exti_line_config(EXTI_SOURCE_GPIOC, EXTI_SOURCE_PIN8);
//外部中断线初始化
exti_init(EXTI_6, EXTI_INTERRUPT, EXTI_TRIG_RISING);//上升沿中断
exti_init(EXTI_7, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_init(EXTI_8, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_init(EXTI_13, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_init(EXTI_14, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_init(EXTI_15, EXTI_INTERRUPT, EXTI_TRIG_RISING);
//清除标志位
exti_interrupt_flag_clear(EXTI_6);
exti_interrupt_flag_clear(EXTI_7);
exti_interrupt_flag_clear(EXTI_8);
exti_interrupt_flag_clear(EXTI_13);
exti_interrupt_flag_clear(EXTI_14);
exti_interrupt_flag_clear(EXTI_15);
BSP_IntVectSet(39,EXTI5_9_IRQHandler);
BSP_IntEn(39);
BSP_IntVectSet(56,EXTI10_15_IRQHandler);
BSP_IntEn(56);
#endif
#ifdef Full_bridge //MOS+栅极驱动
rcu_periph_clock_enable(RCU_SYSCFG);
//配置引脚时钟
rcu_periph_clock_enable(RCU_GPIOE);
//水平——PE10-H1,PE11-H2,PE12-H3，，垂直PE13-H1,PE14-H2,PE15-H3
//设置引脚为输入模式
gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_10);
gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_11);
gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_12);
gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_13);
gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_14);
gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_15);
/*HALL中断，水平——PE10-H1,PE11-H2,PE12-H3，，垂直PE13-H1,PE14-H2,PE15-H3*/
//对应引脚配置外部中断线
syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN10);
syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN11);
syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN12);
syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN13);
syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN14);
syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN15);
//外部中断线初始化
exti_init(EXTI_10, EXTI_INTERRUPT, EXTI_TRIG_RISING);//上升沿中断
exti_init(EXTI_11, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_init(EXTI_12, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_init(EXTI_13, EXTI_INTERRUPT, EXTI_TRIG_RISING);//上升沿中断
exti_init(EXTI_14, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_init(EXTI_15, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_interrupt_enable(EXTI_10);
exti_interrupt_enable(EXTI_11);
exti_interrupt_enable(EXTI_12);
exti_interrupt_enable(EXTI_13);
exti_interrupt_enable(EXTI_14);
exti_interrupt_enable(EXTI_15);
//清除标志位
exti_interrupt_flag_clear(EXTI_10);
exti_interrupt_flag_clear(EXTI_11);
exti_interrupt_flag_clear(EXTI_12);
exti_interrupt_flag_clear(EXTI_13);
exti_interrupt_flag_clear(EXTI_14);
exti_interrupt_flag_clear(EXTI_15);
nvic_irq_enable(EXTI10_15_IRQn, 2U, 2U);//，一个中断函数，中断线10-15
BSP_IntVectSet(56,EXTI10_15_IRQHandler);
BSP_IntEn(56);
{
/*光电开关中断PE7-SW1，PE8-SW2，PE9-SW3*/
syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN7);
syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN8);
@ -142,47 +48,6 @@ void EXTI_IRQ_init()
BSP_IntVectSet(39,EXTI5_9_IRQHandler);
BSP_IntEn(39);
#else
/*光电开关中断PB0-SW1，PB1-SW2，PB2-SW3*/
nvic_irq_enable(EXTI0_IRQn, 2U, 0U);//垂直光电开关SW1
nvic_irq_enable(EXTI1_IRQn, 2U, 1U);//垂直光电开关SW2
nvic_irq_enable(EXTI2_IRQn, 2U, 2U);//水平光电开关SW3
syscfg_exti_line_config(EXTI_SOURCE_GPIOB, EXTI_SOURCE_PIN0);
syscfg_exti_line_config(EXTI_SOURCE_GPIOB, EXTI_SOURCE_PIN1);
syscfg_exti_line_config(EXTI_SOURCE_GPIOB, EXTI_SOURCE_PIN2);
//垂直光电开关SW1
#ifdef PTZ_SW1_DOWN_FALL_UPDATE
exti_init(EXTI_0, EXTI_INTERRUPT, EXTI_TRIG_FALLING);//下降沿中断
#endif
#ifdef PTZ_SW1_UP_RISE_UPDATE
exti_init(EXTI_0, EXTI_INTERRUPT, EXTI_TRIG_BOTH);//边沿中断
#endif
//垂直光电开关SW2
exti_init(EXTI_1, EXTI_INTERRUPT, EXTI_TRIG_FALLING);//下降沿中断
//水平光电开关SW3
#ifdef PTZ_SW3_LEFT_RISE_UPDATE
exti_init(EXTI_2, EXTI_INTERRUPT, EXTI_TRIG_BOTH);//边沿中断
#else
exti_init(EXTI_2, EXTI_INTERRUPT, EXTI_TRIG_FALLING);//下降沿中断
#endif
//清除标志位
exti_interrupt_flag_clear(EXTI_0);
exti_interrupt_flag_clear(EXTI_1);
exti_interrupt_flag_clear(EXTI_2);
BSP_IntVectSet(22,EXTI0_IRQHandler);
BSP_IntEn(22);
BSP_IntVectSet(23,EXTI1_IRQHandler);
BSP_IntEn(23);
BSP_IntVectSet(24,EXTI2_IRQHandler);
BSP_IntEn(24);
#endif
}
@ -594,6 +459,194 @@ void ptz_SW_IRQHandler(exti_line_enum sw_linex)
#endif
#endif
#ifdef PTZ_SERVO_MOTOR
#ifdef PTZ_NO_SELF_CHECK
if(g_ptz.Voltage > SWITCH_IRQ_V)
{
switch(sw_linex)
{
case EXTI_7://垂直光电开关SW1
#ifdef PTZ_PHOTOELECTRIC_SWITCH
asm("nop");
asm("nop");
asm("nop");
asm("nop");
asm("nop");
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
{
if(PS_VERT_SW1_READ == PS_HIGH)
{
g_ptz.vert_ps_sw1_up_rise ++;
}
}
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
{
if(PS_VERT_SW1_READ == PS_LOW)
{
g_ptz.vert_ps_sw1_down_fall ++;
}
}
#endif
break;
case EXTI_8://垂直光电开关SW2
#ifdef PTZ_PHOTOELECTRIC_SWITCH
asm("nop");
asm("nop");
asm("nop");
asm("nop");
asm("nop");
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
{
if(PS_VERT_SW2_READ == PS_LOW)
{
g_ptz.vert_ps_sw2_up_fall ++;
}
}
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
{
if(PS_VERT_SW2_READ == PS_HIGH)
{
g_ptz.vert_ps_sw2_down_rise ++;
}
}
#endif
break;
case EXTI_9://水平光电开关SW3
#ifdef PTZ_PHOTOELECTRIC_SWITCH
asm("nop");
asm("nop");
asm("nop");
asm("nop");
asm("nop");
if(PS_HORI_SW3_READ == PS_LOW)
{
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
{
g_ptz.hori_ps_sw3_right_fall ++;
}
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
{
g_ptz.hori_ps_sw3_left_fall ++;
}
}
if(PS_HORI_SW3_READ == PS_HIGH)//PS_HIGH
{
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
{
g_ptz.hori_ps_sw3_right_rise ++;
}
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
{
g_ptz.hori_ps_sw3_left_rise ++;
// term_printf(" hori_ps_sw3_left_rise \r\n");//中断不能使用串口打印，会占用较长时间
}
}
#endif
break;
default:break;
}
}
#else
switch(sw_linex)
{
case EXTI_7://垂直光电开关SW1
#ifdef PTZ_PHOTOELECTRIC_SWITCH
asm("nop");
asm("nop");
asm("nop");
asm("nop");
asm("nop");
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
{
if(PS_VERT_SW1_READ == PS_HIGH)
{
g_ptz.vert_ps_sw1_up_rise ++;
}
}
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
{
if(PS_VERT_SW1_READ == PS_LOW)
{
g_ptz.vert_ps_sw1_down_fall ++;
}
}
#endif
break;
case EXTI_8://垂直光电开关SW2
#ifdef PTZ_PHOTOELECTRIC_SWITCH
asm("nop");
asm("nop");
asm("nop");
asm("nop");
asm("nop");
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
{
if(PS_VERT_SW2_READ == PS_LOW)
{
g_ptz.vert_ps_sw2_up_fall ++;
}
}
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
{
if(PS_VERT_SW2_READ == PS_HIGH)
{
g_ptz.vert_ps_sw2_down_rise ++;
}
}
#endif
break;
case EXTI_9://水平光电开关SW3
#ifdef PTZ_PHOTOELECTRIC_SWITCH
asm("nop");
asm("nop");
asm("nop");
asm("nop");
asm("nop");
if(PS_HORI_SW3_READ == PS_LOW)
{
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
{
g_ptz.hori_ps_sw3_right_fall ++;
}
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
{
g_ptz.hori_ps_sw3_left_fall ++;
}
}
if(PS_HORI_SW3_READ == PS_HIGH)
{
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
{
g_ptz.hori_ps_sw3_right_rise ++;
}
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
{
g_ptz.hori_ps_sw3_left_rise ++;
// term_printf(" hori_ps_sw3_left_rise \r\n");//中断不能使用串口打印，会占用较长时间
}
}
#endif
break;
default:break;
}
#endif
#endif
}
/// @brief 水平HALL中断处理函数

5
APP/Device/device_Other/device_interrupt.h
View File

@ -51,6 +51,11 @@
#endif
//
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
#define SWITCH_IRQ_V 20
#endif
void ptz_SW_IRQHandler(exti_line_enum sw_linex);
void ptz_H_HALL_IRQHandler(exti_line_enum hall_linex);
void ptz_V_HALL_IRQHandler(exti_line_enum hall_linex);

4
APP/Device/device_Other/device_wdog.c
View File

@ -16,6 +16,7 @@
#include "gd32f4xx_gpio.h"
#include <includes.h>
#include "device_wdog.h"
/// 引脚初始化
///
/// @param none
@ -46,8 +47,7 @@ static void task_feeddog ()
{
OSTimeDlyHMSM(0u,0u,0u,20u);
//翻转电平喂狗
gpio_bit_toggle(GPIOE, GPIO_PIN_5);
gpio_bit_toggle(GPIOE, GPIO_PIN_5);
}
}

3
APP/Service/service_error_count.c
View File

@ -17,6 +17,7 @@
#include "service_error_count.h"
#include "rotate_servo.h"
#include "speed_to_servoMotor.h"
char error_conut_state;
#define COUNT_STATE 1
@ -55,8 +56,10 @@ static void ptz_hori_error_count_task()
case COUNT_STATE:
g_ptz.hori_as5047d.as5047d_state = 0;//数据不可正常使用
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
// set_speed_to_servoMotor(horiMotorType, ERROR_COUNT_SPEED);
// data = ptz_hori_choice_microstep(ERROR_COUNT_SPEED);
// g_ptz.hori_tmc2160 = data;
if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住
{
ptz_hori_start(PTZ_HORI_DIR_LEFT, ERROR_COUNT_SPEED);

7
APP/Service/service_selfcheck.c
View File

@ -21,6 +21,7 @@
#include "service_error_count.h"
#include "rotate_servo.h"
#include "speed_to_servoMotor.h"
#ifdef PTZ_BLDC_MOTOR
/// 云台全范围自检
@ -809,6 +810,7 @@ static unsigned char ptz_hori_complete_self_check_task()
{
//首先让云台水平向右转动
case PTZ_HORI_SELF_CHECK_COMPLETE_STEP:
set_speed_to_servoMotor(horiMotorType, PTZ_HORI_SELF_CHECK_SPEED);
if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住
{
ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED);
@ -885,7 +887,8 @@ static unsigned char ptz_hori_simplify_self_check_task()
switch(g_ptz.hori_self_check)
{
case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP:
if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住
set_speed_to_servoMotor(horiMotorType, PTZ_HORI_SELF_CHECK_SPEED);
if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住
{
ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED);
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 1;
@ -953,6 +956,7 @@ static unsigned char ptz_vert_complete_self_check_task()
{
///首先读取光电开关的状态，初步判断垂直状态
case PTZ_VERT_SELF_CHECK_COMPLETE_STEP:
set_speed_to_servoMotor(vertMotorType, PTZ_VERT_SELF_CHECK_SPEED);
g_ptz.vert_ps_sw1_down_fall = 0;
g_ptz.vert_ps_sw1_up_rise = 0;
g_ptz.vert_ps_sw2_up_fall = 0;
@ -1075,6 +1079,7 @@ static unsigned char ptz_vert_simplify_self_check_task()
switch(g_ptz.vert_self_check)
{
case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP:
set_speed_to_servoMotor(vertMotorType, PTZ_VERT_SELF_CHECK_SPEED);
g_ptz.vert_ps_sw1_down_fall = 0;
g_ptz.vert_ps_sw1_up_rise = 0;
g_ptz.vert_ps_sw2_up_fall = 0;

4
BSP/Driver/drv_adc.c
View File

@ -233,7 +233,7 @@ float ptz_Voltage_collect_adc1_task()
#endif
/* 间接测量，11倍分压/放大 */
adc1_v[adc1_v_num] = (float)value_V / 4096.0 * 3.381 * 12.55;
adc1_v[adc1_v_num] = (float)value_V / 4096.0 * 3.3 * 11;
adc1_v_num++;
if(adc1_v_num >= LB_V_TIMES)
@ -283,7 +283,7 @@ float ptz_Current_collect_adc1_task()
#endif
/* 间接测量 */
adc1_i[adc1_i_num] = ((float)value_I / 4096.0 * 3.381) * 10;
adc1_i[adc1_i_num] = ((float)value_I / 4096.0 * 3.3) * 10;
adc1_i_num++;
if(adc1_i_num >= LB_I_TIMES)

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

@ -208,7 +208,14 @@ static void DmaCofig(void)
dma_circulation_disable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaTxch);
dma_channel_subperipheral_select(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaTxch, g_MotorDmaBuff[i].dmaPeriph);
//dma中断配置
nvic_irq_enable(g_MotorDmaBuff[i].dmaTxIrq, 4, 3);
if( i == H_MOTOR )
{
nvic_irq_enable(g_MotorDmaBuff[H_MOTOR].dmaTxIrq, 1, 2);
}
else
{
nvic_irq_enable(g_MotorDmaBuff[V_MOTOR].dmaTxIrq, 1, 1);
}
dma_interrupt_enable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaTxch, DMA_CHXCTL_FTFIE);//dma发送完成中断
@ -229,7 +236,14 @@ static void DmaCofig(void)
dma_circulation_enable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch);//循环模式
dma_channel_subperipheral_select(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch, g_MotorDmaBuff[i].dmaPeriph);
//中断配置
nvic_irq_enable(g_MotorDmaBuff[i].dmaRxIrq, 4, 2);
if ( i == H_MOTOR )
{
nvic_irq_enable(g_MotorDmaBuff[H_MOTOR].dmaRxIrq, 0, 2);
}
else
{
nvic_irq_enable(g_MotorDmaBuff[V_MOTOR].dmaRxIrq, 0, 1);
}
dma_interrupt_enable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch, DMA_CHXCTL_FTFIE);
dma_channel_enable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch);
}

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

@ -32,7 +32,7 @@
*/
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)
uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, int16_t data)
{
static uint8_t g_writeOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
uint16_t crc;

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

@ -38,7 +38,7 @@
*/
/*基本控制参数H02*/
#define H02_CONTR_MODE_SELEC 0X0200//（Control mode selection）控制模式选择0：速度模式，1：位置模式，2：转矩模式
#define H02_MOTOR_DIR_SELEC 0X0202//电机旋转方向选择
/*DI/DO参数H03~H04*/
#define H03_DI1_FUNC_SELEC 0X0302//DI1端子功能选择,一个 DI 功能选项只能关联一个 DI 端子，不可重复分配
#define H03_DI1_LOGICAL_SELEC 0X0303//DI1端子逻辑选择
@ -68,7 +68,7 @@
* @param data：要向寄存器写入的数据
* @return false:写失败，当前DMA正在发送数据
*/
uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, uint16_t data);
uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, int16_t data);
/**

2
PROJECT/OS2.ewp
View File

@ -826,7 +826,7 @@
</option>
<option>
<name>IlinkIcfFile</name>
<state>D:\CompanyCode\newPro\servoMotor_xr\BSP\IAR\GD32F450xE.icf</state>
<state>D:\psx\Pan-Tilt\1.software\HY\new_ptz\servoMotor\BSP\IAR\GD32F450xE.icf</state>
</option>
<option>
<name>IlinkIcfFileSlave</name>