TMS320F28388D: The interrupt for eqep only runs once, then it stops functioning

//#############################################################################
//
// FILE:   cm_common_config_c28x.c
//
// TITLE:  C28x Common Configurations to be used for the CM Side.
//
//! \addtogroup driver_example_list
//! <h1>C28x Common Configurations</h1>
//!
//! This example configures the GPIOs and Allocates the shared peripherals
//! according to the defines selected by the users.
//!
//
//#############################################################################
//
//
// $Copyright:
// Copyright (C) 2022 Texas Instruments Incorporated - http://www.ti.com
//
// Redistribution and use in source and binary forms, with or without 
// modification, are permitted provided that the following conditions 
// are met:
// 
//   Redistributions of source code must retain the above copyright 
//   notice, this list of conditions and the following disclaimer.
// 
//   Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the 
//   documentation and/or other materials provided with the   
//   distribution.
// 
//   Neither the name of Texas Instruments Incorporated nor the names of
//   its contributors may be used to endorse or promote products derived
//   from this software without specific prior written permission.
// 
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
// $
//#############################################################################

//
// Included Files
//
#include "IQmathLib.h"
#include "driverlib.h"
#include "device.h"
#include "pi.h"
#include "pid.h"
#include "math.h"

//
// Defines
//
#define TB_CLK    DEVICE_SYSCLK_FREQ / 2        // ePWM Time base clock is SYSCLK/2
#define PWM_CLK   5000U                      // PWM frequency as 5 kHz
#define PRD_VAL   (TB_CLK / (PWM_CLK * 2))  // Calculate value period value
#define UNIT_PERIOD  10000U // Specify the unit timeout period in
#define myEPWM1_BASE    EPWM1_BASE
#define myEQEP0_BASE    EQEP1_BASE
#define base1    EPWM1_BASE
#define base2   EPWM2_BASE
#define EPWM_TIMER_TBPRD    625U
#define EQEP1   EQEP1_BASE                                  //0x00005100U
#define EQEP2   EQEP2_BASE                                  //0x00005140U
#define INT_myEQEP0 INT_EQEP1
#define INT_myEQEP0_INTERRUPT_ACK_GROUP INTERRUPT_ACK_GROUP5
//Added 07/12
#ifdef __TMS320C28XX_CLA__
#pragma CODE_SECTION(PI_init,"Cla1Prog2");
#endif
#define IPC_CMD_READ_MEM   0x1001
#define IPC_CMD_RESP       0x2001
#define TEST_PASS          0x5555
#define TEST_FAIL          0xAAAA
// Base/max frequency is 10 kHz
#define BASE_FREQ       10000
// See Equation 5 in eqep_ex1_calculation.c
#define FREQ_SCALER_PR  (((DEVICE_SYSCLK_FREQ / 128) * 8) / (2 * BASE_FREQ))
#pragma DATA_SECTION(sendDataC28, "MSGRAM_CPU_TO_CM")
typedef struct
{
    uint32_t freqScalerPR;  // Parameter: Scaler converting 1/N cycles to a
                            // GLOBAL_Q freq (Q0) - independently with global Q
    uint32_t baseFreq;      // Parameter: Maximum freq

    _iq freqPR;             // Output: Freq in per-unit using capture unit
    int32_t freqHzPR;       // Output: Freq in Hz, measured using Capture unit

} FreqCal_Object;

typedef FreqCal_Object *FreqCal_Handle;


//
// Functions
//
void Board_init(void);                                      // setup for the board
void EPWM_init(uint32_t base);                              // setup for epwm
__interrupt void IPC_ISR1(void);
__interrupt void cpuTimer0ISR(void);                        // setup for cpu0 interrupt
__interrupt void cpuTimer1ISR(void);                        // setup for cpu1 interrupt
__interrupt void cpuTimer2ISR(void);                        // setup for cpu2 interrupt
void initCPUTimers(void);                                   // setup for cpu interrupt
void configCPUTimer(uint32_t cpuTimer, uint32_t period);    // setup for cpu interrupt
void setCPU(void);                                          // setup for cpu interrupt
void EQEP_init(uint32_t base);                              // setup for eqep
__interrupt void INT_myEQEP2_ISR(void);                     // setup for eqep
__interrupt void INT_myEQEP0_ISR(void);
void setGPIO(void);                                         // setup for GPIOs
void INTERRUPT_init(void);                                  // init interrupt
void FreqCal_calculate(FreqCal_Handle, uint32_t);
//__interrupt void IPC_ISR1(void);

//
//Global Variables
//
uint32_t oldcount = 0;          //eqep1, stores the previous position of the clock
int32_t freq_Right = 0;         //eqep1, measured freq
uint32_t count = 0;             //eqep1, check if eqep1 is operating
uint32_t oldcount2 = 0;         //eqep2, stores the previous position of the clock
int32_t freq_Left =0;           //eqep2, measured freq
uint32_t count2 = 0;            //eqep2, check if eqep2 is operating
uint32_t IPCCount1 = 0;
uint32_t cpuTimer0IntCount = 0; //cpuTimer0, check if cpuTimer0 is operating
uint32_t cpuTimer1IntCount = 0; //cpuTimer1, check if cpuTimer1 is operating
uint32_t cpuTimer2IntCount = 0; //cpuTimer2, check if cpuTimer2 is operating
uint32_t eqep1Count = 0;
uint32_t eqep2Count = 0;
int targetRPM_Left = 0;         //cpuTimer1, get the input RPM of the left wheel
int targetRPM_Right = 0;        //cpuTimer1, get the input RPM of the right wheel
uint16_t dutycycle_Left = 0;    //epwm, turn the input RPM into the dutycycle of pwm
uint16_t dutycycle_Right = 0;   //epwm, turn the input RPM into the dutycycle of pwm
const float32_t freq1 = 5000.0; //??
float32_t tmpLeft = 0;          //??
float32_t tmpRight = 0;         //??
float32_t kp=0.75;
float32_t ki=0.06;
float32_t kd=0.45;
PID_Obj handle_Left = {0, 0, 0, 0, 0, 0, 0, -20, 80};
PID_Obj handle_Right = {0, 0, 0, 0, 0, 0, 0, -20, 80};
int temp = 0;
FreqCal_Object freqLeft =
{
    FREQ_SCALER_PR,  // freqScalerPR
    BASE_FREQ,       // baseFreq
    0, 0,   // Initialize outputs to zero
};
FreqCal_Object freqRight =
{
    FREQ_SCALER_PR,  // freqScalerPR
    BASE_FREQ,       // baseFreq
    0, 0,   // Initialize outputs to zero
};

