// Fosc = 10Mhz, Fcy = 8*10/4 = 20 Mhz, Tcy = 0.

05us
// Prescaler = 1:8, 1xung nhip timer ~ 0.05*8 = 0.4us, Ts=10ms =10.000us~25000N;
// doc ADC tu AN7, hien thi dong 1 LCD
// xuat pwm kenh OC5, hien thi Ton
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
float R= 2.41; // Ohm
float L= 15.03e-3; // H
float m= 3.02e-3; // kg
float g= 9.81; // m/s^2
float k= 17.31e-9;
float alpha = 2.48;
float beta = 2.92e-4;
float gamma = 0.48;
float x1k, x2k, x3k, uk, yk, zk;
float x1e, x2e, x3e, ue, ye, ze;
float dx1, dx2, dx3, du, dy, dz; //dx1= x1k-x1e,...
float A[3][3], B[3][2], C[2][3], D[2][2], X[3][1],Klqr[1][3];
float Lest[3][1];

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void main() {
float Channel, Ton, Pwm_Period1,i,round3;
char txt_pwm[14];
//
ADPCFG= 0xFFFF;
PORTB = 0xFF80;
TRISB = 0xFF70;
PORTD = 0xFFF0;
TRISD = 0xFFF0;
TRISBbits.TRISB7 = 1;
// LCD
Lcd_Custom_Config(&PORTB, 3,2,1,0, &PORTD, 0,2,1); // khoi tao
Lcd_Custom_Cmd(LCD_CURSOR_OFF);
// tham so cua mo hinh
x1e = 2e-2;
x2e = 0.;
x3e = 1.*m*g*pow(x1e,4) / k;
ze= 1.*x1e;
ue= 1.*R*x3e;
ye= 1.*gamma*x3e + beta*pow(x1e,-2.) + alpha;
// Ma tran trang thai mien lien tuc
A[1][2]= 1.;
A[2][1]= 4.*g*pow((g*m*R), 0.25) / pow((k*ue), 0.25);
A[2][3]= -1.*g*R/ue;
A[3][3]= -1.*R/L;
B[3][1]= 1./L;
C[1][1]= 1.;
C[2][1]= -2.*beta*pow((g*m*R),0.75)/pow((k*ue),0.75);
C[2][3]= 1.*gamma;
// Ma tran trang thai mien roi rac
A[1][1]= 1.0997; A[1][2]= 0.0103; A[1][3]= -0.0011;
A[2][1]= 20.2679; A[2][2]= 1.0997; A[2][3]= -0.1868;
 A[3][1]= 0. ; A[3][1]= 0. ; A[3][3]= 0.2012;
B[1][1]= -0.0003;
B[2][1]= -0.0761;
B[3][1]= 0.3315;

// Trang thai ban dau t=0;

uk=ue; du=0.;
x1k= x1e; x2k=x2e; x3k=x3e;
dx1= 0; dx2= 0; dx3= 0;
X[1][1]=0.; X[2][1]=0.; X[3][1]=0.;
// Controller
Klqr[1][1]= -281.1732;
Klqr[1][2]= -6.2607;
Klqr[1][3]= 1.0986;
// Prediect Estimator
Lest[1][1]= 0.3825;
Lest[2][1]= 48.5226;
Lest[3][1]= 100.2781;

// Pwm - duty: Ton

Channel =4;
Pwm_Period1 = Pwm_Init(1*1000, Channel, 1, 2); // PreScale Value = * 1
Ton = 2./ (3.54 * 2 / Pwm_Period1); // 2V
Pwm_Init(1*1000, Channel, 1, 2);
Pwm_Start(Channel);
Pwm_Set_Duty(Ton, Channel);
// FloatToStr(1.0000 * Pwm_Period1, txt_pwm);
// Lcd_Custom_Out (1,1, txt_pwm);
Pwm_Stop(Channel);
// Interrupt TMR1
IPC0 = IPC0 | 0x7000; // Interrupt priority level = 7
IFS0 = IFS0 & 0xFFF7; // Clear TMR1IF
TMR1 = 0x0000;
PR1 = 25000;
T1CON= 0x8010; // tre 10ms
delay_ms(1000); // tre de dam bao dien ap cap vao cuon hut = Ue,
// cho phep VXL dieu khien
IEC0 = IEC0 | 0x0008; // Enable Timer1 interrupts
// Endless loop
while(1) {
// Pwm_Set_Duty(Ton, Channel);
asm {
nop
nop
}
}
}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void InterruptTMR() {
unsigned char txt_interrupt[14];
unsigned int i;
// tinh toan zk = x1k
zk= 1. * beta /(yk - alpha - gamma * x3k) ; // zk^2
zk= pow(zk, 0.5);
dz= 0;//zk-ze; //zk-ze;
dx1= X[1][1]; dx2= X[2][1]; dx3= X[3][1];
x1k= dx1+x1e; x2k= dx2+x2e; x3k= dx3+x3e;
Lcd_Custom_Out(2, 9, "dz");
// uoc luong vector trang thai tai thoi diem k
/*
for (i=1; i++; i<4){
X[i][1]=A[1][i] * X[i][1] + A[1][i] * X[i][2] + A[1][i] * X[i][3];// Ax
X[i][1]=X[i][1] + B[i][1] * du;
X[i][1]=X[i][1] + Lest[i][1] * (dz - dx1); // C=[1 0 0]*X => CX = dx1
}
dx1= X[1][1]; dx2= X[2][1]; dx3= X[3][1];
x1k= dx1+x1e; x2k= dx2+x2e; x3k= dx3+x3e;
du= Klqr[1][1]*dx1 + Klqr[1][2]*dx1 + Klqr[1][3]*dx3;
uk= du + ue;
i = (int)(000.5 + 1000 * uk);
uk = 1e-3*i;
FloatToStr(uk,txt_interrupt);
Lcd_Custom_Out(2, 1, txt_interrupt);
*/
// x3k = uk*0.3315 + x3k*0.2012;
}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void Interrupt_Timer1() org 0x1A {
unsigned int i;
unsigned char txt_interrupt[14];
IFS0 = IFS0 & 0xFFF7; // Clear TMR1IF
// do dien ap tra ve tu SenSor
yk = Adc_Read(7);
yk = 5. / 1023. * yk;
i = (int)(000.5 + 1000 * yk);
yk = 1e-3*i;
FloatToStr(yk,txt_interrupt);
Lcd_Custom_Out(1, 1, txt_interrupt);
InterruptTMR();
}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++