enhanced functionality in V01 vs. V00, V02 finished, conversion to double precsision in V03

Dependencies:   mbed

Committer:
pmic
Date:
Fri Mar 09 17:39:49 2018 +0000
Revision:
8:3b98b6e1ead3
Parent:
7:b3c5116e9fab
Child:
9:4e6f3404d473
GPA working, first measurement performed, results reasonable

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