#include "p30f3011.h" #include "svm3011 v1.2.h"

Transcription

1 // // File: sinusoidal BLDC v1.2.c // // Written By: Jorge Zambada, Microchip Technology // // The following files should be included in the MPLAB project: // // sinusoidalbldc v1.2.c -- Main source code file // SVM.c -- Space Vector Modulation file // SVM.h // p30f3011.gld -- Linker script file // // // Revision History // // July/5/ first version // November/18/ Review // Jun/2012 manual controll of volt and Phase(ccw-0-cw),-> svm(volt,angle) // August/ F3011 and 3 MODE's of test operation // // // Phase_Zero set by PotZ at AN1 P3, Press SWITCH_S2 to set it again. // //Switch // Mode 0: PotAI at AN3 P4 = Refspeed open loop. // // Mode 1: PotAI at AN3 P4 = Phase (0-360 ), PotV at AN3 P2 = Power // 'Brake' at "Hals" to see Hall positions at trace t view. // // Mode 2: PotAI at AN1 P2 = Refspeed closed loop, PotV at AN3 P2 = PhaseADD. // // #include "p30f3011.h" #include "svm3011 v1.2.h" // Device Configuration _FOSC(CSW_FSCM_OFF & XT_PLL8); //_FWDT(WDT_OFF); //_FBORPOR(PBOR_ON & BORV_20 & PWRT_64 & MCLR_EN); // typedef signed int SFRAC16;

2 #define CLOSED_LOOP // if defined the speed controller will be enabled #define FCY // xtal = 5Mhz; PLLx16 -> 20 MIPS #define FPWM // 20 khz, so that no audible noise is present. #define _10MILLISEC 1000 // Used as a timeout with no hall effect sensors // transitions and Forcing steps according to the // actual position of the motor #define _100MILLISEC // after this time has elapsed, the motor is // consider stalled and it's stopped // In the sinewave generation algorithm we need an offset to be added to the // pointer when energizing the motor in CCW. This is done to compensate an // asymetry of the sinewave int PhaseOffset = 10500; // These Phase values represent the base Phase value of the sinewave for each // one of the sectors (each sector is a translation of the hall effect sensors // reading //#define PHASE_ZERO //0xDFF2 //#define PHASE_ZERO //0xD548 //#define PHASE_ZERO //0xB552 //#define PHASE_ONE ((PHASE_ZERO /6 + 1) % 65536) //#define PHASE_TWO ((PHASE_ONE /6 + 1) % 65536) //#define PHASE_THREE ((PHASE_TWO /6) % 65536) //#define PHASE_FOUR ((PHASE_THREE /6 + 1) % 65536) //#define PHASE_FIVE ((PHASE_FOUR /6 + 1) % 65536) #define THREE_PHASE (65536/2) // Constants used for properly energizing the motor depending on the // rotor's position //const int PhaseValues[6] = //PHASE_ZERO, PHASE_ONE, PHASE_TWO, PHASE_THREE, PHASE_FOUR, PHASE_FIVE; unsigned int PHASE_ZERO; unsigned int PHASE_ONE; unsigned int PHASE_TWO; unsigned int PHASE_THREE; unsigned int PHASE_FOUR; unsigned int PHASE_FIVE; unsigned int PhaseValues[6];