//Added 07/12
float sendDataC28[10];
uint32_t pass;
float receiveDataC28[9];

//vehicle Added 07/12
const float rw = 0.08; //radius of wheel
const float R = 0.17; //half distance of two wheel
float controlmatrix[2][2];
float inv_controlmatrix[2][2];

//parameters Added 07/12
float feedBackGainEpsilon = 1;
float feedBackGainB = 0.01;

float sideBysideOmegaGain = 15;

float v_max = 0.25;
float w_max = 0.5;
int tmp1 = 0;


typedef struct point
{
    float set_x[4];
    float set_y[4];
    float set_t[4];
    int point_number;

}via_point;
int path_type = 0;
const float RS_EPS = 1e-4;

void main(void)
{
    //
    // Initialize device clock and peripherals
    //
    Device_init();

    //
    // Boot CM core
    //
#ifdef _FLASH
    Device_bootCM(BOOTMODE_BOOT_TO_FLASH_SECTOR0);
#else
    Device_bootCM(BOOTMODE_BOOT_TO_S0RAM);
#endif

    //
    // Disable pin locks and enable internal pull-ups.
    //
    Device_initGPIO();

#ifdef ETHERNET
    //
    // Set up EnetCLK to use SYSPLL as the clock source and set the
    // clock divider to 2.
    //
    // This way we ensure that the PTP clock is 100 MHz. Note that this value
    // is not automatically/dynamically known to the CM core and hence it needs
    // to be made available to the CM side code beforehand.
    SysCtl_setEnetClk(SYSCTL_ENETCLKOUT_DIV_2, SYSCTL_SOURCE_SYSPLL);

    //
    // Configure the GPIOs for ETHERNET.
    //

    //
    // MDIO Signals
    //
    GPIO_setPinConfig(GPIO_105_ENET_MDIO_CLK);
    GPIO_setPinConfig(GPIO_106_ENET_MDIO_DATA);

    //
    // Use this only for RMII Mode
    //GPIO_setPinConfig(GPIO_73_ENET_RMII_CLK);
    //

    //
    //MII Signals
    //
    GPIO_setPinConfig(GPIO_109_ENET_MII_CRS);
    GPIO_setPinConfig(GPIO_110_ENET_MII_COL);

    GPIO_setPinConfig(GPIO_75_ENET_MII_TX_DATA0);
    GPIO_setPinConfig(GPIO_122_ENET_MII_TX_DATA1);
    GPIO_setPinConfig(GPIO_123_ENET_MII_TX_DATA2);
    GPIO_setPinConfig(GPIO_124_ENET_MII_TX_DATA3);

    //
    //Use this only if the TX Error pin has to be connected
    //GPIO_setPinConfig(GPIO_46_ENET_MII_TX_ERR);
    //

    GPIO_setPinConfig(GPIO_118_ENET_MII_TX_EN);

    GPIO_setPinConfig(GPIO_114_ENET_MII_RX_DATA0);
    GPIO_setPinConfig(GPIO_115_ENET_MII_RX_DATA1);
    GPIO_setPinConfig(GPIO_116_ENET_MII_RX_DATA2);
    GPIO_setPinConfig(GPIO_117_ENET_MII_RX_DATA3);
    GPIO_setPinConfig(GPIO_113_ENET_MII_RX_ERR);
    GPIO_setPinConfig(GPIO_112_ENET_MII_RX_DV);

    GPIO_setPinConfig(GPIO_44_ENET_MII_TX_CLK);
    GPIO_setPinConfig(GPIO_111_ENET_MII_RX_CLK);

    //
    //Power down pin to bring the external PHY out of Power down
    //
    GPIO_setDirectionMode(108, GPIO_DIR_MODE_OUT);
    GPIO_setPadConfig(108, GPIO_PIN_TYPE_PULLUP);
    GPIO_writePin(108,1);

    //
    //PHY Reset Pin to be driven High to bring external PHY out of Reset
    //

    GPIO_setDirectionMode(119, GPIO_DIR_MODE_OUT);
    GPIO_setPadConfig(119, GPIO_PIN_TYPE_PULLUP);
    GPIO_writePin(119,1);
#endif

#ifdef MCAN
    //
    // Setting the MCAN Clock.
    //
    SysCtl_setMCANClk(SYSCTL_MCANCLK_DIV_4);

    //
    // Configuring the GPIOs for MCAN.
    //
    GPIO_setPinConfig(DEVICE_GPIO_CFG_MCANRXA);
    GPIO_setPinConfig(DEVICE_GPIO_CFG_MCANTXA);
#endif

#ifdef CANA
    //
    // Configuring the GPIOs for CAN A.
    //
    GPIO_setPinConfig(DEVICE_GPIO_CFG_CANRXA);
    GPIO_setPinConfig(DEVICE_GPIO_CFG_CANTXA);

    //
    // Allocate Shared Peripheral CAN A to the CM Side.
    //
    SysCtl_allocateSharedPeripheral(SYSCTL_PALLOCATE_CAN_A,0x1U);
#endif

#ifdef CANB
    //
    // Configuring the GPIOs for CAN B.
    //
    GPIO_setPinConfig(DEVICE_GPIO_CFG_CANRXB);
    GPIO_setPinConfig(DEVICE_GPIO_CFG_CANTXB);

    //
    // Allocate Shared Peripheral CAN B to the CM Side.
    //
    SysCtl_allocateSharedPeripheral(SYSCTL_PALLOCATE_CAN_B,0x1U);
#endif

#ifdef UART
    //
    // Configure GPIO85 as the UART Rx pin.
    //
    GPIO_setPinConfig(GPIO_85_UARTA_RX);
    GPIO_setDirectionMode(85, GPIO_DIR_MODE_IN);
    GPIO_setPadConfig(85, GPIO_PIN_TYPE_STD);
    GPIO_setQualificationMode(85, GPIO_QUAL_ASYNC);

    //
    // Configure GPIO84 as the UART Tx pin.
    //
    GPIO_setPinConfig(GPIO_84_UARTA_TX);
    GPIO_setDirectionMode(84, GPIO_DIR_MODE_OUT);
    GPIO_setPadConfig(84, GPIO_PIN_TYPE_STD);
    GPIO_setQualificationMode(84, GPIO_QUAL_ASYNC);
#endif

#ifdef USB
#ifdef USE_20MHZ_XTAL
    //
    // Set up the auxiliary PLL so a 60 MHz output clock is provided to the USB module.
    // This fixed frequency is required for all USB operations.
    //
    SysCtl_setAuxClock(SYSCTL_AUXPLL_OSCSRC_XTAL |
                       SYSCTL_AUXPLL_IMULT(48) |
                       SYSCTL_REFDIV(2U) | SYSCTL_ODIV(4U) |
                       SYSCTL_AUXPLL_DIV_2 |
                       SYSCTL_AUXPLL_ENABLE |
                       SYSCTL_DCC_BASE_0);
#else
    //
    // Set up the auxiliary PLL so a 60 MHz output clock is provided to the USB module.
    // This fixed frequency is required for all USB operations.
    //
    SysCtl_setAuxClock(SYSCTL_AUXPLL_OSCSRC_XTAL |
                       SYSCTL_AUXPLL_IMULT(48) |
                       SYSCTL_REFDIV(2U) | SYSCTL_ODIV(5U) |
                       SYSCTL_AUXPLL_DIV_2 |
                       SYSCTL_AUXPLL_ENABLE |
                       SYSCTL_DCC_BASE_0);
#endif

    //
    // Allocate Shared Peripheral USB to the CM Side.
    //
    SysCtl_allocateSharedPeripheral(SYSCTL_PALLOCATE_USBA, 1);

    GPIO_setPinConfig(GPIO_0_GPIO0);
    GPIO_setPadConfig(0, GPIO_PIN_TYPE_STD);
    GPIO_setDirectionMode(0, GPIO_DIR_MODE_OUT);
    GPIO_setMasterCore(0, GPIO_CORE_CM);

    //
    // Set the master core of GPIOs to CM.
    //
    GPIO_setMasterCore(42, GPIO_CORE_CM);
    GPIO_setMasterCore(43, GPIO_CORE_CM);
    GPIO_setMasterCore(46, GPIO_CORE_CM);
    GPIO_setMasterCore(47, GPIO_CORE_CM);
    GPIO_setMasterCore(120, GPIO_CORE_CM);
    GPIO_setMasterCore(121, GPIO_CORE_CM);

    //
    // Set the USB DM and DP GPIOs.
    //
    GPIO_setAnalogMode(42, GPIO_ANALOG_ENABLED);
    GPIO_setAnalogMode(43, GPIO_ANALOG_ENABLED);

    //
    // Set the direction for VBUS and ID.
    //
    GPIO_setDirectionMode(46, GPIO_DIR_MODE_IN);
    GPIO_setDirectionMode(47, GPIO_DIR_MODE_IN);

    //
    // Configure the Power Fault.
    //
    GPIO_setMasterCore(120, GPIO_CORE_CM);
    GPIO_setDirectionMode(120, GPIO_DIR_MODE_IN);

    //
    // Configure the External Power Signal Enable.
    //
    GPIO_setMasterCore(121, GPIO_CORE_CM);
    GPIO_setDirectionMode(121, GPIO_DIR_MODE_OUT);
    GPIO_writePin(121, 1);

    //
    // Set the CM Clock to run at 120MHz.
    // The CM Clock is a fractional multiple of the AUXPLL Clock (120 Mhz) from
    // which the USB Clock (60 MHz) is derived.
    //
    SysCtl_setCMClk(SYSCTL_CMCLKOUT_DIV_1, SYSCTL_SOURCE_AUXPLL);
#endif

    //Modified
    Interrupt_initModule();
    Interrupt_initVectorTable();

    IPC_clearFlagLtoR(IPC_CPU1_L_CM_R, IPC_FLAG_ALL);
    IPC_registerInterrupt(IPC_CPU1_L_CM_R, IPC_INT1, IPC_ISR1);
    //IPC_sync(IPC_CPU1_L_CM_R, IPC_FLAG31);

    Board_init();

    EINT;
    ERTM;

    while(1);
}

