TeamSurface / Mbed 2 deprecated RT2_P3_students

Fertig

Dependencies:   mbed

Fork of RT2_P3_students by RT2_P3_students

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?

UserRevisionLine numberNew 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