gpa in double prescision

Dependencies:   mbed

Revision:
0:15be70d21d7c
Child:
1:2e118d67eeae
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jan 10 16:08:07 2018 +0000
@@ -0,0 +1,145 @@
+#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) ****
+hardware Connections:
+ 
+ CN7                    CN10
+  :                     :
+  :                     :
+ ..                     ..
+ ..            ENC CH A o.
+ o. GND                 ..                  10.
+ o. ENC CH B            ..
+ ..                     ..
+ ..                     ..
+ .o AIN acy (PA_0)      ..  
+ .o AIN acy (PA_1)      ..                  5.
+ .o i_soll(PA_4)        .o Analog GND
+ .o AIN Gyro (PB_0)     ..
+ ..                     ..              
+ ..                     ..                  1. 
+ -------------------------
+ CN7               CN10
+ */
+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(PB_0);                      // Analog IN (gyr z) on PB_0
+AnalogOut out(PA_4);                    // Analog OUT 1.6 V -> 0A 3.2A -> 2A (see ESCON)
+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 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              
+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
+//------------------------------------------
+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);              // 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)
+LinearCharacteristics u2ax(14.67f,1.6378f);         // convert input voltage (0..3.3V) -> acc_x m/s^2
+LinearCharacteristics u2ay(15.02f ,1.6673f);        // convert input voltage (0..3.3V) -> acc_y m/s^2
+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 -----------
+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
+    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()));    
+    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){
+    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
+    out.write(u3k3_TO_1V(i2u(desTorque/0.217f)));                   // convert Nm -> A and write to AOUT  
+    //out.write(u3k3_TO_1V(i2u(pi_w(n_soll-vel))));          // test speed controller 
+    if(++k >= 99){
+        k = 0;
+        pc.printf("phi=%3.2f omega=%3.2f \r\n",ang*180.0f/PI,vel);
+    }
+}
+//******************************************************************************
+//********** User functions like buttens handle etc. **************
+//******************************************************************************
+// pressed button
+//******************************************************************************
+void pressed() 
+{   
+t.start();
+}
+//******************************************************************************
+// analyse pressed button
+//******************************************************************************
+void released()
+{
+    TotalTime = 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
+    {
+        n_soll +=5;            // otherwise increase n_soll is in rev/min
+        TotalTime = 0.0f;
+        }
+    if(n_soll>v_max)           // limit nominal speed
+        n_soll=v_max;
+    if(n_soll<-v_max)
+        n_soll=-v_max;
+}