void Board_init(void) {
    EALLOW;

    setGPIO();
    EQEP_init(EQEP1_BASE);
    EQEP_init(EQEP2_BASE);
    INTERRUPT_init();
    setCPU();
    EPWM_init(base1);
    EPWM_init(base2);

    EDIS;
}

void setGPIO(void) {
    //EQEP1
    GPIO_setPinConfig(GPIO_20_EQEP1_A);
    GPIO_setPadConfig(20, GPIO_PIN_TYPE_STD);
    GPIO_setPinConfig(GPIO_11_EQEP1_B);
    GPIO_setPadConfig(11, GPIO_PIN_TYPE_STD);

    //EQEP2
    GPIO_setPinConfig(GPIO_54_EQEP2_A);
    GPIO_setPadConfig(54, GPIO_PIN_TYPE_STD);
    GPIO_setPinConfig(GPIO_55_EQEP2_B);
    GPIO_setPadConfig(55, GPIO_PIN_TYPE_STD);

    //EPWM1
    GPIO_setPinConfig(GPIO_0_EPWM1A);
    GPIO_setPadConfig(0, GPIO_PIN_TYPE_STD);

    //EPWM2
    GPIO_setPinConfig(GPIO_2_EPWM2A);
    GPIO_setPadConfig(2, GPIO_PIN_TYPE_STD);

    //set GPIO1 for reverse switch left
    GPIO_setPinConfig(GPIO_1_GPIO1);
    GPIO_setPadConfig(1, GPIO_PIN_TYPE_STD);
    GPIO_setDirectionMode(1, GPIO_DIR_MODE_OUT);

    //set GPIO3 for reverse switch right
    GPIO_setPinConfig(GPIO_3_GPIO3);
    GPIO_setPadConfig(3, GPIO_PIN_TYPE_STD);
    GPIO_setDirectionMode(3, GPIO_DIR_MODE_OUT);

    //set GPIO8 for forward switch left
    GPIO_setPinConfig(GPIO_8_GPIO8);
    GPIO_setPadConfig(8, GPIO_PIN_TYPE_STD);
    GPIO_setDirectionMode(8, GPIO_DIR_MODE_OUT);

    //set GPIO9 for forward switch right
    GPIO_setPinConfig(GPIO_9_GPIO9);
    GPIO_setPadConfig(9, GPIO_PIN_TYPE_STD);
    GPIO_setDirectionMode(9, GPIO_DIR_MODE_OUT);
}

