📄 close_loop.c
字号:
//---------------------------------------------------------------------------------------------------------
// 文件名: ClosedLoop.c
//
// 工程节点包括以下三个:
// 1. 主节点: CloseLoop.c
// 2. 芯片链接器描述文件: p30f3010.gld
// 3. PID运算汇编节点: PID.s
//---------------------------------------------------------------------------------------------------------
#include <p30f3010.h>
//--------------------------芯片配置字设置-------------------------------
_FOSC(CSW_FSCM_OFF & XT_PLL16);
_FWDT(WDT_OFF);
_FBORPOR(PBOR_ON & BORV_20 & PWRT_64 & MCLR_EN);
//-----------------------------------------------------------------------
typedef signed int SFRAC16;
#define FCY 20000000 // xtal = 5Mhz; PLLx16 -> 20 MIPS （运行速度20MIPS）
#define FPWM 19531 // PWM频率设置为19.5 kHz, 避免产生人能听到的噪音
#define _10MILLISEC 10
#define _50MILLISEC 50
#define _100MILLISEC 100
#define _500MILLISEC 500
#define HALLA 1 // RB3
#define HALLB 2 // RB4
#define HALLC 4 // RB5
#define CW 0 // 电机顺时针方向（CCW）转动标志位
#define CCW 1 // 电机反时针方向（ CW）转动标志位
#define Switch_2 (!PORTCbits.RC14)
// 计算周期 Period = TMRClock * 6 / RPM
// 例如转速 RPM = 6000 (最大速度)
// 周期 Period = (20,000,000 / 64) * 6 / 6000 = 312.5
// RPM = 60 (最小转速)
// 周期 Period = (20,000,000 / 64) * 6 / 60 = 31250
#define MINPERIOD 313 // 对于 6000 max rpm 和 10 极电机
#define MAXPERIOD 31250 // 对于 60 min rpm 和 10 极电机
#define MINABSSPEED 327
#define MAXABSSPEED 32663
// 当使用floats来初始化有符号16-bit小数变量时使用这个宏
#define SFloat_To_SFrac16(Float_Value) (((Float_Value)<0.0)?(SFRAC16)(32768*(Float_Value)-0.5):(SFRAC16)(32767*(Float_Value)+0.5))
void SixStepComm(int _Current_Sector, int _Required_Voltage); // 换向操作（根据转子的位置）
void InitADC10(void); // 初始化 ADC（用于速度指令）
void InitMCPWM(void); // 初始化 PWM为20kHz, 中心对齐, 互补对称模式，500 ns死区
void InitTMR1(void); // 初始化 TIMER1 （使用速度控制和电机停止保护）
void InitTMR3(void); // 初始化 TIMER3 （作为两个输入捕捉通道的时基）
void InitUserInt(void); // 初始化所有端口 (输入和输出)
void Init_IC_and_CN(void); // 初始化输入捕捉和电平变换中断CN （用于霍尔传感器输入）
void RunMotor(void); // 初始化所有变量和中断（用于启动和运行电机）
void StopMotor(void); // 清零所有标志位, 停止和电机控制相关的操作，禁止PWM
void ForceCommutation(void); // 当电机低速运行，该函数强制换向
void ChargeBootstraps(void); // 在电机运行起始阶段, 对bootstrap 电容进行充电
extern void SpeedControl(void); // 该函数包含所有汇编和C函数，进行速度PID环路控制
extern void SpeedCalculation(void);
// 工程里用到的标志位
struct
{
unsigned MotorRunning :1;
unsigned MotorCommInAdv :1;
unsigned MotorAdvEnabled:1;
unsigned unused :13;
}Flags;
unsigned int HallValue; // 霍尔输入值
unsigned int Sector; // 当前扇区值
unsigned int LastSector; // 最后一个扇区的值，对于滤除因为硬件滤波导致的慢转换速率引起的错误信号
unsigned int MotorStalledCounter = 0; // 每 1 ms加1, 每次检测到新扇区即清零.用于强制换向和电机零速保护
// 电机接线端子 | MCLV板连接
// -------------------------|-------------------
// 白（相） --------|-- M3
// 黑（相） --------|-- M2
// 红（相） --------|-- M1
// 绿（线） --------|-- G
// 红（霍尔电源） --------|-- +5V
// 黑（霍尔地） --------|-- GND
// 白（霍尔） --------|-- HA
// 棕（霍尔） --------|-- HB
// 浅绿（霍尔） --------|-- HC
char SectorTable[] = {-1,4,2,3,0,5,1,-1}; // 这个数组为霍尔状态值与相应换向扇区的对应值。扇区0或7 数非法的因此设置为返回值-1
//*************************************************************
// 下桥驱动表：
// 上桥驱动是PWM信号，下桥要么关闭要么开通
//*************************************************************
unsigned int StateLoTable[] = { 0x0204, /* PWM2L -> 1, PWM1H -> PWM */
0x0210, /* PWM3L -> 1, PWM1H -> PWM */
0x0810, /* PWM3L -> 1, PWM2H -> PWM */
0x0801, /* PWM1L -> 1, PWM2H -> PWM */
0x2001, /* PWM1L -> 1, PWM3H -> PWM */
0x2004}; /* PWM2L -> 1, PWM3H -> PWM */
/* Speed Control Variables */
unsigned char Current_Direction; // 当前电机机械旋转方向是在霍尔中断ISR里计算
unsigned char Required_Direction; // 给定电机机械旋转方向，与 ControlOutput变量有相同符号
unsigned int PastCapture, ActualCapture, Period; // 半个电周期的值，也即霍尔信号相邻沿之间的时间
SFRAC16 Kp = SFloat_To_SFrac16(0.10000); // PID绝对增益
SFRAC16 Ki = SFloat_To_SFrac16(0.01000);
SFRAC16 Kd = SFloat_To_SFrac16(0.00000);
SFRAC16 Speed, RefSpeed; // PID的实际和给定速度，二者差就是误差
SFRAC16 SpeedAbsValue; // 速度变量的绝对值，被相位超前模式使用
SFRAC16 ControlOutput = 0; // 控制器输出, 作为电压输出, 使用其符号作为转向标志
// PID用到的常量, 由于使用了MAC操作，对PID结构作了修改
// 参考SpeedControl()函数里的注解
SFRAC16 ControlDifference[3] __attribute__((__space__(xmemory), __aligned__(4))); // 误差，放置在x数据区，为MAC所使用
SFRAC16 PIDCoefficients[3] __attribute__((__space__(ymemory), __aligned__(4))); // 修改的PID参数，放置在y存储器里，为MAC所使用
SFRAC16 _MINPERIOD = MINPERIOD - 1; // 作为一个临时变量，用于汇编函数里的小数除法使用
//*****************************************************************************************************
// Timer 1 中断服务子程序：
// 1/。 在这个ISR里，将计算周期和速度，通过捕捉霍尔传感器的输入捕捉信号来完成的
// 2/。 在这个ISR里，将调用速度控制函数，从而产生一个新的输出电压：ControlOutput
// 3/。 在这个ISR里，还完成强制换向，在一段时间里速度接收不到电机的霍尔反馈信号就会调用强制换向。
// 假如产生霍尔ISR的时间太长，比如100 ms，强制关闭电机。
// 4/。 采用汇编语言计算速度，充分利用有符号小数除法的快速性（signed fractional division）
//*****************************************************************************************************
void __attribute__((__interrupt__)) _T1Interrupt (void)
{
IFS0bits.T1IF = 0;
Period = ActualCapture - PastCapture; // 这是一个无符号减法
if (Period < (unsigned int)MINPERIOD) // MINPERIOD相当于6000 rpm
Period = MINPERIOD;
else if (Period > (unsigned int)MAXPERIOD) // MAXPERIOD相当于60 rpm
Period = MAXPERIOD;
// 该子程序用汇编语言计算速度（fractional division）
// MINPERIOD
// Speed = (Fractional divide) ---------------
// Period
SpeedCalculation();
// Speed = ( 328/Period) ;
SpeedAbsValue = Speed;
if (Current_Direction == CCW) // 根据当前电机转向进行速度符号调整
Speed = -Speed;
// |————————————————|—————————————|————————————|——————————|—————————|
// | 状态 | RPM | SFRAC16 | SINT | HEX |
// |————————————————|—————————————|————————————|——————————|—————————|
// | 顺时针最大速度 | 6000 RPM | 0.996805 | 32663 | 0x7F97 |
// | 顺时针最小速度 | 60 RPM | 0.009984 | 327 | 0x0147 |
// | 反时针最小速度 | -60 RPM | -0.009984 | -327 | 0xFEB9 |
// | 反时针最大速度 | -6000 RPM | -0.996805 | -32663 | 0x8069 |
// |————————————————|—————————————|————————————|——————————|—————————|
// 这里调用速度PID控制函数，将使用 Speed, RefSpeed, 一些缓冲单元，将为SVM产生新的控制电压
SpeedControl();
asm("nop");
if (ControlOutput < 3276) // 设置最小ControlOutput 为 10% ，保证电机旋转
ControlOutput = 3276;
MotorStalledCounter++; // 在霍尔ISR里清零该变量
if ((MotorStalledCounter % _10MILLISEC) == 0)
{ ForceCommutation(); } // 强制换向
else if (MotorStalledCounter >= _100MILLISEC)
{ StopMotor(); } // 停止电机
return;
}
//************************************************************************************
// 输入电平变换中断（CN5）ISR，实现Hall A 中断：
// 1/。 该函数用于计算电机实际的机械方向，根据所在转子所在扇区调整相位变量
// 2/。 应该核准扇区，避免因为硬件滤波使得霍尔输入转换率降低，避免错误中断
//************************************************************************************
void __attribute__((__interrupt__)) _CNInterrupt (void)
{
IFS0bits.CNIF = 0;
HallValue = (unsigned int)((PORTB >> 3) & 0x0007); // 读取霍尔值
Sector = SectorTable[HallValue]; // 计算扇区值
if (Sector != LastSector) // 为了应对硬件转换速率慢的问题，这个操作是必须的
{
MotorStalledCounter = 0; // 因为检测到了新扇区，清除可能导致电机停止的变量
// 根据扇区计算电机当前旋转方向
if ((Sector == 5) || (Sector == 2)) // 假如往前是下降和上升，那么CCW
Current_Direction = CCW;
else
Current_Direction = CW;
SixStepComm(Sector, ControlOutput); // 假如 ControlOutput > 0 ，将顺时针方向CW运行否则，CCW
LastSector = Sector; // 假如扇区变化，更新最新扇区
}
return;
}
//***********************************************************************************
// 输入捕捉中断（CN7）ISR，实现Hall B 中断：
// 1/。 该函数用于计算电机实际的机械方向，根据所在转子所在扇区调整相位变量
// 2/。 使用输入捕捉（IC7）计算霍尔信号相邻跳变之间的时间，得到机械周期
// 3/。 应该核准扇区，避免因为硬件滤波使得霍尔输入转换率降低，避免错误中断
//***********************************************************************************
void __attribute__((__interrupt__)) _IC7Interrupt (void)
{
IFS1bits.IC7IF = 0; // Cleat interrupt flag
HallValue = (unsigned int)((PORTB >> 3) & 0x0007); // 读取霍尔值
Sector = SectorTable[HallValue]; // 计算扇区值
if (Sector != LastSector) // 为了应对硬件转换速率慢的问题，这个操作是必须的
{
PastCapture = ActualCapture; // 计算霍尔信号周期，相当于半个电周期
ActualCapture = IC7BUF;
IC7BUF; // 清零缓冲区
IC7BUF;
IC7BUF;
MotorStalledCounter = 0; // 因为检测到了新扇区，清除可能导致电机停止的变量
// 根据扇区计算电机当前旋转方向
if ((Sector == 3) || (Sector == 0)) // 假如往前是下降和上升，那么CW
Current_Direction = CCW;
else
Current_Direction = CW;
SixStepComm(Sector, ControlOutput); // 假如 ControlOutput > 0 ，将顺时针方向CW运行否则，CCW
LastSector = Sector; // 假如扇区变化，更新最新扇区
}
return;
}
//*************************************************************************************
// 输入捕捉中断（CN8）ISR，实现Hall C 中断：
// 1/。 该函数用于计算电机实际的机械方向，根据所在转子所在扇区调整相位变量
// 2/。 应该核准扇区，避免因为硬件滤波使得霍尔输入转换率降低，避免错误中断
//*************************************************************************************
void __attribute__((__interrupt__)) _IC8Interrupt (void)
{
IFS1bits.IC8IF = 0; // Cleat interrupt flag
HallValue = (unsigned int)((PORTB >> 3) & 0x0007); // 读取霍尔值
Sector = SectorTable[HallValue]; // 计算扇区值
if (Sector != LastSector) // 为了应对硬件转换速率慢的问题，这个操作是必须的
{
MotorStalledCounter = 0; // 因为检测到了新扇区，清除可能导致电机停止的变量
// 根据扇区计算电机当前旋转方向
if ((Sector == 1) || (Sector == 4)) // 假如往前是下降和上升，那么CW
Current_Direction = CCW;
else
Current_Direction = CW;
SixStepComm(Sector, ControlOutput); // 假如 ControlOutput > 0 ，将顺时针方向CW运行否则，CCW
LastSector = Sector; // 假如扇区变化，更新最新扇区
}
return;
}
//*********************************************************************
// ADC中断服务子程序：
// 根据电位器的数值装载参考速度(RefSpeed)，无符号小数
//*********************************************************************
void __attribute__((__interrupt__)) _ADCInterrupt (void)
{
IFS0bits.ADIF = 0;
RefSpeed = (int)(((unsigned int)ADCBUF0) / 2);
if (RefSpeed < 2000)
RefSpeed = 2000;
return;
}
//****************************************************************************************
// 主函数：
// 初始化外设，根据按键和电机运行状态启动或停止电机。其他操作和状态机根据中断运行
//****************************************************************************************
int main(void)
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -