gpa in double prescision

Dependencies:   mbed

Revision:
7:b3c5116e9fab
Parent:
6:7ea1a4bac3c2
Child:
8:3b98b6e1ead3
--- a/main.cpp	Thu Mar 01 17:47:55 2018 +0000
+++ b/main.cpp	Fri Mar 02 17:11:34 2018 +0000
@@ -41,35 +41,42 @@
  */
 
 Serial pc(SERIAL_TX, SERIAL_RX);        // serial connection via USB - programmer
-InterruptIn button(USER_BUTTON);        // User Button, short presses: reduce speed, long presses: increase speed
+InterruptIn Button(USER_BUTTON);        // User Button, short presses: reduce speed, long presses: increase speed
 AnalogIn ax(PA_0);                      // analog IN (acc x) on PA_0
 AnalogIn ay(PA_1);                      // analog IN (acc y) on PA_1
 AnalogIn gz(PA_4);                      // analog IN (gyr z) on PB_0
 AnalogOut out(PA_5);                    // analog OUT 1.6 V -> 0A 3.2A -> 2A (see ESCON)
 Ticker  ControllerLoopTimer;            // interrupt for control loop
-Timer t;                                // timer to analyse button
+Timer t;                                // timer to analyse Button
 
 // controller parameters etc.
 float out_value = 1.6f;                 // set voltage on 1.6 V (0 A current)
-float kp = 4.0f;                        // speed control gain for motor speed cntrl.
-float Tn = 0.05f;                       // integral time       "     "        "
+// float kp = 3.0f;                        // speed control gain for motor speed cntrl.
+// float Tn = 0.10f;                       // integral time       "     "        "
 float Ts = 0.0025;                      // sample time
 float v_max = 200;                      // maximum speed rad/s
 float n_soll = 0.0f;                    // nominal speed for speed control tests
-float comp_filter_tau =1.0f;            // time constant of complementary filter
+float tau = 1.0f;            // time constant of complementary filter
+float fg = 300.0f;
 
 // output and statemachine
 unsigned int k = 0;                     // counter for serial output
 bool doesStand = 0;                     // state if the cube is standing or not
 bool shouldBalance = 0;                 // state if the controller is active
 
-// set up encoder, controller and filter instanzes           
-EncoderCounter counter1(PB_6, PB_7);    // initialize counter on PB_6 and PB_7
-DiffCounter diff(0.01, Ts);             // discrete differentiate, based on encoder data
+// set up encoder         
+EncoderCounter MotorEncoder(PB_6, PB_7);    // initialize counter on PB_6 and PB_7
+DiffCounter MotorDiff(0.01, Ts);             // discrete differentiate, based on encoder data
+
 PI_Cntrl pi_w2zero(-.01f, 1.0f, 0.4f);  // controller to bring motor speed to zero while balancing
-IIR_filter f_ax(comp_filter_tau,Ts);                // 1st order LP for complementary filter acc_x
-IIR_filter f_ay(comp_filter_tau,Ts);                // 1st order LP for complementary filter acc_y
-IIR_filter f_gz(comp_filter_tau,Ts,comp_filter_tau);// 1st order LP for complementary filter gyro
+PI_Cntrl pi_w(1.0f, 0.1f, 2.0f);              // PI controller for test purposes motor speed (no balance)
+
+IIR_filter FilterACCx(tau, Ts, 1.0f);                // 1st order LP for complementary filter acc_x
+IIR_filter FilterACCy(tau, Ts, 1.0f);                // 1st order LP for complementary filter acc_y
+IIR_filter FilterGYRZ(tau, Ts, tau);     // 1st order LP for complementary filter gyro
+
+// IIR_filter FilterANG(1.0f/(2.0f*PI*fg), Ts, 1.0f);
+// IIR_filter FilterDiffANG(1.0f/(2.0f*PI*fg), Ts);
 
 // linear characteristics
 LinearCharacteristics i2u(0.1067f,-15.0f);          // full range, convert desired current (Amps)  -> voltage 0..3.3V
@@ -82,8 +89,8 @@
 
 // user defined functions
 void updateControllers(void);   // speed controller loop (via interrupt)
-void pressed(void);             // user button pressed
-void released(void);            // user button released
+void pressed(void);             // user Button pressed
+void released(void);            // user Button released
 
 // main program and control loop
 // -----------------------------------------------------------------------------
@@ -93,29 +100,43 @@
     pc.baud(2000000);   
     
     // reset encoder, controller and filters
-    counter1.reset();
-    diff.reset(0.0f,0.0f);
+    MotorEncoder.reset();
+    MotorDiff.reset(0.0f,0.0f);
     pi_w2zero.reset(0.0f);