void EPWM_init(uint32_t base) {
    // setup TBCLK for EPWM
    EPWM_setTimeBasePeriod(base, EPWM_TIMER_TBPRD);
    EPWM_setPhaseShift(base, 0U);
    EPWM_setTimeBaseCounter(base, 0U);
    EPWM_setTimeBaseCounterMode(base, EPWM_COUNTER_MODE_UP_DOWN);
    EPWM_disablePhaseShiftLoad(base);

    // setup epwm prescalar
    EPWM_setClockPrescaler(base, EPWM_CLOCK_DIVIDER_4, EPWM_HSCLOCK_DIVIDER_4);

    // setup compare
    EPWM_setCounterCompareValue(base, EPWM_COUNTER_COMPARE_A, 1*EPWM_TIMER_TBPRD/100);

    // setup actions
    EPWM_setActionQualifierAction(base, EPWM_AQ_OUTPUT_A, EPWM_AQ_OUTPUT_LOW, EPWM_AQ_OUTPUT_ON_TIMEBASE_ZERO);
    EPWM_setActionQualifierAction(base, EPWM_AQ_OUTPUT_A, EPWM_AQ_OUTPUT_HIGH, EPWM_AQ_OUTPUT_ON_TIMEBASE_UP_CMPA);
    EPWM_setActionQualifierAction(base, EPWM_AQ_OUTPUT_A, EPWM_AQ_OUTPUT_NO_CHANGE, EPWM_AQ_OUTPUT_ON_TIMEBASE_PERIOD);
    EPWM_setActionQualifierAction(base, EPWM_AQ_OUTPUT_A, EPWM_AQ_OUTPUT_LOW, EPWM_AQ_OUTPUT_ON_TIMEBASE_DOWN_CMPA);
}

void EQEP_init(uint32_t base) {
    // disable, clear all flags and interrupt
    EQEP_disableInterrupt(base,
        (EQEP_INT_GLOBAL            |
        EQEP_INT_POS_CNT_ERROR      |
        EQEP_INT_PHASE_ERROR        |
        EQEP_INT_DIR_CHANGE         |
        EQEP_INT_WATCHDOG           |
        EQEP_INT_UNDERFLOW          |
        EQEP_INT_OVERFLOW           |
        EQEP_INT_POS_COMP_READY     |
        EQEP_INT_POS_COMP_MATCH     |
        EQEP_INT_STROBE_EVNT_LATCH  |
        EQEP_INT_INDEX_EVNT_LATCH   |
        EQEP_INT_UNIT_TIME_OUT      |
        EQEP_INT_QMA_ERROR));
    EQEP_clearInterruptStatus(base,
        (EQEP_INT_GLOBAL            |
        EQEP_INT_POS_CNT_ERROR      |
        EQEP_INT_PHASE_ERROR        |
        EQEP_INT_DIR_CHANGE         |
        EQEP_INT_WATCHDOG           |
        EQEP_INT_UNDERFLOW          |
        EQEP_INT_OVERFLOW           |
        EQEP_INT_POS_COMP_READY     |
        EQEP_INT_POS_COMP_MATCH     |
        EQEP_INT_STROBE_EVNT_LATCH  |
        EQEP_INT_INDEX_EVNT_LATCH   |
        EQEP_INT_UNIT_TIME_OUT      |
        EQEP_INT_QMA_ERROR));
    EQEP_SourceSelect source_myEQEP0 =
    {
        EQEP_SOURCE_DEVICE_PIN,         // eQEPA source
        EQEP_SOURCE_DEVICE_PIN,     // eQEPB source
        EQEP_SOURCE_DEVICE_PIN,     // eQEP Index source
    };

    // Selects the source for eQEPA/B/I signals
    EQEP_selectSource(base, source_myEQEP0);
    // Set the strobe input source of the eQEP module.
    EQEP_setStrobeSource(base,EQEP_STROBE_FROM_GPIO);
    // Sets the polarity of the eQEP module's input signals.
    EQEP_setInputPolarity(base,false,false,false,false);
    // Configures eQEP module's quadrature decoder unit.
    EQEP_setDecoderConfig(base, (EQEP_CONFIG_UP_COUNT | EQEP_CONFIG_2X_RESOLUTION | EQEP_CONFIG_NO_SWAP | EQEP_CONFIG_IGATE_DISABLE));
    // Set the emulation mode of the eQEP module.
    EQEP_setEmulationMode(base,EQEP_EMULATIONMODE_RUNFREE);
    // Configures eQEP module position counter unit.
    EQEP_setPositionCounterConfig(base,EQEP_POSITION_RESET_IDX,4294967295U);
    // Sets the current encoder position.
    EQEP_setPosition(base,0U);
    // Enables the eQEP module unit timer.
    EQEP_enableUnitTimer(base,2000000U);
    // Disables the eQEP module watchdog timer.
    EQEP_disableWatchdog(base);
    // Configures the quadrature modes in which the position count can be latched.
    EQEP_setLatchMode(base,(EQEP_LATCH_UNIT_TIME_OUT|EQEP_LATCH_RISING_STROBE|EQEP_LATCH_RISING_INDEX));
    // Set the quadrature mode adapter (QMA) module mode.
    EQEP_setQMAModuleMode(base,EQEP_QMA_MODE_BYPASS);
    // Disable Direction Change During Index
    EQEP_disableDirectionChangeDuringIndex(base);
    // Enables individual eQEP module interrupt sources.
    EQEP_enableInterrupt(base,(EQEP_INT_UNIT_TIME_OUT));
    // Configures the mode in which the position counter is initialized.
    EQEP_setPositionInitMode(base,(EQEP_INIT_DO_NOTHING));
    // Sets the software initialization of the encoder position counter.
    EQEP_setSWPositionInit(base,true);
    // Sets the init value for the encoder position counter.
    EQEP_setInitialPosition(base,0U);
    // Enables the eQEP module.
    EQEP_enableModule(base);
    // Configures eQEP module edge-capture unit.
    EQEP_setCaptureConfig(base,EQEP_CAPTURE_CLK_DIV_128,EQEP_UNIT_POS_EVNT_DIV_8);
    // Enables the eQEP module edge-capture unit.
    EQEP_enableCapture(base);
}

