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

Dependencies:   mbed

Committer:
pmic
Date:
Thu Mar 01 17:36:46 2018 +0000
Revision:
5:e443d5ad409f
Parent:
4:47581778e863
Child:
6:7ea1a4bac3c2
cubli can now be turned on and off with the button, if it is balancing the button can be used to set the velocity of the wheel

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
pmic 5:e443d5ad409f 46 AnalogIn ax(PA_0); // analog IN (acc x) on PA_0
pmic 5:e443d5ad409f 47 AnalogIn ay(PA_1); // analog IN (acc y) on PA_1
pmic 5:e443d5ad409f 48 AnalogIn gz(PA_4); // analog IN (gyr z) on PB_0
pmic 5:e443d5ad409f 49 AnalogOut out(PA_5); // analog OUT 1.6 V -> 0A 3.2A -> 2A (see ESCON)
pmic 5:e443d5ad409f 50
pmic 5:e443d5ad409f 51 Ticker ControllerLoopTimer; // interrupt for control loop
pmic 5:e443d5ad409f 52 Timer t; // timer to analyse button
pmic 5:e443d5ad409f 53
rtlabor 0:15be70d21d7c 54 float out_value = 1.6f; // set voltage on 1.6 V (0 A current)
rtlabor 0:15be70d21d7c 55 float kp = 4.0f; // speed control gain for motor speed cntrl.
pmic 5:e443d5ad409f 56 float Tn = 0.05f; // integral time " " "
rtlabor 0:15be70d21d7c 57 float Ts = 0.0025; // sample time
rtlabor 0:15be70d21d7c 58 float v_max = 200; // maximum speed rad/s
pmic 5:e443d5ad409f 59 float n_soll = 0.0f; // nominal speed for speed control tests
pmic 5:e443d5ad409f 60
pmic 5:e443d5ad409f 61 unsigned int k = 0; // standart counter for output
pmic 5:e443d5ad409f 62 bool isStanding = 0; // state if the cube is standing or not
pmic 5:e443d5ad409f 63 bool shouldBalance = 0; // state if the controller is active
pmic 5:e443d5ad409f 64
pmic 5:e443d5ad409f 65 float comp_filter_tau =1.0f; // time constant of complementary filter
pmic 5:e443d5ad409f 66
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
pmic 5:e443d5ad409f 70
rtlabor 0:15be70d21d7c 71 IIR_filter f_ax(comp_filter_tau,Ts); // 1st order LP for complementary filter acc_x
rtlabor 0:15be70d21d7c 72 IIR_filter f_ay(comp_filter_tau,Ts); // 1st order LP for complementary filter acc_y
rtlabor 0:15be70d21d7c 73 IIR_filter f_gz(comp_filter_tau,Ts,comp_filter_tau);// 1st order LP for complementary filter gyro
pmic 5:e443d5ad409f 74
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 5:e443d5ad409f 88 // -----------------------------------------------------------------------------
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 5:e443d5ad409f 123 isStanding = 1;
pmic 5:e443d5ad409f 124 }
pmic 5:e443d5ad409f 125 else{
pmic 5:e443d5ad409f 126 isStanding = 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 5:e443d5ad409f 163 // if the cube isStanding
pmic 5:e443d5ad409f 164 if(isStanding)
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 }