/////////////////////////// 無刷直流電動機 24V seakeke@163.com 和我聯系//////////////////////////////////////////

#include "DSP281x_Device.h"     // DSP281x Headerfile Include File
#include "DSPMotor_Head.h"

#include "IQmathLib.h"
#include "BLDCMotor.h"
#include "parameter.h"

#include <math.h>
///// 函數聲明 ////////////////////////////////////////////////////////////
interrupt void MainISR(void);

///// 全局變量聲明
float32 SpeedRef = 0.20;           // Speed reference (pu)
float32 T = 0.001/ISR_FREQUENCY;   // Samping period (sec), see parameter.h

Uint32 VirtualTimer = 0;
Uint16 ILoopFlag = FALSE;
Uint16 SpeedLoopFlag = FALSE;
int16 DFuncDesired = 0x1A00;      // Desired duty cycle (Q15)=0.2

_iq CurrentSet = _IQ(0.3);

Uint16 IsrTicker = 0;
Uint16 BackTicker = 0;

int16 DlogCh1 = 0;
int16 DlogCh2 = 0;
int16 DlogCh3 = 0;
int16 DlogCh4 = 0;

// Instance PID regulator to regulate the DC-bus current and speed
PIDREG3 pid1_idc = PIDREG3_DEFAULTS;
PIDREG3 pid1_spd = PIDREG3_DEFAULTS;

// Instance a PWM driver instance
PWMGEN pwm1 = PWMGEN_DEFAULTS;

// Create an instance of the ADC driver
ADCVALS adc1 = ADCVALS_DEFAULTS;

// Instance a Hall effect driver
HALL3 hall1 = HALL3_DEFAULTS;

// Instance a ramp controller to smoothly ramp the frequency
RMPCNTL rc1 = RMPCNTL_DEFAULTS;

// Instance a RAMP2 Module
RMP2 rmp2 = RMP2_DEFAULTS;

// Instance a MOD6 Module
MOD6CNT mod1 = MOD6CNT_DEFAULTS;

// Instance a SPEED_PR Module
SPEED_MEAS_CAP speed1 = SPEED_MEAS_CAP_DEFAULTS;

// Create an instance of DATALOG Module
DLOG_4CH dlog = DLOG_4CH_DEFAULTS; 

////////////////////////// 主程序 //////////////////////////////////////////////