3 #define HALLA 1 // Connected to RB4 IC7 P6 #define HALLB 2 // Connected to RD1 IC2 P18 #define HALLC 4 // Connected to RB5 IC8 P7 #define CW 0 // Counter Clock Wise direction #define CCW 1 // Clock Wise direction #define SWITCH_S2 (!PORTCbits.RC14) // P16 Push button S2 #define SW_M PORTD // mode switch #define SW_M_IO TRISDbits.TRISD0 // P23 Test mode sw position (out/in) #define off = 0 #define on = 1 char n=0; unsigned int t[50]; void trace(unsigned int); unsigned int pd; unsigned int pi; int PositionCounter = 0; // This is the maximum value that may be passed to the SVM // function without overmodulation. This limit is equivalent // to 0.866, which is sqrt(3)/2. #define VOLTS_LIMIT // minimum to hold motor in position ad zero speed #define VOLTS_minimum 4500 // Period Calculation // Period = (TMRClock * 60) / (RPM * Motor_Poles) // For example> // Motor_Poles = 10 // RPM = 6000 (Max Speed) // Period = ((20,000,000 / 64) * 60) / (6000 * 10) = // RPM = 60 (Min Speed) // Period = ((20,000,000 / 64) * 60) / (60 * 10) = #define MINPERIOD 313 // For 6000 max rpm and 10 poles motor //#define MAXPERIOD // For 60 min rpm and 10 poles motor //#define MAXPERIOD // For 60 min rpm and 10 poles motor #define MAXPERIOD // Use this MACRO when using floats to initialize signed 16-bit fractional // variables #define SFloat_To_SFrac16(Float_Value) \ ((Float_Value < 0.0)? (SFRAC16)(32768 * (Float_Value) - 0.5) \ : (SFRAC16)(32767 * (Float_Value) + 0.5)) void ICD(void); void InitADC(void); // Initialization of ADC used for Speed & power

4 Command void InitMCPWM(void); // Initialization for PWM at 20kHz, Center aligned, // Complementary mode with 500 ns of deadtime void InitTMR1(void); // Initialization for TIMER1 used for speed control // and motor stalled protection void InitTMR3(void); // Initialization for TIMER3 used as a timebase // for the two input capture channels void InitUserInt(void); // This function initializes all ports // (inputs and outputs) for the application void InitICandCN(void); // Initializes input captures and change notification, // used for the hall sensor inputs void RunMotor(void); // This function initializes all variables // and interrupts used for starting and running // the motor void StopMotor(void); // This function clears all flags, and stops anything // related to motor control, and also disables PWMs void SpeedControl(void); // This function contains all ASM and C operations // for doing the PID Control loop for the speed void Halls(void); // Commen Hall funtion void ForceCommutation(void); // When motor is to slow to generate interrupts // on halls, this function forces a commutation void ChargeBootstraps(void); // At the begining of the motor operation, the // bootstrap caps are charged with this function // Flags used for the application struct unsigned MotorRunning :1; // This bit is 1 if motor running unsigned unused :15; Flags; unsigned int Phase; // This variable is incremented by the PWM interrupt // in order to generate a proper sinewave. Its value // is incremented by a value of PhaseInc, which // represents the frequency of the generated sinewave unsigned int PastPhase; signed int PhaseInc; // Delta increments of the Phase variable, calculated // in the TIMER1 interrupt (each 1 ms) and used in // the PWM interrupt (each 50 us) unsigned int HallValue; // This variable holds the hall sensor input readings unsigned int Sector; // This variables holds present sector value, which is // the rotor position unsigned int LastSector; // This variable holds the last sector value. This // is critical to filter slow slew rate on the Hall // effect sensors hardware

5 unsigned int MotorSlowCounter = 0; // This variable gets incremented each // 1 ms, and is cleared everytime a new // sector is detected. Used for // ForceCommutation and MotorStalled // protection functions // This array translates the hall state value read from the digital I/O to the // proper sector. Hall values of 0 or 7 represent illegal values and therefore // return -1. char SectorTable[] = -1,4,2,3,0,5,1,-1; char Mode = 0; char ICI = 0; char ICN = 0; unsigned int PhaseD = 0; unsigned int PhaseDC = 0; unsigned int Hoek; unsigned int HoekDecm; unsigned char Current_Direction; // Current mechanical motor direction of // rotation Calculated in halls interrupts unsigned char Required_Direction; // Required mechanical motor direction of // rotation, will have the same sign as the // ControlOutput variable from the Speed // Controller // Variables containing the Period of half an electrical cycle, which is an // interrupt each edge of one of the hall sensor input unsigned int PastCapture, ActualCapture, Period; // Used as a temporal variable to perform a fractional divide operation in // assembly SFRAC16 _MINPERIOD = MINPERIOD - 1; SFRAC16 PotAI,PhaseAdd; int PotV; unsigned int PotZ; SFRAC16 MotorI; currenr I on AN1 // Motor SFRAC16 MeasuredSpeed, RefSpeed; // Actual and Desired speeds for the PID // controller, that will generate the error SFRAC16 ControlOutput = 0; // Controller output, used as a voltage output, // use its sign for the required direction // Absolute PID gains used by the controller. Position form implementation of

