File content as of revision 4:3d8dd3d17564:

#include "mbed.h"
#include "math.h" 
//------------------------------------------
#define PI 3.1415927f
//------------------------------------------
#include "EncoderCounter.h"
#include "DiffCounter.h"
#include "IIR_filter.h"
#include "LinearCharacteristics.h"
#include "PI_Cntrl.h"
// #include "GPA.h"
// define STATES:
#define INIT 0              // at very beginning
#define FLAT 10             // cuboid is flat, motor is controlled to zero
#define BALANCE 20          // balancing
#define SWING_DOWN 30       // move cuboid down
 
 
/* 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
  :                         :
  :                         :
 ..                        ..
 ..                        ..                  15.
 ..    AOUT i_des on (PA_5)o.
 ..                        ..
 ..                        ..
 ..               ENC CH A o.
 o. GND                    ..                  10.
 o. ENC CH B               ..
 ..                        ..
 ..                        ..
 .o AIN acx (PA_0)         ..
 .o AIN acy (PA_1)         ..                  5.
 .o AIN Gyro(PA_4)         .o Analog GND
 ..                        ..
 ..                        ..
 ..                        ..                  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
bool key_was_pressed = false;
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 PA_4
AnalogOut out(PA_5);                    // Analog OUT on PA_5   1.6 V -> 0A 3.2A -> 2A (see ESCON)
float out_value = 1.6f;                 // set voltage on 1.6 V (0 A current)
float w_soll = 10.0f;                   // desired velocity
float Ts = 0.002f;                      // sample time of main loops
int k = 0;
float phi1_des = 0.0f;
int CS = INIT;
void pressed(void);             // user Button pressed
void released(void);            // user Button released
 
 
//------------------------------------------
// ... here define variables like gains etc.
//------------------------------------------
LinearCharacteristics i2u(-15.0f,15.0f,0.0f,3.2f / 3.3f);       // output is normalized output
LinearCharacteristics u2ax(0.29719f,0.69768f,-9.81f,9.81f);     // use normalized input
LinearCharacteristics u2ay(0.29890,0.70330f,-9.81f,9.81f);      // use normalized input
LinearCharacteristics u2gz(-4.6517f * 3.3f,0.45495f);           // use normalized input
                // 4.6517f = 1/3.752e-3 [V / °/s] * pi/180
 
//-------------DEFINE FILTERS----------------
float tau = 1.0f;
IIR_filter f_ax(tau,Ts,1.0f);        // filter ax signals
IIR_filter f_ay(tau,Ts,1.0f);        // filter ay signals
IIR_filter f_gz(tau,Ts,tau);    // filter gz signals
//------------------------------------------
float vel = 0.0f;                  // velocity of motor
float gyro = 0.0f;
float phi1 = 0.0f;
//------------------------------------------
Ticker  ControllerLoopTimer;            // interrupt for control loop
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
Timer ti;                               // define global timer
Timer t_but;                            // define global timer
PI_Cntrl vel_cntrl(0.5f,.05f,Ts,0.5f);  // velocity controller for motor
PI_Cntrl om2zero(-0.02f,4.0f,Ts,0.9f);  // slow vel. contrl. to bring motor to zero
//------------------------------------------
// ----- User defined functions -----------
void updateLoop(void);   // loop for State machine (via interrupt)
float cuboid_stab_cntrl(int);   // stabalizer 
void calc_angle_phi1(int);
// ------ END User defined functions ------
 
//******************************************************************************
//---------- main loop -------------
//******************************************************************************
int main()
{
    pc.baud(2000000);   // for serial comm.
    counter1.reset();   // encoder reset
    diff.reset(0.0f,0);  
    ControllerLoopTimer.attach(&updateLoop, Ts); //Assume Fs = ...;
    ti.reset();
    ti.start();
    calc_angle_phi1(1);
    button.fall(&pressed);          // attach key pressed function
    button.rise(&released);         // attach key pressed function
    
}   // END OF main
//******************************************************************************
//---------- main loop (called via interrupt) -------------
//******************************************************************************
void updateLoop(void){
    short counts = counter1;            // get counts from Encoder
    vel = diff(counts);                 // motor velocity 
    float torq = 0.0f;                  // set motor torque to zero (will be overwritten)
    float dt  = ti.read();              // time elapsed in current state
    calc_angle_phi1(0);                 // angle phi1 is calculated in every loop
    // ****************** STATE  MACHINE ***************************************
    switch(CS)  { 
        case INIT:                      // at very beginning
            if (dt > 2.0f){
                CS = FLAT;              // switch to FLAT state
                ti.reset(); 
                }
            break;
        case FLAT:                      // cuboid is flat, keep motor velocity to zero
            torq = vel_cntrl(0.0f - vel); 
            if (key_was_pressed && dt > 1.0f){
                CS = BALANCE;
                torq = cuboid_stab_cntrl(1);
                ti.reset();
                key_was_pressed = false;
                phi1_des = 0.0f;
                vel_cntrl.reset(0.0f);  // reset velocity controller for the next time
                }
            break;
        case BALANCE:                   // balance the cube
            torq = cuboid_stab_cntrl(0);
            if (key_was_pressed && dt > 1.0f){
                CS = SWING_DOWN;
                phi1_des = 0.0f; 
                ti.reset();
                key_was_pressed = false;
                }
            break;  
        case SWING_DOWN: 
            phi1_des = dt;              // ramp the desired angle up to pi/4
            torq = cuboid_stab_cntrl(0);// call balance routine
            if (dt > 1.0f){             // good value for smooth moving down
                CS = FLAT;              // move to flat
                phi1_des = 0.0f; 
                ti.reset(); 
                }
            break;  
    default: break;
    }
    out.write( i2u(torq/0.217f)); // motor const. is 0.217, 
    if(++k >= 249){
        k = 0;
        //pc.printf("phi: %1.5f, Torq: %1.2f  \r\n",phi1,torq);
        pc.printf("ax %1.5f, ay: %1.5f, gyro: %1.5f, ph1: %1.5f\r\n",u2ax(ax.read()),u2ay(ay.read()),u2gz(gz.read()),phi1);
        }

} // END OF updateLoop(void)
 
//******************************************************************************
//        ************ stabalizing controller *****************
//******************************************************************************
 
float cuboid_stab_cntrl(int do_reset){
/*                   | phi_des
                     v 
                    ---
                   | V |  feed forward gain
                    ---
                     |
          -------    v      --------
0 -->O-->|om2zero|-->O---->| System |--o---> x = [phi1 phi1_t]
     ^-   -------    ^-     --------   |
     |               |                 |
    vel              |       ---       |
                      ---  -| K |<-----
                             ---
*/
    if(do_reset == 1)       // reset controller
        om2zero.reset(0.0f);
        
    float M_des = om2zero(0.0f-vel);    // outer controller to bring motor to zero
    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!!!
    //          output PI       V               K(1)              K(2)
    return torq;
}
// ************ calculate angle *****************
void calc_angle_phi1(int do_reset){
    gyro = u2gz(gz.read());
    if(do_reset == 1){
        f_ax.reset(u2ax(ax.read()));
        f_ay.reset(u2ax(ay.read()));
        f_gz.reset(gyro);
        }
    phi1 = atan2(-f_ax(u2ax(ax.read())),f_ay(u2ay(ay.read()))) + PI/4.0f + f_gz(gyro);
    }
 
