Diff: main.cpp

Revision:

0:acf871f26563

Child:

1:b9dc09f13a41

--- /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