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

修复错误，现在水平接收电机返回的数据完全没有问题

This commit is contained in:
REASEARCHER\18383 2025-10-16 16:35:35 +08:00
parent c30d8e8981
commit 165565934a
7 changed files with 64 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APP/Appcfg/os_cfg.h
View File

@ -135,6 +135,6 @@
#define OS_TMR_CFG_MAX 12u /* Maximum number of timers */
#define OS_TMR_CFG_NAME_EN 1u /* Determine timer names */
#define OS_TMR_CFG_WHEEL_SIZE 8u /* Size of timer wheel (#Spokes) */
#define OS_TMR_CFG_TICKS_PER_SEC 1u /* 1s Rate at which timer management task runs (Hz) */
#define OS_TMR_CFG_TICKS_PER_SEC 5u /* 1s Rate at which timer management task runs (Hz) */
#endif

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

@ -80,42 +80,42 @@ void ptz_vert_stop(unsigned short int time)
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);
if (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);
if ( 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);
if ( 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;
if ( servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, 20)
, WRITE_ONE_REG_FRAME_NUM, lowPriority) )
status = 4;
break;
case 4://保存设定的参数并运行
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
, WRITE_ONE_REG_FRAME_NUM, lowPriority);
if ( servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
, WRITE_ONE_REG_FRAME_NUM, lowPriority) )
status = 3;
break;
default:
break;
}
}
@ -130,7 +130,7 @@ static void ptz_hori_rotate_task()
while(1)
{
ptz_hori_rotate_monitor_task();
OSTimeDlyHMSM(0u, 0u, 0u, 100u);
OSTimeDlyHMSM(0u, 0u, 0u, 3000u);
}
}

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

@ -25,7 +25,10 @@ static void ptz_recv_hori_servo_task()
{
}
else
{
H_MOTOR_STOP;
}
}
@ -43,7 +46,7 @@ static void ptz_recv_vert_servo_task()
while(1) {
OSSemPend(g_vertMotorMutex, 0, &err);
stopTimeOut(vertMotorType);
}
@ -98,8 +101,11 @@ static void creat_task_servo_recv_task(void)
void Init_ServoMotorRecv(void)
{
CPU_INT08U err;
g_horiMotorMutex = OSSemCreate(1);
g_vertMotorMutex = OSSemCreate(1);
OSSemPend(g_horiMotorMutex, 1, &err);
OSSemPend(g_vertMotorMutex, 1, &err);
creat_task_servo_recv_task();
}

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

