RT2_Cuboid_demo / Mbed 2 deprecated nucf446-test_GPA

Test of pmic GPA with filter

Dependencies:   mbed

Fork of nucf446-cuboid-balance1_strong by RT2_Cuboid_demo

main.cpp@3:a951d699878b, 2018-03-01 (annotated)

Committer:
altb
Date:
Thu Mar 01 10:59:47 2018 +0000
Revision:
3:a951d699878b
Parent:
2:252a61a7e8f9
Child:
4:6457d61fd234
corrected comments concerning IOs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rtlabor 0:15be70d21d7c 1 #include "mbed.h"
rtlabor 0:15be70d21d7c 2 #include "math.h"
rtlabor 0:15be70d21d7c 3 //------------------------------------------
rtlabor 0:15be70d21d7c 4 #define PI 3.1415927f
rtlabor 0:15be70d21d7c 5 //------------------------------------------
rtlabor 0:15be70d21d7c 6 #include "EncoderCounter.h"
rtlabor 0:15be70d21d7c 7 #include "DiffCounter.h"
rtlabor 0:15be70d21d7c 8 #include "PI_Cntrl.h"
rtlabor 0:15be70d21d7c 9 #include "IIR_filter.h"
rtlabor 0:15be70d21d7c 10 #include "LinearCharacteristics.h"
rtlabor 0:15be70d21d7c 11 /* Cuboid balance on one edge on Nucleo F446RE
rtlabor 0:15be70d21d7c 12
altb 3:a951d699878b 13
rtlabor 0:15be70d21d7c 14 **** IMPORTANT: use ..\Labormodelle\RT-MOD054 - Würfel\Escon_Parameter_4nucleo.edc
rtlabor 0:15be70d21d7c 15 settings for Maxon ESCON controller (upload via ESCON Studio) ****
rtlabor 0:15be70d21d7c 16 hardware Connections:
rtlabor 0:15be70d21d7c 17
rtlabor 0:15be70d21d7c 18 CN7 CN10
altb 3:a951d699878b 19 : :
altb 3:a951d699878b 20 : :
altb 3:a951d699878b 21 .. ..
altb 3:a951d699878b 22 .. .. 15.
altb 3:a951d699878b 23 .. AOUT i_des on (PA_5)o.
altb 3:a951d699878b 24 .. ..
altb 3:a951d699878b 25 .. ..
altb 3:a951d699878b 26 .. ENC CH A o.
altb 3:a951d699878b 27 o. GND .. 10.
altb 3:a951d699878b 28 o. ENC CH B ..
altb 3:a951d699878b 29 .. ..
altb 3:a951d699878b 30 .. ..
altb 3:a951d699878b 31 .o AIN acx (PA_0) ..
altb 3:a951d699878b 32 .o AIN acy (PA_1) .. 5.
altb 3:a951d699878b 33 .o AIN Gyro(PA_4) .o Analog GND
altb 3:a951d699878b 34 .. ..
altb 3:a951d699878b 35 .. ..
altb 3:a951d699878b 36 .. .. 1.
altb 3:a951d699878b 37 ----------------------------
rtlabor 0:15be70d21d7c 38 CN7 CN10
rtlabor 0:15be70d21d7c 39 */
altb 3:a951d699878b 40 Serial pc(SERIAL_TX, SERIAL_RX); // serial connection via USB - programmer
rtlabor 0:15be70d21d7c 41 InterruptIn button(USER_BUTTON); // User Button, short presses: reduce speed, long presses: increase speed
rtlabor 0:15be70d21d7c 42 AnalogIn ax(PA_0); // Analog IN (acc x) on PA_0
rtlabor 0:15be70d21d7c 43 AnalogIn ay(PA_1); // Analog IN (acc y) on PA_1
altb 2:252a61a7e8f9 44 AnalogIn gz(PA_4); // Analog IN (gyr z) on PB_0
altb 2:252a61a7e8f9 45 AnalogOut out(PA_5); // Analog OUT 1.6 V -> 0A 3.2A -> 2A (see ESCON)
rtlabor 0:15be70d21d7c 46 float out_value = 1.6f; // set voltage on 1.6 V (0 A current)
rtlabor 0:15be70d21d7c 47 float kp = 4.0f; // speed control gain for motor speed cntrl.
rtlabor 0:15be70d21d7c 48 float Tn = 0.05f; // Integral time " " "
rtlabor 0:15be70d21d7c 49 float Ts = 0.0025; // sample time
rtlabor 0:15be70d21d7c 50 float v_max = 200; // maximum speed rad/s
rtlabor 0:15be70d21d7c 51 //------------------------------------------
rtlabor 0:15be70d21d7c 52 float n_soll = 10.0f; // nominal speed for speed control tests
rtlabor 0:15be70d21d7c 53 float data[1000][2]; // logging data
rtlabor 0:15be70d21d7c 54 unsigned int k = 0; // standart counter for output
rtlabor 0:15be70d21d7c 55 unsigned int n = 0; // standart counter for output
rtlabor 0:15be70d21d7c 56 //------------------------------------------
rtlabor 0:15be70d21d7c 57 Ticker ControllerLoopTimer; // interrupt for control loop
rtlabor 0:15be70d21d7c 58 Timer t; // Timer to analyse Button
rtlabor 0:15be70d21d7c 59 float TotalTime = 0.0f;
rtlabor 0:15be70d21d7c 60 float comp_filter_tau =1.0f; // time constant of complementary filter
rtlabor 0:15be70d21d7c 61 EncoderCounter counter1(PB_6, PB_7); // initialize counter on PB_6 and PB_7
rtlabor 0:15be70d21d7c 62 DiffCounter diff(0.01,Ts); // discrete differentiate, based on encoder data
rtlabor 0:15be70d21d7c 63 PI_Cntrl pi_w(kp,Tn,2.0f); // PI controller for test purposes motor speed (no balance)
rtlabor 0:15be70d21d7c 64 PI_Cntrl pi_w2zero(-.01f,1.0f,0.4f); // PI controller to bring motor speed to zero while balancing
rtlabor 0:15be70d21d7c 65 //------------------------------------------
rtlabor 0:15be70d21d7c 66 IIR_filter f_ax(comp_filter_tau,Ts); // 1st order LP for complementary filter acc_x
rtlabor 0:15be70d21d7c 67 IIR_filter f_ay(comp_filter_tau,Ts); // 1st order LP for complementary filter acc_y
rtlabor 0:15be70d21d7c 68 IIR_filter f_gz(comp_filter_tau,Ts,comp_filter_tau);// 1st order LP for complementary filter gyro
rtlabor 0:15be70d21d7c 69 // define some linear characteristics -----------------------------------------
rtlabor 0:15be70d21d7c 70 LinearCharacteristics i2u(0.8f,-2.0f); // convert desired current (Amps) -> voltage 0..3.3V
rtlabor 0:15be70d21d7c 71 LinearCharacteristics u2n(312.5f,1.6f); // convert input voltage (0..3.3V) -> speed (1/min)
rtlabor 0:15be70d21d7c 72 LinearCharacteristics u2w(32.725,1.6f); // convert input voltage (0..3.3V) -> speed (rad/sec)
rtlabor 0:15be70d21d7c 73 LinearCharacteristics u2ax(14.67f,1.6378f); // convert input voltage (0..3.3V) -> acc_x m/s^2
rtlabor 0:15be70d21d7c 74 LinearCharacteristics u2ay(15.02f ,1.6673f); // convert input voltage (0..3.3V) -> acc_y m/s^2
rtlabor 0:15be70d21d7c 75 LinearCharacteristics u2gz(-4.652f,1.4949f); // convert input voltage (0..3.3V) -> w_x rad/s
rtlabor 0:15be70d21d7c 76 LinearCharacteristics u3k3_TO_1V(0.303030303f,0,3.3f,0.0f);// normalize output voltage (0..3.3)V -> (0..1) V
rtlabor 0:15be70d21d7c 77
rtlabor 0:15be70d21d7c 78 // ----- User defined functions -----------
rtlabor 0:15be70d21d7c 79 void updateControllers(void); // speed controller loop (via interrupt)
rtlabor 0:15be70d21d7c 80 void pressed(void); // user button pressed
rtlabor 0:15be70d21d7c 81 void released(void); // user button released
rtlabor 0:15be70d21d7c 82 // ------ END User defined functions ------
rtlabor 0:15be70d21d7c 83
rtlabor 0:15be70d21d7c 84 //******************************************************************************
rtlabor 0:15be70d21d7c 85 //---------- main loop -------------
rtlabor 0:15be70d21d7c 86 //******************************************************************************
rtlabor 0:15be70d21d7c 87 int main()
rtlabor 0:15be70d21d7c 88 {
rtlabor 0:15be70d21d7c 89 //attach controller loop to timer interrupt
rtlabor 0:15be70d21d7c 90 pc.baud(2000000); // for serial comm.
rtlabor 0:15be70d21d7c 91 counter1.reset(); // encoder reset
rtlabor 0:15be70d21d7c 92 diff.reset(0.0f,0.0f);
rtlabor 0:15be70d21d7c 93 pi_w.reset(0.0f);
rtlabor 0:15be70d21d7c 94 pi_w2zero.reset(0.0f);
rtlabor 0:15be70d21d7c 95 f_ax.reset(u2ax(3.3f*ax.read()));
rtlabor 0:15be70d21d7c 96 f_ay.reset(u2ay(3.3f*ay.read()));
rtlabor 0:15be70d21d7c 97 f_gz.reset(u2gz(3.3f*gz.read()));
rtlabor 0:15be70d21d7c 98 ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = 400Hz;
rtlabor 0:15be70d21d7c 99 button.fall(&pressed); // attach key pressed function
rtlabor 0:15be70d21d7c 100 button.rise(&released); // attach key pressed function
rtlabor 0:15be70d21d7c 101 }
rtlabor 0:15be70d21d7c 102 //******************************************************************************
rtlabor 0:15be70d21d7c 103 //---------- control loop -------------
rtlabor 0:15be70d21d7c 104 //******************************************************************************
rtlabor 0:15be70d21d7c 105 void updateControllers(void){
rtlabor 0:15be70d21d7c 106 short counts = counter1; // get counts from Encoder
rtlabor 0:15be70d21d7c 107 float vel = diff(counts); // motor velocity
rtlabor 0:15be70d21d7c 108 float wz = u2gz(3.3f*gz.read()); // cuboid rot-velocity
rtlabor 0:15be70d21d7c 109 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
rtlabor 0:15be70d21d7c 110 // K matrix: -5.2142 -0.6247 // from Matlab
rtlabor 1:2e118d67eeae 111 float desTorque = pi_w2zero(n_soll-vel)-(-5.2142f*ang-0.6247f*wz); // state space controller for balance, calc desired Torque
rtlabor 0:15be70d21d7c 112 out.write(u3k3_TO_1V(i2u(desTorque/0.217f))); // convert Nm -> A and write to AOUT
rtlabor 0:15be70d21d7c 113 //out.write(u3k3_TO_1V(i2u(pi_w(n_soll-vel)))); // test speed controller
rtlabor 1:2e118d67eeae 114 if(++k >= 199){
rtlabor 0:15be70d21d7c 115 k = 0;
rtlabor 0:15be70d21d7c 116 pc.printf("phi=%3.2f omega=%3.2f \r\n",ang*180.0f/PI,vel);
altb 2:252a61a7e8f9 117 //pc.printf("ax=%3.2f ay=%3.2f gz=%3.2f \r\n",u2ax(3.3f*ax.read()),u2ay(3.3f*ay.read()),wz);
rtlabor 0:15be70d21d7c 118 }
rtlabor 0:15be70d21d7c 119 }
rtlabor 0:15be70d21d7c 120 //******************************************************************************
rtlabor 0:15be70d21d7c 121 //********** User functions like buttens handle etc. **************
rtlabor 0:15be70d21d7c 122 //******************************************************************************
rtlabor 0:15be70d21d7c 123 // pressed button
rtlabor 0:15be70d21d7c 124 //******************************************************************************
rtlabor 0:15be70d21d7c 125 void pressed()
rtlabor 0:15be70d21d7c 126 {
rtlabor 0:15be70d21d7c 127 t.start();
rtlabor 0:15be70d21d7c 128 }
rtlabor 0:15be70d21d7c 129 //******************************************************************************
rtlabor 0:15be70d21d7c 130 // analyse pressed button
rtlabor 0:15be70d21d7c 131 //******************************************************************************
rtlabor 0:15be70d21d7c 132 void released()
rtlabor 0:15be70d21d7c 133 {
rtlabor 0:15be70d21d7c 134 TotalTime = t.read();
rtlabor 0:15be70d21d7c 135 t.stop();
rtlabor 0:15be70d21d7c 136 t.reset();
rtlabor 0:15be70d21d7c 137 if(TotalTime<0.1f) // short button presses means decrease speed
rtlabor 0:15be70d21d7c 138 {
rtlabor 0:15be70d21d7c 139 n_soll -=5; // decrease nominal speed
rtlabor 0:15be70d21d7c 140 TotalTime = 0.0f; // reset Time
rtlabor 0:15be70d21d7c 141 }
rtlabor 0:15be70d21d7c 142 else
rtlabor 0:15be70d21d7c 143 {
rtlabor 0:15be70d21d7c 144 n_soll +=5; // otherwise increase n_soll is in rev/min
rtlabor 0:15be70d21d7c 145 TotalTime = 0.0f;
rtlabor 0:15be70d21d7c 146 }
rtlabor 0:15be70d21d7c 147 if(n_soll>v_max) // limit nominal speed
rtlabor 0:15be70d21d7c 148 n_soll=v_max;
rtlabor 0:15be70d21d7c 149 if(n_soll<-v_max)
rtlabor 0:15be70d21d7c 150 n_soll=-v_max;
rtlabor 0:15be70d21d7c 151 }