349 lines
7.1 KiB
C
349 lines
7.1 KiB
C
|
|
|
|
#include "stepmotos.h"
|
|
#include "inout.h"
|
|
|
|
//------------------------------------------------------------------------
|
|
|
|
|
|
void DefaultMotoCoilPwm(int coil){};
|
|
void DefaultMotoCoilCtrl(void){};
|
|
|
|
#define FULL_STEP_VAL (1.0f*MOTO_PPC/FULL_STEP_PPC) // 整步电子齿轮值
|
|
#define DEF_MICO_STEP_VAL (1.0f*MOTO_PPC/MICO_STEP_PPC) // 默认微步电子齿轮值
|
|
|
|
|
|
//------------------------------------------------------------------------
|
|
//------------------------------------------------------------------------
|
|
|
|
typedef struct
|
|
{
|
|
POS_TYPE motoPos; // 电机位置
|
|
|
|
POS_TYPE fullStepAng; // 整步电子齿轮值
|
|
POS_TYPE micoStepAng; // 可保存电子齿轮值
|
|
|
|
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; // 电机位置
|
|
g_stepMotoCtrl[i].ccwdir = CCWDIR_POSI; // 电机方向
|
|
|
|
g_stepMotoCtrl[i].fullStepAng = FULL_STEP_VAL; // 整步电子齿轮值
|
|
g_stepMotoCtrl[i].micoStepAng = DEF_MICO_STEP_VAL; // 可保存电子齿轮值
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
//------------------------------------------------------------------------
|
|
// 步进电机控制输出
|
|
// theta 电机角度，每圈 200K个位置 （或 50K 个)
|
|
// effort 电流
|
|
|
|
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); // 相位角相差 90 度
|
|
|
|
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;
|
|
}
|
|
|
|
//--------------------------------------------------------------
|
|
// 电机走一整步1.8度函数
|
|
// dir 方向。 1, 正向; -1, 反向; 0 不走
|
|
// eff 电流值
|
|
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;
|
|
}
|
|
|
|
//-------------------------------------------------------------------------------------
|
|
|
|
// 电机走一个微步函数
|
|
// dir 方向。 1, 正向; -1, 反向; 0 不走
|
|
// eff 电流值
|
|
// 返回电机的位置
|
|
|
|
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;
|
|
}
|
|
|