Test program with the RT black boxes

Dependencies:   mbed

main.cpp

Committer:
pmic
Date:
5 months ago
Revision:
43:a577f752dd7a
Parent:
40:711929f5c2a8

File content as of revision 43:a577f752dd7a:

#include "mbed.h"
#include "math.h"
#define   pi 3.1415927f

#include "PI_Cntrl.h"
#include "IIR_filter.h"
#include "LinearCharacteristics.h"
#include "GPA.h"
#include "PRBS.h"

/* Hardware from Cuboid balance on one edge on Nucleo F446RE
 CN7                    CN10
  :                         :
  :                         :
 ..                        ..
 ..                        ..                  15.
 ..         AOUT 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
AnalogIn inp1(PA_4);                    // analog IN  on PA_4
AnalogIn inp2(PA_1);                    // analog IN  on PA_1
AnalogOut out(PA_5);                    // analog OUT on PA_5
Ticker  ControllerLoopTimer;            // interrupt for control loop
Timer t;                                // timer to analyse Button

// controller parameters etc.
float Ts = 0.0001f;                    // sample time for gpa measurement
// float Ts = 0.001f;                      // sample time for prbs measurement
float u  = 0.0f;
float y1 = 0.0f;
float y2 = 0.0f;
float w  = 0.5f;
float exc  = 0.0f;
int   k  = 0;
bool  doRun = false;

// set up controller and roll-off
IIR_filter rolloff(0.00016f, Ts, 1.0f); // 1st order LP
PI_Cntrl   pi_cntr(10.0f, 0.02f, Ts, 0.8f, 0.2f);   // controller

// set up gpa
float fMin = 0.1f;
float fMax = 0.99f/2.0f/Ts;
const int   NfexcDes = 200;
// float Aexc0 = 0.2f;
// float Aexc1 = 0.1f; // Aexc0/fMax;
float Aexc0 = 0.05f;
float Aexc1 = 0.02f; // Aexc0/fMax;
int   NperMin = 1;
float TmeasMin = 0.2f;
int   NmeasMin = (int)ceil(TmeasMin/Ts);
float Tstart = 3.0f;
int   NstartMin = (int)ceil(Tstart/Ts);
float Tsweep = 0.3f;
int   NsweepMin = (int)ceil(Tsweep/Ts);
GPA   gpa1(fMin, fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1, NstartMin, NsweepMin, false);
GPA   gpa2(fMin, fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1, NstartMin, NsweepMin, false);
GPA::gpadata_t gpaData;
int   gpaIndex = 0;
float gpaMeasArray[NfexcDes][8];
// GPA gpa1(fMin, fMax, NfexcDes, Aexc0, Aexc1, Ts);
// GPA gpa2(fMin, fMax, NfexcDes, Aexc0, Aexc1, Ts);
//float  fexcDes[9] = {0.1f, 0.2f, 0.25f, 0.5f, 1.0f, 2.0f, 3.0f , 4.0f , 5.0f, NstartMin, NsweepMin);
//GPA gpa1(fexcDes, sizeof(fexcDes)/sizeof(fexcDes[0]), NperMin, NmeasMin, Ts, Aexc0, Aexc1, NstartMin, NsweepMin);

// set up prbs generator
int n = 13;
float Aprbs = 0.1f;
int Nprbs = (int)pow((float)2, (float)n) - 1;
int Nexp = 10;
PRBS prbs(n);

// LinearCharacteristics::LinearCharacteristics(float xmin,float xmax, float ymin, float ymax)
LinearCharacteristics out2u(0.0f, 1.0f, 0.0303f, 0.8788f); // normalized u, constraint to (0.1...2.9) V
LinearCharacteristics u2inp(0.0303f, 0.8788f, 0.0f, 1.0f); // normalized input, constraint to (0.1...2.9) V

// user defined functions
void updateControllers(void);   // 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);

    gpa1.reset();
    gpa2.reset();
    gpaIndex = 0;
    // gpa1.printFullGPAmeasPara();
    // gpa1.printGPAmeasPara();

    prbs.reset();
    // prbs.printPRBSind();
    // prbs.printPRBSregister();
    // printLine();

    // reset u
    u = 0.0f;
    out.write(out2u(u));
    k = 0;

    rolloff.reset(0.0f);
    pi_cntr.reset(0.0f);

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

void updateControllers(void)
{
    // read analog inputs
    y1 = u2inp(inp1.read());
    y2 = u2inp(inp2.read());

    /*
    // update prbs, has to be done before writing the output
    if(doRun) exc = Aprbs*prbs();
    else exc = 0.0f;
    if(doRun && k++ < Nexp*Nprbs) pc.printf("%10i %10.3e %10.3e\r\n", k, exc, y2);
    */

    /*
    // open loop measurement
    u = w + exc;
    */

    ///*
    // update controller output
    // u = rolloff(w - y2 + exc);
    // u = pi_cntr(u);

    u = pi_cntr(w - y2 + exc); // no roll-off
    // u = pi_cntr(w - y2)  + exc; // no roll-off
    //*/

    // write controller output
    out.write(out2u(u));

    ///*
    // update gpa, has to be done after writing the outpupt
    if(doRun) exc = gpa1(u, y2);
    else exc = 0.0f;
    
    gpaData = gpa1.getGPAdata();
    if(gpaData.MeasPointFinished) {
        gpaMeasArray[gpaIndex][0] = gpaData.fexc;
        gpaMeasArray[gpaIndex][1] = gpaData.absGyu;
        gpaMeasArray[gpaIndex][2] = gpaData.angGyu;
        gpaMeasArray[gpaIndex][3] = gpaData.absGyr;
        gpaMeasArray[gpaIndex][4] = gpaData.angGyr;
        gpaMeasArray[gpaIndex][5] = gpaData.Umag;
        gpaMeasArray[gpaIndex][6] = gpaData.Ymag;
        gpaMeasArray[gpaIndex][7] = gpaData.Rmag;
        if(gpaIndex == 0) {
            gpa1.printLine();
            printf("  fexc[Hz]    |Gyu|    deg(Gyu)  |Gyr|    deg(Gyr)   |U|       |Y|       |R|\r\n");
            gpa1.printLine();
        }
        pc.printf("%11.4e %9.3e %8.3f %9.3e %8.3f %9.3e %9.3e %9.3e %9i\r\n", gpaData.fexc, gpaData.absGyu, gpaData.angGyu, gpaData.absGyr, gpaData.angGyr, gpaData.Umag, gpaData.Ymag, gpaData.Rmag, gpaIndex);
        gpaIndex++;
    }
    //*/


    //if(k++ < 1000){
    //if(k == 1) printLine();
    //u = 0.5f + 0.5f*sin(2*pi*1.0f*(k-1)*Ts);
    //pc.printf("%10i %10.3e %10.3e\r\n", k-1, u, y1);

    //if(k++ < 1000){
    //if(k == 1) printLine();
    //if(k < 500) u = 0.0f;
    //else u = 0.5f;
    //pc.printf("%10i %10.3e %10.3e\r\n", k-1, u, y1);

}

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

// evaluating statemachine
void released()
{
    // toggle state over boolean
    if(doRun) {
        k = 0;
        gpa1.reset();
        gpa2.reset();
        gpaIndex = 0;
        prbs.reset();
    }
    doRun = !doRun;

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

    // if the cube doesStand
}
/*
void printLine()
{
    printf("-----------------------------------------------------------------------------------------\r\n");
}
*/