void main(void)
{

// Initialize System Control registers, PLL, WatchDog, Clocks to default state:
        // This function is found in the DSP281x_SysCtrl.c file.
	InitSysCtrl();

// HISPCP prescale register settings, normally it will be set to default values
    EALLOW;   // This is needed to write to EALLOW protected registers
    SysCtrlRegs.HISPCP.all = 0x0000;     // SYSCLKOUT/1 
    EDIS;   // This is needed to disable write to EALLOW protected registers 

// Disable and clear all CPU interrupts:
	DINT;
	IER = 0x0000;
	IFR = 0x0000;

// Initialize Pie Control Registers To Default State:
        // This function is found in the DSP281x_PieCtrl.c file.
	InitPieCtrl();

// Initialize the PIE Vector Table To a Known State:
        // This function is found in DSP281x_PieVect.c.
	// This function populates the PIE vector table with pointers
        // to the shell ISR functions found in DSP281x_DefaultIsr.c.
	InitPieVectTable();	
	
// User specific functions, Reassign vectors (optional), Enable Interrupts:
	
// Initialize EVA Timer 1/2:
    // Setup Timer 1/2 Registers (EV A)
    EvaRegs.GPTCONA.all = 0;

    // Set the Period for the GP timer 2
    EvaRegs.T2PR = SYSTEM_FREQUENCY*1000000*T;  // Perscaler X1 (T2), ISR period = T x 1
    
    // Clear the counter/compare Regs for GP timer 2
    EvaRegs.T2CNT = 0x0000;
    EvaRegs.T2CMPR = 0x0000;     

// Enable Period interrupt bits for GP timer 2
    EvaRegs.EVAIMRB.bit.T2PINT = 1;
    EvaRegs.EVAIFRB.bit.T2PINT = 1;

    // Count up, x1, internal clk, disable compare, use own period
    EvaRegs.T2CON.all = 0x1040;

// Reassign ISRs. 
        // Reassign the PIE vector for T2PINT to point to a different 
        // ISR then the shell routine found in DSP281x_DefaultIsr.c.
        // This is done if the user does not want to use the shell ISR routine
        // but instead wants to use their own ISR.
	
	EALLOW;	// This is needed to write to EALLOW protected registers
	PieVectTable.T2PINT = &MainISR;
	EDIS;   // This is needed to disable write to EALLOW protected registers

// Enable PIE group 3 interrupt 1 for T2PINT
    PieCtrlRegs.PIEIER3.all = M_INT1;

// Enable CPU INT3 for T2PINT:
	IER |= M_INT3;

// Initialize PWM module
    pwm1.PeriodMax = (SYSTEM_FREQUENCY/PWM_FREQUENCY)*1000;  // Asymmetric PWM
    pwm1.DutyFunc = DFuncDesired;                            // DutyFunc = Q15  
    pwm1.init(&pwm1);
    
// Initialize DATALOG module        
    dlog.iptr1 = &DlogCh1;  
    dlog.iptr2 = &DlogCh2;
    dlog.iptr3 = &DlogCh3;
    dlog.iptr4 = &DlogCh4;
    dlog.trig_value = 0x01;
    dlog.size = 0x400;
    dlog.prescalar = 1;
    dlog.init(&dlog);

// Initialize ADC module
    adc1.ChSelect = 0x6543;   // for DMC1500 and eZdsp2812/eZdsp2808 boards
	adc1.init(&adc1);

// Initialize the SPEED_PR module (150 MHz, N = 1 event period/rev)
 	speed1.InputSelect = 0;
 	speed1.BaseRpm = 120*(BASE_FREQ/P);
 	speed1.SpeedScaler = (Uint32)(ISR_FREQUENCY/(1*BASE_FREQ*0.001));

// Initialize RMPCNTL module
    rc1.RampDelayMax = 20;
    rc1.RampLowLimit = _IQ(0);
    rc1.RampHighLimit = _IQ(1);

// Initialize Hall module   
    hall1.DebounceAmount = 5;
    hall1.Revolutions = -10;
    hall1.init(&hall1);

// Initialize RMP2 module
	rmp2.Out = (int32)DFuncDesired;
	rmp2.Ramp2Delay = 0x00000050;
    rmp2.Ramp2Max = 0x00007FFF;
    rmp2.Ramp2Min = 0x0000000F;

// Initialize the PID_REG3 module for dc-bus current
    pid1_idc.Kp = _IQ(1);                  
	pid1_idc.Ki = _IQ(T/0.003);			
	pid1_idc.Kd = _IQ(0/T);					
 	pid1_idc.Kc = _IQ(0.2);
    pid1_idc.OutMax = _IQ(0.99);
    pid1_idc.OutMin = _IQ(0); 

// Initialize the PID_REG3 module for speed
    pid1_spd.Kp = _IQ(1);
	pid1_spd.Ki = _IQ(T/0.1);
	pid1_spd.Kd = _IQ(0/T);
 	pid1_spd.Kc = _IQ(0.2);
    pid1_spd.OutMax = _IQ(0.99);
    pid1_spd.OutMin = _IQ(0); 

// Enable global Interrupts and higher priority real-time debug events:
	EINT;   // Enable Global interrupt INTM
	ERTM;	// Enable Global realtime interrupt DBGM

// IDLE loop. Just sit and loop forever:	
	for(;;) BackTicker++;

} 	

