RT2_Cuboid_demo / Mbed 2 deprecated nucf446-cuboid-balance1_strong

Cuboid

Dependencies:   mbed

main.cpp@28:fc53b2d62a1e, 2019-02-07 (annotated)

Committer:
pmic
Date:
Thu Feb 07 09:11:51 2019 +0000
Revision:
28:fc53b2d62a1e
Parent:
27:8f1ab2a2267b 
Add new default instantiate option for GPA

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 "EncoderCounter.h"
rtlabor 0:15be70d21d7c 6 #include "DiffCounter.h"
rtlabor 0:15be70d21d7c 7 #include "PI_Cntrl.h"
rtlabor 0:15be70d21d7c 8 #include "IIR_filter.h"
rtlabor 0:15be70d21d7c 9 #include "LinearCharacteristics.h"
pmic 10:a28f393c6716 10 #include "GPA.h"
pmic 23:1d24b1033371 11 #include "PRBS.h"
pmic 5:d6c7ccbbce78 12
rtlabor 0:15be70d21d7c 13 /* Cuboid balance on one edge on Nucleo F446RE
pmic 5:d6c7ccbbce78 14 // -----------------------------------------------------------------------------
altb 3:a951d699878b 15
pmic 5:d6c7ccbbce78 16 IMPORTANT: use ..\T-RT\Messen_Ausstellungen\Praesentationen_im_Labor\Wuerfel_nucleo\Escon_Parameter_4nucleo_stark.edc
pmic 5:d6c7ccbbce78 17 settings for Maxon ESCON controller (upload via ESCON Studio)
pmic 5:d6c7ccbbce78 18
rtlabor 0:15be70d21d7c 19 hardware Connections:
pmic 5:d6c7ccbbce78 20
rtlabor 0:15be70d21d7c 21 CN7 CN10
altb 3:a951d699878b 22 : :
altb 3:a951d699878b 23 : :
altb 3:a951d699878b 24 .. ..
altb 3:a951d699878b 25 .. .. 15.
altb 3:a951d699878b 26 .. AOUT i_des on (PA_5)o.
altb 3:a951d699878b 27 .. ..
altb 3:a951d699878b 28 .. ..
altb 3:a951d699878b 29 .. ENC CH A o.
altb 3:a951d699878b 30 o. GND .. 10.
altb 3:a951d699878b 31 o. ENC CH B ..
altb 3:a951d699878b 32 .. ..
altb 3:a951d699878b 33 .. ..
pmic 5:d6c7ccbbce78 34 .o AIN acx (PA_0) ..
altb 3:a951d699878b 35 .o AIN acy (PA_1) .. 5.
altb 3:a951d699878b 36 .o AIN Gyro(PA_4) .o Analog GND
altb 3:a951d699878b 37 .. ..
pmic 5:d6c7ccbbce78 38 .. ..
pmic 5:d6c7ccbbce78 39 .. .. 1.
altb 3:a951d699878b 40 ----------------------------
rtlabor 0:15be70d21d7c 41 CN7 CN10
rtlabor 0:15be70d21d7c 42 */
pmic 5:d6c7ccbbce78 43
pmic 5:d6c7ccbbce78 44 Serial pc(SERIAL_TX, SERIAL_RX); // serial connection via USB - programmer
pmic 5:d6c7ccbbce78 45 InterruptIn Button(USER_BUTTON); // User Button, short presses: reduce speed, long presses: increase speed
pmic 5:d6c7ccbbce78 46 AnalogIn ax(PA_0); // analog IN (acc x) on PA_0
pmic 5:d6c7ccbbce78 47 AnalogIn ay(PA_1); // analog IN (acc y) on PA_1
pmic 5:d6c7ccbbce78 48 AnalogIn gz(PA_4); // analog IN (gyr z) on PB_0
pmic 5:d6c7ccbbce78 49 AnalogOut out(PA_5); // analog OUT 1.6 V -> 0A 3.2A -> 2A (see ESCON)
pmic 5:d6c7ccbbce78 50 Ticker ControllerLoopTimer; // interrupt for control loop
pmic 5:d6c7ccbbce78 51 Timer t; // timer to analyse Button
pmic 5:d6c7ccbbce78 52
pmic 5:d6c7ccbbce78 53 // controller parameters etc.
pmic 15:ed33be6f040e 54 float Ts = 0.0025f; // sample time
pmic 15:ed33be6f040e 55 float v_max = 200.0f; // maximum speed rad/s
pmic 23:1d24b1033371 56 float n_soll = 15.0f; // nominal speed for speed control tests
pmic 15:ed33be6f040e 57 float tau = 1.00f; // time constant of complementary filter
pmic 5:d6c7ccbbce78 58
pmic 5:d6c7ccbbce78 59 // output and statemachine
pmic 5:d6c7ccbbce78 60 unsigned int k = 0; // counter for serial output
pmic 5:d6c7ccbbce78 61 bool doesStand = 0; // state if the cube is standing or not
pmic 5:d6c7ccbbce78 62 bool shouldBalance = 0; // state if the controller is active
pmic 13:a308f5e6c306 63 bool shouldGoDown = 0; // state if the controller should swing down
pmic 5:d6c7ccbbce78 64
pmic 5:d6c7ccbbce78 65 // set up encoder
pmic 15:ed33be6f040e 66 EncoderCounter MotorEncoder(PB_6, PB_7); // initialize counter on PB_6 and PB_7
pmic 8:d68e177e2571 67 DiffCounter MotorDiff(1/(2.0f*pi*80.0f), Ts); // discrete differentiate, based on encoder data
pmic 5:d6c7ccbbce78 68
pmic 15:ed33be6f040e 69 // set up controllers
pmic 15:ed33be6f040e 70 float maxCurrent = 15.0f; // 1/0.217 A/Nm -> 2.0/0.217 = 13.82 A
pmic 15:ed33be6f040e 71 float Km = 1/0.217f; // Motorgain: Torque -> km -> Current in A/Nm
pmic 8:d68e177e2571 72 float maxTorque = maxCurrent/Km;
pmic 12:6287235b2570 73 PI_Cntrl pi_w2zero(-.012f, 0.8f, Ts, maxTorque); // controller to bring motor speed to zero while balancing
pmic 15:ed33be6f040e 74 PI_Cntrl pi_w(0.6f, 0.4f, Ts, maxTorque); // PI controller for test purposes motor speed (no balance)
pmic 10:a28f393c6716 75 float desTorque = 0.0f;
pmic 10:a28f393c6716 76
pmic 15:ed33be6f040e 77 // set up gpa
pmic 10:a28f393c6716 78 float fMin = 1.0f;
pmic 10:a28f393c6716 79 float fMax = 1.0f/2.0f/Ts*0.9f;
pmic 10:a28f393c6716 80 int NfexcDes = 150;
pmic 23:1d24b1033371 81 float Aexc0 = 3.0f;
pmic 23:1d24b1033371 82 float Aexc1 = 0.5f; //Aexc0/fMax;
pmic 23:1d24b1033371 83 int NperMin = 10;
pmic 23:1d24b1033371 84 float TmeasMin = 2.0f;
pmic 10:a28f393c6716 85 int NmeasMin = (int)ceil(TmeasMin/Ts);
pmic 10:a28f393c6716 86 GPA Wobble(fMin, fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1);
pmic 11:ed2638662dfa 87 float inpWobble = 0.0f;
pmic 11:ed2638662dfa 88 float outWobble = 0.0f;
pmic 11:ed2638662dfa 89 float excWobble = 0.0f;
pmic 5:d6c7ccbbce78 90
pmic 23:1d24b1033371 91 // set up prbs generator
pmic 23:1d24b1033371 92 int n = 9;
pmic 27:8f1ab2a2267b 93 float Aprbs = 1.0f;
pmic 23:1d24b1033371 94 int Nprbs = (int)pow((float)2, (float)n) - 1;
pmic 23:1d24b1033371 95 int Nexp = 50;
pmic 23:1d24b1033371 96 PRBS prbs(n);
pmic 23:1d24b1033371 97
pmic 15:ed33be6f040e 98 // set up filters for complementary filter
pmic 13:a308f5e6c306 99 IIR_filter FilterACCx(tau, Ts, 1.0f); // 1st order LP for complementary filter acc_x
pmic 13:a308f5e6c306 100 IIR_filter FilterACCy(tau, Ts, 1.0f); // 1st order LP for complementary filter acc_y
pmic 5:d6c7ccbbce78 101 IIR_filter FilterGYRZ(tau, Ts, tau); // 1st order LP for complementary filter gyro
pmic 5:d6c7ccbbce78 102
pmic 15:ed33be6f040e 103 // set up filter for swing down process
pmic 13:a308f5e6c306 104 IIR_filter FilterTrajectory(10.0f, 1.0f, Ts, 1.0f);
pmic 13:a308f5e6c306 105
pmic 5:d6c7ccbbce78 106 // linear characteristics
altb 21:cef093edb441 107 LinearCharacteristics i2u(-15.0f,15.0f,0.0f,3.2f / 3.3f); // output is normalized output
altb 21:cef093edb441 108 LinearCharacteristics u2ax(0.29719f,0.69768f,-9.81f,9.81f); // use normalized input
pmic 23:1d24b1033371 109 LinearCharacteristics u2ay(0.29890f,0.70330f,-9.81f,9.81f); // use normalized input
altb 21:cef093edb441 110 LinearCharacteristics u2gz(-4.6517f * 3.3f,0.45495f); // use normalized input, change sign
altb 21:cef093edb441 111 // 4.6517f = 1/3.752e-3 [V / °/s] * pi/180
rtlabor 0:15be70d21d7c 112
pmic 5:d6c7ccbbce78 113 // user defined functions
rtlabor 0:15be70d21d7c 114 void updateControllers(void); // speed controller loop (via interrupt)
pmic 5:d6c7ccbbce78 115 void pressed(void); // user Button pressed
pmic 5:d6c7ccbbce78 116 void released(void); // user Button released
pmic 8:d68e177e2571 117 void printLine();
pmic 23:1d24b1033371 118 //void printStart();
rtlabor 0:15be70d21d7c 119
pmic 5:d6c7ccbbce78 120 // main program and control loop
pmic 5:d6c7ccbbce78 121 // -----------------------------------------------------------------------------
rtlabor 0:15be70d21d7c 122 int main()
rtlabor 0:15be70d21d7c 123 {
pmic 5:d6c7ccbbce78 124 // for serial comm.
pmic 5:d6c7ccbbce78 125 pc.baud(2000000);
pmic 5:d6c7ccbbce78 126
pmic 5:d6c7ccbbce78 127 // reset encoder, controller and filters
pmic 5:d6c7ccbbce78 128 MotorEncoder.reset();
pmic 5:d6c7ccbbce78 129 MotorDiff.reset(0.0f,0.0f);
rtlabor 0:15be70d21d7c 130 pi_w2zero.reset(0.0f);
pmic 5:d6c7ccbbce78 131 pi_w.reset(0.0f);
pmic 5:d6c7ccbbce78 132
altb 21:cef093edb441 133 FilterACCx.reset(u2ax(ax.read()));
altb 21:cef093edb441 134 FilterACCy.reset(u2ay(ay.read()));
altb 21:cef093edb441 135 FilterGYRZ.reset(u2gz(gz.read()));
pmic 13:a308f5e6c306 136
pmic 13:a308f5e6c306 137 FilterTrajectory.reset(0.0f);
pmic 13:a308f5e6c306 138
pmic 10:a28f393c6716 139 Wobble.reset();
pmic 23:1d24b1033371 140 //Wobble.printGPAmeasPara();
pmic 11:ed2638662dfa 141 inpWobble = 0.0f;
pmic 11:ed2638662dfa 142 outWobble = 0.0f;
pmic 11:ed2638662dfa 143 excWobble = 0.0f;
pmic 23:1d24b1033371 144
pmic 23:1d24b1033371 145 prbs.reset();
pmic 23:1d24b1033371 146 // prbs.printPRBSind();
pmic 23:1d24b1033371 147 // prbs.printPRBSregister();
pmic 23:1d24b1033371 148 // printLine();
pmic 23:1d24b1033371 149
pmic 5:d6c7ccbbce78 150 // reset output
pmic 10:a28f393c6716 151 desTorque = 0.0f;
pmic 23:1d24b1033371 152 out.write(i2u(desTorque*Km));
pmic 5:d6c7ccbbce78 153
pmic 15:ed33be6f040e 154 // reset statemachine
pmic 13:a308f5e6c306 155 shouldGoDown = 0;
pmic 13:a308f5e6c306 156 shouldBalance = 0;
pmic 13:a308f5e6c306 157
pmic 5:d6c7ccbbce78 158 // attach controller loop to timer interrupt
rtlabor 0:15be70d21d7c 159 ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = 400Hz;
pmic 5:d6c7ccbbce78 160 Button.fall(&pressed); // attach key pressed function
pmic 5:d6c7ccbbce78 161 Button.rise(&released); // attach key pressed function
rtlabor 0:15be70d21d7c 162 }
pmic 5:d6c7ccbbce78 163
pmic 5:d6c7ccbbce78 164 void updateControllers(void)
pmic 5:d6c7ccbbce78 165 {
pmic 8:d68e177e2571 166
pmic 5:d6c7ccbbce78 167 // read encoder data
pmic 5:d6c7ccbbce78 168 short counts = MotorEncoder; // counts in 1
pmic 5:d6c7ccbbce78 169 float omega = MotorDiff(counts); // angular velofity motor
pmic 5:d6c7ccbbce78 170
pmic 5:d6c7ccbbce78 171 // read imu data
altb 21:cef093edb441 172 float accx = u2ax(ax.read());
altb 21:cef093edb441 173 float accy = u2ay(ay.read());
altb 21:cef093edb441 174 float gyrz = u2gz(gz.read());
pmic 5:d6c7ccbbce78 175
pmic 5:d6c7ccbbce78 176 // perform complementary filter
pmic 5:d6c7ccbbce78 177 float ang = atan2(-FilterACCx(accx), FilterACCy(accy)) + FilterGYRZ(gyrz) + pi/4.0f;
pmic 5:d6c7ccbbce78 178
pmic 5:d6c7ccbbce78 179 // get current state of the cube
pmic 5:d6c7ccbbce78 180 float actualAngleDegree = ang*180.0f/pi;
pmic 8:d68e177e2571 181 if(actualAngleDegree > -10.0f && actualAngleDegree < 10.0f) {
pmic 5:d6c7ccbbce78 182 doesStand = 1;
pmic 8:d68e177e2571 183 } else {
pmic 5:d6c7ccbbce78 184 doesStand = 0;
pmic 5:d6c7ccbbce78 185 }
pmic 5:d6c7ccbbce78 186
pmic 5:d6c7ccbbce78 187 // update controllers
pmic 8:d68e177e2571 188 if(shouldBalance) {
pmic 23:1d24b1033371 189
pmic 23:1d24b1033371 190 /*
pmic 10:a28f393c6716 191 // balance, set n_soll = 0.0f
pmic 15:ed33be6f040e 192 // ---------------------------------------------------------------------
pmic 23:1d24b1033371 193 // K matrix: [-5.2142 -0.6247] // from Matlab
pmic 13:a308f5e6c306 194 float uPI = pi_w2zero(n_soll - omega); // needs further inverstigation
pmic 23:1d24b1033371 195 float uSS = (-5.2142f*ang -0.6247f*gyrz);
pmic 13:a308f5e6c306 196 desTorque = uPI - uSS; // state space controller for balance, calc desired Torque
pmic 8:d68e177e2571 197 if(abs(desTorque) > maxTorque) {
pmic 8:d68e177e2571 198 desTorque = copysign(maxTorque, desTorque);
pmic 8:d68e177e2571 199 }
pmic 8:d68e177e2571 200 if(k == 0) printLine();
pmic 13:a308f5e6c306 201 if(k++ < 2000) pc.printf("%6.4f %6.4f %6.4f %6.4f\r\n", uPI, uSS, ang, omega);
pmic 23:1d24b1033371 202 */
pmic 15:ed33be6f040e 203
pmic 13:a308f5e6c306 204 /*
pmic 15:ed33be6f040e 205 // step response velocity controller, set n_soll = 0.0f
pmic 15:ed33be6f040e 206 // ---------------------------------------------------------------------
pmic 13:a308f5e6c306 207 desTorque = pi_w(10.0f - omega);
pmic 10:a28f393c6716 208 if(k == 0) printLine();
pmic 10:a28f393c6716 209 if(k++ < 2000) pc.printf("%6.4f %6.4f %6.4f\r\n", 10.0f, omega, desTorque);
pmic 10:a28f393c6716 210 */
pmic 15:ed33be6f040e 211
pmic 17:802aede7b90e 212 /*
pmic 15:ed33be6f040e 213 // gpa measurement, set n_soll = 15.0f
pmic 15:ed33be6f040e 214 // ---------------------------------------------------------------------
pmic 11:ed2638662dfa 215 // measuring the plant P and the closed loop tf T = PC/(1 + PC)
pmic 10:a28f393c6716 216 desTorque = pi_w(n_soll + excWobble - omega);
pmic 11:ed2638662dfa 217 inpWobble = desTorque;
pmic 11:ed2638662dfa 218 outWobble = omega;
pmic 23:1d24b1033371 219 excWobble = Wobble(inpWobble, outWobble);
pmic 11:ed2638662dfa 220 // measuring the controller C and the closed loop tf SC = C/(1 + PC)
pmic 11:ed2638662dfa 221 // desTorque = pi_w(n_soll + excWobble - omega);
pmic 11:ed2638662dfa 222 // inpWobble = n_soll + excWobble - omega;
pmic 11:ed2638662dfa 223 // outWobble = desTorque;
pmic 15:ed33be6f040e 224 // excWobble = Wobble(inpWobble, outWobble);
pmic 23:1d24b1033371 225 // //if(++k == 73000) Wobble.printGPAmeasTime();
pmic 17:802aede7b90e 226 */
pmic 15:ed33be6f040e 227
pmic 23:1d24b1033371 228 ///*
pmic 23:1d24b1033371 229 // prbs measurement, set n_soll = 10.0f
pmic 23:1d24b1033371 230 // ---------------------------------------------------------------------
pmic 23:1d24b1033371 231 // measuring the closed loop tf T = PC/(1 + PC)
pmic 23:1d24b1033371 232 float excPrbs = Aprbs*prbs();
pmic 23:1d24b1033371 233 desTorque = pi_w(n_soll - omega + excPrbs);
pmic 23:1d24b1033371 234 if(k++ < Nexp*Nprbs) pc.printf("%10i %10.3e %10.3e\r\n", k, excPrbs, omega);
pmic 23:1d24b1033371 235 //*/
pmic 23:1d24b1033371 236
pmic 13:a308f5e6c306 237 } else if(shouldGoDown) {
pmic 13:a308f5e6c306 238 // swing down
pmic 23:1d24b1033371 239 // K matrix: [-5.2142 -0.6247] // from Matlab
pmic 23:1d24b1033371 240 // V gain: -2.6080 // from Matlab
pmic 15:ed33be6f040e 241 float ref = FilterTrajectory(pi/4.0f);
pmic 23:1d24b1033371 242 float uV = -2.6080f*ref;
pmic 23:1d24b1033371 243 float uSS = (-5.2142f*ang -0.6247f*gyrz);
pmic 15:ed33be6f040e 244 desTorque = uV - uSS; // state space controller for balance
pmic 13:a308f5e6c306 245 if(abs(desTorque) > maxTorque) {
pmic 13:a308f5e6c306 246 desTorque = copysign(maxTorque, desTorque);
pmic 13:a308f5e6c306 247 }
pmic 13:a308f5e6c306 248 if(abs(ref - ang)/abs(ref) < 0.10f) {
pmic 13:a308f5e6c306 249 shouldGoDown = 0;
pmic 13:a308f5e6c306 250 FilterTrajectory.reset(omega);
pmic 13:a308f5e6c306 251 }
pmic 8:d68e177e2571 252 } else {
pmic 23:1d24b1033371 253 desTorque = pi_w(FilterTrajectory(0.0f)*0 + n_soll - omega); // state space controller for balance
pmic 5:d6c7ccbbce78 254 }
pmic 5:d6c7ccbbce78 255 // convert Nm -> A and write to AOUT
pmic 23:1d24b1033371 256 out.write(i2u(desTorque*Km));
pmic 5:d6c7ccbbce78 257 }
pmic 5:d6c7ccbbce78 258
pmic 5:d6c7ccbbce78 259 // Buttonhandling and statemachine
pmic 5:d6c7ccbbce78 260 // -----------------------------------------------------------------------------
pmic 5:d6c7ccbbce78 261 // start timer as soon as Button is pressed
pmic 5:d6c7ccbbce78 262 void pressed()
pmic 5:d6c7ccbbce78 263 {
pmic 5:d6c7ccbbce78 264 t.start();
pmic 5:d6c7ccbbce78 265 }
pmic 5:d6c7ccbbce78 266
pmic 5:d6c7ccbbce78 267 // evaluating statemachine
pmic 5:d6c7ccbbce78 268 void released()
pmic 5:d6c7ccbbce78 269 {
pmic 5:d6c7ccbbce78 270
pmic 5:d6c7ccbbce78 271 // readout, stop and reset timer
pmic 5:d6c7ccbbce78 272 float ButtonTime = t.read();
pmic 5:d6c7ccbbce78 273 t.stop();
pmic 5:d6c7ccbbce78 274 t.reset();
pmic 5:d6c7ccbbce78 275
pmic 5:d6c7ccbbce78 276 // if the cube doesStand
pmic 5:d6c7ccbbce78 277 if(doesStand) {
pmic 5:d6c7ccbbce78 278 // in - or decrease speed
pmic 12:6287235b2570 279 if(ButtonTime < 3.0f) {
pmic 5:d6c7ccbbce78 280 // press Button long -> increase speed 5 rev/min
pmic 8:d68e177e2571 281 if(ButtonTime > 0.3f) {
pmic 12:6287235b2570 282 n_soll -= 5.0f;
pmic 5:d6c7ccbbce78 283 }
pmic 5:d6c7ccbbce78 284 // press Button short -> decrease speed 5 rev/min
pmic 5:d6c7ccbbce78 285 else {
pmic 12:6287235b2570 286 n_soll += 5.0f;
pmic 5:d6c7ccbbce78 287 }
pmic 5:d6c7ccbbce78 288 // constrain n_soll
pmic 5:d6c7ccbbce78 289 if(n_soll > v_max)
pmic 5:d6c7ccbbce78 290 n_soll = v_max;
pmic 5:d6c7ccbbce78 291 if(n_soll < -v_max)
pmic 5:d6c7ccbbce78 292 n_soll = -v_max;
pmic 5:d6c7ccbbce78 293 }
pmic 5:d6c7ccbbce78 294 // stop balancing
pmic 5:d6c7ccbbce78 295 else {
pmic 5:d6c7ccbbce78 296 n_soll = 0.0f;
pmic 5:d6c7ccbbce78 297 shouldBalance = 0;
pmic 13:a308f5e6c306 298 shouldGoDown = 1;
pmic 13:a308f5e6c306 299 FilterTrajectory.reset(0.0f);
pmic 5:d6c7ccbbce78 300 pi_w2zero.reset(0.0f);
pmic 5:d6c7ccbbce78 301 }
pmic 5:d6c7ccbbce78 302 } else {
pmic 13:a308f5e6c306 303 if(ButtonTime > 3.0f) {
pmic 5:d6c7ccbbce78 304 shouldBalance = 1;
pmic 13:a308f5e6c306 305 shouldGoDown = 0;
pmic 13:a308f5e6c306 306 pi_w.reset(0.0f);
pmic 13:a308f5e6c306 307 }
rtlabor 0:15be70d21d7c 308 }
rtlabor 0:15be70d21d7c 309 }
pmic 8:d68e177e2571 310
pmic 8:d68e177e2571 311 void printLine()
pmic 8:d68e177e2571 312 {
pmic 23:1d24b1033371 313 printf("----------------------------------------------------------------------------------------- \r\n");
pmic 8:d68e177e2571 314 }
pmic 23:1d24b1033371 315
pmic 23:1d24b1033371 316 /*
pmic 23:1d24b1033371 317 void printStart)
pmic 23:1d24b1033371 318 {
pmic 23:1d24b1033371 319 printf("starting \r\n");
pmic 23:1d24b1033371 320 }
pmic 23:1d24b1033371 321 */