Sinusoidal_6015.c

//---------------------------------------------------------------------

//

//               Software License Agreement

//

// The software supplied herewith by Microchip Technology Incorporated 

// (the ?Company?) for its PICmicro® Microcontroller is intended and 

// supplied to you, the Company?s customer, for use solely and 

// exclusively on Microchip PICmicro Microcontroller products. The 

// software is owned by the Company and/or its supplier, and is 

// protected under applicable copyright laws. All rights are reserved. 

//  Any use in violation of the foregoing restrictions may subject the 

// user to criminal sanctions under applicable laws, as well as to 

// civil liability for the breach of the terms and conditions of this 

// license.

//

// THIS SOFTWARE IS PROVIDED IN AN ?AS IS? CONDITION. NO WARRANTIES, 

// WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED 

// TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 

// PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. THE COMPANY SHALL NOT, 

// IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL OR 

// CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.

//

//---------------------------------------------------------------------

//  File:    sinusoidal_6015.c

//

//  Written By:  Jorge Zambada, Microchip Technology

// 

// The following files should be included in the MPLAB project:

//

//    sinusoidal_6015.c  -- Main source code file

//    SVM.c          -- Space Vector Modulation file

//    SVM.h            

//    p30f6015.gld      -- Linker script file

//---------------------------------------------------------------------

#include "p30f6015.h"

#include "svm.h"

//--------------------------Device Configuration------------------------ 

_FOSC(FRC_PLL8 & CSW_FSCM_OFF);

_FWDT(WDT_OFF);

_FBORPOR(PBOR_OFF & BORV27 & PWRT_16 & MCLR_DIS & PWMxH_ACT_HI & PWMxL_ACT_HI & RST_IOPIN);

_FGS(GSS_OFF & GWRP_OFF);

_FBS(NO_BOOT_RAM & NO_BOOT_EEPROM & NO_BOOT_CODE & WR_PROTECT_BOOT_OFF);

_FSS(NO_SEC_RAM & NO_SEC_EEPROM & NO_SEC_CODE & WR_PROT_SEC_OFF);

_ICD(ICS_PGD);

//----------------------------------------------------------------------

//   Hurst Motor Terminals | MC LV PICDEM Board Connection

// -----------------------|---------------------------------

//  Ground Phase ---------|-- G

//  Phase Red    ---------|-- M1

//  Phase Black  ---------|-- M2

//  Phase White  ---------|-- M3

//  Hall White   ---------|-- HA

//  Hall Brown   ---------|-- HB

//  Hall Green   ---------|-- HC

typedef signed int SFRAC16;  

//#define CLOSED_LOOP      // if defined the speed controller will be enabled //dis 29.10.2023

#define  PHASE_ADVANCE    // for extended speed ranges this should be defined

#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

                         // transitions and Forcing steps according to the

                         // actual position of the motor

#define _100MILLISEC 1000 // after this time has elapsed, the motor is

                         // consider stalled and it's stopped

// 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   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 60  // This value represents the maximum allowed phase

                           // advance in electrical degrees. Set a value from

                           // 0 to 60. This value will be used to calculate

                           // phase advance only if PHASE_ADVANCE is defined

// This is the calculation from the required phase advance to the actual

// value to be multiplied by the speed of the motor. So, if PHASE_ADVANCE is

// enabled, a certain amount of shit angle will be added to the generated

// sine wave, up to a maximum of the specified value on MAX_PH_ADV_DEG. This

// maximum phase shift will be present when the MeasuredSpeed variable is a 

// fractional 1.0 (for CW) or -1.0 (for CCW).

#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                                                               //03.04.2024

#define SWITCH_S2  (!PORTCbits.RC14) // Push button S2

// 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) = 312.5

// RPM = 60 (Min Speed)

// Period = ((20,000,000 / 64) * 60) / (60 * 10) = 31250

#define MINPERIOD  156  // For 6000 max rpm and 10 poles motor // vorher 312 29.10.2023

#define MAXPERIOD  51250  // For 60 min rpm and 10 poles motor  //31250

// 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 InitADC10(void);  // Initialization of ADC used for Speed 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 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

// 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}; 

// 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 = 4100;

// 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

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)

signed int PhaseAdvance; // Used for extending motor speed range. This value

                         // is added directly to the parameters passed to the

                         // SVM function (the sine wave generation subroutine)

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

