Mirror actuator for RT2 lab
Dependencies: Library_Cntrl Library_Misc
main.cpp
- Committer:
- altb2
- Date:
- 2019-11-06
- Revision:
- 4:3d8dd3d17564
- Parent:
- 2:b3aed371adb5
- Child:
- 5:768e10f6d372
File content as of revision 4:3d8dd3d17564:
#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, if(++k >= 249){ k = 0; //pc.printf("phi: %1.5f, Torq: %1.2f \r\n",phi1,torq); pc.printf("ax %1.5f, ay: %1.5f, gyro: %1.5f, ph1: %1.5f\r\n",u2ax(ax.read()),u2ay(ay.read()),u2gz(gz.read()),phi1); } } // 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) 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 */