# include<p30f4011.

h>
# include "LCDFUN_Normal.h"

_FOSC(CSW_FSCM_OFF & XT_PLL8);

_FWDT(WDT_OFF);

#define HallA _RD1

#define HallB _RB4
#define HallC _RB5

void Welcome(void);
void Init_Pwm(void);
void Init_Hall(void);
void __attribute__((__interrupt__)) _PWMInterrupt(void);

const int TABLE[257]= {0,804,1608,2410,3212,4011,4808,5602,6393,7179,

7962,8739,9512,10278,11039,11793,12539,13279,14010,
14732,15446,16151,16846,17530,18204,18868,19519,20159,
20787,21403,22005,22594,23170,23731,24279,24811,25329,
25832,26319,26790,27245,27683,28105,28510,28898,29268,
29621,29956,30273,30571,30852,31113,31356,31580,31785,
31971,32137,32285,32412,32521,32609,32678,32728,32757,
32767,32757,32728,32678,32609,32521,32412,32285,32137,
31971,31785,31580,31356,31113,30852,30571,30273,29956,
29621,29268,28898,28510,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
-804,-1608,-2410,-3212,-4011,-4808,-5602,-6393,-7179,
-7962,-8739,-9512,-10278,-11039,-11793,-12539,-13279,
-14010,-14732,-15446,-16151,-16846,-17530,-18204,-18868,
-19519,-20159,-20787,-21403,-22005,-22594,-23170,-23731,
-24279,-24811,-25329,-25832,-26319,-26790,-27245,-27683,
-28105,-28510,-28898,-29268,-29621,-29956,-30273,-30571,
-30852,-31113,-31356,-31580,-31785,-31971,-32137,-32285,
-32412,-32521,-32609,-32678,-32728,-32757,-32767,-32757,
-32728,-32678,-32609,-32521,-32412,-32285,-32137,-31971,
-31785,-31580,-31356,-31113,-30852,-30571,-30273,-29956,
-29621,-29268,-28898,-28510,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0};

struct{
unsigned int Calc;
unsigned int Run;
unsigned int Set;
unsigned int Set1;
unsigned int Cnt;
unsigned int Ov;
unsigned int OvR;
unsigned int PRpm;
unsigned int Acc;
unsigned Dir : 1;
}RPM;

typedef struct SIN

{
unsigned Index;
int Degree;
int Compare;
 int Value;
}SINE;

struct{
unsigned A : 1;
unsigned B : 1;
unsigned C : 1;
unsigned char Val;
unsigned char Prev;
}Hall;

SINE R = {0};
SINE Y = {0xAAAA,85,0};
SINE B = {0x5555,170,0};

unsigned int BLDC_PWM[9] =

{0x0000,0x0809,0x2024,0x2021,0x0212,0x0818,0x0206,0x0424,0x0121};
unsigned int Loop=0,MODE=1,flag=0,flag1=0;
unsigned int FREcnt,AMPLITUDE,a,b,counter,lcdcnt;
float kp=0.08,ki=0.03;
int lcd,error,feedback2,duty,prop,integral;
unsigned long int count=0,count1,count2;

void __attribute__((__interrupt__)) _IC2Interrupt(void)

{
IFS0bits.IC2IF = 0;
count = TMR1;
TMR1 = 0;
RPM.Run = (long)(1171875) / count;
}

void __attribute__((__interrupt__)) _T1Interrupt(void)

{
count1 = count1 + 1;
IFS0bits.T1IF = 0;
}

void __attribute__((__interrupt__)) _PWMInterrupt(void)

{
counter++;
if(counter > 5000)
{
counter = 0;
Call_pi();
}

Hall.A = HallA;
Hall.B = HallB;
Hall.C = HallC;
Hall.Val = (unsigned char)Hall.C * 4 +
(unsigned char)Hall.B * 2 + (unsigned char)Hall.A;

if(Loop == 1)
{
if(!MODE)
{
OVDCON = BLDC_PWM[Hall.Val];
 PDC1 = RPM.Set1 * (float)0.567;
PDC2 = RPM.Set1 * (float)0.567;
PDC3 = RPM.Set1 * (float)0.567;
}
if(MODE)
{
FREcnt = (RPM.Set * (float)0.218) ;
AMPLITUDE = (RPM.Set * (float)0.6);

if(R.Index >= 0xffff)R.Index = 0;

if(Y.Index >= 0xffff)Y.Index = 0;
if(B.Index >= 0xffff)B.Index = 0;

R.Index = R.Index + FREcnt;

R.Degree = (R.Index>>8) & 0xFF;
R.Value = ((long)AMPLITUDE * TABLE[R.Degree])>>15 ;

Y.Index = Y.Index + FREcnt;

Y.Degree = (Y.Index>>8) & 0xFF;
Y.Value = ((long)AMPLITUDE * TABLE[Y.Degree])>>15 ;

B.Index = B.Index + FREcnt;

B.Degree = (B.Index>>8) & 0xFF;
B.Value = ((long)AMPLITUDE * TABLE[B.Degree])>>15 ;

R.Compare = (1000 + R.Value);

Y.Compare = (1000 + Y.Value);
B.Compare = (1000 + B.Value);

PDC1 = R.Compare;
PDC2 = Y.Compare;
PDC3 = B.Compare;
}
}
else if(Loop == 2)
{
if(!MODE)
{
OVDCON = BLDC_PWM[Hall.Val];

PDC1 = duty;
PDC2 = duty;
PDC3 = duty;
}
if(MODE)
{
FREcnt = (RPM.Set * (float)0.218) ;
AMPLITUDE = (RPM.Set * (float)0.6);

if(R.Index >= 0xffff)R.Index = 0;

if(Y.Index >= 0xffff)Y.Index = 0;
if(B.Index >= 0xffff)B.Index = 0;

R.Index = R.Index + FREcnt;

R.Degree = (R.Index>>8) & 0xFF;
R.Value = ((long)AMPLITUDE * TABLE[R.Degree])>>15 ;
 Y.Index = Y.Index + FREcnt;
Y.Degree = (Y.Index>>8) & 0xFF;
Y.Value = ((long)AMPLITUDE * TABLE[Y.Degree])>>15 ;

B.Index = B.Index + FREcnt;

B.Degree = (B.Index>>8) & 0xFF;
B.Value = ((long)AMPLITUDE * TABLE[B.Degree])>>15 ;

R.Compare = (1000 + R.Value);

Y.Compare = (1000 + Y.Value);
B.Compare = (1000 + B.Value);

PDC1 = R.Compare;
PDC2 = Y.Compare;
PDC3 = B.Compare;
}
}
IFS2bits.PWMIF = 0;
}

