File content as of revision 4:6457d61fd234:

#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
  :                         :
  :                         :
 ..                        ..
 ..                        ..                  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
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)
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 -----------------------------------------
//9LinearCharacteristics 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)
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, calc desired Torque
    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 >= 199){
        k = 0;
        pc.printf("phi=%3.2f omega=%3.2f \r\n",ang*180.0f/PI,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();
}
//******************************************************************************
// 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;
}