6 // a digital PID. See SpeedControl subroutine for details SFRAC16 Kp = SFloat_To_SFrac16(0.2); // P Gain SFRAC16 Ki = SFloat_To_SFrac16(0.02); // I Gain SFRAC16 Kd = SFloat_To_SFrac16(0.000); // D Gain // Constants used by the PID controller, since a MAC operation is used, the // PID structure is changed (See SpeedControl() Comments) SFRAC16 ControlDifference[3] \ attribute (( space (xmemory), aligned (4))); SFRAC16 PIDCoefficients[3] \ attribute (( space (ymemory), aligned (4))); Function: void attribute (( interrupt )) _T1Interrupt (void) PreCondition: The motor is running and is generating hall effect sensors interrupts. Also, the actual direction of the motor used in this interrupt is assumed to be previously calculated. In this ISR the Period, Phase Increment and MeasuredSpeed are calculated based on the input capture of one of the halls. The speed controller is also called in this ISR to generate a new output voltage (ControlOutput). The Phase Advance is calculated based on the maximum allowed phase advance (MAX_PH_ADV) and the actual speed of the motor. The last thing done in this ISR is the forced commutation, which happens each time the motor doesn't generate a new hall interrupt after a programmed period of time. If the timeout for generating hall ISR is too much (i.e. 100 ms) the motor is then stopped. Note: The MeasuredSpeed Calculation is made in assembly to take advantage of the signed fractional division. void attribute ((interrupt, no_auto_psv)) _T1Interrupt (void) IFS0bits.T1IF = 0; //PORTD = 0x08; // debug p19 Period = ActualCapture - PastCapture; // This is an UNsigned substraction

7 // to get the Period between one // hall effect sensor transition // These operations limit the Period value to a range from 60 to 6000 rpm 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 in the pointer. // builtin_divud(long Value, Int Value) is a function of the compiler // to do Long over Integer divisions. PhaseInc = builtin_divud(170667ul, Period); // Phase increment is used // pi = builtin_divud(512000ul, Period); // 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) // asm volatile("repeat #17\n\t" "divf %1,%2\n\t" "mov w0,%0" : /* output */ "=g"(measuredspeed) : /* input */ "r"(_minperiod), "e"(period) : /* clobber */ "w0"); // MeasuredSpeed sign adjustment based on current motor direction of // rotation if (Current_Direction == CCW) MeasuredSpeed = -MeasuredSpeed; // The following values represent the MeasuredSpeed values from the // previous operations: // // CONDITION RPM SFRAC16 SINT HEX // Max Speed CW -> 6000 RPM -> > > 0x7F97

8 // Min Speed CW -> 60 RPM -> > 327 -> 0x0147 // Min Speed CCW -> -60 RPM -> > > 0xFEB9 // Max Speed CCW -> RPM -> > > 0x8069 SpeedControl(); // Speed PID controller is called here. It will use // MeasuredSpeed, RefSpeed, some buffers and will generate // the new ControlOutput, which represents a new amplitude // of the sinewave that will be generated by the SVM // subroutine. MotorSlowCounter++; // 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 ((MotorSlowCounter % _10MILLISEC) == 0) ForceCommutation(); // Force Commutation if no hall sensor changes // have occured in specified timeout. else if (MotorSlowCounter >= _100MILLISEC) // StopMotor(); // Stop motor is no hall changes have occured in // specified timeout //PORTD &= 0x07; //debug p19 Function: void attribute (( interrupt )) _IC2Interrupt (void) PreCondition: The inputs of the hall effect sensors should have low pass filters. A simple RC network works. This interrupt represent Hall A ISR. Hall A -> RB3 -> CN5. This is generated by the input change notification CN5. The purpose of this ISR is to Calculate the actual

