cuboid strong

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 //------------------------------------------
00004 #define PI 3.1415927f
00005 //------------------------------------------
00006 #include "EncoderCounter.h"
00007 #include "DiffCounter.h"
00008 #include "PI_Cntrl.h"
00009 #include "IIR_filter.h"
00010 #include "LinearCharacteristics.h"
00011 /* Cuboid balance on one edge on Nucleo F446RE
00012 
00013  **** IMPORTANT: use ..\Labormodelle\RT-MOD054 - Würfel\Escon_Parameter_4nucleo.edc 
00014                  settings for Maxon ESCON controller (upload via ESCON Studio) ****
00015 hardware Connections:
00016  
00017  CN7                    CN10
00018   :                     :
00019   :                     :
00020  ..                     ..
00021  ..            ENC CH A o.
00022  o. GND                 ..                  10.
00023  o. ENC CH B            ..
00024  ..                     ..
00025  ..                     ..
00026  .o AIN acx (PA_0)      ..  
00027  .o AIN acy (PA_1)      ..                  5.
00028  .o i_soll(PA_4)        .o Analog GND
00029  .o AIN Gyro (PB_0)     ..
00030  ..                     ..              
00031  ..                     ..                  1. 
00032  -------------------------
00033  CN7               CN10
00034  */
00035 Serial pc(SERIAL_TX, SERIAL_RX);        // serial connection via USB - programmer
00036 InterruptIn button(USER_BUTTON);        // User Button, short presses: reduce speed, long presses: increase speed
00037 AnalogIn ax(PA_0);                      // Analog IN (acc x) on PA_0
00038 AnalogIn ay(PA_1);                      // Analog IN (acc y) on PA_1
00039 AnalogIn gz(PA_4);                      // Analog IN (gyr z) on PB_0
00040 AnalogOut out(PA_5);                    // Analog OUT 1.6 V -> 0A 3.2A -> 2A (see ESCON)
00041 float out_value = 1.6f;                 // set voltage on 1.6 V (0 A current)
00042 float kp = 4.0f;                        // speed control gain for motor speed cntrl.
00043 float Tn = 0.05f;                       // Integral time       "     "        "
00044 float Ts = 0.0025;                      // sample time
00045 float v_max = 200;                      // maximum speed rad/s
00046 //------------------------------------------
00047 float n_soll = 10.0f;           // nominal speed for speed control tests
00048 float data[1000][2];            // logging data
00049 unsigned int k = 0;             // standart counter for output 
00050 unsigned int n = 0;             // standart counter for output 
00051 //------------------------------------------
00052 Ticker  ControllerLoopTimer;    // interrupt for control loop
00053 Timer t;                        // Timer to analyse Button
00054 float TotalTime = 0.0f;
00055 float comp_filter_tau =1.0f;            // time constant of complementary filter              
00056 EncoderCounter counter1(PB_6, PB_7);    // initialize counter on PB_6 and PB_7
00057 DiffCounter diff(0.01,Ts);              // discrete differentiate, based on encoder data
00058 PI_Cntrl pi_w(kp,Tn,2.0f);              // PI controller for test purposes motor speed (no balance)
00059 PI_Cntrl pi_w2zero(-.01f,1.0f,0.4f);    // PI controller to bring motor speed to zero while balancing
00060 //------------------------------------------
00061 IIR_filter f_ax(comp_filter_tau,Ts);                // 1st order LP for complementary filter acc_x
00062 IIR_filter f_ay(comp_filter_tau,Ts);                // 1st order LP for complementary filter acc_y
00063 IIR_filter f_gz(comp_filter_tau,Ts,comp_filter_tau);// 1st order LP for complementary filter gyro
00064 // define some linear characteristics -----------------------------------------
00065 LinearCharacteristics i2u(0.8f,-2.0f);              // convert desired current (Amps)  -> voltage 0..3.3V
00066 LinearCharacteristics u2n(312.5f,1.6f);             // convert input voltage (0..3.3V) -> speed (1/min)
00067 LinearCharacteristics u2w(32.725,1.6f);             // convert input voltage (0..3.3V) -> speed (rad/sec)
00068 LinearCharacteristics u2ax(14.67f,1.6378f);         // convert input voltage (0..3.3V) -> acc_x m/s^2
00069 LinearCharacteristics u2ay(15.02f ,1.6673f);        // convert input voltage (0..3.3V) -> acc_y m/s^2
00070 LinearCharacteristics u2gz(-4.652f,1.4949f);        // convert input voltage (0..3.3V) -> w_x rad/s
00071 LinearCharacteristics u3k3_TO_1V(0.303030303f,0,3.3f,0.0f);// normalize output voltage (0..3.3)V -> (0..1) V
00072 
00073 // ----- User defined functions -----------
00074 void updateControllers(void);   // speed controller loop (via interrupt)
00075 void pressed(void);             // user button pressed
00076 void released(void);            // user button released
00077 // ------ END User defined functions ------
00078 
00079 //******************************************************************************
00080 //---------- main loop -------------
00081 //******************************************************************************
00082 int main()
00083 {
00084     //attach controller loop to timer interrupt
00085     pc.baud(2000000);   // for serial comm.
00086     counter1.reset();   // encoder reset
00087     diff.reset(0.0f,0.0f);
00088     pi_w.reset(0.0f);
00089     pi_w2zero.reset(0.0f);
00090     f_ax.reset(u2ax(3.3f*ax.read()));
00091     f_ay.reset(u2ay(3.3f*ay.read()));
00092     f_gz.reset(u2gz(3.3f*gz.read()));    
00093     ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = 400Hz;
00094     button.fall(&pressed);          // attach key pressed function
00095     button.rise(&released);         // attach key pressed function
00096 }
00097 //******************************************************************************
00098 //---------- control loop -------------
00099 //******************************************************************************
00100 void updateControllers(void){
00101     short counts = counter1;            // get counts from Encoder
00102     float vel = diff(counts);           // motor velocity 
00103     float wz = u2gz(3.3f*gz.read());    // cuboid rot-velocity
00104     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
00105     // K matrix: -5.2142   -0.6247  // from Matlab
00106     float desTorque = pi_w2zero(n_soll-vel)-(-5.2142f*ang-0.6247f*wz);     // state space controller for balance, calc desired Torque
00107     out.write(u3k3_TO_1V(i2u(desTorque/0.217f)));                   // convert Nm -> A and write to AOUT  
00108     //out.write(u3k3_TO_1V(i2u(pi_w(n_soll-vel))));          // test speed controller 
00109     if(++k >= 199){
00110         k = 0;
00111         pc.printf("phi=%3.2f omega=%3.2f \r\n",ang*180.0f/PI,vel);
00112         //pc.printf("ax=%3.2f ay=%3.2f gz=%3.2f \r\n",u2ax(3.3f*ax.read()),u2ay(3.3f*ay.read()),wz);
00113     }
00114 }
00115 //******************************************************************************
00116 //********** User functions like buttens handle etc. **************
00117 //******************************************************************************
00118 // pressed button
00119 //******************************************************************************
00120 void pressed() 
00121 {   
00122 t.start();
00123 }
00124 //******************************************************************************
00125 // analyse pressed button
00126 //******************************************************************************
00127 void released()
00128 {
00129     TotalTime = t.read();
00130     t.stop();
00131     t.reset(); 
00132     if(TotalTime<0.1f)          // short button presses means decrease speed
00133     {
00134         n_soll -=5;            // decrease nominal speed
00135         TotalTime = 0.0f;       // reset Time
00136         }
00137     else
00138     {
00139         n_soll +=5;            // otherwise increase n_soll is in rev/min
00140         TotalTime = 0.0f;
00141         }
00142     if(n_soll>v_max)           // limit nominal speed
00143         n_soll=v_max;
00144     if(n_soll<-v_max)
00145         n_soll=-v_max;
00146 }