untilyou_92
25-10-2013, 10:42 AM
Mọi người vào check dùm !! mình k hiểu lỗi này thế nào mình nghĩ câu lệnh đúng chứ nhỉ r cả lệnh #else nữa sau #else mình viết hàm thì chữ nó chuyển sang dạng như kiểu chú thíc ấy
Còn đây là code :
#include "p30f4012.h"
#include "svm.h"
_FOSC(CSW_FSCM_OFF & XT_PLL16);
_FWDT(WDT_OFF);
_FBORPOR(PBOR_ON & BORV_20 & PWRT_64 & MCLR_EN);
//-----------------------------------------------------------------------------
typedef signed int SFRAC16;
#define CLOSED_LOOP
#define PHASE_ADVANCE
#define FCY 20000000 // xtal = 5Mhz; PLLx16 -> 20 MIPS
#define FPWM 20000 // 20 kHz, so that no audible noise is present.
#define _10MILLISEC 10 // Used as a timeout with no hall effect sensors
#define _100MILLISEC 100
#define _1000MILLISEC 1000
#define PHASE_ZERO 57344
#define PHASE_ONE ((PHASE_ZERO + 65536/6) % 65536)
#define PHASE_TWO ((PHASE_ONE + 65536/6) % 65536)
#define PHASE_THREE ((PHASE_TWO + 65536/6) % 65536)
#define PHASE_FOUR ((PHASE_THREE + 65536/6) % 65536)
#define PHASE_FIVE ((PHASE_FOUR + 65536/6) % 65536)
#define MAX_PH_ADV_DEG 40
#define MAX_PH_ADV (int)(((float)MAX_PH_ADV_DEG / 360.0) * 65536.0)
#define HALLA 1 // Connected to RB3
#define HALLB 2 // Connected to RB4
#define HALLC 4 // Connected to RB5
#define CW 0 // Counter Clock Wise direction
#define CCW 1 // Clock Wise direction
#define SWITCH_S2 (!PORTCbits.RC14) // Push button S2
#define MINPERIOD 313 // For 6000 max rpm and 10 poles motor
#define MAXPERIOD 31250 // For 60 min rpm and 10 poles motor
#define SFloat_To_SFrac16(Float_Value) \
((Float_Value < 0.0) ? (SFRAC16)(32768 * (Float_Value) - 0.5) \
: (SFRAC16)(32767 * (Float_Value) + 0.5))
void InitADC10(void);
void InitMCPWM(void);
void InitTMR1(void);
void InitTMR3(void);
void InitUserInt(void);
void InitICandCN(void);
void RunMotor(void);
void StopMotor(void);
void SpeedControl(void);
void ForceCommutation(void);
void ChargeBootstraps(void);
int PhaseValues[6] __attribute__((far,section(".const,r")))=
{PHASE_ZERO, PHASE_ONE, PHASE_TWO, PHASE_THREE, PHASE_FOUR, PHASE_FIVE};
int PhaseOffset = 4100;
struct
{
unsigned MotorRunning :1; // This bit is 1 if motor running
unsigned unused :15;
}Flags;
unsigned int Phase;
signed int PhaseInc;
signed int PhaseAdvance;
unsigned int HallValue;
unsigned int Sector;
unsigned int LastSector;
unsigned int MotorStalledCounter = 0;
char SectorTable[] = {-1,4,2,3,0,5,1,-1};
unsigned char Current_Direction;
unsigned char Required_Direction;
unsigned int PastCapture, ActualCapture, Period;
SFRAC16 _MINPERIOD = MINPERIOD - 1;
SFRAC16 MeasuredSpeed, RefSpeed;
SFRAC16 ControlOutput = 0;
SFRAC16 Kp = SFloat_To_SFrac16(0.1); // P Gain
SFRAC16 Ki = SFloat_To_SFrac16(0.01); // I Gain
SFRAC16 Kd = SFloat_To_SFrac16(0.000); // D Gain
SFRAC16 ControlDifference[3] \
__attribute__((__space__(xmemory), __aligned__(4)));
SFRAC16 PIDCoefficients[3] \
__attribute__((__space__(ymemory), __aligned__(4)));
SFRAC16 _MAX_PH_ADV = MAX_PH_ADV;
void __attribute__((interrupt, no_auto_psv)) _T1Interrupt (void)
{
IFS0bits.T1IF = 0;
Period = ActualCapture - PastCapture;
if (Period < (unsigned int)MINPERIOD) // MINPERIOD or 6000 rpm
Period = MINPERIOD;
else if (Period > (unsigned int)MAXPERIOD) // MAXPERIOD or 60 rpm
Period = MAXPERIOD;
// PhaseInc is a value added to the Phase variable to generate the sine
// voltages. 1 electrical degree corresponds to a PhaseInc value of 184,
// since the pointer to the sine table is a 16bit value, where 360 Elec
// Degrees represents 65535 in the pointer.
// __builtin_divud(Long Value, Int Value) is a function of the compiler
// to do Long over Integer divisions.
PhaseInc = __builtin_divud(512000UL, Period); // Phase increment is used
// by the PWM isr (SVM)
// This subroutine in assembly calculates the MeasuredSpeed using
// fractional division. These operations in assembly perform the following
// formula:
// MINPERIOD (in fractional)
// MeasuredSpeed = ---------------------------
// Period (in fractional)
//
{ int divr;
__asm__ volatile("repeat #17\n\t"
"divf %1,%2\n\t"
: /* output */ "=a"(divr)
: /* input */ "r"(_MINPERIOD),
"e"(Period)
);
MeasuredSpeed = divr;
}
if (Current_Direction == CCW)
MeasuredSpeed = -MeasuredSpeed;
SpeedControl();
#ifdef PHASE_ADVANCE
#if !defined(__C30_VERSION__) || (__C30_VERSION__ < 200) || (__C30_VERSION__ == 300) || defined(TEST_ASM)
{ register int wreg4 asm("w4") = _MAX_PH_ADV;
register int wreg5 asm("w5") = MeasuredSpeed;
asm volatile("mpy %0*%1, A" : /* no outputs */
: "r"(wreg4), "r"(wreg5));
asm volatile("sac A, %0" : "=r"(PhaseAdvance));
}
#else
{ register int a_reg asm("A");
a_reg = __builtin_mpy(_MAX_PH_ADV, MeasuredSpeed, 0,0,0,0,0,0);
PhaseAdvance = __builtin_sac(a_reg, 0);
}
#endif
#endif
MotorStalledCounter++; // We increment a timeout variable to see if the
// motor is too slow (not generating hall effect
// sensors interrupts frequently enough) or if
// the motor is stalled. This variable is cleared
// in halls ISRs
if ((MotorStalledCounter % _10MILLISEC) == 0)
{
ForceCommutation(); // Force Commutation if no hall sensor changes
// have occured in specified timeout.
}
else if (MotorStalledCounter >= _1000MILLISEC)
{
StopMotor(); // Stop motor is no hall changes have occured in
// specified timeout
}
return;
}
void __attribute__((interrupt, no_auto_psv)) _CNInterrupt (void)
{
IFS0bits.CNIF = 0; // Clear interrupt flag
HallValue = (unsigned int)((PORTB >> 3) & 0x0007); // Read halls
Sector = SectorTable[HallValue]; // Get Sector from table
// This MUST be done for getting around the HW slow rate
if (Sector != LastSector)
{
// Since a new sector is detected, clear variable that would stop
// the motor if stalled.
MotorStalledCounter = 0;
// Motor current direction is computed based on Sector
if ((Sector == 5) || (Sector == 2))
Current_Direction = CCW;
else
Current_Direction = CW;
if (Required_Direction == CW)
{
Phase = PhaseValues[Sector];
}
else
{
Phase = PhaseValues[(Sector + 3) % 6] + PhaseOffset;
}
LastSector = Sector; // Update last sector
}
return;
}
void __attribute__((interrupt, no_auto_psv)) _IC7Interrupt (void)
{
IFS1bits.IC7IF = 0; // Cleat interrupt flag
HallValue = (unsigned int)((PORTB >> 3) & 0x0007); // Read halls
Sector = SectorTable[HallValue]; // Get Sector from table
// This MUST be done for getting around the HW slow rate
if (Sector != LastSector)
{
// Calculate Hall period corresponding to half an electrical cycle
PastCapture = ActualCapture;
ActualCapture = IC7BUF;
IC7BUF;
IC7BUF;
IC7BUF;
MotorStalledCounter = 0;
if ((Sector == 3) || (Sector == 0))
Current_Direction = CCW;
else
Current_Direction = CW;
if (Required_Direction == CW)
{
Phase = PhaseValues[Sector];
}
else
{
Phase = PhaseValues[(Sector + 3) % 6] + PhaseOffset;
}
LastSector = Sector; // Update last sector
}
return;
}
void __attribute__((interrupt, no_auto_psv)) _IC8Interrupt (void)
{
IFS1bits.IC8IF = 0; // Cleat interrupt flag
HallValue = (unsigned int)((PORTB >> 3) & 0x0007); // Read halls
Sector = SectorTable[HallValue]; // Get Sector from table
// This MUST be done for getting around the HW slow rate
if (Sector != LastSector)
{
// Since a new sector is detected, clear variable that would stop
// the motor if stalled.
MotorStalledCounter = 0;
// Motor current direction is computed based on Sector
if ((Sector == 1) || (Sector == 4))
Current_Direction = CCW;
else
Current_Direction = CW;
// Motor commutation is actually based on the required direction, not
// the current dir. This allows driving the motor in four quadrants
if (Required_Direction == CW)
{
Phase = PhaseValues[Sector];
}
else
{
Phase = PhaseValues[(Sector + 3) % 6] + PhaseOffset;
}
LastSector = Sector; // Update last sector
}
return;
}
void __attribute__((interrupt, no_auto_psv)) _PWMInterrupt (void)
{
IFS2bits.PWMIF = 0; // Clear interrupt flag
if (Required_Direction == CW)
{
if (Current_Direction == CW)
Phase += PhaseInc; // Increment Phase if CW to generate the
// sinewave only if both directions are equal
// If Required_Direction is CW (forward) POSITIVE voltage is applied
#ifdef PHASE_ADVANCE
SVM(ControlOutput, Phase + PhaseAdvance);
#else
SVM(ControlOutput, Phase);
#endif
}
else
{
if (Current_Direction == CCW)
Phase -= PhaseInc; // Decrement Phase if CCW to generate
// the sinewave only if both
// directions are equal
// If Required_Direction is CCW (reverse) NEGATIVE voltage is applied
#ifdef PHASE_ADVANCE
SVM(-(ControlOutput+1), Phase + PhaseAdvance);// PhaseAdvance addition
// produces the sinewave
// phase shift
#else
SVM(-(ControlOutput+1), Phase);
#endif
}
return;
}
void __attribute__((interrupt, no_auto_psv)) _ADCInterrupt (void)
{
IFS0bits.ADIF = 0; // Clear interrupt flag
RefSpeed = ADCBUF0; // Read POT value to set Reference Speed
return;
}
int main(void)
{
InitUserInt(); // Initialize User Interface I/Os
InitADC10(); // Initialize ADC to be signed fractional
InitTMR1(); // Initialize TMR1 for 1 ms periodic ISR
InitTMR3(); // Initialize TMR3 for timebase of capture
InitICandCN(); // Initialize Hall sensor inputs ISRs
InitMCPWM(); // Initialize PWM @ 20 kHz, center aligned, 1 us of
// deadtime
for(;;)
{
if ((SWITCH_S2) && (!Flags.MotorRunning))
{
while(SWITCH_S2);
RunMotor(); // Run motor if push button is pressed and motor is
// stopped
}
else if ((SWITCH_S2) && (Flags.MotorRunning))
{
while(SWITCH_S2);
StopMotor();// Stop motor if push button is pressed and motor is
// running
}
}
return 0;
}
void ChargeBootstraps(void)
{
unsigned int i;
OVDCON = 0x0015; // Turn ON low side transistors to charge
for (i = 0; i < 33330; i++) // 10 ms Delay at 20 MIPs
;
PWMCON2bits.UDIS = 1;
PDC1 = PTPER; // Initialize as 0 voltage
PDC2 = PTPER; // Initialize as 0 voltage
PDC3 = PTPER; // Initialize as 0 voltage
OVDCON = 0x3F00; // Configure PWM0-5 to be governed by PWM module
PWMCON2bits.UDIS = 0;
return;
}
void RunMotor(void)
{
ChargeBootstraps();
// init variables
ControlDifference[0] = 0; // Error at K (most recent)
ControlDifference[1] = 0; // Error at K-1
ControlDifference[2] = 0; // Error at K-2 (least recent)
PIDCoefficients[0] = Kp + Ki + Kd; // Modified coefficient for using MACs
PIDCoefficients[1] = -(Ki + 2*Kd); // Modified coefficient for using MACs
PIDCoefficients[2] = Kd; // Modified coefficient for using MACs
TMR1 = 0; // Reset timer 1 for speed control
TMR3 = 0; // Reset timer 3 for speed measurement
ActualCapture = MAXPERIOD; // Initialize captures for minimum speed
//(60 RPMs)
PastCapture = 0;
// Initialize direction with required direction
// Remember that ADC is not stopped.
HallValue = (unsigned int)((PORTB >> 3) & 0x0007); // Read halls
LastSector = Sector = SectorTable[HallValue];
if (RefSpeed < 0)
{
ControlOutput = 0; // Initial output voltage
Current_Direction = Required_Direction = CCW;
Phase = PhaseValues[(Sector + 3) % 6] + PhaseOffset;
}
else
{
ControlOutput = 0; // Initial output voltage
Current_Direction = Required_Direction = CW;
Phase = PhaseValues[Sector];
}
MotorStalledCounter = 0;
PhaseInc = __builtin_divud(512000UL, MAXPERIOD);
// Clear all interrupts flags
IFS0bits.T1IF = 0; // Clear timer 1 flag
IFS0bits.CNIF = 0; // Clear interrupt flag
IFS1bits.IC7IF = 0; // Clear interrupt flag
IFS1bits.IC8IF = 0; // Clear interrupt flag
IFS2bits.PWMIF = 0; // Clear interrupt flag
// enable all interrupts
__asm__ volatile ("DISI #0x3FFF");
IEC0bits.T1IE = 1; // Enable interrupts for timer 1
IEC0bits.CNIE = 1; // Enable interrupts on CN5
IEC1bits.IC7IE = 1; // Enable interrupts on IC7
IEC1bits.IC8IE = 1; // Enable interrupts on IC8
IEC2bits.PWMIE = 1; // Enable PWM interrupts
DISICNT = 0;
Flags.MotorRunning = 1; // Indicate that the motor is running
return;
}
void StopMotor(void)
{
OVDCON = 0x0000; // turn OFF every transistor
// disable all interrupts
__asm__ volatile ("DISI #0x3FFF");
IEC0bits.T1IE = 0; // Disable interrupts for timer 1
IEC0bits.CNIE = 0; // Disable interrupts on CN5
IEC1bits.IC7IE = 0; // Disable interrupts on IC7
IEC1bits.IC8IE = 0; // Disable interrupts on IC8
IEC2bits.PWMIE = 0; // Disable PWM interrupts
DISICNT = 0;
Flags.MotorRunning = 0; // Indicate that the motor has been stopped
return;
}
#if !defined(__C30_VERSION__) || (__C30_VERSION__ < 200)
void SpeedControl(void)
{ register SFRAC16 *ControlDifferencePtr asm("w8") = ControlDifference;
register SFRAC16 *PIDCoefficientsPtr asm("w10") = PIDCoefficients;
register SFRAC16 x_prefetch asm("w4");
register SFRAC16 y_prefetch asm("w5");
CORCONbits.SATA = 1; // Enable Saturation on Acc A
// Calculate most recent error with saturation, no limit checking required
__asm__ volatile ("LAC %0, A" : /* no outputs */ : "r"(RefSpeed));
__asm__ volatile ("LAC %0, B" : /* no outputs */ : "r"(MeasuredSpeed));
__asm__ volatile ("SUB A");
__asm__ volatile ("SAC A, [%0]" : /* no outputs */ :
"r"(ControlDifferencePtr));
// Prepare MAC Operands
__asm__ volatile ("MOVSAC A, [%0]+=2, %2, [%1]+=2, %3" :
/* outputs */ "+r"(ControlDifferencePtr),
"+r"(PIDCoefficientsPtr),
"=r"(x_prefetch),
"=r"(y_prefetch));
__asm__ volatile ("LAC %0, A" : /* no outpus */ : "r"(ControlOutput)); // Load Acc with last output
// Perform MAC
__asm__ volatile ("REPEAT #2\n\t"
"MAC %2*%3, A, [%0]+=2, %2, [%1]+=2, %3" :
/* outputs */ "+r"(ControlDifferencePtr),
"+r"(PIDCoefficientsPtr),
"+r"(x_prefetch),
"+r"(y_prefetch));
// Store result in ControlOutput with saturation
__asm__ volatile ("SAC A, %0" : "=r"(ControlOutput));
CORCONbits.SATA = 0; // Disable Saturation on Acc A
// Store last 2 errors
ControlDifference[2] = ControlDifference[1];
ControlDifference[1] = ControlDifference[0];
#ifndef CLOSED_LOOP
ControlOutput = RefSpeed;
#endif
// ControlOutput will determine the motor required direction
if (ControlOutput < 0)
Required_Direction = CCW;
else
Required_Direction = CW;
return;
}
#else
void SpeedControl_using_inline_asm (void) {
SFRAC16 *ControlDifferencePtr = ControlDifference;
SFRAC16 *PIDCoefficientsPtr = PIDCoefficients;
register SFRAC16 x_prefetch asm("w4");
register SFRAC16 y_prefetch;
CORCONbits.SATA = 1; // Enable Saturation on Acc A
// Calculate most recent error with saturation, no limit checking required
__asm__ volatile ("LAC %0, A" : /* no outputs */ : "r"(RefSpeed));
__asm__ volatile ("LAC %0, B" : /* no outputs */ : "r"(MeasuredSpeed));
__asm__ volatile ("SUB A");
__asm__ volatile ("SAC A, [%0]" : /* no outputs */ :
"r"(ControlDifferencePtr));
// Prepare MAC Operands
__asm__ volatile ("MOVSAC A, [%0]+=2, %2, [%1]+=2, %3" :
/* outputs */ "+x"(ControlDifferencePtr),
"+y"(PIDCoefficientsPtr),
"=z"(x_prefetch),
"=z"(y_prefetch));
__asm__ volatile ("LAC %0, A" : /* no outpus */ : "r"(ControlOutput)); // Load Acc with last output
// Perform MAC
__asm__ volatile ("REPEAT #2\n\t"
"MAC %2*%3, A, [%0]+=2, %2, [%1]+=2, %3" :
/* outputs */ "+x"(ControlDifferencePtr),
"+y"(PIDCoefficientsPtr),
"+z"(x_prefetch),
"+z"(y_prefetch));
// Store result in ControlOutput with saturation
__asm__ volatile ("SAC A, %0" : "=r"(ControlOutput));
CORCONbits.SATA = 0; // Disable Saturation on Acc A
// Store last 2 errors
ControlDifference[2] = ControlDifference[1];
ControlDifference[1] = ControlDifference[0];
// If CLOSED_LOOP is undefined (running open loop) overide ControlOutput
// with value read from the external potentiometer
#ifndef CLOSED_LOOP
ControlOutput = RefSpeed;
#endif
// ControlOutput will determine the motor required direction
if (ControlOutput < 0)
Required_Direction = CCW;
else
Required_Direction = CW;
return;
}
void SpeedControl(void) {
SFRAC16 *ControlDifferencePtr = ControlDifference;
SFRAC16 *PIDCoefficientsPtr = PIDCoefficients;
SFRAC16 x_prefetch;
SFRAC16 y_prefetch;
register int reg_a asm("A");
register int reg_b asm("B");
CORCONbits.SATA = 1; // Enable Saturation on Acc A
reg_a = __builtin_lac(RefSpeed,0);
reg_b = __builtin_lac(MeasuredSpeed,0);
reg_a = __builtin_subab();
*ControlDifferencePtr = __builtin_sac(reg_a,0);
reg_a = __builtin_movsac(&ControlDifferencePtr, &x_prefetch, 2,
&PIDCoefficientsPtr, &y_prefetch, 2, 0);
reg_a = __builtin_lac(ControlOutput, 0);
reg_a = __builtin_mac(x_prefetch,y_prefetch,
&ControlDifferencePtr, &x_prefetch, 2,
&PIDCoefficientsPtr, &y_prefetch, 2, 0);
reg_a = __builtin_mac(x_prefetch,y_prefetch,
&ControlDifferencePtr, &x_prefetch, 2,
&PIDCoefficientsPtr, &y_prefetch, 2, 0);
reg_a = __builtin_mac(x_prefetch,y_prefetch,
&ControlDifferencePtr, &x_prefetch, 2,
&PIDCoefficientsPtr, &y_prefetch, 2, 0);
ControlOutput = __builtin_sac(reg_a, 0);
CORCONbits.SATA = 0; // Disable Saturation on Acc A
// Store last 2 errors
ControlDifference[2] = ControlDifference[1];
ControlDifference[1] = ControlDifference[0];
// If CLOSED_LOOP is undefined (running open loop) overide ControlOutput
// with value read from the external potentiometer
#ifndef CLOSED_LOOP
ControlOutput = RefSpeed;
#endif
// ControlOutput will determine the motor required direction
if (ControlOutput < 0)
Required_Direction = CCW;
else
Required_Direction = CW;
return;
}
#endif
void ForceCommutation(void)
{
HallValue = (unsigned int)((PORTB >> 3) & 0x0007); // Read halls
Sector = SectorTable[HallValue]; // Read sector based on halls
if (Sector != -1) // If the sector is invalid don't do anything
{
// Depending on the required direction, a new phase is fetched
if (Required_Direction == CW)
{
// Motor is required to run forward, so read directly the table
Phase = PhaseValues[Sector];
}
else
{
// Motor is required to run reverse, so calculate new phase and
// add offset to compensate asymmetries
Phase = PhaseValues[(Sector + 3) % 6] + PhaseOffset;
}
}
return;
}
void InitADC10(void)
{
ADPCFG = 0x0038; // RB3, RB4, and RB5 are digital
ADCON1 = 0x036E; // PWM starts conversion
// Signed fractional conversions
ADCON2 = 0x0000;
ADCHS = 0x0002; // Pot is connected to AN2
ADCON3 = 0x0003;
IFS0bits.ADIF = 0; // Clear ISR flag
IEC0bits.ADIE = 1; // Enable interrupts
ADCON1bits.ADON = 1; // turn ADC ON
return;
}
void InitMCPWM(void)
{
TRISE = 0x0100; // PWM pins as outputs, and FLTA as input
PTPER = (FCY/FPWM - 1) >> 1; // Compute Period based on CPU speed and
// required PWM frequency (see defines)
OVDCON = 0x0000; // Disable all PWM outputs.
DTCON1 = 0x0010; // ~1 us of dead time
PWMCON1 = 0x0077; // Enable PWM output pins and configure them as
// complementary mode
PDC1 = PTPER; // Initialize as 0 voltage
PDC2 = PTPER; // Initialize as 0 voltage
PDC3 = PTPER; // Initialize as 0 voltage
SEVTCMP = 1; // Enable triggering for ADC
PWMCON2 = 0x0F02; // 16 postscale values, for achieving 20 kHz
PTCON = 0x8002; // start PWM as center aligned mode
return;
}
void InitICandCN(void)
{
//Hall A -> CN5. Hall A is only used for commutation.
//Hall B -> IC7. Hall B is used for Speed measurement and commutation.
//Hall C -> IC8. Hall C is only used for commutation.
// Init Input change notification 5
TRISB |= 0x38; // Ensure that hall connections are inputs
CNPU1 = 0; // Disable all CN pull ups
CNEN1 = 0x20; // Enable CN5
IFS0bits.CNIF = 0; // Clear interrupt flag
// Init Input Capture 7
IC7CON = 0x0001; // Input capture every edge with interrupts and TMR3
IFS1bits.IC7IF = 0; // Clear interrupt flag
// Init Input Capture 8
IC8CON = 0x0001; // Input capture every edge with interrupts and TMR3
IFS1bits.IC8IF = 0; // Clear interrupt flag
return;
}
void InitTMR1(void)
{
T1CON = 0x0020; // internal Tcy/64 clock
TMR1 = 0;
PR1 = 313; // 1 ms interrupts for 20 MIPS
T1CONbits.TON = 1; // turn on timer 1
return;
}
void InitTMR3(void)
{
T3CON = 0x0020; // internal Tcy/64 clock
TMR3 = 0;
PR3 = 0xFFFF;
T3CONbits.TON = 1; // turn on timer 3
return;
}
void InitUserInt(void)
{
TRISC |= 0x4000; // S2/RC14 as input
// Analog pin for POT already initialized in ADC init subroutine
PORTF = 0x0008; // RS232 Initial values
TRISF = 0xFFF7; // TX as output
return;
}
Còn đây là code :
#include "p30f4012.h"
#include "svm.h"
_FOSC(CSW_FSCM_OFF & XT_PLL16);
_FWDT(WDT_OFF);
_FBORPOR(PBOR_ON & BORV_20 & PWRT_64 & MCLR_EN);
//-----------------------------------------------------------------------------
typedef signed int SFRAC16;
#define CLOSED_LOOP
#define PHASE_ADVANCE
#define FCY 20000000 // xtal = 5Mhz; PLLx16 -> 20 MIPS
#define FPWM 20000 // 20 kHz, so that no audible noise is present.
#define _10MILLISEC 10 // Used as a timeout with no hall effect sensors
#define _100MILLISEC 100
#define _1000MILLISEC 1000
#define PHASE_ZERO 57344
#define PHASE_ONE ((PHASE_ZERO + 65536/6) % 65536)
#define PHASE_TWO ((PHASE_ONE + 65536/6) % 65536)
#define PHASE_THREE ((PHASE_TWO + 65536/6) % 65536)
#define PHASE_FOUR ((PHASE_THREE + 65536/6) % 65536)
#define PHASE_FIVE ((PHASE_FOUR + 65536/6) % 65536)
#define MAX_PH_ADV_DEG 40
#define MAX_PH_ADV (int)(((float)MAX_PH_ADV_DEG / 360.0) * 65536.0)
#define HALLA 1 // Connected to RB3
#define HALLB 2 // Connected to RB4
#define HALLC 4 // Connected to RB5
#define CW 0 // Counter Clock Wise direction
#define CCW 1 // Clock Wise direction
#define SWITCH_S2 (!PORTCbits.RC14) // Push button S2
#define MINPERIOD 313 // For 6000 max rpm and 10 poles motor
#define MAXPERIOD 31250 // For 60 min rpm and 10 poles motor
#define SFloat_To_SFrac16(Float_Value) \
((Float_Value < 0.0) ? (SFRAC16)(32768 * (Float_Value) - 0.5) \
: (SFRAC16)(32767 * (Float_Value) + 0.5))
void InitADC10(void);
void InitMCPWM(void);
void InitTMR1(void);
void InitTMR3(void);
void InitUserInt(void);
void InitICandCN(void);
void RunMotor(void);
void StopMotor(void);
void SpeedControl(void);
void ForceCommutation(void);
void ChargeBootstraps(void);
int PhaseValues[6] __attribute__((far,section(".const,r")))=
{PHASE_ZERO, PHASE_ONE, PHASE_TWO, PHASE_THREE, PHASE_FOUR, PHASE_FIVE};
int PhaseOffset = 4100;
struct
{
unsigned MotorRunning :1; // This bit is 1 if motor running
unsigned unused :15;
}Flags;
unsigned int Phase;
signed int PhaseInc;
signed int PhaseAdvance;
unsigned int HallValue;
unsigned int Sector;
unsigned int LastSector;
unsigned int MotorStalledCounter = 0;
char SectorTable[] = {-1,4,2,3,0,5,1,-1};
unsigned char Current_Direction;
unsigned char Required_Direction;
unsigned int PastCapture, ActualCapture, Period;
SFRAC16 _MINPERIOD = MINPERIOD - 1;
SFRAC16 MeasuredSpeed, RefSpeed;
SFRAC16 ControlOutput = 0;
SFRAC16 Kp = SFloat_To_SFrac16(0.1); // P Gain
SFRAC16 Ki = SFloat_To_SFrac16(0.01); // I Gain
SFRAC16 Kd = SFloat_To_SFrac16(0.000); // D Gain
SFRAC16 ControlDifference[3] \
__attribute__((__space__(xmemory), __aligned__(4)));
SFRAC16 PIDCoefficients[3] \
__attribute__((__space__(ymemory), __aligned__(4)));
SFRAC16 _MAX_PH_ADV = MAX_PH_ADV;
void __attribute__((interrupt, no_auto_psv)) _T1Interrupt (void)
{
IFS0bits.T1IF = 0;
Period = ActualCapture - PastCapture;
if (Period < (unsigned int)MINPERIOD) // MINPERIOD or 6000 rpm
Period = MINPERIOD;
else if (Period > (unsigned int)MAXPERIOD) // MAXPERIOD or 60 rpm
Period = MAXPERIOD;
// PhaseInc is a value added to the Phase variable to generate the sine
// voltages. 1 electrical degree corresponds to a PhaseInc value of 184,
// since the pointer to the sine table is a 16bit value, where 360 Elec
// Degrees represents 65535 in the pointer.
// __builtin_divud(Long Value, Int Value) is a function of the compiler
// to do Long over Integer divisions.
PhaseInc = __builtin_divud(512000UL, Period); // Phase increment is used
// by the PWM isr (SVM)
// This subroutine in assembly calculates the MeasuredSpeed using
// fractional division. These operations in assembly perform the following
// formula:
// MINPERIOD (in fractional)
// MeasuredSpeed = ---------------------------
// Period (in fractional)
//
{ int divr;
__asm__ volatile("repeat #17\n\t"
"divf %1,%2\n\t"
: /* output */ "=a"(divr)
: /* input */ "r"(_MINPERIOD),
"e"(Period)
);
MeasuredSpeed = divr;
}
if (Current_Direction == CCW)
MeasuredSpeed = -MeasuredSpeed;
SpeedControl();
#ifdef PHASE_ADVANCE
#if !defined(__C30_VERSION__) || (__C30_VERSION__ < 200) || (__C30_VERSION__ == 300) || defined(TEST_ASM)
{ register int wreg4 asm("w4") = _MAX_PH_ADV;
register int wreg5 asm("w5") = MeasuredSpeed;
asm volatile("mpy %0*%1, A" : /* no outputs */
: "r"(wreg4), "r"(wreg5));
asm volatile("sac A, %0" : "=r"(PhaseAdvance));
}
#else
{ register int a_reg asm("A");
a_reg = __builtin_mpy(_MAX_PH_ADV, MeasuredSpeed, 0,0,0,0,0,0);
PhaseAdvance = __builtin_sac(a_reg, 0);
}
#endif
#endif
MotorStalledCounter++; // We increment a timeout variable to see if the
// motor is too slow (not generating hall effect
// sensors interrupts frequently enough) or if
// the motor is stalled. This variable is cleared
// in halls ISRs
if ((MotorStalledCounter % _10MILLISEC) == 0)
{
ForceCommutation(); // Force Commutation if no hall sensor changes
// have occured in specified timeout.
}
else if (MotorStalledCounter >= _1000MILLISEC)
{
StopMotor(); // Stop motor is no hall changes have occured in
// specified timeout
}
return;
}
void __attribute__((interrupt, no_auto_psv)) _CNInterrupt (void)
{
IFS0bits.CNIF = 0; // Clear interrupt flag
HallValue = (unsigned int)((PORTB >> 3) & 0x0007); // Read halls
Sector = SectorTable[HallValue]; // Get Sector from table
// This MUST be done for getting around the HW slow rate
if (Sector != LastSector)
{
// Since a new sector is detected, clear variable that would stop
// the motor if stalled.
MotorStalledCounter = 0;
// Motor current direction is computed based on Sector
if ((Sector == 5) || (Sector == 2))
Current_Direction = CCW;
else
Current_Direction = CW;
if (Required_Direction == CW)
{
Phase = PhaseValues[Sector];
}
else
{
Phase = PhaseValues[(Sector + 3) % 6] + PhaseOffset;
}
LastSector = Sector; // Update last sector
}
return;
}
void __attribute__((interrupt, no_auto_psv)) _IC7Interrupt (void)
{
IFS1bits.IC7IF = 0; // Cleat interrupt flag
HallValue = (unsigned int)((PORTB >> 3) & 0x0007); // Read halls
Sector = SectorTable[HallValue]; // Get Sector from table
// This MUST be done for getting around the HW slow rate
if (Sector != LastSector)
{
// Calculate Hall period corresponding to half an electrical cycle
PastCapture = ActualCapture;
ActualCapture = IC7BUF;
IC7BUF;
IC7BUF;
IC7BUF;
MotorStalledCounter = 0;
if ((Sector == 3) || (Sector == 0))
Current_Direction = CCW;
else
Current_Direction = CW;
if (Required_Direction == CW)
{
Phase = PhaseValues[Sector];
}
else
{
Phase = PhaseValues[(Sector + 3) % 6] + PhaseOffset;
}
LastSector = Sector; // Update last sector
}
return;
}
void __attribute__((interrupt, no_auto_psv)) _IC8Interrupt (void)
{
IFS1bits.IC8IF = 0; // Cleat interrupt flag
HallValue = (unsigned int)((PORTB >> 3) & 0x0007); // Read halls
Sector = SectorTable[HallValue]; // Get Sector from table
// This MUST be done for getting around the HW slow rate
if (Sector != LastSector)
{
// Since a new sector is detected, clear variable that would stop
// the motor if stalled.
MotorStalledCounter = 0;
// Motor current direction is computed based on Sector
if ((Sector == 1) || (Sector == 4))
Current_Direction = CCW;
else
Current_Direction = CW;
// Motor commutation is actually based on the required direction, not
// the current dir. This allows driving the motor in four quadrants
if (Required_Direction == CW)
{
Phase = PhaseValues[Sector];
}
else
{
Phase = PhaseValues[(Sector + 3) % 6] + PhaseOffset;
}
LastSector = Sector; // Update last sector
}
return;
}
void __attribute__((interrupt, no_auto_psv)) _PWMInterrupt (void)
{
IFS2bits.PWMIF = 0; // Clear interrupt flag
if (Required_Direction == CW)
{
if (Current_Direction == CW)
Phase += PhaseInc; // Increment Phase if CW to generate the
// sinewave only if both directions are equal
// If Required_Direction is CW (forward) POSITIVE voltage is applied
#ifdef PHASE_ADVANCE
SVM(ControlOutput, Phase + PhaseAdvance);
#else
SVM(ControlOutput, Phase);
#endif
}
else
{
if (Current_Direction == CCW)
Phase -= PhaseInc; // Decrement Phase if CCW to generate
// the sinewave only if both
// directions are equal
// If Required_Direction is CCW (reverse) NEGATIVE voltage is applied
#ifdef PHASE_ADVANCE
SVM(-(ControlOutput+1), Phase + PhaseAdvance);// PhaseAdvance addition
// produces the sinewave
// phase shift
#else
SVM(-(ControlOutput+1), Phase);
#endif
}
return;
}
void __attribute__((interrupt, no_auto_psv)) _ADCInterrupt (void)
{
IFS0bits.ADIF = 0; // Clear interrupt flag
RefSpeed = ADCBUF0; // Read POT value to set Reference Speed
return;
}
int main(void)
{
InitUserInt(); // Initialize User Interface I/Os
InitADC10(); // Initialize ADC to be signed fractional
InitTMR1(); // Initialize TMR1 for 1 ms periodic ISR
InitTMR3(); // Initialize TMR3 for timebase of capture
InitICandCN(); // Initialize Hall sensor inputs ISRs
InitMCPWM(); // Initialize PWM @ 20 kHz, center aligned, 1 us of
// deadtime
for(;;)
{
if ((SWITCH_S2) && (!Flags.MotorRunning))
{
while(SWITCH_S2);
RunMotor(); // Run motor if push button is pressed and motor is
// stopped
}
else if ((SWITCH_S2) && (Flags.MotorRunning))
{
while(SWITCH_S2);
StopMotor();// Stop motor if push button is pressed and motor is
// running
}
}
return 0;
}
void ChargeBootstraps(void)
{
unsigned int i;
OVDCON = 0x0015; // Turn ON low side transistors to charge
for (i = 0; i < 33330; i++) // 10 ms Delay at 20 MIPs
;
PWMCON2bits.UDIS = 1;
PDC1 = PTPER; // Initialize as 0 voltage
PDC2 = PTPER; // Initialize as 0 voltage
PDC3 = PTPER; // Initialize as 0 voltage
OVDCON = 0x3F00; // Configure PWM0-5 to be governed by PWM module
PWMCON2bits.UDIS = 0;
return;
}
void RunMotor(void)
{
ChargeBootstraps();
// init variables
ControlDifference[0] = 0; // Error at K (most recent)
ControlDifference[1] = 0; // Error at K-1
ControlDifference[2] = 0; // Error at K-2 (least recent)
PIDCoefficients[0] = Kp + Ki + Kd; // Modified coefficient for using MACs
PIDCoefficients[1] = -(Ki + 2*Kd); // Modified coefficient for using MACs
PIDCoefficients[2] = Kd; // Modified coefficient for using MACs
TMR1 = 0; // Reset timer 1 for speed control
TMR3 = 0; // Reset timer 3 for speed measurement
ActualCapture = MAXPERIOD; // Initialize captures for minimum speed
//(60 RPMs)
PastCapture = 0;
// Initialize direction with required direction
// Remember that ADC is not stopped.
HallValue = (unsigned int)((PORTB >> 3) & 0x0007); // Read halls
LastSector = Sector = SectorTable[HallValue];
if (RefSpeed < 0)
{
ControlOutput = 0; // Initial output voltage
Current_Direction = Required_Direction = CCW;
Phase = PhaseValues[(Sector + 3) % 6] + PhaseOffset;
}
else
{
ControlOutput = 0; // Initial output voltage
Current_Direction = Required_Direction = CW;
Phase = PhaseValues[Sector];
}
MotorStalledCounter = 0;
PhaseInc = __builtin_divud(512000UL, MAXPERIOD);
// Clear all interrupts flags
IFS0bits.T1IF = 0; // Clear timer 1 flag
IFS0bits.CNIF = 0; // Clear interrupt flag
IFS1bits.IC7IF = 0; // Clear interrupt flag
IFS1bits.IC8IF = 0; // Clear interrupt flag
IFS2bits.PWMIF = 0; // Clear interrupt flag
// enable all interrupts
__asm__ volatile ("DISI #0x3FFF");
IEC0bits.T1IE = 1; // Enable interrupts for timer 1
IEC0bits.CNIE = 1; // Enable interrupts on CN5
IEC1bits.IC7IE = 1; // Enable interrupts on IC7
IEC1bits.IC8IE = 1; // Enable interrupts on IC8
IEC2bits.PWMIE = 1; // Enable PWM interrupts
DISICNT = 0;
Flags.MotorRunning = 1; // Indicate that the motor is running
return;
}
void StopMotor(void)
{
OVDCON = 0x0000; // turn OFF every transistor
// disable all interrupts
__asm__ volatile ("DISI #0x3FFF");
IEC0bits.T1IE = 0; // Disable interrupts for timer 1
IEC0bits.CNIE = 0; // Disable interrupts on CN5
IEC1bits.IC7IE = 0; // Disable interrupts on IC7
IEC1bits.IC8IE = 0; // Disable interrupts on IC8
IEC2bits.PWMIE = 0; // Disable PWM interrupts
DISICNT = 0;
Flags.MotorRunning = 0; // Indicate that the motor has been stopped
return;
}
#if !defined(__C30_VERSION__) || (__C30_VERSION__ < 200)
void SpeedControl(void)
{ register SFRAC16 *ControlDifferencePtr asm("w8") = ControlDifference;
register SFRAC16 *PIDCoefficientsPtr asm("w10") = PIDCoefficients;
register SFRAC16 x_prefetch asm("w4");
register SFRAC16 y_prefetch asm("w5");
CORCONbits.SATA = 1; // Enable Saturation on Acc A
// Calculate most recent error with saturation, no limit checking required
__asm__ volatile ("LAC %0, A" : /* no outputs */ : "r"(RefSpeed));
__asm__ volatile ("LAC %0, B" : /* no outputs */ : "r"(MeasuredSpeed));
__asm__ volatile ("SUB A");
__asm__ volatile ("SAC A, [%0]" : /* no outputs */ :
"r"(ControlDifferencePtr));
// Prepare MAC Operands
__asm__ volatile ("MOVSAC A, [%0]+=2, %2, [%1]+=2, %3" :
/* outputs */ "+r"(ControlDifferencePtr),
"+r"(PIDCoefficientsPtr),
"=r"(x_prefetch),
"=r"(y_prefetch));
__asm__ volatile ("LAC %0, A" : /* no outpus */ : "r"(ControlOutput)); // Load Acc with last output
// Perform MAC
__asm__ volatile ("REPEAT #2\n\t"
"MAC %2*%3, A, [%0]+=2, %2, [%1]+=2, %3" :
/* outputs */ "+r"(ControlDifferencePtr),
"+r"(PIDCoefficientsPtr),
"+r"(x_prefetch),
"+r"(y_prefetch));
// Store result in ControlOutput with saturation
__asm__ volatile ("SAC A, %0" : "=r"(ControlOutput));
CORCONbits.SATA = 0; // Disable Saturation on Acc A
// Store last 2 errors
ControlDifference[2] = ControlDifference[1];
ControlDifference[1] = ControlDifference[0];
#ifndef CLOSED_LOOP
ControlOutput = RefSpeed;
#endif
// ControlOutput will determine the motor required direction
if (ControlOutput < 0)
Required_Direction = CCW;
else
Required_Direction = CW;
return;
}
#else
void SpeedControl_using_inline_asm (void) {
SFRAC16 *ControlDifferencePtr = ControlDifference;
SFRAC16 *PIDCoefficientsPtr = PIDCoefficients;
register SFRAC16 x_prefetch asm("w4");
register SFRAC16 y_prefetch;
CORCONbits.SATA = 1; // Enable Saturation on Acc A
// Calculate most recent error with saturation, no limit checking required
__asm__ volatile ("LAC %0, A" : /* no outputs */ : "r"(RefSpeed));
__asm__ volatile ("LAC %0, B" : /* no outputs */ : "r"(MeasuredSpeed));
__asm__ volatile ("SUB A");
__asm__ volatile ("SAC A, [%0]" : /* no outputs */ :
"r"(ControlDifferencePtr));
// Prepare MAC Operands
__asm__ volatile ("MOVSAC A, [%0]+=2, %2, [%1]+=2, %3" :
/* outputs */ "+x"(ControlDifferencePtr),
"+y"(PIDCoefficientsPtr),
"=z"(x_prefetch),
"=z"(y_prefetch));
__asm__ volatile ("LAC %0, A" : /* no outpus */ : "r"(ControlOutput)); // Load Acc with last output
// Perform MAC
__asm__ volatile ("REPEAT #2\n\t"
"MAC %2*%3, A, [%0]+=2, %2, [%1]+=2, %3" :
/* outputs */ "+x"(ControlDifferencePtr),
"+y"(PIDCoefficientsPtr),
"+z"(x_prefetch),
"+z"(y_prefetch));
// Store result in ControlOutput with saturation
__asm__ volatile ("SAC A, %0" : "=r"(ControlOutput));
CORCONbits.SATA = 0; // Disable Saturation on Acc A
// Store last 2 errors
ControlDifference[2] = ControlDifference[1];
ControlDifference[1] = ControlDifference[0];
// If CLOSED_LOOP is undefined (running open loop) overide ControlOutput
// with value read from the external potentiometer
#ifndef CLOSED_LOOP
ControlOutput = RefSpeed;
#endif
// ControlOutput will determine the motor required direction
if (ControlOutput < 0)
Required_Direction = CCW;
else
Required_Direction = CW;
return;
}
void SpeedControl(void) {
SFRAC16 *ControlDifferencePtr = ControlDifference;
SFRAC16 *PIDCoefficientsPtr = PIDCoefficients;
SFRAC16 x_prefetch;
SFRAC16 y_prefetch;
register int reg_a asm("A");
register int reg_b asm("B");
CORCONbits.SATA = 1; // Enable Saturation on Acc A
reg_a = __builtin_lac(RefSpeed,0);
reg_b = __builtin_lac(MeasuredSpeed,0);
reg_a = __builtin_subab();
*ControlDifferencePtr = __builtin_sac(reg_a,0);
reg_a = __builtin_movsac(&ControlDifferencePtr, &x_prefetch, 2,
&PIDCoefficientsPtr, &y_prefetch, 2, 0);
reg_a = __builtin_lac(ControlOutput, 0);
reg_a = __builtin_mac(x_prefetch,y_prefetch,
&ControlDifferencePtr, &x_prefetch, 2,
&PIDCoefficientsPtr, &y_prefetch, 2, 0);
reg_a = __builtin_mac(x_prefetch,y_prefetch,
&ControlDifferencePtr, &x_prefetch, 2,
&PIDCoefficientsPtr, &y_prefetch, 2, 0);
reg_a = __builtin_mac(x_prefetch,y_prefetch,
&ControlDifferencePtr, &x_prefetch, 2,
&PIDCoefficientsPtr, &y_prefetch, 2, 0);
ControlOutput = __builtin_sac(reg_a, 0);
CORCONbits.SATA = 0; // Disable Saturation on Acc A
// Store last 2 errors
ControlDifference[2] = ControlDifference[1];
ControlDifference[1] = ControlDifference[0];
// If CLOSED_LOOP is undefined (running open loop) overide ControlOutput
// with value read from the external potentiometer
#ifndef CLOSED_LOOP
ControlOutput = RefSpeed;
#endif
// ControlOutput will determine the motor required direction
if (ControlOutput < 0)
Required_Direction = CCW;
else
Required_Direction = CW;
return;
}
#endif
void ForceCommutation(void)
{
HallValue = (unsigned int)((PORTB >> 3) & 0x0007); // Read halls
Sector = SectorTable[HallValue]; // Read sector based on halls
if (Sector != -1) // If the sector is invalid don't do anything
{
// Depending on the required direction, a new phase is fetched
if (Required_Direction == CW)
{
// Motor is required to run forward, so read directly the table
Phase = PhaseValues[Sector];
}
else
{
// Motor is required to run reverse, so calculate new phase and
// add offset to compensate asymmetries
Phase = PhaseValues[(Sector + 3) % 6] + PhaseOffset;
}
}
return;
}
void InitADC10(void)
{
ADPCFG = 0x0038; // RB3, RB4, and RB5 are digital
ADCON1 = 0x036E; // PWM starts conversion
// Signed fractional conversions
ADCON2 = 0x0000;
ADCHS = 0x0002; // Pot is connected to AN2
ADCON3 = 0x0003;
IFS0bits.ADIF = 0; // Clear ISR flag
IEC0bits.ADIE = 1; // Enable interrupts
ADCON1bits.ADON = 1; // turn ADC ON
return;
}
void InitMCPWM(void)
{
TRISE = 0x0100; // PWM pins as outputs, and FLTA as input
PTPER = (FCY/FPWM - 1) >> 1; // Compute Period based on CPU speed and
// required PWM frequency (see defines)
OVDCON = 0x0000; // Disable all PWM outputs.
DTCON1 = 0x0010; // ~1 us of dead time
PWMCON1 = 0x0077; // Enable PWM output pins and configure them as
// complementary mode
PDC1 = PTPER; // Initialize as 0 voltage
PDC2 = PTPER; // Initialize as 0 voltage
PDC3 = PTPER; // Initialize as 0 voltage
SEVTCMP = 1; // Enable triggering for ADC
PWMCON2 = 0x0F02; // 16 postscale values, for achieving 20 kHz
PTCON = 0x8002; // start PWM as center aligned mode
return;
}
void InitICandCN(void)
{
//Hall A -> CN5. Hall A is only used for commutation.
//Hall B -> IC7. Hall B is used for Speed measurement and commutation.
//Hall C -> IC8. Hall C is only used for commutation.
// Init Input change notification 5
TRISB |= 0x38; // Ensure that hall connections are inputs
CNPU1 = 0; // Disable all CN pull ups
CNEN1 = 0x20; // Enable CN5
IFS0bits.CNIF = 0; // Clear interrupt flag
// Init Input Capture 7
IC7CON = 0x0001; // Input capture every edge with interrupts and TMR3
IFS1bits.IC7IF = 0; // Clear interrupt flag
// Init Input Capture 8
IC8CON = 0x0001; // Input capture every edge with interrupts and TMR3
IFS1bits.IC8IF = 0; // Clear interrupt flag
return;
}
void InitTMR1(void)
{
T1CON = 0x0020; // internal Tcy/64 clock
TMR1 = 0;
PR1 = 313; // 1 ms interrupts for 20 MIPS
T1CONbits.TON = 1; // turn on timer 1
return;
}
void InitTMR3(void)
{
T3CON = 0x0020; // internal Tcy/64 clock
TMR3 = 0;
PR3 = 0xFFFF;
T3CONbits.TON = 1; // turn on timer 3
return;
}
void InitUserInt(void)
{
TRISC |= 0x4000; // S2/RC14 as input
// Analog pin for POT already initialized in ADC init subroutine
PORTF = 0x0008; // RS232 Initial values
TRISF = 0xFFF7; // TX as output
return;
}