....
Dependencies: Library_Cntrl Library_Misc_cuboid
Fork of cuboid_balance_ros by
main.cpp@1:b9dc09f13a41, 2019-11-22 (annotated)
- Committer:
- altb2
- Date:
- Fri Nov 22 16:44:48 2019 +0000
- Revision:
- 1:b9dc09f13a41
- Parent:
- 0:acf871f26563
updated ros
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb2 | 0:acf871f26563 | 1 | #include "mbed.h" |
altb2 | 0:acf871f26563 | 2 | #include "math.h" |
altb2 | 0:acf871f26563 | 3 | //------------------------------------------ |
altb2 | 0:acf871f26563 | 4 | #define PI 3.1415927f |
altb2 | 0:acf871f26563 | 5 | //------------------------------------------ |
altb2 | 0:acf871f26563 | 6 | #include "EncoderCounter.h" |
altb2 | 0:acf871f26563 | 7 | #include "DiffCounter.h" |
altb2 | 0:acf871f26563 | 8 | #include "IIR_filter.h" |
altb2 | 0:acf871f26563 | 9 | #include "LinearCharacteristics.h" |
altb2 | 0:acf871f26563 | 10 | #include "update_loop.h" |
altb2 | 0:acf871f26563 | 11 | |
altb2 | 0:acf871f26563 | 12 | |
altb2 | 0:acf871f26563 | 13 | /* Cuboid balance on one edge on Nucleo F446RE |
altb2 | 0:acf871f26563 | 14 | |
altb2 | 0:acf871f26563 | 15 | **** IMPORTANT: use ..\Labormodelle\RT-MOD054 - Würfel\Escon_Parameter_4nucleo.edc |
altb2 | 0:acf871f26563 | 16 | settings for Maxon ESCON controller (upload via ESCON Studio) **** |
altb2 | 0:acf871f26563 | 17 | hardware Connections: |
altb2 | 0:acf871f26563 | 18 | |
altb2 | 0:acf871f26563 | 19 | CN7 CN10 |
altb2 | 0:acf871f26563 | 20 | : : |
altb2 | 0:acf871f26563 | 21 | : : |
altb2 | 0:acf871f26563 | 22 | .. .. |
altb2 | 0:acf871f26563 | 23 | .. .. 15. |
altb2 | 0:acf871f26563 | 24 | .. AOUT i_des on (PA_5)o. |
altb2 | 0:acf871f26563 | 25 | .. .. |
altb2 | 0:acf871f26563 | 26 | .. .. |
altb2 | 0:acf871f26563 | 27 | .. ENC CH A o. |
altb2 | 0:acf871f26563 | 28 | o. GND .. 10. |
altb2 | 0:acf871f26563 | 29 | o. ENC CH B .. |
altb2 | 0:acf871f26563 | 30 | .. .. |
altb2 | 0:acf871f26563 | 31 | .. .. |
altb2 | 0:acf871f26563 | 32 | .o AIN acx (PA_0) .. |
altb2 | 0:acf871f26563 | 33 | .o AIN acy (PA_1) .. 5. |
altb2 | 0:acf871f26563 | 34 | .o AIN Gyro(PA_4) .o Analog GND |
altb2 | 0:acf871f26563 | 35 | .. .. |
altb2 | 0:acf871f26563 | 36 | .. .. |
altb2 | 0:acf871f26563 | 37 | .. .. 1. |
altb2 | 0:acf871f26563 | 38 | ---------------------------- |
altb2 | 0:acf871f26563 | 39 | CN7 CN10 |
altb2 | 0:acf871f26563 | 40 | */ |
altb2 | 1:b9dc09f13a41 | 41 | Serial pc(SERIAL_TX, SERIAL_RX,115200); // serial connection via USB - programmer |
altb2 | 0:acf871f26563 | 42 | float out_value = 1.6f; // set voltage on 1.6 V (0 A current) |
altb2 | 0:acf871f26563 | 43 | float w_soll = 10.0f; // desired velocity |
altb2 | 0:acf871f26563 | 44 | float Ts = 0.002f; // sample time of main loops |
altb2 | 0:acf871f26563 | 45 | int k = 0; |
altb2 | 0:acf871f26563 | 46 | |
altb2 | 0:acf871f26563 | 47 | |
altb2 | 0:acf871f26563 | 48 | //------------------------------------------ |
altb2 | 0:acf871f26563 | 49 | float vel = 0.0f; // velocity of motor |
altb2 | 0:acf871f26563 | 50 | float gyro = 0.0f; |
altb2 | 0:acf871f26563 | 51 | float phi1 = 0.0f; |
altb2 | 0:acf871f26563 | 52 | //------------------------------------------ |
altb2 | 0:acf871f26563 | 53 | Ticker ControllerLoopTimer; // interrupt for control loop |
altb2 | 0:acf871f26563 | 54 | //------------------------------------------ |
altb2 | 0:acf871f26563 | 55 | // ----- User defined functions ----------- |
altb2 | 0:acf871f26563 | 56 | |
altb2 | 0:acf871f26563 | 57 | //****************************************************************************** |
altb2 | 0:acf871f26563 | 58 | //---------- main loop ------------- |
altb2 | 0:acf871f26563 | 59 | //****************************************************************************** |
altb2 | 0:acf871f26563 | 60 | int main() |
altb2 | 0:acf871f26563 | 61 | { |
altb2 | 1:b9dc09f13a41 | 62 | pc.printf("Start loop...\r\n"); |
altb2 | 0:acf871f26563 | 63 | update_loop UL(Ts,USER_BUTTON); //Assume Fs = ...; |
altb2 | 0:acf871f26563 | 64 | while(1); |
altb2 | 0:acf871f26563 | 65 | } // END OF main |
altb2 | 0:acf871f26563 | 66 | |
altb2 | 0:acf871f26563 | 67 | //****************************************************************************** |
altb2 | 0:acf871f26563 | 68 | // ************ stabalizing controller ***************** |
altb2 | 0:acf871f26563 | 69 | //****************************************************************************** |
altb2 | 0:acf871f26563 | 70 | |
altb2 | 0:acf871f26563 | 71 | |
altb2 | 0:acf871f26563 | 72 | |
altb2 | 0:acf871f26563 | 73 | /* MATLAB CODE FOR CUBOID: |
altb2 | 0:acf871f26563 | 74 | %% Skript fuer cuboid Praktikum |
altb2 | 0:acf871f26563 | 75 | m_geh=.938; % Masse des Gehaeses |
altb2 | 0:acf871f26563 | 76 | m_sb=1.243; % Masse Scheibe |
altb2 | 0:acf871f26563 | 77 | m_g=m_geh+m_sb; % Gesamtmasse |
altb2 | 0:acf871f26563 | 78 | J_geh=.00408; % Traegheit Gehaeuse (CM) |
altb2 | 0:acf871f26563 | 79 | J_sb=.00531; % Traegheit Scheibe (CM) |
altb2 | 0:acf871f26563 | 80 | J1=diag([0 0 J_geh]); |
altb2 | 0:acf871f26563 | 81 | J2=diag([0 0 J_sb]); |
altb2 | 0:acf871f26563 | 82 | l=.17; % Kantenlaenge |
altb2 | 0:acf871f26563 | 83 | sy=.085; |
altb2 | 0:acf871f26563 | 84 | g=9.81; |
altb2 | 0:acf871f26563 | 85 | phi0=0; |
altb2 | 0:acf871f26563 | 86 | J_g=J_geh+(m_geh+m_sb)*(l/sqrt(2))^2; |
altb2 | 0:acf871f26563 | 87 | |
altb2 | 0:acf871f26563 | 88 | %% Linearisiertes Modell |
altb2 | 0:acf871f26563 | 89 | A2=[0 1;m_g*g*l/sqrt(2)/J_g 0]; |
altb2 | 0:acf871f26563 | 90 | B2=[0;-1/J_g]; |
altb2 | 0:acf871f26563 | 91 | K=place(A2,B2,10*[-1+1j -1-1j]); |
altb2 | 0:acf871f26563 | 92 | s_cl=ss(A2-B2*K,B2,[1 0],0); |
altb2 | 0:acf871f26563 | 93 | V=1/dcgain(s_cl); |
altb2 | 0:acf871f26563 | 94 | |
altb2 | 0:acf871f26563 | 95 | %% Gesamtsystem mit Scheibe, Zustaende [phi_1 om_1 om2~] (om2~ = Absolutkoordinaten) |
altb2 | 0:acf871f26563 | 96 | A3=[A2-B2*K [0;0];... |
altb2 | 0:acf871f26563 | 97 | -1/J_sb*K 0]; % Auf Scheibenbeschleunigung wirken ganz |
altb2 | 0:acf871f26563 | 98 | % entsprechend die Zustaende x1, x2 ueber die Zustandsrueckfuehrung |
altb2 | 0:acf871f26563 | 99 | B3=[B2;1/J_sb]; |
altb2 | 0:acf871f26563 | 100 | C3=[0 -1 1]; % Gemessen wird die Relativgeschwindigkeit zwischen Scheibe-Gehaeuse |
altb2 | 0:acf871f26563 | 101 | s3=ss(A3,B3,C3,0); |
altb2 | 0:acf871f26563 | 102 | s3=ss2ss(s3,[1 0 0;0 1 0;0 -1 1]); % Transformiere 3ten Zustand (eigentlich unnoetig) |
altb2 | 0:acf871f26563 | 103 | Tn=4; |
altb2 | 0:acf871f26563 | 104 | PI=-tf([Tn 1],[Tn 0]); % Langsamer PI Regler, negatives gain erforderlich!! |
altb2 | 0:acf871f26563 | 105 | rlocus(s3*PI);grid |
altb2 | 0:acf871f26563 | 106 | */ |