void INTERRUPT_init() {
    // Interrupt Setings for INT_myEQEP0
    Interrupt_register(INT_myEQEP0, &INT_myEQEP0_ISR);
    Interrupt_enable(INT_myEQEP0);

    // Interrupt Settings for INT_myEQEP2
    Interrupt_register(INT_EQEP2, &INT_myEQEP2_ISR);
    Interrupt_enable(INT_EQEP2);

    //Interrupt Setting for cpuTimer0ISR
    Interrupt_register(INT_TIMER0, &cpuTimer0ISR);
    Interrupt_enable(INT_TIMER0);

    //Interrupt Setting for cpuTimer1ISR
    Interrupt_register(INT_TIMER1, &cpuTimer1ISR);
    Interrupt_enable(INT_TIMER1);

    //Interrupt Setting for cpuTimer2ISR
    Interrupt_register(INT_TIMER2, &cpuTimer2ISR);
    Interrupt_enable(INT_TIMER2);
}

void setCPU() {
    initCPUTimers();

    cpuTimer0IntCount = 0;
    cpuTimer1IntCount = 0;
    cpuTimer2IntCount = 0;

    configCPUTimer(CPUTIMER0_BASE, 10000);
    configCPUTimer(CPUTIMER1_BASE, 10000);
    configCPUTimer(CPUTIMER2_BASE, 10000);

    CPUTimer_enableInterrupt(CPUTIMER0_BASE);
    CPUTimer_enableInterrupt(CPUTIMER1_BASE);
    CPUTimer_enableInterrupt(CPUTIMER2_BASE);

    CPUTimer_startTimer(CPUTIMER0_BASE);
    CPUTimer_startTimer(CPUTIMER1_BASE);
    CPUTimer_startTimer(CPUTIMER2_BASE);
}

void initCPUTimers(void) {
    CPUTimer_setPeriod(CPUTIMER0_BASE, 0xFFFFFFFF);
    CPUTimer_setPeriod(CPUTIMER1_BASE, 0xFFFFFFFF);
    CPUTimer_setPeriod(CPUTIMER2_BASE, 0xFFFFFFFF);

    CPUTimer_setPreScaler(CPUTIMER0_BASE, 0);
    CPUTimer_setPreScaler(CPUTIMER1_BASE, 0);
    CPUTimer_setPreScaler(CPUTIMER2_BASE, 0);

    CPUTimer_stopTimer(CPUTIMER0_BASE);
    CPUTimer_stopTimer(CPUTIMER1_BASE);
    CPUTimer_stopTimer(CPUTIMER2_BASE);

    CPUTimer_reloadTimerCounter(CPUTIMER0_BASE);
    CPUTimer_reloadTimerCounter(CPUTIMER1_BASE);
    CPUTimer_reloadTimerCounter(CPUTIMER2_BASE);

    cpuTimer0IntCount = 0;
    cpuTimer1IntCount = 0;
    cpuTimer2IntCount = 0;
}

void configCPUTimer(uint32_t cpuTimer, uint32_t period) {
    uint32_t temp, freq = DEVICE_SYSCLK_FREQ;

    //
    // Initialize timer period:
    //
    temp = ((freq / 1000000) * period);
    CPUTimer_setPeriod(cpuTimer, temp);

    //
    // Set pre-scale counter to divide by 1 (SYSCLKOUT):
    //
    CPUTimer_setPreScaler(cpuTimer, 0);

    //
    // Initializes timer control register. The timer is stopped, reloaded,
    // free run disabled, and interrupt enabled.
    // Additionally, the free and soft bits are set
    //
    CPUTimer_stopTimer(cpuTimer);
    CPUTimer_reloadTimerCounter(cpuTimer);
    CPUTimer_setEmulationMode(cpuTimer,
                              CPUTIMER_EMULATIONMODE_STOPAFTERNEXTDECREMENT);
    CPUTimer_enableInterrupt(cpuTimer);

    //
    // Resets interrupt counters for the three cpuTimers
    //
    if (cpuTimer == CPUTIMER0_BASE) {
        cpuTimer0IntCount = 0;
    }
    if (cpuTimer == CPUTIMER1_BASE) {
        cpuTimer1IntCount = 0;
    }
    if (cpuTimer == CPUTIMER2_BASE) {
        cpuTimer2IntCount = 0;
    }
}

