gpa in double prescision

Dependencies:   mbed

Revision:
5:e443d5ad409f
Parent:
4:47581778e863
Child:
6:7ea1a4bac3c2
--- a/main.cpp	Thu Mar 01 13:49:29 2018 +0000
+++ b/main.cpp	Thu Mar 01 17:36:46 2018 +0000
@@ -1,18 +1,20 @@
 #include "mbed.h"
 #include "math.h"
-//------------------------------------------
+
 #define PI 3.1415927f
-//------------------------------------------
+
 #include "EncoderCounter.h"
 #include "DiffCounter.h"
 #include "PI_Cntrl.h"
 #include "IIR_filter.h"
 #include "LinearCharacteristics.h"
+
 /* Cuboid balance on one edge on Nucleo F446RE
-
+// -----------------------------------------------------------------------------
 
- **** IMPORTANT: use ..\Labormodelle\RT-MOD054 - Würfel\Escon_Parameter_4nucleo.edc 
-                 settings for Maxon ESCON controller (upload via ESCON Studio) ****
+IMPORTANT: use ..\T-RT\Messen_Ausstellungen\Praesentationen_im_Labor\Wuerfel_nucleo\Escon_Parameter_4nucleo_stark.edc
+           settings for Maxon ESCON controller (upload via ESCON Studio)
+           
 hardware Connections:
  
  CN7                    CN10
@@ -37,37 +39,39 @@
  ----------------------------
  CN7               CN10
  */
- Serial pc(SERIAL_TX, SERIAL_RX);        // serial connection via USB - programmer
+
+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
-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)
+
+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
+
 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 Tn = 0.05f;                       // integral time       "     "        "
 float Ts = 0.0025;                      // sample time
 float v_max = 200;                      // maximum speed rad/s
-//------------------------------------------
-float n_soll = 10.0f;           // nominal speed for speed control tests
-float data[1000][2];            // logging data
-unsigned int k = 0;             // standart counter for output 
-unsigned int n = 0;             // standart counter for output 
-//------------------------------------------
-Ticker  ControllerLoopTimer;    // interrupt for control loop
-Timer t;                        // Timer to analyse Button
-float TotalTime = 0.0f;
-float comp_filter_tau =1.0f;            // time constant of complementary filter              
+float n_soll = 0.0f;                    // nominal speed for speed control tests
+
+unsigned int k = 0;                     // standart counter for output 
+bool isStanding = 0;                    // state if the cube is standing or not
+bool shouldBalance = 0;                 // state if the controller is active
+
+float comp_filter_tau =1.0f;            // time constant of complementary filter  
+            
 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
-PI_Cntrl pi_w(kp,Tn,2.0f);              // PI controller for test purposes motor speed (no balance)
-PI_Cntrl pi_w2zero(-.01f,1.0f,0.4f);    // PI controller to bring motor speed to zero while balancing
-//------------------------------------------
+DiffCounter diff(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
-// define some linear characteristics -----------------------------------------
-//LinearCharacteristics i2u(0.8f,-2.0f);              // max. 2 A, convert desired current (Amps)  -> voltage 0..3.3V
+
 LinearCharacteristics i2u(0.1067f,-15.0f);          // full range, convert desired current (Amps)  -> voltage 0..3.3V
 LinearCharacteristics u2n(312.5f,1.6f);             // convert input voltage (0..3.3V) -> speed (1/min)
 LinearCharacteristics u2w(32.725,1.6f);             // convert input voltage (0..3.3V) -> speed (rad/sec)
@@ -76,77 +80,114 @@
 LinearCharacteristics u2gz(-4.652f,1.4949f);        // convert input voltage (0..3.3V) -> w_x rad/s
 LinearCharacteristics u3k3_TO_1V(0.303030303f,0,3.3f,0.0f);// normalize output voltage (0..3.3)V -> (0..1) V
 
-// ----- User defined functions -----------
+// user defined functions
 void updateControllers(void);   // speed controller loop (via interrupt)
 void pressed(void);             // user button pressed
 void released(void);            // user button released
-// ------ END User defined functions ------
 
-//******************************************************************************
-//---------- main loop -------------
-//******************************************************************************
+// -----------------------------------------------------------------------------
+
 int main()
 {
-    //attach controller loop to timer interrupt
-    pc.baud(2000000);   // for serial comm.
-    counter1.reset();   // encoder reset
+    // for serial comm.
+    pc.baud(2000000);   
+    
+    // reset encoder, controller and filters
+    counter1.reset();
     diff.reset(0.0f,0.0f);
-    pi_w.reset(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()));    
+    f_gz.reset(u2gz(3.3f*gz.read()));
+    
+    // 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
 }
-//******************************************************************************
-//---------- control loop -------------
-//******************************************************************************
+
 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; // compl. filter
-    // 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
-    out.write(u3k3_TO_1V(i2u(desTorque/0.217f)));                   // convert Nm -> A and write to AOUT  
+    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
+    
+    // get current state of the cube
+    float actualAngleDegree = ang*180.0f/PI;
+    if(actualAngleDegree > -10.0f && actualAngleDegree < 10.0f){
+        isStanding = 1;
+    }
+    else{
+        isStanding = 0;
+    }
+    
+    // update controller
+    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)));     
+    }
+    else{
+        out.write(u3k3_TO_1V(i2u(0.0f)));  
+    }               
+    
     //out.write(u3k3_TO_1V(i2u(pi_w(n_soll-vel))));          // test speed controller 
     if(++k >= 199){
         k = 0;
-        pc.printf("phi=%3.2f omega=%3.2f \r\n",ang*180.0f/PI,vel);
+        pc.printf("phi=%3.2f omega=%3.2f \r\n", actualAngleDegree, vel);
         //pc.printf("ax=%3.2f ay=%3.2f gz=%3.2f \r\n",u2ax(3.3f*ax.read()),u2ay(3.3f*ay.read()),wz);
     }
 }
-//******************************************************************************
-//********** User functions like buttens handle etc. **************
-//******************************************************************************
-// pressed button
-//******************************************************************************
-void pressed() 
-{   
-t.start();
+
+// buttonhandling and statemachine
+// -----------------------------------------------------------------------------
+// start timer as soon as button is pressed
+void pressed(){   
+    t.start();
 }
-//******************************************************************************
-// analyse pressed button
-//******************************************************************************
-void released()
-{
-    TotalTime = t.read();
+
+// evaluating statemachine
+void released(){
+    
+    // readout, stop and reset timer
+    float buttonTime = t.read();
     t.stop();
-    t.reset(); 
-    if(TotalTime<0.1f)          // short button presses means decrease speed
-    {
-        n_soll -=5;            // decrease nominal speed
-        TotalTime = 0.0f;       // reset Time
-        }
-    else
+    t.reset();
+    
+    // if the cube isStanding
+    if(isStanding)
     {
-        n_soll +=5;            // otherwise increase n_soll is in rev/min
-        TotalTime = 0.0f;
+        // in - or decrease speed 
+        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
+            else{
+                n_soll += 5.0f;
+            }
+            // constrain n_soll
+            if(n_soll >  v_max)
+                n_soll = v_max; 
+            if(n_soll < -v_max)
+                n_soll = -v_max;
         }
-    if(n_soll>v_max)           // limit nominal speed
-        n_soll=v_max;
-    if(n_soll<-v_max)
-        n_soll=-v_max;
+        // stop balancing
+        else{
+                n_soll = 0.0f;
+            shouldBalance = 0;
+            pi_w2zero.reset(0.0f);
+        }
+    }
+    else{
+        if(buttonTime > 2.0f)
+           shouldBalance = 1;
+    }
 }