....
Dependencies: Library_Cntrl Library_Misc_cuboid
Fork of cuboid_balance_ros by
main.cpp
- Committer:
- altb2
- Date:
- 2019-11-22
- Revision:
- 2:9b068d2a7994
- Parent:
- 1:b9dc09f13a41
File content as of revision 2:9b068d2a7994:
#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,115200); // 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() { pc.printf("Start loop...\r\n"); 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 */