![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
for Infotag
Dependencies: Library_Cntrl Library_Misc
Diff: main.cpp
- Revision:
- 0:a56e9c932891
- Child:
- 1:f39d75a48955
diff -r 000000000000 -r a56e9c932891 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Mar 07 06:52:26 2019 +0000 @@ -0,0 +1,261 @@ +#include "mbed.h" +#include "math.h" +//------------------------------------------ +#define PI 3.1415927f +//------------------------------------------ +#include "EncoderCounter.h" +#include "DiffCounter.h" +#include "IIR_filter.h" +#include "LinearCharacteristics.h" +#include "PI_Cntrl.h" +// #include "GPA.h" +// define STATES: +#define INIT 0 // at very beginning +#define FLAT 10 // cuboid is flat, motor is controlled to zero +#define BALANCE 20 // balancing +#define SWING_DOWN 30 // move cuboid down + + +/* Cuboid balance on one edge on Nucleo F446RE + + **** IMPORTANT: use ..\Labormodelle\RT-MOD054 - Würfel\Escon_Parameter_4nucleo.edc + settings for Maxon ESCON controller (upload via ESCON Studio) **** +hardware Connections: + + CN7 CN10 + : : + : : + .. .. + .. .. 15. + .. AOUT i_des on (PA_5)o. + .. .. + .. .. + .. ENC CH A o. + o. GND .. 10. + o. ENC CH B .. + .. .. + .. .. + .o AIN acx (PA_0) .. + .o AIN acy (PA_1) .. 5. + .o AIN Gyro(PA_4) .o Analog GND + .. .. + .. .. + .. .. 1. + ---------------------------- + CN7 CN10 + */ +Serial pc(SERIAL_TX, SERIAL_RX); // serial connection via USB - programmer +InterruptIn button(USER_BUTTON); // User Button, short presses: reduce speed, long presses: increase speed +bool key_was_pressed = false; +AnalogIn ax(PA_0); // Analog IN (acc x) on PA_0 +AnalogIn ay(PA_1); // Analog IN (acc y) on PA_1 +AnalogIn gz(PA_4); // Analog IN (gyr z) on PA_4 +AnalogOut out(PA_5); // Analog OUT on PA_5 1.6 V -> 0A 3.2A -> 2A (see ESCON) +float out_value = 1.6f; // set voltage on 1.6 V (0 A current) +float w_soll = 10.0f; // desired velocity +float Ts = 0.002f; // sample time of main loops +int k = 0; +float phi1_des = 0.0f; +int CS = INIT; +void pressed(void); // user Button pressed +void released(void); // user Button released + + +//------------------------------------------ +// ... here define variables like gains etc. +//------------------------------------------ +LinearCharacteristics i2u(-15.0f,15.0f,0.0f,3.2f / 3.3f); // output is normalized output +LinearCharacteristics u2ax(0.29719f,0.69768f,-9.81f,9.81f); // use normalized input +LinearCharacteristics u2ay(0.29890,0.70330f,-9.81f,9.81f); // use normalized input +LinearCharacteristics u2gz(-4.6517f * 3.3f,0.45495f); // use normalized input + // 4.6517f = 1/3.752e-3 [V / °/s] * pi/180 + +//-------------DEFINE FILTERS---------------- +float tau = 1.0f; +IIR_filter f_ax(tau,Ts,1.0f); // filter ax signals +IIR_filter f_ay(tau,Ts,1.0f); // filter ay signals +IIR_filter f_gz(tau,Ts,tau); // filter gz signals +//------------------------------------------ +float vel = 0.0f; // velocity of motor +float gyro = 0.0f; +float phi1 = 0.0f; +//------------------------------------------ +Ticker ControllerLoopTimer; // interrupt for control loop +EncoderCounter counter1(PB_6, PB_7); // initialize counter on PB_6 and PB_7 +DiffCounter diff(0.01,Ts); // discrete differentiate, based on encoder data +Timer ti; // define global timer +Timer t_but; // define global timer +PI_Cntrl vel_cntrl(0.5f,.05f,Ts,0.5f); // velocity controller for motor +PI_Cntrl om2zero(-0.02f,4.0f,Ts,0.9f); // slow vel. contrl. to bring motor to zero +//------------------------------------------ +// ----- User defined functions ----------- +void updateLoop(void); // loop for State machine (via interrupt) +float cuboid_stab_cntrl(int); // stabalizer +void calc_angle_phi1(int); +// ------ END User defined functions ------ + +//****************************************************************************** +//---------- main loop ------------- +//****************************************************************************** +int main() +{ + pc.baud(2000000); // for serial comm. + counter1.reset(); // encoder reset + diff.reset(0.0f,0); + ControllerLoopTimer.attach(&updateLoop, Ts); //Assume Fs = ...; + ti.reset(); + ti.start(); + calc_angle_phi1(1); + button.fall(&pressed); // attach key pressed function + button.rise(&released); // attach key pressed function + +} // END OF main +//****************************************************************************** +//---------- main loop (called via interrupt) ------------- +//****************************************************************************** +void updateLoop(void){ + short counts = counter1; // get counts from Encoder + vel = diff(counts); // motor velocity + float torq = 0.0f; // set motor torque to zero (will be overwritten) + float dt = ti.read(); // time elapsed in current state + calc_angle_phi1(0); // angle phi1 is calculated in every loop + // ****************** STATE MACHINE *************************************** + switch(CS) { + case INIT: // at very beginning + if (dt > 2.0f){ + CS = FLAT; // switch to FLAT state + ti.reset(); + } + break; + case FLAT: // cuboid is flat, keep motor velocity to zero + torq = vel_cntrl(0.0f - vel); + if (key_was_pressed && dt > 1.0f){ + CS = BALANCE; + torq = cuboid_stab_cntrl(1); + ti.reset(); + key_was_pressed = false; + phi1_des = 0.0f; + vel_cntrl.reset(0.0f); // reset velocity controller for the next time + } + break; + case BALANCE: // balance the cube + torq = cuboid_stab_cntrl(0); + if (key_was_pressed && dt > 1.0f){ + CS = SWING_DOWN; + phi1_des = 0.0f; + ti.reset(); + key_was_pressed = false; + } + break; + case SWING_DOWN: + phi1_des = dt; // ramp the desired angle up to pi/4 + torq = cuboid_stab_cntrl(0);// call balance routine + if (dt > 1.0f){ // good value for smooth moving down + CS = FLAT; // move to flat + phi1_des = 0.0f; + ti.reset(); + } + break; + default: break; + } + out.write( i2u(torq/0.217f)); // motor const. is 0.217, + // transform M -> i -> u -> normalized output +} // END OF updateLoop(void) + +//****************************************************************************** +// ************ stabalizing controller ***************** +//****************************************************************************** + +float cuboid_stab_cntrl(int do_reset){ +/* | phi_des + v + --- + | V | feed forward gain + --- + | + ------- v -------- +0 -->O-->|om2zero|-->O---->| System |--o---> x = [phi1 phi1_t] + ^- ------- ^- -------- | + | | | + vel | --- | + --- -| K |<----- + --- +*/ + if(do_reset == 1) // reset controller + om2zero.reset(0.0f); + + float M_des = om2zero(0.0f-vel); // outer controller to bring motor to zero + 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!!! + // output PI V K(1) K(2) + if(++k >= 249){ + k = 0; + pc.printf("phi: %1.5f, Torq: %1.2f, M_soll %1.2f \r\n",phi1,torq,M_des); + } + return torq; +} +// ************ calculate angle ***************** +void calc_angle_phi1(int do_reset){ + gyro = u2gz(gz.read()); + if(do_reset == 1){ + f_ax.reset(u2ax(ax.read())); + f_ay.reset(u2ax(ay.read())); + f_gz.reset(gyro); + } + phi1 = atan2(-f_ax(u2ax(ax.read())),f_ay(u2ay(ay.read()))) + PI/4.0f + f_gz(gyro); + } + +// start timer as soon as Button is pressed +void pressed() +{ + t_but.start(); + key_was_pressed = false; +} + +// evaluating statemachine +void released() +{ + + // readout, stop and reset timer + float ButtonTime = t_but.read(); + t_but.stop(); + t_but.reset(); + if(ButtonTime > 0.05f) + key_was_pressed = true; +} + + + +/* MATLAB CODE FOR CUBOID: +%% Skript fuer cuboid Praktikum + m_geh=.938; % Masse des Gehaeses + m_sb=1.243; % Masse Scheibe + m_g=m_geh+m_sb; % Gesamtmasse + J_geh=.00408; % Traegheit Gehaeuse (CM) + J_sb=.00531; % Traegheit Scheibe (CM) + J1=diag([0 0 J_geh]); + J2=diag([0 0 J_sb]); + l=.17; % Kantenlaenge + sy=.085; + g=9.81; + phi0=0; + J_g=J_geh+(m_geh+m_sb)*(l/sqrt(2))^2; + + %% Linearisiertes Modell + A2=[0 1;m_g*g*l/sqrt(2)/J_g 0]; + B2=[0;-1/J_g]; + K=place(A2,B2,10*[-1+1j -1-1j]); + s_cl=ss(A2-B2*K,B2,[1 0],0); + V=1/dcgain(s_cl); + + %% Gesamtsystem mit Scheibe, Zustaende [phi_1 om_1 om2~] (om2~ = Absolutkoordinaten) + A3=[A2-B2*K [0;0];... + -1/J_sb*K 0]; % Auf Scheibenbeschleunigung wirken ganz + % entsprechend die Zustaende x1, x2 ueber die Zustandsrueckfuehrung + B3=[B2;1/J_sb]; + C3=[0 -1 1]; % Gemessen wird die Relativgeschwindigkeit zwischen Scheibe-Gehaeuse + s3=ss(A3,B3,C3,0); + s3=ss2ss(s3,[1 0 0;0 1 0;0 -1 1]); % Transformiere 3ten Zustand (eigentlich unnoetig) + Tn=4; + PI=-tf([Tn 1],[Tn 0]); % Langsamer PI Regler, negatives gain erforderlich!! + rlocus(s3*PI);grid + */ \ No newline at end of file