unsigned int MotorStalledCounter = 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};

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 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 

// a digital PID. See SpeedControl subroutine for details

SFRAC16 Kp = SFloat_To_SFrac16(0.1);   // P Gain  //0,1

SFRAC16 Ki = SFloat_To_SFrac16(0.01);  // I Gain   //0,01

SFRAC16 Kd = SFloat_To_SFrac16(0.000); // D Gain  //0,000

// 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)));

// Used as a temporal variable to perform a fractional divide operation in 

// assembly

SFRAC16 _MAX_PH_ADV = MAX_PH_ADV;

/*********************************************************************

  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.

  Input:           None.

  Output:          None.

  Side Effects:    None.

  Overview:        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;

  Period = ActualCapture - PastCapture;  // This is an UNsigned substraction

                                           // 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 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)

    //

  __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  -> 0.996805  -> 32663  -> 0x7F97

  // Min Speed CW  -> 60 RPM    -> 0.009984  -> 327    -> 0x0147

  // Min Speed CCW -> -60 RPM   -> -0.009984 -> -327   -> 0xFEB9

  // Max Speed CCW -> -6000 RPM -> -0.996805 -> -32663 -> 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.

#ifdef PHASE_ADVANCE

  // Calculate Phase Advance Based on Actual Speed and MAX_PH_ADV define

        // The following assembly instruction perform the following formula

        // using fractional multiplication:

        // 

        // PhaseAdvance = MAX_PH_ADV * MeasuredSpeed

        //

        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

//  MotorStalledCounter++;  // We increment a timeout variable to see if the////////////////////////04.03.2024

                            // 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 >= _100MILLISEC)

  {

    StopMotor(); // Stop motor is no hall changes have occured in 

                     // specified timeout

  }

  return;

}

/*********************************************************************

  Function:        void __attribute__((__interrupt__)) _CNInterrupt (void)

  PreCondition:    The inputs of the hall effect sensors should have low pass

                   filters. A simple RC network works.

  Input:           None.

  Output:          None.

  Side Effects:    None.

  Overview:        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 

                   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)) _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;

        // 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

    {

      // 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;

    }

    LastSector = Sector; // Update last sector

  }

  return;

}

/*********************************************************************

  Function:        void __attribute__((__interrupt__)) _IC7Interrupt (void)

  PreCondition:    The inputs of the hall effect sensors should have low pass

                   filters. A simple RC network works.

  Input:           None.

  Output:          None.

  Side Effects:    None.

  Overview:        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

  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;

    // 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 == 3) || (Sector == 0))

      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

    {

      // 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;

    }

    LastSector = Sector; // Update last sector

  }

  return;

}

/*********************************************************************

  Function:        void __attribute__((__interrupt__)) _IC8Interrupt (void)

  PreCondition:    The inputs of the hall effect sensors should have low pass

                   filters. A simple RC network works.

  Input:           None.

  Output:          None.

  Side Effects:    None.

  Overview:        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

  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

    {

      // 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;

    }

    LastSector = Sector; // Update last sector

  }

  return;

}

/*********************************************************************

  Function:        void __attribute__((__interrupt__)) _PWMInterrupt (void)

  PreCondition:    None.

  Input:           None.

  Output:          None.

  Side Effects:    None.

  Overview:        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.

                   If PHASE_ADVANCE is defined, a value corresponding to the

                   multiplication of the actual speed * maximum phase advance

                   is added to the sine wave phase to produce the phase shift

  Note:            None.

********************************************************************/

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);  // PhaseAdvance addition

                          // produces the sinewave

                          // phase shift

    #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;

}

/*********************************************************************

  Function:        void __attribute__((__interrupt__)) _ADCInterrupt (void)

  PreCondition:    None.

  Input:           None.

  Output:          None.

  Side Effects:    None.

  Overview:        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:            None.

********************************************************************/

void __attribute__((interrupt, no_auto_psv)) _ADCInterrupt (void)