9 mechanical direction of rotation of the motor, and to adjust the Phase variable depending on the sector the rotor is in. Note 1: The sector is validated in order to avoid any spurious interrupt due to a slow slew rate on the halls inputs due to hardware filtering. Note 2: For Phase adjustment in CCW, an offset is added to compensate non-symetries in the sine table used. void attribute ((interrupt, no_auto_psv)) _IC2Interrupt (void) IFS0bits.IC2IF = 0; // Clear interrupt flag // Read halls HallValue = (unsigned int)(((portb >> 3) & 0x0006) ((PORTD >> 1) & 0x0001)); 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 = IC2BUF; IC2BUF; IC2BUF; IC2BUF; ICN = 2; // Motor current direction is computed based on Sector if ((Sector == 5) (Sector == 2)) Current_Direction = CCW; else Current_Direction = CW; Halls(); Function: void attribute (( interrupt )) _IC7Interrupt (void) PreCondition: The inputs of the hall effect sensors should have low pass filters. A simple RC network works.

10 This interrupt represent Hall B ISR. Hall B -> RB4 -> IC7. This is generated by the input Capture Channel IC7. The purpose of this ISR is to Calculate the actual Period between hall effect sensor transitions, calculate the actual mechanical direction of rotation of the motor, and also to adjust the Phase variable depending on the sector the rotor is in. Note 1: The sector is validated in order to avoid any spurious interrupt due to a slow slew rate on the halls inputs due to hardware filtering. Note 2: For Phase adjustment in CCW, an offset is added to compensate non-symetries in the sine table used. void attribute ((interrupt, no_auto_psv)) _IC7Interrupt (void) IFS1bits.IC7IF = 0; // Cleat interrupt flag // Read halls HallValue = (unsigned int)(((portb >> 3) & 0x0006) ((PORTD >> 1) & 0x0001)); 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; // Motor current direction is computed based on Sector if ((Sector == 3) (Sector == 0)) Current_Direction = CCW; else Current_Direction = CW;

11 ICN = 7; Halls(); Function: void attribute (( interrupt )) _IC8Interrupt (void) PreCondition: The inputs of the hall effect sensors should have low pass filters. A simple RC network works. This interrupt represent Hall C ISR. Hall C -> RB5 -> IC8. This is generated by the input Capture Channel IC8. The purpose of this ISR is to Calculate the actual mechanical direction of rotation of the motor, and to adjust the Phase variable depending on the sector the rotor is in. Note 1: The sector is validated in order to avoid any spurious interrupt due to a slow slew rate on the halls inputs due to hardware filtering. Note 2: For Phase adjustment in CCW, an offset is added to compensate non-symetries in the sine table used. void attribute ((interrupt, no_auto_psv)) _IC8Interrupt (void) IFS1bits.IC8IF = 0; // Cleat interrupt flag // Read halls HallValue = (unsigned int)(((portb >> 3) & 0x0006) ((PORTD >> 1) & 0x0001)); 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 = IC8BUF;

12 ICN = 8; IC8BUF; IC8BUF; IC8BUF; // Motor current direction is computed based on Sector if ((Sector == 1) (Sector == 4)) Current_Direction = CCW; else Current_Direction = CW; Halls(); Function: void attribute (( interrupt )) _PWMInterrupt (void) PreCondition: in this ISR the sinewave is generated. If the current motor direction of rotation is different from the required direction then the motor is operated in braking mode and step commutation is performed. Once both directions are equal then the sinewave is fed into the motor windings. Note: void attribute ((interrupt, no_auto_psv)) _PWMInterrupt (void) IFS2bits.PWMIF = 0; // Clear interrupt flag //PORTD = 0x04; //debug p22 if (Required_Direction == CW) if (Current_Direction == CW) Phase += PhaseInc; // Increment Phase if CW to generate the // sinewave only if both directions are equal