@ -93,6 +93,8 @@ void servoLinkListMemPut(uint8_t motorType)
OSMemPut(g_memPtr, (uint8_t *)ptr);
}
}
//释放信号量，通知能发送一次
// OSSemPost(g_horiSpeedMutex);
}
else if (motorType == vertMotorType) {
if (g_servoMotorLinkList.vertMotor.linkListNum == highPriority) {
@ -123,10 +125,11 @@ void servoLinkListMemPut(uint8_t motorType)
OSMemPut(g_memPtr, (uint8_t *)ptr);
}
}
//释放信号量，通知能发送一次
// OSSemPost(g_vertSpeedMutex);
}
//释放信号量，通知能发送一次
OSSemPost(g_horiSpeedMutex);
}
/*!
@ -140,19 +143,19 @@ void servoLinkListMemPut(uint8_t motorType)
\param[out] none
\retval none
*/
void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priority)
bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priority)
{
if ((motor != horiMotorType) && (motor!= vertMotorType)) {
return;
return false;
}
if (data == NULL) {
return;
return false;
}
if (dataLen > sendDataBufLen) {
return;
return false;
}
if ((priority != highPriority) && (priority != lowPriority)) {
return;
return false;
}
linkList *ptr = NULL;
@ -161,7 +164,7 @@ void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
//保存数据到链表节点
ptr = OSMemGet(g_memPtr, &err);
if (ptr == NULL) {
return;
return false;
}
ptr->length = dataLen;
memcpy(ptr->data, data, dataLen);
@ -170,7 +173,7 @@ void 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;
return false;
}
if (priority == highPriority) {
@ -208,7 +211,7 @@ void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
else {
if (g_servoMotorLinkList.vertMotor.linkListNum > sendDataBufNumber) {
OSMemPut(g_memPtr, ptr);
return;
return false;
}
if (priority == highPriority) {
@ -243,6 +246,7 @@ void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
//释放信号量，通知能发送一次
OSSemPost(g_vertSpeedSem);
}
return true;
}
static void ptz_hori_servo_task()
@ -266,6 +270,7 @@ static void ptz_hori_servo_task()
, g_servoMotorLinkList.horiMotor.LinkListHead_H->length);
g_servoMotorLinkList.horiMotor.linkListNum = highPriority;
startTimeOut(horiMotorType);
continue;
}
@ -276,6 +281,7 @@ static void ptz_hori_servo_task()
, g_servoMotorLinkList.horiMotor.LinkListHead_L->length);
g_servoMotorLinkList.horiMotor.linkListNum = lowPriority;
startTimeOut(horiMotorType);
}
else {
pdebug(DEBUG_LEVEL_FATAL,"ptz_hori_servo_task no data...\n\r");
@ -300,9 +306,9 @@ static void ptz_vert_servo_task()
// 高优先级链表中数据先发送
if (g_servoMotorLinkList.vertMotor.LinkListHead_H != NULL) {
g_servoMotorLinkList.vertMotor.linkListNum = highPriority;
CommuTransData(V_MOTOR, g_servoMotorLinkList.vertMotor.LinkListHead_H->data
CommuTransData(V_MOTOR, g_servoMotorLinkList.vertMotor.LinkListHead_H->data
, g_servoMotorLinkList.vertMotor.LinkListHead_H->length);
startTimeOut(V_MOTOR);
continue;
}
@ -311,7 +317,7 @@ static void ptz_vert_servo_task()
g_servoMotorLinkList.vertMotor.linkListNum = highPriority;
CommuTransData(V_MOTOR, g_servoMotorLinkList.vertMotor.LinkListHead_L->data
, g_servoMotorLinkList.vertMotor.LinkListHead_L->length);
startTimeOut(V_MOTOR);
}
else {
pdebug(DEBUG_LEVEL_FATAL,"ptz_vert_servo_task no data...\n\r");
@ -388,9 +394,9 @@ BSP_OS_TMR vertServoSoftWareTim;
void servoCommSoftWareTimInit()
{
CPU_INT08U ServoSoftWareTimErr;
horiServoSoftWareTim = OSTmrCreate(10
, 10
, OS_TMR_OPT_PERIODIC
horiServoSoftWareTim = OSTmrCreate(1
, 100//1*200ms
, OS_TMR_OPT_ONE_SHOT
, (OS_TMR_CALLBACK)horiServoTimeOut
, (void *)0
, "tmr1"
@ -402,9 +408,9 @@ void servoCommSoftWareTimInit()
pdebug(DEBUG_LEVEL_FATAL,"create horiServoSoftWareTim failed...\n\r");
}
vertServoSoftWareTim = OSTmrCreate(10
, 10
, OS_TMR_OPT_PERIODIC
vertServoSoftWareTim = OSTmrCreate(1
, 1
, OS_TMR_OPT_ONE_SHOT
, (OS_TMR_CALLBACK)vertServoTimeOut
, (void *)0
, "tmr2"

2
APP/Device/Device_speed/speed_to_servoMotor.h
View File

@ -67,7 +67,7 @@ extern ptzServoLinkList g_servoMotorLinkList;
void ptz_send_speed(char dev, char speed);
void init_speed_module(void);
void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priority);
bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priority);
void servoLinkListMemPut(uint8_t motorType);
void stopTimeOut(uint8_t motorType);
void startTimeOut(uint8_t motorType);

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

@ -227,8 +227,8 @@ 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);
// dma_interrupt_enable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch, DMA_CHXCTL_FTFIE);
nvic_irq_enable(g_MotorDmaBuff[i].dmaRxIrq, 4, 2);
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);
}
}
@ -293,7 +293,6 @@ static void CommuDmaTra(uint8_t devNo, uint8_t *buffer,uint16_t len)
void DMA0_Channel3_IRQHandler(void)
{
/*
* 配合串口的发送。功能是继续发送缓冲区未发送的数据。
* 发送完成配置的CNT次数后，会进入此中断函数
*/
if(dma_interrupt_flag_get(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaTxch, DMA_INT_FLAG_FTF))
@ -302,9 +301,9 @@ void DMA0_Channel3_IRQHandler(void)
CommuInfo_t *pCommuDeal = g_commuInfoBuff[H_MOTOR].pCommuInfo;
while(usart_flag_get(g_motorCommuBuff[H_MOTOR].uartNo, USART_FLAG_TC) == RESET); // 等待串口发送完成
servoLinkListMemPut(horiMotorType);
H_COMMU_RS485_RX; //485切换为接收
pCommuDeal->dmaTranFlag = DMA_TRANS_IDLE;
servoLinkListMemPut(horiMotorType);
}
}
@ -324,19 +323,15 @@ void USART2_IRQHandler(void)
{
/* clear IDLE flag */
usart_interrupt_flag_clear(g_motorCommuBuff[H_MOTOR].uartNo, USART_INT_FLAG_IDLE); //第一步，读取stat0寄存器，清除IDLE标志位
usart_data_receive(g_motorCommuBuff[H_MOTOR].uartNo); //第二步，读取数据寄存器，清除IDLE标志位
// CommuInfo_t *pCommuDeal = g_commuInfoBuff[H_MOTOR].pCommuInfo;
usart_data_receive(g_motorCommuBuff[H_MOTOR].uartNo); //第二步，读取数据寄存器，清除IDLE标志位
g_horiLastPos = g_horiNowPos;
g_horiNowPos = DMA_BUFF_SIZE - dma_transfer_number_get(g_MotorDmaBuff[H_MOTOR].dmaNo,
g_MotorDmaBuff[H_MOTOR].dmaRxch);
//释放信号量，通知接收到一包数据，任务可以处理了
g_MotorDmaBuff[H_MOTOR].dmaRxch);//函数获取的是还有多少个没传输，而不是已经传输了多少
//释放信号量，通知能处理数据
OSSemPost(g_horiMotorMutex);
/*计算在DMA缓冲区需要获取的数据长度*/
// DmaIdleNum = dma_transfer_number_get(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);//获取的是还有多少个没传输，而不是已经传输了多少
//释放信号量，通知能发送一次
OSSemPost(g_horiSpeedMutex);
}
}
@ -382,7 +377,6 @@ void DMA1_Channel7_IRQHandler(void)
servoLinkListMemPut(vertMotorType);
V_COMMU_RS485_RX; //485切换为接收
pCommuDeal->dmaTranFlag = DMA_TRANS_IDLE;
}
}
@ -407,8 +401,10 @@ void USART5_IRQHandler(void)
g_vertLastPos = g_vertNowPos;
g_vertNowPos = DMA_BUFF_SIZE - dma_transfer_number_get(g_MotorDmaBuff[H_MOTOR].dmaNo,
g_MotorDmaBuff[H_MOTOR].dmaRxch);
//释放信号量，通知接收到一包数据，任务可以处理了
//释放信号量，通知接收任务处理
OSSemPost(g_vertMotorMutex);
//释放信号量，通知能发送一次
OSSemPost(g_vertSpeedMutex);
}
}
@ -420,10 +416,6 @@ void DMA1_Channel1_IRQHandler(void)
if(dma_interrupt_flag_get(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch, DMA_INT_FLAG_FTF))
{
dma_interrupt_flag_clear(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch, DMA_INT_FLAG_FTF);
// CommuInfo_t *pCommuDeal = g_commuInfoBuff[V_MOTOR].pCommuInfo;
//释放信号量，通知接收到一包数据，任务可以处理了
// OSSemPost(g_vertMotorMutex);
}
}

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