{

  IFS0bits.ADIF = 0;  // Clear interrupt flag

  RefSpeed = ADCBUF0/2;  //(ADCBUF0/2)+0x8000; // Read POT value to set Reference Speed                      //03.04.2024

    RefSpeed = RefSpeed + 0x8000;                                                           //rückwärts 06.05.2024

    /*

    __asm ("push W0");

    __asm ("push W1");

    __asm ("mov #0xffc0,W0");

    __asm ("mov _RefSpeed,W1");

    __asm ("com  W1,W0") ;

    __asm ("and  W1,W0,W1") ;

    __asm ("mov W1, _RefSpeed") ;

    __asm ("pop W0");

    __asm ("pop W1");

    RefSpeed = RefSpeed + 0x8000;

     */

  return;

}

/*********************************************************************

  Function:        int main(void)

  PreCondition:    None.

  Input:           None.

  Output:          None.

  Side Effects:    None.

  Overview:        main function of the application. Peripherals are 

                   initialized, and then, depending on the motor status 

                   (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:            None.

********************************************************************/

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, 500 ns 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;

}

/*********************************************************************

  Function:        void ChargeBootstraps(void)

  PreCondition:    None.

  Input:           None.

  Output:          None.

  Side Effects:    None.

  Overview:        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:            None.

********************************************************************/

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;

}

/*********************************************************************

  Function:        void RunMotor(void)

  PreCondition:    None.

  Input:           None.

  Output:          None.

  Side Effects:    None.

  Overview:        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:            None.

********************************************************************/

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;

  // Initialize direction with required direction

  // Remember that ADC is not stopped.

  HallValue = (unsigned int)((PORTB >> 3) & 0x0007);  // Read halls

  LastSector = Sector = SectorTable[HallValue];  // 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

    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;  // 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.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;

}

/*********************************************************************

  Function:        void StopMotor(void)

  PreCondition:    None.

  Input:           None.

  Output:          None.

  Side Effects:    None.

  Overview:        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:            None.

********************************************************************/

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;

}

/*********************************************************************

  Function:        void SpeedControl(void)

  PreCondition:    None.

  Input:           None.

  Output:          None.

  Side Effects:    None.

  Overview:        This subroutine implements a PID in assembly using the MAC

                   instruction of the dsPIC.

  Note:            None.

********************************************************************/

/*

                                             ----   Proportional

                                            |    |  Output

                             ---------------| Kp |-----------------

                            |               |    |                 |

                            |                ----                  |

Reference                   |                                     --- 

Speed         ---           |           --------------  Integral | + | Control   -------

     --------| + |  Error   |          |      Ki      | Output   |   | Output   |       |

             |   |----------|----------| ------------ |----------|+  |----------| Plant |--

        -----| - |          |          |  1 - 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]

   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);

    reg_a = __builtin_movsac(&ControlDifferencePtr, &x_prefetch, 2,

         &PIDCoefficientsPtr, &y_prefetch, 2, 0, reg_b);

    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

        #ifndef CLOSED_LOOP

                ControlOutput = RefSpeed;

        #endif

        // ControlOutput will determine the motor required direction

        if (ControlOutput < 0)

                Required_Direction = CCW;

        else

                Required_Direction = CW;

        return;

}

/*********************************************************************

  Function:        void ForceCommutation(void)

  PreCondition:    None.

  Input:           None.

  Output:          None.

  Side Effects:    None.

  Overview:        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:            None.

********************************************************************/

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;

}

/*********************************************************************

  Function:        void InitADC10(void)

  PreCondition:    None.

  Input:           None.

  Output:          None.

  Side Effects:    None.

  Overview:        Below is the code required to setup the ADC registers for:

                 1. 1 channel conversion (in this case RB2/AN2)

                   2. PWM trigger starts conversion

                   3. Pot is connected to CH0 and RB2/AN2

                   4. The data format will be signed fractional    

  Note:            None.

********************************************************************/

void InitADC10(void)

{

  ADPCFG = 0x0038;        // RB3, RB4, and RB5 are digital            //04.05.2024 vorher 0x2038

    ADCON3 = 0x0003;

    ADCON2 = 0x0404;

    ADCHS = 0;

    ADCON1 = 0x8066;

    ADCON1 = 0x0266;

    ADCSSL = 0x2004;

    IFS0bits.ADIF = 0;

    IEC0bits.ADIE = 1;        // Enable interrupts

    /*

  ADCON1 = 0x026E;        // PWM starts conversion                                //03.04.2024

                  // Signed fractional conversions

  ADCON2 = 0x000;                           

  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;

}