yangmeng/optical
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 3 Wiki Activity
27e0231636
optical/NxFuncs/closedriver/drivermotion.c

487 lines
11 KiB
C
Raw Normal View History

调试完成
2025-09-04 01:45:08 +00:00
#include "drivermotion.h"
#if (DEV_MOTIONS_NUM > 0)
//----------------------------------------------------------------
// <20><><EFBFBD>ּ<EFBFBD><D6BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD>Ƶļ<C6B5><C4BC><EFBFBD>
#ifndef CALC_TIMES_PER_SEC
#define CALC_TIMES_PER_SEC (10000) // ÿ<><C3BF><EFBFBD>Ӽ<EFBFBD><D3BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#endif
#ifndef USE_FLOAT_CALC
#define USE_FLOAT_CALC 1 // 0, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, 1, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#endif
#if (USE_FLOAT_CALC == 1)
#define CALC_TYPE float
#define CALC_MUTI 1.0f
#else
#define CALC_TYPE s32
#define CALC_MUTI 10000
#endif
//----------------------------------------------------------------
typedef struct
{
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
CALC_TYPE startSpd; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3>ٶ<EFBFBD>
CALC_TYPE runSpd; // <20>˶<EFBFBD><CBB6>ٶ<EFBFBD>
CALC_TYPE runAdd; // <20><>ͣ<EFBFBD><CDA3><EFBFBD>ٶ<EFBFBD>
CALC_TYPE brkAdd; // ɲ<><C9B2><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
u32 funTestTime; // <20><><EFBFBD>й<EFBFBD><D0B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ⲿ<EFBFBD><E2B2BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1> 0:<3A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ⲿ<EFBFBD><E2B2BF><EFBFBD><EFBFBD> (<28><>λ:0.1ms)
// <20><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
int (*GetNormalStop)(u32,u32); // һ<><D2BB>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD>ٵ<EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3><EFBFBD>ٶȽ<D9B6><C8BD>٣<EFBFBD>
int (*GetQuickStop)(u32,u32); // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>; <20><>Ϊ<EFBFBD><CEAA>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD>ٵ<EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD>ٶȽ<D9B6><C8BD>٣<EFBFBD>
u32 condPara1, condPara2; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int blockRunflag; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>б<EFBFBD>־, =1<><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,ͬ<><CDAC><EFBFBD>˶<EFBFBD> =0,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C>첽<EFBFBD>˶<EFBFBD>
void (*ExecWhenRun)(void); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><D0B9><EFBFBD><EFBFBD><EFBFBD>ִ<EFBFBD>еĺ<D0B5><C4BA><EFBFBD>
void (*ExecWhenStart)(void); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱִ<CAB1>еĺ<D0B5><C4BA><EFBFBD>
void (*ExecWhenStop)(void); // <20><><EFBFBD><EFBFBD>ֹͣʱִ<CAB1>еĺ<D0B5><C4BA><EFBFBD>
void (*DelayWhenBlock)(u32); // <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>Delay<61><79><EFBFBD><EFBFBD>
//---------------------
// ״̬<D7B4><CCAC><EFBFBD><EFBFBD>
int runState; // <20><><EFBFBD><EFBFBD>״̬
s32 realpos; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int startrunflag; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>б<EFBFBD>־
int stopflag; // ֹͣ<CDA3><D6B9>־
//---------------------
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ʊ<EFBFBD><C6B1><EFBFBD>
int rundir; // <20><><EFBFBD>з<EFBFBD><D0B7><EFBFBD>
u32 totalNums; // <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
u32 pulseCount; // <20><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
u32 decPos; // <20><><EFBFBD>ٵ<EFBFBD>λ<EFBFBD><CEBB>
u32 funTestCount; // <20><><EFBFBD>й<EFBFBD><D0B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ⲿ<EFBFBD><E2B2BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
CALC_TYPE currentSpd; // <20><>ǰ<EFBFBD>ٶ<EFBFBD>
CALC_TYPE currentAcc; // <20><>ǰ<EFBFBD><C7B0><EFBFBD>ٶ<EFBFBD>
CALC_TYPE currentPos; // <20><>ǰλ<C7B0><CEBB>
//---------------------
} DriverMotionCtrl;
DriverMotionCtrl g_drvMotionsCtrlList[DEV_MOTIONS_NUM];
//----------------------------------------------------------------
// <20><><EFBFBD>ּ<EFBFBD><D6BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void IntegrationMotionCalc(DriverMotionCtrl * pCtrl)
{
if (pCtrl == NULL ||
0 )
{
return;
}
if (pCtrl->runState <= DRV_MOTION_STOP ||
pCtrl->startrunflag <= 0) // ֹͣ״̬
{
return; // <20><>ִ<EFBFBD><D6B4>
}
if (pCtrl->stopflag != 0)
{
if (pCtrl->stopflag == 1)
{
pCtrl->currentAcc = pCtrl->runAdd; // <20><>ǰ<EFBFBD><C7B0><EFBFBD>ٶ<EFBFBD>
pCtrl->runState = DRV_MOTION_NSTOP;
}
else
{
pCtrl->currentAcc = pCtrl->brkAdd; // <20><>ǰ<EFBFBD><C7B0><EFBFBD>ٶ<EFBFBD>
pCtrl->runState = DRV_MOTION_ESTOP;
}
}
if (pCtrl->funTestTime > 0) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
pCtrl->funTestCount++; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if ((pCtrl->funTestCount >= pCtrl->funTestTime) && // <20><><EFBFBD>˲<EFBFBD><CBB2><EFBFBD>ʱ<EFBFBD><CAB1>
1 )
{
pCtrl->funTestCount = 0;
if (pCtrl->GetNormalStop != NULL) // һ<><D2BB>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
{
if ((pCtrl->runState == DRV_MOTION_ADD) || // <20><><EFBFBD><EFBFBD>
(pCtrl->runState == DRV_MOTION_RUN) || // <20><><EFBFBD><EFBFBD>
(pCtrl->runState == DRV_MOTION_DEC) || // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
0 )
{
if (pCtrl->GetNormalStop(pCtrl->condPara1, pCtrl->condPara2) != 0)
{
pCtrl->currentAcc = pCtrl->runAdd; // <20><>ǰ<EFBFBD><C7B0><EFBFBD>ٶ<EFBFBD>
pCtrl->runState = DRV_MOTION_NSTOP; // <20><>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
}
}
}
if (pCtrl->GetQuickStop != NULL) // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
{
if ((pCtrl->runState == DRV_MOTION_ADD) || // <20><><EFBFBD><EFBFBD>
(pCtrl->runState == DRV_MOTION_RUN) || // <20><><EFBFBD><EFBFBD>
(pCtrl->runState == DRV_MOTION_DEC) || // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
(pCtrl->runState == DRV_MOTION_ESTOP) || // <20><><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
0 )
{
if (pCtrl->GetQuickStop(pCtrl->condPara1, pCtrl->condPara2) != 0)
{
pCtrl->currentAcc = pCtrl->brkAdd; // <20><>ǰ<EFBFBD><C7B0><EFBFBD>ٶ<EFBFBD>
pCtrl->runState = DRV_MOTION_ESTOP; // <20><>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
}
}
}
if (pCtrl->ExecWhenRun != NULL) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><D0B9><EFBFBD><EFBFBD><EFBFBD>ִ<EFBFBD>еĺ<D0B5><C4BA><EFBFBD>
{
pCtrl->ExecWhenRun();
}
}
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (pCtrl->runState == DRV_MOTION_ADD)
{
pCtrl->currentSpd += pCtrl->currentAcc; // <20>ı䵱ǰ<E4B5B1>ٶ<EFBFBD>
if (pCtrl->currentSpd >= pCtrl->runSpd) // <20><>ǰ<EFBFBD>ٶȴ<D9B6><C8B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬
{
pCtrl->runState = DRV_MOTION_RUN;
pCtrl->currentSpd = pCtrl->runSpd;
pCtrl->decPos = (pCtrl->totalNums - pCtrl->pulseCount); // <20><><EFBFBD>ٵ<EFBFBD>λ<EFBFBD><CEBB>
}
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (pCtrl->runState == DRV_MOTION_DEC) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
pCtrl->currentSpd -= pCtrl->currentAcc; // <20><>ǰ<EFBFBD>ٶ<EFBFBD>
if (pCtrl->currentSpd <= pCtrl->startSpd)
{
pCtrl->currentSpd = pCtrl->startSpd;
}
}
// <20><><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD>
if ((pCtrl->runState == DRV_MOTION_NSTOP) || // <20><><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
(pCtrl->runState == DRV_MOTION_ESTOP) || // <20><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
0 )
{
pCtrl->currentSpd -= pCtrl->currentAcc; // <20><>ǰ<EFBFBD>ٶ<EFBFBD>
if (pCtrl->currentSpd <= pCtrl->startSpd) // <20><>ǰ<EFBFBD>ٶ<EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>,<2C><><EFBFBD><EFBFBD>ֹͣ״̬
{
pCtrl->currentSpd = pCtrl->startSpd;
pCtrl->runState = DRV_MOTION_STOPING;
}
}
// λ<>ü<EFBFBD><C3BC><EFBFBD>
{
pCtrl->currentPos += pCtrl->currentSpd * pCtrl->rundir; // <20>ٶȻ<D9B6><C8BB><EFBFBD>
if (pCtrl->currentPos > CALC_MUTI || pCtrl->currentPos < -CALC_MUTI) // λ<><CEBB><EFBFBD>Ƴ<EFBFBD>
{
int ofst = (int)(pCtrl->currentPos/CALC_MUTI); // <20><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>ƫ<EFBFBD><C6AB>
pCtrl->currentPos -= ofst*CALC_MUTI;
pCtrl->realpos += ofst; // λ<>øı<C3B8>
pCtrl->pulseCount += abs(ofst); // <20><><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>
}
}
// ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
{
if ((pCtrl->pulseCount >= pCtrl->totalNums) || // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
(pCtrl->runState <= DRV_MOTION_STOP) || // ֹͣ<CDA3>ź<EFBFBD>
(pCtrl->runState == DRV_MOTION_STOPING) || // ֹͣ<CDA3><D6B9>
0 )
{
if (pCtrl->runState > DRV_MOTION_STOP) // <20><><EFBFBD><EFBFBD>״̬
{
pCtrl->runState = DRV_MOTION_STOP; // <20><><EFBFBD><EFBFBD>Ϊ<EFBFBD>ر<EFBFBD>״̬
pCtrl->startrunflag = 0; // <20>ر<EFBFBD><D8B1><EFBFBD><EFBFBD>б<EFBFBD>־
if (pCtrl->ExecWhenStop != NULL) // ִ<>йر<D0B9>ʱ<EFBFBD>ĺ<EFBFBD><C4BA><EFBFBD>
{
pCtrl->ExecWhenStop();
}
}
}
else
{
if ( (pCtrl->runState == DRV_MOTION_ADD) && // <20><><EFBFBD><EFBFBD>״̬<D7B4><CCAC>
(pCtrl->pulseCount >= (pCtrl->totalNums>>1)) && // <20><><EFBFBD><EFBFBD>λ<EFBFBD>Ƶ<EFBFBD>һ<EFBFBD><D2BB>
1 )
{
pCtrl->runState = DRV_MOTION_RUN;
pCtrl->currentAcc = pCtrl->runAdd; // <20><>ǰ<EFBFBD><C7B0><EFBFBD>ٶ<EFBFBD>
pCtrl->decPos = pCtrl->pulseCount;
#if (0)
#if (USE_FLOAT_CALC == 0)
printf("enter dec, spd=%d\r\n", pCtrl->currentSpd);
#else
#endif
#endif
}
if ( (pCtrl->runState == DRV_MOTION_RUN) && // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
(pCtrl->pulseCount >= pCtrl->decPos) && // <20><><EFBFBD>ڽ<EFBFBD><DABD>ٵ<EFBFBD>
1 )
{
pCtrl->runState = DRV_MOTION_DEC;
pCtrl->currentAcc = pCtrl->runAdd; // <20><>ǰ<EFBFBD><C7B0><EFBFBD>ٶ<EFBFBD>
}
}
}
}
CALC_TYPE pps_to_ppg(s32 pps)
{
return pps*CALC_MUTI / CALC_TIMES_PER_SEC;
}
//----------------------------------------------------------------
// <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD>ƣ<EFBFBD>ֹͣ״̬ʱ<CCAC><CAB1><EFBFBD>ã<EFBFBD>
int DrvMotionStart(int driverIdx, DrvMotionPara * pPara)
{
DriverMotionCtrl * pCtrl;
if (driverIdx < 0 || driverIdx >= DEV_MOTIONS_NUM)
{
printf("DrvMotionStart fail, driverIdx error\r\n");
return -1;
}
pCtrl = &(g_drvMotionsCtrlList[driverIdx]);
if (pPara == NULL)
{
printf("DrvMotionStart fail, pPara is NULL\r\n");
return -1;
}
if (pCtrl->runState <= DRV_MOTION_STOP) // ͣ<><CDA3>/<2F><><EFBFBD><EFBFBD>״̬<D7B4><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
pCtrl->startrunflag = 0; // <20>ر<EFBFBD><D8B1><EFBFBD><EFBFBD>б<EFBFBD>־
if (pPara->movement == 0) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
pCtrl->runState = DRV_MOTION_STOP;
printf("driverIdx=%d, movement=0\r\n", driverIdx);
pCtrl->rundir = 0;
return 0;
}
if (pPara->startPPS < 10)
{
pPara->startPPS = 10;
}
if (pPara->runPPS < 10)
{
pPara->runPPS = 10;
}
if (pPara->startPPS > pPara->runPPS)
{
pPara->startPPS = pPara->runPPS;
}
if (pPara->addPPSS < 1)
{
pPara->addPPSS = 1;
}
// <20><><EFBFBD><EFBFBD><EFBFBD>ٶȲ<D9B6><C8B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pCtrl->startSpd = pps_to_ppg(pPara->startPPS); // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3>ٶ<EFBFBD>
pCtrl->runSpd = pps_to_ppg(pPara->runPPS); // <20>˶<EFBFBD><CBB6>ٶ<EFBFBD>
pCtrl->runAdd = pps_to_ppg(pPara->addPPSS); // <20><>ͣ<EFBFBD><CDA3><EFBFBD>ٶ<EFBFBD>
pCtrl->brkAdd = pps_to_ppg(pPara->brkPPSS); // ɲ<><C9B2><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
pCtrl->currentSpd = pCtrl->startSpd; // <20><>ǰ<EFBFBD>ٶ<EFBFBD>
pCtrl->currentAcc = pCtrl-> runAdd; // <20><>ǰ<EFBFBD><C7B0><EFBFBD>ٶ<EFBFBD>
pCtrl->currentPos = 1; // <20><>ǰλ<C7B0><CEBB>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pCtrl->runState = DRV_MOTION_ADD; // <20><><EFBFBD><EFBFBD>״̬, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pCtrl->funTestTime = pPara->funTestTime;
pCtrl->funTestCount = 0; // <20><><EFBFBD>й<EFBFBD><D0B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ⲿ<EFBFBD><E2B2BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// <20><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
pCtrl->GetNormalStop = pPara->GetNormalStop; // һ<><D2BB>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pCtrl->GetQuickStop = pPara->GetQuickStop; // <20><><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pCtrl->condPara1 = pPara->condPara1; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pCtrl->condPara2 = pPara->condPara2; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pCtrl->blockRunflag = pPara->blockRunflag; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>б<EFBFBD>־
pCtrl->ExecWhenStart = pPara->ExecWhenStart; // <20><><EFBFBD><EFBFBD>ʱִ<CAB1>еĺ<D0B5><C4BA><EFBFBD>
pCtrl->ExecWhenRun = pPara->ExecWhenRun; // <20><><EFBFBD>й<EFBFBD><D0B9><EFBFBD><EFBFBD><EFBFBD>ִ<EFBFBD>еĺ<D0B5><C4BA><EFBFBD>
pCtrl->ExecWhenStop = pPara->ExecWhenStop; // ֹͣʱִ<CAB1>еĺ<D0B5><C4BA><EFBFBD>
pCtrl->DelayWhenBlock = pPara->DelayWhenBlock; // <20><><EFBFBD><EFBFBD>ʱִ<CAB1>еĺ<D0B5><C4BA><EFBFBD>
if (pCtrl->ExecWhenStart != NULL) // <20><><EFBFBD><EFBFBD>ǰִ<C7B0><D6B4>
{
pCtrl->ExecWhenStart();
}
//---------------------
// λ<><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pCtrl->rundir = (pPara->movement > 0) ? 1:-1; // <20><><EFBFBD><EFBFBD>
pCtrl->totalNums = abs(pPara->movement); // <20><>λ<EFBFBD><CEBB>
pCtrl->pulseCount = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pCtrl->decPos = pCtrl->totalNums; // <20><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pCtrl->stopflag = 0;
pCtrl->startrunflag = 1; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>б<EFBFBD>־
return DrvMotionRun(driverIdx);
}
else
{
printf("DrvMotionStart failed, drv%d is running\r\n", driverIdx);
return -2; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
}
// ֹͣ<CDA3>˶<EFBFBD><CBB6><EFBFBD><EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>ʱ<EFBFBD>õ<EFBFBD><C3B5><EFBFBD>
void DrvMotionStop(int driverIdx)
{
DriverMotionCtrl * pCtrl;
if (driverIdx < 0 || driverIdx >= DEV_MOTIONS_NUM)
{
return;
}
pCtrl = &(g_drvMotionsCtrlList[driverIdx]);
pCtrl->stopflag = 1;
}
// ֹͣ<CDA3>˶<EFBFBD><CBB6><EFBFBD><EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>ʱ<EFBFBD>õ<EFBFBD><C3B5><EFBFBD>
void DrvMotionBreak(int driverIdx)
{
DriverMotionCtrl * pCtrl;
if (driverIdx < 0 || driverIdx >= DEV_MOTIONS_NUM)
{
return;
}
pCtrl = &(g_drvMotionsCtrlList[driverIdx]);
pCtrl->stopflag = 2;
}
int DrvMotionRun(int driverIdx)
{
DriverMotionCtrl * pCtrl;
if (driverIdx < 0 || driverIdx >= DEV_MOTIONS_NUM)
{
return -1;
}
pCtrl = &(g_drvMotionsCtrlList[driverIdx]);
// <20><><EFBFBD><EFBFBD>ѭ<EFBFBD><D1AD>
do
{
if (pCtrl->runState <= DRV_MOTION_STOP)
{
break;
}
if (pCtrl->blockRunflag == 0) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>б<EFBFBD>־
{
break;
}
if (pCtrl->DelayWhenBlock != NULL)
{
pCtrl->DelayWhenBlock(0);
}
}while(1);
return pCtrl->runState;
}
// <20>õ<EFBFBD><C3B5>˶<EFBFBD>״̬<D7B4><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>ʱ<EFBFBD>õ<EFBFBD><C3B5><EFBFBD>
int GetDrvMotionState(int driverIdx)
{
DriverMotionCtrl * pCtrl;
if (driverIdx < 0 || driverIdx >= DEV_MOTIONS_NUM)
{
return 0;
}
pCtrl = &(g_drvMotionsCtrlList[driverIdx]);
return pCtrl->runState;
}
// <20>õ<EFBFBD><C3B5><EFBFBD>ǰ<EFBFBD>˶<EFBFBD>λ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>ʱ<EFBFBD>õ<EFBFBD><C3B5><EFBFBD>
s32 GetDrvMotionRealPos(int driverIdx)
{
DriverMotionCtrl * pCtrl;
if (driverIdx < 0 || driverIdx >= DEV_MOTIONS_NUM)
{
return 0;
}
pCtrl = &(g_drvMotionsCtrlList[driverIdx]);
return pCtrl->realpos;
}
// <20><><EFBFBD>õ<EFBFBD>ǰ<EFBFBD>˶<EFBFBD>λ<EFBFBD>ã<EFBFBD>ֹͣ״̬ʱ<CCAC><CAB1><EFBFBD>ã<EFBFBD>
// <20><>ֹӦ<D6B9>ò<EFBFBD>ʹ<EFBFBD>ô˴<C3B4><CBB4><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><>ʹ<EFBFBD><CAB9>ResetDriverPos
void SetDrvMotionRealPos(int driverIdx, s32 pos)
{
DriverMotionCtrl * pCtrl;
if (driverIdx < 0 || driverIdx >= DEV_MOTIONS_NUM)
{
return ;
}
pCtrl = &(g_drvMotionsCtrlList[driverIdx]);
pCtrl->realpos = pos;
// printf("set drv%d realpos to %d\r\n", driverIdx+1, pos);
}
//----------------------------------------------------------------
// <20><>ʼ<EFBFBD><CABC>
void InitDrvMotions(void)
{
memset(&g_drvMotionsCtrlList, 0, sizeof(DriverMotionCtrl)*DEV_MOTIONS_NUM);
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void DrvMotionCalcProc(void)
{
DriverMotionCtrl * pCtrl;
int i;
for (i = 0; i < DEV_MOTIONS_NUM; i++)
{
pCtrl = &(g_drvMotionsCtrlList[i]);
IntegrationMotionCalc(pCtrl);
}
}
//----------------------------------------------------------------
#endif
Reference in New Issue Copy Permalink