@ -42,11 +42,11 @@ uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, uint16_t data)
}
g_writeOneRegBuff[1] = WRITE_ONE_REG;
g_writeOneRegBuff[2] = regAddr >> 8;
g_writeOneRegBuff[3] = regAddr & 0xff;
g_writeOneRegBuff[3] = (uint8_t)(regAddr & 0xff);
g_writeOneRegBuff[4] = data >> 8;
g_writeOneRegBuff[5] = data & 0xff;
crc = ModbusCRC16(g_writeOneRegBuff, WRITE_ONE_REG_FRAME_NUM - 2);
g_writeOneRegBuff[6] = crc & 0xff;
g_writeOneRegBuff[6] = (uint8_t)(crc & 0xff);
g_writeOneRegBuff[7] = crc >> 8;
return g_writeOneRegBuff;
}
@ -56,9 +56,10 @@ uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, uint16_t data)
* @param motorNo：写垂直电机还是水平电机
* @return false:错误，读出的内容与写入的内容不一致
*/
static uint8_t motorReplybuff[WRITE_ONE_REG_FRAME_NUM];
bool MotorReplyForWrite(uint8_t motorNo)
{
static uint8_t motorReplybuff[WRITE_ONE_REG_FRAME_NUM];
/*提取数据失败*/
if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
{
@ -155,22 +156,4 @@ void servoMotorInit(void)
V_MOTOR_OPEN;
CommuDrvInit();//伺服电机RS485通讯初始化
// OSTimeDlyHMSM(0u, 0u, 0u, 500u);//等待硬件初始化成功
// WriteMotorOneReg(H_MOTOR, H02_CONTR_MODE_SELEC, 0);//H0200,选择速度模式
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
// WriteMotorOneReg(H_MOTOR, H04_DO1_FUNC_SELEC, 19);
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
// WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, 100);//速度设置为100rpm
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
// WriteMotorOneReg(H_MOTOR, H06_SPEED_UP_SLOPE_VALUE, 3000);//加速度3000
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
// WriteMotorOneReg(H_MOTOR, H06_SPEED_DOWN_SLOPE_VALUE, 2000);//减速度2000
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
// WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1);//只启动水平电机
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
}