James Kiwic
/
RT2_P3_students
Fertig
Fork of RT2_P3_students by
main.cpp
- Committer:
- Kiwicjam
- Date:
- 2018-05-07
- Revision:
- 12:1aa5ff8333e8
- Parent:
- 11:a0074351e154
- Child:
- 13:724759951a6f
File content as of revision 12:1aa5ff8333e8:
#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" /* 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 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; //------------------------------------------ // ... here define variables like gains etc. //------------------------------------------ //LinearCharacteristics i2u(0.8f,-2.0f); //LinearCharacteristics i2u(-2.0f,2.0f,0.0f,3.2f); // schwach parametrierung LinearCharacteristics i2u(-15.0f,15.0f,0.0f,3.2f); // starke parametrierung LinearCharacteristics u2ax(0.69630f,0.29792,9.81f,-9.81f); LinearCharacteristics u2ay(0.70012f,0.29750,9.81f,-9.81f); LinearCharacteristics u2gyro(-4.65f,1.5f); //------------------------------------------ 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 //------------------------------------------ // ... here define instantiate classes //------------------------------------------ PI_Cntrl vel_cntrl(0.5f,0.05f,Ts,0.4f); PI_Cntrl omega2zero(-0.002f,2.0f,Ts,0.5); float tau = 1.0f; IIR_filter LPF_gyro(tau,Ts,tau); IIR_filter LPF_accX(tau,Ts,1.0f); IIR_filter LPF_accY(tau,Ts,1.0f); //GPA gpa1(1.0f, 200.0f, 150, 4, 400, Ts, 10.0f, 0.3f); //float excWobble = 0.0f; // GPA(t fMin, t fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1) // ... define some linear characteristics ----------------------------------------- // ----- User defined functions ----------- void updateControllers(void); // speed controller loop (via interrupt) // ------ END User defined functions ------ //****************************************************************************** //---------- main loop ------------- //****************************************************************************** int main() { //attach controller loop to timer interrupt pc.baud(2000000); // for serial comm. counter1.reset(); // encoder reset diff.reset(0.0f,0.0f); ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = ...; } //****************************************************************************** //---------- control loop (called via interrupt) ------------- //****************************************************************************** void updateControllers(void){ short counts = counter1; // get counts from Encoder float vel = diff(counts); // motor velocity //desTorque = vel_cntrl(w_soll - vel); /* outWobble = omega; excWobble = Wobble(excWobble, outWobble); */ //float torq_des = vel_cntrl(w_soll - vel); //out.write(i2u(torq_des/0.217f)); // the controller! convert torque to Amps km = 0.217 Nm/A float gyro_dir = (u2gyro(gz.read()*3.3f)); float gyroV = LPF_gyro.filter(gyro_dir); float accX = LPF_accX.filter(u2ax(ax.read())); float accY = LPF_accY.filter(u2ay(ay.read())); // komplementärfilter float phi_mes = ((PI/4) + atan2(-accX, accY) + gyroV); // regler float torque = omega2zero(0-vel)-(-9.6910f*phi_mes - 0.71190f * gyro_dir); out.write(i2u(torque/0.217f)/3.3f); // OUTPUT if(++k >= 150){ k = 0; //pc.printf("Des. velocity: %3.3f, Velocity: %3.3f\r\n",w_soll,vel); pc.printf("Uax val %3.5f; ax val %3.5f; Uay val %3.5f; ay val %3.5f; gyro %3.5f\r\n",ax.read(),u2ax(ax.read()),ay.read(),u2ay(ay.read()), gz.read()); //pc.printf("Gyro: %3.3f rad/s\r\n",gyroCalc(gz.read())); //pc.printf("Gyro filt: %3.3f, Gyro roh; %3.3f, accx filt: %3.3f, accy filt: %3.3f\r\n", gyroV,gz.read()*3.3f, accX, accY); pc.printf("Phii: %3.3f\r\n", phi_mes); } } //****************************************************************************** //********** User functions like buttens handle etc. ************** //****************************************************************************** // pressed button //****************************************************************************** //...