for Infotag
Dependencies: Library_Cntrl Library_Misc
main.cpp@1:f39d75a48955, 2019-03-07 (annotated)
- Committer:
- altb2
- Date:
- Thu Mar 07 07:04:11 2019 +0000
- Revision:
- 1:f39d75a48955
- Parent:
- 0:a56e9c932891
- Child:
- 2:b3aed371adb5
With new Libraries
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb2 | 0:a56e9c932891 | 1 | #include "mbed.h" |
altb2 | 0:a56e9c932891 | 2 | #include "math.h" |
altb2 | 0:a56e9c932891 | 3 | //------------------------------------------ |
altb2 | 0:a56e9c932891 | 4 | #define PI 3.1415927f |
altb2 | 0:a56e9c932891 | 5 | //------------------------------------------ |
altb2 | 0:a56e9c932891 | 6 | #include "EncoderCounter.h" |
altb2 | 0:a56e9c932891 | 7 | #include "DiffCounter.h" |
altb2 | 0:a56e9c932891 | 8 | #include "IIR_filter.h" |
altb2 | 0:a56e9c932891 | 9 | #include "LinearCharacteristics.h" |
altb2 | 0:a56e9c932891 | 10 | #include "PI_Cntrl.h" |
altb2 | 0:a56e9c932891 | 11 | // #include "GPA.h" |
altb2 | 0:a56e9c932891 | 12 | // define STATES: |
altb2 | 0:a56e9c932891 | 13 | #define INIT 0 // at very beginning |
altb2 | 0:a56e9c932891 | 14 | #define FLAT 10 // cuboid is flat, motor is controlled to zero |
altb2 | 0:a56e9c932891 | 15 | #define BALANCE 20 // balancing |
altb2 | 0:a56e9c932891 | 16 | #define SWING_DOWN 30 // move cuboid down |
altb2 | 0:a56e9c932891 | 17 | |
altb2 | 0:a56e9c932891 | 18 | |
altb2 | 0:a56e9c932891 | 19 | /* Cuboid balance on one edge on Nucleo F446RE |
altb2 | 0:a56e9c932891 | 20 | |
altb2 | 0:a56e9c932891 | 21 | **** IMPORTANT: use ..\Labormodelle\RT-MOD054 - Würfel\Escon_Parameter_4nucleo.edc |
altb2 | 0:a56e9c932891 | 22 | settings for Maxon ESCON controller (upload via ESCON Studio) **** |
altb2 | 0:a56e9c932891 | 23 | hardware Connections: |
altb2 | 0:a56e9c932891 | 24 | |
altb2 | 0:a56e9c932891 | 25 | CN7 CN10 |
altb2 | 0:a56e9c932891 | 26 | : : |
altb2 | 0:a56e9c932891 | 27 | : : |
altb2 | 0:a56e9c932891 | 28 | .. .. |
altb2 | 0:a56e9c932891 | 29 | .. .. 15. |
altb2 | 0:a56e9c932891 | 30 | .. AOUT i_des on (PA_5)o. |
altb2 | 0:a56e9c932891 | 31 | .. .. |
altb2 | 0:a56e9c932891 | 32 | .. .. |
altb2 | 0:a56e9c932891 | 33 | .. ENC CH A o. |
altb2 | 0:a56e9c932891 | 34 | o. GND .. 10. |
altb2 | 0:a56e9c932891 | 35 | o. ENC CH B .. |
altb2 | 0:a56e9c932891 | 36 | .. .. |
altb2 | 0:a56e9c932891 | 37 | .. .. |
altb2 | 0:a56e9c932891 | 38 | .o AIN acx (PA_0) .. |
altb2 | 0:a56e9c932891 | 39 | .o AIN acy (PA_1) .. 5. |
altb2 | 0:a56e9c932891 | 40 | .o AIN Gyro(PA_4) .o Analog GND |
altb2 | 0:a56e9c932891 | 41 | .. .. |
altb2 | 0:a56e9c932891 | 42 | .. .. |
altb2 | 0:a56e9c932891 | 43 | .. .. 1. |
altb2 | 0:a56e9c932891 | 44 | ---------------------------- |
altb2 | 0:a56e9c932891 | 45 | CN7 CN10 |
altb2 | 0:a56e9c932891 | 46 | */ |
altb2 | 0:a56e9c932891 | 47 | Serial pc(SERIAL_TX, SERIAL_RX); // serial connection via USB - programmer |
altb2 | 0:a56e9c932891 | 48 | InterruptIn button(USER_BUTTON); // User Button, short presses: reduce speed, long presses: increase speed |
altb2 | 0:a56e9c932891 | 49 | bool key_was_pressed = false; |
altb2 | 0:a56e9c932891 | 50 | AnalogIn ax(PA_0); // Analog IN (acc x) on PA_0 |
altb2 | 0:a56e9c932891 | 51 | AnalogIn ay(PA_1); // Analog IN (acc y) on PA_1 |
altb2 | 0:a56e9c932891 | 52 | AnalogIn gz(PA_4); // Analog IN (gyr z) on PA_4 |
altb2 | 0:a56e9c932891 | 53 | AnalogOut out(PA_5); // Analog OUT on PA_5 1.6 V -> 0A 3.2A -> 2A (see ESCON) |
altb2 | 0:a56e9c932891 | 54 | float out_value = 1.6f; // set voltage on 1.6 V (0 A current) |
altb2 | 0:a56e9c932891 | 55 | float w_soll = 10.0f; // desired velocity |
altb2 | 0:a56e9c932891 | 56 | float Ts = 0.002f; // sample time of main loops |
altb2 | 0:a56e9c932891 | 57 | int k = 0; |
altb2 | 0:a56e9c932891 | 58 | float phi1_des = 0.0f; |
altb2 | 0:a56e9c932891 | 59 | int CS = INIT; |
altb2 | 0:a56e9c932891 | 60 | void pressed(void); // user Button pressed |
altb2 | 0:a56e9c932891 | 61 | void released(void); // user Button released |
altb2 | 0:a56e9c932891 | 62 | |
altb2 | 0:a56e9c932891 | 63 | |
altb2 | 0:a56e9c932891 | 64 | //------------------------------------------ |
altb2 | 0:a56e9c932891 | 65 | // ... here define variables like gains etc. |
altb2 | 0:a56e9c932891 | 66 | //------------------------------------------ |
altb2 | 0:a56e9c932891 | 67 | LinearCharacteristics i2u(-15.0f,15.0f,0.0f,3.2f / 3.3f); // output is normalized output |
altb2 | 0:a56e9c932891 | 68 | LinearCharacteristics u2ax(0.29719f,0.69768f,-9.81f,9.81f); // use normalized input |
altb2 | 0:a56e9c932891 | 69 | LinearCharacteristics u2ay(0.29890,0.70330f,-9.81f,9.81f); // use normalized input |
altb2 | 0:a56e9c932891 | 70 | LinearCharacteristics u2gz(-4.6517f * 3.3f,0.45495f); // use normalized input |
altb2 | 0:a56e9c932891 | 71 | // 4.6517f = 1/3.752e-3 [V / °/s] * pi/180 |
altb2 | 0:a56e9c932891 | 72 | |
altb2 | 0:a56e9c932891 | 73 | //-------------DEFINE FILTERS---------------- |
altb2 | 0:a56e9c932891 | 74 | float tau = 1.0f; |
altb2 | 0:a56e9c932891 | 75 | IIR_filter f_ax(tau,Ts,1.0f); // filter ax signals |
altb2 | 0:a56e9c932891 | 76 | IIR_filter f_ay(tau,Ts,1.0f); // filter ay signals |
altb2 | 0:a56e9c932891 | 77 | IIR_filter f_gz(tau,Ts,tau); // filter gz signals |
altb2 | 0:a56e9c932891 | 78 | //------------------------------------------ |
altb2 | 0:a56e9c932891 | 79 | float vel = 0.0f; // velocity of motor |
altb2 | 0:a56e9c932891 | 80 | float gyro = 0.0f; |
altb2 | 0:a56e9c932891 | 81 | float phi1 = 0.0f; |
altb2 | 0:a56e9c932891 | 82 | //------------------------------------------ |
altb2 | 0:a56e9c932891 | 83 | Ticker ControllerLoopTimer; // interrupt for control loop |
altb2 | 0:a56e9c932891 | 84 | EncoderCounter counter1(PB_6, PB_7); // initialize counter on PB_6 and PB_7 |
altb2 | 0:a56e9c932891 | 85 | DiffCounter diff(0.01,Ts); // discrete differentiate, based on encoder data |
altb2 | 0:a56e9c932891 | 86 | Timer ti; // define global timer |
altb2 | 0:a56e9c932891 | 87 | Timer t_but; // define global timer |
altb2 | 0:a56e9c932891 | 88 | PI_Cntrl vel_cntrl(0.5f,.05f,Ts,0.5f); // velocity controller for motor |
altb2 | 0:a56e9c932891 | 89 | PI_Cntrl om2zero(-0.02f,4.0f,Ts,0.9f); // slow vel. contrl. to bring motor to zero |
altb2 | 0:a56e9c932891 | 90 | //------------------------------------------ |
altb2 | 0:a56e9c932891 | 91 | // ----- User defined functions ----------- |
altb2 | 0:a56e9c932891 | 92 | void updateLoop(void); // loop for State machine (via interrupt) |
altb2 | 0:a56e9c932891 | 93 | float cuboid_stab_cntrl(int); // stabalizer |
altb2 | 0:a56e9c932891 | 94 | void calc_angle_phi1(int); |
altb2 | 0:a56e9c932891 | 95 | // ------ END User defined functions ------ |
altb2 | 0:a56e9c932891 | 96 | |
altb2 | 0:a56e9c932891 | 97 | //****************************************************************************** |
altb2 | 0:a56e9c932891 | 98 | //---------- main loop ------------- |
altb2 | 0:a56e9c932891 | 99 | //****************************************************************************** |
altb2 | 0:a56e9c932891 | 100 | int main() |
altb2 | 0:a56e9c932891 | 101 | { |
altb2 | 0:a56e9c932891 | 102 | pc.baud(2000000); // for serial comm. |
altb2 | 0:a56e9c932891 | 103 | counter1.reset(); // encoder reset |
altb2 | 0:a56e9c932891 | 104 | diff.reset(0.0f,0); |
altb2 | 0:a56e9c932891 | 105 | ControllerLoopTimer.attach(&updateLoop, Ts); //Assume Fs = ...; |
altb2 | 0:a56e9c932891 | 106 | ti.reset(); |
altb2 | 0:a56e9c932891 | 107 | ti.start(); |
altb2 | 0:a56e9c932891 | 108 | calc_angle_phi1(1); |
altb2 | 0:a56e9c932891 | 109 | button.fall(&pressed); // attach key pressed function |
altb2 | 0:a56e9c932891 | 110 | button.rise(&released); // attach key pressed function |
altb2 | 0:a56e9c932891 | 111 | |
altb2 | 0:a56e9c932891 | 112 | } // END OF main |
altb2 | 0:a56e9c932891 | 113 | //****************************************************************************** |
altb2 | 0:a56e9c932891 | 114 | //---------- main loop (called via interrupt) ------------- |
altb2 | 0:a56e9c932891 | 115 | //****************************************************************************** |
altb2 | 0:a56e9c932891 | 116 | void updateLoop(void){ |
altb2 | 0:a56e9c932891 | 117 | short counts = counter1; // get counts from Encoder |
altb2 | 0:a56e9c932891 | 118 | vel = diff(counts); // motor velocity |
altb2 | 0:a56e9c932891 | 119 | float torq = 0.0f; // set motor torque to zero (will be overwritten) |
altb2 | 0:a56e9c932891 | 120 | float dt = ti.read(); // time elapsed in current state |
altb2 | 0:a56e9c932891 | 121 | calc_angle_phi1(0); // angle phi1 is calculated in every loop |
altb2 | 0:a56e9c932891 | 122 | // ****************** STATE MACHINE *************************************** |
altb2 | 1:f39d75a48955 | 123 | switch(CS) { |
altb2 | 0:a56e9c932891 | 124 | case INIT: // at very beginning |
altb2 | 0:a56e9c932891 | 125 | if (dt > 2.0f){ |
altb2 | 0:a56e9c932891 | 126 | CS = FLAT; // switch to FLAT state |
altb2 | 0:a56e9c932891 | 127 | ti.reset(); |
altb2 | 0:a56e9c932891 | 128 | } |
altb2 | 0:a56e9c932891 | 129 | break; |
altb2 | 0:a56e9c932891 | 130 | case FLAT: // cuboid is flat, keep motor velocity to zero |
altb2 | 0:a56e9c932891 | 131 | torq = vel_cntrl(0.0f - vel); |
altb2 | 0:a56e9c932891 | 132 | if (key_was_pressed && dt > 1.0f){ |
altb2 | 0:a56e9c932891 | 133 | CS = BALANCE; |
altb2 | 0:a56e9c932891 | 134 | torq = cuboid_stab_cntrl(1); |
altb2 | 0:a56e9c932891 | 135 | ti.reset(); |
altb2 | 0:a56e9c932891 | 136 | key_was_pressed = false; |
altb2 | 0:a56e9c932891 | 137 | phi1_des = 0.0f; |
altb2 | 0:a56e9c932891 | 138 | vel_cntrl.reset(0.0f); // reset velocity controller for the next time |
altb2 | 0:a56e9c932891 | 139 | } |
altb2 | 0:a56e9c932891 | 140 | break; |
altb2 | 0:a56e9c932891 | 141 | case BALANCE: // balance the cube |
altb2 | 0:a56e9c932891 | 142 | torq = cuboid_stab_cntrl(0); |
altb2 | 0:a56e9c932891 | 143 | if (key_was_pressed && dt > 1.0f){ |
altb2 | 0:a56e9c932891 | 144 | CS = SWING_DOWN; |
altb2 | 0:a56e9c932891 | 145 | phi1_des = 0.0f; |
altb2 | 0:a56e9c932891 | 146 | ti.reset(); |
altb2 | 0:a56e9c932891 | 147 | key_was_pressed = false; |
altb2 | 0:a56e9c932891 | 148 | } |
altb2 | 0:a56e9c932891 | 149 | break; |
altb2 | 0:a56e9c932891 | 150 | case SWING_DOWN: |
altb2 | 0:a56e9c932891 | 151 | phi1_des = dt; // ramp the desired angle up to pi/4 |
altb2 | 0:a56e9c932891 | 152 | torq = cuboid_stab_cntrl(0);// call balance routine |
altb2 | 0:a56e9c932891 | 153 | if (dt > 1.0f){ // good value for smooth moving down |
altb2 | 0:a56e9c932891 | 154 | CS = FLAT; // move to flat |
altb2 | 0:a56e9c932891 | 155 | phi1_des = 0.0f; |
altb2 | 0:a56e9c932891 | 156 | ti.reset(); |
altb2 | 0:a56e9c932891 | 157 | } |
altb2 | 0:a56e9c932891 | 158 | break; |
altb2 | 0:a56e9c932891 | 159 | default: break; |
altb2 | 0:a56e9c932891 | 160 | } |
altb2 | 0:a56e9c932891 | 161 | out.write( i2u(torq/0.217f)); // motor const. is 0.217, |
altb2 | 0:a56e9c932891 | 162 | // transform M -> i -> u -> normalized output |
altb2 | 0:a56e9c932891 | 163 | } // END OF updateLoop(void) |
altb2 | 0:a56e9c932891 | 164 | |
altb2 | 0:a56e9c932891 | 165 | //****************************************************************************** |
altb2 | 0:a56e9c932891 | 166 | // ************ stabalizing controller ***************** |
altb2 | 0:a56e9c932891 | 167 | //****************************************************************************** |
altb2 | 0:a56e9c932891 | 168 | |
altb2 | 0:a56e9c932891 | 169 | float cuboid_stab_cntrl(int do_reset){ |
altb2 | 0:a56e9c932891 | 170 | /* | phi_des |
altb2 | 0:a56e9c932891 | 171 | v |
altb2 | 0:a56e9c932891 | 172 | --- |
altb2 | 0:a56e9c932891 | 173 | | V | feed forward gain |
altb2 | 0:a56e9c932891 | 174 | --- |
altb2 | 0:a56e9c932891 | 175 | | |
altb2 | 0:a56e9c932891 | 176 | ------- v -------- |
altb2 | 0:a56e9c932891 | 177 | 0 -->O-->|om2zero|-->O---->| System |--o---> x = [phi1 phi1_t] |
altb2 | 0:a56e9c932891 | 178 | ^- ------- ^- -------- | |
altb2 | 0:a56e9c932891 | 179 | | | | |
altb2 | 0:a56e9c932891 | 180 | vel | --- | |
altb2 | 0:a56e9c932891 | 181 | --- -| K |<----- |
altb2 | 0:a56e9c932891 | 182 | --- |
altb2 | 0:a56e9c932891 | 183 | */ |
altb2 | 0:a56e9c932891 | 184 | if(do_reset == 1) // reset controller |
altb2 | 0:a56e9c932891 | 185 | om2zero.reset(0.0f); |
altb2 | 0:a56e9c932891 | 186 | |
altb2 | 0:a56e9c932891 | 187 | float M_des = om2zero(0.0f-vel); // outer controller to bring motor to zero |
altb2 | 0:a56e9c932891 | 188 | float torq = M_des + (-7.1*phi1_des) -(-9.6910f * phi1 -0.7119f * gyro); // calculationof gains are based on the MATLAB script at the end of this file!!! |
altb2 | 0:a56e9c932891 | 189 | // output PI V K(1) K(2) |
altb2 | 0:a56e9c932891 | 190 | if(++k >= 249){ |
altb2 | 0:a56e9c932891 | 191 | k = 0; |
altb2 | 0:a56e9c932891 | 192 | pc.printf("phi: %1.5f, Torq: %1.2f, M_soll %1.2f \r\n",phi1,torq,M_des); |
altb2 | 0:a56e9c932891 | 193 | } |
altb2 | 0:a56e9c932891 | 194 | return torq; |
altb2 | 0:a56e9c932891 | 195 | } |
altb2 | 0:a56e9c932891 | 196 | // ************ calculate angle ***************** |
altb2 | 0:a56e9c932891 | 197 | void calc_angle_phi1(int do_reset){ |
altb2 | 0:a56e9c932891 | 198 | gyro = u2gz(gz.read()); |
altb2 | 0:a56e9c932891 | 199 | if(do_reset == 1){ |
altb2 | 0:a56e9c932891 | 200 | f_ax.reset(u2ax(ax.read())); |
altb2 | 0:a56e9c932891 | 201 | f_ay.reset(u2ax(ay.read())); |
altb2 | 0:a56e9c932891 | 202 | f_gz.reset(gyro); |
altb2 | 0:a56e9c932891 | 203 | } |
altb2 | 0:a56e9c932891 | 204 | phi1 = atan2(-f_ax(u2ax(ax.read())),f_ay(u2ay(ay.read()))) + PI/4.0f + f_gz(gyro); |
altb2 | 0:a56e9c932891 | 205 | } |
altb2 | 0:a56e9c932891 | 206 | |
altb2 | 0:a56e9c932891 | 207 | // start timer as soon as Button is pressed |
altb2 | 0:a56e9c932891 | 208 | void pressed() |
altb2 | 0:a56e9c932891 | 209 | { |
altb2 | 0:a56e9c932891 | 210 | t_but.start(); |
altb2 | 0:a56e9c932891 | 211 | key_was_pressed = false; |
altb2 | 0:a56e9c932891 | 212 | } |
altb2 | 0:a56e9c932891 | 213 | |
altb2 | 0:a56e9c932891 | 214 | // evaluating statemachine |
altb2 | 0:a56e9c932891 | 215 | void released() |
altb2 | 0:a56e9c932891 | 216 | { |
altb2 | 0:a56e9c932891 | 217 | |
altb2 | 0:a56e9c932891 | 218 | // readout, stop and reset timer |
altb2 | 0:a56e9c932891 | 219 | float ButtonTime = t_but.read(); |
altb2 | 0:a56e9c932891 | 220 | t_but.stop(); |
altb2 | 0:a56e9c932891 | 221 | t_but.reset(); |
altb2 | 0:a56e9c932891 | 222 | if(ButtonTime > 0.05f) |
altb2 | 0:a56e9c932891 | 223 | key_was_pressed = true; |
altb2 | 0:a56e9c932891 | 224 | } |
altb2 | 0:a56e9c932891 | 225 | |
altb2 | 0:a56e9c932891 | 226 | |
altb2 | 0:a56e9c932891 | 227 | |
altb2 | 0:a56e9c932891 | 228 | /* MATLAB CODE FOR CUBOID: |
altb2 | 0:a56e9c932891 | 229 | %% Skript fuer cuboid Praktikum |
altb2 | 0:a56e9c932891 | 230 | m_geh=.938; % Masse des Gehaeses |
altb2 | 0:a56e9c932891 | 231 | m_sb=1.243; % Masse Scheibe |
altb2 | 0:a56e9c932891 | 232 | m_g=m_geh+m_sb; % Gesamtmasse |
altb2 | 0:a56e9c932891 | 233 | J_geh=.00408; % Traegheit Gehaeuse (CM) |
altb2 | 0:a56e9c932891 | 234 | J_sb=.00531; % Traegheit Scheibe (CM) |
altb2 | 0:a56e9c932891 | 235 | J1=diag([0 0 J_geh]); |
altb2 | 0:a56e9c932891 | 236 | J2=diag([0 0 J_sb]); |
altb2 | 0:a56e9c932891 | 237 | l=.17; % Kantenlaenge |
altb2 | 0:a56e9c932891 | 238 | sy=.085; |
altb2 | 0:a56e9c932891 | 239 | g=9.81; |
altb2 | 0:a56e9c932891 | 240 | phi0=0; |
altb2 | 0:a56e9c932891 | 241 | J_g=J_geh+(m_geh+m_sb)*(l/sqrt(2))^2; |
altb2 | 0:a56e9c932891 | 242 | |
altb2 | 0:a56e9c932891 | 243 | %% Linearisiertes Modell |
altb2 | 0:a56e9c932891 | 244 | A2=[0 1;m_g*g*l/sqrt(2)/J_g 0]; |
altb2 | 0:a56e9c932891 | 245 | B2=[0;-1/J_g]; |
altb2 | 0:a56e9c932891 | 246 | K=place(A2,B2,10*[-1+1j -1-1j]); |
altb2 | 0:a56e9c932891 | 247 | s_cl=ss(A2-B2*K,B2,[1 0],0); |
altb2 | 0:a56e9c932891 | 248 | V=1/dcgain(s_cl); |
altb2 | 0:a56e9c932891 | 249 | |
altb2 | 0:a56e9c932891 | 250 | %% Gesamtsystem mit Scheibe, Zustaende [phi_1 om_1 om2~] (om2~ = Absolutkoordinaten) |
altb2 | 0:a56e9c932891 | 251 | A3=[A2-B2*K [0;0];... |
altb2 | 0:a56e9c932891 | 252 | -1/J_sb*K 0]; % Auf Scheibenbeschleunigung wirken ganz |
altb2 | 0:a56e9c932891 | 253 | % entsprechend die Zustaende x1, x2 ueber die Zustandsrueckfuehrung |
altb2 | 0:a56e9c932891 | 254 | B3=[B2;1/J_sb]; |
altb2 | 0:a56e9c932891 | 255 | C3=[0 -1 1]; % Gemessen wird die Relativgeschwindigkeit zwischen Scheibe-Gehaeuse |
altb2 | 0:a56e9c932891 | 256 | s3=ss(A3,B3,C3,0); |
altb2 | 0:a56e9c932891 | 257 | s3=ss2ss(s3,[1 0 0;0 1 0;0 -1 1]); % Transformiere 3ten Zustand (eigentlich unnoetig) |
altb2 | 0:a56e9c932891 | 258 | Tn=4; |
altb2 | 0:a56e9c932891 | 259 | PI=-tf([Tn 1],[Tn 0]); % Langsamer PI Regler, negatives gain erforderlich!! |
altb2 | 0:a56e9c932891 | 260 | rlocus(s3*PI);grid |
altb2 | 0:a56e9c932891 | 261 | */ |