Cuboid
Dependencies: mbed
main.cpp@15:ed33be6f040e, 2018-04-09 (annotated)
- Committer:
- pmic
- Date:
- Mon Apr 09 08:02:04 2018 +0000
- Revision:
- 15:ed33be6f040e
- Parent:
- 13:a308f5e6c306
- Child:
- 17:802aede7b90e
clean code in main and set parameters so that the system perfomes a gpa measurement
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" |
pmic | 5:d6c7ccbbce78 | 3 | #define pi 3.1415927f |
pmic | 5:d6c7ccbbce78 | 4 | |
rtlabor | 0:15be70d21d7c | 5 | #include "EncoderCounter.h" |
rtlabor | 0:15be70d21d7c | 6 | #include "DiffCounter.h" |
rtlabor | 0:15be70d21d7c | 7 | #include "PI_Cntrl.h" |
rtlabor | 0:15be70d21d7c | 8 | #include "IIR_filter.h" |
rtlabor | 0:15be70d21d7c | 9 | #include "LinearCharacteristics.h" |
pmic | 10:a28f393c6716 | 10 | #include "GPA.h" |
pmic | 5:d6c7ccbbce78 | 11 | |
rtlabor | 0:15be70d21d7c | 12 | /* Cuboid balance on one edge on Nucleo F446RE |
pmic | 5:d6c7ccbbce78 | 13 | // ----------------------------------------------------------------------------- |
altb | 3:a951d699878b | 14 | |
pmic | 5:d6c7ccbbce78 | 15 | IMPORTANT: use ..\T-RT\Messen_Ausstellungen\Praesentationen_im_Labor\Wuerfel_nucleo\Escon_Parameter_4nucleo_stark.edc |
pmic | 5:d6c7ccbbce78 | 16 | settings for Maxon ESCON controller (upload via ESCON Studio) |
pmic | 5:d6c7ccbbce78 | 17 | |
rtlabor | 0:15be70d21d7c | 18 | hardware Connections: |
pmic | 5:d6c7ccbbce78 | 19 | |
rtlabor | 0:15be70d21d7c | 20 | CN7 CN10 |
altb | 3:a951d699878b | 21 | : : |
altb | 3:a951d699878b | 22 | : : |
altb | 3:a951d699878b | 23 | .. .. |
altb | 3:a951d699878b | 24 | .. .. 15. |
altb | 3:a951d699878b | 25 | .. AOUT i_des on (PA_5)o. |
altb | 3:a951d699878b | 26 | .. .. |
altb | 3:a951d699878b | 27 | .. .. |
altb | 3:a951d699878b | 28 | .. ENC CH A o. |
altb | 3:a951d699878b | 29 | o. GND .. 10. |
altb | 3:a951d699878b | 30 | o. ENC CH B .. |
altb | 3:a951d699878b | 31 | .. .. |
altb | 3:a951d699878b | 32 | .. .. |
pmic | 5:d6c7ccbbce78 | 33 | .o AIN acx (PA_0) .. |
altb | 3:a951d699878b | 34 | .o AIN acy (PA_1) .. 5. |
altb | 3:a951d699878b | 35 | .o AIN Gyro(PA_4) .o Analog GND |
altb | 3:a951d699878b | 36 | .. .. |
pmic | 5:d6c7ccbbce78 | 37 | .. .. |
pmic | 5:d6c7ccbbce78 | 38 | .. .. 1. |
altb | 3:a951d699878b | 39 | ---------------------------- |
rtlabor | 0:15be70d21d7c | 40 | CN7 CN10 |
rtlabor | 0:15be70d21d7c | 41 | */ |
pmic | 5:d6c7ccbbce78 | 42 | |
pmic | 5:d6c7ccbbce78 | 43 | Serial pc(SERIAL_TX, SERIAL_RX); // serial connection via USB - programmer |
pmic | 5:d6c7ccbbce78 | 44 | InterruptIn Button(USER_BUTTON); // User Button, short presses: reduce speed, long presses: increase speed |
pmic | 5:d6c7ccbbce78 | 45 | AnalogIn ax(PA_0); // analog IN (acc x) on PA_0 |
pmic | 5:d6c7ccbbce78 | 46 | AnalogIn ay(PA_1); // analog IN (acc y) on PA_1 |
pmic | 5:d6c7ccbbce78 | 47 | AnalogIn gz(PA_4); // analog IN (gyr z) on PB_0 |
pmic | 5:d6c7ccbbce78 | 48 | AnalogOut out(PA_5); // analog OUT 1.6 V -> 0A 3.2A -> 2A (see ESCON) |
pmic | 5:d6c7ccbbce78 | 49 | Ticker ControllerLoopTimer; // interrupt for control loop |
pmic | 5:d6c7ccbbce78 | 50 | Timer t; // timer to analyse Button |
pmic | 5:d6c7ccbbce78 | 51 | |
pmic | 5:d6c7ccbbce78 | 52 | // controller parameters etc. |
pmic | 15:ed33be6f040e | 53 | float Ts = 0.0025f; // sample time |
pmic | 15:ed33be6f040e | 54 | float v_max = 200.0f; // maximum speed rad/s |
pmic | 15:ed33be6f040e | 55 | float n_soll = 15.0f; // nominal speed for speed control tests |
pmic | 15:ed33be6f040e | 56 | float tau = 1.00f; // time constant of complementary filter |
pmic | 5:d6c7ccbbce78 | 57 | |
pmic | 5:d6c7ccbbce78 | 58 | // output and statemachine |
pmic | 5:d6c7ccbbce78 | 59 | unsigned int k = 0; // counter for serial output |
pmic | 5:d6c7ccbbce78 | 60 | bool doesStand = 0; // state if the cube is standing or not |
pmic | 5:d6c7ccbbce78 | 61 | bool shouldBalance = 0; // state if the controller is active |
pmic | 13:a308f5e6c306 | 62 | bool shouldGoDown = 0; // state if the controller should swing down |
pmic | 5:d6c7ccbbce78 | 63 | |
pmic | 5:d6c7ccbbce78 | 64 | // set up encoder |
pmic | 15:ed33be6f040e | 65 | EncoderCounter MotorEncoder(PB_6, PB_7); // initialize counter on PB_6 and PB_7 |
pmic | 8:d68e177e2571 | 66 | DiffCounter MotorDiff(1/(2.0f*pi*80.0f), Ts); // discrete differentiate, based on encoder data |
pmic | 5:d6c7ccbbce78 | 67 | |
pmic | 15:ed33be6f040e | 68 | // set up controllers |
pmic | 15:ed33be6f040e | 69 | float maxCurrent = 15.0f; // 1/0.217 A/Nm -> 2.0/0.217 = 13.82 A |
pmic | 15:ed33be6f040e | 70 | float Km = 1/0.217f; // Motorgain: Torque -> km -> Current in A/Nm |
pmic | 8:d68e177e2571 | 71 | float maxTorque = maxCurrent/Km; |
pmic | 12:6287235b2570 | 72 | PI_Cntrl pi_w2zero(-.012f, 0.8f, Ts, maxTorque); // controller to bring motor speed to zero while balancing |
pmic | 15:ed33be6f040e | 73 | PI_Cntrl pi_w(0.6f, 0.4f, Ts, maxTorque); // PI controller for test purposes motor speed (no balance) |
pmic | 10:a28f393c6716 | 74 | float desTorque = 0.0f; |
pmic | 10:a28f393c6716 | 75 | |
pmic | 15:ed33be6f040e | 76 | // set up gpa |
pmic | 10:a28f393c6716 | 77 | float fMin = 1.0f; |
pmic | 10:a28f393c6716 | 78 | float fMax = 1.0f/2.0f/Ts*0.9f; |
pmic | 10:a28f393c6716 | 79 | int NfexcDes = 150; |
pmic | 10:a28f393c6716 | 80 | float Aexc0 = 5.0f; |
pmic | 10:a28f393c6716 | 81 | float Aexc1 = 0.3f; //Aexc0/fMax; |
pmic | 10:a28f393c6716 | 82 | int NperMin = 3; |
pmic | 10:a28f393c6716 | 83 | float TmeasMin = 1.0f; |
pmic | 10:a28f393c6716 | 84 | int NmeasMin = (int)ceil(TmeasMin/Ts); |
pmic | 10:a28f393c6716 | 85 | GPA Wobble(fMin, fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1); |
pmic | 11:ed2638662dfa | 86 | float inpWobble = 0.0f; |
pmic | 11:ed2638662dfa | 87 | float outWobble = 0.0f; |
pmic | 11:ed2638662dfa | 88 | float excWobble = 0.0f; |
pmic | 5:d6c7ccbbce78 | 89 | |
pmic | 15:ed33be6f040e | 90 | // set up filters for complementary filter |
pmic | 13:a308f5e6c306 | 91 | IIR_filter FilterACCx(tau, Ts, 1.0f); // 1st order LP for complementary filter acc_x |
pmic | 13:a308f5e6c306 | 92 | IIR_filter FilterACCy(tau, Ts, 1.0f); // 1st order LP for complementary filter acc_y |
pmic | 5:d6c7ccbbce78 | 93 | IIR_filter FilterGYRZ(tau, Ts, tau); // 1st order LP for complementary filter gyro |
pmic | 5:d6c7ccbbce78 | 94 | |
pmic | 15:ed33be6f040e | 95 | // set up filter for swing down process |
pmic | 13:a308f5e6c306 | 96 | IIR_filter FilterTrajectory(10.0f, 1.0f, Ts, 1.0f); |
pmic | 13:a308f5e6c306 | 97 | float V = -2.6080f; |
pmic | 13:a308f5e6c306 | 98 | |
pmic | 5:d6c7ccbbce78 | 99 | // linear characteristics |
pmic | 8:d68e177e2571 | 100 | LinearCharacteristics i2u(0.1067f, -15.0f); // full range, convert desired current (Amps) -> voltage 0..3.3V |
pmic | 8:d68e177e2571 | 101 | LinearCharacteristics u2n(312.5f, 1.6f); // convert input voltage (0..3.3V) -> speed (1/min) |
pmic | 8:d68e177e2571 | 102 | LinearCharacteristics u2w(32.725, 1.6f); // convert input voltage (0..3.3V) -> speed (rad/sec) |
pmic | 8:d68e177e2571 | 103 | LinearCharacteristics u2ax(14.67f, 1.6378f); // convert input voltage (0..3.3V) -> acc_x m/s^2 |
pmic | 8:d68e177e2571 | 104 | LinearCharacteristics u2ay(15.02f, 1.6673f); // convert input voltage (0..3.3V) -> acc_y m/s^2 |
pmic | 8:d68e177e2571 | 105 | LinearCharacteristics u2gz(-4.652f, 1.4949f); // convert input voltage (0..3.3V) -> w_x rad/s |
pmic | 8:d68e177e2571 | 106 | LinearCharacteristics u3k3_TO_1V(0.303030303f, 0.0f, 3.3f, 0.0f);// normalize output voltage (0..3.3)V -> (0..1) V |
rtlabor | 0:15be70d21d7c | 107 | |
pmic | 5:d6c7ccbbce78 | 108 | // user defined functions |
rtlabor | 0:15be70d21d7c | 109 | void updateControllers(void); // speed controller loop (via interrupt) |
pmic | 5:d6c7ccbbce78 | 110 | void pressed(void); // user Button pressed |
pmic | 5:d6c7ccbbce78 | 111 | void released(void); // user Button released |
pmic | 8:d68e177e2571 | 112 | void printLine(); |
rtlabor | 0:15be70d21d7c | 113 | |
pmic | 5:d6c7ccbbce78 | 114 | // main program and control loop |
pmic | 5:d6c7ccbbce78 | 115 | // ----------------------------------------------------------------------------- |
rtlabor | 0:15be70d21d7c | 116 | int main() |
rtlabor | 0:15be70d21d7c | 117 | { |
pmic | 5:d6c7ccbbce78 | 118 | // for serial comm. |
pmic | 5:d6c7ccbbce78 | 119 | pc.baud(2000000); |
pmic | 5:d6c7ccbbce78 | 120 | |
pmic | 5:d6c7ccbbce78 | 121 | // reset encoder, controller and filters |
pmic | 5:d6c7ccbbce78 | 122 | MotorEncoder.reset(); |
pmic | 5:d6c7ccbbce78 | 123 | MotorDiff.reset(0.0f,0.0f); |
rtlabor | 0:15be70d21d7c | 124 | pi_w2zero.reset(0.0f); |
pmic | 5:d6c7ccbbce78 | 125 | pi_w.reset(0.0f); |
pmic | 5:d6c7ccbbce78 | 126 | |
pmic | 5:d6c7ccbbce78 | 127 | FilterACCx.reset(u2ax(3.3f*ax.read())); |
pmic | 5:d6c7ccbbce78 | 128 | FilterACCy.reset(u2ay(3.3f*ay.read())); |
pmic | 5:d6c7ccbbce78 | 129 | FilterGYRZ.reset(u2gz(3.3f*gz.read())); |
pmic | 13:a308f5e6c306 | 130 | |
pmic | 13:a308f5e6c306 | 131 | FilterTrajectory.reset(0.0f); |
pmic | 13:a308f5e6c306 | 132 | |
pmic | 10:a28f393c6716 | 133 | Wobble.reset(); |
pmic | 10:a28f393c6716 | 134 | Wobble.printGPAmeasPara(); |
pmic | 11:ed2638662dfa | 135 | inpWobble = 0.0f; |
pmic | 11:ed2638662dfa | 136 | outWobble = 0.0f; |
pmic | 11:ed2638662dfa | 137 | excWobble = 0.0f; |
pmic | 8:d68e177e2571 | 138 | |
pmic | 5:d6c7ccbbce78 | 139 | // reset output |
pmic | 10:a28f393c6716 | 140 | desTorque = 0.0f; |
pmic | 10:a28f393c6716 | 141 | out.write(u3k3_TO_1V(i2u(desTorque*Km))); |
pmic | 5:d6c7ccbbce78 | 142 | |
pmic | 15:ed33be6f040e | 143 | // reset statemachine |
pmic | 13:a308f5e6c306 | 144 | shouldGoDown = 0; |
pmic | 13:a308f5e6c306 | 145 | shouldBalance = 0; |
pmic | 13:a308f5e6c306 | 146 | |
pmic | 5:d6c7ccbbce78 | 147 | // attach controller loop to timer interrupt |
rtlabor | 0:15be70d21d7c | 148 | ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = 400Hz; |
pmic | 5:d6c7ccbbce78 | 149 | Button.fall(&pressed); // attach key pressed function |
pmic | 5:d6c7ccbbce78 | 150 | Button.rise(&released); // attach key pressed function |
rtlabor | 0:15be70d21d7c | 151 | } |
pmic | 5:d6c7ccbbce78 | 152 | |
pmic | 5:d6c7ccbbce78 | 153 | void updateControllers(void) |
pmic | 5:d6c7ccbbce78 | 154 | { |
pmic | 8:d68e177e2571 | 155 | |
pmic | 5:d6c7ccbbce78 | 156 | // read encoder data |
pmic | 5:d6c7ccbbce78 | 157 | short counts = MotorEncoder; // counts in 1 |
pmic | 5:d6c7ccbbce78 | 158 | float omega = MotorDiff(counts); // angular velofity motor |
pmic | 5:d6c7ccbbce78 | 159 | |
pmic | 5:d6c7ccbbce78 | 160 | // read imu data |
pmic | 5:d6c7ccbbce78 | 161 | float accx = u2ax(3.3f*ax.read()); |
pmic | 5:d6c7ccbbce78 | 162 | float accy = u2ay(3.3f*ay.read()); |
pmic | 5:d6c7ccbbce78 | 163 | float gyrz = u2gz(3.3f*gz.read()); |
pmic | 5:d6c7ccbbce78 | 164 | |
pmic | 5:d6c7ccbbce78 | 165 | // perform complementary filter |
pmic | 5:d6c7ccbbce78 | 166 | float ang = atan2(-FilterACCx(accx), FilterACCy(accy)) + FilterGYRZ(gyrz) + pi/4.0f; |
pmic | 5:d6c7ccbbce78 | 167 | |
pmic | 5:d6c7ccbbce78 | 168 | // get current state of the cube |
pmic | 5:d6c7ccbbce78 | 169 | float actualAngleDegree = ang*180.0f/pi; |
pmic | 8:d68e177e2571 | 170 | if(actualAngleDegree > -10.0f && actualAngleDegree < 10.0f) { |
pmic | 5:d6c7ccbbce78 | 171 | doesStand = 1; |
pmic | 8:d68e177e2571 | 172 | } else { |
pmic | 5:d6c7ccbbce78 | 173 | doesStand = 0; |
pmic | 5:d6c7ccbbce78 | 174 | } |
pmic | 5:d6c7ccbbce78 | 175 | |
pmic | 5:d6c7ccbbce78 | 176 | // update controllers |
pmic | 8:d68e177e2571 | 177 | if(shouldBalance) { |
pmic | 15:ed33be6f040e | 178 | |
pmic | 15:ed33be6f040e | 179 | /* |
pmic | 10:a28f393c6716 | 180 | // balance, set n_soll = 0.0f |
pmic | 15:ed33be6f040e | 181 | // --------------------------------------------------------------------- |
pmic | 5:d6c7ccbbce78 | 182 | // K matrix: -5.2142 -0.6247 // from Matlab |
pmic | 13:a308f5e6c306 | 183 | float uPI = pi_w2zero(n_soll - omega); // needs further inverstigation |
pmic | 13:a308f5e6c306 | 184 | float uSS = (-5.2142f*ang - 0.6247f*gyrz); |
pmic | 13:a308f5e6c306 | 185 | desTorque = uPI - uSS; // state space controller for balance, calc desired Torque |
pmic | 8:d68e177e2571 | 186 | if(abs(desTorque) > maxTorque) { |
pmic | 8:d68e177e2571 | 187 | desTorque = copysign(maxTorque, desTorque); |
pmic | 8:d68e177e2571 | 188 | } |
pmic | 8:d68e177e2571 | 189 | if(k == 0) printLine(); |
pmic | 13:a308f5e6c306 | 190 | if(k++ < 2000) pc.printf("%6.4f %6.4f %6.4f %6.4f\r\n", uPI, uSS, ang, omega); |
pmic | 15:ed33be6f040e | 191 | */ |
pmic | 15:ed33be6f040e | 192 | |
pmic | 13:a308f5e6c306 | 193 | /* |
pmic | 15:ed33be6f040e | 194 | // step response velocity controller, set n_soll = 0.0f |
pmic | 15:ed33be6f040e | 195 | // --------------------------------------------------------------------- |
pmic | 13:a308f5e6c306 | 196 | desTorque = pi_w(10.0f - omega); |
pmic | 10:a28f393c6716 | 197 | if(k == 0) printLine(); |
pmic | 10:a28f393c6716 | 198 | if(k++ < 2000) pc.printf("%6.4f %6.4f %6.4f\r\n", 10.0f, omega, desTorque); |
pmic | 10:a28f393c6716 | 199 | */ |
pmic | 15:ed33be6f040e | 200 | |
pmic | 15:ed33be6f040e | 201 | ///* |
pmic | 15:ed33be6f040e | 202 | // gpa measurement, set n_soll = 15.0f |
pmic | 15:ed33be6f040e | 203 | // --------------------------------------------------------------------- |
pmic | 11:ed2638662dfa | 204 | // measuring the plant P and the closed loop tf T = PC/(1 + PC) |
pmic | 10:a28f393c6716 | 205 | desTorque = pi_w(n_soll + excWobble - omega); |
pmic | 11:ed2638662dfa | 206 | inpWobble = desTorque; |
pmic | 11:ed2638662dfa | 207 | outWobble = omega; |
pmic | 13:a308f5e6c306 | 208 | excWobble = Wobble(excWobble, outWobble); |
pmic | 11:ed2638662dfa | 209 | // measuring the controller C and the closed loop tf SC = C/(1 + PC) |
pmic | 11:ed2638662dfa | 210 | // desTorque = pi_w(n_soll + excWobble - omega); |
pmic | 11:ed2638662dfa | 211 | // inpWobble = n_soll + excWobble - omega; |
pmic | 11:ed2638662dfa | 212 | // outWobble = desTorque; |
pmic | 15:ed33be6f040e | 213 | // excWobble = Wobble(inpWobble, outWobble); |
pmic | 10:a28f393c6716 | 214 | if(++k == 73000) Wobble.printGPAmeasTime(); |
pmic | 15:ed33be6f040e | 215 | //*/ |
pmic | 15:ed33be6f040e | 216 | |
pmic | 13:a308f5e6c306 | 217 | } else if(shouldGoDown) { |
pmic | 13:a308f5e6c306 | 218 | // swing down |
pmic | 13:a308f5e6c306 | 219 | // K matrix: -5.2142 -0.6247 // from Matlab |
pmic | 13:a308f5e6c306 | 220 | // V gain: -2.6080 // from Matlab |
pmic | 15:ed33be6f040e | 221 | float ref = FilterTrajectory(pi/4.0f); |
pmic | 13:a308f5e6c306 | 222 | float uV = V*ref; |
pmic | 13:a308f5e6c306 | 223 | float uSS = (-5.2142f*ang - 0.6247f*gyrz); |
pmic | 15:ed33be6f040e | 224 | desTorque = uV - uSS; // state space controller for balance |
pmic | 13:a308f5e6c306 | 225 | if(abs(desTorque) > maxTorque) { |
pmic | 13:a308f5e6c306 | 226 | desTorque = copysign(maxTorque, desTorque); |
pmic | 13:a308f5e6c306 | 227 | } |
pmic | 13:a308f5e6c306 | 228 | if(abs(ref - ang)/abs(ref) < 0.10f) { |
pmic | 13:a308f5e6c306 | 229 | shouldGoDown = 0; |
pmic | 13:a308f5e6c306 | 230 | FilterTrajectory.reset(omega); |
pmic | 13:a308f5e6c306 | 231 | } |
pmic | 8:d68e177e2571 | 232 | } else { |
pmic | 15:ed33be6f040e | 233 | desTorque = pi_w(FilterTrajectory(0.0f) - omega); // state space controller for balance |
pmic | 5:d6c7ccbbce78 | 234 | } |
pmic | 5:d6c7ccbbce78 | 235 | // convert Nm -> A and write to AOUT |
pmic | 8:d68e177e2571 | 236 | out.write(u3k3_TO_1V(i2u(desTorque*Km))); |
pmic | 5:d6c7ccbbce78 | 237 | } |
pmic | 5:d6c7ccbbce78 | 238 | |
pmic | 5:d6c7ccbbce78 | 239 | // Buttonhandling and statemachine |
pmic | 5:d6c7ccbbce78 | 240 | // ----------------------------------------------------------------------------- |
pmic | 5:d6c7ccbbce78 | 241 | // start timer as soon as Button is pressed |
pmic | 5:d6c7ccbbce78 | 242 | void pressed() |
pmic | 5:d6c7ccbbce78 | 243 | { |
pmic | 5:d6c7ccbbce78 | 244 | t.start(); |
pmic | 5:d6c7ccbbce78 | 245 | } |
pmic | 5:d6c7ccbbce78 | 246 | |
pmic | 5:d6c7ccbbce78 | 247 | // evaluating statemachine |
pmic | 5:d6c7ccbbce78 | 248 | void released() |
pmic | 5:d6c7ccbbce78 | 249 | { |
pmic | 5:d6c7ccbbce78 | 250 | |
pmic | 5:d6c7ccbbce78 | 251 | // readout, stop and reset timer |
pmic | 5:d6c7ccbbce78 | 252 | float ButtonTime = t.read(); |
pmic | 5:d6c7ccbbce78 | 253 | t.stop(); |
pmic | 5:d6c7ccbbce78 | 254 | t.reset(); |
pmic | 5:d6c7ccbbce78 | 255 | |
pmic | 5:d6c7ccbbce78 | 256 | // if the cube doesStand |
pmic | 5:d6c7ccbbce78 | 257 | if(doesStand) { |
pmic | 5:d6c7ccbbce78 | 258 | // in - or decrease speed |
pmic | 12:6287235b2570 | 259 | if(ButtonTime < 3.0f) { |
pmic | 5:d6c7ccbbce78 | 260 | // press Button long -> increase speed 5 rev/min |
pmic | 8:d68e177e2571 | 261 | if(ButtonTime > 0.3f) { |
pmic | 12:6287235b2570 | 262 | n_soll -= 5.0f; |
pmic | 5:d6c7ccbbce78 | 263 | } |
pmic | 5:d6c7ccbbce78 | 264 | // press Button short -> decrease speed 5 rev/min |
pmic | 5:d6c7ccbbce78 | 265 | else { |
pmic | 12:6287235b2570 | 266 | n_soll += 5.0f; |
pmic | 5:d6c7ccbbce78 | 267 | } |
pmic | 5:d6c7ccbbce78 | 268 | // constrain n_soll |
pmic | 5:d6c7ccbbce78 | 269 | if(n_soll > v_max) |
pmic | 5:d6c7ccbbce78 | 270 | n_soll = v_max; |
pmic | 5:d6c7ccbbce78 | 271 | if(n_soll < -v_max) |
pmic | 5:d6c7ccbbce78 | 272 | n_soll = -v_max; |
pmic | 5:d6c7ccbbce78 | 273 | } |
pmic | 5:d6c7ccbbce78 | 274 | // stop balancing |
pmic | 5:d6c7ccbbce78 | 275 | else { |
pmic | 5:d6c7ccbbce78 | 276 | n_soll = 0.0f; |
pmic | 5:d6c7ccbbce78 | 277 | shouldBalance = 0; |
pmic | 13:a308f5e6c306 | 278 | shouldGoDown = 1; |
pmic | 13:a308f5e6c306 | 279 | FilterTrajectory.reset(0.0f); |
pmic | 5:d6c7ccbbce78 | 280 | pi_w2zero.reset(0.0f); |
pmic | 5:d6c7ccbbce78 | 281 | } |
pmic | 5:d6c7ccbbce78 | 282 | } else { |
pmic | 13:a308f5e6c306 | 283 | if(ButtonTime > 3.0f) { |
pmic | 5:d6c7ccbbce78 | 284 | shouldBalance = 1; |
pmic | 13:a308f5e6c306 | 285 | shouldGoDown = 0; |
pmic | 13:a308f5e6c306 | 286 | pi_w.reset(0.0f); |
pmic | 13:a308f5e6c306 | 287 | } |
rtlabor | 0:15be70d21d7c | 288 | } |
rtlabor | 0:15be70d21d7c | 289 | } |
pmic | 8:d68e177e2571 | 290 | |
pmic | 8:d68e177e2571 | 291 | void printLine() |
pmic | 8:d68e177e2571 | 292 | { |
pmic | 8:d68e177e2571 | 293 | printf("-----------------------------------------------------------------------------------------\r\n"); |
pmic | 8:d68e177e2571 | 294 | } |