Mirror actuator for RT2 lab
Dependencies: Library_Cntrl Library_Misc
Diff: main.cpp
- Revision:
- 5:768e10f6d372
- Parent:
- 4:3d8dd3d17564
- Child:
- 6:9ebeffe446e4
--- a/main.cpp Wed Nov 06 08:00:26 2019 +0000 +++ b/main.cpp Thu Feb 25 20:28:45 2021 +0000 @@ -4,259 +4,107 @@ #define PI 3.1415927f //------------------------------------------ #include "EncoderCounter.h" +#include "EncoderCounterIndex.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: +#include "PID_Cntrl.h" +#include "Unwrapper_2pi.h" +#include "path_1d.h" +#include "GPA.h" +#include "ControllerLoop.h" + + - 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 +Serial pc(USBTX, USBRX,115200); +InterruptIn button(USER_BUTTON); // User Button, short and long presses! 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 +float Ts=.001; // sampling time +void pressed(void); +void released(void); +float vel; +//------------- DEFINE FILTERS ---------------- +// missing +//----------------------------------- +Timer glob_ti; +EncoderCounter counter1(PA_6, PC_7); // initialize counter on PA_6 and PC_7 +InterruptIn indexpulse1(PA_8); +EncoderCounterIndex index1(counter1,indexpulse1); +// +EncoderCounter counter2(PB_6, PB_7); // initialize counter on PB_6 and PB_7 +InterruptIn indexpulse2(PB_4); +EncoderCounterIndex index2(counter2,indexpulse2); // initialize counter on PA_6 and PC_7 +// +DiffCounter diff1(0.0008,Ts); // discrete differentiate, based on encoder1 data +DiffCounter diff2(0.0008,Ts); // discrete differentiate, based on encoder2 data +LinearCharacteristics i2pwm(-1.0,1.0,0.01,0.99,.01,.99); +// +PwmOut i_des1(PB_10); +PwmOut i_des2(PA_15); +DigitalOut i_enable(PC_4); +float data[2000][4]; +Unwrapper_2pi uw2pi; //------------------------------------------ // ----- 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 ------ - +ControllerLoop loop(Ts); +path_1d p1; +path_1d p2; +path_1d *current_path; +float A = 2.7; +float dc=0.0; +//GPA myGPA(1, 2500, 100, 30, 20, Ts); +//GPA myGPA(5, 2500, 80, 0.3, 0.3, Ts); +float exc=0.0; // excitation GPA +// f1 f2 N A1 A2 Ts + +// ***************************************************************************** +void rise_edge(void) + { + //glob_ti.reset(); + } +// ***************************************************************************** +void fall_edge(void) + { + uint32_t time_us = glob_ti.read_us(); + dc = (float)time_us/200.0f; + } +//InterruptIn i_des2(PA_6); + + //****************************************************************************** //---------- main loop ------------- //****************************************************************************** int main() { - pc.baud(2000000); // for serial comm. + i_des1.period_us(200); + i_des2.period_us(200); // PWM Frequency of i_desired + i_enable = 0; // disable current first 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 + counter2.reset(); // encoder reset + ThisThread::sleep_for(100); + i_enable = 1; + + glob_ti.start(); + glob_ti.reset(); + printf("Start Mirror\r\n"); + p1.initialize(300,10,A,0,0,0); + p2.initialize(300,10,-A,0,0,A); + for(int wk =0;wk<10;wk++) + { + current_path = &p1; + current_path->start(glob_ti.read()); + while(!current_path->finished) + wait(.1); + current_path = &p2; + current_path->start(glob_ti.read()); + while(!current_path->finished) + wait(.1); + ThisThread::sleep_for(100); + } + i_enable = 0; + while(1) + wait(1); + + } // 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 - */ \ No newline at end of file