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