Cuboid
Dependencies: mbed
main.cpp
- Committer:
- rtlabor
- Date:
- 2018-01-10
- Revision:
- 0:15be70d21d7c
- Child:
- 1:2e118d67eeae
File content as of revision 0:15be70d21d7c:
#include "mbed.h" #include "math.h" //------------------------------------------ #define PI 3.1415927f //------------------------------------------ #include "EncoderCounter.h" #include "DiffCounter.h" #include "PI_Cntrl.h" #include "IIR_filter.h" #include "LinearCharacteristics.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 : : : : .. .. .. ENC CH A o. o. GND .. 10. o. ENC CH B .. .. .. .. .. .o AIN acy (PA_0) .. .o AIN acy (PA_1) .. 5. .o i_soll(PA_4) .o Analog GND .o AIN Gyro (PB_0) .. .. .. .. .. 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(PB_0); // Analog IN (gyr z) on PB_0 AnalogOut out(PA_4); // Analog OUT 1.6 V -> 0A 3.2A -> 2A (see ESCON) float out_value = 1.6f; // set voltage on 1.6 V (0 A current) float kp = 4.0f; // speed control gain for motor speed cntrl. float Tn = 0.05f; // Integral time " " " float Ts = 0.0025; // sample time float v_max = 200; // maximum speed rad/s //------------------------------------------ float n_soll = 10.0f; // nominal speed for speed control tests float data[1000][2]; // logging data unsigned int k = 0; // standart counter for output unsigned int n = 0; // standart counter for output //------------------------------------------ Ticker ControllerLoopTimer; // interrupt for control loop Timer t; // Timer to analyse Button float TotalTime = 0.0f; float comp_filter_tau =1.0f; // time constant of complementary filter 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 PI_Cntrl pi_w(kp,Tn,2.0f); // PI controller for test purposes motor speed (no balance) PI_Cntrl pi_w2zero(-.01f,1.0f,0.4f); // PI controller to bring motor speed to zero while balancing //------------------------------------------ IIR_filter f_ax(comp_filter_tau,Ts); // 1st order LP for complementary filter acc_x IIR_filter f_ay(comp_filter_tau,Ts); // 1st order LP for complementary filter acc_y IIR_filter f_gz(comp_filter_tau,Ts,comp_filter_tau);// 1st order LP for complementary filter gyro // define some linear characteristics ----------------------------------------- LinearCharacteristics i2u(0.8f,-2.0f); // convert desired current (Amps) -> voltage 0..3.3V LinearCharacteristics u2n(312.5f,1.6f); // convert input voltage (0..3.3V) -> speed (1/min) LinearCharacteristics u2w(32.725,1.6f); // convert input voltage (0..3.3V) -> speed (rad/sec) LinearCharacteristics u2ax(14.67f,1.6378f); // convert input voltage (0..3.3V) -> acc_x m/s^2 LinearCharacteristics u2ay(15.02f ,1.6673f); // convert input voltage (0..3.3V) -> acc_y m/s^2 LinearCharacteristics u2gz(-4.652f,1.4949f); // convert input voltage (0..3.3V) -> w_x rad/s LinearCharacteristics u3k3_TO_1V(0.303030303f,0,3.3f,0.0f);// normalize output voltage (0..3.3)V -> (0..1) V // ----- User defined functions ----------- void updateControllers(void); // speed controller loop (via interrupt) void pressed(void); // user button pressed void released(void); // user button released // ------ 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); pi_w.reset(0.0f); pi_w2zero.reset(0.0f); f_ax.reset(u2ax(3.3f*ax.read())); f_ay.reset(u2ay(3.3f*ay.read())); f_gz.reset(u2gz(3.3f*gz.read())); ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = 400Hz; button.fall(&pressed); // attach key pressed function button.rise(&released); // attach key pressed function } //****************************************************************************** //---------- control loop ------------- //****************************************************************************** void updateControllers(void){ short counts = counter1; // get counts from Encoder float vel = diff(counts); // motor velocity float wz = u2gz(3.3f*gz.read()); // cuboid rot-velocity float ang = atan2(-f_ax(u2ax(3.3f*ax.read())),f_ay(u2ay(3.3f*ay.read())))+f_gz(wz)+PI/4.0f; // compl. filter // K matrix: -5.2142 -0.6247 // from Matlab float desTorque = pi_w2zero(n_soll-vel)-(-5.2142f*ang-0.6247f*wz); // state space controller for balance out.write(u3k3_TO_1V(i2u(desTorque/0.217f))); // convert Nm -> A and write to AOUT //out.write(u3k3_TO_1V(i2u(pi_w(n_soll-vel)))); // test speed controller if(++k >= 99){ k = 0; pc.printf("phi=%3.2f omega=%3.2f \r\n",ang*180.0f/PI,vel); } } //****************************************************************************** //********** User functions like buttens handle etc. ************** //****************************************************************************** // pressed button //****************************************************************************** void pressed() { t.start(); } //****************************************************************************** // analyse pressed button //****************************************************************************** void released() { TotalTime = t.read(); t.stop(); t.reset(); if(TotalTime<0.1f) // short button presses means decrease speed { n_soll -=5; // decrease nominal speed TotalTime = 0.0f; // reset Time } else { n_soll +=5; // otherwise increase n_soll is in rev/min TotalTime = 0.0f; } if(n_soll>v_max) // limit nominal speed n_soll=v_max; if(n_soll<-v_max) n_soll=-v_max; }