altb_pmic / Mbed OS cuboid_balance_ros

....

Dependencies:   Library_Cntrl Library_Misc_cuboid

Fork of cuboid_balance_ros by Ruprecht Altenburger

main.cpp@2:9b068d2a7994, 2019-11-22 (annotated)

Committer:
altb2
Date:
Fri Nov 22 16:46:16 2019 +0000
Revision:
2:9b068d2a7994
Parent:
1:b9dc09f13a41 
updated ros version

Who changed what in which revision?

UserRevisionLine numberNew 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 */