RT2_Cuboid_demo / Mbed 2 deprecated nucf446-test_GPA

Test of pmic GPA with filter

Dependencies:   mbed

Fork of nucf446-cuboid-balance1_strong by RT2_Cuboid_demo

main.cpp@9:30ee1d386a1d, 2018-03-22 (annotated)

Committer:
pmic
Date:
Thu Mar 22 17:34:18 2018 +0000
Revision:
9:30ee1d386a1d
Parent:
8:d68e177e2571
Child:
10:600d7cf652e7
ups... forgot to comments something out

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 5:d6c7ccbbce78 10
rtlabor 0:15be70d21d7c 11 /* Cuboid balance on one edge on Nucleo F446RE
pmic 5:d6c7ccbbce78 12 // -----------------------------------------------------------------------------
altb 3:a951d699878b 13
pmic 5:d6c7ccbbce78 14 IMPORTANT: use ..\T-RT\Messen_Ausstellungen\Praesentationen_im_Labor\Wuerfel_nucleo\Escon_Parameter_4nucleo_stark.edc
pmic 5:d6c7ccbbce78 15 settings for Maxon ESCON controller (upload via ESCON Studio)
pmic 5:d6c7ccbbce78 16
rtlabor 0:15be70d21d7c 17 hardware Connections:
pmic 5:d6c7ccbbce78 18
rtlabor 0:15be70d21d7c 19 CN7 CN10
altb 3:a951d699878b 20 : :
altb 3:a951d699878b 21 : :
altb 3:a951d699878b 22 .. ..
altb 3:a951d699878b 23 .. .. 15.
altb 3:a951d699878b 24 .. AOUT i_des on (PA_5)o.
altb 3:a951d699878b 25 .. ..
altb 3:a951d699878b 26 .. ..
altb 3:a951d699878b 27 .. ENC CH A o.
altb 3:a951d699878b 28 o. GND .. 10.
altb 3:a951d699878b 29 o. ENC CH B ..
altb 3:a951d699878b 30 .. ..
altb 3:a951d699878b 31 .. ..
pmic 5:d6c7ccbbce78 32 .o AIN acx (PA_0) ..
altb 3:a951d699878b 33 .o AIN acy (PA_1) .. 5.
altb 3:a951d699878b 34 .o AIN Gyro(PA_4) .o Analog GND
altb 3:a951d699878b 35 .. ..
pmic 5:d6c7ccbbce78 36 .. ..
pmic 5:d6c7ccbbce78 37 .. .. 1.
altb 3:a951d699878b 38 ----------------------------
rtlabor 0:15be70d21d7c 39 CN7 CN10
rtlabor 0:15be70d21d7c 40 */
pmic 5:d6c7ccbbce78 41
pmic 5:d6c7ccbbce78 42 Serial pc(SERIAL_TX, SERIAL_RX); // serial connection via USB - programmer
pmic 5:d6c7ccbbce78 43 InterruptIn Button(USER_BUTTON); // User Button, short presses: reduce speed, long presses: increase speed
pmic 5:d6c7ccbbce78 44 AnalogIn ax(PA_0); // analog IN (acc x) on PA_0
pmic 5:d6c7ccbbce78 45 AnalogIn ay(PA_1); // analog IN (acc y) on PA_1
pmic 5:d6c7ccbbce78 46 AnalogIn gz(PA_4); // analog IN (gyr z) on PB_0
pmic 5:d6c7ccbbce78 47 AnalogOut out(PA_5); // analog OUT 1.6 V -> 0A 3.2A -> 2A (see ESCON)
pmic 5:d6c7ccbbce78 48 Ticker ControllerLoopTimer; // interrupt for control loop
pmic 5:d6c7ccbbce78 49 Timer t; // timer to analyse Button
pmic 5:d6c7ccbbce78 50
pmic 5:d6c7ccbbce78 51 // controller parameters etc.
rtlabor 0:15be70d21d7c 52 float out_value = 1.6f; // set voltage on 1.6 V (0 A current)
rtlabor 0:15be70d21d7c 53 float Ts = 0.0025; // sample time
rtlabor 0:15be70d21d7c 54 float v_max = 200; // maximum speed rad/s
pmic 5:d6c7ccbbce78 55 float n_soll = 0.0f; // nominal speed for speed control tests
pmic 8:d68e177e2571 56 float tau = 1.05f; // time constant of complementary filter
pmic 5:d6c7ccbbce78 57 float fg = 300.0f;
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 5:d6c7ccbbce78 63
pmic 5:d6c7ccbbce78 64 // set up encoder
pmic 8:d68e177e2571 65 EncoderCounter MotorEncoder(PB_6, PB_7); // initialize counter on PB_6 and PB_7
pmic 8:d68e177e2571 66 DiffCounter MotorDiff(1/(2.0f*pi*80.0f), Ts); // discrete differentiate, based on encoder data
pmic 5:d6c7ccbbce78 67
pmic 8:d68e177e2571 68 // 1/0.217 A/Nm -> 2.0/0.217 = 13.82 A
pmic 8:d68e177e2571 69 float maxCurrent = 15.0f;
pmic 8:d68e177e2571 70 float Km = 1/0.217f; // Motorgain: Torque -> km -> Current in A/Nm
pmic 8:d68e177e2571 71 float maxTorque = maxCurrent/Km;
pmic 8:d68e177e2571 72 PI_Cntrl pi_w2zero(-.01f, 0.8f, maxTorque); // controller to bring motor speed to zero while balancing
pmic 8:d68e177e2571 73 PI_Cntrl pi_w(0.5f, 0.2f, maxTorque); // PI controller for test purposes motor speed (no balance)
pmic 5:d6c7ccbbce78 74
pmic 5:d6c7ccbbce78 75 IIR_filter FilterACCx(tau, Ts, 1.0f); // 1st order LP for complementary filter acc_x
pmic 5:d6c7ccbbce78 76 IIR_filter FilterACCy(tau, Ts, 1.0f); // 1st order LP for complementary filter acc_y
pmic 5:d6c7ccbbce78 77 IIR_filter FilterGYRZ(tau, Ts, tau); // 1st order LP for complementary filter gyro
pmic 5:d6c7ccbbce78 78
pmic 8:d68e177e2571 79 // IIR_filter FilterDiffANG(1.0f/(2.0f*pi*180.0f), Ts);
pmic 8:d68e177e2571 80
pmic 5:d6c7ccbbce78 81 // linear characteristics
pmic 8:d68e177e2571 82 LinearCharacteristics i2u(0.1067f, -15.0f); // full range, convert desired current (Amps) -> voltage 0..3.3V
pmic 8:d68e177e2571 83 LinearCharacteristics u2n(312.5f, 1.6f); // convert input voltage (0..3.3V) -> speed (1/min)
pmic 8:d68e177e2571 84 LinearCharacteristics u2w(32.725, 1.6f); // convert input voltage (0..3.3V) -> speed (rad/sec)
pmic 8:d68e177e2571 85 LinearCharacteristics u2ax(14.67f, 1.6378f); // convert input voltage (0..3.3V) -> acc_x m/s^2
pmic 8:d68e177e2571 86 LinearCharacteristics u2ay(15.02f, 1.6673f); // convert input voltage (0..3.3V) -> acc_y m/s^2
pmic 8:d68e177e2571 87 LinearCharacteristics u2gz(-4.652f, 1.4949f); // convert input voltage (0..3.3V) -> w_x rad/s
pmic 8:d68e177e2571 88 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 89
pmic 5:d6c7ccbbce78 90 // user defined functions
rtlabor 0:15be70d21d7c 91 void updateControllers(void); // speed controller loop (via interrupt)
pmic 5:d6c7ccbbce78 92 void pressed(void); // user Button pressed
pmic 5:d6c7ccbbce78 93 void released(void); // user Button released
pmic 8:d68e177e2571 94 void printLine();
rtlabor 0:15be70d21d7c 95
pmic 5:d6c7ccbbce78 96 // main program and control loop
pmic 5:d6c7ccbbce78 97 // -----------------------------------------------------------------------------
rtlabor 0:15be70d21d7c 98 int main()
rtlabor 0:15be70d21d7c 99 {
pmic 5:d6c7ccbbce78 100 // for serial comm.
pmic 5:d6c7ccbbce78 101 pc.baud(2000000);
pmic 5:d6c7ccbbce78 102
pmic 5:d6c7ccbbce78 103 // reset encoder, controller and filters
pmic 5:d6c7ccbbce78 104 MotorEncoder.reset();
pmic 5:d6c7ccbbce78 105 MotorDiff.reset(0.0f,0.0f);
rtlabor 0:15be70d21d7c 106 pi_w2zero.reset(0.0f);
pmic 5:d6c7ccbbce78 107 pi_w.reset(0.0f);
pmic 5:d6c7ccbbce78 108
pmic 5:d6c7ccbbce78 109 FilterACCx.reset(u2ax(3.3f*ax.read()));
pmic 5:d6c7ccbbce78 110 FilterACCy.reset(u2ay(3.3f*ay.read()));
pmic 5:d6c7ccbbce78 111 FilterGYRZ.reset(u2gz(3.3f*gz.read()));
pmic 5:d6c7ccbbce78 112
pmic 9:30ee1d386a1d 113 // FilterDiffANG.reset(u2gz(0.0f));
pmic 8:d68e177e2571 114
pmic 5:d6c7ccbbce78 115 // reset output
pmic 5:d6c7ccbbce78 116 out.write(u3k3_TO_1V(i2u(0.0f)));
pmic 5:d6c7ccbbce78 117
pmic 5:d6c7ccbbce78 118 // attach controller loop to timer interrupt
rtlabor 0:15be70d21d7c 119 ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = 400Hz;
pmic 5:d6c7ccbbce78 120 Button.fall(&pressed); // attach key pressed function
pmic 5:d6c7ccbbce78 121 Button.rise(&released); // attach key pressed function
rtlabor 0:15be70d21d7c 122 }
pmic 5:d6c7ccbbce78 123
pmic 5:d6c7ccbbce78 124 void updateControllers(void)
pmic 5:d6c7ccbbce78 125 {
pmic 8:d68e177e2571 126
pmic 5:d6c7ccbbce78 127 // read encoder data
pmic 5:d6c7ccbbce78 128 short counts = MotorEncoder; // counts in 1
pmic 5:d6c7ccbbce78 129 float omega = MotorDiff(counts); // angular velofity motor
pmic 5:d6c7ccbbce78 130
pmic 5:d6c7ccbbce78 131 // read imu data
pmic 5:d6c7ccbbce78 132 float accx = u2ax(3.3f*ax.read());
pmic 5:d6c7ccbbce78 133 float accy = u2ay(3.3f*ay.read());
pmic 5:d6c7ccbbce78 134 float gyrz = u2gz(3.3f*gz.read());
pmic 5:d6c7ccbbce78 135
pmic 5:d6c7ccbbce78 136 // perform complementary filter
pmic 5:d6c7ccbbce78 137 float ang = atan2(-FilterACCx(accx), FilterACCy(accy)) + FilterGYRZ(gyrz) + pi/4.0f;
pmic 8:d68e177e2571 138 // float dang = FilterDiffANG(ang);
pmic 5:d6c7ccbbce78 139
pmic 5:d6c7ccbbce78 140 // get current state of the cube
pmic 5:d6c7ccbbce78 141 float actualAngleDegree = ang*180.0f/pi;
pmic 8:d68e177e2571 142 if(actualAngleDegree > -10.0f && actualAngleDegree < 10.0f) {
pmic 5:d6c7ccbbce78 143 doesStand = 1;
pmic 8:d68e177e2571 144 } else {
pmic 5:d6c7ccbbce78 145 doesStand = 0;
pmic 5:d6c7ccbbce78 146 }
pmic 5:d6c7ccbbce78 147
pmic 5:d6c7ccbbce78 148 // update controllers
pmic 5:d6c7ccbbce78 149 float desTorque = 0.0f;
pmic 8:d68e177e2571 150 if(shouldBalance) {
pmic 5:d6c7ccbbce78 151 // K matrix: -5.2142 -0.6247 // from Matlab
pmic 8:d68e177e2571 152 float uPiC = pi_w2zero(n_soll - omega - 24.9f); // needs further inverstigation
pmic 8:d68e177e2571 153 float uSsC = (-5.2142f*ang - 0.6247f*gyrz);
pmic 8:d68e177e2571 154 desTorque = uPiC - uSsC; // state space controller for balance, calc desired Torque
pmic 8:d68e177e2571 155 if(abs(desTorque) > maxTorque) {
pmic 8:d68e177e2571 156 desTorque = copysign(maxTorque, desTorque);
pmic 8:d68e177e2571 157 }
pmic 8:d68e177e2571 158 if(k == 0) printLine();
pmic 8:d68e177e2571 159 if(k++ < 2000) pc.printf("%6.4f %6.4f %6.4f %6.4f\r\n", uPiC, uSsC, ang, omega);
pmic 8:d68e177e2571 160 } else {
pmic 5:d6c7ccbbce78 161 desTorque = pi_w(n_soll-omega); // state space controller for balance, calc desired Torque
pmic 5:d6c7ccbbce78 162 }
pmic 5:d6c7ccbbce78 163 // convert Nm -> A and write to AOUT
pmic 8:d68e177e2571 164 out.write(u3k3_TO_1V(i2u(desTorque*Km)));
pmic 5:d6c7ccbbce78 165
pmic 8:d68e177e2571 166 // if(k == 0) printLine();
pmic 8:d68e177e2571 167 // if(k++ < 2000) pc.printf("%6.4f %6.4f %6.4f\r\n", accx, accy, gyrz);
pmic 5:d6c7ccbbce78 168 //out.write(u3k3_TO_1V(i2u(pi_w(n_soll-omega)))); // test speed controller
pmic 8:d68e177e2571 169 // if(++k >= 199){
pmic 8:d68e177e2571 170 // k = 0;
pmic 8:d68e177e2571 171 // pc.printf("phi=%3.2f omega=%3.2f omega=%3.2f \r\n", actualAngleDegree, omega, n_soll);
pmic 8:d68e177e2571 172 //}
pmic 8:d68e177e2571 173
pmic 5:d6c7ccbbce78 174 }
pmic 5:d6c7ccbbce78 175
pmic 5:d6c7ccbbce78 176 // Buttonhandling and statemachine
pmic 5:d6c7ccbbce78 177 // -----------------------------------------------------------------------------
pmic 5:d6c7ccbbce78 178 // start timer as soon as Button is pressed
pmic 5:d6c7ccbbce78 179 void pressed()
pmic 5:d6c7ccbbce78 180 {
pmic 5:d6c7ccbbce78 181 t.start();
pmic 5:d6c7ccbbce78 182 }
pmic 5:d6c7ccbbce78 183
pmic 5:d6c7ccbbce78 184 // evaluating statemachine
pmic 5:d6c7ccbbce78 185 void released()
pmic 5:d6c7ccbbce78 186 {
pmic 5:d6c7ccbbce78 187
pmic 5:d6c7ccbbce78 188 // readout, stop and reset timer
pmic 5:d6c7ccbbce78 189 float ButtonTime = t.read();
pmic 5:d6c7ccbbce78 190 t.stop();
pmic 5:d6c7ccbbce78 191 t.reset();
pmic 5:d6c7ccbbce78 192
pmic 5:d6c7ccbbce78 193 // if the cube doesStand
pmic 5:d6c7ccbbce78 194 if(doesStand) {
pmic 5:d6c7ccbbce78 195 // in - or decrease speed
pmic 8:d68e177e2571 196 if(ButtonTime < 4.0f) {
pmic 5:d6c7ccbbce78 197 // press Button long -> increase speed 5 rev/min
pmic 8:d68e177e2571 198 if(ButtonTime > 0.3f) {
pmic 8:d68e177e2571 199 n_soll -= 1.0f;
pmic 5:d6c7ccbbce78 200 }
pmic 5:d6c7ccbbce78 201 // press Button short -> decrease speed 5 rev/min
pmic 5:d6c7ccbbce78 202 else {
pmic 8:d68e177e2571 203 n_soll += 1.0f;
pmic 5:d6c7ccbbce78 204 }
pmic 5:d6c7ccbbce78 205 // constrain n_soll
pmic 5:d6c7ccbbce78 206 if(n_soll > v_max)
pmic 5:d6c7ccbbce78 207 n_soll = v_max;
pmic 5:d6c7ccbbce78 208 if(n_soll < -v_max)
pmic 5:d6c7ccbbce78 209 n_soll = -v_max;
pmic 5:d6c7ccbbce78 210 }
pmic 5:d6c7ccbbce78 211 // stop balancing
pmic 5:d6c7ccbbce78 212 else {
pmic 5:d6c7ccbbce78 213 n_soll = 0.0f;
pmic 5:d6c7ccbbce78 214 shouldBalance = 0;
pmic 5:d6c7ccbbce78 215 pi_w2zero.reset(0.0f);
pmic 5:d6c7ccbbce78 216 }
pmic 5:d6c7ccbbce78 217 } else {
pmic 8:d68e177e2571 218 if(ButtonTime > 4.0f)
pmic 5:d6c7ccbbce78 219 shouldBalance = 1;
pmic 5:d6c7ccbbce78 220 pi_w.reset(0.0f);
rtlabor 0:15be70d21d7c 221 }
rtlabor 0:15be70d21d7c 222 }
pmic 8:d68e177e2571 223
pmic 8:d68e177e2571 224 void printLine()
pmic 8:d68e177e2571 225 {
pmic 8:d68e177e2571 226 printf("-----------------------------------------------------------------------------------------\r\n");
pmic 8:d68e177e2571 227 }