interrupt void MainISR(void)
{

// Verifying the ISR
     IsrTicker++;

// ------------------------------------------------------------------------------
//    Call the ADC04U_DRV read function.
// ------------------------------------------------------------------------------
	  adc1.read(&adc1);

// ------------------------------------------------------------------------------
//    Connect inputs of the RMP module and call the Ramp control
//    calculation function.
// ------------------------------------------------------------------------------
      rc1.TargetValue = _IQ(SpeedRef);
      rc1.calc(&rc1);

// ------------------------------------------------------------------------------
//    Connect inputs of the HALL module and call the Hall sensor
//    read function.
// ------------------------------------------------------------------------------
      hall1.HallMapPointer = (int16)mod1.Counter; 
      hall1.read(&hall1);

// ------------------------------------------------------------------------------
//    Connect inputs of the MOD6 module and call the Modulo 6 counter
//    calculation function.
// ------------------------------------------------------------------------------
      mod1.TrigInput =(int32)hall1.CmtnTrigHall;
      mod1.Counter = (int32)hall1.HallMapPointer; 
      mod1.calc(&mod1);

// ------------------------------------------------------------------------------
//    Connect inputs of the RMP2 module and call the Ramp control 2
//    calculation function.
// ------------------------------------------------------------------------------
      rmp2.DesiredInput = (int32)DFuncDesired;
      rmp2.calc(&rmp2);

// ------------------------------------------------------------------------------
//    Connect inputs of the PID_REG3 module and call the PID speed controller
//    calculation function.
// ------------------------------------------------------------------------------  
      pid1_spd.Ref = rc1.SetpointValue;
      pid1_spd.Fdb = speed1.Speed;
      pid1_spd.calc(&pid1_spd);

// ------------------------------------------------------------------------------
//    Set the speed closed loop flag once the speed is built up to a desired value. 
// ------------------------------------------------------------------------------
      if (rc1.EqualFlag == 0x7FFFFFFF)  {
         SpeedLoopFlag = TRUE; 
         rc1.RampDelayMax = 300;  
      }

// ------------------------------------------------------------------------------
//    Connect inputs of the PWM_DRV module and call the PWM signal generation
//    update function.
// ------------------------------------------------------------------------------
   // Switch from fixed duty-cycle or controlled Speed duty-cycle by SpeedLoopFlag variable
      if (SpeedLoopFlag == FALSE)
         pwm1.DutyFunc = (int16)rmp2.Out;           // fixed duty-cycle
      else 
         pwm1.DutyFunc = (int16)_IQtoIQ15(pid1_spd.Out);   // controlled Speed duty-cycle

      pwm1.CmtnPointer = (int16)mod1.Counter;
      pwm1.update(&pwm1);

// ------------------------------------------------------------------------------
//    Connect inputs of the SPEED_PR module and call the speed calculation 
//    function.
// ------------------------------------------------------------------------------  
      if ((mod1.Counter == 5)&(hall1.CmtnTrigHall == 0x7FFF)) {
         speed1.TimeStamp = VirtualTimer;
         speed1.calc(speed1);
      }

// ------------------------------------------------------------------------------
//    Connect inputs of the DATALOG module 
// ------------------------------------------------------------------------------
      DlogCh1 = (int16)mod1.Counter; 
      DlogCh2 = (int16)mod1.TrigInput;
      DlogCh3 = (int16)_IQtoIQ15(pid1_spd.Ref);
      DlogCh4 = (int16)_IQtoIQ15(pid1_spd.Fdb);

// ------------------------------------------------------------------------------
//    Increase virtual timer and force 15 bit wrap around
// ------------------------------------------------------------------------------
	VirtualTimer++;
	VirtualTimer &= 0x00007FFF;

// ------------------------------------------------------------------------------
//    Call the DATALOG update function.
// ------------------------------------------------------------------------------
    dlog.update(&dlog);

// Enable more interrupts from this timer
	EvaRegs.EVAIMRB.bit.T2PINT = 1;
	
// Note: To be safe, use a mask value to write to the entire
	// EVAIFRB register.  Writing to one bit will cause a read-modify-write
	// operation that may have the result of writing 1's to clear 
	// bits other then those intended. 
    EvaRegs.EVAIFRB.all = BIT0;
	
// Acknowledge interrupt to recieve more interrupts from PIE group 3
	PieCtrlRegs.PIEACK.all |= PIEACK_GROUP3;
}


//===========================================================================
// No more.
//===========================================================================