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

Dependencies:   mbed

Committer:
pmic
Date:
Thu Mar 01 17:47:55 2018 +0000
Revision:
6:7ea1a4bac3c2
Parent:
5:e443d5ad409f
Child:
7:b3c5116e9fab
make it look better

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:e443d5ad409f 3
rtlabor 0:15be70d21d7c 4 #define PI 3.1415927f
pmic 5:e443d5ad409f 5
rtlabor 0:15be70d21d7c 6 #include "EncoderCounter.h"
rtlabor 0:15be70d21d7c 7 #include "DiffCounter.h"
rtlabor 0:15be70d21d7c 8 #include "PI_Cntrl.h"
rtlabor 0:15be70d21d7c 9 #include "IIR_filter.h"
rtlabor 0:15be70d21d7c 10 #include "LinearCharacteristics.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 5:e443d5ad409f 17
rtlabor 0:15be70d21d7c 18 hardware Connections:
rtlabor 0:15be70d21d7c 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 .. ..
altb 3:a951d699878b 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 .. ..
altb 3:a951d699878b 37 .. ..
altb 3:a951d699878b 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
rtlabor 0:15be70d21d7c 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 5:e443d5ad409f 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)
rtlabor 0:15be70d21d7c 54 float kp = 4.0f; // speed control gain for motor speed cntrl.
pmic 5:e443d5ad409f 55 float Tn = 0.05f; // 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 5:e443d5ad409f 58 float n_soll = 0.0f; // nominal speed for speed control tests
pmic 6:7ea1a4bac3c2 59 float comp_filter_tau =1.0f; // time constant of complementary filter
pmic 5:e443d5ad409f 60
pmic 6:7ea1a4bac3c2 61 // output and statemachine
pmic 6:7ea1a4bac3c2 62 unsigned int k = 0; // counter for serial output
pmic 6:7ea1a4bac3c2 63 bool doesStand = 0; // state if the cube is standing or not
pmic 5:e443d5ad409f 64 bool shouldBalance = 0; // state if the controller is active
pmic 5:e443d5ad409f 65
pmic 6:7ea1a4bac3c2 66 // set up encoder, controller and filter instanzes
rtlabor 0:15be70d21d7c 67 EncoderCounter counter1(PB_6, PB_7); // initialize counter on PB_6 and PB_7
pmic 5:e443d5ad409f 68 DiffCounter diff(0.01, Ts); // discrete differentiate, based on encoder data
pmic 5:e443d5ad409f 69 PI_Cntrl pi_w2zero(-.01f, 1.0f, 0.4f); // controller to bring motor speed to zero while balancing
rtlabor 0:15be70d21d7c 70 IIR_filter f_ax(comp_filter_tau,Ts); // 1st order LP for complementary filter acc_x
rtlabor 0:15be70d21d7c 71 IIR_filter f_ay(comp_filter_tau,Ts); // 1st order LP for complementary filter acc_y
rtlabor 0:15be70d21d7c 72 IIR_filter f_gz(comp_filter_tau,Ts,comp_filter_tau);// 1st order LP for complementary filter gyro
pmic 5:e443d5ad409f 73
pmic 6:7ea1a4bac3c2 74 // linear characteristics
pmic 4:47581778e863 75 LinearCharacteristics i2u(0.1067f,-15.0f); // full range, convert desired current (Amps) -> voltage 0..3.3V
rtlabor 0:15be70d21d7c 76 LinearCharacteristics u2n(312.5f,1.6f); // convert input voltage (0..3.3V) -> speed (1/min)
rtlabor 0:15be70d21d7c 77 LinearCharacteristics u2w(32.725,1.6f); // convert input voltage (0..3.3V) -> speed (rad/sec)
rtlabor 0:15be70d21d7c 78 LinearCharacteristics u2ax(14.67f,1.6378f); // convert input voltage (0..3.3V) -> acc_x m/s^2
rtlabor 0:15be70d21d7c 79 LinearCharacteristics u2ay(15.02f ,1.6673f); // convert input voltage (0..3.3V) -> acc_y m/s^2
rtlabor 0:15be70d21d7c 80 LinearCharacteristics u2gz(-4.652f,1.4949f); // convert input voltage (0..3.3V) -> w_x rad/s
rtlabor 0:15be70d21d7c 81 LinearCharacteristics u3k3_TO_1V(0.303030303f,0,3.3f,0.0f);// normalize output voltage (0..3.3)V -> (0..1) V
rtlabor 0:15be70d21d7c 82
pmic 5:e443d5ad409f 83 // user defined functions
rtlabor 0:15be70d21d7c 84 void updateControllers(void); // speed controller loop (via interrupt)
rtlabor 0:15be70d21d7c 85 void pressed(void); // user button pressed
rtlabor 0:15be70d21d7c 86 void released(void); // user button released
rtlabor 0:15be70d21d7c 87
pmic 6:7ea1a4bac3c2 88 // main program and control loop
pmic 5:e443d5ad409f 89 // -----------------------------------------------------------------------------
rtlabor 0:15be70d21d7c 90 int main()
rtlabor 0:15be70d21d7c 91 {
pmic 5:e443d5ad409f 92 // for serial comm.
pmic 5:e443d5ad409f 93 pc.baud(2000000);
pmic 5:e443d5ad409f 94
pmic 5:e443d5ad409f 95 // reset encoder, controller and filters
pmic 5:e443d5ad409f 96 counter1.reset();
rtlabor 0:15be70d21d7c 97 diff.reset(0.0f,0.0f);
rtlabor 0:15be70d21d7c 98 pi_w2zero.reset(0.0f);
rtlabor 0:15be70d21d7c 99 f_ax.reset(u2ax(3.3f*ax.read()));
rtlabor 0:15be70d21d7c 100 f_ay.reset(u2ay(3.3f*ay.read()));
pmic 5:e443d5ad409f 101 f_gz.reset(u2gz(3.3f*gz.read()));
pmic 5:e443d5ad409f 102
pmic 5:e443d5ad409f 103 // reset output
pmic 5:e443d5ad409f 104 out.write(u3k3_TO_1V(i2u(0.0f)));
pmic 5:e443d5ad409f 105
pmic 5:e443d5ad409f 106 // attach controller loop to timer interrupt
rtlabor 0:15be70d21d7c 107 ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = 400Hz;
rtlabor 0:15be70d21d7c 108 button.fall(&pressed); // attach key pressed function
rtlabor 0:15be70d21d7c 109 button.rise(&released); // attach key pressed function
rtlabor 0:15be70d21d7c 110 }
pmic 5:e443d5ad409f 111
rtlabor 0:15be70d21d7c 112 void updateControllers(void){
pmic 5:e443d5ad409f 113
pmic 5:e443d5ad409f 114 // read sensor data
rtlabor 0:15be70d21d7c 115 short counts = counter1; // get counts from Encoder
rtlabor 0:15be70d21d7c 116 float vel = diff(counts); // motor velocity
rtlabor 0:15be70d21d7c 117 float wz = u2gz(3.3f*gz.read()); // cuboid rot-velocity
pmic 5:e443d5ad409f 118 float ang = atan2( -f_ax(u2ax(3.3f*ax.read())), f_ay(u2ay(3.3f*ay.read()))) + f_gz(wz) + PI/4.0f; // spaghetti compl. filter
pmic 5:e443d5ad409f 119
pmic 5:e443d5ad409f 120 // get current state of the cube
pmic 5:e443d5ad409f 121 float actualAngleDegree = ang*180.0f/PI;
pmic 5:e443d5ad409f 122 if(actualAngleDegree > -10.0f && actualAngleDegree < 10.0f){
pmic 6:7ea1a4bac3c2 123 doesStand = 1;
pmic 5:e443d5ad409f 124 }
pmic 5:e443d5ad409f 125 else{
pmic 6:7ea1a4bac3c2 126 doesStand = 0;
pmic 5:e443d5ad409f 127 }
pmic 5:e443d5ad409f 128
pmic 5:e443d5ad409f 129 // update controller
pmic 5:e443d5ad409f 130 if(shouldBalance){
pmic 5:e443d5ad409f 131 // K matrix: -5.2142 -0.6247 // from Matlab
pmic 5:e443d5ad409f 132 float desTorque = pi_w2zero(n_soll-vel)-(-5.2142f*ang-0.6247f*wz); // state space controller for balance, calc desired Torque
pmic 5:e443d5ad409f 133 // convert Nm -> A and write to AOUT
pmic 5:e443d5ad409f 134 out.write(u3k3_TO_1V(i2u(desTorque/0.217f)));
pmic 5:e443d5ad409f 135 }
pmic 5:e443d5ad409f 136 else{
pmic 5:e443d5ad409f 137 out.write(u3k3_TO_1V(i2u(0.0f)));
pmic 5:e443d5ad409f 138 }
pmic 5:e443d5ad409f 139
rtlabor 0:15be70d21d7c 140 //out.write(u3k3_TO_1V(i2u(pi_w(n_soll-vel)))); // test speed controller
rtlabor 1:2e118d67eeae 141 if(++k >= 199){
rtlabor 0:15be70d21d7c 142 k = 0;
pmic 5:e443d5ad409f 143 pc.printf("phi=%3.2f omega=%3.2f \r\n", actualAngleDegree, vel);
altb 2:252a61a7e8f9 144 //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 145 }
rtlabor 0:15be70d21d7c 146 }
pmic 5:e443d5ad409f 147
pmic 5:e443d5ad409f 148 // buttonhandling and statemachine
pmic 5:e443d5ad409f 149 // -----------------------------------------------------------------------------
pmic 5:e443d5ad409f 150 // start timer as soon as button is pressed
pmic 5:e443d5ad409f 151 void pressed(){
pmic 5:e443d5ad409f 152 t.start();
rtlabor 0:15be70d21d7c 153 }
pmic 5:e443d5ad409f 154
pmic 5:e443d5ad409f 155 // evaluating statemachine
pmic 5:e443d5ad409f 156 void released(){
pmic 5:e443d5ad409f 157
pmic 5:e443d5ad409f 158 // readout, stop and reset timer
pmic 5:e443d5ad409f 159 float buttonTime = t.read();
rtlabor 0:15be70d21d7c 160 t.stop();
pmic 5:e443d5ad409f 161 t.reset();
pmic 5:e443d5ad409f 162
pmic 6:7ea1a4bac3c2 163 // if the cube doesStand
pmic 6:7ea1a4bac3c2 164 if(doesStand)
rtlabor 0:15be70d21d7c 165 {
pmic 5:e443d5ad409f 166 // in - or decrease speed
pmic 5:e443d5ad409f 167 if(buttonTime < 2.0f){
pmic 5:e443d5ad409f 168 // press button long -> increase speed 5 rev/min
pmic 5:e443d5ad409f 169 if(buttonTime > 0.5f){
pmic 5:e443d5ad409f 170 n_soll -= 5.0f;
pmic 5:e443d5ad409f 171 }
pmic 5:e443d5ad409f 172 // press button short -> decrease speed 5 rev/min
pmic 5:e443d5ad409f 173 else{
pmic 5:e443d5ad409f 174 n_soll += 5.0f;
pmic 5:e443d5ad409f 175 }
pmic 5:e443d5ad409f 176 // constrain n_soll
pmic 5:e443d5ad409f 177 if(n_soll > v_max)
pmic 5:e443d5ad409f 178 n_soll = v_max;
pmic 5:e443d5ad409f 179 if(n_soll < -v_max)
pmic 5:e443d5ad409f 180 n_soll = -v_max;
rtlabor 0:15be70d21d7c 181 }
pmic 5:e443d5ad409f 182 // stop balancing
pmic 5:e443d5ad409f 183 else{
pmic 5:e443d5ad409f 184 n_soll = 0.0f;
pmic 5:e443d5ad409f 185 shouldBalance = 0;
pmic 5:e443d5ad409f 186 pi_w2zero.reset(0.0f);
pmic 5:e443d5ad409f 187 }
pmic 5:e443d5ad409f 188 }
pmic 5:e443d5ad409f 189 else{
pmic 5:e443d5ad409f 190 if(buttonTime > 2.0f)
pmic 5:e443d5ad409f 191 shouldBalance = 1;
pmic 5:e443d5ad409f 192 }
rtlabor 0:15be70d21d7c 193 }