__interrupt void INT_myEQEP0_ISR(void) {
    FreqCal_calculate(&freqLeft, EQEP1_BASE);
    eqep1Count++;
    EQEP_clearInterruptStatus(myEQEP0_BASE,EQEP_INT_UNIT_TIME_OUT|EQEP_INT_GLOBAL);
}

__interrupt void INT_myEQEP2_ISR(void) {
    FreqCal_calculate(&freqRight, EQEP2_BASE);
    eqep2Count++;
    EQEP_clearInterruptStatus(EQEP2,EQEP_INT_UNIT_TIME_OUT|EQEP_INT_GLOBAL);
}

__interrupt void cpuTimer0ISR(void) {
    /*
    //send what
    sendDataC28[0] = 0;
    sendDataC28[1] = 0;
    sendDataC28[2] = 0;
    sendDataC28[3] = 0;
    IPC_sendCommand(IPC_CPU1_L_CM_R, IPC_FLAG0, IPC_ADDR_CORRECTION_ENABLE,
                    IPC_CMD_READ_MEM, (uint32_t)sendDataC28, 10);

    IPC_waitForAck(IPC_CPU1_L_CM_R, IPC_FLAG0);
    if(IPC_getResponse(IPC_CPU1_L_CM_R) == TEST_PASS){
        pass = 1;
    }
    else{
        pass = 0;
    }
    */

    cpuTimer0IntCount++;
    Interrupt_clearACKGroup(INTERRUPT_ACK_GROUP1);
}

__interrupt void cpuTimer1ISR(void) {
    PID_setGains(&handle_Left, kp, ki, kd);
    PID_setGains(&handle_Right, kp, ki, kd);
    //step.1-1 turn rpm into dutycycle
    //available zone [Forward:10~80 RPM, Backward:6~20 RPM]
    if (targetRPM_Left<0){
        //PID_run_parallel [handle, DesireValue, FeedbackValue, FeedforwardValue, Pointer to the output]
        PID_run_parallel(&handle_Left, targetRPM_Left, -freq_Left/4, 0, &tmpLeft);
        dutycycle_Left = (1-(-tmpLeft*0.0165+0.1928))*100;
    }
    else{
        PID_run_parallel(&handle_Left, targetRPM_Left, freq_Left/4, 0, &tmpLeft);
        dutycycle_Left = (1-(tmpLeft*0.0041+0.2963))*100;
    }
    if (targetRPM_Right<0){
        PID_run_parallel(&handle_Right, targetRPM_Right, -freq_Right/4, 0, &tmpRight);
        dutycycle_Right = (1-(-tmpRight*0.0171+0.1928))*100;
    }
    else{
        PID_run_parallel(&handle_Right, targetRPM_Right, freq_Right/4, 0, &tmpRight);
        dutycycle_Right = (1-(tmpRight*0.0041+0.2996))*100;
    }

    if (dutycycle_Left>=70){
        EPWM_setCounterCompareValue(base1, EPWM_COUNTER_COMPARE_A, 99*EPWM_TIMER_TBPRD/100);
        PID_setUi(&handle_Left, 0);
    }
    else{
        EPWM_setCounterCompareValue(base1, EPWM_COUNTER_COMPARE_A, dutycycle_Left*EPWM_TIMER_TBPRD/100);
    }
    if (dutycycle_Right>=70){
        EPWM_setCounterCompareValue(base2, EPWM_COUNTER_COMPARE_A, 99*EPWM_TIMER_TBPRD/100);
        PID_setUi(&handle_Right, 0);
    }
    else{
        EPWM_setCounterCompareValue(base2, EPWM_COUNTER_COMPARE_A, dutycycle_Right*EPWM_TIMER_TBPRD/100);
    }
    //step.1-2 decide the direction
    //the direction for the left wheel
    if (targetRPM_Left>0){
        GPIO_writePin(8,1);
        GPIO_writePin(1,0);
    }
    else if (targetRPM_Left<0){
        GPIO_writePin(8,0);
        GPIO_writePin(1,1);
    }
    else{
        GPIO_writePin(8,0);
        GPIO_writePin(1,0);
    }
    //the direction for the right wheel
    if (targetRPM_Right>0){
        GPIO_writePin(9,1);
        GPIO_writePin(3,0);
    }
    else if (targetRPM_Right<0){
        GPIO_writePin(9,0);
        GPIO_writePin(3,1);
    }
    else{
        GPIO_writePin(9,0);
        GPIO_writePin(3,0);
    }

    //step.2 let f2838x generate the dutycycle we want

    cpuTimer1IntCount++;
    Interrupt_clearACKGroup(INTERRUPT_ACK_GROUP1);
}

__interrupt void cpuTimer2ISR(void) {
    cpuTimer2IntCount++;
    Interrupt_clearACKGroup(INTERRUPT_ACK_GROUP1);
}

__interrupt void IPC_ISR1() {
    uint32_t command, addr, data;
    int i;
    bool status = false;
    IPC_readCommand(IPC_CPU1_L_CM_R, IPC_FLAG1, IPC_ADDR_CORRECTION_ENABLE,
                    &command, &addr, &data);
    if(command == IPC_CMD_READ_MEM)
    {
        status = true;
        for(i=0; i<data; i++)
        {
            receiveDataC28[i] = *((float *)addr + i);
        }
    }
    if(status)
    {
        IPC_sendResponse(IPC_CPU1_L_CM_R, TEST_PASS);
    }
    else
    {
        IPC_sendResponse(IPC_CPU1_L_CM_R, TEST_FAIL);
    }
    IPC_ackFlagRtoL(IPC_CPU1_L_CM_R, IPC_FLAG1);
    //Interrupt_clearACKGroup(INTERRUPT_ACK_GROUP11);
    IPCCount1++;
}