-    f_ax.reset(u2ax(3.3f*ax.read()));
-    f_ay.reset(u2ay(3.3f*ay.read()));
-    f_gz.reset(u2gz(3.3f*gz.read()));
+    pi_w.reset(0.0f);
+    
+    FilterACCx.reset(u2ax(3.3f*ax.read()));
+    FilterACCy.reset(u2ay(3.3f*ay.read()));
+    FilterGYRZ.reset(u2gz(3.3f*gz.read()));
+    
+    // FilterANG.reset(atan2(-u2ax(3.3f*ax.read()), u2ay(3.3f*ay.read())) + PI/4.0f);
+    // FilterDiffANG.reset(0.0f);
     
     // reset output
     out.write(u3k3_TO_1V(i2u(0.0f))); 
     
     // attach controller loop to timer interrupt
     ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = 400Hz;
-    button.fall(&pressed);          // attach key pressed function
-    button.rise(&released);         // attach key pressed function
+    Button.fall(&pressed);          // attach key pressed function
+    Button.rise(&released);         // attach key pressed function
 }
 
 void updateControllers(void){
     
-    // read sensor data
-    short counts = counter1;            // get counts from Encoder
-    float vel = diff(counts);           // motor velocity 
-    float wz = u2gz(3.3f*gz.read());    // cuboid rot-velocity
-    float ang = atan2( -f_ax(u2ax(3.3f*ax.read())), f_ay(u2ay(3.3f*ay.read()))) + f_gz(wz) + PI/4.0f; // spaghetti compl. filter
+    // read encoder data
+    short counts = MotorEncoder;            // counts in 1
+    float omega = MotorDiff(counts);        // angular velofity motor
+    
+    // read imu data
+    float accx = u2ax(3.3f*ax.read());
+    float accy = u2ay(3.3f*ay.read());
+    float gyrz = u2gz(3.3f*gz.read());
+    
+    // perform complementary filter
+    float ang = atan2(-FilterACCx(accx), FilterACCy(accy)) + FilterGYRZ(gyrz) + PI/4.0f;
+    
+    // float angf = FilterANG(ang);
+    // float dangf = FilterDiffANG(ang);
     
     // get current state of the cube
     float actualAngleDegree = ang*180.0f/PI;
@@ -126,28 +147,29 @@
         doesStand = 0;
     }
     
-    // update controller
+    // update controllers
+    float desTorque = 0.0f;
     if(shouldBalance){       
         // K matrix: -5.2142   -0.6247  // from Matlab
-        float desTorque = pi_w2zero(n_soll-vel)-(-5.2142f*ang-0.6247f*wz);     // state space controller for balance, calc desired Torque
-        // convert Nm -> A and write to AOUT 
-        out.write(u3k3_TO_1V(i2u(desTorque/0.217f)));     
+        desTorque = pi_w2zero(n_soll-omega)-(-5.2142f*ang-0.6247f*gyrz);     // state space controller for balance, calc desired Torque 
     }
     else{
-        out.write(u3k3_TO_1V(i2u(0.0f)));  
-    }               
+        desTorque = pi_w(n_soll-omega);     // state space controller for balance, calc desired Torque 
+    }
+    // convert Nm -> A and write to AOUT 
+    out.write(u3k3_TO_1V(i2u(desTorque/0.217f)));                
     
-    //out.write(u3k3_TO_1V(i2u(pi_w(n_soll-vel))));          // test speed controller 
+    //out.write(u3k3_TO_1V(i2u(pi_w(n_soll-omega))));          // test speed controller 
     if(++k >= 199){
         k = 0;
-        pc.printf("phi=%3.2f omega=%3.2f \r\n", actualAngleDegree, vel);
+        pc.printf("phi=%3.2f omega=%3.2f \r\n", actualAngleDegree, omega);
         //pc.printf("ax=%3.2f ay=%3.2f gz=%3.2f \r\n",u2ax(3.3f*ax.read()),u2ay(3.3f*ay.read()),wz);
     }
 }
 
-// buttonhandling and statemachine
+// Buttonhandling and statemachine
 // -----------------------------------------------------------------------------
-// start timer as soon as button is pressed
+// start timer as soon as Button is pressed
 void pressed(){   
     t.start();
 }
@@ -156,7 +178,7 @@
 void released(){
     
     // readout, stop and reset timer
-    float buttonTime = t.read();
+    float ButtonTime = t.read();
     t.stop();
     t.reset();
     
@@ -164,12 +186,12 @@
     if(doesStand)
     {
         // in - or decrease speed 
-        if(buttonTime < 2.0f){
-            // press button long -> increase speed 5 rev/min 
-            if(buttonTime > 0.5f){
+        if(ButtonTime < 2.0f){
+            // press Button long -> increase speed 5 rev/min 
+            if(ButtonTime > 0.5f){
                 n_soll -= 5.0f;
             }
-            // press button short -> decrease speed 5 rev/min
+            // press Button short -> decrease speed 5 rev/min
             else{
                 n_soll += 5.0f;
             }
@@ -187,7 +209,8 @@
         }
     }
     else{
-        if(buttonTime > 2.0f)
+        if(ButtonTime > 2.0f)
            shouldBalance = 1;
+           pi_w.reset(0.0f);
     }
 }