....
Dependencies: Library_Cntrl Library_Misc_cuboid
Fork of cuboid_balance_ros by
Diff: main.cpp
- Revision:
- 0:acf871f26563
- Child:
- 1:b9dc09f13a41
diff -r 000000000000 -r acf871f26563 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Mar 08 13:34:59 2019 +0000 @@ -0,0 +1,105 @@ +#include "mbed.h" +#include "math.h" +//------------------------------------------ +#define PI 3.1415927f +//------------------------------------------ +#include "EncoderCounter.h" +#include "DiffCounter.h" +#include "IIR_filter.h" +#include "LinearCharacteristics.h" +#include "update_loop.h" + + +/* 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 +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 vel = 0.0f; // velocity of motor +float gyro = 0.0f; +float phi1 = 0.0f; +//------------------------------------------ +Ticker ControllerLoopTimer; // interrupt for control loop +//------------------------------------------ +// ----- User defined functions ----------- + +//****************************************************************************** +//---------- main loop ------------- +//****************************************************************************** +int main() +{ + update_loop UL(Ts,USER_BUTTON); //Assume Fs = ...; + while(1); +} // END OF main + +//****************************************************************************** +// ************ stabalizing controller ***************** +//****************************************************************************** + + + +/* 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