Diff: main.cpp

Revision:

22:715d351d0be7

Parent:

12:93fd84766578

Child:

26:492c7ab05e67

diff -r d9bda15377e2 -r 715d351d0be7 main.cpp
--- a/main.cpp	Mon Apr 09 07:06:04 2018 +0000
+++ b/main.cpp	Mon Apr 09 14:48:55 2018 +0000
@@ -19,11 +19,17 @@
 PI_Cntrl PI1(1,.01,Ts);
 // controller parameters etc.
 
-// 
+/*
 float B[3] ={1.515974984336e-3,3.031949968672e-3,1.515974984336e-3}; // 2nd order System low Damping, Ts =1/400
 // MATLAB:  w0=5*2*pi;D=.2;Ts=1/400;G=tf(w0^2,[1 2*D*w0 w0^2]);Gd=c2d(G,Ts,'tustin');format long;[n,d]=tfdata(Gd,'v')
 float A[2]={ -1.963052911280484,0.969116811217828};
 IIR_filter pt2(B,A,sizeof(B)/4,sizeof(A)/4);         // length of float is 4
+*/
+
+float w0 = 2.0f*pi*5.0f;
+float D = 0.2f;
+IIR_filter pt2(w0, D, Ts, 1.0f);
+
 GPA gpa1(1.0f, 200.0f,50, 5, 100, Ts, 1.0f,1.0f);       // init GPA, see references there
 // user defined functions
 void updateControllers(void);   // speed controller loop (via interrupt)
@@ -38,17 +44,30 @@
 // main program and control loop
 // -----------------------------------------------------------------------------
 int main()
-{
+{ 
     // for serial comm.
     pc.baud(2000000);
+    
+    gpa1.reset();
+    pt2.reset(0.0f);
+    
     // attach controller loop to timer interrupt
     ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = 400Hz;
 }
 
 void updateControllers(void)
-{
+{   
     inp = gpa1(inp,out);
-    out=pt2(inp);           // Test GPA with pt2 system
+    out = pt2(inp);           // Test GPA with pt2 system
+    
+    // u = Wobble(u, y) + Oexc;
+    // y = SysSecOrder(u);
+    
+    // desTorque = pi_w(omega_desired - omega + excWobble);
+    // inpWobble = desTorque;
+    // outWobble = omega;
+    // excWobble = Wobble(excWobble, outWobble);
+    
 }