Ruprecht Altenburger
Simple program for introduction of mirror actuator.
Diff: main.cpp
- Revision:
- 0:a56e9c932891
- Child:
- 1:f39d75a48955
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Mar 07 06:52:26 2019 +0000
@@ -0,0 +1,261 @@
+#include "mbed.h"
+#include "math.h"
+//------------------------------------------
+#define PI 3.1415927f
+//------------------------------------------
+#include "EncoderCounter.h"
+#include "DiffCounter.h"
+#include "IIR_filter.h"
+#include "LinearCharacteristics.h"
+#include "PI_Cntrl.h"
+// #include "GPA.h"
+// define STATES:
+#define INIT 0 // at very beginning
+#define FLAT 10 // cuboid is flat, motor is controlled to zero
+#define BALANCE 20 // balancing
+#define SWING_DOWN 30 // move cuboid down
+
+
+/* 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
+InterruptIn button(USER_BUTTON); // User Button, short presses: reduce speed, long presses: increase speed
+bool key_was_pressed = false;
+AnalogIn ax(PA_0); // Analog IN (acc x) on PA_0
+AnalogIn ay(PA_1); // Analog IN (acc y) on PA_1
+AnalogIn gz(PA_4); // Analog IN (gyr z) on PA_4
+AnalogOut out(PA_5); // Analog OUT on PA_5 1.6 V -> 0A 3.2A -> 2A (see ESCON)
+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 phi1_des = 0.0f;
+int CS = INIT;
+void pressed(void); // user Button pressed
+void released(void); // user Button released
+
+
+//------------------------------------------
+// ... here define variables like gains etc.
+//------------------------------------------
+LinearCharacteristics i2u(-15.0f,15.0f,0.0f,3.2f / 3.3f); // output is normalized output
+LinearCharacteristics u2ax(0.29719f,0.69768f,-9.81f,9.81f); // use normalized input
+LinearCharacteristics u2ay(0.29890,0.70330f,-9.81f,9.81f); // use normalized input
+LinearCharacteristics u2gz(-4.6517f * 3.3f,0.45495f); // use normalized input
+ // 4.6517f = 1/3.752e-3 [V / °/s] * pi/180
+
+//-------------DEFINE FILTERS----------------
+float tau = 1.0f;
+IIR_filter f_ax(tau,Ts,1.0f); // filter ax signals
+IIR_filter f_ay(tau,Ts,1.0f); // filter ay signals
+IIR_filter f_gz(tau,Ts,tau); // filter gz signals
+//------------------------------------------
+float vel = 0.0f; // velocity of motor
+float gyro = 0.0f;
+float phi1 = 0.0f;
+//------------------------------------------
+Ticker ControllerLoopTimer; // interrupt for control loop
+EncoderCounter counter1(PB_6, PB_7); // initialize counter on PB_6 and PB_7
+DiffCounter diff(0.01,Ts); // discrete differentiate, based on encoder data
+Timer ti; // define global timer
+Timer t_but; // define global timer
+PI_Cntrl vel_cntrl(0.5f,.05f,Ts,0.5f); // velocity controller for motor
+PI_Cntrl om2zero(-0.02f,4.0f,Ts,0.9f); // slow vel. contrl. to bring motor to zero
+//------------------------------------------
+// ----- User defined functions -----------
+void updateLoop(void); // loop for State machine (via interrupt)
+float cuboid_stab_cntrl(int); // stabalizer
+void calc_angle_phi1(int);
+// ------ END User defined functions ------
+
+//******************************************************************************
+//---------- main loop -------------
+//******************************************************************************
+int main()
+{
+ pc.baud(2000000); // for serial comm.
+ counter1.reset(); // encoder reset
+ diff.reset(0.0f,0);
+ ControllerLoopTimer.attach(&updateLoop, Ts); //Assume Fs = ...;
+ ti.reset();
+ ti.start();
+ calc_angle_phi1(1);
+ button.fall(&pressed); // attach key pressed function
+ button.rise(&released); // attach key pressed function
+
+} // END OF main
+//******************************************************************************
+//---------- main loop (called via interrupt) -------------
+//******************************************************************************
+void updateLoop(void){
+ short counts = counter1; // get counts from Encoder
+ vel = diff(counts); // motor velocity
+ float torq = 0.0f; // set motor torque to zero (will be overwritten)
+ float dt = ti.read(); // time elapsed in current state
+ calc_angle_phi1(0); // angle phi1 is calculated in every loop
+ // ****************** STATE MACHINE ***************************************
+ switch(CS) {
+ case INIT: // at very beginning
+ if (dt > 2.0f){
+ CS = FLAT; // switch to FLAT state
+ ti.reset();
+ }
+ break;
+ case FLAT: // cuboid is flat, keep motor velocity to zero
+ torq = vel_cntrl(0.0f - vel);
+ if (key_was_pressed && dt > 1.0f){
+ CS = BALANCE;
+ torq = cuboid_stab_cntrl(1);
+ ti.reset();
+ key_was_pressed = false;
+ phi1_des = 0.0f;
+ vel_cntrl.reset(0.0f); // reset velocity controller for the next time
+ }
+ break;
+ case BALANCE: // balance the cube
+ torq = cuboid_stab_cntrl(0);
+ if (key_was_pressed && dt > 1.0f){
+ CS = SWING_DOWN;
+ phi1_des = 0.0f;
+ ti.reset();
+ key_was_pressed = false;
+ }
+ break;
+ case SWING_DOWN:
+ phi1_des = dt; // ramp the desired angle up to pi/4
+ torq = cuboid_stab_cntrl(0);// call balance routine
+ if (dt > 1.0f){ // good value for smooth moving down
+ CS = FLAT; // move to flat
+ phi1_des = 0.0f;
+ ti.reset();
+ }
+ break;
+ default: break;
+ }
+ out.write( i2u(torq/0.217f)); // motor const. is 0.217,
+ // transform M -> i -> u -> normalized output
+} // END OF updateLoop(void)
+
+//******************************************************************************
+// ************ stabalizing controller *****************
+//******************************************************************************
+
+float cuboid_stab_cntrl(int do_reset){
+/* | phi_des
+ v
+ ---
+ | V | feed forward gain
+ ---
+ |
+ ------- v --------
+0 -->O-->|om2zero|-->O---->| System |--o---> x = [phi1 phi1_t]
+ ^- ------- ^- -------- |
+ | | |
+ vel | --- |
+ --- -| K |<-----
+ ---
+*/
+ if(do_reset == 1) // reset controller
+ om2zero.reset(0.0f);
+
+ float M_des = om2zero(0.0f-vel); // outer controller to bring motor to zero
+ float torq = M_des + (-7.1*phi1_des) -(-9.6910f * phi1 -0.7119f * gyro); // calculationof gains are based on the MATLAB script at the end of this file!!!
+ // output PI V K(1) K(2)
+ if(++k >= 249){
+ k = 0;
+ pc.printf("phi: %1.5f, Torq: %1.2f, M_soll %1.2f \r\n",phi1,torq,M_des);
+ }
+ return torq;
+}
+// ************ calculate angle *****************
+void calc_angle_phi1(int do_reset){
+ gyro = u2gz(gz.read());
+ if(do_reset == 1){
+ f_ax.reset(u2ax(ax.read()));
+ f_ay.reset(u2ax(ay.read()));
+ f_gz.reset(gyro);
+ }
+ phi1 = atan2(-f_ax(u2ax(ax.read())),f_ay(u2ay(ay.read()))) + PI/4.0f + f_gz(gyro);
+ }
+
+// start timer as soon as Button is pressed
+void pressed()
+{
+ t_but.start();
+ key_was_pressed = false;
+}
+
+// evaluating statemachine
+void released()
+{
+
+ // readout, stop and reset timer
+ float ButtonTime = t_but.read();
+ t_but.stop();
+ t_but.reset();
+ if(ButtonTime > 0.05f)
+ key_was_pressed = true;
+}
+
+
+
+/* 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