File content as of revision 17:802aede7b90e:

#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"
#include "GPA.h"

/* Cuboid balance on one edge on Nucleo F446RE
// -----------------------------------------------------------------------------

IMPORTANT: use ..\T-RT\Messen_Ausstellungen\Praesentationen_im_Labor\Wuerfel_nucleo\Escon_Parameter_4nucleo_stark.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)
Ticker  ControllerLoopTimer;            // interrupt for control loop
Timer t;                                // timer to analyse Button

// controller parameters etc.
float Ts = 0.0025f;                     // sample time
float v_max = 200.0f;                   // maximum speed rad/s
float n_soll = 15.0f;                   // nominal speed for speed control tests
float tau = 1.00f;                      // time constant of complementary filter

// output and statemachine
unsigned int k = 0;                     // counter for serial output
bool doesStand = 0;                     // state if the cube is standing or not
bool shouldBalance = 0;                 // state if the controller is active
bool shouldGoDown = 0;                  // state if the controller should swing down

// set up encoder
EncoderCounter MotorEncoder(PB_6, PB_7);        // initialize counter on PB_6 and PB_7
DiffCounter MotorDiff(1/(2.0f*pi*80.0f), Ts);   // discrete differentiate, based on encoder data

// set up controllers
float maxCurrent = 15.0f;                         // 1/0.217 A/Nm -> 2.0/0.217 = 13.82 A
float Km = 1/0.217f;                              // Motorgain: Torque -> km -> Current in A/Nm
float maxTorque = maxCurrent/Km;
PI_Cntrl pi_w2zero(-.012f, 0.8f, Ts, maxTorque);  // controller to bring motor speed to zero while balancing
PI_Cntrl pi_w(0.6f, 0.4f, Ts, maxTorque);         // PI controller for test purposes motor speed (no balance)
float desTorque = 0.0f;

// set up gpa
float fMin = 1.0f;
float fMax = 1.0f/2.0f/Ts*0.9f;
int NfexcDes = 150;
float Aexc0 = 5.0f;
float Aexc1 = 0.3f; //Aexc0/fMax;
int NperMin = 3;
float TmeasMin = 1.0f;
int NmeasMin = (int)ceil(TmeasMin/Ts);
GPA Wobble(fMin, fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1);
float inpWobble = 0.0f;
float outWobble = 0.0f;
float excWobble = 0.0f;

// set up filters for complementary filter
IIR_filter FilterACCx(tau, Ts, 1.0f);    // 1st order LP for complementary filter acc_x
IIR_filter FilterACCy(tau, Ts, 1.0f);    // 1st order LP for complementary filter acc_y
IIR_filter FilterGYRZ(tau, Ts, tau);     // 1st order LP for complementary filter gyro

// set up filter for swing down process
IIR_filter FilterTrajectory(10.0f, 1.0f, Ts, 1.0f);
float V = -2.6080f;

// linear characteristics
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.0f, 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
void printLine();

// main program and control loop
// -----------------------------------------------------------------------------
int main()
{
    // for serial comm.
    pc.baud(2000000);

    // reset encoder, controller and filters
    MotorEncoder.reset();
    MotorDiff.reset(0.0f,0.0f);
    pi_w2zero.reset(0.0f);
    pi_w.reset(0.0f);

    FilterACCx.reset(u2ax(3.3f*ax.read()));
    FilterACCy.reset(u2ay(3.3f*ay.read()));
    FilterGYRZ.reset(u2gz(3.3f*gz.read()));

    FilterTrajectory.reset(0.0f);

    Wobble.reset();
    Wobble.printGPAmeasPara();
    inpWobble = 0.0f;
    outWobble = 0.0f;
    excWobble = 0.0f;

    // reset output
    desTorque = 0.0f;
    out.write(u3k3_TO_1V(i2u(desTorque*Km)));

    // reset statemachine
    shouldGoDown = 0;
    shouldBalance = 0;

    // attach controller loop to timer interrupt
    ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = 400Hz;
    Button.fall(&pressed);          // attach key pressed function
    Button.rise(&released);         // attach key pressed function
}

void updateControllers(void)
{

    // read encoder data
    short counts = MotorEncoder;            // counts in 1
    float omega = MotorDiff(counts);        // angular velofity motor

    // read imu data
    float accx = u2ax(3.3f*ax.read());
    float accy = u2ay(3.3f*ay.read());
    float gyrz = u2gz(3.3f*gz.read());

    // perform complementary filter
    float ang = atan2(-FilterACCx(accx), FilterACCy(accy)) + FilterGYRZ(gyrz) + pi/4.0f;

    // get current state of the cube
    float actualAngleDegree = ang*180.0f/pi;
    if(actualAngleDegree > -10.0f && actualAngleDegree < 10.0f) {
        doesStand = 1;
    } else {
        doesStand = 0;
    }

    // update controllers
    if(shouldBalance) {
        
        ///*
        // balance, set n_soll = 0.0f
        // ---------------------------------------------------------------------
        // K matrix: -5.2142   -0.6247  // from Matlab
        float uPI = pi_w2zero(n_soll - omega); // needs further inverstigation
        float uSS = (-5.2142f*ang - 0.6247f*gyrz);
        desTorque = uPI - uSS;     // state space controller for balance, calc desired Torque
        if(abs(desTorque) > maxTorque) {
            desTorque = copysign(maxTorque, desTorque);
        }
        if(k == 0) printLine();
        if(k++ < 2000) pc.printf("%6.4f %6.4f %6.4f %6.4f\r\n", uPI, uSS, ang, omega);
        //*/
        
        /*
        // step response velocity controller, set n_soll = 0.0f
        // ---------------------------------------------------------------------
        desTorque = pi_w(10.0f - omega);
        if(k == 0) printLine();
        if(k++ < 2000) pc.printf("%6.4f %6.4f %6.4f\r\n", 10.0f, omega, desTorque);
        */
        
        /*
        // gpa measurement, set n_soll = 15.0f
        // ---------------------------------------------------------------------
        // measuring the plant P and the closed loop tf T = PC/(1 + PC)
        desTorque = pi_w(n_soll + excWobble - omega);
        inpWobble = desTorque;
        outWobble = omega;
        excWobble = Wobble(excWobble, outWobble);
        // measuring the controller C and the closed loop tf SC = C/(1 + PC)
        // desTorque = pi_w(n_soll + excWobble - omega);
        // inpWobble = n_soll + excWobble - omega;
        // outWobble = desTorque;
        // excWobble = Wobble(inpWobble, outWobble);
        if(++k == 73000) Wobble.printGPAmeasTime();
        */
        
    } else if(shouldGoDown) {
        // swing down
        // K matrix: -5.2142   -0.6247  // from Matlab
        // V gain: -2.6080              // from Matlab
        float ref = FilterTrajectory(pi/4.0f);
        float uV  = V*ref;
        float uSS = (-5.2142f*ang - 0.6247f*gyrz);
        desTorque = uV - uSS;     // state space controller for balance
        if(abs(desTorque) > maxTorque) {
            desTorque = copysign(maxTorque, desTorque);
        }
        if(abs(ref - ang)/abs(ref) < 0.10f) {
            shouldGoDown = 0;
            FilterTrajectory.reset(omega);
        }
    } else {
        desTorque = pi_w(FilterTrajectory(0.0f) - omega);     // state space controller for balance
    }
    // convert Nm -> A and write to AOUT
    out.write(u3k3_TO_1V(i2u(desTorque*Km)));
}