void
FreqCal_calculate(FreqCal_Object *p, uint32_t base)
{
    uint32_t temp;
    //
    // **** Frequency calculation using eQEP capture counter ****
    //
    // Check for unit position event
    //
    if((EQEP_getStatus(base) & EQEP_STS_UNIT_POS_EVNT) != 0)
    {
        //
        // No capture overflow
        //
        if((EQEP_getStatus(base) & EQEP_STS_CAP_OVRFLW_ERROR) == 0)
        {
            temp = (uint32_t)EQEP_getCapturePeriodLatch(base);
        }
        else
        {
            //
            // Capture overflow, saturate the result
            //
            temp = 0xFFFF;
        }

        //
        // p->freqPR = X / [(t2 - t1) * 10kHz]
        //
        p->freqPR = _IQdiv(p->freqScalerPR, temp);
        temp = p->freqPR;

        if(temp > _IQ(1))
        {
            p->freqPR = _IQ(1);
        }
        else
        {
            p->freqPR = temp;
        }

        //
        // Q0 = Q0 * GLOBAL_Q => _IQXmpy(), X = GLOBAL_Q
        // p->freqHzPR = (p->freqPR) * 10kHz = X / (t2 - t1)
        //
        p->freqHzPR = _IQmpy(p->baseFreq, p->freqPR);

        //
        // Clear unit position event flag and overflow error flag
        //
        EQEP_clearStatus(base, (EQEP_STS_UNIT_POS_EVNT |
                                      EQEP_STS_CAP_OVRFLW_ERROR));
    }
    Interrupt_clearACKGroup(INT_myEQEP0_INTERRUPT_ACK_GROUP);
}

//#############################################################################
//
// FILE:   eqep_ex1_freq_cal.c
//
// TITLE:  Frequency Measurement Using eQEP
//
//! \addtogroup driver_example_list
//! <h1>Frequency Measurement Using eQEP</h1>
//!
//! This example will calculate the frequency of an input signal using the eQEP
//! module.  ePWM1A is configured to generate this input signal with a
//! frequency of 5 kHz. It will interrupt once every period and call the
//! frequency calculation function. This example uses the IQMath library to
//! simplify high-precision calculations.
//!
//! In addition to the main example file, the following files must be included
//! in this project:
//! - \b eqep_ex1_calculation.c - contains frequency calculation function
//! - \b eqep_ex1_calculation.h - includes initialization values for frequency
//!                               structure
//!
//! The configuration for this example is as follows
//! - Maximum frequency is configured to 10KHz (baseFreq)
//! - Minimum frequency is assumed at 50Hz for capture pre-scalar selection
//!
//! \b SPEED_FR: High Frequency Measurement is obtained by counting the
//! external input pulses for 10ms (unit timer set to 100Hz).
//! \f[ SPEED\_FR = \frac{Count\ Delta}{10ms} \f]
//!
//! \b SPEED_PR: Low Frequency Measurement is obtained by measuring time
//! period of input edges. Time measurement is averaged over 64 edges for
//! better results and the capture unit performs the time measurement using
//! pre-scaled SYSCLK.
//!
//! Note that the pre-scaler for capture unit clock is selected such that the
//! capture timer does not overflow at the required minimum frequency. This
//! example runs indefinitely until the user stops it.
//!
//! For more information about the frequency calculation see the comments at
//! the beginning of eqep_ex1_calculation.c and the XLS file provided with the
//! project, eqep_ex1_calculation.xls.
//!
//! \b External \b Connections \n
//! - Connect GPIO20/eQEP1A to GPIO0/ePWM1A
//!
//! \b Watch \b Variables \n
//! - \b freq.freqHzFR - Frequency measurement using position counter/unit
//!   time out
//! - \b freq.freqHzPR - Frequency measurement using capture unit
//!
//
//#############################################################################
//
//
// $Copyright:
// Copyright (C) 2022 Texas Instruments Incorporated - http://www.ti.com
//
// Redistribution and use in source and binary forms, with or without 
// modification, are permitted provided that the following conditions 
// are met:
// 
//   Redistributions of source code must retain the above copyright 
//   notice, this list of conditions and the following disclaimer.
// 
//   Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the 
//   documentation and/or other materials provided with the   
//   distribution.
// 
//   Neither the name of Texas Instruments Incorporated nor the names of
//   its contributors may be used to endorse or promote products derived
//   from this software without specific prior written permission.
// 
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
// $
//#############################################################################

//
// Included Files
//
#include "driverlib.h"
#include "device.h"
#include "IQmathLib.h"
#include "eqep_ex1_calculation.h"
#include "board.h"


//
// Defines
//
#define TB_CLK    DEVICE_SYSCLK_FREQ / 2       // Time base clock is SYSCLK/2
#define PWM_CLK   5000                      // We want to output at 5 kHz
#define PRD_VAL   (TB_CLK / (PWM_CLK * 2))  // Calculate value period value
                                            // for up-down count mode

// Base/max frequency is 10 kHz
#define BASE_FREQ       10000
// See Equation 5 in eqep_ex1_calculation.c
#define FREQ_SCALER_PR  (((DEVICE_SYSCLK_FREQ / 128) * 8) / (2 * BASE_FREQ))
// See Equation 2 in eqep_ex1_calculation.c
#define FREQ_SCALER_FR  ((BASE_FREQ * 2) / 100)

//
// Function Prototypes
//
void initEPWM(void);
__interrupt void epwmISR(void);

