FOC PMSM EPS SVPWM 电驱
SVPWM头文件
UserParms.h
C文件
ACIM.c
Encoder.c
InitCurModel.c
UserParms.h
//#define TORQUE_MODE
#define DIAGNOSTICS
//**************** 振荡器 ************************************
#define dFoscExt 7372800 // 外部晶振或时钟频率 (Hz)
#define dPLL 8 // PLL比率
#define dLoopTimeInSec 0.00005 // PWM周期 - 100 uS, 10Khz PWM
#define dDeadTimeSec 0.000002 // 以秒为单位的死区时间
// Derived
#define dFosc (dFoscExt*dPLL) // 时钟频率 ( Hz)
#define dFcy (dFosc/4) // 指令频率 ( Hz)
#define dTcy (1.0/dFcy) // 指令周期 (s)
#define dDeadTime (int)(dDeadTimeSec*dFcy) // 以 dTcys为单位的死区时间
#define dLoopInTcy (dLoopTimeInSec/dTcy) // 以 Tcy为单位的基本循环周期
#define dDispLoopTime 0.100 // 显示和按钮状态查询循环
//**************** 电机参数 ********************************
#define diPoles 1 // 极对数
#define diCntsPerRev 2000 // 每转的编码器线数
#define diNomRPM 3600 // 电机铭牌上的转速 RPM
#define dfRotorTmConst 0.078 // 以秒为单位的转子时间常数来自电机制造商
//**************** 测量 *************************************
#define diIrpPerCalc 30 // 每次速度计算的 PWM循环次数
//************** PI 系数 ************************************
#define dDqKp 0x2000 // 4.0 (NKo = 4)
#define dDqKi 0x0100; // 0.125
#define dDqKc 0x0100; // 0.125
#define dDqOutMax 0x5A82; // 0.707设定该值以避免饱和
#define dQqKp 0x2000; // 4.0 (NKo = 4)
#define dQqKi 0x0100; // 0.125
#define dQqKc 0x0100; // 0.125
#define dQqOutMax 0x5A82; // 0.707设定该值以避免饱和
#define dQrefqKp 0x4000 // 8.0 (NKo = 4)
#define dQrefqKi 0x0800 // 1.0
#define dQrefqKc 0x0800 // 1.0
#define dQrefqOutMax 0x3FFF // 0.4999设定该值以避免饱和
//************** ADC换算 ************************************
// 标定常数 :由校准或硬件设计确定。
#define dqK 0x3FFF; // 等于 0.4999
#define dqKa 0x3FFF; // 等于 0.4999
#define dqKb 0x3FFF; // 等于 0.4999
//************** 弱磁 ****************************************
// 在恒转矩范围内的磁通给定值。
// 根据经验确定、给出额定压 /频比
#define dqK1 3750; //
ACIM.c
/**********************************************************************
**
* 作者 : John Theys/Dave Ross *
**
* 文件名 : ACIM.c *
* 日期 : 10/31/03 ** 文件版本 : 3.00 *
**
* 使用工具 : MPLAB -> 6.43 *
* 编译器 -> 1.20.00 *
**
* 链接文件 : p30f6010.gld *
**
**
***********************************************************************
*10/31/03 2.00 发布电机运行正常但仍有些遗留的小问题
*
*12/19/03 2.01 完成结构为所有用户定义变量创建 UserParms.h。
*
*02/12/043.00-从项目中去除了不需要的文件。
* -将 iRPM 改为 int以纠正浮点计算问题。
* -CalcVel() 和转速控制环仅在 iIrpPerCalc指定的数个循环周期后执行
*
*-增加了 iDispLoopCount 变量以安排显示和按钮子程序的执行时间
* -trig.s文件改为使用程序空间来存储正弦数据。
*-增加了 DiagnosticsOutput()函数该函数使用输出比较通道来输出控
* 制变量信息。
*-增加了 TORQUE_MODE 定义以忽略转速控制环。
*-关闭 curmodel.s文件中的 SATDW位。自动饱和功能阻止转差角计算正确
* 翻转返回。
************************************************************************
*代码说明
*
*该文件给出了使用 dsPIC30F实现的三相交流感应电机矢量控制实例。
*采用空间矢量调制作为控制策略。
***********************************************************************/
/*************************** 全局定义 ***********************/
#define INITIALIZE
#include "Motor.h"
#include "Parms.h"
#include "Encoder.h"
#include "SVGen.h"
#include "ReadADC.h"
#include "MeasCurr.h"
#include "CurModel.h"
#include "FdWeak.h"
#include "Control.h"
#include "PI.h"
#include "Park.h"#include "OpenLoop.h"
#include "LCD.h"
#include "bin2dec.h"
#include "UserParms.h"
/*********************** 全局定义结束 ********************/
unsigned short uWork;
short iCntsPerRev;
short iDeltaPos;
union {
struct
{
unsigned DoLoop:1;
unsigned OpenLoop:1;
unsigned RunMotor:1;
unsigned Btn1Pressed:1;
unsigned Btn2Pressed:1;
unsigned Btn3Pressed:1;
unsigned Btn4Pressed:1;
unsigned ChangeMode:1;
unsigned ChangeSpeed:1;
unsigned :7;
}bit;
WORD Word;
} uGF; // 通用标志
tPIParm PIParmQ;
tPIParm PIParmQref;
tPIParm PIParmD;
tReadADCParm ReadADCParm;
int iRPM;
WORD iMaxLoopCnt;
WORD iLoopCnt;
WORD iDispLoopCnt;
/**********************************************************************/
void __attribute__((__interrupt__)) _ADCInterrupt(void);
void SetupBoard( void );
bool SetupParm(void);
void DoControl( void );
void Dis_RPM( BYTE bChrPosC, BYTE bChrPosR );
void DiagnosticsOutput(void);
/******************** 主函数开头 *************************/
int main ( void )
{
SetupPorts();
InitLCD();
while(1)
{
uGF.Word = 0; // 清除标志
// 初始化模式
uGF.bit.OpenLoop = 1; // 以开环模式起动
// 初始化 LED
pinLED1 = 0;
pinLED2 = !uGF.bit.OpenLoop;
pinLED3 = 0;
pinLED4 = 0;
// 初始化控制板
SetupBoard();
// 对用户指定参数进行初始化并在出错时停止
if( SetupParm() )
{
//错误
uGF.bit.RunMotor=0;
return;
}
// 清零 i和
PIParmD.qdSum = 0;
PIParmQ.qdSum = 0;
PIParmQref.qdSum = 0;
iMaxLoopCnt = 0;
Wrt_S_LCD("Vector Control ", 0 , 0);
Wrt_S_LCD("S4-Run/Stop ", 0, 1);
// 使能 ADC中断并开始主循环定时
IFS0bits.ADIF = 0;
IEC0bits.ADIE = 1;
if(!uGF.bit.RunMotor)
{
// 初始化电流偏移量补偿
while(!pinButton1) //在此处等待直至按钮 1按下
{
ClrWdt();
// 开始偏移量累加 //并在等待时对电流偏移量进行累加
MeasCompCurr();
}
while(pinButton1); //当按钮 1释放时
uGF.bit.RunMotor = 1; //随后起动电机
}
//电机运行
uGF.bit.ChangeMode = 1;
// 使能电机控制 PCB上的驱动器 IC
pinPWMOutputEnable_ = 0;
Wrt_S_LCD("RPM= ", 0, 0);
Wrt_S_LCD("S5-Cls. Lp S6-2x", 0, 1);
//电机运行循环
while(1)
{
ClrWdt();// 如果使用 OC7和 OC8显示矢量控制变量
// 调用更新代码。
#ifdefDIAGNOSTICS
DiagnosticsOutput();
#endif
// 每隔 50毫秒执行更新 LCD显示和查询按钮状态的代码。
//
if(iDispLoopCnt >= dDispLoopCnt)
{
//Display RPM
Dis_RPM(5,0);
// 按钮 1控制电机的起停
if(pinButton1)
{
if( !uGF.bit.Btn1Pressed )
uGF.bit.Btn1Pressed = 1;
}
else
{
if( uGF.bit.Btn1Pressed )
{
// 按钮刚被释放
uGF.bit.Btn1Pressed = 0;
// 开始停止过程
uGF.bit.RunMotor = 0;
pinPWMOutputEnable_ = 1;
break;
}
}
// 在运行时按钮 2将控制开 /闭环模式之间的切换
if(pinButton2)
{
if( !uGF.bit.Btn2Pressed )
uGF.bit.Btn2Pressed = 1;
}
else
{
if( uGF.bit.Btn2Pressed )
{
// 按钮刚释放
uGF.bit.Btn2Pressed = 0;
uGF.bit.ChangeMode = 1;
uGF.bit.OpenLoop = ! uGF.bit.OpenLoop;
pinLED2 = !uGF.bit.OpenLoop;
}
}
//在运行时按钮 3将加倍 /减半速度或转矩给定
if(pinButton3)
{
if( !uGF.bit.Btn3Pressed )
uGF.bit.Btn3Pressed = 1;
LATGbits.LATG0 = 0;
}
else
{if( uGF.bit.Btn3Pressed )
{
// 按钮刚释放
uGF.bit.Btn3Pressed = 0;
uGF.bit.ChangeSpeed = !uGF.bit.ChangeSpeed;
pinLED3 = uGF.bit.ChangeSpeed;
LATGbits.LATG0 = 1;
}
}
// 按钮 4不具备任何功能
if(pinButton4)
{
if( !uGF.bit.Btn4Pressed )
uGF.bit.Btn4Pressed = 1;
}
else
{
if( uGF.bit.Btn4Pressed )
{
// 按钮刚被释放
uGF.bit.Btn4Pressed = 0;
//*** 此处加入按钮 4功能的代码
}
}
} // 显示和按钮查询代码结束
} // 电机运行循环结束
} //主循环结束
// 不应执行到此处
while(1){}
}
//--------------------------------------------------------------------//
对 Id控制环、 Iq控制环和速度控制环中的每个控制环执行一次 PI迭代
void DoControl( void )
{
short i;
// 假定 ADC通道 0具有来自速度电位器 AN7的有符号小数格式原始 A/D值
ReadSignedADC0( &ReadADCParm );
// 设定给定速度
if(uGF.bit.ChangeSpeed)
CtrlParm.qVelRef = ReadADCParm.qADValue/8;
else
CtrlParm.qVelRef = ReadADCParm.qADValue/16;
if( uGF.bit.OpenLoop )
{
// 开环强制旋转角、 Vd和 Vq
if( uGF.bit.ChangeMode )
{
// 改变为开环模式
uGF.bit.ChangeMode = 0;
// 同步角度
OpenLoopParm.qAngFlux = CurModelParm.qAngFlux;
// 未使用 VqRef和 VdRef
CtrlParm.qVqRef = 0;
CtrlParm.qVdRef = 0;
}
OpenLoopParm.qVelMech = CtrlParm.qVelRef;
// 为 CorrectPhase所需的给定值和符号
// 计算 1.15格式的转子磁通旋转角。
CurModelParm.qVelMech = EncoderParm.qVelMech;
CurModel();
ParkParm.qVq = 0;
if( OpenLoopParm.qVelMech >= 0 )
i = OpenLoopParm.qVelMech;
else
i = -OpenLoopParm.qVelMech;
uWork = i <<2;
if( uWork > 0x5a82 )
uWork = 0x5a82;
if( uWork < 0x1000 )
uWork = 0x1000;
ParkParm.qVd = uWork;
OpenLoop();
ParkParm.qAngle = OpenLoopParm.qAngFlux;
}
else
// 闭环矢量控制
{
if( uGF.bit.ChangeMode )
{
// 改变为闭环模式
uGF.bit.ChangeMode = 0;
// 同步角度以及准备 qdImag
CurModelParm.qAngFlux = OpenLoopParm.qAngFlux;
CurModelParm.qdImag = ParkParm.qId;
}
// 根据电流模型计算角度
CurModelParm.qVelMech = EncoderParm.qVelMech;
CurModel();
ParkParm.qAngle = CurModelParm.qAngFlux;
// 计算弱磁控制模式的 qVdRef
FdWeakening();
// 设定给定速度
// 如果应用运行在转矩模式转速控制环将被忽略。
// 从电位器读取的转速给定值将直接用作转矩给定 VqRef。
#ifdefTORQUE_MODE
CtrlParm.qVqRef = CtrlParm.qVelRef;
#else
// 通过对比每一次转速计算中的中断数和速度计数采样数来确定是否可以获得新的转速信
息。
//
// 如果可以获得新的转速信息则计算新的转速值并执行
// 转速控制环。
if(EncoderParm.iVelCntDwn == EncoderParm.iIrpPerCalc)
{
// 根据编码器累计计数值来计算转速
CalcVel();
// 执行转速控制环
PIParmQref.qInMeas = EncoderParm.qVelMech;
PIParmQref.qInRef = CtrlParm.qVelRef;
CalcPI(&PIParmQref);
CtrlParm.qVqRef = PIParmQref.qOut;
}
#endif
// Q的 PI控制
PIParmQ.qInMeas = ParkParm.qIq;
PIParmQ.qInRef = CtrlParm.qVqRef;
CalcPI(&PIParmQ);
ParkParm.qVq = PIParmQ.qOut;
// D的 PI控制
PIParmD.qInMeas = ParkParm.qId;
PIParmD.qInRef = CtrlParm.qVdRef;
CalcPI(&PIParmD);
ParkParm.qVd = PIParmD.qOut;
}
}
//--------------------------------------------------------------------//
ADC中断服务程序执行速度计算以及电压矢量更新循环。
// ADC采样和转换由 PWM周期触发。
// 速度计算假定计算之间的间隔时间是固定的。
//--------------------------------------------------------------------
void __attribute__((__interrupt__)) _ADCInterrupt(void)
{
IFS0bits.ADIF = 0;
// 递增控制显示和按钮功能执行的计数变量。
//
iDispLoopCnt++;
// 累计自上一次中断后的编码器计数
CalcVelIrp();
if( uGF.bit.RunMotor )
{
// 置位用于诊断的 LED1
pinLED1 = 1;
// 使用 TMR1来测量用于诊断的中断时间
TMR1 = 0;
iLoopCnt = TMR1;
MeasCompCurr();
// 根据 qSin、 qCos、 qIa、 qIb计算 qId、 qIq
ClarkePark();
// 计算控制值
DoControl();
// 根据 qAngle计算 qSin、 qCos
SinCos();
// 根据 qSin、 qCos、 qVd、 qVq计算 qValpha、 qVbeta
InvPark();
// 根据 qValpha、 qVbeta计算 Vr1、 Vr2、 Vr3。
CalcRefVec();
// 根据 Vr1、 Vr2、 Vr3计算和设定 PWM占空比
CalcSVGen();
// 测量循环时间
iLoopCnt = TMR1 - iLoopCnt;
if( iLoopCnt > iMaxLoopCnt )
iMaxLoopCnt = iLoopCnt;
// 清零用于诊断的 LED1
pinLED1 = 0;
}
}
//--------------------------------------------------------------------//
SetupBoard
//
// 初始化控制板
//--------------------------------------------------------------------
void SetupBoard( void )
{
BYTE b;
//禁止 ADC中断
IEC0bits.ADIE = 0;
// 复位电机控制功率模块上的所有故障。
pinFaultReset = 1;
for(b=0;b<10;b++)
Nop();
pinFaultReset = 0;
// 确保 PFC开关是关闭的。
pinPFCFire = 0;
// 确保制动开关是关闭的。
pinBrakeFire = 0;
}
//--------------------------------------------------------------------//
Dis_RPM
//
// 显示 RPM
//--------------------------------------------------------------------
void Dis_RPM( BYTE bChrPosC, BYTE bChrPosR )
{
if (EncoderParm.iDeltaCnt < 0)
Wrt_S_LCD("-", bChrPosC, bChrPosR);
else
Wrt_S_LCD(" ", bChrPosC, bChrPosR);
iRPM = EncoderParm.iDeltaCnt*60/
(MotorParm.fLoopPeriod*MotorParm.iIrpPerCalc*EncoderParm.iCntsPerRev);
Wrt_Signed_Int_LCD( iRPM, bChrPosC+1, bChrPosR);
}
//--------------------------------------------------------------------bool
SetupParm(void)
{
// 开启抗饱和功能以确保能够平滑处理溢出。
CORCONbits.SATA = 0;
// 设置所需参数
// 选取定标值为 8倍速度和电流标称值
// 在定标时使用 8倍电机标称机械转速单位为 RPM
MotorParm.iScaleMechRPM = diNomRPM*8;
// 极对数
MotorParm.iPoles = diPoles ;
//由 dsPIC正交编码配置检测的每转编码器计数值。
//
MotorParm.iCntsPerRev = diCntsPerRev;
// 以秒为单位的转子时间常数
MotorParm.fRotorTmConst = dfRotorTmConst;
// 基本循环周期单位为秒。 PWM中断周期
MotorParm.fLoopPeriod = dLoopInTcy * dTcy; //循环周期以周期为单位 * 秒 /周期
// 编码器转速中断周期单位为秒。
MotorParm.fVelIrpPeriod = MotorParm.fLoopPeriod;
// 每次转速计算的 vel中断数。
MotorParm.iIrpPerCalc = diIrpPerCalc; // 以循环为单位
// 电机的定标机械速度单位为转 /秒
MotorParm.fScaleMechRPS = MotorParm.iScaleMechRPM/60.0;
// 定标后的电机磁通转速单位为转 /秒
//通过该值定标后的所有无量纲磁通转速。
MotorParm.fScaleFluxRPS = MotorParm.iPoles*MotorParm.fScaleMechRPS;
// 磁通矢量每转一周的最小周期时间单位为秒
MotorParm.fScaleFluxPeriod = 1.0/MotorParm.fScaleFluxRPS;
//在最大磁通转速时的每个 LoopTime的转数系数
MotorParm.fScaleFracRevPerLoop = MotorParm.fLoopPeriod * MotorParm.fScaleFluxRPS;
// 定标后的电机磁通转速单位为弧度 /秒
// 通过该值定标后的所有无量纲转速单位为弧度 /秒。
MotorParm.fScaleFluxSpeed = 6.283 * MotorParm.fScaleFluxRPS;
// iScaleMechRPM时的编码器计数频率
MotorParm.lScaleCntRate = MotorParm.iCntsPerRev * (MotorParm.iScaleMechRPM/60.0);
// ============= 开环 ======================
OpenLoopParm.qKdelta = 32768.0 * 2 * MotorParm.iPoles * MotorParm.fLoopPeriod *
MotorParm.fScaleMechRPS;
OpenLoopParm.qVelMech = dqOL_VelMech;
CtrlParm.qVelRef = OpenLoopParm.qVelMech;
InitOpenLoop();
// ============= 编码器 ===============
if( InitEncoderScaling() )
// 出错
return True;
// ============= ADC - 测量电流和电位器 ======================
// 定标常数由校准或硬件设计决定。
ReadADCParm.qK = dqK;
MeasCurrParm.qKa = dqKa;
MeasCurrParm.qKb = dqKb;
// 初始偏移量
InitMeasCompCurr( 450, 730 );
// ============= 电流模型 ===============
if(InitCurModelScaling())
// 出错
return True;
// ============= 弱磁 ===============
// 恒转矩范围的弱磁常数
FdWeakParm.qK1 = dqK1; // 磁通给定值
// ============= PI D项 ===============
PIParmD.qKp = dDqKp;
PIParmD.qKi = dDqKi;
PIParmD.qKc = dDqKc;
PIParmD.qOutMax = dDqOutMax;
PIParmD.qOutMin = -PIParmD.qOutMax;
InitPI(&PIParmD);
// ============= PI Q项 ===============
PIParmQ.qKp = dQqKp;
PIParmQ.qKi = dQqKi;
PIParmQ.qKc = dQqKc;
PIParmQ.qOutMax = dQqOutMax;
PIParmQ.qOutMin = -PIParmQ.qOutMax;
InitPI(&PIParmQ);
// ============= PI Qref项 ===============
PIParmQref.qKp = dQrefqKp;
PIParmQref.qKi = dQrefqKi;
PIParmQref.qKc = dQrefqKc;
PIParmQref.qOutMax = dQrefqOutMax;
PIParmQref.qOutMin = -PIParmQref.qOutMax;
InitPI(&PIParmQref);
// ============= SVGen ===============
// 将 PWM周期设定为循环时间
SVGenParm.iPWMPeriod = dLoopInTcy;
// ============= TIMER #1 ======================
PR1 = 0xFFFF;
T1CONbits.TON = 1;
T1CONbits.TCKPS = 1; // 预分频比为 8时 => 一个单位为 1.08504 uS
// ============= 电机 PWM ======================
PDC1 = 0;
PDC2 = 0;
PDC3 = 0;
PDC4 = 0;
// 中心对齐 PWM。
// 注PWM周期设定为 dLoopInTcy/2但由于先采用递增计数随后为递减计数
// =>在计数到零时中断标志置 1 =>因此实际中断周期为 dLoopInTcy
//
PTPER = dLoopInTcy/2; // 将 PWM周期设定为循环时间该参数在 parms.h中定义
PWMCON1 = 0x0077; // 使能 PWM 1,2,3对工作在互补模式
DTCON1 = dDeadTime; // 死区时间
DTCON2 = 0;
FLTACON = 0; // 未使用 PWM故障引脚
FLTBCON = 0;
PTCON = 0x8002; // 使能 PWM中心对齐
// SEVTCMP: 特殊事件比较计数寄存器
// 相对于 PWM周期的 ADC捕获设定相位 0偏移量和递增计数
SEVTCMP = 2; // 不能为 0否则关断触发器文档中未列明
SEVTCMPbits.SEVTDIR = 0;
// ============= 编码器 ===============
MAXCNT = MotorParm.iCntsPerRev;
POSCNT = 0;
QEICON = 0;
QEICONbits.QEIM = 7; // 通过 MAXCNT脉冲复位 x4
QEICONbits.POSRES = 0; // 不要让索引脉冲复位计数器
QEICONbits.SWPAB = 0; // 方向
DFLTCON = 0; // 数字滤波器设定为关闭
// ============= ADC - 测量电流和电位器给定值 ======================
// ADC设定为对以下通道同时进行采样
// CH0=AN7, CH1=AN0, CH2=AN1, CH3=AN2
// 采样由 PWM触发且采样结果以有符号小数形式存放。
ADCON1 = 0;
// 有符号小数格式 DOUT = sddd dddd dd00 0000
ADCON1bits.FORM = 3;
// 电机控制 PWM间隔终止采样并启动转换
ADCON1bits.SSRC = 3;
// 同时采样选择位仅当 CHPS =01或 1x时应用
// 同时采样 CH0、 CH1、 CH2和 CH3当 CHPS = 1x时
// 同时采样 CH0和 CH1当 CHPS=01时
ADCON1bits.SIMSAM = 1;
//在上一次转换结束后立即开始采样。
// SAMP位自动置位。
ADCON1bits.ASAM = 1;
ADCON2 = 0;
// 同时采样 CH0、 CH1、 CH2、 CH3当 CHPS = 1x时
ADCON2bits.CHPS = 2;
ADCON3 = 0;
// A/D转换时钟选择位 = 8 * Tcy
ADCON3bits.ADCS = 15;
/* ADCHSADC输入通道选择寄存器 */
ADCHS = 0;
// CH0 为 AN7
ADCHSbits.CH0SA = 7;
// CH1正极性输入为 AN0 CH2正极性输入为 AN1 CH3正极性输入为 AN2
ADCHSbits.CH123SA = 0;
/* ADPCFGADC端口配置寄存器 */
// 将所有端口设置为数字端口
ADPCFG = 0xFFFF;
ADPCFGbits.PCFG0 = 0; // AN0 模拟
ADPCFGbits.PCFG1 = 0; // AN1 模拟
ADPCFGbits.PCFG2 = 0; // AN2 模拟ADPCFGbits.PCFG7 = 0; // AN7 模拟
/* ADCSSLADC输入扫描选择寄存器 */
ADCSSL = 0;
// 开启 A/D模块
ADCON1bits.ADON = 1;
#ifdefDIAGNOSTICS
// 对用于诊断模式的输出比较通道 7和 8进行初始化。
// PWM模式中使用比较
// Timer2用作时基
PR2 = 0x1FFF;
OC7CON = 0x0006;
OC8CON = 0x0006;
T2CONbits.TON = 1;
#endif
return False;
}
#ifdefDIAGNOSTICS
void DiagnosticsOutput(void)
{
int Data;
if(IFS0bits.T2IF)
{
IFS0bits.T2IF = 0;
Data = (ParkParm.qIq >> 4) + 0xfff;
if(Data > 0x1ff0) Data = 0x1ff0;
if(Data < 0x000f) Data = 0x000f;
OC7RS = Data;
Data = (EncoderParm.qVelMech) + 0x0fff;if(Data > 0x1ff0) Data = 0x1ff0;
if(Data < 0x000f) Data = 0x000f;
OC8RS = Data;
}
}
#endif
Encoder.c
// 编码器子程序的定标
#include "general.h"
#include "Parms.h"
#include "Encoder.h"
/**********************************************************
InitEncoderScaling
对编码器子程序的定标常量进行初始化。
函数参数
CntsPerRev来自正交编码器的每转编码器计数值
ScalingSpeedInRPS用于基本转速定标的每秒转数
IrpPerCalc每次转速计算的 CalcVelIrp中断数
VelIrpPeriodVelCalcIrp中断间的周期单位为秒
对于
CalcAng
运行时方程
qMechAng = qKang * (POSCNT*4) / 2^Nang
定标方程
qKang = (2^15)*(2^Nang)/CntsPerRev. 对于 CalcVelIrp、 CalcVel
运行时方程
qMechVel = qKvel * (2^15 * Delta / 2^Nvel)
定标方程
fVelCalcPeriod = fVelIrpPeriod * iIrpPerCalc
MaxCntRate = CntsPerRev * ScaleMechRPS
MaxDeltaCnt = fVelCalcPeriod * MaxCntRate
qKvel = (2^15)*(2^Nvel)/MaxDeltaCnt
**********************************************************/
bool InitEncoderScaling( void )
{
float fVelCalcPeriod, fMaxCntRate;
long MaxDeltaCnt;
long K;
EncoderParm.iCntsPerRev = MotorParm.iCntsPerRev;
K = 32768;
K *= 1 << Nang;
EncoderParm.qKang = K/EncoderParm.iCntsPerRev;
EncoderParm.iIrpPerCalc = MotorParm.iIrpPerCalc;
fVelCalcPeriod = MotorParm.fVelIrpPeriod * MotorParm.iIrpPerCalc;
fMaxCntRate = EncoderParm.iCntsPerRev * MotorParm.fScaleMechRPS;
MaxDeltaCnt = fVelCalcPeriod * fMaxCntRate;
// qKvel = (2^15)*(2^Nvel)/MaxDeltaCnt
K = 32768;
K *= 1 << Nvel;
K /= MaxDeltaCnt;if( K >= 32768 )
// 出错
return True;
EncoderParm.qKvel = K;
// 对 CalcVelIrp使用的局部变量进行初始化。
InitCalcVel();
return False;
}
InitCurModel.c
// 电流模型子程序定标
#include "general.h"
#include "Parms.h"
#include "CurModel.h"
/**********************************************************
InitCurModelScaling
对电流模型子程序中的定标常数进行初始化。
物理常数
fRotorTmConst 转子时间常数单位为秒。
方程的物理形式
励磁电流安培
Imag = Imag + (fLoopPeriod/fRotorTmConst)*(Id - Imag)
转差速度单位为 RPSVelSlipRPS = (1/fRotorTmConst) * Iq/Imag / (2*pi)
转子磁通速度单位为 RPS
VelFluxRPS = iPoles * VelMechRPS + VelSlipRPS
转子磁通角弧度
AngFlux = AngFlux + fLoopPeriod * 2 * pi * VelFluxRPS
定标后的变量
qImag 采用最大电流定标后的励磁电流。
qVelSlip采用 fScaleMechRPS定标后的机械转差速度单位为 RPS。
qAngFlux采用 pi定标后的磁通角。
定标后的方程
qImag = qImag + qKcur * (qId - qImag)
qVelSlip = Kslip * qIq/qImag
qAngFlux = qAngFlux + Kdelta * (qVelMech + qVelSlip)
定标因子
qKcur = (2^15) * (fLoopPeriod/fRotorTmConst)
qKdelta = (2^15) * 2 * iPoles * fLoopPeriod * fScaleMechRPS
qKslip = (2^15)/(2 * pi * fRotorTmConst * iPoles * fScaleMechRPS)
**********************************************************/
bool InitCurModelScaling( void )
{
CurModelParm.qKcur = 32768.0 * MotorParm.fLoopPeriod / MotorParm.fRotorTmConst;
CurModelParm.qKdelta = 32768.0 * 2 * MotorParm.iPoles * MotorParm.fLoopPeriod *
MotorParm.fScaleMechRPS;
CurModelParm.qKslip = 32768.0/(6.2832 * MotorParm.iPoles *MotorParm.fScaleMechRPS*MotorParm.fRotorTmConst);
// 允许的最大转差速度
CurModelParm.qMaxSlipVel = 32768.0/8;
//对 CurrModel使用的局部变量进行初始化。
InitCurModel();
return False;
}
MeasCurr.s
;*******************************************************************
; MeasCompCurr
;
; 说明
;读 ADC通道 1和通道 2使用 qKa和 qKb将其换算为有符号小数值
;并将结果存放到 ParkParm的 qIa和 qIb中。
; ADC-Ave的滚动平均值被保持并在换算前从 ADC值中减去。
;
;
;具体来说偏移量将作为 32位有符号整数进行累计。
; iOffset += (ADC-Offset)
;并采用以下公式通过偏移量来校正原始的 ADC读数
; CorrADC = ADCBUFn - iOffset/2^16
;将给出一个偏移时间常数 ~ MeasurementPeriod*2^16
;
;在转换结束之前不要调用该子程序。
;
;定标常数 qKa和 qKb必须在其他代码中设定使
; qIa = 2 * qKa * CorrADC1
; qIb = 2 * qKb * CorrADC2
;采用 2作为因子以允许 qKa和 qKb保持 1.15格式。
;
; 函数原型
; void MeasCompCurr( void );
; void InitMeasCompCurr( short iOffset_a, short iOffset_b );;
; 开始时必须调用 InitMeasCompCurr。
; 进入时 MeasCurrParm结构必须包含 qKa和 qKb。
; ADC通道 1和 2必须包括有符号小数值。
; 退出时 ParkParm将包含 qIa和 qIb。
;
;参数
;输入参数
;无
;返回值
; Void
;所需的 SFR设定
; CORCON.SATA = 0
;如果累加器可能溢出必须设定
; CORCON.SATDW = 1
;
;所需的支持子程序
;无
;局部堆栈使用
;无
;修改的寄存器
; w0,w1,w4,w5
;执行时间
; 29个周期
;*******************************************************************
global _MeasCompCurr
global MeasCompCurr
_MeasCompCurr:
MeasCompCurr:
;; CorrADC1 = ADCBUF1 - iOffsetHa/2^16
;; qIa = 2 * qKa * CorrADC1
mov.w _MeasCurrParm+ADC_iOffsetHa,w0
sub.w _ADCBUF1,WREG ; w0 = ADC - Offset
clr.w w1btsc w0,#15
setm w1
mov.w w0,w5
mov.w _MeasCurrParm+ADC_qKa,w4
mpy w4*w5,A
sac A,#-1,w4
mov.w w4,_ParkParm+Park_qIa
;; iOffset += (ADC-Offset)
add _MeasCurrParm+ADC_iOffsetLa
mov.w w1,w0
addc _MeasCurrParm+ADC_iOffsetHa
;; CorrADC2 = ADCBUF2 - iOffsetHb/2^16
;; qIb = 2 * qKb * CorrADC2
mov.w _MeasCurrParm+ADC_iOffsetHb,w0
sub.w _ADCBUF2,WREG ; w0 = ADC -Offset
clr.w w1
btsc w0,#15
setm w1
mov.w w0,w5
mov.w _MeasCurrParm+ADC_qKb,w4
mpy w4*w5,A
sac A,#-1,w4
mov.w w4,_ParkParm+Park_qIb
;; iOffset += (ADC-Offset)
add _MeasCurrParm+ADC_iOffsetLb
mov.w w1,w0
addc _MeasCurrParm+ADC_iOffsetHb
return
ClarkePark.s
;*******************************************************************
; ClarkePark
;
; 说明
;计算 Clarke和 Park变换。
;假定 Cos和 Sin值在 qSin和 qCos中。
;
; Ialpha = Ia
; Ibeta = Ia*dOneBySq3 + 2*Ib*dOneBySq3;
;其中 Ia+Ib+Ic = 0
;
; Id = Ialpha*cos(Angle) + Ibeta*sin(Angle)
; Iq = -Ialpha*sin(Angle) + Ibeta*cos(Angle)
;
;该子程序同样适用于整数定标和 1.15定标格式。
;
; 函数原型
;
; void ClarkePark( void )
;
;进入时 ParkParm结构必须包含 qSin、 qCos、 qIa和 qIb。
;退出时 ParkParm将包含 qId和 qIq。
;
; 参数
;输入参数
;返回值
; Void
;所需的 SFR设定
; CORCON.SATA = 0
;如果 (Ia+2*Ib)/sqrt(3)可能出现溢出必须设定
; CORCON.SATDW = 1
;
; 所需的支持子程序
;无
; 局部堆栈使用
;无
; 修改的寄存器
; w3 -> w7
; 执行时间; 20个周期
;*******************************************************************
;
include "general.inc"
; 外部引用
include "park.inc"
; 寄存器使用
.equ ParmW, w3 ; 指向 ParkParm结构的指针
.equ Sq3W, w4 ; OneBySq3
.equ SinW, w4 ;替代 Work0W
.equ CosW, w5
.equ IaW, w6 ; qIa的拷贝
.equ IalphaW, w6 ; 替代 Ia
.equ IbW, w7 ; qIb的拷贝
.equ IbetaW, w7 ; 用 Ibeta替代 Ib
; 常量
equ OneBySq3, 0x49E7 ; 1/sqrt(3)采用 1.15格式
;=================== 代码 =====================
section .text
global _ClarkePark
global ClarkePark
_ClarkePark:
ClarkePark:
;; Ibeta = Ia*OneBySq3 + 2*Ib*OneBySq3;
mov.w #OneBySq3,Sq3W ; 1/sqrt(3)采用 1.15格式
mov.w _ParkParm+Park_qIa,IaW
mpy Sq3W*IaW,A
mov.w _ParkParm+Park_qIb,IbW
mac Sq3W*IbW,A
mac Sq3W*IbW,A
mov.w _ParkParm+Park_qIa,IalphaW
mov.w IalphaW,_ParkParm+Park_qIalpha
sac A,IbetaW
mov.w IbetaW,_ParkParm+Park_qIbeta
;; 已经计算 Ialpha 和 Ibeta。现在进行旋转。
;; 得到 ParkParm结构的 qSin、 qCos 。
mov.w _ParkParm+Park_qSin,SinW
mov.w _ParkParm+Park_qCos,CosW
;; Id = Ialpha*cos(Angle) + Ibeta*sin(Angle)
mpy SinW*IbetaW,A ; Ibeta*qSin -> A
mac CosW*IalphaW,A ; 将 Ialpha*qCos加到 A
mov.w #_ParkParm+Park_qId,ParmW
sac A, ; 存放到 qId将指针递增 1指向 qIq
;; Iq = -Ialpha*sin(Angle) + Ibeta*cos(Angle)
mpy CosW*IbetaW,A ; Ibeta*qCos -> A
msc SinW*IalphaW,A ; 从 A减去 Ialpha*qSin
sac A, ; 存入 qIq
return
.end
CurModel.s
;*******************************************************************
;子程序 CurModel
;*******************************************************************
;为文件中所有子程序共有
.include "general.inc"
.include "curmodel.inc"
.include "park.inc"
;*******************************************************************
; CurModel
;
; 说明
;
; 物理常数
; fRotorTmConst转子时间常数单位为秒
;
;方程的物理形式
;励磁电流安培
; Imag = Imag + (fLoopPeriod/fRotorTmConst)*(Id - Imag)
;
;转差速度单位为 RPS
; VelSlipRPS = (1/fRotorTmConst) * Iq/Imag / (2*pi)
;
;转子磁通速度单位为 RPS
; VelFluxRPS = iPoles * VelMechRPS + VelSlipRPS
;
;转子磁通角弧度
; AngFlux = AngFlux + fLoopPeriod * 2 * pi * VelFluxRPS
;
; 定标后的变量
; qdImag采用最大电流定标的励磁电流 1.31
; qVelSlip采用 fScaleMechRPS定标后的机械转差速度单位为 RPS
; qAngFlux采用 pi定标后的磁通角
;
; 定标后的方程
; qdImag = qdImag + qKcur * (qId - qdImag)
; qVelSlip = qKslip * qIq/qdImag
; qAngFlux = qAngFlux + qKdelta * (qVelMech + qVelSlip)
;
; 定标因子
; qKcur = (2^15) * (fLoopPeriod/fRotorTmConst)
; qKdelta = (2^15) * 2 * iPoles * fLoopPeriod * fScaleMechRPS
; qKslip = (2^15)/(2 * pi * fRotorTmConst * iPoles * fScaleMechRPS)
;
; 函数原型
;
; void CurModel( void )
;
; 进入时 CurModelParm结构必须包含 qKcur、 qKslip、 iKpoles、
; qKdelta、 qVelMech和 qMaxSlipVel
; 退出时 CurModelParm将包含 qAngFlux、 qdImag和 qVelSlip
;
; 参数
;输入参数
;无
;返回值
; Void
;所需的 SFR设定
; CORCON.SATA = 0
; CORCON.IF = 0
;
;所需的支持子程序
;无
;局部堆栈使用
;0
;修改的寄存器
: w0-w7,AccA
;执行时间
; 72个指令周期
;*******************************************************************
;
;=================== 代码 =====================
.section .text
;用于 CurModel的寄存器
.equ SignW, w2 ; 跟踪符号变化
.equ ShiftW, w3 ; 执行除法之前的移位位数
.equ IqW, w4 ; Q电流 1.15
.equ KslipW, w5 ; Kslip常数 1.15
.equ ImagW, w7 ; 励磁电流 1.15
.global _CurModel
.global CurModel
_CurModel:
CurModel:
;; qdImag = qdImag + qKcur * (qId - qdImag) ;; 励磁电流
mov.w _CurModelParm+CurMod_qdImag,w6
mov.w _CurModelParm+CurMod_qdImag+2,w7
lac w7,A
mov.w w6,ACCALL
mov.w _ParkParm+Park_qId,w4
sub.w w4,w7,w4 ; qId-qdImagH
mov.w _CurModelParm+CurMod_qKcur,w5
mac w4*w5,A ; 将 Kcur*(Id-Imag)加到 Imag
sac A,w7
mov.w ACCALL,w6
mov.w w6,_CurModelParm+CurMod_qdImag
mov.w w7,_CurModelParm+CurMod_qdImag+2
;; qVelSlip = qKslip * qIq/qdImag
;; 首先将 qIqW 和 qdImagW置为正数并将符号位存放在 SignW中
clr SignW ; 将标志符号设定为正
;; if( IqW < 0 ) => 翻转 SignW并设定 IqW = -IqW
mov.w _ParkParm+Park_qIq,IqW
cp0 IqW
bra Z,jCurModSkip
bra NN,jCurMod1
neg IqW,IqW
com SignW,SignW ;翻转符号位
jCurMod1:;; if( ImagW < 0 ) => 翻转 SignW并设定 ImagW = -ImagW
cp0 ImagW
bra NN,jCurMod2
neg ImagW,ImagW
com SignW,SignW ;翻转符号位
jCurMod2:
;; 在 Acc A中计算 Kslip*|IqW|以保持 1.31格式
mov.w _CurModelParm+CurMod_qKslip,KslipW
mpy IqW*KslipW,A
;; 确保 denominator > numerator否则跳过项
sac A,w0 ; 暂时的
cp ImagW,w0 ; |qdImag| - |Kslip*qIq|
bra LEU,jCurModSkip ; 跳过项 |qdImag| <= |Kslip*qIq|
;; 在 6010 <SILICON_ERR>芯片本身的错误以后版本中将不再需要。
clr.w ShiftW
;; 计算不将最高有效位直接置 1保留符号位的情况下可将 ImagW移位多少位。
;;
ff1l ImagW,ShiftW
sub.w ShiftW,#2,ShiftW ; 为将 1放入 bit 14需要移位的位数
;; 移位ImagW = ImagW << ShiftW
sl ImagW,ShiftW,ImagW
;; 对 AccA进行移位需要将 (-ShiftW)左移。
neg ShiftW,ShiftW;; |Kslip*qIq| = |Kslip*qIq| << ShiftW
sftac A,ShiftW
;; 执行除法操作 |qKslip*qIq|/|ImagW|。此时结果将为正且 < 1.0同时具有最高的精度。
;;
;;
sac A,w6
repeat #17
divf w6,ImagW ; w0 = KslipW*IqW/ImagW, w1 = remainder
;; 限制最大转差速度
mov.w _CurModelParm+CurMod_qMaxSlipVel,w1
cp w1,w0 ; qMaxSlipSpeed - | Kslip*qIq/qdImag |
bra NN,jCurMod4
;; 结果太大 :用 qMaxSlipSpeed代替
mov.w w1,w0
bra jCurMod4
jCurModSkip:
;; 整个项被跳过 -将其置为 = 0
clr.w w0
jCurMod4:
;; 设定正确的符号
btsc SignW,#0
neg w0,w0
;; 用于测试
mov.w w0,_CurModelParm+CurMod_qVelSlip
;; 加入机械转速
mov.w _CurModelParm+CurMod_qVelMech,w4
add.w w0,w4,w4
mov.w w4,_CurModelParm+CurMod_qVelFlux
;; 将 AngFlux 载入 Acc A
mov.w _CurModelParm+CurMod_qAngFlux,w1
lac w1,A
mov.w _CurModelParm+CurMod_qKdelta,w5
mac w4*w5,A
sac A,w4
mov.w w4,_CurModelParm+CurMod_qAngFlux
return
InvPark.s
;*******************************************************************
; InvPark
;
;说明 :
;计算 Park反变换。假定 Cos和 Sin值位于 ParkParm结构中。
;
; Valpha = Vd*cos(Angle) - Vq*sin(Angle)
; Vbeta = Vd*sin(Angle) + Vq*cos(Angle)
;该子程序同样适用于整数定标和 1.15定标格式。
;
;函数原型
; void InvPark( void )
;进入时 ParkParm结构必须包含 qCos、 qSin、 qVd和 qVq。
;退出时 ParkParm将包含 qValpha和 qVbeta。
;
;参数
;输入参数无
;返回值 : Void
;所需的 SFR设定 CORCON.SATA = 0
;所需的支持子程序无
;局部堆栈使用 :无
;修改的寄存器 w3 -> w7, A
;执行时间大约 14个指令周期
;*******************************************************************
;
include "general.inc"
;外部引用
include "park.inc"
;寄存器使用
.equ ParmW, w3 ; 指向 ParkParm结构的指针
.equ SinW, w4
.equ CosW, w5
.equ VdW, w6 ; qVd的拷贝
.equ VqW, w7 ; qVq的拷贝
;=================== 代码 =====================
.section .text
.global _InvPark
.global InvPark
_InvPark:
InvPark:
;; 从 ParkParm结构获得 qVd和 qVq
mov.w _ParkParm+Park_qVd,VdW
mov.w _ParkParm+Park_qVq,VqW
;; 从 ParkParm结构获得 qSin和 qCos
mov.w _ParkParm+Park_qSin,SinW
mov.w _ParkParm+Park_qCos,CosW
;; Valpha = Vd*cos(Angle) - Vq*sin(Angle)
mpy CosW*VdW,A ; Vd*qCos -> A
msc SinW*VqW,A ;从 A减去 Vq*qSin
mov.w #_ParkParm+Park_qValpha,ParmW
sac A, ; 存储到 qValpha指针递增 1指向 qVbeta
;; Vbeta = Vd*sin(Angle) + Vq*cos(Angle)
mpy SinW*VdW,A ; Vd*qSin -> A
mac CosW*VqW,A ;将 Vq*qCos加到 A
sac A, ; 存储到 Vbeta
return
CalcRef.s
;*******************************************************************
; CalcRefVec
;
; 说明
;根据 qValpha和 qVbeta计算定标后的参考矢量 Vr1, Vr2, Vr3。
;该方法是一种改进后的 Clarke反变换。与常规 Clarke反变换相比将 Valpha和 Vbeta
;进行了交换。
;
; Vr1 = Vbeta
; Vr2 = (-Vbeta/2 + sqrt(3)/2 * Valpha)
; Vr3 = (-Vbeta/2 - sqrt(3/2) * Valpha)
;
; 函数原型
;
; void CalcRefVec( void )
;
; 进入时 ParkParm结构必须包含 qCos、 qSin、 qValpha和 qVbeta。
; 退出时 SVGenParm将包含 qVr1、 qVr2和 qVr3
;
; 参数
;输入参数
;无
; 返回值
; Void
; 所需的 SFR设定
; CORCON.SATA = 0
; 所需的支持子程序
;无
; 局部堆栈使用
;无
; 修改的寄存器
; w0, w4, w5, w6
; 执行时间
;大约 20个指令周期
;*******************************************************************
;
.include "general.inc"
; 外部引用
.include "park.inc"
.include "SVGen.inc"
; 寄存器使用
.equ WorkW, w0 ; 工作寄存器
.equ ValphaW, w4 ; qValpha定标后
.equ VbetaW, w5 ; qVbeta定标后
.equ ScaleW, w6 ; 定标
; 常量
.equ Sq3OV2,0x6ED9 ; sqrt(3)/2采用 1.15格式
;=================== 代码 =====================
.section .text
.global _CalcRefVec
.global CalcRefVec
_CalcRefVec:
CalcRefVec:
;; 从 ParkParm结构获得 qValpha和 qVbeta。
mov.w ParkParm+Park_qValpha,ValphaW
mov.w _ParkParm+Park_qVbeta,VbetaW
;; 使 Vr1 = Vbeta
mov.w VbetaW,_SVGenParm+SVGen_qVr1
;; 装载 Sq(3)/2
mov.w #Sq3OV2,ScaleW
;; AccA = -Vbeta/2
neg.w VbetaW,VbetaW
lac VbetaW,#1,A
;; Vr2 = -Vbeta/2 + sqrt(3)2 * Valpha)
mac ValphaW*ScaleW,A ; 将 Valpha*sqrt(3)/2加到 A
sac A,WorkW
mov.w WorkW,_SVGenParm+SVGen_qVr2
;; AccA = -Vbeta/2
lac VbetaW,#1,A
;; Vr3 = (-Vbeta/2 - sqrt(3)2 * Valpha)
msc ValphaW*ScaleW,A ; 从 A减去 Valpha*sqrt(3)/2。
sac A,WorkW
mov.w WorkW,_SVGenParm+SVGen_qVr3
return
.end
CalcVel.s
;*******************************************************************
; 子程序 InitCalcVel, CalcVel
;
;*******************************************************************
; 为文件中所有子程序共有
.include "general.inc"
.include "encoder.inc"
;*******************************************************************
; void InitCalcVel(void)
;对局部转速变量进行初始化。;在进入时 iIrpPerCalc必须被设定。
;*******************************************************************
; InitCalcVel的寄存器使用
.equ Work0W, w4 ; 工作寄存器
.equ PosW, w5 ; 当前位置 POSCNT
;*******************************************************************
.global _InitCalcVel
.global InitCalcVel
_InitCalcVel:
InitCalcVel:
;; 此后的 5条指令禁止中断。
DISI #5
;; 装载 iPrevCnt和 zero Delta
;; 编码器值。注意要获得精确的转速 qVelMech必须计算两次。
;;
mov.w POSCNT,PosW ; 当前编码器值
mov.w PosW,_EncoderParm+Encod_iPrevCnt
clr.w _EncoderParm+Encod_iAccumCnt
;; 装载 iVelCntDwn
mov.w _EncoderParm+Encod_iIrpPerCalc,WREG
mov.w WREG,_EncoderParm+Encod_iVelCntDwn
return
;*******************************************************************
; CalcVelIrp
;
; 以指定的间隔被定时器中断调用。
;
; 中断间隔 VelPeriod必须小于最大转速时转过 1/2转所需的最小时间。
;
; 该子程序将对 iIrpPerCalc中断的编码器计数变化进行累加
; 时间周期 = iIrpPerCalc * VelPeriod随后将累加值复制到
; iDeltaCnt以供 CalcVel子程序进行转速计算使用。
; 随后该累加值被置回零并开始一个新的累加操作。
;
;函数原型 void CalcVelIrp( void );
;
;进入时 EncoderParm必须包含 iPrevCnt、 iAccumCnt和 iVelCntDwn
;
;退出时 EncoderParm 将包含 iPrevCnt、 iAccumCnt和 iDeltaCnt
;(如果递减计数到达零。
;
;参数
;输入参数无
;
;返回
; Void
;
;所需的 SFR设定 :无
;
;所需的支持子程序无
;
;局部堆栈使用 3
;
;修改的寄存器无
;
;执行时间大约 29个指令周期如果产生了新的 iDeltaCnt。
;
;=====================================================
; 等效的 C代码
;{
; register short Pos, Delta;
;
; Pos = POSCNT;
;
; Delta = Pos - EncoderParm.iPrevCnt;
; EncoderParm.iPrevCnt = Pos;
;
; if( iDelta >= 0 )
;{
; // Delta > 0因为
; // 1) vel > 0或
; // 2) Vel < 0且编码器计数值出现翻转返回。
;
; if( Delta >= EncoderParm.iCntsPerRev/2 )
;{
; // Delta >= EncoderParm.iCntsPerRev/2 => 负转速出现翻转返回
;
; Delta -= EncoderParm.iCntsPerRev;
;}
;}
; else
;{
; // Delta < 0因为
; // 1) vel < 0或
; // 2) Vel > 0并出现翻转返回
;;
; if( Delta < -EncoderParm.iCntsPerRev/2 )
;{
; // Delta < -EncoderParm.iCntsPerRev/2 => 正转速出现翻转返回
;
; Delta += EncoderParm.iCntsPerRev;
;}
;}
;
; EncoderParm.iAccumCnt += Delta;
;
; EncoderParm.iVelCntDwn--;
; if(EncoderParm.iVelCntDwn)
; return;
;; iVelCntDwn = iIrpPerCalc;
; qVelMech = qKvel * iAccumCnt * 2^Nvel;
; EncoderParm.iAccumCnt = 0;
;}
;=================== 代码 =====================
; CalcVelIrp的寄存器使用
.equ PosW, w0 ; 当前位置 POSCNT
.equ WorkW, w4 ; 工作寄存器
.equ DeltaW, w6 ; NewCnt - PrevCnt
.global _CalcVelIrp
.global CalcVelIrp
_CalcVelIrp:
CalcVelIrp:
;; 保存寄存器内容
push w0
push w4
push w6
;; Pos = uTestPos;
.ifdef SIMU
mov.w _uTestPos,PosW ; 编码器值 ??
.else
mov.w POSCNT,PosW ; 编码器值
.endif
mov.w _EncoderParm+Encod_iPrevCnt,WorkW
;; 用新的 cnt更新前一个 cnt。
mov.w PosW,_EncoderParm+Encod_iPrevCnt
;; 计算 Delta = New - Prev
sub.w PosW,WorkW,DeltaW
bra N,jEncoder5 ; Delta < 0
;; Delta > 0因为
;; 1) vel > 0或
;; 2) Vel < 0并出现翻转返回
lsr.w _EncoderParm+Encod_iCntsPerRev,WREG ; WREG = CntsPerRev/2
;; Delta < CntsPerRev/2
sub.w DeltaW,w0,WorkW ; Delta-CntsPerRev/2
bra N,jEncoder20 ; 0 < Delta < CntsPerRev/2, Vel > 0
;; Delta >= CntsPerRev/2 => 负转速出现翻转返回
;; Delta = Delta - CntsPerRev
mov.w _EncoderParm+Encod_iCntsPerRev,w0
sub.w DeltaW,w0,DeltaW
;; Delta < 0, Vel < 0
bra jEncoder20
jEncoder5:
;; Delta < 0因为
;; 1) vel < 0或
;; 2) Vel > 0并出现翻转返回
lsr.w _EncoderParm+Encod_iCntsPerRev,WREG ; WREG = CntsPerRev/2
;; Is Delta + CntsPerRev/2 < 0
add.w DeltaW,w0,WorkW ; Delta+CntsPerRev/2
bra NN,jEncoder20 ; -CntsPerRev/2 <= Delta < 0, Vel > 0
;; Delta < -CntsPerRev/2 => 正转速出现翻转返回
;; Delta = Delta + CntsPerRev
mov.w _EncoderParm+Encod_iCntsPerRev,w0
add.w DeltaW,w0,DeltaW
;; Delta < -CntsPerRev/2, Vel > 0
jEncoder20:
;; Delta现在包含位置的有符号变化
;; EncoderParm.Delta += Delta;
mov.w DeltaW,w0
add.w _EncoderParm+Encod_iAccumCnt
;; EncoderParm.iVelCntDwn--;
;; if(EncoderParm.iVelCntDwn) return;
dec.w _EncoderParm+Encod_iVelCntDwn
cp0.w _EncoderParm+Encod_iVelCntDwn
bra NZ,jEncoder40
;; 重新装载 iVelCntDwn iVelCntDwn = iIrpPerCalc;
mov.w _EncoderParm+Encod_iIrpPerCalc,WREG
mov.w WREG,_EncoderParm+Encod_iVelCntDwn
;;将 iAccumCnt复制到 iDeltaCnt然后 iAccumCnt = 0
mov.w _EncoderParm+Encod_iAccumCnt,DeltaW
mov.w DeltaW,_EncoderParm+Encod_iDeltaCnt
clr.w _EncoderParm+Encod_iAccumCnt
jEncoder40:
;; 恢复寄存器内容
pop w6
pop w4
pop w0
return
;*******************************************************************
; CalcVel
;
; 根据中断程序 CalcVelIrp产生的上一个 iDeltaCnt值计算 qVelMech。
;
;
;函数原型 void CalcVel( void );
;
;进入时 EncoderParm必须包含 iDeltaCnt和 qKvel
;
;退出时 EncoderParm将包含 qVelMech
;
;参数
; 输入参数无
; 返回值
; Void
;
; 所需的 SFR设定无
;
; 所需的支持子程序无
;
; 局部堆栈使用无
;
; 修改的寄存器无
;
; 执行时间大约 8个指令周期
;
;*******************************************************************
.global _CalcVel
.global CalcVel
_CalcVel:
CalcVel:
;; qVelMech = qKvel * ( Delta / 2^Nvel / 2^15)
;; iDeltaCnt为一个整数但作为 Q15 iDeltaCnt=(iDeltaCnt/2^15)
mov.w _EncoderParm+Encod_iDeltaCnt,DeltaW
mov.w _EncoderParm+Encod_qKvel,WorkW
mpy WorkW*DeltaW,A ; dKvel * (Delta/2^15)
sac A,#(Nvel-15),WorkW ; 左移 15-Nvel位
;; qVelMech = qKvel * Q15( Delta / 2^Nvel )
mov.w WorkW,_EncoderParm+Encod_qVelMech
return
.end
ClarkePark.s
;*******************************************************************
; ClarkePark
;
;说明
; 计算 Clarke和 Park变换。
; 假定 Cos和 Sin值位于 qSin和 qCos中。
;
; Ialpha = Ia
; Ibeta = Ia*dOneBySq3 + 2*Ib*dOneBySq3;
;其中 Ia+Ib+Ic = 0
;
; Id = Ialpha*cos(Angle) + Ibeta*sin(Angle)
; Iq = -Ialpha*sin(Angle) + Ibeta*cos(Angle)
;
;该子程序同样适用于整数定标和 1.15定标格式。
;
;函数原型
;
; void ClarkePark( void )
;
;进入时 ParkParm结构必须包含 qSin、 qCos、 qIa和 qIb。
;
;退出时 ParkParm将包含 qId和 qIq
;
;参数
; 输入参数无
;
; 返回值
; Void
;
; 所需的 SFR设定
; CORCON.SATA = 0
; 如果 (Ia+2*Ib)/sqrt(3)可能溢出必须设定
; CORCON.SATDW = 1
;
; 所需的支持子程序无
;
; 局部堆栈使用无
;
; 修改的寄存器 w3 -> w7
;
; 执行时间 20个指令周期
;
;*******************************************************************
;
.include "general.inc"
; 外部引用
.include "park.inc"
; 寄存器使用
.equ ParmW, w3 ; 指向 ParkParm结构的指针
.equ Sq3W, w4 ; OneBySq3
.equ SinW, w4 ;代替 Work0W
.equ CosW, w5
.equ IaW, w6 ; qIa的拷贝
.equ IalphaW, w6 ; 代替 Ia
.equ IbW, w7 ; qIb的拷贝
.equ IbetaW, w7 ; 用 Ibeta代替 Ib
; 常量
.equ OneBySq3,0x49E7 ; 1/sqrt(3)采用 1.15格式
;=================== 代码 =====================
.section .text
.global _ClarkePark
.global ClarkePark
_ClarkePark:
ClarkePark:
;; Ibeta = Ia*OneBySq3 + 2*Ib*OneBySq3;
mov.w #OneBySq3,Sq3W ;1/sqrt(3)采用 1.15格式
mov.w _ParkParm+Park_qIa,IaW
mpy Sq3W*IaW,A
mov.w _ParkParm+Park_qIb,IbW
mac Sq3W*IbW,A
mac Sq3W*IbW,A
mov.w _ParkParm+Park_qIa,IalphaW
mov.w IalphaW,_ParkParm+Park_qIalpha
sac A,IbetaW
mov.w IbetaW,_ParkParm+Park_qIbeta
;; 已经计算好 Ialpha和 Ibeta。现进行旋转。
;; 从 ParkParm结构获得 qSin和 qCos。
mov.w _ParkParm+Park_qSin,SinW
mov.w _ParkParm+Park_qCos,CosW
;; Id = Ialpha*cos(Angle) + Ibeta*sin(Angle)
mpy SinW*IbetaW,A ; Ibeta*qSin -> A
mac CosW*IalphaW,A ; 将 Ialpha*qCos加到 A
mov.w #_ParkParm+Park_qId,ParmW
sac A, ; 存入 qId指针递增 1指向 qIq
;; Iq = -Ialpha*sin(Angle) + Ibeta*cos(Angle)
mpy CosW*IbetaW,A ; Ibeta*qCos -> A
msc SinW*IalphaW,A ; 从 A减去 Ialpha*qSin。
sac A, ; 存入 qIq
return
.end
CurModel.s
;*******************************************************************
; 子程序 CurModel;;*******************************************************************
; 为文件中所有子程序共用。
.include "general.inc"
.include "curmodel.inc"
.include "park.inc"
;*******************************************************************
; CurModel
;
;说明
;
;
;物理常量
; fRotorTmConst转子时间常数单位为秒。
;
;方程的物理形式
;励磁电流安培
; Imag = Imag + (fLoopPeriod/fRotorTmConst)*(Id - Imag)
;
;转差速度单位是 RPS
; VelSlipRPS = (1/fRotorTmConst) * Iq/Imag / (2*pi)
;
;转子磁通速度单位是 RPS
; VelFluxRPS = iPoles * VelMechRPS + VelSlipRPS
;
;转子磁通角弧度
; AngFlux = AngFlux + fLoopPeriod * 2 * pi * VelFluxRPS
;
;定标后的变量
; qdImag采用最大电流 1.31格式定标的励磁电流
; qVelSlip采用 fScaleMechRPS定标的机械转差速度单位为 RPS。
; qAngFlux采用 pi定标后的磁通角
;
;定标后的方程
; qdImag = qdImag + qKcur * (qId - qdImag)
; qVelSlip = qKslip * qIq/qdImag
; qAngFlux = qAngFlux + qKdelta * (qVelMech + qVelSlip)
;
;定标因子
; qKcur = (2^15) * (fLoopPeriod/fRotorTmConst)
; qKdelta = (2^15) * 2 * iPoles * fLoopPeriod * fScaleMechRPS; qKslip = (2^15)/(2 * pi * fRotorTmConst * iPoles * fScaleMechRPS)
;
;
;函数原型
;
; void CurModel( void )
;
;进入时 CurModelParm结构必须包含 qKcur、 qKslip、 iKpoles、
; qKdelta、 qVelMech和 qMaxSlipVel
;
;退出时 CurModelParm将包含 qAngFlux、 qdImag和 qVelSlip
;
;参数
; 输入参数无
;
; 返回值
; Void
;
; 所需的 SFR设定
; CORCON.SATA = 0
; CORCON.IF = 0
;
; 所需的支持子程序无
; 局部堆栈使用 0
; 修改的寄存器 w0-w7,AccA
; 执行时间 2个指令周期
;*******************************************************************
;
;=================== 代码 =====================
.section .text
; CurModel的寄存器使用
.equ SignW, w2 ; 跟踪符号变化
.equ ShiftW, w3 ; 在进行除法操作前的移位位数.equ IqW, w4 ; Q电流 1.15
.equ KslipW, w5 ; Kslip常数 1.15
.equ ImagW, w7 ; 励磁电流 1.15
.global _CurModel
.global CurModel
_CurModel:
CurModel:
;; qdImag = qdImag + qKcur * (qId - qdImag) ;; 励磁电流
mov.w _CurModelParm+CurMod_qdImag,w6
mov.w _CurModelParm+CurMod_qdImag+2,w7
lac w7,A
mov.w w6,ACCALL
mov.w _ParkParm+Park_qId,w4
sub.w w4,w7,w4 ; qId-qdImagH
mov.w _CurModelParm+CurMod_qKcur,w5
mac w4*w5,A ; 将 Kcur*(Id-Imag)加到 Imag
sac A,w7
mov.w ACCALL,w6
mov.w w6,_CurModelParm+CurMod_qdImag
mov.w w7,_CurModelParm+CurMod_qdImag+2
;; qVelSlip = qKslip * qIq/qdImag
;; 首先置 qIqW和 qdImagW为正并将符号位存放在 SignW中。
clr SignW ; 将标志符号设置为正
;; if( IqW < 0 ) => 翻转 SignW并设定 IqW = -IqW
mov.w _ParkParm+Park_qIq,IqW
cp0 IqW
bra Z,jCurModSkip
bra NN,jCurMod1
neg IqW,IqW
com SignW,SignW ; 翻转符号
jCurMod1:
;; if( ImagW < 0 ) => 翻转 SignW并设定 ImagW = -ImagW
cp0 ImagW
bra NN,jCurMod2
neg ImagW,ImagW
com SignW,SignW ;翻转符号
jCurMod2:
;; 用 Acc A计算 Kslip*|IqW|以保持为 1.31格式
mov.w _CurModelParm+CurMod_qKslip,KslipW
mpy IqW*KslipW,A
;; 确保分母 > 分子否则跳过项
sac A,w0 ; 暂存
cp ImagW,w0 ; |qdImag| - |Kslip*qIq|
bra LEU,jCurModSkip ; 跳过项 |qdImag| <= |Kslip*qIq|
;; 在 6010 <SILICON_ERR>以后版本中将不再需要这个操作。
clr.w ShiftW
;; 计算不让 “1”进入最高位的前提下 ImagW可移位的最多位数
;; 保护符号位
ff1l ImagW,ShiftW
sub.w ShiftW,#2,ShiftW ; 为将 1放入 bit 14需要移位的位数
;; 移位 ImagW = ImagW << ShiftW
sl ImagW,ShiftW,ImagW
;; 对 AccA进行移位需要左移 (-ShiftW)位对 shiftW取反。
neg ShiftW,ShiftW
;; |Kslip*qIq| = |Kslip*qIq| << ShiftW
sftac A,ShiftW
;; 进行除法操作 |qKslip*qIq|/|ImagW|。此时结果将为正且 < 1.0。
;; 此时具有最高精度。
;;
sac A,w6
repeat #17
divf w6,ImagW ; w0 = KslipW*IqW/ImagW, w1 = 余数
;;限制最大转差速度
mov.w _CurModelParm+CurMod_qMaxSlipVel,w1
cp w1,w0 ; qMaxSlipSpeed - | Kslip*qIq/qdImag |
bra NN,jCurMod4
;; 结果数值太大使用 qMaxSlipSpeed对其进行替换
mov.w w1,w0
bra jCurMod4
jCurModSkip:
;; 该项将被完全跳过 -将其设定为 0
clr.w w0
jCurMod4:
;; 设定正确的符号
btsc SignW,#0
neg w0,w0
;; 用于测试
mov.w w0,_CurModelParm+CurMod_qVelSlip
;; 加上机械转速
mov.w _CurModelParm+CurMod_qVelMech,w4
add.w w0,w4,w4
mov.w w4,_CurModelParm+CurMod_qVelFlux
;; 将 AngFlux载入 Acc A
mov.w _CurModelParm+CurMod_qAngFlux,w1
lac w1,A
mov.w _CurModelParm+CurMod_qKdelta,w5
mac w4*w5,A
; 在累加器进行存储操作时需要暂时关闭数据空间写饱和功能。
;
; 这是因为 qAngFlux代表磁通角且必须能够在超过 0x7FFF时
; 进行翻转返回。
;
bclr CORCON,#SATDW
sac A,w4
bset CORCON,#SATDW
mov.w w4,_CurModelParm+CurMod_qAngFlux
return
;*******************************************************************
; void InitCurModel(void)
;对 CurModel的局部变量进行初始化。
;*******************************************************************
; InitCurModel的寄存器使用
;*******************************************************************
.global _InitCurModel
.global InitCurModel
_InitCurModel:
InitCurModel:
clr.w _CurModelParm+CurMod_qAngFlux
clr.w _CurModelParm+CurMod_qdImag
clr.w _CurModelParm+CurMod_qdImag+2
return
.end
FdWeak.s
;*******************************************************************
; 子程序 FdWeak
;
;*******************************************************************
; 为文件中所有子程序共用
.include "general.inc"
.include "Control.inc"
.include "FdWeak.inc"
;*******************************************************************
; FdWeak
;
;说明
;
;方程
;
;定标因子
;函数原型
;
; void FdWeak( void )
;
;进入时 FdWeakParm结构必须包含 _FdWeakParm+FdWeak_qK1
;
;退出时 FdWeakParm 将包含 _CtrlParm+Ctrl_qVdRef
;
;参数; 输入参数无
;
; 返回值
; Void
;
; 所需的 SFR设定
; CORCON.SATA = 0
; CORCON.IF = 0
;
; 所需的支持子程序无
; 局部堆栈使用 0
; 修改的寄存器 ??w4,w5,AccA
; 执行时间 ??8个指令周期
;
;*******************************************************************
;
;=================== 代码 =====================
.section .text
;FdWeak的寄存器使用
.global _FdWeakening
.global FdWeakening
_FdWeakening:
FdWeakening:
mov.w _FdWeakParm+FdWeak_qK1,w0
mov.w w0,_CtrlParm+Ctrl_qVdRef
return
.end
InvPark.s
;*******************************************************************
; InvPark
;
;说明
; 计算 Park反变换。假定 Cos和 Sin值都位于 ParkParm结构中。
;
;
; Valpha = Vd*cos(Angle) - Vq*sin(Angle)
; Vbeta = Vd*sin(Angle) + Vq*cos(Angle)
;
; 该子程序对于整数定标和 1.15定标格式都适用。
;
;函数原型
;
; void InvPark( void )
;
;进入时 ParkParm结构必须包含 qCos、 qSin、 qVd和 qVq。
;
;退出时 ParkParm将包含 qValpha和 qVbeta
;
;参数
; 输入参数无
;
; 返回值
; Void
;
; 所需的 SFR设定
; CORCON.SATA = 0
; 所需的支持子程序无
;
; 局部堆栈使用无
;
; 修改的寄存器 w3 -> w7, A
;
; 执行时间大约 14个指令周期
;
;*******************************************************************;
.include "general.inc"
; 外部引用
.include "park.inc"
; 寄存器使用
.equ ParmW, w3 ; 指向 ParkParm结构的指针
.equ SinW, w4
.equ CosW, w5
.equ VdW, w6 ; qVd的拷贝
.equ VqW, w7 ; qVq的拷贝
;=================== 代码 =====================
.section .text
.global _InvPark
.global InvPark
_InvPark:
InvPark:
;; 从 ParkParm结构获得 qVd和 qVq。
mov.w _ParkParm+Park_qVd,VdW
mov.w _ParkParm+Park_qVq,VqW
;; 从 ParkParm结构获得 qSin和 qCos。
mov.w _ParkParm+Park_qSin,SinW
mov.w _ParkParm+Park_qCos,CosW
;; Valpha = Vd*cos(Angle) - Vq*sin(Angle)
mpy CosW*VdW,A ; Vd*qCos -> A
msc SinW*VqW,A ; 从 A减去 Vq*qSin
mov.w #_ParkParm+Park_qValpha,ParmW
sac A, ; 存入 qValpha指针递增 1指向 qVbeta
;; Vbeta = Vd*sin(Angle) + Vq*cos(Angle)
mpy SinW*VdW,A ; Vd*qSin -> A
mac CosW*VqW,A ; 将 Vq*qCos加到 A
sac A, ; 存入 Vbeta
return
.end
MeasCur.s
;*******************************************************************
; MeasCompCurr
;
;说明
; 读 ADC通道 1和 2使用 qKa和 qKb将其换算为有符号小数值。
; 并将结果存放入 ParkParm结构的 qIa和 qIb中。
; 保留 ADC-Ave的滚动平均值并在标定前将其从 ADC值中减去。
;
;
; 具体地偏移量作为 32位有符号整数进行累加
; iOffset += (ADC- Offset)
; 并用来对原始 ADC数据进行校正
; CorrADC = ADCBUFn - iOffset/2^16
; 这样将给出一个偏移量时间常数 ~ MeasurementPeriod*2^16
;
; 在 A/D转换结束前不要调用该子程序。
;
; 定标常数 qKa和 qKb必须在其他代码中设定使
; qIa = 2 * qKa * CorrADC1
; qIb = 2 * qKb * CorrADC2
; 设计为采用因子 2以允许 qKa和 qKb表示为 1.15格式。
;
;
;函数原型
; void MeasCompCurr( void );
; void InitMeasCompCurr( short iOffset_a, short iOffset_b );
;
;开始时 必须调用 InitMeasCompCurr。
;
;进入时 MeasCurrParm结构必须包含 qKa和 qKb。 ADC通道 1和 2
; 必须包含有符号小数值。
;
;退出时 ParkParm将包含 qIa和 qIb。
;
;参数
; 输入参数无
;
; 返回值
; Void
;
; 所需的 SFR设定
; CORCON.SATA = 0
; 如果累加器可能溢出必须设定
; CORCON.SATDW = 1
;
; 所需的支持子程序无
;
; 局部堆栈使用无
;
; 修改的寄存器 w0,w1,w4,w5
;
; 执行时间 29个指令周期
;
;*******************************************************************
.include "general.inc"
; 外部引用
.include "MeasCurr.inc"
.include "Park.inc"
; 寄存器使用
.equ Work0W, w4
.equ Work1W, w5.equ OffsetHW, w3
.global _MeasCompCurr
.global MeasCompCurr
_MeasCompCurr:
MeasCompCurr:
;; CorrADC1 = ADCBUF1 - iOffsetHa/2^16
;; qIa = 2 * qKa * CorrADC1
mov.w _MeasCurrParm+ADC_iOffsetHa,w0
sub.w _ADCBUF1,WREG ; w0 = ADC - Offset
clr.w w1
btsc w0,#15
setm w1
mov.w w0,w5
mov.w _MeasCurrParm+ADC_qKa,w4
mpy w4*w5,A
sac A,#-1,w4
mov.w w4,_ParkParm+Park_qIa
;; iOffset += (ADC- Offset)
add _MeasCurrParm+ADC_iOffsetLa
mov.w w1,w0
addc _MeasCurrParm+ADC_iOffsetHa
;; CorrADC2 = ADCBUF2 - iOffsetHb/2^16
;; qIb = 2 * qKb * CorrADC2
mov.w _MeasCurrParm+ADC_iOffsetHb,w0
sub.w _ADCBUF2,WREG ; w0 = ADC - Offset
clr.w w1
btsc w0,#15
setm w1
mov.w w0,w5
mov.w _MeasCurrParm+ADC_qKb,w4
mpy w4*w5,A
sac A,#-1,w4
mov.w w4,_ParkParm+Park_qIb
;; iOffset += (ADC- Offset)
add _MeasCurrParm+ADC_iOffsetLb
mov.w w1,w0
addc _MeasCurrParm+ADC_iOffsetHb
return
.global _InitMeasCompCurr
.global InitMeasCompCurr
_InitMeasCompCurr:
InitMeasCompCurr:
clr.w _MeasCurrParm+ADC_iOffsetLa
mov.w w0,_MeasCurrParm+ADC_iOffsetHa
clr.w _MeasCurrParm+ADC_iOffsetLb
mov.w w1,_MeasCurrParm+ADC_iOffsetHbreturn
.end
OpenLoop.s
;*******************************************************************
;子程序 : OpenLoop
;*******************************************************************
; 为文件中所有子程序所共用
.include "general.inc"
.include "openloop.inc"
;*******************************************************************
; OpenLoop
;
;说明
;方程
; qDeltaFlux = Kdelta * qVelMech
; qAngFlux = qAngFlux + Kdelta * qVelMech ;; 转子磁通角
;
; qKdelta = (2^15) * 2 * iPoles * fLoopPeriod * fScaleMechRPS
;其中 qVelMech为经过 fScaleMechRPS定标后的机械速度单位为 RPS
;需要 iPoles来从机械转速获得磁通转速
;2用来将 +/- 2*pi换算为 +/- pi
; 函数原型
;
; void OpenLoop( void )
;
; 进入时 OpenLoopParm结构必须包含
;
; 退出时 OpenLoopParm将包含
;
; 参数; 输入参数无
;
; 返回值
; Void
;
; 所需的 SFR设定
; CORCON.SATA = 0
; CORCON.IF = 0
;
; 所需的支持子程序无
; 局部堆栈使用 0
; 修改的寄存器 ??w4,w5,AccA
; 执行时间 ??8个指令周期
;*******************************************************************
;
;=================== 代码 =====================
.section .text
; OpenLoop的寄存器使用
.equ Work0W, w4 ; 工作寄存器
.equ Work1W, w5 ; 工作寄存器
.global _OpenLoop
.global OpenLoop
_OpenLoop:
OpenLoop:
mov.w _OpenLoopParm+OpLoop_qVelMech,Work0W
mov.w _OpenLoopParm+OpLoop_qKdelta,Work1W
mpy Work0W*Work1W,A
sac A,Work0W
mov.w Work0W,_OpenLoopParm+OpLoop_qDeltaFlux
;; qAngFlux = qAngFlux + qDeltaFlux
mov.w OpenLoopParm+OpLoop_qAngFlux,Work1W
add.w Work0W,Work1W,Work0W
mov.w Work0W,_OpenLoopParm+OpLoop_qAngFluxreturn
;*******************************************************************
; void InitOpenLoop(void)
; 对 OpenLoop中的局部变量进行初始化。
;*******************************************************************
; InitOpenLoop的寄存器使用
;*******************************************************************
.global _InitOpenLoop
.global InitOpenLoop
_InitOpenLoop:
InitOpenLoop:
clr.w _OpenLoopParm+OpLoop_qAngFlux
clr.w _OpenLoopParm+OpLoop_qDeltaFlux
return
.end
PI.s
;*******************************************************************
; PI
;
;说明计算 PI校正。
;
;void CalcPI( tPIParm *pParm)
;{
; Err = InRef - InMeas
; U = Sum + Kp * Err; if( U > Outmax )
; Out = Outmax
; else if( U < Outmin )
; Out = Outmin
; else
; Out = U
; Exc = U - Out
; Sum = Sum + Ki * Err - Kc * Exc
;}
;
;void InitPI( tPIParm *pParm)
;{
; Sum = 0
; Out = 0
;}
;
;---------------------------;
PI常数的表示
; 用 2的幂调整常数 Kp将其定标为 1.15格式表示。
; 当计算完成时去除这种调整。
;
;
; Kp定标为 Kp = qKp * 2^NKo
;
; Ki和 Kc定标为 Ki = qKi, Kc = qKc
;
;
;函数原型
;
; void InitPI( tPIParm *pParm)
; void CalcPI( tPIParm *pParm)
;
;进入时 PIParm结构必须包含 qKp、 qKi、 qKc、 qOutMax、 qOutMin、
; InRef和 InMeas
;退出时 PIParm将包含 qOut
;
;参数
; 输入参数 tPIParm *pParm
;
; 返回值
; Void
;
; 所需的 SFR设定
; CORCON.SATA= 0; CORCON.IF = 0
;
; 所需的支持子程序无
; 局部堆栈使用 0
; 修改的寄存器 w0-w6 AccA
;
; 执行时间
; 最大 31个指令周期最小 28个指令周期
;*******************************************************************
;
.include "general.inc"
; 外部引用
.include "PI.inc"
; 寄存器使用
.equ BaseW0, w0 ; parm结构的基址
.equ OutW1, w1 ; 输出
.equ SumLW2, w2 ; 积分和
.equ SumHW3, w3 ; 积分和
.equ ErrW4, w4 ; 误差项 InRef-InMeas
.equ WorkW5, w5 ; 工作寄存器
.equ Unlimit W6,w6 ; U:无限制输出
.equ WorkW7, w7 ; 工作寄存器
;=================== 代码 =====================
.section .text
.global _InitPI
.global InitPI
_InitPI:InitPI:
mov.w w1,
return
.global _CalcPI
.global CalcPI
_CalcPI:
CalcPI:
;; Err = InRef - InMeas
mov.w ,WorkW7
mov.w ,WorkW5
sub.w WorkW7,WorkW5,ErrW4
;; U = Sum + Kp * Err * 2^NKo
lac [++BaseW0],B ; AccB = Sum
mov.w [--BaseW0],WorkW5
mov.w WorkW5,ACCBLL
mov.w ,WorkW5
mpy ErrW4*WorkW5,A
sftac A,#-NKo ; AccA = Kp*Err*2^NKo
add A ; Sum = Sum + Kp*Err*2^NKo
sac A,UnlimitW6 ; 在测试前存储 U
;; if( U > Outmax )
;; Out = Outmax
;; else if( U < Outmin )
;; Out = Outmin
;; else
;; Out = U
mov.w ,OutW1
cp UnlimitW6,OutW1
bra GT,jPI5 ; U > Outmax; OutW1 = Outmax
mov.w ,OutW1
cp UnlimitW6,OutW1
bra LE,jPI5 ; U < Outmin OutW1 = Outmin
mov.w UnlimitW6,OutW1 ; OutW1 = U
jPI5:
mov.w OutW1,
;; Ki * Err
mov.w ,WorkW5
mpy ErrW4*WorkW5,A
;; Exc = U - Out
sub.w UnlimitW6,OutW1,UnlimitW6
;; Ki * Err - Kc * Exc
mov.w ,WorkW5
msc WorkW5*UnlimitW6,A
;; Sum = Sum + Ki * Err - Kc * Exc
add A
sac A,[++BaseW0] ; 存储 Sum
mov.w ACCALL,WorkW5
mov.w WorkW5,[--BaseW0]
return
.end
ReadADC0.s
;*******************************************************************
; ReadADC0和 ReadSignedADC0
;
;说明
; 读 ADC的通道 0使用 qK对其进行换算并将结果存放到 qADValue。
; 在 A/D转换结束之前不要调用该子程序。
;
; ReadADC0范围是 qK*(0.0 ->0.9999)。
; ReadSignedADC0范围是 qK*(-1.0 ->0.9999)。
;
; 换算常数 qK必须在其他代码中设定使得
; iResult = 2 * qK * ADCBUF0
; 设定因子为 2以允许 qK采用 1.15格式表示。
;
;
;函数原型
;
; void ReadADC0( tReadADCParm* pParm ) :计算无符号值 0 -> 2*qK
; void ReadSignedADC0( tReadADCParm* pParm ) :计算有符号值 -2*qK -> 2*qK
;
;进入时 ReadADCParm结构必须包含 qK。 ADC通道 0必须包含有符号小数值。
;
;退出时 ReadADCParm将包含 qADValue
;
;参数
; 输入参数无
;
; 返回值
; Void
;
; 所需的 SFR设定
; CORCON.SATA = 0
;如果累加器有可能溢出必须设定
; CORCON.SATDW = 1
;
; 所需的支持子程序无
; 局部堆栈使用无
; 修改的寄存器 w0,w4,w5
; 执行时间 13个周期
;
;*******************************************************************
;.include "general.inc"
; 外部引用
.include "ReadADC.inc"
; 寄存器使用
.equ ParmBaseW,w0 ; parm结构基址
.equ Work0W, w4
.equ Work1W, w5
;=================== 代码 =====================
.section .text
.global _ReadADC0
.global ReadADC0
_ReadADC0:
ReadADC0:
;; iResult = 2 * qK * ADCBUF0
mov.w ,Work0W
mov.w _ADCBUF0,Work1W
;; 从有符号小数变为无符号小数即将
;; -1->.9999转换为 0 -> 0.9999
btg Work1W,#15
lsr.w Work1W,Work1W
mpy Work0W*Work1W,A
sac A,#-1,Work0W
mov.w Work0W,
return
.global _ReadSignedADC0
.global ReadSignedADC0
_ReadSignedADC0:
ReadSignedADC0:
;; iResult = 2 * qK * ADCBUF0
mov.w ,Work0W
mov.w _ADCBUF0,Work1W
mpy Work0W*Work1W,A
sac A,#-1,Work0W
mov.w Work0W,
return
.end
SVGen.s
;*******************************************************************
; SVGen
;
; 说明计算和装载 SVGen PWM值。
;
; 函数原型
; void CalcSVGen( void )
;
; 进入时 SVGenParm结构必须包含 qVr1、 qVr2和 qVr3
; 退出时 PWM寄存器已装载
;
; 参数
;输入参数
;无;返回值
; Void
;所需的 SFR设定
; CORCON.SATA = 0
; CORCON.IF = 0
;所需的支持子程序
;无
; 局部堆栈使用
; 0
; 修改的寄存器
; w0, w2, w3, w4, w5, w6, AccA
; 执行时间
; 34个指令周期
;*******************************************************************
; C版本代码
; void CalcRefVec( void )
; {
; if( Vr1 >= 0 )
;{
; // (xx1)
; if( Vr2 >= 0 )
;{
; // (x11)
; // 由于不允许采用 Sector 7因此一定是 Sector 3
; // Sector 3: (0,1,1) 0-60电角度
; T1 = Vr2
; T2 = Vr1
; CalcTimes();
; dPWM1 = Ta
; dPWM2 = Tb
; dPWM3 = Tc
;}
; else
;{
; // (x01)
; if( Vr3 >= 0 )
;{
; // Sector 5: (1,0,1) 120-180电角度
; T1 = Vr1
; T2 = Vr3
; CalcTimes();
; dPWM1 = Tc
; dPWM2 = Ta; dPWM3 = Tb
;}
; else
; {
; // Sector 1: (0,0,1) 60-120电角度
; T1 = -Vr2;
; T2 = -Vr3;
; CalcTimes();
; dPWM1 = Tb
; dPWM2 = Ta
; dPWM3 = Tc
; }
; }
; }
; else
; {
; // (xx0)
; if( Vr2 >= 0 )
;{
; // (x10)
; if( Vr3 >= 0 )
;{
; // Sector 6: (1,1,0) 240-300电角度
; T1 = Vr3
; T2 = Vr2
; CalcTimes();
; dPWM1 = Tb
; dPWM2 = Tc
; dPWM3 = Ta
;}
; else
;{
; // Sector 2: (0,1,0) 300-0电角度
; T1 = -Vr3
; T2 = -Vr1
; CalcTimes();
; dPWM1 = Ta
; dPWM2 = Tc
; dPWM3 = Tb;}
;}
; else
;{
; // (x00)
; // 由于不允许采用 Sector 0因此一定是 Sector 4。
; // Sector 4: (1,0,0) 180-240电角度
; T1 = -Vr1
; T2 = -Vr2
; CalcTimes();
; dPWM1 = Tc
; dPWM2 = Tb
; dPWM3 = Ta
;
;}
;}
; }
;
; void CalcTimes(void)
; {
; T1 = PWM*T1
; T2 = PWM*T2
; Tc = (PWM-T1-T2)/2
; Tb = Ta + T1
; Ta = Tb + T2
; }
;*******************************************************************
;
.include "general.inc"
; 外部引用
.include "Park.inc"
.include "SVGen.inc"
.include "CurModel.inc"
; 寄存器使用
.equ WorkW, w1 ; 工作寄存器
.equ T1W, w2
.equ T2W, w3
.equ WorkDLoW, w4 ; 双字 (乘法结果 )
.equ Vr1W, w4
.equ TaW, w4
.equ WorkDHiW, w5 ; 双字 (乘法结果 )
.equ Vr2W, w5
.equ TbW, w5
.equ Vr3W, w6
.equ TcW, w6
.equ dPWM1, PDC1
.equ dPWM2, PDC2
.equ dPWM3, PDC3
;=================== 代码 =====================
.section .text
.global _CalcSVGen
.global CalcSVGen
_CalcSVGen:
CalcSVGen:
;; 获得 qVr1、 qVr2和 qVr3
mov.w _SVGenParm+SVGen_qVr1,Vr1W
mov.w _SVGenParm+SVGen_qVr2,Vr2W
mov.w _SVGenParm+SVGen_qVr3,Vr3W
;; 测试 Vr1
cp0 Vr1W
bra LT,jCalcRef20 ; Vr1W < 0
;; 测试 Vr2
cp0 Vr2W
bra LT,jCalcRef10 ; Vr2W < 0
;; 由于不允许采用 Sector 7因此一定是 Sector 3。
;; Sector 3: (0,1,1) 0-60电角度
;; T1 = Vr2
;; T2 = Vr1
mov.w Vr2W,T2W
mov.w Vr1W,T1W
rcall CalcTimes
;; dPWM1 = Ta
;; dPWM2 = Tb
;; dPWM3 = Tc
mov.w TaW,dPWM1
mov.w TbW,dPWM2
mov.w TcW,dPWM3
return
jCalcRef10:
;; 测试 Vr3
cp0 Vr3W
bra LT,jCalcRef15 ; Vr3W < 0
;; Sector 5: (1,0,1) 120-180电角度
;; T1 = Vr1
;; T2 = Vr3
mov.w Vr1W,T2W
mov.w Vr3W,T1W
rcall CalcTimes
;; dPWM1 = Tc
;; dPWM2 = Ta
;; dPWM3 = Tb
mov.w TcW,dPWM1
mov.w TaW,dPWM2
mov.w TbW,dPWM3
returnjCalcRef15:
;; Sector 1: (0,0,1) 60-120电角度
;; T1 = -Vr2
;; T2 = -Vr3
neg.w Vr2W,T2W
neg.w Vr3W,T1W
rcall CalcTimes
;; dPWM1 = Tb
;; dPWM2 = Ta
;; dPWM3 = Tc
mov.w TbW,dPWM1
mov.w TaW,dPWM2
mov.w TcW,dPWM3
return
jCalcRef20:
;; 测试 Vr2
cp0 Vr2W
bra LT,jCalcRef30 ; Vr2W < 0
;; Test Vr3
cp0 Vr3W
bra LT,jCalcRef25 ; Vr3W < 0
;; Sector 6: (1,1,0) 240-300电角度
;; T1 = Vr3
;; T2 = Vr2
mov.w Vr3W,T2W
mov.w Vr2W,T1W
rcall CalcTimes
;; dPWM1 = Tb
;; dPWM2 = Tc
;; dPWM3 = Ta
mov.w TbW,dPWM1
mov.w TcW,dPWM2
mov.w TaW,dPWM3
return
jCalcRef25:
;; Sector 2: (0,1,0) 300-360电角度
;; T1 = -Vr3
;; T2 = -Vr1
neg.w Vr3W,T2W
neg.w Vr1W,T1W
rcall CalcTimes
;; dPWM1 = Ta
;; dPWM2 = Tc
;; dPWM3 = Tbmov.w TaW,dPWM1
mov.w TcW,dPWM2
mov.w TbW,dPWM3
return
jCalcRef30:
;; 由于不允许是 Sector 0因此一定是 Sector 4
;; Sector 4: (1,0,0) 180-240电角度
;; T1 = -Vr1
;; T2 = -Vr2
neg.w Vr1W,T2W
neg.w Vr2W,T1W
rcall CalcTimes
;; dPWM1 = Tc
;; dPWM2 = Tb
;; dPWM3 = Ta
mov.w TcW,dPWM1
mov.w TbW,dPWM2
mov.w TaW,dPWM3
return
;*******************************************************************
; CalcTimes
;
; void CalcTimes(void)
; {
; T1 = PWM*T1
; T2 = PWM*T2
; Tc = (PWM-T1-T2)/2
; Tb = Ta + T1
; Ta = Tb + T2
; }
;
; 执行时间 17个指令周期;*******************************************************************
CalcTimes:
;; T1 = PWM*T1
;; 由于 T1为 1.15格式而 PWM采用整数格式我们按照整数格式进行乘法
;; 2*PWM*T1并使用结果的高位字
;; 装载 PWMPeriod
sl.w _SVGenParm+SVGen_iPWMPeriod,WREG ; 进行乘法 PWM * 2以允许
电压满量程
mul.us w0,T1W,WorkDLoW
mov.w WorkDHiW,T1W
;; T2 = PWM*T2
mul.us w0,T2W,WorkDLoW
mov.w WorkDHiW,T2W
;; Tc = (PWM-T1-T2)/2
;mov.w _SVGenParm+SVGen_iPWMPeriod,WorkW
mov.w _SVGenParm+SVGen_iPWMPeriod,WREG
sub.w w0,T1W,WorkW ;PWM-T1
sub.w WorkW,T2W,WorkW ; -T2
asr.w WorkW,WorkW ; /2
mov.w WorkW,TcW ; 存储 Tc
;; Tb = Tc + T1
add.w WorkW,T1W,WorkW
mov.w WorkW,TbW
;; Ta = Tb + T2
add.w WorkW,T2W,WorkW
mov.w WorkW,TaW
return
Trig.s
;*******************************************************************
; Trig;
; 说明
; 使用基于 128字查找表的线性插值法计算指定角度的正弦和余弦值。
;
;
; 该子程序同样适用于整数定标和 1.15定标格式。
;
; 对于整数定标格式定标后 0 <= 角度 < 2*pi
; 对应于 0 <= 角度 < 0xFFFF。最终的正弦和余弦返回值
; 定标为 -32768 -> 32767即 (0x8000 -> 0x7FFF)。
;
; 对于 1.15定标格式定标后 -pi <= 角度 < pi
;对应于 -1 -> 0.9999即 (0x8000 <= 角度 < 0x7FFF)。最终的
;正弦和余弦返回值定标为 -1 -> 0.9999
; 即 (0x8000 -> 0x7FFF)。
;
; 函数原型
; void SinCos( void )
;
;进入时 ParkParm结构必须包含 qAngle
;退出时 ParkParm将包含 qSin和 qCos。 qAngle不变。
;
; 参数
; 输入参数
;无
;返回值
; Void
;所需的 SFR设定
; CORCON.IF = 0
;所需的支持子程序
;无
;局部堆栈使用
;0
;修改的寄存器
; w0-w7
;执行时间
;大约 28个指令周期
;*******************************************************************
;
.include "general.inc"
;外部引用
.include "park.inc"
;常量.equ TableSize,128
;局部寄存器使用
.equ Work0W, w0 ; 工作寄存器
.equ Work1W, w1 ; 工作寄存器
.equ RemainderW, w2 ; 插值的小数 0->0xFFFF
.equ IndexW, w3 ; 表的索引
.equ pTabPtrW, w4 ; 指向表的指针
.equ pTabBaseW, w5 ; 指向表基址的指针
.equ Y0W, w6 ; Y0 = SinTable
.equ ParkParmW, w7 ; ParkParm结构的基址
;; 注RemainderW和 Work0W必须是偶数寄存器
;=================== 局部数据 =====================
.section .ndata, "d"
SinTable
.word 0,1608,3212,4808,6393,7962,9512,11039
.word 12540,14010,15446,16846,18205,19520,20787,22005
.word 23170,24279,25330,26319,27245,28106,28898,29621
.word 30273,30852,31357,31785,32138,32413,32610,32728
.word 32767,32728,32610,32413,32138,31785,31357,30852
.word 30273,29621,28898,28106,27245,26319,25330,24279
.word 23170,22005,20787,19520,18205,16846,15446,14010
.word 12540,11039,9512,7962,6393,4808,3212,1608
.word 0,-1608,-3212,-4808,-6393,-7962,-9512,-11039
.word -12540,-14010,-15446,-16846,-18205,-19520,-20787,-22005
.word -23170,-24279,-25330,-26319,-27245,-28106,-28898,-29621
.word -30273,-30852,-31357,-31785,-32138,-32413,-32610,-32728
.word -32767,-32728,-32610,-32413,-32138,-31785,-31357,-30852
.word -30273,-29621,-28898,-28106,-27245,-26319,-25330,-24279
.word -23170,-22005,-20787,-19520,-18205,-16846,-15446,-14010
.word -12540,-11039,-9512,-7962,-6393,-4808,-3212,-1608
;=================== 代码 =====================
.section .text
.global _SinCos
.global SinCos
_SinCos:
SinCos:
;; ParkParm结构中 qAngle、 qSin和 qCos组的基址mov.w #_ParkParm+#Park_qAngle,ParkParmW
;;计算用于 Sin的预取和插值的索引和余数
mov.w #TableSize,Work0W
mov.w ,Work1W ; 装载 qAngle并将指针递增 1指向 qCos
mul.uu Work0W,Work1W,RemainderW ; IndexW中的高位字
;; 由于偏移量以字节为单位而不是以字为单位因此索引值加倍
add.w IndexW,IndexW,IndexW
;; 注意此时 IndexW寄存器具有值 0x00nn其中 nn
;; 为对 TabBase(表基址 )的偏移量以字节为单位。如果下面我们总是对 IndexW寄存器
使用字
节操作
;; 对于 128字大小的查找表程序将自动实现正确的返回翻转。
;;
mov.w #SinTable,pTabBaseW ; 指向表基址的指针
;; 检查余数是否为零
cp0.w RemainderW
bra nz,jInterpolate
;; 余数为零允许我们跳过插值操作直接使用表中的值
;;
add.w IndexW,pTabBaseW,pTabPtrW
mov.w , ; 写 qSin并使指针递增 1指向 qCos
;; 在 Sin索引上加上 0x40得到 Cos索引。这样可能超出表的末尾。
;; 但是如果我们仅使用字节操作则将会实现自动的翻转返回。
add.b #0x40,IndexW
add.w IndexW,pTabBaseW,pTabPtrW
mov.w , ; 写 qCos
return
jInterpolate:
;; 获得 Y1-Y0 = SinTable - SinTableadd.w IndexW,pTabBaseW,pTabPtrW
mov.w ,Y0W ; Y0
inc2.b IndexW,IndexW ; (Index += 2)&0xFF
add.w IndexW,pTabBaseW,pTabPtrW
subr.w Y0W,,Work0W ; Y1 - Y0
;; 计算 Delta = (Remainder*(Y1-Y0)) >> 16
mul.us RemainderW,Work0W,Work0W
;; Work1W包含 (Remainder*(Y1-Y0))的高位字
;; *pSin = Y0 + Delta
add.w Work1W,Y0W, ; 写 qSin并将指针递增 1指向 qCos
;; ================= COS =========================
;; 将 Sin索引加上 0x40得到 Cos索引。这样可能超出表的末尾。
;; 但是如果我们仅使用字节操作将会实现自动的翻转返回。
;; 实际上只加上了 0x3E这是因为在前面索引已经递增了 2。
add.b #0x3E,IndexW
add.w IndexW,pTabBaseW,pTabPtrW
;; 获得 Y1-Y0 = SinTable - SinTable
add.w IndexW,pTabBaseW,pTabPtrW
mov.w ,Y0W ; Y0
inc2.b IndexW,IndexW ; (Index += 2)&0xFF
add.w IndexW,pTabBaseW,pTabPtrW
subr.w Y0W,,Work0W ; Y1 - Y0
;; 计算 Delta = (Remainder*(Y1-Y0)) >> 16
mul.us RemainderW,Work0W,Work0W
;; Work1W包含 (Remainder*(Y1-Y0))的高位字
;; *pCos = Y0 + Delta
add.w Work1W,Y0W, ; 写 qCosreturn
.end
不明觉厉！ 贴这么长不容易哈 是不是两相的SVPWM更复杂？ 看到的基本都是关于三相的SVPWM研究 太长，还是支持一下 rifjft 发表于 2015-12-19 19:21
是不是两相的SVPWM更复杂？ 看到的基本都是关于三相的SVPWM研究
现在还没有去研究你说的两相的。{:smile:} 好长好长。。。 有人会看完吗？？呵 呵呵，那么这样说来，就要精减了。 买了2只800w eps马达，楼主的东西应该可以用，不过没有开发板就难了 还是默默的拉到后面下载了 谢谢无私奉献！！ 好帖,收藏 {:lol:}兄弟，这帖子长得蛋疼
太长，还是支持一下
页:
[1]