Test program with the RT black boxes

Dependencies:   mbed

Committer:
pmic
Date:
Wed May 15 13:58:10 2019 +0000
Revision:
43:a577f752dd7a
Parent:
40:711929f5c2a8
Add new instantiation option to surpress the printout for rt mbed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rtlabor 0:15be70d21d7c 1 #include "mbed.h"
rtlabor 0:15be70d21d7c 2 #include "math.h"
pmic 5:d6c7ccbbce78 3 #define pi 3.1415927f
pmic 5:d6c7ccbbce78 4
rtlabor 0:15be70d21d7c 5 #include "PI_Cntrl.h"
rtlabor 0:15be70d21d7c 6 #include "IIR_filter.h"
rtlabor 0:15be70d21d7c 7 #include "LinearCharacteristics.h"
pmic 10:a28f393c6716 8 #include "GPA.h"
pmic 23:1d24b1033371 9 #include "PRBS.h"
pmic 5:d6c7ccbbce78 10
pmic 27:fe807559476c 11 /* Hardware from Cuboid balance on one edge on Nucleo F446RE
rtlabor 0:15be70d21d7c 12 CN7 CN10
altb 3:a951d699878b 13 : :
altb 3:a951d699878b 14 : :
altb 3:a951d699878b 15 .. ..
altb 3:a951d699878b 16 .. .. 15.
pmic 28:f1e7a69b51ae 17 .. AOUT on (PA_5) o.
altb 3:a951d699878b 18 .. ..
altb 3:a951d699878b 19 .. ..
altb 3:a951d699878b 20 .. ENC CH A o.
altb 3:a951d699878b 21 o. GND .. 10.
altb 3:a951d699878b 22 o. ENC CH B ..
altb 3:a951d699878b 23 .. ..
altb 3:a951d699878b 24 .. ..
pmic 5:d6c7ccbbce78 25 .o AIN acx (PA_0) ..
altb 3:a951d699878b 26 .o AIN acy (PA_1) .. 5.
altb 3:a951d699878b 27 .o AIN Gyro(PA_4) .o Analog GND
altb 3:a951d699878b 28 .. ..
pmic 5:d6c7ccbbce78 29 .. ..
pmic 5:d6c7ccbbce78 30 .. .. 1.
altb 3:a951d699878b 31 ----------------------------
rtlabor 0:15be70d21d7c 32 CN7 CN10
rtlabor 0:15be70d21d7c 33 */
pmic 5:d6c7ccbbce78 34
pmic 5:d6c7ccbbce78 35 Serial pc(SERIAL_TX, SERIAL_RX); // serial connection via USB - programmer
pmic 27:fe807559476c 36 InterruptIn Button(USER_BUTTON); // User Button
pmic 28:f1e7a69b51ae 37 AnalogIn inp1(PA_4); // analog IN on PA_4
pmic 30:48b233b596b8 38 AnalogIn inp2(PA_1); // analog IN on PA_1
pmic 28:f1e7a69b51ae 39 AnalogOut out(PA_5); // analog OUT on PA_5
pmic 5:d6c7ccbbce78 40 Ticker ControllerLoopTimer; // interrupt for control loop
pmic 5:d6c7ccbbce78 41 Timer t; // timer to analyse Button
pmic 5:d6c7ccbbce78 42
pmic 5:d6c7ccbbce78 43 // controller parameters etc.
pmic 39:590aa4d162da 44 float Ts = 0.0001f; // sample time for gpa measurement
pmic 39:590aa4d162da 45 // float Ts = 0.001f; // sample time for prbs measurement
pmic 31:b78b299c248b 46 float u = 0.0f;
pmic 31:b78b299c248b 47 float y1 = 0.0f;
pmic 31:b78b299c248b 48 float y2 = 0.0f;
pmic 31:b78b299c248b 49 float w = 0.5f;
pmic 31:b78b299c248b 50 float exc = 0.0f;
pmic 31:b78b299c248b 51 int k = 0;
pmic 31:b78b299c248b 52 bool doRun = false;
pmic 31:b78b299c248b 53
pmic 31:b78b299c248b 54 // set up controller and roll-off
pmic 33:4de33aa9bc86 55 IIR_filter rolloff(0.00016f, Ts, 1.0f); // 1st order LP
pmic 33:4de33aa9bc86 56 PI_Cntrl pi_cntr(10.0f, 0.02f, Ts, 0.8f, 0.2f); // controller
pmic 10:a28f393c6716 57
pmic 15:ed33be6f040e 58 // set up gpa
pmic 39:590aa4d162da 59 float fMin = 0.1f;
pmic 30:48b233b596b8 60 float fMax = 0.99f/2.0f/Ts;
pmic 39:590aa4d162da 61 const int NfexcDes = 200;
pmic 35:009a1efb5e82 62 // float Aexc0 = 0.2f;
pmic 35:009a1efb5e82 63 // float Aexc1 = 0.1f; // Aexc0/fMax;
pmic 35:009a1efb5e82 64 float Aexc0 = 0.05f;
pmic 35:009a1efb5e82 65 float Aexc1 = 0.02f; // Aexc0/fMax;
pmic 29:f24c48ea8f1c 66 int NperMin = 1;
pmic 30:48b233b596b8 67 float TmeasMin = 0.2f;
pmic 27:fe807559476c 68 int NmeasMin = (int)ceil(TmeasMin/Ts);
pmic 30:48b233b596b8 69 float Tstart = 3.0f;
pmic 30:48b233b596b8 70 int NstartMin = (int)ceil(Tstart/Ts);
pmic 40:711929f5c2a8 71 float Tsweep = 0.3f;
pmic 30:48b233b596b8 72 int NsweepMin = (int)ceil(Tsweep/Ts);
pmic 43:a577f752dd7a 73 GPA gpa1(fMin, fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1, NstartMin, NsweepMin, false);
pmic 43:a577f752dd7a 74 GPA gpa2(fMin, fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1, NstartMin, NsweepMin, false);
pmic 37:352bacb64f3f 75 GPA::gpadata_t gpaData;
pmic 39:590aa4d162da 76 int gpaIndex = 0;
pmic 37:352bacb64f3f 77 float gpaMeasArray[NfexcDes][8];
pmic 37:352bacb64f3f 78 // GPA gpa1(fMin, fMax, NfexcDes, Aexc0, Aexc1, Ts);
pmic 37:352bacb64f3f 79 // GPA gpa2(fMin, fMax, NfexcDes, Aexc0, Aexc1, Ts);
pmic 30:48b233b596b8 80 //float fexcDes[9] = {0.1f, 0.2f, 0.25f, 0.5f, 1.0f, 2.0f, 3.0f , 4.0f , 5.0f, NstartMin, NsweepMin);
pmic 30:48b233b596b8 81 //GPA gpa1(fexcDes, sizeof(fexcDes)/sizeof(fexcDes[0]), NperMin, NmeasMin, Ts, Aexc0, Aexc1, NstartMin, NsweepMin);
pmic 5:d6c7ccbbce78 82
pmic 23:1d24b1033371 83 // set up prbs generator
pmic 28:f1e7a69b51ae 84 int n = 13;
pmic 28:f1e7a69b51ae 85 float Aprbs = 0.1f;
pmic 23:1d24b1033371 86 int Nprbs = (int)pow((float)2, (float)n) - 1;
pmic 28:f1e7a69b51ae 87 int Nexp = 10;
pmic 23:1d24b1033371 88 PRBS prbs(n);
pmic 23:1d24b1033371 89
pmic 27:fe807559476c 90 // LinearCharacteristics::LinearCharacteristics(float xmin,float xmax, float ymin, float ymax)
pmic 31:b78b299c248b 91 LinearCharacteristics out2u(0.0f, 1.0f, 0.0303f, 0.8788f); // normalized u, constraint to (0.1...2.9) V
pmic 28:f1e7a69b51ae 92 LinearCharacteristics u2inp(0.0303f, 0.8788f, 0.0f, 1.0f); // normalized input, constraint to (0.1...2.9) V
rtlabor 0:15be70d21d7c 93
pmic 5:d6c7ccbbce78 94 // user defined functions
pmic 27:fe807559476c 95 void updateControllers(void); // controller loop (via interrupt)
pmic 5:d6c7ccbbce78 96 void pressed(void); // user Button pressed
pmic 5:d6c7ccbbce78 97 void released(void); // user Button released
pmic 8:d68e177e2571 98 void printLine();
rtlabor 0:15be70d21d7c 99
pmic 5:d6c7ccbbce78 100 // main program and control loop
pmic 5:d6c7ccbbce78 101 // -----------------------------------------------------------------------------
rtlabor 0:15be70d21d7c 102 int main()
rtlabor 0:15be70d21d7c 103 {
pmic 5:d6c7ccbbce78 104 // for serial comm.
pmic 5:d6c7ccbbce78 105 pc.baud(2000000);
pmic 5:d6c7ccbbce78 106
pmic 35:009a1efb5e82 107 gpa1.reset();
pmic 35:009a1efb5e82 108 gpa2.reset();
pmic 39:590aa4d162da 109 gpaIndex = 0;
pmic 36:7b78426ea107 110 // gpa1.printFullGPAmeasPara();
pmic 36:7b78426ea107 111 // gpa1.printGPAmeasPara();
pmic 37:352bacb64f3f 112
pmic 23:1d24b1033371 113 prbs.reset();
pmic 23:1d24b1033371 114 // prbs.printPRBSind();
pmic 23:1d24b1033371 115 // prbs.printPRBSregister();
pmic 23:1d24b1033371 116 // printLine();
pmic 37:352bacb64f3f 117
pmic 31:b78b299c248b 118 // reset u
pmic 31:b78b299c248b 119 u = 0.0f;
pmic 31:b78b299c248b 120 out.write(out2u(u));
pmic 28:f1e7a69b51ae 121 k = 0;
pmic 13:a308f5e6c306 122
pmic 31:b78b299c248b 123 rolloff.reset(0.0f);
pmic 31:b78b299c248b 124 pi_cntr.reset(0.0f);
pmic 37:352bacb64f3f 125
pmic 5:d6c7ccbbce78 126 // attach controller loop to timer interrupt
pmic 30:48b233b596b8 127 ControllerLoopTimer.attach(&updateControllers, Ts);
pmic 5:d6c7ccbbce78 128 Button.fall(&pressed); // attach key pressed function
pmic 5:d6c7ccbbce78 129 Button.rise(&released); // attach key pressed function
rtlabor 0:15be70d21d7c 130 }
pmic 5:d6c7ccbbce78 131
pmic 5:d6c7ccbbce78 132 void updateControllers(void)
pmic 5:d6c7ccbbce78 133 {
pmic 27:fe807559476c 134 // read analog inputs
pmic 31:b78b299c248b 135 y1 = u2inp(inp1.read());
pmic 31:b78b299c248b 136 y2 = u2inp(inp2.read());
pmic 37:352bacb64f3f 137
pmic 37:352bacb64f3f 138 /*
pmic 35:009a1efb5e82 139 // update prbs, has to be done before writing the output
pmic 35:009a1efb5e82 140 if(doRun) exc = Aprbs*prbs();
pmic 35:009a1efb5e82 141 else exc = 0.0f;
pmic 35:009a1efb5e82 142 if(doRun && k++ < Nexp*Nprbs) pc.printf("%10i %10.3e %10.3e\r\n", k, exc, y2);
pmic 37:352bacb64f3f 143 */
pmic 37:352bacb64f3f 144
pmic 37:352bacb64f3f 145 /*
pmic 36:7b78426ea107 146 // open loop measurement
pmic 36:7b78426ea107 147 u = w + exc;
pmic 37:352bacb64f3f 148 */
pmic 37:352bacb64f3f 149
pmic 37:352bacb64f3f 150 ///*
pmic 31:b78b299c248b 151 // update controller output
pmic 31:b78b299c248b 152 // u = rolloff(w - y2 + exc);
pmic 31:b78b299c248b 153 // u = pi_cntr(u);
pmic 37:352bacb64f3f 154
pmic 35:009a1efb5e82 155 u = pi_cntr(w - y2 + exc); // no roll-off
pmic 35:009a1efb5e82 156 // u = pi_cntr(w - y2) + exc; // no roll-off
pmic 37:352bacb64f3f 157 //*/
pmic 37:352bacb64f3f 158
pmic 31:b78b299c248b 159 // write controller output
pmic 31:b78b299c248b 160 out.write(out2u(u));
pmic 37:352bacb64f3f 161
pmic 37:352bacb64f3f 162 ///*
pmic 35:009a1efb5e82 163 // update gpa, has to be done after writing the outpupt
pmic 31:b78b299c248b 164 if(doRun) exc = gpa1(u, y2);
pmic 31:b78b299c248b 165 else exc = 0.0f;
pmic 39:590aa4d162da 166
pmic 37:352bacb64f3f 167 gpaData = gpa1.getGPAdata();
pmic 37:352bacb64f3f 168 if(gpaData.MeasPointFinished) {
pmic 39:590aa4d162da 169 gpaMeasArray[gpaIndex][0] = gpaData.fexc;
pmic 39:590aa4d162da 170 gpaMeasArray[gpaIndex][1] = gpaData.absGyu;
pmic 39:590aa4d162da 171 gpaMeasArray[gpaIndex][2] = gpaData.angGyu;
pmic 39:590aa4d162da 172 gpaMeasArray[gpaIndex][3] = gpaData.absGyr;
pmic 39:590aa4d162da 173 gpaMeasArray[gpaIndex][4] = gpaData.angGyr;
pmic 39:590aa4d162da 174 gpaMeasArray[gpaIndex][5] = gpaData.Umag;
pmic 39:590aa4d162da 175 gpaMeasArray[gpaIndex][6] = gpaData.Ymag;
pmic 39:590aa4d162da 176 gpaMeasArray[gpaIndex][7] = gpaData.Rmag;
pmic 39:590aa4d162da 177 if(gpaIndex == 0) {
pmic 39:590aa4d162da 178 gpa1.printLine();
pmic 37:352bacb64f3f 179 printf(" fexc[Hz] |Gyu| deg(Gyu) |Gyr| deg(Gyr) |U| |Y| |R|\r\n");
pmic 39:590aa4d162da 180 gpa1.printLine();
pmic 37:352bacb64f3f 181 }
pmic 39:590aa4d162da 182 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);
pmic 39:590aa4d162da 183 gpaIndex++;
pmic 37:352bacb64f3f 184 }
pmic 37:352bacb64f3f 185 //*/
pmic 37:352bacb64f3f 186
pmic 37:352bacb64f3f 187
pmic 28:f1e7a69b51ae 188 //if(k++ < 1000){
pmic 28:f1e7a69b51ae 189 //if(k == 1) printLine();
pmic 31:b78b299c248b 190 //u = 0.5f + 0.5f*sin(2*pi*1.0f*(k-1)*Ts);
pmic 37:352bacb64f3f 191 //pc.printf("%10i %10.3e %10.3e\r\n", k-1, u, y1);
pmic 37:352bacb64f3f 192
pmic 31:b78b299c248b 193 //if(k++ < 1000){
pmic 31:b78b299c248b 194 //if(k == 1) printLine();
pmic 31:b78b299c248b 195 //if(k < 500) u = 0.0f;
pmic 31:b78b299c248b 196 //else u = 0.5f;
pmic 37:352bacb64f3f 197 //pc.printf("%10i %10.3e %10.3e\r\n", k-1, u, y1);
pmic 37:352bacb64f3f 198
pmic 5:d6c7ccbbce78 199 }
pmic 5:d6c7ccbbce78 200
pmic 37:352bacb64f3f 201 // Buttonhandling
pmic 5:d6c7ccbbce78 202 // -----------------------------------------------------------------------------
pmic 5:d6c7ccbbce78 203 // start timer as soon as Button is pressed
pmic 5:d6c7ccbbce78 204 void pressed()
pmic 5:d6c7ccbbce78 205 {
pmic 5:d6c7ccbbce78 206 t.start();
pmic 5:d6c7ccbbce78 207 }
pmic 5:d6c7ccbbce78 208
pmic 5:d6c7ccbbce78 209 // evaluating statemachine
pmic 5:d6c7ccbbce78 210 void released()
pmic 5:d6c7ccbbce78 211 {
pmic 28:f1e7a69b51ae 212 // toggle state over boolean
pmic 28:f1e7a69b51ae 213 if(doRun) {
pmic 28:f1e7a69b51ae 214 k = 0;
pmic 30:48b233b596b8 215 gpa1.reset();
pmic 30:48b233b596b8 216 gpa2.reset();
pmic 39:590aa4d162da 217 gpaIndex = 0;
pmic 28:f1e7a69b51ae 218 prbs.reset();
pmic 28:f1e7a69b51ae 219 }
pmic 28:f1e7a69b51ae 220 doRun = !doRun;
pmic 37:352bacb64f3f 221
pmic 5:d6c7ccbbce78 222 // readout, stop and reset timer
pmic 5:d6c7ccbbce78 223 float ButtonTime = t.read();
pmic 5:d6c7ccbbce78 224 t.stop();
pmic 5:d6c7ccbbce78 225 t.reset();
pmic 5:d6c7ccbbce78 226
pmic 28:f1e7a69b51ae 227 // if the cube doesStand
rtlabor 0:15be70d21d7c 228 }
pmic 39:590aa4d162da 229 /*
pmic 8:d68e177e2571 230 void printLine()
pmic 8:d68e177e2571 231 {
pmic 28:f1e7a69b51ae 232 printf("-----------------------------------------------------------------------------------------\r\n");
pmic 8:d68e177e2571 233 }
pmic 39:590aa4d162da 234 */
pmic 23:1d24b1033371 235