James Kiwic
/
RT2_P3_students
Fertig
Fork of RT2_P3_students by
main.cpp@12:1aa5ff8333e8, 2018-05-07 (annotated)
- Committer:
- Kiwicjam
- Date:
- Mon May 07 09:00:43 2018 +0000
- Revision:
- 12:1aa5ff8333e8
- Parent:
- 11:a0074351e154
- Child:
- 13:724759951a6f
Fertig;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb | 0:78ca29b4c49e | 1 | #include "mbed.h" |
altb | 0:78ca29b4c49e | 2 | #include "math.h" |
altb | 0:78ca29b4c49e | 3 | //------------------------------------------ |
altb | 0:78ca29b4c49e | 4 | #define PI 3.1415927f |
altb | 0:78ca29b4c49e | 5 | //------------------------------------------ |
altb | 0:78ca29b4c49e | 6 | #include "EncoderCounter.h" |
altb | 0:78ca29b4c49e | 7 | #include "DiffCounter.h" |
altb | 0:78ca29b4c49e | 8 | #include "IIR_filter.h" |
altb | 0:78ca29b4c49e | 9 | #include "LinearCharacteristics.h" |
altb | 5:d79c437626e7 | 10 | #include "PI_Cntrl.h" |
altb | 8:8ed679044a72 | 11 | #include "GPA.h" |
altb | 0:78ca29b4c49e | 12 | /* Cuboid balance on one edge on Nucleo F446RE |
altb | 0:78ca29b4c49e | 13 | |
altb | 0:78ca29b4c49e | 14 | **** IMPORTANT: use ..\Labormodelle\RT-MOD054 - Würfel\Escon_Parameter_4nucleo.edc |
altb | 0:78ca29b4c49e | 15 | settings for Maxon ESCON controller (upload via ESCON Studio) **** |
altb | 0:78ca29b4c49e | 16 | hardware Connections: |
altb | 0:78ca29b4c49e | 17 | |
altb | 0:78ca29b4c49e | 18 | CN7 CN10 |
altb | 0:78ca29b4c49e | 19 | : : |
altb | 0:78ca29b4c49e | 20 | : : |
altb | 0:78ca29b4c49e | 21 | .. .. |
altb | 0:78ca29b4c49e | 22 | .. .. 15. |
altb | 0:78ca29b4c49e | 23 | .. AOUT i_des on (PA_5)o. |
altb | 0:78ca29b4c49e | 24 | .. .. |
altb | 0:78ca29b4c49e | 25 | .. .. |
altb | 0:78ca29b4c49e | 26 | .. ENC CH A o. |
altb | 0:78ca29b4c49e | 27 | o. GND .. 10. |
altb | 0:78ca29b4c49e | 28 | o. ENC CH B .. |
altb | 0:78ca29b4c49e | 29 | .. .. |
altb | 0:78ca29b4c49e | 30 | .. .. |
altb | 0:78ca29b4c49e | 31 | .o AIN acx (PA_0) .. |
altb | 0:78ca29b4c49e | 32 | .o AIN acy (PA_1) .. 5. |
altb | 0:78ca29b4c49e | 33 | .o AIN Gyro(PA_4) .o Analog GND |
altb | 0:78ca29b4c49e | 34 | .. .. |
altb | 0:78ca29b4c49e | 35 | .. .. |
altb | 0:78ca29b4c49e | 36 | .. .. 1. |
altb | 0:78ca29b4c49e | 37 | ---------------------------- |
altb | 0:78ca29b4c49e | 38 | CN7 CN10 |
altb | 0:78ca29b4c49e | 39 | */ |
altb | 0:78ca29b4c49e | 40 | Serial pc(SERIAL_TX, SERIAL_RX); // serial connection via USB - programmer |
altb | 0:78ca29b4c49e | 41 | InterruptIn button(USER_BUTTON); // User Button, short presses: reduce speed, long presses: increase speed |
altb | 0:78ca29b4c49e | 42 | AnalogIn ax(PA_0); // Analog IN (acc x) on PA_0 |
altb | 0:78ca29b4c49e | 43 | AnalogIn ay(PA_1); // Analog IN (acc y) on PA_1 |
altb | 0:78ca29b4c49e | 44 | AnalogIn gz(PA_4); // Analog IN (gyr z) on PA_4 |
altb | 0:78ca29b4c49e | 45 | AnalogOut out(PA_5); // Analog OUT on PA_5 1.6 V -> 0A 3.2A -> 2A (see ESCON) |
altb | 0:78ca29b4c49e | 46 | float out_value = 1.6f; // set voltage on 1.6 V (0 A current) |
altb | 0:78ca29b4c49e | 47 | float w_soll = 10.0f; // desired velocity |
Kiwicjam | 9:dc0eb7dd0d92 | 48 | float Ts = 0.002f; // sample time of main loops |
altb | 3:769ce5f06d3e | 49 | int k = 0; |
altb | 6:2cc56521aa16 | 50 | |
altb | 0:78ca29b4c49e | 51 | //------------------------------------------ |
altb | 0:78ca29b4c49e | 52 | // ... here define variables like gains etc. |
altb | 0:78ca29b4c49e | 53 | //------------------------------------------ |
Kiwicjam | 9:dc0eb7dd0d92 | 54 | //LinearCharacteristics i2u(0.8f,-2.0f); |
Kiwicjam | 12:1aa5ff8333e8 | 55 | //LinearCharacteristics i2u(-2.0f,2.0f,0.0f,3.2f); // schwach parametrierung |
Kiwicjam | 12:1aa5ff8333e8 | 56 | LinearCharacteristics i2u(-15.0f,15.0f,0.0f,3.2f); // starke parametrierung |
Kiwicjam | 12:1aa5ff8333e8 | 57 | LinearCharacteristics u2ax(0.69630f,0.29792,9.81f,-9.81f); |
Kiwicjam | 12:1aa5ff8333e8 | 58 | LinearCharacteristics u2ay(0.70012f,0.29750,9.81f,-9.81f); |
Kiwicjam | 10:ddbc7e4c41a0 | 59 | LinearCharacteristics u2gyro(-4.65f,1.5f); |
Kiwicjam | 10:ddbc7e4c41a0 | 60 | |
Kiwicjam | 10:ddbc7e4c41a0 | 61 | |
altb | 0:78ca29b4c49e | 62 | |
altb | 0:78ca29b4c49e | 63 | //------------------------------------------ |
altb | 0:78ca29b4c49e | 64 | Ticker ControllerLoopTimer; // interrupt for control loop |
altb | 0:78ca29b4c49e | 65 | EncoderCounter counter1(PB_6, PB_7); // initialize counter on PB_6 and PB_7 |
altb | 0:78ca29b4c49e | 66 | DiffCounter diff(0.01,Ts); // discrete differentiate, based on encoder data |
altb | 0:78ca29b4c49e | 67 | //------------------------------------------ |
altb | 0:78ca29b4c49e | 68 | // ... here define instantiate classes |
altb | 0:78ca29b4c49e | 69 | //------------------------------------------ |
Kiwicjam | 12:1aa5ff8333e8 | 70 | PI_Cntrl vel_cntrl(0.5f,0.05f,Ts,0.4f); |
Kiwicjam | 12:1aa5ff8333e8 | 71 | PI_Cntrl omega2zero(-0.002f,2.0f,Ts,0.5); |
Kiwicjam | 10:ddbc7e4c41a0 | 72 | |
Kiwicjam | 10:ddbc7e4c41a0 | 73 | float tau = 1.0f; |
Kiwicjam | 10:ddbc7e4c41a0 | 74 | IIR_filter LPF_gyro(tau,Ts,tau); |
Kiwicjam | 10:ddbc7e4c41a0 | 75 | IIR_filter LPF_accX(tau,Ts,1.0f); |
Kiwicjam | 10:ddbc7e4c41a0 | 76 | IIR_filter LPF_accY(tau,Ts,1.0f); |
Kiwicjam | 10:ddbc7e4c41a0 | 77 | |
Kiwicjam | 10:ddbc7e4c41a0 | 78 | |
Kiwicjam | 9:dc0eb7dd0d92 | 79 | //GPA gpa1(1.0f, 200.0f, 150, 4, 400, Ts, 10.0f, 0.3f); |
Kiwicjam | 9:dc0eb7dd0d92 | 80 | //float excWobble = 0.0f; |
altb | 8:8ed679044a72 | 81 | // GPA(t fMin, t fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1) |
altb | 0:78ca29b4c49e | 82 | // ... define some linear characteristics ----------------------------------------- |
altb | 0:78ca29b4c49e | 83 | |
altb | 0:78ca29b4c49e | 84 | // ----- User defined functions ----------- |
altb | 0:78ca29b4c49e | 85 | void updateControllers(void); // speed controller loop (via interrupt) |
altb | 0:78ca29b4c49e | 86 | // ------ END User defined functions ------ |
altb | 0:78ca29b4c49e | 87 | |
altb | 0:78ca29b4c49e | 88 | //****************************************************************************** |
altb | 0:78ca29b4c49e | 89 | //---------- main loop ------------- |
altb | 0:78ca29b4c49e | 90 | //****************************************************************************** |
altb | 0:78ca29b4c49e | 91 | int main() |
altb | 0:78ca29b4c49e | 92 | { |
altb | 0:78ca29b4c49e | 93 | //attach controller loop to timer interrupt |
altb | 0:78ca29b4c49e | 94 | pc.baud(2000000); // for serial comm. |
altb | 0:78ca29b4c49e | 95 | counter1.reset(); // encoder reset |
altb | 0:78ca29b4c49e | 96 | diff.reset(0.0f,0.0f); |
altb | 0:78ca29b4c49e | 97 | ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = ...; |
altb | 5:d79c437626e7 | 98 | |
altb | 3:769ce5f06d3e | 99 | |
altb | 0:78ca29b4c49e | 100 | } |
altb | 0:78ca29b4c49e | 101 | //****************************************************************************** |
altb | 0:78ca29b4c49e | 102 | //---------- control loop (called via interrupt) ------------- |
altb | 0:78ca29b4c49e | 103 | //****************************************************************************** |
altb | 0:78ca29b4c49e | 104 | void updateControllers(void){ |
altb | 0:78ca29b4c49e | 105 | short counts = counter1; // get counts from Encoder |
altb | 0:78ca29b4c49e | 106 | float vel = diff(counts); // motor velocity |
Kiwicjam | 9:dc0eb7dd0d92 | 107 | //desTorque = vel_cntrl(w_soll - vel); |
Kiwicjam | 9:dc0eb7dd0d92 | 108 | /* outWobble = omega; |
altb | 8:8ed679044a72 | 109 | excWobble = Wobble(excWobble, outWobble); */ |
altb | 8:8ed679044a72 | 110 | |
Kiwicjam | 10:ddbc7e4c41a0 | 111 | //float torq_des = vel_cntrl(w_soll - vel); |
Kiwicjam | 10:ddbc7e4c41a0 | 112 | //out.write(i2u(torq_des/0.217f)); // the controller! convert torque to Amps km = 0.217 Nm/A |
Kiwicjam | 10:ddbc7e4c41a0 | 113 | |
Kiwicjam | 10:ddbc7e4c41a0 | 114 | float gyro_dir = (u2gyro(gz.read()*3.3f)); |
Kiwicjam | 10:ddbc7e4c41a0 | 115 | float gyroV = LPF_gyro.filter(gyro_dir); |
Kiwicjam | 10:ddbc7e4c41a0 | 116 | float accX = LPF_accX.filter(u2ax(ax.read())); |
Kiwicjam | 10:ddbc7e4c41a0 | 117 | float accY = LPF_accY.filter(u2ay(ay.read())); |
Kiwicjam | 10:ddbc7e4c41a0 | 118 | |
Kiwicjam | 12:1aa5ff8333e8 | 119 | // komplementärfilter |
Kiwicjam | 12:1aa5ff8333e8 | 120 | float phi_mes = ((PI/4) + atan2(-accX, accY) + gyroV); |
Kiwicjam | 10:ddbc7e4c41a0 | 121 | |
Kiwicjam | 12:1aa5ff8333e8 | 122 | // regler |
Kiwicjam | 12:1aa5ff8333e8 | 123 | float torque = omega2zero(0-vel)-(-9.6910f*phi_mes - 0.71190f * gyro_dir); |
Kiwicjam | 10:ddbc7e4c41a0 | 124 | out.write(i2u(torque/0.217f)/3.3f); |
altb | 8:8ed679044a72 | 125 | |
altb | 8:8ed679044a72 | 126 | |
Kiwicjam | 10:ddbc7e4c41a0 | 127 | // OUTPUT |
Kiwicjam | 10:ddbc7e4c41a0 | 128 | if(++k >= 150){ |
Kiwicjam | 9:dc0eb7dd0d92 | 129 | k = 0; |
Kiwicjam | 10:ddbc7e4c41a0 | 130 | //pc.printf("Des. velocity: %3.3f, Velocity: %3.3f\r\n",w_soll,vel); |
Kiwicjam | 10:ddbc7e4c41a0 | 131 | 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()); |
Kiwicjam | 10:ddbc7e4c41a0 | 132 | //pc.printf("Gyro: %3.3f rad/s\r\n",gyroCalc(gz.read())); |
Kiwicjam | 10:ddbc7e4c41a0 | 133 | //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); |
Kiwicjam | 10:ddbc7e4c41a0 | 134 | pc.printf("Phii: %3.3f\r\n", phi_mes); |
Kiwicjam | 9:dc0eb7dd0d92 | 135 | } |
altb | 0:78ca29b4c49e | 136 | } |
altb | 0:78ca29b4c49e | 137 | //****************************************************************************** |
altb | 0:78ca29b4c49e | 138 | //********** User functions like buttens handle etc. ************** |
altb | 0:78ca29b4c49e | 139 | //****************************************************************************** |
altb | 0:78ca29b4c49e | 140 | // pressed button |
altb | 0:78ca29b4c49e | 141 | //****************************************************************************** |
altb | 0:78ca29b4c49e | 142 | |
Kiwicjam | 12:1aa5ff8333e8 | 143 | //... |
Kiwicjam | 12:1aa5ff8333e8 | 144 | |
Kiwicjam | 12:1aa5ff8333e8 | 145 |