// start timer as soon as Button is pressed
void pressed()
{
    t_but.start();
    key_was_pressed = false;
}
 
// evaluating statemachine
void released()
{
 
    // readout, stop and reset timer
    float ButtonTime = t_but.read();
    t_but.stop();
    t_but.reset();
    if(ButtonTime > 0.05f) 
        key_was_pressed = true;
}
 
 
 
/* MATLAB CODE FOR CUBOID:
%% Skript fuer cuboid Praktikum 
    m_geh=.938;     % Masse des Gehaeses 
    m_sb=1.243;     % Masse Scheibe 
    m_g=m_geh+m_sb; % Gesamtmasse
    J_geh=.00408;   % Traegheit Gehaeuse (CM)
    J_sb=.00531;    % Traegheit Scheibe (CM)
    J1=diag([0 0 J_geh]);
    J2=diag([0 0 J_sb]);
    l=.17;          % Kantenlaenge
    sy=.085;
    g=9.81;
    phi0=0;
    J_g=J_geh+(m_geh+m_sb)*(l/sqrt(2))^2;
 
    %% Linearisiertes Modell
    A2=[0 1;m_g*g*l/sqrt(2)/J_g 0];
    B2=[0;-1/J_g];
    K=place(A2,B2,10*[-1+1j -1-1j]);
    s_cl=ss(A2-B2*K,B2,[1 0],0);
    V=1/dcgain(s_cl);
    
    %% Gesamtsystem mit Scheibe, Zustaende [phi_1 om_1 om2~] (om2~ = Absolutkoordinaten)
    A3=[A2-B2*K [0;0];...
         -1/J_sb*K 0];  % Auf Scheibenbeschleunigung wirken ganz 
                        % entsprechend die Zustaende x1, x2 ueber die Zustandsrueckfuehrung
    B3=[B2;1/J_sb];
    C3=[0 -1 1];        % Gemessen wird die Relativgeschwindigkeit zwischen Scheibe-Gehaeuse
    s3=ss(A3,B3,C3,0);
    s3=ss2ss(s3,[1 0 0;0 1 0;0 -1 1]);  % Transformiere 3ten Zustand (eigentlich unnoetig)
    Tn=4;
    PI=-tf([Tn 1],[Tn 0]);  % Langsamer PI Regler, negatives gain erforderlich!!
    rlocus(s3*PI);grid
    */