13 applied generate else // If Required_Direction is CW (forward) POSITIVE voltage is SVM(ControlOutput, Phase); if (Current_Direction == CCW) Phase -= PhaseInc; // Decrement Phase if CCW to // the sinewave only if both // directions are equal // If Required_Direction is CCW (reverse) NEGATIVE voltage is applied SVM(-(ControlOutput+1), Phase); //PORTD &= 0x0B; //debug p22 Function: void attribute (( interrupt )) _ADCInterrupt (void) PreCondition: The ADC interrupt loads the reference speed (RefSpeed) with the respective value of the POT. The value will be a signed fractional value, so it doesn't need any scaling. Note: void attribute ((interrupt, no_auto_psv)) _ADCInterrupt (void) IFS0bits.ADIF = 0; // Clear interrupt flag //PORTD = 0x02; //debug p18 PotAI = ADCBUF3; PotZ = ADCBUF2;

14 PotV = ADCBUF1; if (Mode < 1) //MODE 0 // Phase = Incremented by PotAI. // // RefSpeed = PotV. RefSpeed = PotAI; PhaseAdd = PotV / 16; else if (Mode < 2) // MODE 1 // Phase = potai. // // RefSpeed = PotV. Phase = PotAI; RefSpeed = PotV / 2; RefSpeed += 0x4000; PhaseAdd = 0; else // MODE 2 // Phase = Incremented by PotAI, // // RefSpeed = related to Angle Increment. RefSpeed = PotAI; PhaseAdd = PotV / 16; // PhaseAdd = 0; //PORTD &= 0x0D; //debug p18 Function: int main(void) PreCondition: main function of the application. Peripherals are initialized, and then, depending on the motor status

15 (running or stopped) and if the push button is pressed, the motor is started or stopped. All other operations and state machines are performed with interrupts. Note: int main(void) int D2; int xx; for(;;) InitUserInt(); // Initialize User Interface I/Os Mode = 0; InitADC(); // Initialize ADC to be signed fractional // set Sector Zero InitTMR1(); // Initialize TMR1 for 1 ms periodic ISR InitTMR3(); // Initialize TMR3 for timebase of capture InitICandCN(); // Initialize Hall sensor inputs ISRs InitMCPWM(); // Initialize 20 khz, center aligned, 500 ns of // deadtime RunMotor(); while (!SWITCH_S2) // 3 positions: off=0v, open, on=5v if (SW_M & 0x01) // Test Mode switch // = on SW_M_IO = 0; // set to output SW_M &= 0x0E; // force off SW_M_IO =1; // set to input xx = D2; // wait to setle xx = D2; // wait to setle xx = D2; // wait to setle xx = D2; // wait to setle if (SW_M & 0x01) // Read again // Still on = realy on Incremented by PotI, // RefSpeed = related to Angle Increment. if (Mode!= 2) Mode = 2; // Phase =

16 to off, thus = open Incremented by PotI. else // Changed if (Mode!= 0) Mode = 0; // Phase = // Refspeed = PotV. to on, thus = open Incremented by PotI. else // = off SW_M_IO =0; // set to output SW_M = 0x01; // force on SW_M_IO =1; // set to input xx = D2; // wait to setle xx = D2; // wait to setle xx = D2; // wait to setle xx = D2; // wait to setle if (SW_M & 0x01) // Read again // Changed if (Mode!= 0) Mode = 0; // Phase = // Refspeed = PotV. realy off return 0; StopMotor(); else // Still off = if (Mode!= 1) Mode = 1;

