RT2_Cuboid_demo / Mbed 2 deprecated nucf446-cuboid-balance1_strong

Cuboid

Dependencies:   mbed

main.cpp@13:a308f5e6c306, 2018-04-05 (annotated)

Committer:
pmic
Date:
Thu Apr 05 10:07:29 2018 +0000
Revision:
13:a308f5e6c306
Parent:
12:6287235b2570
Child:
15:ed33be6f040e
implement swing down logic (it is a bit a mess, but it is working)

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