Lab1

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 "IIR_filter.h"
00009 #include "LinearCharacteristics.h"
00010 #include "PI_Cntrl.h"
00011 #include "GPA.h"
00012 // define STATES:
00013 #define INIT 0              // at very beginning
00014 #define FLAT 10             // cuboid is flat, motor is controlled to zero
00015 #define BALANCE 20          // balancing
00016 #define SWING_DOWN 30       // move cuboid down
00017 
00018 
00019 /* Cuboid balance on one edge on Nucleo F446RE
00020 
00021  **** IMPORTANT: use ..\Labormodelle\RT-MOD054 - Würfel\Escon_Parameter_4nucleo.edc 
00022                  settings for Maxon ESCON controller (upload via ESCON Studio) ****
00023 hardware Connections:
00024  
00025  CN7                    CN10
00026   :                         :
00027   :                         :
00028  ..                        ..
00029  ..                        ..                  15.
00030  ..    AOUT i_des on (PA_5)o.
00031  ..                        ..
00032  ..                        ..
00033  ..               ENC CH A o.
00034  o. GND                    ..                  10.
00035  o. ENC CH B               ..
00036  ..                        ..
00037  ..                        ..
00038  .o AIN acx (PA_0)         ..
00039  .o AIN acy (PA_1)         ..                  5.
00040  .o AIN Gyro(PA_4)         .o Analog GND
00041  ..                        ..
00042  ..                        ..
00043  ..                        ..                  1.
00044  ----------------------------
00045  CN7               CN10
00046  */
00047 Serial pc(SERIAL_TX, SERIAL_RX);        // serial connection via USB - programmer
00048 InterruptIn button(USER_BUTTON);        // User Button, short presses: reduce speed, long presses: increase speed
00049 bool key_was_pressed = false;
00050 AnalogIn ax(PA_0);                      // Analog IN (acc x) on PA_0
00051 AnalogIn ay(PA_1);                      // Analog IN (acc y) on PA_1
00052 AnalogIn gz(PA_4);                      // Analog IN (gyr z) on PA_4
00053 AnalogOut out(PA_5);                    // Analog OUT on PA_5   1.6 V -> 0A 3.2A -> 2A (see ESCON)
00054 float out_value = 1.6f;                 // set voltage on 1.6 V (0 A current)
00055 float w_soll = 10.0f;                   // desired velocity
00056 float Ts = 0.002f;                      // sample time of main loops
00057 int k = 0;
00058 float phi1_des = 0.0f;
00059 int CS = INIT;
00060 void pressed(void);             // user Button pressed
00061 void released(void);            // user Button released
00062 
00063 
00064 //------------------------------------------
00065 // ... here define variables like gains etc.
00066 //------------------------------------------
00067 LinearCharacteristics i2u(-15.0f,15.0f,0.0f,3.2f / 3.3f);       // output is normalized output
00068 LinearCharacteristics u2ax(0.29719f,0.69768f,-9.81f,9.81f);     // use normalized input
00069 LinearCharacteristics u2ay(0.29890,0.70330f,-9.81f,9.81f);      // use normalized input
00070 LinearCharacteristics u2gz(-4.6517f * 3.3f,0.45495f);           // use normalized input
00071                 // 4.6517f = 1/3.752e-3 [V / °/s] * pi/180
00072 
00073 //-------------DEFINE FILTERS----------------
00074 float tau = 1.0f;
00075 IIR_filter f_ax(tau,Ts,1.0f);        // filter ax signals
00076 IIR_filter f_ay(tau,Ts,1.0f);        // filter ay signals
00077 IIR_filter f_gz(tau,Ts,tau);    // filter gz signals
00078 //------------------------------------------
00079 float vel = 0.0f;                  // velocity of motor
00080 float gyro = 0.0f;
00081 float phi1 = 0.0f;
00082 //------------------------------------------
00083 Ticker  ControllerLoopTimer;            // interrupt for control loop
00084 EncoderCounter counter1(PB_6, PB_7);    // initialize counter on PB_6 and PB_7
00085 DiffCounter diff(0.01,Ts);              // discrete differentiate, based on encoder data
00086 Timer ti;                               // define global timer
00087 Timer t_but;                            // define global timer
00088 PI_Cntrl vel_cntrl(0.5f,.05f,Ts,0.5f);  // velocity controller for motor
00089 PI_Cntrl om2zero(-0.02f,4.0f,Ts,0.9f);  // slow vel. contrl. to bring motor to zero
00090 //------------------------------------------
00091 // ----- User defined functions -----------
00092 void updateLoop(void);   // loop for State machine (via interrupt)
00093 float cuboid_stab_cntrl(int);   // stabalizer 
00094 void calc_angle_phi1(int);
00095 // ------ END User defined functions ------
00096 
00097 //******************************************************************************
00098 //---------- main loop -------------
00099 //******************************************************************************
00100 int main()
00101 {
00102     pc.baud(2000000);   // for serial comm.
00103     counter1.reset();   // encoder reset
00104     diff.reset(0.0f,0);  
00105     ControllerLoopTimer.attach(&updateLoop, Ts); //Assume Fs = ...;
00106     ti.reset();
00107     ti.start();
00108     calc_angle_phi1(1);
00109     button.fall(&pressed);          // attach key pressed function
00110     button.rise(&released);         // attach key pressed function
00111     
00112 }   // END OF main
00113 //******************************************************************************
00114 //---------- main loop (called via interrupt) -------------
00115 //******************************************************************************
00116 void updateLoop(void){
00117     short counts = counter1;            // get counts from Encoder
00118     vel = diff(counts);                 // motor velocity 
00119     float torq = 0.0f;                  // set motor torque to zero (will be overwritten)
00120     float dt  = ti.read();              // time elapsed in current state
00121     calc_angle_phi1(0);                 // angle phi1 is calculated in every loop
00122     // ****************** STATE  MACHINE ***************************************
00123     switch(CS)  {
00124         case INIT:                      // at very beginning
00125             if (dt > 2.0f){
00126                 CS = FLAT;              // switch to FLAT state
00127                 ti.reset(); 
00128                 }
00129             break;
00130         case FLAT:                      // cuboid is flat, keep motor velocity to zero
00131             torq = vel_cntrl(0.0f - vel); 
00132             if (key_was_pressd && dt > 1.0f){
00133                 CS = BALANCE;
00134                 torq = cuboid_stab_cntrl(1);
00135                 ti.reset();
00136                 key_was_pressed = false;
00137                 phi1_des = 0.0f;
00138                 vel_cntrl.reset(0.0f);  // reset velocity controller for the next time
00139                 }
00140             break;
00141         case BALANCE:                   // balance the cube
00142             torq = cuboid_stab_cntrl(0);
00143             if (key_was_pressd && dt > 1.0f){
00144                 CS = SWING_DOWN;
00145                 phi1_des = 0.0f; 
00146                 ti.reset();
00147                 key_was_pressed = false;
00148                 }
00149             break;  
00150         case SWING_DOWN: 
00151             phi1_des = dt;              // ramp the desired angle up to pi/4
00152             torq = cuboid_stab_cntrl(0);// call balance routine
00153             if (dt > 1.0f){             // good value for smooth moving down
00154                 CS = FLAT;              // move to flat
00155                 phi1_des = 0.0f; 
00156                 ti.reset(); 
00157                 }
00158             break;  
00159     default: break;
00160     }
00161     out.write( i2u(torq/0.217f)); // motor const. is 0.217, 
00162                                         // transform M -> i -> u -> normalized output
00163 } // END OF updateLoop(void)
00164 
00165 //******************************************************************************
00166 //        ************ stabalizing controller *****************
00167 //******************************************************************************
00168 
00169 float cuboid_stab_cntrl(int do_reset){
00170 /*                   | phi_des
00171                      v 
00172                     ---
00173                    | V |  feed forward gain
00174                     ---
00175                      |
00176           -------    v      --------
00177 0 -->O-->|om2zero|-->O---->| System |--o---> x = [phi1 phi1_t]
00178      ^-   -------    ^-     --------   |
00179      |               |                 |
00180     vel              |       ---       |
00181                       ---  -| K |<-----
00182                              ---
00183 */
00184     if(do_reset == 1)       // reset controller
00185         om2zero.reset(0.0f);
00186         
00187     float M_des = om2zero(0.0f-vel);    // outer controller to bring motor to zero
00188     float torq = M_des    +  (-7.1*phi1_des) -(-9.6910f * phi1 -0.7119f * gyro);  // calculationof gains are based on the MATLAB script at the end of this file!!!
00189     //          output PI       V               K(1)              K(2)
00190         if(++k >= 249){
00191             k = 0;
00192             pc.printf("phi: %1.5f, Torq: %1.2f, M_soll %1.2f  \r\n",phi1,torq,M_des);
00193             }
00194     return torq;
00195 }
00196 // ************ calculate angle *****************
00197 void calc_angle_phi1(int do_reset){
00198     gyro = u2gz(gz.read());
00199     if(do_reset == 1){
00200         f_ax.reset(u2ax(ax.read()));
00201         f_ay.reset(u2ax(ay.read()));
00202         f_gz.reset(gyro);
00203         }
00204     phi1 = atan2(-f_ax(u2ax(ax.read())),f_ay(u2ay(ay.read()))) + PI/4.0f + f_gz(gyro);
00205     }
00206 
00207 // start timer as soon as Button is pressed
00208 void pressed()
00209 {
00210     t_but.start();
00211     key_was_pressed = false;
00212 }
00213 
00214 // evaluating statemachine
00215 void released()
00216 {
00217 
00218     // readout, stop and reset timer
00219     float ButtonTime = t_but.read();
00220     t_but.stop();
00221     t_but.reset();
00222     if(ButtonTime > 0.05f) 
00223         key_was_pressed = true;
00224 }
00225 
00226 
00227 
00228 /* MATLAB CODE FOR CUBOID:
00229 %% Skript fuer cuboid Praktikum 
00230     m_geh=.938;     % Masse des Gehaeses 
00231     m_sb=1.243;     % Masse Scheibe 
00232     m_g=m_geh+m_sb; % Gesamtmasse
00233     J_geh=.00408;   % Traegheit Gehaeuse (CM)
00234     J_sb=.00531;    % Traegheit Scheibe (CM)
00235     J1=diag([0 0 J_geh]);
00236     J2=diag([0 0 J_sb]);
00237     l=.17;          % Kantenlaenge
00238     sy=.085;
00239     g=9.81;
00240     phi0=0;
00241     J_g=J_geh+(m_geh+m_sb)*(l/sqrt(2))^2;
00242 
00243     %% Linearisiertes Modell
00244     A2=[0 1;m_g*g*l/sqrt(2)/J_g 0];
00245     B2=[0;-1/J_g];
00246     K=place(A2,B2,10*[-1+1j -1-1j]);
00247     s_cl=ss(A2-B2*K,B2,[1 0],0);
00248     V=1/dcgain(s_cl);
00249     
00250     %% Gesamtsystem mit Scheibe, Zustaende [phi_1 om_1 om2~] (om2~ = Absolutkoordinaten)
00251     A3=[A2-B2*K [0;0];...
00252          -1/J_sb*K 0];  % Auf Scheibenbeschleunigung wirken ganz 
00253                         % entsprechend die Zustaende x1, x2 ueber die Zustandsrueckfuehrung
00254     B3=[B2;1/J_sb];
00255     C3=[0 -1 1];        % Gemessen wird die Relativgeschwindigkeit zwischen Scheibe-Gehaeuse
00256     s3=ss(A3,B3,C3,0);
00257     s3=ss2ss(s3,[1 0 0;0 1 0;0 -1 1]);  % Transformiere 3ten Zustand (eigentlich unnoetig)
00258     Tn=4;
00259     PI=-tf([Tn 1],[Tn 0]);  % Langsamer PI Regler, negatives gain erforderlich!!
00260     rlocus(s3*PI);grid
00261     */