int main()
{
RPM.Set = 5;
RPM.Set1 = 500;
Welcome();
Init_Pwm();
Init_Hall();
Init_Keys();
while(1)
{
if(flag == 0)
{
if(key1 == 0 && key2 == 1)
{
data_write("M.I 120 MODE ",0x80,16);
data_write(" ",0xC0,16);
delay(300);
flag = Loop = 1;
TMR1 = 0;
PR1 = 65535; //
838.85ms.
//T1CON = 0x8030;
IEC2bits.PWMIE = 1;
}
if(key2 == 0 && key1 == 1)
{
data_write("M.I 120 MODE ",0x80,16);
data_write(" ",0xC0,16);
delay(300);
flag = Loop = 2;
TMR1 = 0;
PR1 = 65535; //
838.85ms.
IEC2bits.PWMIE = 1;
}
}
else if(flag == 1)
 {
ReadK();
//RPM.Run = (long)(1171875) / count;
//if(count == 0) RPM.Run = 0;
lcdcnt++;
if(lcdcnt > 100)
{
lcdcnt = 0;
if(!MODE) LCDDisp_INT(RPM.Run,0xC6,4);
//LCDDisp_INT(count,0xCb,4);
}
if(MODE)LCDDisp_INT(RPM.Set,0xC0,4);
else LCDDisp_INT(RPM.Set1/35,0xC0,4);

}
else if(flag == 2)
{
ReadK();
//RPM.Run = (long)(1171875) / count;
//if(count == 0) RPM.Run = 0;
lcdcnt++;
if(lcdcnt > 100)
{
lcdcnt = 0;
if(!MODE) LCDDisp_INT(RPM.Run,0xC6,4);
}
if(MODE)LCDDisp_INT(RPM.Set,0xC0,4);
else LCDDisp_INT(RPM.Set1,0xC0,4);

}
}
}

void Welcome(void)
{
cmd_write();

data_write(" BLDC ",0xc0,16);

data_write(" SENSORLESS ",0x80,16);
delay(1300);

data_write(" 3 PHASE BLDC ",0x80,16);

data_write(" MOTOR CONTROL ",0xC0,16);
delay(1300);

data_write(" 1.OPEN LOOP ",0x80,16);

data_write(" 2.CLOSED LOP ",0xC0,16);
}

void Init_Hall(void)
{
ADPCFGbits.PCFG4 = 1;
ADPCFGbits.PCFG5 = 1;

TRISBbits.TRISB4 = 1;
TRISBbits.TRISB5 = 1;
TRISDbits.TRISD1 = 1;
 IC2CON = 0x0003;
IEC0bits.IC2IE = 1;
IEC0bits.T1IE = 1;
}

void Init_Pwm(void)
{
PTPER = 1000;
PTCON = 0x8002;
PDC1 = 0;
PDC2 = 0;
PDC3 = 0;
PWMCON1 = 0x0077;
}

void ReadK(void)
{
if(key3 == 1 && key4 == 0)
{
MODE = 0;
if(Loop == 1)
{
data_write("DUTY SPEED SLESS",0x80,16);
data_write(" ",0xC0,16);
}
else if(Loop == 2)
{
data_write("SET ACT SLESS",0x80,16);
data_write(" ",0xC0,16);
}
T1CON = 0x8030;
}
else if(key4 == 1 && key3 == 0)
{
if(Loop == 1)
{
data_write("M.I 120 MODE ",0x80,16);
data_write(" ",0xC0,16);
}
else if(Loop == 2)
{
data_write("M.I 120 MODE ",0x80,16);
data_write(" ",0xC0,16);
}
MODE = 1;

if(MODE)
{
if(key1 == 0 && key2 == 1)
{
if( RPM.Set < 1500 )
{
RPM.Set++;
}
}
if(key1 == 1 && key2 == 0)
 {
if( RPM.Set > 5 )
{
RPM.Set--;
}
}
PWMCON1 = 0x0077;
}
else if(!MODE)
{
if(key1 == 0 && key2 == 1)
{
if( RPM.Set1 < 3000 )
{
RPM.Set1++;
}
}
if(key1 == 1 && key2 == 0)
{
if( RPM.Set1 > 500 )
{
RPM.Set1--;
}
}
PWMCON1 = 0x0777;
}
}

void Call_pi()
{
error = RPM.Set1 - RPM.Run;
prop = error * (float)kp;
lcd = RPM.Run - feedback2;
integral = lcd * (float)ki;
duty = duty + prop + integral;
feedback2 = RPM.Run;
if(duty>1900) duty = 1900;
if(duty<200) duty = 200;
}