// Buttonhandling and statemachine
// -----------------------------------------------------------------------------
// start timer as soon as Button is pressed
void pressed()
{
    t.start();
}

// evaluating statemachine
void released()
{

    // readout, stop and reset timer
    float ButtonTime = t.read();
    t.stop();
    t.reset();

    // if the cube doesStand
    if(doesStand) {
        // in - or decrease speed
        if(ButtonTime < 3.0f) {
            // press Button long -> increase speed 5 rev/min
            if(ButtonTime > 0.3f) {
                n_soll -= 5.0f;
            }
            // press Button short -> decrease speed 5 rev/min
            else {
                n_soll += 5.0f;
            }
            // constrain n_soll
            if(n_soll >  v_max)
                n_soll = v_max;
            if(n_soll < -v_max)
                n_soll = -v_max;
        }
        // stop balancing
        else {
            n_soll = 0.0f;
            shouldBalance = 0;
            shouldGoDown = 1;
            FilterTrajectory.reset(0.0f);
            pi_w2zero.reset(0.0f);
        }
    } else {
        if(ButtonTime > 3.0f) {
            shouldBalance = 1;
            shouldGoDown = 0;
            pi_w.reset(0.0f);
        }
    }
}

void printLine()
{
    printf("-----------------------------------------------------------------------------------------\r\n");
}