yangmeng/optical
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 3 Wiki Activity
optical-embos-V10.01.02
optical/NxFuncs/closedriver/stepmotos.c

349 lines
7.1 KiB
C
Raw Permalink Normal View History

调试完成
2025-09-04 01:45:08 +00:00
#include "stepmotos.h"
#include "inout.h"
//------------------------------------------------------------------------
void DefaultMotoCoilPwm(int coil){};
void DefaultMotoCoilCtrl(void){};
#define FULL_STEP_VAL (1.0f*MOTO_PPC/FULL_STEP_PPC) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӳ<EFBFBD><D3B3><EFBFBD>ֵ
#define DEF_MICO_STEP_VAL (1.0f*MOTO_PPC/MICO_STEP_PPC) // Ĭ<><C4AC>΢<EFBFBD><CEA2><EFBFBD><EFBFBD><EFBFBD>ӳ<EFBFBD><D3B3><EFBFBD>ֵ
//------------------------------------------------------------------------
//------------------------------------------------------------------------
typedef struct
{
POS_TYPE motoPos; // <20><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
POS_TYPE fullStepAng; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӳ<EFBFBD><D3B3><EFBFBD>ֵ
POS_TYPE micoStepAng; // <20>ɱ<EFBFBD><C9B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӳ<EFBFBD><D3B3><EFBFBD>ֵ
int ccwdir;
void (*MotoCoilAPwm)(int);
void (*SetMotoACPOn)(void);
void (*SetMotoACPOff)(void);
void (*SetMotoACNOn)(void);
void (*SetMotoACNOff)(void);
void (*MotoCoilBPwm)(int);
void (*SetMotoBCPOn)(void);
void (*SetMotoBCPOff)(void);
void (*SetMotoBCNOn)(void);
void (*SetMotoBCNOff)(void);
}StepMotoCtrl;
StepMotoCtrl g_stepMotoCtrl[STEP_MOTOS_NUM];
//------------------------------------------------------------------------
void InitStepMotos(void)
{
memset(g_stepMotoCtrl, 0, sizeof(StepMotoCtrl)*STEP_MOTOS_NUM);
for (int i = 0; i < STEP_MOTOS_NUM; i++)
{
g_stepMotoCtrl[i].motoPos = 0; // <20><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
g_stepMotoCtrl[i].ccwdir = CCWDIR_POSI; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
g_stepMotoCtrl[i].fullStepAng = FULL_STEP_VAL; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӳ<EFBFBD><D3B3><EFBFBD>ֵ
g_stepMotoCtrl[i].micoStepAng = DEF_MICO_STEP_VAL; // <20>ɱ<EFBFBD><C9B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӳ<EFBFBD><D3B3><EFBFBD>ֵ
g_stepMotoCtrl[i].MotoCoilAPwm = DefaultMotoCoilPwm;
g_stepMotoCtrl[i].SetMotoACPOn = DefaultMotoCoilCtrl;
g_stepMotoCtrl[i].SetMotoACPOff = DefaultMotoCoilCtrl;
g_stepMotoCtrl[i].SetMotoACNOn = DefaultMotoCoilCtrl;
g_stepMotoCtrl[i].SetMotoACNOff = DefaultMotoCoilCtrl;
g_stepMotoCtrl[i].MotoCoilBPwm= DefaultMotoCoilPwm;
g_stepMotoCtrl[i].SetMotoBCPOn = DefaultMotoCoilCtrl;
g_stepMotoCtrl[i].SetMotoBCPOff = DefaultMotoCoilCtrl;
g_stepMotoCtrl[i].SetMotoBCNOn = DefaultMotoCoilCtrl;
g_stepMotoCtrl[i].SetMotoBCNOff = DefaultMotoCoilCtrl;
}
}
//------------------------------------------------------------------------
void SetMotoCtrlFuncs(int motoIdx,
MotoCoilPwm coilapwm, MotoCoilCtrl acpon, MotoCoilCtrl acpoff, MotoCoilCtrl acnon, MotoCoilCtrl acnoff,
MotoCoilPwm coilbpwm, MotoCoilCtrl bcpon, MotoCoilCtrl bcpoff, MotoCoilCtrl bcnon, MotoCoilCtrl bcnoff)
{
StepMotoCtrl * pCtrl;
if (motoIdx < 0 || motoIdx >= STEP_MOTOS_NUM)
{
return;
}
pCtrl = &(g_stepMotoCtrl[motoIdx]);
pCtrl->MotoCoilAPwm = coilapwm;
pCtrl->SetMotoACPOn = acpon;
pCtrl->SetMotoACPOff = acpoff;
pCtrl->SetMotoACNOn = acnon;
pCtrl->SetMotoACNOff = acnoff;
pCtrl->MotoCoilBPwm= coilbpwm;
pCtrl->SetMotoBCPOn = bcpon;
pCtrl->SetMotoBCPOff = bcpoff;
pCtrl->SetMotoBCNOn = bcnon;
pCtrl->SetMotoBCNOff = bcnoff;
}
void SetMotoCCWDir(int motoIdx, int dir)
{
StepMotoCtrl * pCtrl;
if (motoIdx < 0 || motoIdx >= STEP_MOTOS_NUM)
{
return;
}
pCtrl = &(g_stepMotoCtrl[motoIdx]);
if (dir >= 0)
{
pCtrl->ccwdir = CCWDIR_POSI;
}
else
{
pCtrl->ccwdir = CCWDIR_NEGA;
}
}
//------------------------------------------------------------------------
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// theta <20><><EFBFBD><EFBFBD><EFBFBD>Ƕȣ<C7B6>ÿȦ 200K<30><4B>λ<EFBFBD><CEBB> <20><><EFBFBD><EFBFBD> 50K <20><>)
// effort <20><><EFBFBD><EFBFBD>
int StepMotoOut(int motoIdx, int theta, int effort)
{
int angleA, angleB;
int coilA, coilB;
int sinA, sinB;
StepMotoCtrl * pCtrl;
if (motoIdx < 0 || motoIdx >= STEP_MOTOS_NUM)
{
return 0;
}
pCtrl = &(g_stepMotoCtrl[motoIdx]);
if (effort < 0)
{
effort = -effort;
}
if (effort > MAX_OUT_LEV)
{
effort = MAX_OUT_LEV;
}
theta *= pCtrl->ccwdir;
angleA = theta % SIN_TAB_SIZE; // ((int)(theta * PHASE_MUTI)) % SIN_TAB_SIZE;
angleB = angleA + (SIN_TAB_SIZE/4); // <20><>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 90 <20><>
sinA = GetSinValue(angleA);
sinB = GetSinValue(angleB);
coilA = (int)(effort * sinA / SIN_ITEM_MUTI);
coilB = (int)(effort * sinB / SIN_ITEM_MUTI);
if (coilA > 0)
{
pCtrl->MotoCoilAPwm(coilA);
pCtrl->SetMotoACNOn();
pCtrl->SetMotoACPOff();
}
else if (coilA < 0)
{
pCtrl->MotoCoilAPwm(-coilA);
pCtrl->SetMotoACPOn();
pCtrl->SetMotoACNOff();
}
else
{
pCtrl->MotoCoilAPwm(0);
pCtrl->SetMotoACNOn();
pCtrl->SetMotoACPOn();
}
if (coilB > 0)
{
pCtrl->MotoCoilBPwm(coilB);
pCtrl->SetMotoBCNOn();
pCtrl->SetMotoBCPOff();
}
else if (coilB < 0)
{
pCtrl->MotoCoilBPwm(-coilB);
pCtrl->SetMotoBCPOn();
pCtrl->SetMotoBCNOff();
}
else
{
pCtrl->MotoCoilBPwm(0);
pCtrl->SetMotoBCNOn();
pCtrl->SetMotoBCPOn();
}
#if (0)
printf("StepMotoOut, idx=%d, theta=%d, effort=%d\r\n", motoIdx, theta, effort);
printf("angleA=%d, sinA=%d, coilA=%d\r\n", angleA, sinA, coilA);
printf("angleB=%d, sinB=%d, coilB=%d\r\n", angleB, sinB, coilB);
#endif
return 0;
}
//--------------------------------------------------------------
void ResetMotoPos(int motoIdx, int eff)
{
int pos;
StepMotoCtrl * pCtrl;
if (motoIdx < 0 || motoIdx >= STEP_MOTOS_NUM)
{
return;
}
pCtrl = &(g_stepMotoCtrl[motoIdx]);
pCtrl->motoPos = 0;
pos = (int)pCtrl->motoPos;
StepMotoOut(motoIdx, pos, eff);
}
void SetMotoEnable(int motoIdx, int eff)
{
int pos;
StepMotoCtrl * pCtrl;
if (motoIdx < 0 || motoIdx >= STEP_MOTOS_NUM)
{
return;
}
pCtrl = &(g_stepMotoCtrl[motoIdx]);
pos = (int)pCtrl->motoPos;
StepMotoOut(motoIdx, pos, eff);
}
void SetMotoPosTo(int motoIdx, POS_TYPE pos, int eff)
{
StepMotoCtrl * pCtrl;
if (motoIdx < 0 || motoIdx >= STEP_MOTOS_NUM)
{
return;
}
pCtrl = &(g_stepMotoCtrl[motoIdx]);
pCtrl->motoPos = pos;
StepMotoOut(motoIdx, pos, eff);
}
POS_TYPE GetMotoPos(int motoIdx)
{
StepMotoCtrl * pCtrl;
if (motoIdx < 0 || motoIdx >= STEP_MOTOS_NUM)
{
return 0;
}
pCtrl = &(g_stepMotoCtrl[motoIdx]);
return pCtrl->motoPos;
}
//--------------------------------------------------------------
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>1.8<EFBFBD>Ⱥ<EFBFBD><EFBFBD><EFBFBD>
// dir <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 1, <20><><EFBFBD><EFBFBD>; -1, <20><><EFBFBD><EFBFBD>; 0 <20><><EFBFBD><EFBFBD>
// eff <20><><EFBFBD><EFBFBD>ֵ
int MotoOneFullStep(int motoIdx, int dir, int eff)
{
int pos;
StepMotoCtrl * pCtrl;
if (motoIdx < 0 || motoIdx >= STEP_MOTOS_NUM)
{
return 0;
}
pCtrl = &(g_stepMotoCtrl[motoIdx]);
if (dir > 0)
{
pCtrl->motoPos += pCtrl->fullStepAng;
}
else if (dir < 0)
{
pCtrl->motoPos -= pCtrl->fullStepAng;
}
pos = (int)(pCtrl->motoPos + 0.5f*(pCtrl->motoPos>=0?1:-1));
if (pos > MAX_MOTO_POS)
{
pos -= MAX_MOTO_POS;
pCtrl->motoPos -= MAX_MOTO_POS;
}
if (pos < -MAX_MOTO_POS)
{
pos += MAX_MOTO_POS;
pCtrl->motoPos += MAX_MOTO_POS;
}
StepMotoOut(motoIdx, pos, eff);
return pos;
}
//-------------------------------------------------------------------------------------
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>΢<EFBFBD><CEA2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// dir <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 1, <20><><EFBFBD><EFBFBD>; -1, <20><><EFBFBD><EFBFBD>; 0 <20><><EFBFBD><EFBFBD>
// eff <20><><EFBFBD><EFBFBD>ֵ
// <20><><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
int MotoOneMicoStep(int motoIdx, int dir, int eff)
{
int pos;
StepMotoCtrl * pCtrl;
if (motoIdx < 0 || motoIdx >= STEP_MOTOS_NUM)
{
return 0;
}
pCtrl = &(g_stepMotoCtrl[motoIdx]);
if (dir > 0)
{
pCtrl->motoPos += pCtrl->micoStepAng;
}
else if (dir < 0)
{
pCtrl->motoPos -= pCtrl->micoStepAng;
}
pos = (int)(pCtrl->motoPos + (0.5f*(pCtrl->motoPos>=0?1:-1)));
if (pos > MAX_MOTO_POS)
{
pos -= MAX_MOTO_POS;
pCtrl->motoPos -= MAX_MOTO_POS;
}
if (pos < -MAX_MOTO_POS)
{
pos += MAX_MOTO_POS;
pCtrl->motoPos += MAX_MOTO_POS;
}
StepMotoOut(motoIdx, pos, eff);
return pos;
}
Reference in New Issue Copy Permalink