//
// Globals
//
FreqCal_Object freq =
{
    FREQ_SCALER_PR,  // freqScalerPR
    BASE_FREQ,       // baseFreq
    0, 0,   // Initialize outputs to zero
};
uint32_t count =0;  // counter to check measurement gets saturated
uint32_t pass=0, fail =0; // Pass or fail indicator

//
// Main
//
void main(void)
{
    //
    // Initialize device clock and peripherals
    //
    Device_init();
    
    //
    // Disable pin locks and enable internal pullups.
    //
    Device_initGPIO();

    //
    // Initialize PIE and clear PIE registers. Disables CPU interrupts.
    //
    Interrupt_initModule();

    //
    // Initialize the PIE vector table with pointers to the shell Interrupt
    // Service Routines (ISR).
    //
    Interrupt_initVectorTable();

    //
    // Board Initialization
    // Setup eQEP1, configuring the unit timer and quadrature capture units
    //
    Board_init();

    //
    // Initialize GPIOs for use as EPWM1A and EQEP1A
    //
    GPIO_setPinConfig(GPIO_0_EPWM1A);
    GPIO_setPadConfig(0, GPIO_PIN_TYPE_STD);

    //
    // Interrupts that are used in this example are re-mapped to ISR functions
    // found within this file.
    //
    Interrupt_register(INT_EPWM1, &epwmISR);

    //
    // Setup ePWM1 to generate a 5 kHz signal to be an input to the eQEP
    //
    initEPWM();

    //
    // Enable interrupts required for this example
    //
    Interrupt_enable(INT_EPWM1);

    //
    // Enable Global Interrupt (INTM) and realtime interrupt (DBGM)
    //
    EINT;
    ERTM;

    //
    // Setup eQEP1, configuring the unit timer and quadrature capture units
    //
   // initEQEP();  -- >change with Board_init(); soon

    //
    // Loop indefinitely
    //
    while(1)
    {
        ;
    }
}


//
// initEPWM - Function to configure ePWM1 to generate a 5 kHz signal.
//
void
initEPWM(void)
{
    //
    // Disable the ePWM time base clock before configuring the module
    //
    SysCtl_disablePeripheral(SYSCTL_PERIPH_CLK_TBCLKSYNC);

    //
    // Set phase shift to 0 and clear the time base counter
    //
    EPWM_setPhaseShift(EPWM1_BASE, 0);
    EPWM_setTimeBaseCounter(EPWM1_BASE, 0);

    //
    // Disable the shadow load; the load will be immediate instead
    //
    EPWM_disableCounterCompareShadowLoadMode(EPWM1_BASE,
                                             EPWM_COUNTER_COMPARE_A);
    EPWM_disableCounterCompareShadowLoadMode(EPWM1_BASE,
                                             EPWM_COUNTER_COMPARE_B);

    //
    // Set the compare A value to half the period value, compare B to 0
    //
    EPWM_setCounterCompareValue(EPWM1_BASE, EPWM_COUNTER_COMPARE_A, PRD_VAL/2);
    //
    // Set action qualifier behavior on compare A events
    // - EPWM1A --> 1 when CTR = CMPA and increasing
    // - EPWM1A --> 0 when CTR = CMPA and decreasing
    //
    EPWM_setActionQualifierAction(EPWM1_BASE, EPWM_AQ_OUTPUT_A,
                                  EPWM_AQ_OUTPUT_HIGH,
                                  EPWM_AQ_OUTPUT_ON_TIMEBASE_UP_CMPA);
    EPWM_setActionQualifierAction(EPWM1_BASE, EPWM_AQ_OUTPUT_A,
                                  EPWM_AQ_OUTPUT_LOW,
                                  EPWM_AQ_OUTPUT_ON_TIMEBASE_DOWN_CMPA);

    //
    // Configure EPWM1B to be complementary to EPWM1A
    //
    EPWM_setDeadBandDelayPolarity(EPWM1_BASE, EPWM_DB_FED,
                                  EPWM_DB_POLARITY_ACTIVE_LOW);
    EPWM_setDeadBandDelayMode(EPWM1_BASE, EPWM_DB_FED, true);
    EPWM_setDeadBandDelayMode(EPWM1_BASE, EPWM_DB_RED, true);

    //
    // Enable interrupt when the counter is equal to 0
    //
    EPWM_setInterruptSource(EPWM1_BASE, EPWM_INT_TBCTR_ZERO);
    EPWM_enableInterrupt(EPWM1_BASE);

    //
    // Interrupt on first event
    //
    EPWM_setInterruptEventCount(EPWM1_BASE, 1);

    //
    // Set the time base clock prescaler to /1
    //
    EPWM_setClockPrescaler(EPWM1_BASE, EPWM_CLOCK_DIVIDER_1,
                           EPWM_HSCLOCK_DIVIDER_1);

    //
    // Set the period value; don't shadow the register
    //
    EPWM_setPeriodLoadMode(EPWM1_BASE, EPWM_PERIOD_DIRECT_LOAD);
    EPWM_setTimeBasePeriod(EPWM1_BASE, PRD_VAL);

    //
    // Put the time base counter into up-down count mode
    //
    EPWM_setTimeBaseCounterMode(EPWM1_BASE, EPWM_COUNTER_MODE_UP_DOWN);

    //
    // Sync the ePWM time base clock
    //
    SysCtl_enablePeripheral(SYSCTL_PERIPH_CLK_TBCLKSYNC);
}

//
// ePWM1 ISR- interrupts once per ePWM period
//
__interrupt void
epwmISR(void)
{
    //
    // Checks for events and calculates frequency.
    //
    FreqCal_calculate(&freq);

    //
    // Clear interrupt flag and issue ACK
    //
    EPWM_clearEventTriggerInterruptFlag(EPWM1_BASE);
    Interrupt_clearACKGroup(INTERRUPT_ACK_GROUP3);
}

//
// End of File
//