cuboid strong
Dependencies: mbed
main.cpp
00001 #include "mbed.h" 00002 #include "math.h" 00003 //------------------------------------------ 00004 #define PI 3.1415927f 00005 //------------------------------------------ 00006 #include "EncoderCounter.h" 00007 #include "DiffCounter.h" 00008 #include "PI_Cntrl.h" 00009 #include "IIR_filter.h" 00010 #include "LinearCharacteristics.h" 00011 /* Cuboid balance on one edge on Nucleo F446RE 00012 00013 **** IMPORTANT: use ..\Labormodelle\RT-MOD054 - Würfel\Escon_Parameter_4nucleo.edc 00014 settings for Maxon ESCON controller (upload via ESCON Studio) **** 00015 hardware Connections: 00016 00017 CN7 CN10 00018 : : 00019 : : 00020 .. .. 00021 .. ENC CH A o. 00022 o. GND .. 10. 00023 o. ENC CH B .. 00024 .. .. 00025 .. .. 00026 .o AIN acx (PA_0) .. 00027 .o AIN acy (PA_1) .. 5. 00028 .o i_soll(PA_4) .o Analog GND 00029 .o AIN Gyro (PB_0) .. 00030 .. .. 00031 .. .. 1. 00032 ------------------------- 00033 CN7 CN10 00034 */ 00035 Serial pc(SERIAL_TX, SERIAL_RX); // serial connection via USB - programmer 00036 InterruptIn button(USER_BUTTON); // User Button, short presses: reduce speed, long presses: increase speed 00037 AnalogIn ax(PA_0); // Analog IN (acc x) on PA_0 00038 AnalogIn ay(PA_1); // Analog IN (acc y) on PA_1 00039 AnalogIn gz(PA_4); // Analog IN (gyr z) on PB_0 00040 AnalogOut out(PA_5); // Analog OUT 1.6 V -> 0A 3.2A -> 2A (see ESCON) 00041 float out_value = 1.6f; // set voltage on 1.6 V (0 A current) 00042 float kp = 4.0f; // speed control gain for motor speed cntrl. 00043 float Tn = 0.05f; // Integral time " " " 00044 float Ts = 0.0025; // sample time 00045 float v_max = 200; // maximum speed rad/s 00046 //------------------------------------------ 00047 float n_soll = 10.0f; // nominal speed for speed control tests 00048 float data[1000][2]; // logging data 00049 unsigned int k = 0; // standart counter for output 00050 unsigned int n = 0; // standart counter for output 00051 //------------------------------------------ 00052 Ticker ControllerLoopTimer; // interrupt for control loop 00053 Timer t; // Timer to analyse Button 00054 float TotalTime = 0.0f; 00055 float comp_filter_tau =1.0f; // time constant of complementary filter 00056 EncoderCounter counter1(PB_6, PB_7); // initialize counter on PB_6 and PB_7 00057 DiffCounter diff(0.01,Ts); // discrete differentiate, based on encoder data 00058 PI_Cntrl pi_w(kp,Tn,2.0f); // PI controller for test purposes motor speed (no balance) 00059 PI_Cntrl pi_w2zero(-.01f,1.0f,0.4f); // PI controller to bring motor speed to zero while balancing 00060 //------------------------------------------ 00061 IIR_filter f_ax(comp_filter_tau,Ts); // 1st order LP for complementary filter acc_x 00062 IIR_filter f_ay(comp_filter_tau,Ts); // 1st order LP for complementary filter acc_y 00063 IIR_filter f_gz(comp_filter_tau,Ts,comp_filter_tau);// 1st order LP for complementary filter gyro 00064 // define some linear characteristics ----------------------------------------- 00065 LinearCharacteristics i2u(0.8f,-2.0f); // convert desired current (Amps) -> voltage 0..3.3V 00066 LinearCharacteristics u2n(312.5f,1.6f); // convert input voltage (0..3.3V) -> speed (1/min) 00067 LinearCharacteristics u2w(32.725,1.6f); // convert input voltage (0..3.3V) -> speed (rad/sec) 00068 LinearCharacteristics u2ax(14.67f,1.6378f); // convert input voltage (0..3.3V) -> acc_x m/s^2 00069 LinearCharacteristics u2ay(15.02f ,1.6673f); // convert input voltage (0..3.3V) -> acc_y m/s^2 00070 LinearCharacteristics u2gz(-4.652f,1.4949f); // convert input voltage (0..3.3V) -> w_x rad/s 00071 LinearCharacteristics u3k3_TO_1V(0.303030303f,0,3.3f,0.0f);// normalize output voltage (0..3.3)V -> (0..1) V 00072 00073 // ----- User defined functions ----------- 00074 void updateControllers(void); // speed controller loop (via interrupt) 00075 void pressed(void); // user button pressed 00076 void released(void); // user button released 00077 // ------ END User defined functions ------ 00078 00079 //****************************************************************************** 00080 //---------- main loop ------------- 00081 //****************************************************************************** 00082 int main() 00083 { 00084 //attach controller loop to timer interrupt 00085 pc.baud(2000000); // for serial comm. 00086 counter1.reset(); // encoder reset 00087 diff.reset(0.0f,0.0f); 00088 pi_w.reset(0.0f); 00089 pi_w2zero.reset(0.0f); 00090 f_ax.reset(u2ax(3.3f*ax.read())); 00091 f_ay.reset(u2ay(3.3f*ay.read())); 00092 f_gz.reset(u2gz(3.3f*gz.read())); 00093 ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = 400Hz; 00094 button.fall(&pressed); // attach key pressed function 00095 button.rise(&released); // attach key pressed function 00096 } 00097 //****************************************************************************** 00098 //---------- control loop ------------- 00099 //****************************************************************************** 00100 void updateControllers(void){ 00101 short counts = counter1; // get counts from Encoder 00102 float vel = diff(counts); // motor velocity 00103 float wz = u2gz(3.3f*gz.read()); // cuboid rot-velocity 00104 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 00105 // K matrix: -5.2142 -0.6247 // from Matlab 00106 float desTorque = pi_w2zero(n_soll-vel)-(-5.2142f*ang-0.6247f*wz); // state space controller for balance, calc desired Torque 00107 out.write(u3k3_TO_1V(i2u(desTorque/0.217f))); // convert Nm -> A and write to AOUT 00108 //out.write(u3k3_TO_1V(i2u(pi_w(n_soll-vel)))); // test speed controller 00109 if(++k >= 199){ 00110 k = 0; 00111 pc.printf("phi=%3.2f omega=%3.2f \r\n",ang*180.0f/PI,vel); 00112 //pc.printf("ax=%3.2f ay=%3.2f gz=%3.2f \r\n",u2ax(3.3f*ax.read()),u2ay(3.3f*ay.read()),wz); 00113 } 00114 } 00115 //****************************************************************************** 00116 //********** User functions like buttens handle etc. ************** 00117 //****************************************************************************** 00118 // pressed button 00119 //****************************************************************************** 00120 void pressed() 00121 { 00122 t.start(); 00123 } 00124 //****************************************************************************** 00125 // analyse pressed button 00126 //****************************************************************************** 00127 void released() 00128 { 00129 TotalTime = t.read(); 00130 t.stop(); 00131 t.reset(); 00132 if(TotalTime<0.1f) // short button presses means decrease speed 00133 { 00134 n_soll -=5; // decrease nominal speed 00135 TotalTime = 0.0f; // reset Time 00136 } 00137 else 00138 { 00139 n_soll +=5; // otherwise increase n_soll is in rev/min 00140 TotalTime = 0.0f; 00141 } 00142 if(n_soll>v_max) // limit nominal speed 00143 n_soll=v_max; 00144 if(n_soll<-v_max) 00145 n_soll=-v_max; 00146 }
Generated on Fri Jul 15 2022 09:11:08 by
1.7.2