Test program with the RT black boxes

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #define   pi 3.1415927f
00004 
00005 #include "PI_Cntrl.h"
00006 #include "IIR_filter.h"
00007 #include "LinearCharacteristics.h"
00008 #include "GPA.h"
00009 #include "PRBS.h"
00010 
00011 /* Hardware from Cuboid balance on one edge on Nucleo F446RE
00012  CN7                    CN10
00013   :                         :
00014   :                         :
00015  ..                        ..
00016  ..                        ..                  15.
00017  ..         AOUT on (PA_5) o.
00018  ..                        ..
00019  ..                        ..
00020  ..               ENC CH A o.
00021  o. GND                    ..                  10.
00022  o. ENC CH B               ..
00023  ..                        ..
00024  ..                        ..
00025  .o AIN acx (PA_0)         ..
00026  .o AIN acy (PA_1)         ..                  5.
00027  .o AIN Gyro(PA_4)         .o Analog GND
00028  ..                        ..
00029  ..                        ..
00030  ..                        ..                  1.
00031  ----------------------------
00032  CN7               CN10
00033  */
00034 
00035 Serial pc(SERIAL_TX, SERIAL_RX);        // serial connection via USB - programmer
00036 InterruptIn Button(USER_BUTTON);        // User Button
00037 AnalogIn inp1(PA_4);                    // analog IN  on PA_4
00038 AnalogIn inp2(PA_1);                    // analog IN  on PA_1
00039 AnalogOut out(PA_5);                    // analog OUT on PA_5
00040 Ticker  ControllerLoopTimer;            // interrupt for control loop
00041 Timer t;                                // timer to analyse Button
00042 
00043 // controller parameters etc.
00044 float Ts = 0.0001f;                    // sample time for gpa measurement
00045 // float Ts = 0.001f;                      // sample time for prbs measurement
00046 float u  = 0.0f;
00047 float y1 = 0.0f;
00048 float y2 = 0.0f;
00049 float w  = 0.5f;
00050 float exc  = 0.0f;
00051 int   k  = 0;
00052 bool  doRun = false;
00053 
00054 // set up controller and roll-off
00055 IIR_filter rolloff(0.00016f, Ts, 1.0f); // 1st order LP
00056 PI_Cntrl   pi_cntr(10.0f, 0.02f, Ts, 0.8f, 0.2f);   // controller
00057 
00058 // set up gpa
00059 float fMin = 0.1f;
00060 float fMax = 0.99f/2.0f/Ts;
00061 const int   NfexcDes = 200;
00062 // float Aexc0 = 0.2f;
00063 // float Aexc1 = 0.1f; // Aexc0/fMax;
00064 float Aexc0 = 0.05f;
00065 float Aexc1 = 0.02f; // Aexc0/fMax;
00066 int   NperMin = 1;
00067 float TmeasMin = 0.2f;
00068 int   NmeasMin = (int)ceil(TmeasMin/Ts);
00069 float Tstart = 3.0f;
00070 int   NstartMin = (int)ceil(Tstart/Ts);
00071 float Tsweep = 0.3f;
00072 int   NsweepMin = (int)ceil(Tsweep/Ts);
00073 GPA   gpa1(fMin, fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1, NstartMin, NsweepMin, false);
00074 GPA   gpa2(fMin, fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1, NstartMin, NsweepMin, false);
00075 GPA::gpadata_t gpaData;
00076 int   gpaIndex = 0;
00077 float gpaMeasArray[NfexcDes][8];
00078 // GPA gpa1(fMin, fMax, NfexcDes, Aexc0, Aexc1, Ts);
00079 // GPA gpa2(fMin, fMax, NfexcDes, Aexc0, Aexc1, Ts);
00080 //float  fexcDes[9] = {0.1f, 0.2f, 0.25f, 0.5f, 1.0f, 2.0f, 3.0f , 4.0f , 5.0f, NstartMin, NsweepMin);
00081 //GPA gpa1(fexcDes, sizeof(fexcDes)/sizeof(fexcDes[0]), NperMin, NmeasMin, Ts, Aexc0, Aexc1, NstartMin, NsweepMin);
00082 
00083 // set up prbs generator
00084 int n = 13;
00085 float Aprbs = 0.1f;
00086 int Nprbs = (int)pow((float)2, (float)n) - 1;
00087 int Nexp = 10;
00088 PRBS prbs(n);
00089 
00090 // LinearCharacteristics::LinearCharacteristics(float xmin,float xmax, float ymin, float ymax)
00091 LinearCharacteristics out2u(0.0f, 1.0f, 0.0303f, 0.8788f); // normalized u, constraint to (0.1...2.9) V
00092 LinearCharacteristics u2inp(0.0303f, 0.8788f, 0.0f, 1.0f); // normalized input, constraint to (0.1...2.9) V
00093 
00094 // user defined functions
00095 void updateControllers(void);   // controller loop (via interrupt)
00096 void pressed(void);             // user Button pressed
00097 void released(void);            // user Button released
00098 void printLine();
00099 
00100 // main program and control loop
00101 // -----------------------------------------------------------------------------
00102 int main()
00103 {
00104     // for serial comm.
00105     pc.baud(2000000);
00106 
00107     gpa1.reset();
00108     gpa2.reset();
00109     gpaIndex = 0;
00110     // gpa1.printFullGPAmeasPara();
00111     // gpa1.printGPAmeasPara();
00112 
00113     prbs.reset();
00114     // prbs.printPRBSind();
00115     // prbs.printPRBSregister();
00116     // printLine();
00117 
00118     // reset u
00119     u = 0.0f;
00120     out.write(out2u(u));
00121     k = 0;
00122 
00123     rolloff.reset(0.0f);
00124     pi_cntr.reset(0.0f);
00125 
00126     // attach controller loop to timer interrupt
00127     ControllerLoopTimer.attach(&updateControllers, Ts);
00128     Button.fall(&pressed);          // attach key pressed function
00129     Button.rise(&released);         // attach key pressed function
00130 }
00131 
00132 void updateControllers(void)
00133 {
00134     // read analog inputs
00135     y1 = u2inp(inp1.read());
00136     y2 = u2inp(inp2.read());
00137 
00138     /*
00139     // update prbs, has to be done before writing the output
00140     if(doRun) exc = Aprbs*prbs();
00141     else exc = 0.0f;
00142     if(doRun && k++ < Nexp*Nprbs) pc.printf("%10i %10.3e %10.3e\r\n", k, exc, y2);
00143     */
00144 
00145     /*
00146     // open loop measurement
00147     u = w + exc;
00148     */
00149 
00150     ///*
00151     // update controller output
00152     // u = rolloff(w - y2 + exc);
00153     // u = pi_cntr(u);
00154 
00155     u = pi_cntr(w - y2 + exc); // no roll-off
00156     // u = pi_cntr(w - y2)  + exc; // no roll-off
00157     //*/
00158 
00159     // write controller output
00160     out.write(out2u(u));
00161 
00162     ///*
00163     // update gpa, has to be done after writing the outpupt
00164     if(doRun) exc = gpa1(u, y2);
00165     else exc = 0.0f;
00166     
00167     gpaData = gpa1.getGPAdata();
00168     if(gpaData.MeasPointFinished) {
00169         gpaMeasArray[gpaIndex][0] = gpaData.fexc;
00170         gpaMeasArray[gpaIndex][1] = gpaData.absGyu;
00171         gpaMeasArray[gpaIndex][2] = gpaData.angGyu;
00172         gpaMeasArray[gpaIndex][3] = gpaData.absGyr;
00173         gpaMeasArray[gpaIndex][4] = gpaData.angGyr;
00174         gpaMeasArray[gpaIndex][5] = gpaData.Umag;
00175         gpaMeasArray[gpaIndex][6] = gpaData.Ymag;
00176         gpaMeasArray[gpaIndex][7] = gpaData.Rmag;
00177         if(gpaIndex == 0) {
00178             gpa1.printLine();
00179             printf("  fexc[Hz]    |Gyu|    deg(Gyu)  |Gyr|    deg(Gyr)   |U|       |Y|       |R|\r\n");
00180             gpa1.printLine();
00181         }
00182         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);
00183         gpaIndex++;
00184     }
00185     //*/
00186 
00187 
00188     //if(k++ < 1000){
00189     //if(k == 1) printLine();
00190     //u = 0.5f + 0.5f*sin(2*pi*1.0f*(k-1)*Ts);
00191     //pc.printf("%10i %10.3e %10.3e\r\n", k-1, u, y1);
00192 
00193     //if(k++ < 1000){
00194     //if(k == 1) printLine();
00195     //if(k < 500) u = 0.0f;
00196     //else u = 0.5f;
00197     //pc.printf("%10i %10.3e %10.3e\r\n", k-1, u, y1);
00198 
00199 }
00200 
00201 // Buttonhandling
00202 // -----------------------------------------------------------------------------
00203 // start timer as soon as Button is pressed
00204 void pressed()
00205 {
00206     t.start();
00207 }
00208 
00209 // evaluating statemachine
00210 void released()
00211 {
00212     // toggle state over boolean
00213     if(doRun) {
00214         k = 0;
00215         gpa1.reset();
00216         gpa2.reset();
00217         gpaIndex = 0;
00218         prbs.reset();
00219     }
00220     doRun = !doRun;
00221 
00222     // readout, stop and reset timer
00223     float ButtonTime = t.read();
00224     t.stop();
00225     t.reset();
00226 
00227     // if the cube doesStand
00228 }
00229 /*
00230 void printLine()
00231 {
00232     printf("-----------------------------------------------------------------------------------------\r\n");
00233 }
00234 */
00235