File content as of revision 12:1aa5ff8333e8:

#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"
/* 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 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;

//------------------------------------------
// ... here define variables like gains etc.
//------------------------------------------
//LinearCharacteristics i2u(0.8f,-2.0f);
//LinearCharacteristics i2u(-2.0f,2.0f,0.0f,3.2f);    // schwach parametrierung
LinearCharacteristics i2u(-15.0f,15.0f,0.0f,3.2f);    // starke parametrierung
LinearCharacteristics u2ax(0.69630f,0.29792,9.81f,-9.81f);
LinearCharacteristics u2ay(0.70012f,0.29750,9.81f,-9.81f);
LinearCharacteristics u2gyro(-4.65f,1.5f);



//------------------------------------------
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
//------------------------------------------
// ... here define instantiate classes
//------------------------------------------
PI_Cntrl vel_cntrl(0.5f,0.05f,Ts,0.4f);
PI_Cntrl omega2zero(-0.002f,2.0f,Ts,0.5);

float tau = 1.0f;
IIR_filter LPF_gyro(tau,Ts,tau);
IIR_filter LPF_accX(tau,Ts,1.0f);
IIR_filter LPF_accY(tau,Ts,1.0f);


//GPA gpa1(1.0f, 200.0f,      150,       4,      400, Ts, 10.0f, 0.3f);
//float excWobble = 0.0f;
// GPA(t fMin, t fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1)
// ... define some linear characteristics -----------------------------------------

// ----- User defined functions -----------
void updateControllers(void);   // speed controller loop (via interrupt)
// ------ 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);
    ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = ...;
        
    
}
//******************************************************************************
//---------- control loop (called via interrupt) -------------
//******************************************************************************
void updateControllers(void){
    short counts = counter1;            // get counts from Encoder
    float vel = diff(counts);           // motor velocity 
      //desTorque = vel_cntrl(w_soll - vel);
     /*       outWobble = omega;
            excWobble = Wobble(excWobble, outWobble); */
    
    //float torq_des = vel_cntrl(w_soll - vel);
    //out.write(i2u(torq_des/0.217f));     // the controller! convert torque to Amps km = 0.217 Nm/A
    
    float gyro_dir = (u2gyro(gz.read()*3.3f));
    float gyroV = LPF_gyro.filter(gyro_dir);
    float accX = LPF_accX.filter(u2ax(ax.read()));
    float accY = LPF_accY.filter(u2ay(ay.read()));
    
    // komplementärfilter
    float phi_mes = ((PI/4) + atan2(-accX, accY) + gyroV);
    
    // regler
    float torque = omega2zero(0-vel)-(-9.6910f*phi_mes - 0.71190f * gyro_dir);
    out.write(i2u(torque/0.217f)/3.3f);
    
    
    // OUTPUT
    if(++k >= 150){
        k = 0;
        //pc.printf("Des. velocity: %3.3f, Velocity: %3.3f\r\n",w_soll,vel);
        pc.printf("Uax val %3.5f; ax val %3.5f; Uay val %3.5f; ay val %3.5f; gyro %3.5f\r\n",ax.read(),u2ax(ax.read()),ay.read(),u2ay(ay.read()), gz.read());
        //pc.printf("Gyro: %3.3f rad/s\r\n",gyroCalc(gz.read()));
        //pc.printf("Gyro filt: %3.3f, Gyro roh; %3.3f, accx filt: %3.3f, accy filt: %3.3f\r\n", gyroV,gz.read()*3.3f, accX, accY);
        pc.printf("Phii: %3.3f\r\n", phi_mes);
    }
}
//******************************************************************************
//********** User functions like buttens handle etc. **************
//******************************************************************************
// pressed button
//******************************************************************************

//...