17 Function: void ChargeBootstraps(void) PreCondition: In the topology used, it is necessary to charge the bootstrap caps each time the motor is energized for the first time after an undetermined amount of time. ChargeBootstraps subroutine turns ON the lower transistors for 10 ms to ensure voltage on these caps, and then it transfers the control of the outputs to the PWM module. Note: 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; Function: PreCondition: void RunMotor(void)

18 Call this subroutine when first trying to run the motor and the motor is previously stopped. RunMotor will charge bootstrap caps, will initialize application variables, and will enable all ISRs. Note: 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] = -(Kp + 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; 0x0001)); // Initialize direction with required direction // Remember that ADC is not stopped. // Read halls HallValue = (unsigned int)(((portb >> 3) & 0x0006) ((PORTD >> 1) & LastSector = Sector = SectorTable[HallValue]; // variable // Initialize Sector // RefSpeed's sign will determine if the motor should be run at CW // (+RefSpeed) or CCW (-RefSpeed) ONLY at start up, since when the motor // has started, the required direction will be set by the control output // variable to be able to operate in the four quadrants if (RefSpeed < 0) ControlOutput = 0; // Initial output voltage

19 else Current_Direction = Required_Direction = CCW; Phase = PhaseValues[(Sector + 3) % 6] + PhaseOffset; ControlOutput = 0; // Initial output voltage Current_Direction = Required_Direction = CW; Phase = PhaseValues[Sector]; MotorSlowCounter = 0; // Reset motor stalled protection counter // Set initial Phase increment with minimum value. This will change if a // costing operation is required by the application PhaseInc = builtin_divud(512000ul, MAXPERIOD); // Clear all interrupts flags IFS0bits.T1IF = 0; // Clear timer 1 flag IFS0bits.IC2IF = 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.IC2IE = 1; // Enable interrupts on IC2 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; if (Mode!= 1) //debug trace(sector 0xCF00); trace (Phase); Hoek = Phase / 182; HoekDecm = Hoek / 100 << 8; Hoek = Hoek % 100; HoekDecm = Hoek / 10 << 4; HoekDecm = Hoek % 10; trace (HoekDecm); n=n; // Indicate that the motor is running

20 Function: PreCondition: void StopMotor(void) Call this subroutine whenever the user want to stop the motor. This subroutine will clear interrupts properly, and will also turn OFF all PWM channels. Note: 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.IC2IE = 0; // Disable interrupts on IC2 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 Function: void SpeedControl(void) PreCondition: This subroutine implements a PID in assembly using the MAC instruction of the dspic.

21 Note: /* ---- Proportional Output Kp Reference --- Speed Integral + Control Error Ki Output Output Plant Z^(-1) Measured Deriv Speed Output - Kd * (1 - Z^(-1)) ControlOutput(K) = ControlOutput(K-1) + ControlDifference(K) * (Kp + Ki + Kd) + ControlDifference(K-1) * (-Ki - 2*Kd) + ControlDifference(K-2) * Kd Using PIDCoefficients: PIDCoefficients[0] = Kp + Ki + Kd PIDCoefficients[1] = -(Kp + 2*Kd) PIDCoefficients[2] = Kd and leting: ControlOutput -> ControlOutput(K) and ControlOutput(K-1) ControlDifference[0] -> ControlDifference(K) ControlDifference[1] -> ControlDifference(K-1) ControlDifference[2] -> ControlDifference(K-2) ControlOutput = ControlOutput + ControlDifference[0] * PIDCoefficients[0] + ControlDifference[1] * PIDCoefficients[1] + ControlDifference[2] * PIDCoefficients[2]

22 This was implemented using Assembly with signed fractional and saturation enabled with MAC instruction */ 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 #if C30_VERSION == 320 #error "This Demo is not supported with v3.20" #endif #if C30_VERSION < 320 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); #else reg_a = builtin_lac(refspeed,0); reg_b = builtin_lac(measuredspeed,0); reg_a = builtin_subab(reg_a,reg_b); *ControlDifferencePtr = builtin_sac(reg_a,0);

