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_MAX 12u /* Maximum number of timers */
#define OS_TMR_CFG_NAME_EN 1u /* Determine timer names */ #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_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 #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 void ptz_hori_rotate_monitor_task()
{ {
static uint8_t status; static uint8_t status;
static uint16_t speed = 100;
switch( status ) switch( status )
{ {
case 0://切换为速度控制模式 case 0://切换为速度控制模式
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H02_CONTR_MODE_SELEC, 0) if (servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H02_CONTR_MODE_SELEC, 0)
, WRITE_ONE_REG_FRAME_NUM, lowPriority); , WRITE_ONE_REG_FRAME_NUM, lowPriority) )
status = 1; status = 1;
break; break;
case 1://设置加速度时间常数 case 1://设置加速度时间常数
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_UP_SLOPE_VALUE, 3000) if ( servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_UP_SLOPE_VALUE, 3000)
, WRITE_ONE_REG_FRAME_NUM, lowPriority); , WRITE_ONE_REG_FRAME_NUM, lowPriority) )
status = 2; status = 2;
break; break;
case 2://设置减速度时间常数 case 2://设置减速度时间常数
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_DOWN_SLOPE_VALUE, 2000) if ( servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_DOWN_SLOPE_VALUE, 2000)
, WRITE_ONE_REG_FRAME_NUM, lowPriority); , WRITE_ONE_REG_FRAME_NUM, lowPriority) )
status = 3; status = 3;
break; break;
case 3://设置运行速度 case 3://设置运行速度
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed) if ( servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, 20)
, WRITE_ONE_REG_FRAME_NUM, lowPriority); , WRITE_ONE_REG_FRAME_NUM, lowPriority) )
speed += 20;
speed %= 500;
status = 4; status = 4;
break; break;
case 4://保存设定的参数并运行 case 4://保存设定的参数并运行
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1) if ( servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
, WRITE_ONE_REG_FRAME_NUM, lowPriority); , WRITE_ONE_REG_FRAME_NUM, lowPriority) )
status = 3; status = 3;
break; break;
default:
break;
} }
} }
@ -130,7 +130,7 @@ static void ptz_hori_rotate_task()
while(1) while(1)
{ {
ptz_hori_rotate_monitor_task(); 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) { while(1) {
OSSemPend(g_vertMotorMutex, 0, &err); OSSemPend(g_vertMotorMutex, 0, &err);
stopTimeOut(vertMotorType);
} }
@ -98,8 +101,11 @@ static void creat_task_servo_recv_task(void)
void Init_ServoMotorRecv(void) void Init_ServoMotorRecv(void)
{ {
CPU_INT08U err;
g_horiMotorMutex = OSSemCreate(1); g_horiMotorMutex = OSSemCreate(1);
g_vertMotorMutex = OSSemCreate(1); g_vertMotorMutex = OSSemCreate(1);
OSSemPend(g_horiMotorMutex, 1, &err);
OSSemPend(g_vertMotorMutex, 1, &err);
creat_task_servo_recv_task(); creat_task_servo_recv_task();
} }

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

28
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_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); 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); nvic_irq_enable(g_MotorDmaBuff[i].dmaRxIrq, 4, 2);
// dma_interrupt_enable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch, DMA_CHXCTL_FTFIE); 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); 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) void DMA0_Channel3_IRQHandler(void)
{ {
/* /*
* 配合串口的发送。功能是继续发送缓冲区未发送的数据。
* 发送完成配置的CNT次数后，会进入此中断函数 * 发送完成配置的CNT次数后，会进入此中断函数
*/ */
if(dma_interrupt_flag_get(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaTxch, DMA_INT_FLAG_FTF)) 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; CommuInfo_t *pCommuDeal = g_commuInfoBuff[H_MOTOR].pCommuInfo;
while(usart_flag_get(g_motorCommuBuff[H_MOTOR].uartNo, USART_FLAG_TC) == RESET); // 等待串口发送完成 while(usart_flag_get(g_motorCommuBuff[H_MOTOR].uartNo, USART_FLAG_TC) == RESET); // 等待串口发送完成
servoLinkListMemPut(horiMotorType);
H_COMMU_RS485_RX; //485切换为接收 H_COMMU_RS485_RX; //485切换为接收
pCommuDeal->dmaTranFlag = DMA_TRANS_IDLE; pCommuDeal->dmaTranFlag = DMA_TRANS_IDLE;
servoLinkListMemPut(horiMotorType);
} }
} }
@ -326,17 +325,13 @@ void USART2_IRQHandler(void)
usart_interrupt_flag_clear(g_motorCommuBuff[H_MOTOR].uartNo, USART_INT_FLAG_IDLE); //第一步，读取stat0寄存器，清除IDLE标志位 usart_interrupt_flag_clear(g_motorCommuBuff[H_MOTOR].uartNo, USART_INT_FLAG_IDLE); //第一步，读取stat0寄存器，清除IDLE标志位
usart_data_receive(g_motorCommuBuff[H_MOTOR].uartNo); //第二步，读取数据寄存器，清除IDLE标志位 usart_data_receive(g_motorCommuBuff[H_MOTOR].uartNo); //第二步，读取数据寄存器，清除IDLE标志位
// CommuInfo_t *pCommuDeal = g_commuInfoBuff[H_MOTOR].pCommuInfo;
g_horiLastPos = g_horiNowPos; g_horiLastPos = g_horiNowPos;
g_horiNowPos = DMA_BUFF_SIZE - dma_transfer_number_get(g_MotorDmaBuff[H_MOTOR].dmaNo, 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); OSSemPost(g_horiMotorMutex);
//释放信号量，通知能发送一次
/*计算在DMA缓冲区需要获取的数据长度*/ OSSemPost(g_horiSpeedMutex);
// DmaIdleNum = dma_transfer_number_get(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);//获取的是还有多少个没传输，而不是已经传输了多少
} }
} }
@ -382,7 +377,6 @@ void DMA1_Channel7_IRQHandler(void)
servoLinkListMemPut(vertMotorType); servoLinkListMemPut(vertMotorType);
V_COMMU_RS485_RX; //485切换为接收 V_COMMU_RS485_RX; //485切换为接收
pCommuDeal->dmaTranFlag = DMA_TRANS_IDLE; pCommuDeal->dmaTranFlag = DMA_TRANS_IDLE;
} }
} }
@ -407,8 +401,10 @@ void USART5_IRQHandler(void)
g_vertLastPos = g_vertNowPos; g_vertLastPos = g_vertNowPos;
g_vertNowPos = DMA_BUFF_SIZE - dma_transfer_number_get(g_MotorDmaBuff[H_MOTOR].dmaNo, g_vertNowPos = DMA_BUFF_SIZE - dma_transfer_number_get(g_MotorDmaBuff[H_MOTOR].dmaNo,
g_MotorDmaBuff[H_MOTOR].dmaRxch); g_MotorDmaBuff[H_MOTOR].dmaRxch);
//释放信号量，通知接收到一包数据，任务可以处理了 //释放信号量，通知接收任务处理
OSSemPost(g_vertMotorMutex); 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)) 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); 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[1] = WRITE_ONE_REG;
g_writeOneRegBuff[2] = regAddr >> 8; g_writeOneRegBuff[2] = regAddr >> 8;
g_writeOneRegBuff[3] = regAddr & 0xff; g_writeOneRegBuff[3] = (uint8_t)(regAddr & 0xff);
g_writeOneRegBuff[4] = data >> 8; g_writeOneRegBuff[4] = data >> 8;
g_writeOneRegBuff[5] = data & 0xff; g_writeOneRegBuff[5] = data & 0xff;
crc = ModbusCRC16(g_writeOneRegBuff, WRITE_ONE_REG_FRAME_NUM - 2); 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; g_writeOneRegBuff[7] = crc >> 8;
return g_writeOneRegBuff; return g_writeOneRegBuff;
} }
@ -56,9 +56,10 @@ uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, uint16_t data)
* @param motorNo：写垂直电机还是水平电机 * @param motorNo：写垂直电机还是水平电机
* @return false:错误，读出的内容与写入的内容不一致 * @return false:错误，读出的内容与写入的内容不一致
*/ */
static uint8_t motorReplybuff[WRITE_ONE_REG_FRAME_NUM];
bool MotorReplyForWrite(uint8_t motorNo) bool MotorReplyForWrite(uint8_t motorNo)
{ {
static uint8_t motorReplybuff[WRITE_ONE_REG_FRAME_NUM];
/*提取数据失败*/ /*提取数据失败*/
if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false ) if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
{ {
@ -155,22 +156,4 @@ void servoMotorInit(void)
V_MOTOR_OPEN; V_MOTOR_OPEN;
CommuDrvInit();//伺服电机RS485通讯初始化 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);
} }