1519 lines
32 KiB
C
1519 lines
32 KiB
C
|
|
|
|
#include "config.h"
|
|
#include "driversctrl.h"
|
|
|
|
#if (DRIVERS_NUM > 0)
|
|
|
|
#include "stepmotos.h"
|
|
|
|
#include "inout.h"
|
|
#include "delay.h"
|
|
#include "shell.h"
|
|
#include "trigger.h"
|
|
#include "cpuid.h"
|
|
|
|
#include "driverparas.h"
|
|
|
|
//------------------------------------------------------------------------
|
|
|
|
#include "drivermotion.h"
|
|
|
|
//------------------------------------------------------------------------
|
|
|
|
#ifndef MOTO_ECD_PER_CIRCLE
|
|
#define MOTO_ECD_PER_CIRCLE 4000
|
|
#endif
|
|
|
|
//------------------------------------------------------------------------
|
|
|
|
#define OUT_INFO_NUM 0
|
|
|
|
typedef struct
|
|
{
|
|
s32 cmdPosition; // 指令位置
|
|
s32 targetMotoPos; // 目标位置
|
|
s32 ecdPosition; // 编码器位置
|
|
|
|
s32 thisPosErr; // 位置偏差
|
|
s32 integral; // 偏差积分
|
|
s32 differential; // 位置微分
|
|
s32 thisactVel; // 本次检测速度值
|
|
|
|
s32 pospid; // 位置PID
|
|
|
|
s32 setMotoPos; // 设置位置
|
|
s32 effort; // 设置电流
|
|
|
|
}DrvOutInfo;
|
|
|
|
#ifndef DEF_SPD_FIL
|
|
#define DEF_SPD_FIL 0
|
|
#endif
|
|
|
|
typedef struct
|
|
{
|
|
//----------------------
|
|
int motoIdx; // 电机索引
|
|
|
|
void (*InitEncoder)(void);
|
|
int (*ReadEncoderCounter)(void);
|
|
|
|
//----------------------
|
|
DriverPara paras; // 参数
|
|
|
|
//----------------------
|
|
int powerEnFlag; // 使能标志
|
|
//----------------------
|
|
float stepAngle; // 细分
|
|
//----------------------
|
|
s32 runspeed; // 运行速度
|
|
s32 speedadd; // 内部运行加速度
|
|
int runsta;
|
|
//----------------------
|
|
s32 cmdPosition; // 指令位置统计（电角度）
|
|
s32 ecdPosition; // 编码器位置统计（电角度）
|
|
s32 runPosition; // 运行位置统计（电角度）
|
|
|
|
s32 lastPosErr; // 上次的偏差(电角度)
|
|
s32 integral;
|
|
s32 differential;
|
|
#if (DEF_SPD_FIL > 1)
|
|
s32 spdBuff[DEF_SPD_FIL];
|
|
s32 spdIdx;
|
|
s32 spdSum;
|
|
#endif
|
|
|
|
// s32 lastVel;
|
|
// s32 lastVErr;
|
|
// s32 lastDErr; // 上上次的偏差
|
|
|
|
s32 intev;
|
|
s32 effort; // 输出电流
|
|
//----------------------
|
|
s32 lastThetaGap;
|
|
//----------------------
|
|
s32 exCmdCounter; // 外部指令计数
|
|
//----------------------
|
|
s32 posOffset;
|
|
//----------------------
|
|
|
|
int first;
|
|
s32 oldTheta; // 上次的角度(电角度)
|
|
s32 oldCounter; // 上次的指令计数
|
|
s32 halfCounter; // 开环半流计数器
|
|
//----------------------
|
|
int errorCode; // 错误代码
|
|
//----------------------
|
|
#if (OUT_INFO_NUM != 0)
|
|
int timeCount;
|
|
int timeGap;
|
|
int outInfoIdx;
|
|
int outInfoEn;
|
|
|
|
DrvOutInfo outInfoList[OUT_INFO_NUM+1];
|
|
#endif
|
|
|
|
}DriverCtrl;
|
|
|
|
//------------------------------------------------------------------------
|
|
|
|
DriverCtrl g_driverCtrl[DRIVERS_NUM];
|
|
|
|
//------------------------------------------------------------------------
|
|
|
|
void InitCloseCtrl(int driverIdx);
|
|
|
|
int GetThetaValue(int driverIdx);
|
|
void SetMotoDirectionCtrl(int driverIdx, int dirbit);
|
|
void SetMotoPulseCtrl(int driverIdx, int dirbit);
|
|
|
|
//------------------------------------------------------------------------
|
|
|
|
float CalcStepAngle(s32 stepPerCircle, s32 numerator, s32 denominator);
|
|
|
|
int GetThetaFromEcd(int ecdvalue);
|
|
int LoadCtrlPara();
|
|
|
|
//------------------------------------------------------------------------
|
|
|
|
const char * DRV_HLP_STR =
|
|
"\t""测试电机驱动" "\r\n"
|
|
"\t" "p1 p2" "\r\n"
|
|
"\t" "0 x" "显示x驱动参数, 0 全部显示" "\r\n"
|
|
"\t""x0d n" "设置x驱动参数d的值为n" "\r\n"
|
|
"\t""x000 m" "开环步进方式运动x电机" "\r\n"
|
|
"\t\t" "m=0" "失能" "\r\n"
|
|
"\t\t" "m=1" "使能" "\r\n"
|
|
"\t\t" "m=2" "正向整步" "\r\n"
|
|
"\t\t" "m=3" "负向整步" "\r\n"
|
|
"\t\t" "m=4" "正向微步" "\r\n"
|
|
"\t\t" "m=5" "负向微步" "\r\n"
|
|
"\t" "x000d n" "驱动器方式运动x电机,位移为n" "\r\n"
|
|
;
|
|
|
|
void TestDriver(char * para1, char * para2);
|
|
void ShowDriver(char * para1, char * para2);
|
|
|
|
//------------------------------------------------------------------------
|
|
|
|
#define RD_CALIB_PPC (100) // (4000)
|
|
|
|
//-----------------------------------------------------------
|
|
#define IDX_RANGE 10
|
|
#define PID_RANGE 1024 // PID 参数分母
|
|
|
|
#define PULSE_COUNTER_RELOAD (S32_MAX-1) // 外部脉冲计数器重装载值
|
|
|
|
#define LOW_CIRCLE_TIME 2000 // 低电流锁定时间 (2k)
|
|
#define ADVANCE_ADJ_MAX ((MOTO_PPC)/MOTO_NOPP/5) // 调整偏差
|
|
|
|
#define MIN_PE_LIMIT ((MOTO_PPC)/MOTO_NOPP/32) // 最小偏差允许
|
|
#define MAX_PE_LIMIT (MOTO_PPC/10) // 最大偏差限制
|
|
|
|
#define MAX_IG_LIMIT (RD_CALIB_PPC*PID_RANGE) // 积分项目饱和限制
|
|
|
|
#define MAX_IS_LIMIT (MAX_IG_LIMIT*90/100) // 积分分段最大限制
|
|
#define MIN_IS_LIMIT (MAX_IG_LIMIT*50/100) // 积分分段最小限制
|
|
|
|
#define EFF_MAX (MAX_OUT_LEV *1)
|
|
#define EFF_MIN (MAX_OUT_LEV *-1)
|
|
|
|
#define MIN_OUT_EFF 0
|
|
|
|
#define DEF_OUT_EFF 50
|
|
|
|
#define POS_PER_CIRCLE (MOTO_ECD_PER_CIRCLE)
|
|
|
|
//------------------------------------------------------------------------
|
|
|
|
|
|
//------------------------------------------------------------------------
|
|
#define MOTO_INIT(id) do \
|
|
{\
|
|
SetMotoCtrlFuncs(DRIVER##id##_IDX, \
|
|
SetMoto##id##CoilAPwm, SetMoto##id##ACPOn, SetMoto##id##ACPOff, SetMoto##id##ACNOn, SetMoto##id##ACNOff, \
|
|
SetMoto##id##CoilBPwm, SetMoto##id##BCPOn, SetMoto##id##BCPOff, SetMoto##id##BCNOn, SetMoto##id##BCNOff); \
|
|
\
|
|
SetMotoCCWDir(DRIVER##id##_IDX, MOTO##id##_CCWDIR); /* 设置方向 */ \
|
|
\
|
|
SetMoto##id##CoilPwmEnable(); /* 允许PWM输出 */ \
|
|
\
|
|
g_driverCtrl[id-1].ReadEncoderCounter = ReadMoto##id##EncoderCounter; \
|
|
g_driverCtrl[id-1].InitEncoder = InitMoto##id##Encoder; \
|
|
}while(0)
|
|
|
|
//------------------------------------------------------------------------
|
|
|
|
void InitDriversCtrl(void)
|
|
{
|
|
int i;
|
|
|
|
// 电机初始化
|
|
InitStepMotos(); // 初始化步进电机控制
|
|
|
|
InitDrvMotions(); // 初始化内置运动控制
|
|
|
|
// 驱动控制
|
|
memset(g_driverCtrl, 0, sizeof(DriverCtrl)*DRIVERS_NUM);
|
|
|
|
#if (DRIVERS_NUM >= 1)
|
|
MOTO_INIT(1);
|
|
#endif
|
|
|
|
#if (DRIVERS_NUM >= 2)
|
|
MOTO_INIT(2);
|
|
#endif
|
|
|
|
#if (DRIVERS_NUM >= 3)
|
|
MOTO_INIT(3);
|
|
#endif
|
|
|
|
#if (DRIVERS_NUM >= 4)
|
|
MOTO_INIT(4);
|
|
#endif
|
|
|
|
// 装载默认参数
|
|
InitDriverParas();
|
|
|
|
for (i = 0; i < DRIVERS_NUM; i++)
|
|
{
|
|
g_driverCtrl[i].motoIdx = i;
|
|
|
|
// 从参数文件装载参数值
|
|
memcpy(&(g_driverCtrl[i].paras), GetDriverParas(i), sizeof(DriverPara));
|
|
|
|
// 计算细分
|
|
g_driverCtrl[i].stepAngle = CalcStepAngle(g_driverCtrl[i].paras.stepPerCircle, g_driverCtrl[i].paras.numerator, g_driverCtrl[i].paras.denominator);
|
|
printf("moto%d stepAngle=%.2f\r\n", i+1, g_driverCtrl[i].stepAngle);
|
|
|
|
// 重置默认参数
|
|
|
|
}
|
|
|
|
//-----------------------
|
|
|
|
//-----------------------
|
|
// 闭环控制初始位置
|
|
for (i = 0; i < DRIVERS_NUM; i++)
|
|
{
|
|
SetEnableCtrl(i, 0);
|
|
}
|
|
|
|
DelayMs(10);
|
|
|
|
// 开始时固定在整步位置
|
|
for (i = 0; i < DRIVERS_NUM; i++)
|
|
{
|
|
if (g_driverCtrl[i].paras.closeCtrlMode != 0)
|
|
{
|
|
ResetMotoPos(i, 100);
|
|
}
|
|
else
|
|
{
|
|
g_driverCtrl[i].halfCounter = LOW_CIRCLE_TIME;
|
|
}
|
|
}
|
|
|
|
DelayMs(200);
|
|
|
|
// 初始化编码器计数（根据固定整步位置）
|
|
for (i = 0; i < DRIVERS_NUM; i++)
|
|
{
|
|
g_driverCtrl[i].InitEncoder();
|
|
}
|
|
|
|
DelayMs(10);
|
|
|
|
// 使能电机
|
|
for (i = 0; i < DRIVERS_NUM; i++)
|
|
{
|
|
SetEnableCtrl(i, 1);
|
|
}
|
|
|
|
DelayMs(100);
|
|
|
|
#if (OUT_INFO_NUM != 0)
|
|
g_driverCtrl[0].outInfoIdx = 0;
|
|
g_driverCtrl[0].timeGap = 0;
|
|
g_driverCtrl[0].outInfoEn = 1;
|
|
#endif
|
|
|
|
// 打开控制周期，开始控制
|
|
if (1)
|
|
{
|
|
printf("StartControlCycle\r\n");
|
|
StartControlCycle();
|
|
}
|
|
|
|
#if (0)
|
|
AddShellCmd("DRIVER", DRV_HLP_STR, TestDriver);
|
|
#else
|
|
AddShellCmd("DRIVER", "", ShowDriver);
|
|
#endif
|
|
}
|
|
|
|
//------------------------------------------------------------------------
|
|
|
|
|
|
void InitCloseCtrl(int driverIdx)
|
|
{
|
|
s32 rdTheta = 0;
|
|
DriverCtrl * pCtrl;
|
|
if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
|
|
{
|
|
return;
|
|
}
|
|
pCtrl = &g_driverCtrl[driverIdx];
|
|
|
|
rdTheta = GetThetaValue(driverIdx);
|
|
|
|
pCtrl->cmdPosition = rdTheta / pCtrl->stepAngle; // 指令位置统计（电角度）
|
|
pCtrl->ecdPosition = rdTheta; // 编码器位置统计（电角度）
|
|
pCtrl->runPosition = rdTheta; // 运行位置统计（电角度）
|
|
|
|
pCtrl->lastPosErr = 0; // 上次的偏差
|
|
pCtrl->integral = 0; // 积分
|
|
// pCtrl->lastDErr = 0; // 上上次的偏差
|
|
pCtrl->effort = 0; // 输出电流
|
|
|
|
printf("\r\n InitCloseCtrl, g_driverCtrl[%d]:\r\n"
|
|
"\t rdTheta=%d, \r\n"
|
|
"\t stepAngle=%f, \r\n"
|
|
"\t cmdPosition=%d, \r\n"
|
|
"\t ecdPosition=%d, \r\n"
|
|
"\t runPosition=%d, \r\n",
|
|
driverIdx, rdTheta, pCtrl->stepAngle, pCtrl->cmdPosition, pCtrl->ecdPosition, pCtrl->runPosition);
|
|
|
|
pCtrl->first = 1;
|
|
}
|
|
|
|
void SetEnableCtrl(int driverIdx, int enbit)
|
|
{
|
|
DriverCtrl * pCtrl;
|
|
if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
|
|
{
|
|
return;
|
|
}
|
|
pCtrl = &g_driverCtrl[driverIdx];
|
|
|
|
if (enbit != 0)
|
|
{
|
|
if (pCtrl->powerEnFlag == 0 && pCtrl->paras.closeCtrlMode != 0)
|
|
{
|
|
InitCloseCtrl(driverIdx);
|
|
SetMotoPosTo(pCtrl->motoIdx, pCtrl->ecdPosition, DEF_OUT_EFF); // 行走到目标位置
|
|
}
|
|
pCtrl->powerEnFlag = 1;
|
|
|
|
printf("set moto%d en on\r\n", pCtrl->motoIdx+1);
|
|
}
|
|
else
|
|
{
|
|
if (pCtrl->powerEnFlag != 0)
|
|
{
|
|
pCtrl->powerEnFlag = 0;
|
|
DelayUs(200);
|
|
SetMotoEnable(pCtrl->motoIdx, 0);
|
|
}
|
|
printf("set moto%d en off\r\n", pCtrl->motoIdx+1);
|
|
}
|
|
}
|
|
|
|
|
|
int GetThetaFromEcd(int ecdvalue)
|
|
{
|
|
#if (SIN_TAB_SIZE == POS_PER_CIRCLE) // 简化计算, 编码器和正弦表数量相同
|
|
ecdvalue *= MOTO_NOPP;
|
|
#else
|
|
ecdvalue *= MOTO_PPC;
|
|
ecdvalue /= POS_PER_CIRCLE;
|
|
#endif
|
|
return ecdvalue;
|
|
}
|
|
|
|
int GetMotoThetaValue(DriverCtrl * pCtrl)
|
|
{
|
|
if (pCtrl == NULL)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
int ecdvalue = pCtrl->ReadEncoderCounter();
|
|
ecdvalue += pCtrl->paras.ecdAdjust;
|
|
|
|
return GetThetaFromEcd(ecdvalue);
|
|
}
|
|
|
|
//
|
|
int GetThetaValue(int driverIdx)
|
|
{
|
|
DriverCtrl * pCtrl;
|
|
if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
|
|
{
|
|
return 0;
|
|
}
|
|
pCtrl = &g_driverCtrl[driverIdx];
|
|
return GetMotoThetaValue(pCtrl);
|
|
}
|
|
|
|
|
|
float CalcStepAngle(s32 stepPerCircle, s32 numerator, s32 denominator)
|
|
{
|
|
float stepAngle = 1.0f;
|
|
if (stepPerCircle != 0)
|
|
{
|
|
stepAngle = (1.0f*MOTO_PPC/stepPerCircle);
|
|
}
|
|
else if (denominator != 0)
|
|
{
|
|
stepAngle = (1.0f*MOTO_PPC*numerator/denominator);
|
|
}
|
|
|
|
return stepAngle;
|
|
}
|
|
//------------------------------------------------------------------------
|
|
//------------------------------------------------------------------------------------
|
|
|
|
void MotoCloseCtrlProc(DriverCtrl * pCtrl)
|
|
{
|
|
if (pCtrl == NULL)
|
|
{
|
|
return;
|
|
}
|
|
if (pCtrl->powerEnFlag <= 0) // 非使能状态下
|
|
{
|
|
return;
|
|
}
|
|
|
|
#if (OUT_INFO_NUM != 0)
|
|
pCtrl->timeCount++;
|
|
|
|
if (pCtrl->outInfoEn != 0)
|
|
{
|
|
if (pCtrl->timeGap <= 0)
|
|
{
|
|
pCtrl->timeGap = 1;
|
|
}
|
|
if ((pCtrl->timeCount % pCtrl->timeGap) == 0)
|
|
{
|
|
pCtrl->outInfoIdx++;
|
|
if (pCtrl->outInfoIdx >= OUT_INFO_NUM)
|
|
{
|
|
pCtrl->outInfoEn = 0;
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
|
|
s32 targetMotoPos; // 理论目标位置
|
|
s32 setMotoPos; // 本次计算实际目标位置
|
|
s32 effort; // 电流
|
|
s32 tmps32;
|
|
s32 thisactVel = 0; // 和上一次比较的电角度差值
|
|
// 读取编码器位置
|
|
// if (pCtrl->paras.closeCtrlMode != 0)
|
|
{
|
|
s32 rdTheta; // 当前电角度
|
|
s32 thisThetaGap; // 电角度差值(和上一次比较)
|
|
rdTheta = GetMotoThetaValue(pCtrl); // 读取编码器, 转换为电角度
|
|
|
|
if (pCtrl->first == 1)
|
|
{
|
|
pCtrl->oldTheta = rdTheta;
|
|
}
|
|
|
|
thisThetaGap = rdTheta - pCtrl->oldTheta; // 电角度差值
|
|
// 转换为-180-180度之间
|
|
if (thisThetaGap < -MOTO_PPC/2) // 反转
|
|
{
|
|
thisThetaGap += MOTO_PPC;
|
|
}
|
|
else if (thisThetaGap > MOTO_PPC/2)
|
|
{
|
|
thisThetaGap -= MOTO_PPC;
|
|
}
|
|
|
|
#if (DEF_SPD_FIL > 1)
|
|
pCtrl->spdIdx++;
|
|
if (pCtrl->spdIdx >= DEF_SPD_FIL)
|
|
{
|
|
pCtrl->spdIdx = 0;
|
|
}
|
|
pCtrl->spdSum -= pCtrl->spdBuff[pCtrl->spdIdx];
|
|
pCtrl->spdBuff[pCtrl->spdIdx] = thisThetaGap;
|
|
pCtrl->spdSum += thisThetaGap;
|
|
thisactVel = pCtrl->spdSum/DEF_SPD_FIL; // 速度值
|
|
#else
|
|
thisactVel = thisThetaGap;
|
|
#endif
|
|
pCtrl->ecdPosition += thisThetaGap;
|
|
|
|
#if (OUT_INFO_NUM != 0)
|
|
pCtrl->outInfoList[pCtrl->outInfoIdx].thisactVel = thisactVel;
|
|
pCtrl->outInfoList[pCtrl->outInfoIdx].ecdPosition = pCtrl->ecdPosition;
|
|
#endif
|
|
|
|
pCtrl->oldTheta = rdTheta;
|
|
}
|
|
|
|
// 读取指令位置
|
|
{
|
|
s32 thisCmdGap; // 本次和上次的指令位置偏差
|
|
s32 rdCounter; // 目标位置,指令计数
|
|
rdCounter = pCtrl->exCmdCounter;
|
|
|
|
if (pCtrl->first == 1)
|
|
{
|
|
pCtrl->oldCounter = rdCounter;
|
|
|
|
if (pCtrl->paras.closeCtrlMode != 0) // 闭环模式
|
|
{
|
|
pCtrl->cmdPosition = pCtrl->ecdPosition / pCtrl->stepAngle;
|
|
}
|
|
}
|
|
|
|
thisCmdGap = rdCounter - pCtrl->oldCounter;
|
|
if (thisCmdGap < -1*(s32)(PULSE_COUNTER_RELOAD/2)) // 反转
|
|
{
|
|
thisCmdGap += PULSE_COUNTER_RELOAD;
|
|
}
|
|
else if (thisCmdGap > PULSE_COUNTER_RELOAD/2)
|
|
{
|
|
thisCmdGap -= PULSE_COUNTER_RELOAD;
|
|
}
|
|
pCtrl->oldCounter = rdCounter;
|
|
|
|
// 累计指令位置
|
|
pCtrl->cmdPosition += thisCmdGap;
|
|
|
|
#if (OUT_INFO_NUM != 0)
|
|
pCtrl->outInfoList[pCtrl->outInfoIdx].cmdPosition = pCtrl->cmdPosition;
|
|
#endif
|
|
}
|
|
|
|
targetMotoPos = (s32)(pCtrl->cmdPosition * pCtrl->stepAngle); // 计算位置初值为指令位置
|
|
|
|
#if (OUT_INFO_NUM != 0)
|
|
pCtrl->outInfoList[pCtrl->outInfoIdx].targetMotoPos = targetMotoPos;
|
|
#endif
|
|
|
|
if (pCtrl->paras.closeCtrlMode == 0) // 开环模式
|
|
{
|
|
if (targetMotoPos == pCtrl->runPosition)
|
|
{
|
|
pCtrl->halfCounter++;
|
|
}
|
|
else
|
|
{
|
|
pCtrl->halfCounter = 0;
|
|
|
|
// 开环模式下的速度限制, 防止丢步
|
|
tmps32 = targetMotoPos - pCtrl->runPosition;
|
|
if (tmps32 > pCtrl->paras.openMaxOffset)
|
|
{
|
|
targetMotoPos = pCtrl->runPosition + pCtrl->paras.openMaxOffset;
|
|
}
|
|
else if (tmps32 < -1*pCtrl->paras.openMaxOffset)
|
|
{
|
|
targetMotoPos = pCtrl->runPosition - pCtrl->paras.openMaxOffset;
|
|
}
|
|
}
|
|
setMotoPos = targetMotoPos;
|
|
|
|
if (pCtrl->halfCounter > LOW_CIRCLE_TIME)
|
|
{
|
|
pCtrl->halfCounter = LOW_CIRCLE_TIME;
|
|
effort = pCtrl->paras.openLockCircle; // 锁定电流
|
|
}
|
|
else
|
|
{
|
|
effort = pCtrl->paras.openRunCircle; // 驱动电流
|
|
}
|
|
}
|
|
else // 闭环控制
|
|
{
|
|
// 位置闭环
|
|
s32 pospid = 0;
|
|
s32 thisPosErr = 0; // 位置偏差(电角度)
|
|
{
|
|
int index = 0;
|
|
thisPosErr = targetMotoPos - pCtrl->ecdPosition;
|
|
|
|
// 最大偏差36度?
|
|
if (thisPosErr > MAX_PE_LIMIT) // 限制偏差范围
|
|
{
|
|
thisPosErr = MAX_PE_LIMIT;
|
|
}
|
|
else if (thisPosErr < -MAX_PE_LIMIT)
|
|
{
|
|
thisPosErr = -MAX_PE_LIMIT;
|
|
}
|
|
|
|
// 位置PID
|
|
// 分段积分
|
|
if ((thisPosErr >= 0 && pCtrl->lastPosErr <= 0) ||
|
|
(thisPosErr <= 0 && pCtrl->lastPosErr >= 0) ) // 本次与上次运动方向相反
|
|
{
|
|
pCtrl->integral /= 128; // 积分快速下降
|
|
}
|
|
|
|
if (abs(thisPosErr + pCtrl->lastPosErr) < 10) // 是否可以理解为接近目标位置或在目标位置附近抖动
|
|
{
|
|
index = IDX_RANGE;
|
|
pCtrl->integral /= 16; // 2 // 积分快速下降
|
|
}
|
|
else if (abs(thisPosErr) > MAX_IS_LIMIT)
|
|
{
|
|
index = 0;
|
|
}
|
|
else
|
|
{
|
|
if (abs(thisPosErr) < MIN_IS_LIMIT)
|
|
{
|
|
index = IDX_RANGE;
|
|
}
|
|
else
|
|
{
|
|
index = ((MAX_IS_LIMIT) - abs(thisPosErr)) * IDX_RANGE / ((MAX_IS_LIMIT - MIN_IS_LIMIT));
|
|
}
|
|
|
|
// 抗积分饱和
|
|
if (pCtrl->effort > EFF_MAX)
|
|
{
|
|
if (thisPosErr < 0)
|
|
{
|
|
pCtrl->integral += thisPosErr;
|
|
}
|
|
}
|
|
else if (pCtrl->effort < EFF_MIN)
|
|
{
|
|
if (thisPosErr > 0)
|
|
{
|
|
pCtrl->integral += thisPosErr;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
pCtrl->integral += thisPosErr;
|
|
}
|
|
|
|
// 积分项饱和限制
|
|
if (pCtrl->integral > MAX_IG_LIMIT)
|
|
{
|
|
pCtrl->integral = MAX_IG_LIMIT;
|
|
}
|
|
else if (pCtrl->integral < -MAX_IG_LIMIT)
|
|
{
|
|
pCtrl->integral = -MAX_IG_LIMIT;
|
|
}
|
|
}
|
|
|
|
// 微分项, 微分滤波
|
|
tmps32 = (thisPosErr - pCtrl->lastPosErr);
|
|
pCtrl->differential = (pCtrl->differential * 26 + tmps32 * 230) / 256;
|
|
|
|
pCtrl->lastPosErr = thisPosErr; // 记录上次的误差
|
|
|
|
// 计算位置PID
|
|
pospid = ( ( (pCtrl->paras.kp * thisPosErr)
|
|
+ (pCtrl->paras.ki * pCtrl->integral * index / PID_RANGE)
|
|
+ (pCtrl->paras.kd * pCtrl->differential) ) / PID_RANGE);
|
|
#if (OUT_INFO_NUM != 0)
|
|
pCtrl->outInfoList[pCtrl->outInfoIdx].thisPosErr = thisPosErr;
|
|
pCtrl->outInfoList[pCtrl->outInfoIdx].integral = pCtrl->integral;
|
|
pCtrl->outInfoList[pCtrl->outInfoIdx].differential = pCtrl->differential;
|
|
|
|
pCtrl->outInfoList[pCtrl->outInfoIdx].pospid = pospid;
|
|
#endif
|
|
}
|
|
|
|
// 位置前馈
|
|
tmps32 = pCtrl->ecdPosition + pCtrl->paras.closeAdjust; // 以编码器位置为基准位置
|
|
|
|
s32 advance = thisactVel; // 速度作为 前馈角度
|
|
s32 basecircle = pCtrl->paras.basecircle;
|
|
|
|
#if (0)
|
|
// 20240105 临时改进
|
|
if (pospid > 200)
|
|
{
|
|
advance += (MAX_ADVANCE+MICO_ADVANCE+MIN_ADVANCE);
|
|
}
|
|
else if(pospid < -200)
|
|
{
|
|
advance -= (MAX_ADVANCE+MICO_ADVANCE+MIN_ADVANCE);
|
|
}
|
|
else if (pospid > 100)
|
|
{
|
|
advance += (MAX_ADVANCE+MIN_ADVANCE);
|
|
}
|
|
else if(pospid < -100)
|
|
{
|
|
advance -= (MAX_ADVANCE+MIN_ADVANCE);
|
|
}
|
|
else
|
|
#endif
|
|
if (pospid > 50)
|
|
{
|
|
advance += (MAX_ADVANCE); // 1.8度
|
|
}
|
|
else if(pospid < -50)
|
|
{
|
|
advance -= (MAX_ADVANCE);
|
|
}
|
|
else if (pospid > 10)
|
|
{
|
|
advance += (MICO_ADVANCE); // 0.36度
|
|
}
|
|
else if(pospid < -10)
|
|
{
|
|
advance -= (MICO_ADVANCE);
|
|
}
|
|
else
|
|
{
|
|
#if (0)// 临时测试使用
|
|
static int flag = 0;
|
|
if (abs(advance) < MIN_ADVANCE)
|
|
{
|
|
if (flag == 0)
|
|
{
|
|
printf("pid=%d,A:%d\r\n", pospid, advance);
|
|
flag = 1;
|
|
}
|
|
basecircle = 0;
|
|
advance = 0;
|
|
}
|
|
else
|
|
{
|
|
flag = 0;
|
|
}
|
|
#else
|
|
if (abs(advance) <= MIN_ADVANCE)
|
|
{
|
|
basecircle = 0;
|
|
advance = 0;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
setMotoPos = tmps32 + advance;
|
|
// 电流
|
|
#if (0) // 20240826
|
|
effort = abs(pospid);
|
|
if (effort > 4000)
|
|
{
|
|
effort /= 8;
|
|
}
|
|
else if (effort > 2000)
|
|
{
|
|
effort /= 6;
|
|
}
|
|
else if (effort > 800)
|
|
{
|
|
effort /= 4;
|
|
}
|
|
else if (effort > 200)
|
|
{
|
|
effort /= 2;
|
|
}
|
|
effort = basecircle + effort * pCtrl->paras.closeCircle / PID_RANGE;
|
|
#else
|
|
effort = basecircle + abs(pospid)/8 * pCtrl->paras.closeCircle / PID_RANGE;
|
|
#endif
|
|
// 20240109 ljs 电流延迟下降
|
|
if (effort < pCtrl->effort)
|
|
{
|
|
if (abs(effort - pCtrl->effort) > 10)
|
|
{
|
|
effort = pCtrl->effort - 10;
|
|
}
|
|
}
|
|
}
|
|
|
|
// 电流控制
|
|
{
|
|
SetMotoPosTo(pCtrl->motoIdx, setMotoPos, effort); // 行走到目标位置
|
|
|
|
pCtrl->runPosition = setMotoPos; // 记录电机位置（电角度）
|
|
pCtrl->effort = effort;
|
|
|
|
#if (OUT_INFO_NUM != 0)
|
|
pCtrl->outInfoList[pCtrl->outInfoIdx].setMotoPos = setMotoPos;
|
|
pCtrl->outInfoList[pCtrl->outInfoIdx].effort = effort;
|
|
#endif
|
|
}
|
|
#if (0)
|
|
if (pCtrl->first == 1)
|
|
{
|
|
printf("MotoCloseCtrlProc first run\r\n");
|
|
}
|
|
#endif
|
|
pCtrl->first = 0;
|
|
}
|
|
|
|
//------------------------------------------------------------------------
|
|
|
|
#define CPU_RATE 0 // 测试利用率
|
|
|
|
u32 g_workGap = 0;
|
|
float g_cpuRate = 0;
|
|
|
|
void CPURateIntProc(int section)
|
|
{
|
|
#if (CPU_RATE != 0)
|
|
#if (1)
|
|
if (section == 0)
|
|
{
|
|
SetDebugOn();
|
|
}
|
|
else if (section == 1)
|
|
{
|
|
SetDebugOff();
|
|
}
|
|
#else
|
|
static u32 workBeg = 0;
|
|
static u32 workEnd = 0;
|
|
static u32 lastEnd = 0;
|
|
|
|
if (section == 0)
|
|
{
|
|
SetDebugOn();
|
|
workBeg = GetUsSoftTimer();
|
|
}
|
|
else if (section == 1)
|
|
{
|
|
workEnd = GetUsSoftTimer();
|
|
g_workGap = workEnd - lastEnd;
|
|
lastEnd = workEnd;
|
|
if (g_workGap != 0)
|
|
{
|
|
g_cpuRate = (workEnd - workBeg) * 100.0 / g_workGap;
|
|
}
|
|
|
|
SetDebugOff();
|
|
}
|
|
#endif
|
|
#endif
|
|
}
|
|
|
|
// 控制中断， 10K
|
|
void DriversCtrlIntProc(void)
|
|
{
|
|
#if (1)
|
|
// 运动控制计算
|
|
DrvMotionCalcProc();
|
|
#endif
|
|
|
|
#if (1)
|
|
// 电机控制
|
|
DriverCtrl * pCtrl;
|
|
for (int i = 0; i < DRIVERS_NUM; i++)
|
|
{
|
|
pCtrl = &g_driverCtrl[i];
|
|
if (pCtrl->paras.cmdSource == 0) // 内部指令
|
|
{
|
|
pCtrl->exCmdCounter = GetDrvMotionRealPos(i) + pCtrl->posOffset; // 更新运动控制位置到指令位置
|
|
}
|
|
MotoCloseCtrlProc(pCtrl);
|
|
}
|
|
|
|
#endif
|
|
}
|
|
|
|
//------------------------------------------------------------------------
|
|
|
|
u32 GetDrvErrorCode(int driverIdx)
|
|
{
|
|
DriverCtrl * pCtrl;
|
|
if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
|
|
{
|
|
return 0;
|
|
}
|
|
pCtrl = &g_driverCtrl[driverIdx];
|
|
return pCtrl->errorCode;
|
|
}
|
|
|
|
void CleanDrvErrorCode(int driverIdx)
|
|
{
|
|
DriverCtrl * pCtrl;
|
|
if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
|
|
{
|
|
return;
|
|
}
|
|
pCtrl = &g_driverCtrl[driverIdx];
|
|
pCtrl->errorCode = 0;
|
|
}
|
|
|
|
void SetDrvErrorCode(int driverIdx, int code)
|
|
{
|
|
DriverCtrl * pCtrl;
|
|
if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
|
|
{
|
|
return;
|
|
}
|
|
pCtrl = &g_driverCtrl[driverIdx];
|
|
pCtrl->errorCode = code;
|
|
}
|
|
|
|
//--------------------------------------------------------------------------
|
|
#ifndef CLOSEMOTO_POSERR_MAXVAL
|
|
#define CLOSEMOTO_POSERR_MAXVAL (MOTO_PPC*2)
|
|
#endif
|
|
|
|
void DriverRunTask(void)
|
|
{
|
|
s32 poserr = 0;
|
|
DriverCtrl * pCtrl;
|
|
for (int i = 0; i < DRIVERS_NUM; i++)
|
|
{
|
|
pCtrl = &g_driverCtrl[i];
|
|
if (pCtrl->errorCode == 0 &&
|
|
pCtrl->paras.closeCtrlMode == CTRLMODE_CLOSE &&
|
|
1)
|
|
{
|
|
// 检测位置误差过大错误
|
|
poserr = abs(pCtrl->cmdPosition*pCtrl->stepAngle - pCtrl->ecdPosition);
|
|
if (poserr > CLOSEMOTO_POSERR_MAXVAL)
|
|
{
|
|
printf("driver %d pos error, cmdPosition=%d, ecdPosition=%d, sub=%d\r\n", i+1, pCtrl->cmdPosition, pCtrl->ecdPosition, poserr);
|
|
pCtrl->errorCode = DR_ERR_POS; // 出现错误
|
|
SetEnableCtrl(i, 0); // 断使能
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
//------------------------------------------------------------------------
|
|
|
|
// 重置驱动器位置
|
|
void ResetDriverPos(int driverIdx, s32 pos)
|
|
{
|
|
DriverCtrl * pCtrl;
|
|
if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
|
|
{
|
|
return;
|
|
}
|
|
pCtrl = &g_driverCtrl[driverIdx];
|
|
|
|
printf("reset pos to=%d\r\n", pos);
|
|
|
|
EnterCritical();
|
|
s32 cpos = GetDrvMotionRealPos(driverIdx);
|
|
SetDrvMotionRealPos(driverIdx, pos);
|
|
s32 ofst = cpos - pos;
|
|
pCtrl->posOffset += ofst;
|
|
ExitCritical();
|
|
}
|
|
|
|
void SetDriverTargetPos(int driverIdx, s32 tpos)
|
|
{
|
|
SetDrvMotionRealPos(driverIdx, tpos);
|
|
}
|
|
|
|
void SetDriverOffset(int driverIdx, s32 ofst)
|
|
{
|
|
s32 cpos = GetDrvMotionRealPos(driverIdx);
|
|
s32 tpos = cpos + ofst;
|
|
SetDrvMotionRealPos(driverIdx, tpos);
|
|
}
|
|
|
|
//------------------------------------------------------------------------------------
|
|
|
|
void SetOpenRunCircle(int driverIdx, int circle)
|
|
{
|
|
DriverCtrl * pCtrl;
|
|
if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
|
|
{
|
|
return;
|
|
}
|
|
pCtrl = &g_driverCtrl[driverIdx];
|
|
|
|
pCtrl->paras.openRunCircle = circle;
|
|
}
|
|
|
|
void SetOpenLockCircle(int driverIdx, int circle)
|
|
{
|
|
DriverCtrl * pCtrl;
|
|
if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
|
|
{
|
|
return;
|
|
}
|
|
pCtrl = &g_driverCtrl[driverIdx];
|
|
|
|
pCtrl->paras.openLockCircle = circle;
|
|
}
|
|
|
|
void SetOpenMaxOffset(int driverIdx, int maxofst)
|
|
{
|
|
DriverCtrl * pCtrl;
|
|
if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
|
|
{
|
|
return;
|
|
}
|
|
pCtrl = &g_driverCtrl[driverIdx];
|
|
|
|
pCtrl->paras.openMaxOffset = maxofst;
|
|
}
|
|
|
|
s32 GetClosePositionFromEcd(int driverIdx)
|
|
{
|
|
DriverCtrl * pCtrl;
|
|
if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
|
|
{
|
|
return 0;
|
|
}
|
|
pCtrl = &g_driverCtrl[driverIdx];
|
|
return pCtrl->ecdPosition / pCtrl->stepAngle;
|
|
}
|
|
|
|
void SetCloseCircle(int driverIdx, int circle)
|
|
{
|
|
DriverCtrl * pCtrl;
|
|
if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
|
|
{
|
|
return;
|
|
}
|
|
pCtrl = &g_driverCtrl[driverIdx];
|
|
|
|
pCtrl->paras.closeCircle = circle;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------------
|
|
|
|
#define ANGLE_PER_CIRCLE 36000 // 360 度
|
|
|
|
s32 CalcAngleByEcdValue(s32 ecd)
|
|
{
|
|
s32 angle = -1;
|
|
float counter;
|
|
counter = ecd;
|
|
if (counter < 0)
|
|
{
|
|
}
|
|
counter = (counter * ANGLE_PER_CIRCLE / MOTO_ECD_PER_CIRCLE); // 计算角度
|
|
|
|
angle = counter + (counter>0?(counter!=0?1:0):-1) * (0.500001f);
|
|
|
|
// 角度值在区间 [0, ANGLE_PER_CIRCLE)
|
|
while (angle < 0)
|
|
{
|
|
angle += ANGLE_PER_CIRCLE;
|
|
}
|
|
while (angle >= ANGLE_PER_CIRCLE)
|
|
{
|
|
angle -= ANGLE_PER_CIRCLE;
|
|
}
|
|
return angle;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------------
|
|
|
|
|
|
//------------------------------------------------------------------------------------
|
|
|
|
// 设置电机控制模式, 0开环 1闭环
|
|
void SetDriverCtrlMode(int driverIdx, int mode)
|
|
{
|
|
if (driverIdx < 0 || driverIdx >= DRIVERS_NUM ||
|
|
(mode != 0 && mode != 1) ||
|
|
0)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// 设置参数，先去使能
|
|
SetEnableCtrl(driverIdx, 0);
|
|
int ctrlmod = g_driverCtrl[driverIdx].paras.closeCtrlMode;
|
|
|
|
if (mode == 0)
|
|
{
|
|
printf("set moto%d ctrlmode to open\r\n", driverIdx+1);
|
|
}
|
|
else if (mode == 1)
|
|
{
|
|
printf("set moto%d ctrlmode to close\r\n", driverIdx+1);
|
|
}
|
|
|
|
g_driverCtrl[driverIdx].paras.closeCtrlMode = mode;
|
|
|
|
if (ctrlmod != g_driverCtrl[driverIdx].paras.closeCtrlMode && // 控制模式不同
|
|
g_driverCtrl[driverIdx].paras.closeCtrlMode != 0 && // 闭环控制
|
|
1)
|
|
{
|
|
ResetMotoPos(driverIdx, 100);
|
|
DelayMs(200);
|
|
g_driverCtrl[driverIdx].InitEncoder();
|
|
DelayMs(10);
|
|
}
|
|
|
|
DelayMs(1);
|
|
|
|
SetEnableCtrl(driverIdx, 1);
|
|
}
|
|
|
|
//------------------------------------------------------------------------------------
|
|
|
|
void DlBlk(u32 dl)
|
|
{
|
|
|
|
}
|
|
|
|
// DRIVER
|
|
void TestDriver(char * para1, char * para2)
|
|
{
|
|
int p1, p2;
|
|
|
|
p1 = p2 = 0;
|
|
if (para1 != NULL && strcmp(para1, "") != 0)
|
|
{
|
|
p1 = atoi(para1);
|
|
}
|
|
if (para2 != NULL && strcmp(para2, "") != 0)
|
|
{
|
|
p2 = atoi(para2);
|
|
}
|
|
if (p1 == p2)
|
|
{
|
|
}
|
|
//--------------------------------
|
|
DriverPara * pParas;
|
|
|
|
if (p1 >= 0 && p1 < 500)
|
|
{
|
|
int i;
|
|
int alllop = 0;
|
|
int dridx = p1/100;
|
|
int paidx = p1%100;
|
|
|
|
int lopcount;
|
|
int didxcount;
|
|
|
|
if (dridx == 0)
|
|
{
|
|
if (p2 == 0)
|
|
{
|
|
printf("gap=%d, cpu rate=%.2f%%\r\n", g_workGap, g_cpuRate);
|
|
alllop = DRIVERS_NUM;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
dridx -= 1;
|
|
}
|
|
|
|
lopcount = alllop;
|
|
didxcount = dridx;
|
|
do
|
|
{
|
|
if (didxcount >= 0 && didxcount < DRIVERS_NUM)
|
|
{
|
|
pParas = &(g_driverCtrl[didxcount].paras);
|
|
|
|
if (paidx == 0)
|
|
{
|
|
printf("\r\nmoto%d paras:\r\n", didxcount+1);
|
|
for (i = 0; i < DRV_PARA_NUMS; i++)
|
|
{
|
|
const char * parastr = GetDriverParaName(i);
|
|
if (parastr != NULL && strlen(parastr) != 0)
|
|
{
|
|
printf(" %d. %s = %d\r\n", i+1, parastr, *((int*)(((u32)pParas) + i*4)));
|
|
}
|
|
}
|
|
|
|
printf("\r\n g_driverCtrl[%d] values:\r\n"
|
|
"\t cmdPosition=%d, \r\n"
|
|
"\t ecdPosition=%d, \r\n"
|
|
"\t runPosition=%d, \r\n",
|
|
didxcount, g_driverCtrl[didxcount].cmdPosition, g_driverCtrl[didxcount].ecdPosition, g_driverCtrl[didxcount].runPosition);
|
|
}
|
|
else
|
|
{
|
|
// 设置参数，先去使能
|
|
SetEnableCtrl(didxcount, 0);
|
|
int ctrlmod = g_driverCtrl[dridx].paras.closeCtrlMode;
|
|
|
|
printf("set %s = %d, old = %d\r\n", GetDriverParaName(paidx-1), p2, *((int*)(((u32)pParas) + (paidx-1)*4)));
|
|
|
|
*((int*)(((u32)pParas) + (paidx-1)*4)) = p2;
|
|
g_driverCtrl[dridx].stepAngle = CalcStepAngle(g_driverCtrl[dridx].paras.stepPerCircle, g_driverCtrl[dridx].paras.numerator, g_driverCtrl[dridx].paras.denominator);
|
|
|
|
if (ctrlmod != g_driverCtrl[dridx].paras.closeCtrlMode &&
|
|
g_driverCtrl[dridx].paras.closeCtrlMode != 0 )
|
|
{
|
|
ResetMotoPos(dridx, 50);
|
|
DelayMs(200);
|
|
g_driverCtrl[dridx].InitEncoder();
|
|
DelayMs(10);
|
|
}
|
|
printf("moto%d stepAngle=%.2f\r\n", dridx+1, g_driverCtrl[dridx].stepAngle);
|
|
|
|
DelayMs(1);
|
|
|
|
#if (OUT_INFO_NUM != 0)
|
|
g_driverCtrl[dridx].outInfoIdx = 0;
|
|
g_driverCtrl[dridx].timeGap = 0;
|
|
g_driverCtrl[dridx].outInfoEn = 1;
|
|
#endif
|
|
SetEnableCtrl(didxcount, 1);
|
|
}
|
|
}
|
|
if (lopcount != 0)
|
|
{
|
|
lopcount--;
|
|
didxcount++;
|
|
DelayMs(100);
|
|
continue;
|
|
}
|
|
else
|
|
{
|
|
break;
|
|
}
|
|
|
|
}while(1);
|
|
}
|
|
#if (1)
|
|
if (p1 >= 1000 && p1 < 5000)
|
|
{// 测试电机编码器
|
|
int dridx = p1/1000 - 1;
|
|
int paidx = p1%1000;
|
|
|
|
if (dridx < 0 || dridx >= DRIVERS_NUM)
|
|
{
|
|
return;
|
|
}
|
|
|
|
int sel, count;
|
|
sel = p2%10;
|
|
count = p2/10;
|
|
|
|
DriverCtrl * pCtrl;
|
|
pCtrl = &(g_driverCtrl[dridx]);
|
|
|
|
int rslt, arsult;
|
|
int ang;
|
|
|
|
SetEnableCtrl(dridx, 0);
|
|
|
|
DelayMs(1);
|
|
|
|
do
|
|
{
|
|
rslt = pCtrl->ReadEncoderCounter();
|
|
ang = CalcAngleByEcdValue(rslt);
|
|
printf("before run moto%d, rslt=%d(0x%x), ang=%.2f\r\n", dridx+1, rslt, rslt, ang/100.0);
|
|
|
|
if (paidx == 0)
|
|
{
|
|
if (sel == 0)
|
|
{
|
|
ResetMotoPos(dridx, 0);
|
|
}
|
|
else if (sel == 1)
|
|
{
|
|
ResetMotoPos(dridx, 50);
|
|
}
|
|
else if (sel == 2)
|
|
{
|
|
MotoOneFullStep(dridx, 1, 50);
|
|
}
|
|
else if (sel == 3)
|
|
{
|
|
MotoOneFullStep(dridx, -1, 50);
|
|
}
|
|
else if (sel == 4)
|
|
{
|
|
MotoOneMicoStep(dridx, 1, 50);
|
|
}
|
|
else if (sel == 5)
|
|
{
|
|
MotoOneMicoStep(dridx, -1, 50);
|
|
}
|
|
else if (sel == 100)
|
|
{
|
|
}
|
|
}
|
|
DelayMs(50);
|
|
arsult = pCtrl->ReadEncoderCounter();
|
|
ang = CalcAngleByEcdValue(arsult);
|
|
printf("after run moto%d, rslt=%d(0x%x), ang=%.2f\r\n", dridx+1, arsult, arsult, ang/100.0, arsult-rslt);
|
|
count--;
|
|
}while(count > 0);
|
|
}
|
|
#endif
|
|
if (p1 >= 10000 && p1 < 50000)
|
|
{// 测试电机运动
|
|
int dridx = p1/10000 - 1;
|
|
u32 timer;
|
|
int rslt, arsult, ang;
|
|
if (dridx < 0 || dridx >= DRIVERS_NUM)
|
|
{
|
|
return;
|
|
}
|
|
|
|
DriverCtrl * pCtrl;
|
|
pCtrl = &(g_driverCtrl[dridx]);
|
|
|
|
rslt = pCtrl->ReadEncoderCounter();
|
|
ang = CalcAngleByEcdValue(rslt);
|
|
printf("before run moto%d, rslt=%d(0x%x), ang=%.2f, sub=%d\r\n", dridx+1, rslt, rslt, ang/100.0);
|
|
|
|
printf("test run start, idx=%d, ", dridx);
|
|
|
|
timer = GetMsSoftTimer();
|
|
|
|
{
|
|
DrvMotionPara drpara;
|
|
memset(&drpara, 0, sizeof(DrvMotionPara));
|
|
|
|
// 输入参数
|
|
drpara.movement = p2; // 位移量
|
|
drpara.startPPS = pCtrl->paras.startPPS; // 启动速度
|
|
drpara.runPPS = pCtrl->paras.runPPS; // 运动速度
|
|
drpara.addPPSS = pCtrl->paras.addPPSS; // 启停加速度
|
|
drpara.brkPPSS = pCtrl->paras.brkPPSS; // 刹车加速度
|
|
drpara.DelayWhenBlock = DlBlk;
|
|
|
|
drpara.blockRunflag = 1; // 阻塞运行标志, =1阻塞运行,同步运动 =0,非阻塞,异步运动
|
|
|
|
DrvMotionStart(dridx, &drpara); // 启动运动控制
|
|
}
|
|
|
|
timer = GetMsSoftTimer() - timer;
|
|
printf("test run over, time=%dms\r\n", timer);
|
|
|
|
DelayMs(100);
|
|
|
|
arsult = pCtrl->ReadEncoderCounter();
|
|
ang = CalcAngleByEcdValue(arsult);
|
|
printf("after run moto%d, rslt=%d(0x%x), ang=%.2f\r\n", dridx+1, arsult, arsult, ang/100.0, arsult-rslt);
|
|
}
|
|
|
|
if (p1 >= 100000 && p1 < 200000)
|
|
{
|
|
int dridx = 0;
|
|
|
|
// 输入参数
|
|
DrvMotionPara drpara;
|
|
DriverCtrl * pCtrl;
|
|
|
|
for (dridx = 0; dridx < DRIVERS_NUM; dridx++)
|
|
{
|
|
printf("test run start, idx=%d, ", dridx);
|
|
pCtrl = &(g_driverCtrl[dridx]);
|
|
|
|
memset(&drpara, 0, sizeof(DrvMotionPara));
|
|
|
|
drpara.movement = p2; // 位移量
|
|
drpara.startPPS = pCtrl->paras.startPPS; // 启动速度
|
|
drpara.runPPS = pCtrl->paras.runPPS; // 运动速度
|
|
drpara.addPPSS = pCtrl->paras.addPPSS; // 启停加速度
|
|
drpara.brkPPSS = pCtrl->paras.brkPPSS; // 刹车加速度
|
|
|
|
drpara.funTestTime = 0; // 运行过程中外部函数检测间隔时间计数器
|
|
|
|
drpara.blockRunflag = 0; // 阻塞运行标志, =1阻塞运行,同步运动 =0,非阻塞,异步运动
|
|
|
|
DrvMotionStart(dridx, &drpara); // 启动运动控制
|
|
}
|
|
|
|
do
|
|
{
|
|
printf("gap=%d, cpu rate=%.2f%%\r\n", g_workGap, g_cpuRate);
|
|
if (IsConsoleCancel() != 0)
|
|
{
|
|
break;
|
|
}
|
|
|
|
DelayMs(10);
|
|
|
|
}while(1);
|
|
}
|
|
|
|
if (p1 >= 200000 && p1 <= 240000)
|
|
{
|
|
#if (OUT_INFO_NUM != 0)
|
|
int dridx = (p1-200000)/10000;
|
|
if (dridx < 0 || dridx >= DRIVERS_NUM)
|
|
{
|
|
return;
|
|
}
|
|
|
|
DriverCtrl * pCtrl;
|
|
pCtrl = &(g_driverCtrl[dridx]);
|
|
int gap = p1 % 10000;
|
|
|
|
if (p2 < 0)
|
|
{
|
|
int i = 0;
|
|
int num = pCtrl->outInfoIdx;
|
|
printf("out info num=%d\r\n", num);
|
|
|
|
for (i = 1; i < num; i++)
|
|
{
|
|
printf( "%d "
|
|
"%d "
|
|
"%d "
|
|
"%d "
|
|
"%d "
|
|
"%d "
|
|
"%d "
|
|
"%d "
|
|
|
|
"%d "
|
|
"%d "
|
|
"%d "
|
|
"\r\n"
|
|
,i
|
|
,pCtrl->outInfoList[i].cmdPosition // 指令位置
|
|
,pCtrl->outInfoList[i].targetMotoPos // 目标位置
|
|
,pCtrl->outInfoList[i].ecdPosition // 编码器位置
|
|
,pCtrl->outInfoList[i].thisPosErr // 位置偏差
|
|
,pCtrl->outInfoList[i].integral // 偏差积分
|
|
,pCtrl->outInfoList[i].differential // 位置微分
|
|
,pCtrl->outInfoList[i].pospid // 位置PID
|
|
,pCtrl->outInfoList[i].thisactVel // 本次速度
|
|
|
|
,pCtrl->outInfoList[i].setMotoPos // 设置位置
|
|
,pCtrl->outInfoList[i].effort // 设置电流
|
|
);
|
|
|
|
if (IsConsoleCancel() != 0)
|
|
{
|
|
break;
|
|
}
|
|
DelayMs(10);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
pCtrl->outInfoIdx = 0;
|
|
pCtrl->timeGap = gap;
|
|
pCtrl->outInfoEn = 1;
|
|
|
|
printf("test out begin, idx=%d\r\n", dridx);
|
|
|
|
// 输入参数
|
|
DrvMotionPara drpara;
|
|
memset(&drpara, 0, sizeof(DrvMotionPara));
|
|
|
|
drpara.movement = p2; // 位移量
|
|
drpara.startPPS = pCtrl->paras.startPPS; // 启动速度
|
|
drpara.runPPS = pCtrl->paras.runPPS; // 运动速度
|
|
drpara.addPPSS = pCtrl->paras.addPPSS; // 启停加速度
|
|
drpara.brkPPSS = pCtrl->paras.brkPPSS; // 刹车加速度
|
|
|
|
drpara.blockRunflag = 0; // 阻塞运行标志, =1阻塞运行,同步运动 =0,非阻塞,异步运动
|
|
|
|
DrvMotionStart(dridx, &drpara); // 启动运动控制
|
|
|
|
do
|
|
{
|
|
if (pCtrl->outInfoEn == 0)
|
|
{
|
|
break;
|
|
}
|
|
|
|
if (IsConsoleCancel() != 0)
|
|
{
|
|
break;
|
|
}
|
|
}while(1);
|
|
|
|
printf("test out end\r\n");
|
|
}
|
|
#endif
|
|
}
|
|
|
|
}
|
|
|
|
void ShowDriver(char * para1, char * para2)
|
|
{
|
|
int p1, p2;
|
|
|
|
p1 = p2 = 0;
|
|
if (para1 != NULL && strcmp(para1, "") != 0)
|
|
{
|
|
p1 = atoi(para1);
|
|
}
|
|
if (para2 != NULL && strcmp(para2, "") != 0)
|
|
{
|
|
p2 = atoi(para2);
|
|
}
|
|
if (p1 == p2)
|
|
{
|
|
}
|
|
|
|
if (p1 == 1)
|
|
{
|
|
if (p2 > 0 && p2 <= DRIVERS_NUM)
|
|
{
|
|
printf("%d.cmdPosition=%d, ecdPosition=%d, runPosition=%d\r\n",
|
|
p2, g_driverCtrl[p2-1].cmdPosition, g_driverCtrl[p2-1].ecdPosition, g_driverCtrl[p2-1].runPosition);
|
|
}
|
|
}
|
|
}
|
|
|
|
#endif
|