Lab1
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "math.h" 00003 //------------------------------------------ 00004 #define PI 3.1415927f 00005 //------------------------------------------ 00006 #include "EncoderCounter.h" 00007 #include "DiffCounter.h" 00008 #include "IIR_filter.h" 00009 #include "LinearCharacteristics.h" 00010 #include "PI_Cntrl.h" 00011 #include "GPA.h" 00012 // define STATES: 00013 #define INIT 0 // at very beginning 00014 #define FLAT 10 // cuboid is flat, motor is controlled to zero 00015 #define BALANCE 20 // balancing 00016 #define SWING_DOWN 30 // move cuboid down 00017 00018 00019 /* Cuboid balance on one edge on Nucleo F446RE 00020 00021 **** IMPORTANT: use ..\Labormodelle\RT-MOD054 - Würfel\Escon_Parameter_4nucleo.edc 00022 settings for Maxon ESCON controller (upload via ESCON Studio) **** 00023 hardware Connections: 00024 00025 CN7 CN10 00026 : : 00027 : : 00028 .. .. 00029 .. .. 15. 00030 .. AOUT i_des on (PA_5)o. 00031 .. .. 00032 .. .. 00033 .. ENC CH A o. 00034 o. GND .. 10. 00035 o. ENC CH B .. 00036 .. .. 00037 .. .. 00038 .o AIN acx (PA_0) .. 00039 .o AIN acy (PA_1) .. 5. 00040 .o AIN Gyro(PA_4) .o Analog GND 00041 .. .. 00042 .. .. 00043 .. .. 1. 00044 ---------------------------- 00045 CN7 CN10 00046 */ 00047 Serial pc(SERIAL_TX, SERIAL_RX); // serial connection via USB - programmer 00048 InterruptIn button(USER_BUTTON); // User Button, short presses: reduce speed, long presses: increase speed 00049 bool key_was_pressed = false; 00050 AnalogIn ax(PA_0); // Analog IN (acc x) on PA_0 00051 AnalogIn ay(PA_1); // Analog IN (acc y) on PA_1 00052 AnalogIn gz(PA_4); // Analog IN (gyr z) on PA_4 00053 AnalogOut out(PA_5); // Analog OUT on PA_5 1.6 V -> 0A 3.2A -> 2A (see ESCON) 00054 float out_value = 1.6f; // set voltage on 1.6 V (0 A current) 00055 float w_soll = 10.0f; // desired velocity 00056 float Ts = 0.002f; // sample time of main loops 00057 int k = 0; 00058 float phi1_des = 0.0f; 00059 int CS = INIT; 00060 void pressed(void); // user Button pressed 00061 void released(void); // user Button released 00062 00063 00064 //------------------------------------------ 00065 // ... here define variables like gains etc. 00066 //------------------------------------------ 00067 LinearCharacteristics i2u(-15.0f,15.0f,0.0f,3.2f / 3.3f); // output is normalized output 00068 LinearCharacteristics u2ax(0.29719f,0.69768f,-9.81f,9.81f); // use normalized input 00069 LinearCharacteristics u2ay(0.29890,0.70330f,-9.81f,9.81f); // use normalized input 00070 LinearCharacteristics u2gz(-4.6517f * 3.3f,0.45495f); // use normalized input 00071 // 4.6517f = 1/3.752e-3 [V / °/s] * pi/180 00072 00073 //-------------DEFINE FILTERS---------------- 00074 float tau = 1.0f; 00075 IIR_filter f_ax(tau,Ts,1.0f); // filter ax signals 00076 IIR_filter f_ay(tau,Ts,1.0f); // filter ay signals 00077 IIR_filter f_gz(tau,Ts,tau); // filter gz signals 00078 //------------------------------------------ 00079 float vel = 0.0f; // velocity of motor 00080 float gyro = 0.0f; 00081 float phi1 = 0.0f; 00082 //------------------------------------------ 00083 Ticker ControllerLoopTimer; // interrupt for control loop 00084 EncoderCounter counter1(PB_6, PB_7); // initialize counter on PB_6 and PB_7 00085 DiffCounter diff(0.01,Ts); // discrete differentiate, based on encoder data 00086 Timer ti; // define global timer 00087 Timer t_but; // define global timer 00088 PI_Cntrl vel_cntrl(0.5f,.05f,Ts,0.5f); // velocity controller for motor 00089 PI_Cntrl om2zero(-0.02f,4.0f,Ts,0.9f); // slow vel. contrl. to bring motor to zero 00090 //------------------------------------------ 00091 // ----- User defined functions ----------- 00092 void updateLoop(void); // loop for State machine (via interrupt) 00093 float cuboid_stab_cntrl(int); // stabalizer 00094 void calc_angle_phi1(int); 00095 // ------ END User defined functions ------ 00096 00097 //****************************************************************************** 00098 //---------- main loop ------------- 00099 //****************************************************************************** 00100 int main() 00101 { 00102 pc.baud(2000000); // for serial comm. 00103 counter1.reset(); // encoder reset 00104 diff.reset(0.0f,0); 00105 ControllerLoopTimer.attach(&updateLoop, Ts); //Assume Fs = ...; 00106 ti.reset(); 00107 ti.start(); 00108 calc_angle_phi1(1); 00109 button.fall(&pressed); // attach key pressed function 00110 button.rise(&released); // attach key pressed function 00111 00112 } // END OF main 00113 //****************************************************************************** 00114 //---------- main loop (called via interrupt) ------------- 00115 //****************************************************************************** 00116 void updateLoop(void){ 00117 short counts = counter1; // get counts from Encoder 00118 vel = diff(counts); // motor velocity 00119 float torq = 0.0f; // set motor torque to zero (will be overwritten) 00120 float dt = ti.read(); // time elapsed in current state 00121 calc_angle_phi1(0); // angle phi1 is calculated in every loop 00122 // ****************** STATE MACHINE *************************************** 00123 switch(CS) { 00124 case INIT: // at very beginning 00125 if (dt > 2.0f){ 00126 CS = FLAT; // switch to FLAT state 00127 ti.reset(); 00128 } 00129 break; 00130 case FLAT: // cuboid is flat, keep motor velocity to zero 00131 torq = vel_cntrl(0.0f - vel); 00132 if (key_was_pressd && dt > 1.0f){ 00133 CS = BALANCE; 00134 torq = cuboid_stab_cntrl(1); 00135 ti.reset(); 00136 key_was_pressed = false; 00137 phi1_des = 0.0f; 00138 vel_cntrl.reset(0.0f); // reset velocity controller for the next time 00139 } 00140 break; 00141 case BALANCE: // balance the cube 00142 torq = cuboid_stab_cntrl(0); 00143 if (key_was_pressd && dt > 1.0f){ 00144 CS = SWING_DOWN; 00145 phi1_des = 0.0f; 00146 ti.reset(); 00147 key_was_pressed = false; 00148 } 00149 break; 00150 case SWING_DOWN: 00151 phi1_des = dt; // ramp the desired angle up to pi/4 00152 torq = cuboid_stab_cntrl(0);// call balance routine 00153 if (dt > 1.0f){ // good value for smooth moving down 00154 CS = FLAT; // move to flat 00155 phi1_des = 0.0f; 00156 ti.reset(); 00157 } 00158 break; 00159 default: break; 00160 } 00161 out.write( i2u(torq/0.217f)); // motor const. is 0.217, 00162 // transform M -> i -> u -> normalized output 00163 } // END OF updateLoop(void) 00164 00165 //****************************************************************************** 00166 // ************ stabalizing controller ***************** 00167 //****************************************************************************** 00168 00169 float cuboid_stab_cntrl(int do_reset){ 00170 /* | phi_des 00171 v 00172 --- 00173 | V | feed forward gain 00174 --- 00175 | 00176 ------- v -------- 00177 0 -->O-->|om2zero|-->O---->| System |--o---> x = [phi1 phi1_t] 00178 ^- ------- ^- -------- | 00179 | | | 00180 vel | --- | 00181 --- -| K |<----- 00182 --- 00183 */ 00184 if(do_reset == 1) // reset controller 00185 om2zero.reset(0.0f); 00186 00187 float M_des = om2zero(0.0f-vel); // outer controller to bring motor to zero 00188 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!!! 00189 // output PI V K(1) K(2) 00190 if(++k >= 249){ 00191 k = 0; 00192 pc.printf("phi: %1.5f, Torq: %1.2f, M_soll %1.2f \r\n",phi1,torq,M_des); 00193 } 00194 return torq; 00195 } 00196 // ************ calculate angle ***************** 00197 void calc_angle_phi1(int do_reset){ 00198 gyro = u2gz(gz.read()); 00199 if(do_reset == 1){ 00200 f_ax.reset(u2ax(ax.read())); 00201 f_ay.reset(u2ax(ay.read())); 00202 f_gz.reset(gyro); 00203 } 00204 phi1 = atan2(-f_ax(u2ax(ax.read())),f_ay(u2ay(ay.read()))) + PI/4.0f + f_gz(gyro); 00205 } 00206 00207 // start timer as soon as Button is pressed 00208 void pressed() 00209 { 00210 t_but.start(); 00211 key_was_pressed = false; 00212 } 00213 00214 // evaluating statemachine 00215 void released() 00216 { 00217 00218 // readout, stop and reset timer 00219 float ButtonTime = t_but.read(); 00220 t_but.stop(); 00221 t_but.reset(); 00222 if(ButtonTime > 0.05f) 00223 key_was_pressed = true; 00224 } 00225 00226 00227 00228 /* MATLAB CODE FOR CUBOID: 00229 %% Skript fuer cuboid Praktikum 00230 m_geh=.938; % Masse des Gehaeses 00231 m_sb=1.243; % Masse Scheibe 00232 m_g=m_geh+m_sb; % Gesamtmasse 00233 J_geh=.00408; % Traegheit Gehaeuse (CM) 00234 J_sb=.00531; % Traegheit Scheibe (CM) 00235 J1=diag([0 0 J_geh]); 00236 J2=diag([0 0 J_sb]); 00237 l=.17; % Kantenlaenge 00238 sy=.085; 00239 g=9.81; 00240 phi0=0; 00241 J_g=J_geh+(m_geh+m_sb)*(l/sqrt(2))^2; 00242 00243 %% Linearisiertes Modell 00244 A2=[0 1;m_g*g*l/sqrt(2)/J_g 0]; 00245 B2=[0;-1/J_g]; 00246 K=place(A2,B2,10*[-1+1j -1-1j]); 00247 s_cl=ss(A2-B2*K,B2,[1 0],0); 00248 V=1/dcgain(s_cl); 00249 00250 %% Gesamtsystem mit Scheibe, Zustaende [phi_1 om_1 om2~] (om2~ = Absolutkoordinaten) 00251 A3=[A2-B2*K [0;0];... 00252 -1/J_sb*K 0]; % Auf Scheibenbeschleunigung wirken ganz 00253 % entsprechend die Zustaende x1, x2 ueber die Zustandsrueckfuehrung 00254 B3=[B2;1/J_sb]; 00255 C3=[0 -1 1]; % Gemessen wird die Relativgeschwindigkeit zwischen Scheibe-Gehaeuse 00256 s3=ss(A3,B3,C3,0); 00257 s3=ss2ss(s3,[1 0 0;0 1 0;0 -1 1]); % Transformiere 3ten Zustand (eigentlich unnoetig) 00258 Tn=4; 00259 PI=-tf([Tn 1],[Tn 0]); % Langsamer PI Regler, negatives gain erforderlich!! 00260 rlocus(s3*PI);grid 00261 */
Generated on Tue Jul 12 2022 18:49:18 by
1.7.2