23 reg_b); reg_a = builtin_movsac(&controldifferenceptr, &x_prefetch, 2, &PIDCoefficientsPtr, &y_prefetch, 2, 0, reg_a = builtin_lac(controloutput, 0); reg_a = builtin_mac(reg_a,x_prefetch,y_prefetch, &ControlDifferencePtr, &x_prefetch, 2, &PIDCoefficientsPtr, &y_prefetch, 2, 0, reg_b); reg_a = builtin_mac(reg_a,x_prefetch,y_prefetch, &ControlDifferencePtr, &x_prefetch, 2, &PIDCoefficientsPtr, &y_prefetch, 2, 0, reg_b); reg_a = builtin_mac(reg_a,x_prefetch,y_prefetch, &ControlDifferencePtr, &x_prefetch, 2, &PIDCoefficientsPtr, &y_prefetch, 2, 0, reg_b); ControlOutput = builtin_sac(reg_a, 0); #endif 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 if (Mode!= 2) ControlOutput = RefSpeed; // ControlOutput will determine the motor required direction if (ControlOutput < 0) Required_Direction = CCW; else Required_Direction = CW; //trace(phase); Function void Halls(voip) Overview Commen Hall funtion * void Halls(void) // Since a new sector is detected, clear variable that counts // the slow time

24 //trace (MotorSlowCounter); MotorSlowCounter = 0; if (Mode!= 2) //debug trace(sector 0xCc00); //debug trace(phase); PastPhase = Phase; Hoek = Phase / 182; HoekDecm = Hoek / 100 << 8; Hoek = Hoek % 100; HoekDecm = Hoek / 10 << 4; HoekDecm = Hoek % 10; trace (HoekDecm); if (Mode == 1) Required_Direction = Current_Direction; // Motor commutation is actually based on the required direction, not // the current dir. This allows driving the motor in four quadrants if (Mode!= 1) if (Required_Direction == CW) PositionCounter += 1; Phase = PhaseValues[Sector]; Phase += PhaseAdd; else PositionCounter -= 1; // For CCW an offset must be added to compensate difference in // symmetry of the sine table used for CW and CCW Phase = PhaseValues[(Sector + 3) % 6] + PhaseOffset; Phase -= PhaseAdd; if (Mode!= 2) //debug trace(phase); Hoek = Phase / 182; HoekDecm = Hoek / 100 << 8; Hoek = Hoek % 100; HoekDecm = Hoek / 10 << 4; HoekDecm = Hoek % 10; trace (HoekDecm); if (Current_Direction == 0) Hoek = (Phase - PastPhase + PhaseInc) / 182;

25 else Hoek = (PastPhase - Phase - PhaseInc) / 182; HoekDecm = Hoek / 100 << 8; Hoek = Hoek % 100; HoekDecm = Hoek / 10 << 4; HoekDecm = Hoek % 10; trace (HoekDecm); //debug ICD(); //debug LastSector = Sector; // Update last sector Function: void ForceCommutation(void) PreCondition: This function is called each time the motor doesn't generate hall change interrupt, which means that the motor running too slow or is stalled. If it is stalled, the motor is stopped, but if it is only slow, this function is called and forces a commutation based on the actual hall sensor position and the required direction of rotation. Note: void ForceCommutation(void) // Read halls HallValue = (unsigned int)(((portb >> 3) & 0x0006) ((PORTD >> 1) & 0x0001)); Sector = SectorTable[HallValue]; // Read sector based on halls if (Sector!= -1) // If the sector is invalid don't do anything table // Depending on the required direction, a new phase is fetched if (Required_Direction == CW) // Motor is required to run forward, so read directly the

26 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; Function: void InitADC(void) PreCondition: Below is the code required to setup the ADC registers for: void InitADC(void) unsigned int i; ADPCFG = 0x0030; ADCON1 = 0x036E; conversion // Open loop // RB4, RB5 are digital // PWM starts // Signed fractional conversions ADCON2 = 0x0200; // DD,SS, scan 0,1,2,3, int after 4conv ADCON3 = 0x000E; // Sample time 1Tad, conv clock 3Tcy ADCHS = 0x0006; // CH0 = AN6 = // CH1 = AN0 = PotV // CH2 = AN1 = PotZ

27 // CH3 = AN2 = PotAI ADCON1bits.ADON = 1; // turn ADC ON //read potz ADCON1 = 0x0002; // Sampel on for (i = 0; i < 33330; i++) // 10 ms Delay at 20 MIPs ; ADCON1 = 0xFFFD; // Sampel off while (!ADCON1bits.DONE); PotZ = ADCBUF2; PhaseValues[0] = PotZ; PhaseValues[1] = ((PhaseValues[0] /6 + 1) % 65536); PhaseValues[2] = ((PhaseValues[1] /6 + 1) % 65536); PhaseValues[3] = ((PhaseValues[2] /6) % 65536); PhaseValues[4] = ((PhaseValues[3] /6 + 1) % 65536); PhaseValues[5] = ((PhaseValues[4] /6 + 1) % 65536); IFS0bits.ADIF = 0; IEC0bits.ADIE = 1; // Clear ISR flag // Enable interrupts Function: void InitMCPWM(void) PreCondition: InitMCPWM, intializes the PWM as follows: 1. FPWM = hz 2. Complementary PWMs with center aligned 3. Set Duty Cycle to 0 for complementary, which is half the period 4. Set ADC to be triggered by PWM special trigger 5. Configure deadtime to be 500 ns Note:

28 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 them as // required PWM frequency (see defines) OVDCON = 0x0000; // Disable all PWM outputs. DTCON1 = 0x0008; // ~500 ns of dead time PWMCON1 = 0x0077; // Enable PWM output pins and configure // 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 Function: void InitICandCN(void) PreCondition: Configure Hall sensor inputs, one change notification and two input captures. on IC7 the actual capture value is used for further period calculation Note: void InitICandCN(void) //Hall A -> IC2. Hall A is used for Speed commutation. //Hall B -> IC7. Hall B is used for Speed measurement and commutation. //Hall C -> IC8. Hall C is only used for commutation. TRISB = 0x30; TRISD = 0x02; // Ensure that hall connections are inputs // Ensure that hall connections are inputs

29 // Init Input Capture 2 IC2CON = 0x0001; // Input capture every edge with interrupts and TMR3 IFS0bits.IC2IF = 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 Function: void InitTMR1(void) PreCondition: Initialization of timer 1 as a periodic interrupt each 1 ms for speed control, motor stalled protection which includes: forced commutation if the motor is too slow, or motor stopped if the motor is stalled. Note: 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

30 Function: PreCondition: void InitTMR3(void) Initialization of timer 3 as the timebase for the capture channels for calculating the period of the halls. Note: void InitTMR3(void) T3CON = 0x0020; // internal Tcy/64 clock TMR3 = 0; PR3 = 0xFFFF; T3CONbits.TON = 1; // turn on timer 3 Function: void InitUserInt(void) PreCondition: Initialization of the IOs used by the application. The IOs for the PWM and for the ADC are initialized in their respective peripheral initialization subroutine. Note: void InitUserInt(void) TRISC = 0x4000; // S2/RC14 as input

31 TRISD = 0x0000; //debug // Analog pin for POT already initialized in ADC init subroutine PORTF = 0x0008; // RS232 Initial values TRISF = 0xFFF7; // TX as output void ICD(void) Mode = Mode; Mode = Mode; void trace(unsigned int v) t[n] = v; if (v == 0) v = v; v = v; n += 1; if (n > 50) n=n; n = 0; // End of SinusoidalBLDC v1.2.c