Ruprecht Altenburger / Mbed OS mirror_actuator_V1

Mirror actuator for RT2 lab

Dependencies:   FastPWM

main.cpp@4:3d8dd3d17564, 2019-11-06 (annotated)

Committer:
altb2
Date:
Wed Nov 06 08:00:26 2019 +0000
Revision:
4:3d8dd3d17564
Parent:
2:b3aed371adb5
Child:
5:768e10f6d372
different keypressed function for Infotag

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 0:a56e9c932891 1 #include "mbed.h"
altb2 0:a56e9c932891 2 #include "math.h"
altb2 0:a56e9c932891 3 //------------------------------------------
altb2 0:a56e9c932891 4 #define PI 3.1415927f
altb2 0:a56e9c932891 5 //------------------------------------------
altb2 0:a56e9c932891 6 #include "EncoderCounter.h"
altb2 0:a56e9c932891 7 #include "DiffCounter.h"
altb2 0:a56e9c932891 8 #include "IIR_filter.h"
altb2 0:a56e9c932891 9 #include "LinearCharacteristics.h"
altb2 0:a56e9c932891 10 #include "PI_Cntrl.h"
altb2 0:a56e9c932891 11 // #include "GPA.h"
altb2 0:a56e9c932891 12 // define STATES:
altb2 0:a56e9c932891 13 #define INIT 0 // at very beginning
altb2 0:a56e9c932891 14 #define FLAT 10 // cuboid is flat, motor is controlled to zero
altb2 0:a56e9c932891 15 #define BALANCE 20 // balancing
altb2 0:a56e9c932891 16 #define SWING_DOWN 30 // move cuboid down
altb2 0:a56e9c932891 17
altb2 0:a56e9c932891 18
altb2 0:a56e9c932891 19 /* Cuboid balance on one edge on Nucleo F446RE
altb2 0:a56e9c932891 20
altb2 0:a56e9c932891 21 **** IMPORTANT: use ..\Labormodelle\RT-MOD054 - Würfel\Escon_Parameter_4nucleo.edc
altb2 0:a56e9c932891 22 settings for Maxon ESCON controller (upload via ESCON Studio) ****
altb2 0:a56e9c932891 23 hardware Connections:
altb2 0:a56e9c932891 24
altb2 0:a56e9c932891 25 CN7 CN10
altb2 0:a56e9c932891 26 : :
altb2 0:a56e9c932891 27 : :
altb2 0:a56e9c932891 28 .. ..
altb2 0:a56e9c932891 29 .. .. 15.
altb2 0:a56e9c932891 30 .. AOUT i_des on (PA_5)o.
altb2 0:a56e9c932891 31 .. ..
altb2 0:a56e9c932891 32 .. ..
altb2 0:a56e9c932891 33 .. ENC CH A o.
altb2 0:a56e9c932891 34 o. GND .. 10.
altb2 0:a56e9c932891 35 o. ENC CH B ..
altb2 0:a56e9c932891 36 .. ..
altb2 0:a56e9c932891 37 .. ..
altb2 0:a56e9c932891 38 .o AIN acx (PA_0) ..
altb2 0:a56e9c932891 39 .o AIN acy (PA_1) .. 5.
altb2 0:a56e9c932891 40 .o AIN Gyro(PA_4) .o Analog GND
altb2 0:a56e9c932891 41 .. ..
altb2 0:a56e9c932891 42 .. ..
altb2 0:a56e9c932891 43 .. .. 1.
altb2 0:a56e9c932891 44 ----------------------------
altb2 0:a56e9c932891 45 CN7 CN10
altb2 0:a56e9c932891 46 */
altb2 0:a56e9c932891 47 Serial pc(SERIAL_TX, SERIAL_RX); // serial connection via USB - programmer
altb2 0:a56e9c932891 48 InterruptIn button(USER_BUTTON); // User Button, short presses: reduce speed, long presses: increase speed
altb2 0:a56e9c932891 49 bool key_was_pressed = false;
altb2 0:a56e9c932891 50 AnalogIn ax(PA_0); // Analog IN (acc x) on PA_0
altb2 0:a56e9c932891 51 AnalogIn ay(PA_1); // Analog IN (acc y) on PA_1
altb2 0:a56e9c932891 52 AnalogIn gz(PA_4); // Analog IN (gyr z) on PA_4
altb2 0:a56e9c932891 53 AnalogOut out(PA_5); // Analog OUT on PA_5 1.6 V -> 0A 3.2A -> 2A (see ESCON)
altb2 0:a56e9c932891 54 float out_value = 1.6f; // set voltage on 1.6 V (0 A current)
altb2 0:a56e9c932891 55 float w_soll = 10.0f; // desired velocity
altb2 0:a56e9c932891 56 float Ts = 0.002f; // sample time of main loops
altb2 0:a56e9c932891 57 int k = 0;
altb2 0:a56e9c932891 58 float phi1_des = 0.0f;
altb2 0:a56e9c932891 59 int CS = INIT;
altb2 0:a56e9c932891 60 void pressed(void); // user Button pressed
altb2 0:a56e9c932891 61 void released(void); // user Button released
altb2 0:a56e9c932891 62
altb2 0:a56e9c932891 63
altb2 0:a56e9c932891 64 //------------------------------------------
altb2 0:a56e9c932891 65 // ... here define variables like gains etc.
altb2 0:a56e9c932891 66 //------------------------------------------
altb2 0:a56e9c932891 67 LinearCharacteristics i2u(-15.0f,15.0f,0.0f,3.2f / 3.3f); // output is normalized output
altb2 0:a56e9c932891 68 LinearCharacteristics u2ax(0.29719f,0.69768f,-9.81f,9.81f); // use normalized input
altb2 0:a56e9c932891 69 LinearCharacteristics u2ay(0.29890,0.70330f,-9.81f,9.81f); // use normalized input
altb2 0:a56e9c932891 70 LinearCharacteristics u2gz(-4.6517f * 3.3f,0.45495f); // use normalized input
altb2 0:a56e9c932891 71 // 4.6517f = 1/3.752e-3 [V / °/s] * pi/180
altb2 0:a56e9c932891 72
altb2 0:a56e9c932891 73 //-------------DEFINE FILTERS----------------
altb2 0:a56e9c932891 74 float tau = 1.0f;
altb2 0:a56e9c932891 75 IIR_filter f_ax(tau,Ts,1.0f); // filter ax signals
altb2 0:a56e9c932891 76 IIR_filter f_ay(tau,Ts,1.0f); // filter ay signals
altb2 0:a56e9c932891 77 IIR_filter f_gz(tau,Ts,tau); // filter gz signals
altb2 0:a56e9c932891 78 //------------------------------------------
altb2 0:a56e9c932891 79 float vel = 0.0f; // velocity of motor
altb2 0:a56e9c932891 80 float gyro = 0.0f;
altb2 0:a56e9c932891 81 float phi1 = 0.0f;
altb2 0:a56e9c932891 82 //------------------------------------------
altb2 0:a56e9c932891 83 Ticker ControllerLoopTimer; // interrupt for control loop
altb2 0:a56e9c932891 84 EncoderCounter counter1(PB_6, PB_7); // initialize counter on PB_6 and PB_7
altb2 0:a56e9c932891 85 DiffCounter diff(0.01,Ts); // discrete differentiate, based on encoder data
altb2 0:a56e9c932891 86 Timer ti; // define global timer
altb2 0:a56e9c932891 87 Timer t_but; // define global timer
altb2 0:a56e9c932891 88 PI_Cntrl vel_cntrl(0.5f,.05f,Ts,0.5f); // velocity controller for motor
altb2 0:a56e9c932891 89 PI_Cntrl om2zero(-0.02f,4.0f,Ts,0.9f); // slow vel. contrl. to bring motor to zero
altb2 0:a56e9c932891 90 //------------------------------------------
altb2 0:a56e9c932891 91 // ----- User defined functions -----------
altb2 0:a56e9c932891 92 void updateLoop(void); // loop for State machine (via interrupt)
altb2 0:a56e9c932891 93 float cuboid_stab_cntrl(int); // stabalizer
altb2 0:a56e9c932891 94 void calc_angle_phi1(int);
altb2 0:a56e9c932891 95 // ------ END User defined functions ------
altb2 0:a56e9c932891 96
altb2 0:a56e9c932891 97 //******************************************************************************
altb2 0:a56e9c932891 98 //---------- main loop -------------
altb2 0:a56e9c932891 99 //******************************************************************************
altb2 0:a56e9c932891 100 int main()
altb2 0:a56e9c932891 101 {
altb2 4:3d8dd3d17564 102 pc.baud(2000000); // for serial comm.
altb2 0:a56e9c932891 103 counter1.reset(); // encoder reset
altb2 0:a56e9c932891 104 diff.reset(0.0f,0);
altb2 0:a56e9c932891 105 ControllerLoopTimer.attach(&updateLoop, Ts); //Assume Fs = ...;
altb2 0:a56e9c932891 106 ti.reset();
altb2 0:a56e9c932891 107 ti.start();
altb2 0:a56e9c932891 108 calc_angle_phi1(1);
altb2 0:a56e9c932891 109 button.fall(&pressed); // attach key pressed function
altb2 0:a56e9c932891 110 button.rise(&released); // attach key pressed function
altb2 0:a56e9c932891 111
altb2 0:a56e9c932891 112 } // END OF main
altb2 0:a56e9c932891 113 //******************************************************************************
altb2 0:a56e9c932891 114 //---------- main loop (called via interrupt) -------------
altb2 0:a56e9c932891 115 //******************************************************************************
altb2 0:a56e9c932891 116 void updateLoop(void){
altb2 0:a56e9c932891 117 short counts = counter1; // get counts from Encoder
altb2 0:a56e9c932891 118 vel = diff(counts); // motor velocity
altb2 0:a56e9c932891 119 float torq = 0.0f; // set motor torque to zero (will be overwritten)
altb2 0:a56e9c932891 120 float dt = ti.read(); // time elapsed in current state
altb2 0:a56e9c932891 121 calc_angle_phi1(0); // angle phi1 is calculated in every loop
altb2 0:a56e9c932891 122 // ****************** STATE MACHINE ***************************************
altb2 1:f39d75a48955 123 switch(CS) {
altb2 0:a56e9c932891 124 case INIT: // at very beginning
altb2 0:a56e9c932891 125 if (dt > 2.0f){
altb2 0:a56e9c932891 126 CS = FLAT; // switch to FLAT state
altb2 0:a56e9c932891 127 ti.reset();
altb2 0:a56e9c932891 128 }
altb2 0:a56e9c932891 129 break;
altb2 0:a56e9c932891 130 case FLAT: // cuboid is flat, keep motor velocity to zero
altb2 0:a56e9c932891 131 torq = vel_cntrl(0.0f - vel);
altb2 0:a56e9c932891 132 if (key_was_pressed && dt > 1.0f){
altb2 0:a56e9c932891 133 CS = BALANCE;
altb2 0:a56e9c932891 134 torq = cuboid_stab_cntrl(1);
altb2 0:a56e9c932891 135 ti.reset();
altb2 0:a56e9c932891 136 key_was_pressed = false;
altb2 0:a56e9c932891 137 phi1_des = 0.0f;
altb2 0:a56e9c932891 138 vel_cntrl.reset(0.0f); // reset velocity controller for the next time
altb2 0:a56e9c932891 139 }
altb2 0:a56e9c932891 140 break;
altb2 0:a56e9c932891 141 case BALANCE: // balance the cube
altb2 0:a56e9c932891 142 torq = cuboid_stab_cntrl(0);
altb2 0:a56e9c932891 143 if (key_was_pressed && dt > 1.0f){
altb2 0:a56e9c932891 144 CS = SWING_DOWN;
altb2 0:a56e9c932891 145 phi1_des = 0.0f;
altb2 0:a56e9c932891 146 ti.reset();
altb2 0:a56e9c932891 147 key_was_pressed = false;
altb2 0:a56e9c932891 148 }
altb2 0:a56e9c932891 149 break;
altb2 0:a56e9c932891 150 case SWING_DOWN:
altb2 0:a56e9c932891 151 phi1_des = dt; // ramp the desired angle up to pi/4
altb2 0:a56e9c932891 152 torq = cuboid_stab_cntrl(0);// call balance routine
altb2 0:a56e9c932891 153 if (dt > 1.0f){ // good value for smooth moving down
altb2 0:a56e9c932891 154 CS = FLAT; // move to flat
altb2 0:a56e9c932891 155 phi1_des = 0.0f;
altb2 0:a56e9c932891 156 ti.reset();
altb2 0:a56e9c932891 157 }
altb2 0:a56e9c932891 158 break;
altb2 0:a56e9c932891 159 default: break;
altb2 0:a56e9c932891 160 }
altb2 0:a56e9c932891 161 out.write( i2u(torq/0.217f)); // motor const. is 0.217,
altb2 2:b3aed371adb5 162 if(++k >= 249){
altb2 2:b3aed371adb5 163 k = 0;
altb2 2:b3aed371adb5 164 //pc.printf("phi: %1.5f, Torq: %1.2f \r\n",phi1,torq);
altb2 2:b3aed371adb5 165 pc.printf("ax %1.5f, ay: %1.5f, gyro: %1.5f, ph1: %1.5f\r\n",u2ax(ax.read()),u2ay(ay.read()),u2gz(gz.read()),phi1);
altb2 2:b3aed371adb5 166 }
altb2 2:b3aed371adb5 167
altb2 0:a56e9c932891 168 } // END OF updateLoop(void)
altb2 0:a56e9c932891 169
altb2 0:a56e9c932891 170 //******************************************************************************
altb2 0:a56e9c932891 171 // ************ stabalizing controller *****************
altb2 0:a56e9c932891 172 //******************************************************************************
altb2 0:a56e9c932891 173
altb2 0:a56e9c932891 174 float cuboid_stab_cntrl(int do_reset){
altb2 0:a56e9c932891 175 /* | phi_des
altb2 0:a56e9c932891 176 v
altb2 0:a56e9c932891 177 ---
altb2 0:a56e9c932891 178 | V | feed forward gain
altb2 0:a56e9c932891 179 ---
altb2 0:a56e9c932891 180 |
altb2 0:a56e9c932891 181 ------- v --------
altb2 0:a56e9c932891 182 0 -->O-->|om2zero|-->O---->| System |--o---> x = [phi1 phi1_t]
altb2 0:a56e9c932891 183 ^- ------- ^- -------- |
altb2 0:a56e9c932891 184 | | |
altb2 0:a56e9c932891 185 vel | --- |
altb2 0:a56e9c932891 186 --- -| K |<-----
altb2 0:a56e9c932891 187 ---
altb2 0:a56e9c932891 188 */
altb2 0:a56e9c932891 189 if(do_reset == 1) // reset controller
altb2 0:a56e9c932891 190 om2zero.reset(0.0f);
altb2 0:a56e9c932891 191
altb2 0:a56e9c932891 192 float M_des = om2zero(0.0f-vel); // outer controller to bring motor to zero
altb2 0:a56e9c932891 193 float torq = M_des + (-7.1*phi1_des) -(-9.6910f * phi1 -0.7119f * gyro); // calculationof gains are based on the MATLAB script at the end of this file!!!
altb2 0:a56e9c932891 194 // output PI V K(1) K(2)
altb2 0:a56e9c932891 195 return torq;
altb2 0:a56e9c932891 196 }
altb2 0:a56e9c932891 197 // ************ calculate angle *****************
altb2 0:a56e9c932891 198 void calc_angle_phi1(int do_reset){
altb2 0:a56e9c932891 199 gyro = u2gz(gz.read());
altb2 0:a56e9c932891 200 if(do_reset == 1){
altb2 0:a56e9c932891 201 f_ax.reset(u2ax(ax.read()));
altb2 0:a56e9c932891 202 f_ay.reset(u2ax(ay.read()));
altb2 0:a56e9c932891 203 f_gz.reset(gyro);
altb2 0:a56e9c932891 204 }
altb2 0:a56e9c932891 205 phi1 = atan2(-f_ax(u2ax(ax.read())),f_ay(u2ay(ay.read()))) + PI/4.0f + f_gz(gyro);
altb2 0:a56e9c932891 206 }
altb2 0:a56e9c932891 207
altb2 0:a56e9c932891 208 // start timer as soon as Button is pressed
altb2 0:a56e9c932891 209 void pressed()
altb2 0:a56e9c932891 210 {
altb2 0:a56e9c932891 211 t_but.start();
altb2 0:a56e9c932891 212 key_was_pressed = false;
altb2 0:a56e9c932891 213 }
altb2 0:a56e9c932891 214
altb2 0:a56e9c932891 215 // evaluating statemachine
altb2 0:a56e9c932891 216 void released()
altb2 0:a56e9c932891 217 {
altb2 0:a56e9c932891 218
altb2 0:a56e9c932891 219 // readout, stop and reset timer
altb2 0:a56e9c932891 220 float ButtonTime = t_but.read();
altb2 0:a56e9c932891 221 t_but.stop();
altb2 0:a56e9c932891 222 t_but.reset();
altb2 0:a56e9c932891 223 if(ButtonTime > 0.05f)
altb2 0:a56e9c932891 224 key_was_pressed = true;
altb2 0:a56e9c932891 225 }
altb2 0:a56e9c932891 226
altb2 0:a56e9c932891 227
altb2 0:a56e9c932891 228
altb2 0:a56e9c932891 229 /* MATLAB CODE FOR CUBOID:
altb2 0:a56e9c932891 230 %% Skript fuer cuboid Praktikum
altb2 0:a56e9c932891 231 m_geh=.938; % Masse des Gehaeses
altb2 0:a56e9c932891 232 m_sb=1.243; % Masse Scheibe
altb2 0:a56e9c932891 233 m_g=m_geh+m_sb; % Gesamtmasse
altb2 0:a56e9c932891 234 J_geh=.00408; % Traegheit Gehaeuse (CM)
altb2 0:a56e9c932891 235 J_sb=.00531; % Traegheit Scheibe (CM)
altb2 0:a56e9c932891 236 J1=diag([0 0 J_geh]);
altb2 0:a56e9c932891 237 J2=diag([0 0 J_sb]);
altb2 0:a56e9c932891 238 l=.17; % Kantenlaenge
altb2 0:a56e9c932891 239 sy=.085;
altb2 0:a56e9c932891 240 g=9.81;
altb2 0:a56e9c932891 241 phi0=0;
altb2 0:a56e9c932891 242 J_g=J_geh+(m_geh+m_sb)*(l/sqrt(2))^2;
altb2 0:a56e9c932891 243
altb2 0:a56e9c932891 244 %% Linearisiertes Modell
altb2 0:a56e9c932891 245 A2=[0 1;m_g*g*l/sqrt(2)/J_g 0];
altb2 0:a56e9c932891 246 B2=[0;-1/J_g];
altb2 0:a56e9c932891 247 K=place(A2,B2,10*[-1+1j -1-1j]);
altb2 0:a56e9c932891 248 s_cl=ss(A2-B2*K,B2,[1 0],0);
altb2 0:a56e9c932891 249 V=1/dcgain(s_cl);
altb2 0:a56e9c932891 250
altb2 0:a56e9c932891 251 %% Gesamtsystem mit Scheibe, Zustaende [phi_1 om_1 om2~] (om2~ = Absolutkoordinaten)
altb2 0:a56e9c932891 252 A3=[A2-B2*K [0;0];...
altb2 0:a56e9c932891 253 -1/J_sb*K 0]; % Auf Scheibenbeschleunigung wirken ganz
altb2 0:a56e9c932891 254 % entsprechend die Zustaende x1, x2 ueber die Zustandsrueckfuehrung
altb2 0:a56e9c932891 255 B3=[B2;1/J_sb];
altb2 0:a56e9c932891 256 C3=[0 -1 1]; % Gemessen wird die Relativgeschwindigkeit zwischen Scheibe-Gehaeuse
altb2 0:a56e9c932891 257 s3=ss(A3,B3,C3,0);
altb2 0:a56e9c932891 258 s3=ss2ss(s3,[1 0 0;0 1 0;0 -1 1]); % Transformiere 3ten Zustand (eigentlich unnoetig)
altb2 0:a56e9c932891 259 Tn=4;
altb2 0:a56e9c932891 260 PI=-tf([Tn 1],[Tn 0]); % Langsamer PI Regler, negatives gain erforderlich!!
altb2 0:a56e9c932891 261 rlocus(s3*PI);grid
altb2 0:a56e9c932891 262 */