mag niet van hendrik D:
Dependencies: mbed MatrixMath QEI HIDScope Matrix biquadFilter MODSERIAL FastPWM
main.cpp@21:394a7a1deb73, 2019-10-14 (annotated)
- Committer:
- Hendrikvg
- Date:
- Mon Oct 14 08:50:26 2019 +0000
- Revision:
- 21:394a7a1deb73
- Parent:
- 20:ac1b4ffa3323
- Child:
- 22:6cc93216b323
State machine en PID controller
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Hendrikvg | 17:cacf9e75eda7 | 1 | #include "QEI.h" |
Hendrikvg | 15:80b3ac2b8448 | 2 | #include "mbed.h" |
Hendrikvg | 20:ac1b4ffa3323 | 3 | #include "BiQuad.h" |
Hendrikvg | 14:20f11bb58244 | 4 | #include "FastPWM.h" |
Hendrikvg | 17:cacf9e75eda7 | 5 | #include "HIDScope.h" |
Hendrikvg | 16:40183eeadb6d | 6 | #include "MODSERIAL.h" |
Hendrikvg | 9:12b9865e7373 | 7 | |
Hendrikvg | 21:394a7a1deb73 | 8 | // Pins |
Hendrikvg | 21:394a7a1deb73 | 9 | MODSERIAL pc(USBTX, USBRX); |
Hendrikvg | 21:394a7a1deb73 | 10 | InterruptIn stepresponse(D0); |
Hendrikvg | 21:394a7a1deb73 | 11 | FastPWM PWM_motor_1(D6); |
Hendrikvg | 20:ac1b4ffa3323 | 12 | DigitalOut direction_motor_1(D7); |
Hendrikvg | 21:394a7a1deb73 | 13 | InterruptIn button_1(SW2); |
Hendrikvg | 21:394a7a1deb73 | 14 | InterruptIn button_2(SW3); |
Hendrikvg | 17:cacf9e75eda7 | 15 | |
Hendrikvg | 15:80b3ac2b8448 | 16 | // variables |
Hendrikvg | 21:394a7a1deb73 | 17 | Ticker TickerStateMachine; |
Hendrikvg | 21:394a7a1deb73 | 18 | Ticker ControlMotor1; |
Hendrikvg | 21:394a7a1deb73 | 19 | Ticker RW_scope; |
Hendrikvg | 21:394a7a1deb73 | 20 | Timer sinus_time; |
Hendrikvg | 21:394a7a1deb73 | 21 | enum states {start, motor_calibration, demo_mode, emg_calibration, vertical_movement, horizontal_movement}; |
Hendrikvg | 21:394a7a1deb73 | 22 | states CurrentState = start; |
Hendrikvg | 21:394a7a1deb73 | 23 | bool StateChanged = true; |
Hendrikvg | 21:394a7a1deb73 | 24 | bool demo = false; |
Hendrikvg | 21:394a7a1deb73 | 25 | bool emg = false; |
Hendrikvg | 21:394a7a1deb73 | 26 | bool next = false; |
Hendrikvg | 21:394a7a1deb73 | 27 | HIDScope scope(3); |
Hendrikvg | 21:394a7a1deb73 | 28 | QEI encoder(D12,D13,NC,8400,QEI::X4_ENCODING); |
Hendrikvg | 21:394a7a1deb73 | 29 | volatile double theta; |
Hendrikvg | 21:394a7a1deb73 | 30 | float theta_error; |
Hendrikvg | 21:394a7a1deb73 | 31 | volatile float theta_reference; |
Hendrikvg | 21:394a7a1deb73 | 32 | float K = 1; |
Hendrikvg | 21:394a7a1deb73 | 33 | float ti = 0.1; |
Hendrikvg | 21:394a7a1deb73 | 34 | float td = 10; |
Hendrikvg | 21:394a7a1deb73 | 35 | float Kp = K*(1+td/ti); |
Hendrikvg | 21:394a7a1deb73 | 36 | float Ki = K/ti; |
Hendrikvg | 21:394a7a1deb73 | 37 | float Kd = K*td; |
Hendrikvg | 21:394a7a1deb73 | 38 | float Ts = 0.001; |
Hendrikvg | 20:ac1b4ffa3323 | 39 | |
Hendrikvg | 21:394a7a1deb73 | 40 | // functies |
Hendrikvg | 21:394a7a1deb73 | 41 | float CalculateError() |
Hendrikvg | 21:394a7a1deb73 | 42 | { |
Hendrikvg | 21:394a7a1deb73 | 43 | theta_error = theta_reference-theta; |
Hendrikvg | 21:394a7a1deb73 | 44 | return theta_error; |
Hendrikvg | 21:394a7a1deb73 | 45 | } |
Hendrikvg | 20:ac1b4ffa3323 | 46 | |
Hendrikvg | 21:394a7a1deb73 | 47 | float Controller(float theta_error) |
Hendrikvg | 21:394a7a1deb73 | 48 | { |
Hendrikvg | 21:394a7a1deb73 | 49 | static float error_integral = 0; |
Hendrikvg | 21:394a7a1deb73 | 50 | static float error_prev = 0; |
Hendrikvg | 21:394a7a1deb73 | 51 | static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); |
Hendrikvg | 20:ac1b4ffa3323 | 52 | |
Hendrikvg | 20:ac1b4ffa3323 | 53 | // Proportional part: |
Hendrikvg | 20:ac1b4ffa3323 | 54 | float torque_p = Kp * theta_error; |
Hendrikvg | 20:ac1b4ffa3323 | 55 | |
Hendrikvg | 20:ac1b4ffa3323 | 56 | // Integral part: |
Hendrikvg | 21:394a7a1deb73 | 57 | error_integral = error_integral + theta_error * Ts; |
Hendrikvg | 21:394a7a1deb73 | 58 | float torque_i = Ki * error_integral; |
Hendrikvg | 20:ac1b4ffa3323 | 59 | |
Hendrikvg | 20:ac1b4ffa3323 | 60 | // Derivative part: |
Hendrikvg | 21:394a7a1deb73 | 61 | float error_derivative = (theta_error - error_prev)/Ts; |
Hendrikvg | 21:394a7a1deb73 | 62 | float filtered_error_derivative = LowPassFilter.step(error_derivative); |
Hendrikvg | 21:394a7a1deb73 | 63 | float torque_d = Kd * filtered_error_derivative; |
Hendrikvg | 21:394a7a1deb73 | 64 | error_prev = theta_error; |
Hendrikvg | 20:ac1b4ffa3323 | 65 | |
Hendrikvg | 20:ac1b4ffa3323 | 66 | // Sum all parts and return it |
Hendrikvg | 21:394a7a1deb73 | 67 | float torque = torque_p + torque_i + torque_d; |
Hendrikvg | 21:394a7a1deb73 | 68 | return torque; |
Hendrikvg | 21:394a7a1deb73 | 69 | } |
Hendrikvg | 16:40183eeadb6d | 70 | |
Hendrikvg | 21:394a7a1deb73 | 71 | void CalculateDirectionMotor1() |
Hendrikvg | 21:394a7a1deb73 | 72 | { |
Hendrikvg | 21:394a7a1deb73 | 73 | direction_motor_1 = Controller(CalculateError()) <= 0.0f ; |
Hendrikvg | 21:394a7a1deb73 | 74 | } |
Hendrikvg | 20:ac1b4ffa3323 | 75 | |
Hendrikvg | 21:394a7a1deb73 | 76 | void WriteScope() |
Hendrikvg | 21:394a7a1deb73 | 77 | { |
Hendrikvg | 21:394a7a1deb73 | 78 | scope.set(0,theta); |
Hendrikvg | 21:394a7a1deb73 | 79 | //scope.set(1,Controller(CalculateError())); |
Hendrikvg | 21:394a7a1deb73 | 80 | scope.set(1,CalculateError()); |
Hendrikvg | 21:394a7a1deb73 | 81 | scope.set(2,theta_reference); |
Hendrikvg | 21:394a7a1deb73 | 82 | scope.send(); |
Hendrikvg | 21:394a7a1deb73 | 83 | } |
Hendrikvg | 16:40183eeadb6d | 84 | |
Hendrikvg | 21:394a7a1deb73 | 85 | void ReadEncoder(){ |
Hendrikvg | 21:394a7a1deb73 | 86 | theta = ((360.0f/64.0f)*(float)encoder.getPulses())/131.25f; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360. |
Hendrikvg | 20:ac1b4ffa3323 | 87 | } |
Hendrikvg | 16:40183eeadb6d | 88 | |
Hendrikvg | 21:394a7a1deb73 | 89 | void Motor_1() { |
Hendrikvg | 21:394a7a1deb73 | 90 | ReadEncoder(); |
Hendrikvg | 21:394a7a1deb73 | 91 | theta_reference = 360*sin(0.1f*sinus_time.read()*2*3.14f); |
Hendrikvg | 21:394a7a1deb73 | 92 | CalculateDirectionMotor1(); |
Hendrikvg | 21:394a7a1deb73 | 93 | PWM_motor_1.write(fabs(Controller(CalculateError())/360.0f)); |
Hendrikvg | 21:394a7a1deb73 | 94 | } |
Hendrikvg | 21:394a7a1deb73 | 95 | |
Hendrikvg | 21:394a7a1deb73 | 96 | void go_next_1() |
Hendrikvg | 21:394a7a1deb73 | 97 | { |
Hendrikvg | 21:394a7a1deb73 | 98 | demo = !demo; |
Hendrikvg | 21:394a7a1deb73 | 99 | } |
Hendrikvg | 21:394a7a1deb73 | 100 | |
Hendrikvg | 21:394a7a1deb73 | 101 | void go_next_2() |
Hendrikvg | 21:394a7a1deb73 | 102 | { |
Hendrikvg | 21:394a7a1deb73 | 103 | emg = !emg; |
Hendrikvg | 21:394a7a1deb73 | 104 | next = emg; |
Hendrikvg | 21:394a7a1deb73 | 105 | } |
Hendrikvg | 21:394a7a1deb73 | 106 | |
Hendrikvg | 21:394a7a1deb73 | 107 | void while_start() |
Hendrikvg | 21:394a7a1deb73 | 108 | { |
Hendrikvg | 21:394a7a1deb73 | 109 | // Do startup stuff |
Hendrikvg | 21:394a7a1deb73 | 110 | CurrentState = motor_calibration; |
Hendrikvg | 21:394a7a1deb73 | 111 | StateChanged = true; |
Hendrikvg | 21:394a7a1deb73 | 112 | } |
Hendrikvg | 21:394a7a1deb73 | 113 | |
Hendrikvg | 21:394a7a1deb73 | 114 | void while_motor_calibration() |
Hendrikvg | 21:394a7a1deb73 | 115 | { |
Hendrikvg | 21:394a7a1deb73 | 116 | // Do motor calibration stuff |
Hendrikvg | 21:394a7a1deb73 | 117 | if (demo) // bool aanmaken voor demo (switch oid aanmaken) |
Hendrikvg | 21:394a7a1deb73 | 118 | { |
Hendrikvg | 21:394a7a1deb73 | 119 | CurrentState = demo_mode; |
Hendrikvg | 21:394a7a1deb73 | 120 | StateChanged = true; |
Hendrikvg | 21:394a7a1deb73 | 121 | } |
Hendrikvg | 21:394a7a1deb73 | 122 | if (emg) // bool aanmaken voor EMG (switch oid aanmaken) |
Hendrikvg | 21:394a7a1deb73 | 123 | { |
Hendrikvg | 21:394a7a1deb73 | 124 | CurrentState = emg_calibration; |
Hendrikvg | 21:394a7a1deb73 | 125 | StateChanged = true; |
Hendrikvg | 21:394a7a1deb73 | 126 | } |
Hendrikvg | 21:394a7a1deb73 | 127 | } |
Hendrikvg | 21:394a7a1deb73 | 128 | |
Hendrikvg | 21:394a7a1deb73 | 129 | void while_demo_mode() |
Hendrikvg | 21:394a7a1deb73 | 130 | { |
Hendrikvg | 21:394a7a1deb73 | 131 | // Do demo mode stuff |
Hendrikvg | 21:394a7a1deb73 | 132 | if (!demo) // bool aanmaken voor demo (switch oid) |
Hendrikvg | 21:394a7a1deb73 | 133 | { |
Hendrikvg | 21:394a7a1deb73 | 134 | emg = true; |
Hendrikvg | 21:394a7a1deb73 | 135 | } |
Hendrikvg | 21:394a7a1deb73 | 136 | if (emg) // bool aanmaken voor EMG (switch oid aanmaken) |
Hendrikvg | 21:394a7a1deb73 | 137 | { |
Hendrikvg | 21:394a7a1deb73 | 138 | CurrentState = emg_calibration; |
Hendrikvg | 21:394a7a1deb73 | 139 | StateChanged = true; |
Hendrikvg | 21:394a7a1deb73 | 140 | } |
Hendrikvg | 21:394a7a1deb73 | 141 | } |
Hendrikvg | 21:394a7a1deb73 | 142 | |
Hendrikvg | 21:394a7a1deb73 | 143 | void while_emg_calibration() |
Hendrikvg | 21:394a7a1deb73 | 144 | { |
Hendrikvg | 21:394a7a1deb73 | 145 | // Do emg calibration stuff |
Hendrikvg | 21:394a7a1deb73 | 146 | if (!emg) // bool aanmaken voor EMG (switch) |
Hendrikvg | 21:394a7a1deb73 | 147 | { |
Hendrikvg | 21:394a7a1deb73 | 148 | CurrentState = vertical_movement; |
Hendrikvg | 21:394a7a1deb73 | 149 | StateChanged = true; |
Hendrikvg | 21:394a7a1deb73 | 150 | } |
Hendrikvg | 21:394a7a1deb73 | 151 | } |
Hendrikvg | 21:394a7a1deb73 | 152 | |
Hendrikvg | 21:394a7a1deb73 | 153 | void while_vertical_movement() |
Hendrikvg | 21:394a7a1deb73 | 154 | { |
Hendrikvg | 21:394a7a1deb73 | 155 | // Do vertical movement stuff |
Hendrikvg | 21:394a7a1deb73 | 156 | if (next) // EMG gebaseerde threshold aanmaken |
Hendrikvg | 21:394a7a1deb73 | 157 | { |
Hendrikvg | 21:394a7a1deb73 | 158 | CurrentState = horizontal_movement; |
Hendrikvg | 21:394a7a1deb73 | 159 | StateChanged = true; |
Hendrikvg | 21:394a7a1deb73 | 160 | } |
Hendrikvg | 21:394a7a1deb73 | 161 | } |
Hendrikvg | 17:cacf9e75eda7 | 162 | |
Hendrikvg | 21:394a7a1deb73 | 163 | void while_horizontal_movement() |
Hendrikvg | 21:394a7a1deb73 | 164 | { |
Hendrikvg | 21:394a7a1deb73 | 165 | // Do horizontal movement stuff |
Hendrikvg | 21:394a7a1deb73 | 166 | if (!next) // EMG gebaseerde threshold aanmaken |
Hendrikvg | 21:394a7a1deb73 | 167 | { |
Hendrikvg | 21:394a7a1deb73 | 168 | CurrentState = vertical_movement; |
Hendrikvg | 21:394a7a1deb73 | 169 | StateChanged = true; |
Hendrikvg | 21:394a7a1deb73 | 170 | } |
Hendrikvg | 21:394a7a1deb73 | 171 | } |
Hendrikvg | 21:394a7a1deb73 | 172 | |
Hendrikvg | 21:394a7a1deb73 | 173 | void StateTransition() |
Hendrikvg | 21:394a7a1deb73 | 174 | { |
Hendrikvg | 21:394a7a1deb73 | 175 | if (StateChanged) |
Hendrikvg | 21:394a7a1deb73 | 176 | { |
Hendrikvg | 21:394a7a1deb73 | 177 | if (CurrentState == start) |
Hendrikvg | 21:394a7a1deb73 | 178 | { |
Hendrikvg | 21:394a7a1deb73 | 179 | pc.printf("Initiating start.\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 180 | } |
Hendrikvg | 21:394a7a1deb73 | 181 | if (CurrentState == motor_calibration) |
Hendrikvg | 21:394a7a1deb73 | 182 | { |
Hendrikvg | 21:394a7a1deb73 | 183 | pc.printf("Initiating motor_calibration.\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 184 | } |
Hendrikvg | 21:394a7a1deb73 | 185 | if (CurrentState == demo_mode) |
Hendrikvg | 21:394a7a1deb73 | 186 | { |
Hendrikvg | 21:394a7a1deb73 | 187 | pc.printf("Initiating demo_mode.\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 188 | } |
Hendrikvg | 21:394a7a1deb73 | 189 | if (CurrentState == emg_calibration) |
Hendrikvg | 21:394a7a1deb73 | 190 | { |
Hendrikvg | 21:394a7a1deb73 | 191 | pc.printf("Initiating emg_calibration.\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 192 | } |
Hendrikvg | 21:394a7a1deb73 | 193 | if (CurrentState == vertical_movement) |
Hendrikvg | 21:394a7a1deb73 | 194 | { |
Hendrikvg | 21:394a7a1deb73 | 195 | pc.printf("Initiating vertical_movement.\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 196 | } |
Hendrikvg | 21:394a7a1deb73 | 197 | if (CurrentState == horizontal_movement) |
Hendrikvg | 21:394a7a1deb73 | 198 | { |
Hendrikvg | 21:394a7a1deb73 | 199 | pc.printf("Initiating horizontal_movement.\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 200 | } |
Hendrikvg | 21:394a7a1deb73 | 201 | StateChanged = false; |
Hendrikvg | 21:394a7a1deb73 | 202 | } |
Hendrikvg | 21:394a7a1deb73 | 203 | } |
Hendrikvg | 21:394a7a1deb73 | 204 | |
Hendrikvg | 21:394a7a1deb73 | 205 | void StateMachine() |
Hendrikvg | 21:394a7a1deb73 | 206 | { |
Hendrikvg | 21:394a7a1deb73 | 207 | switch(CurrentState) |
Hendrikvg | 21:394a7a1deb73 | 208 | { |
Hendrikvg | 21:394a7a1deb73 | 209 | case start: |
Hendrikvg | 21:394a7a1deb73 | 210 | StateTransition(); |
Hendrikvg | 21:394a7a1deb73 | 211 | while_start(); |
Hendrikvg | 21:394a7a1deb73 | 212 | break; |
Hendrikvg | 21:394a7a1deb73 | 213 | case motor_calibration: |
Hendrikvg | 21:394a7a1deb73 | 214 | StateTransition(); |
Hendrikvg | 21:394a7a1deb73 | 215 | while_motor_calibration(); |
Hendrikvg | 21:394a7a1deb73 | 216 | break; |
Hendrikvg | 21:394a7a1deb73 | 217 | case demo_mode: |
Hendrikvg | 21:394a7a1deb73 | 218 | StateTransition(); |
Hendrikvg | 21:394a7a1deb73 | 219 | while_demo_mode(); |
Hendrikvg | 21:394a7a1deb73 | 220 | break; |
Hendrikvg | 21:394a7a1deb73 | 221 | case emg_calibration: |
Hendrikvg | 21:394a7a1deb73 | 222 | StateTransition(); |
Hendrikvg | 21:394a7a1deb73 | 223 | while_emg_calibration(); |
Hendrikvg | 21:394a7a1deb73 | 224 | break; |
Hendrikvg | 21:394a7a1deb73 | 225 | case vertical_movement: |
Hendrikvg | 21:394a7a1deb73 | 226 | StateTransition(); |
Hendrikvg | 21:394a7a1deb73 | 227 | while_vertical_movement(); |
Hendrikvg | 21:394a7a1deb73 | 228 | break; |
Hendrikvg | 21:394a7a1deb73 | 229 | case horizontal_movement: |
Hendrikvg | 21:394a7a1deb73 | 230 | StateTransition(); |
Hendrikvg | 21:394a7a1deb73 | 231 | while_horizontal_movement(); |
Hendrikvg | 21:394a7a1deb73 | 232 | break; |
Hendrikvg | 21:394a7a1deb73 | 233 | default: |
Hendrikvg | 21:394a7a1deb73 | 234 | pc.printf("Unknown or unimplemented state reached!\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 235 | } |
Hendrikvg | 21:394a7a1deb73 | 236 | } |
Hendrikvg | 21:394a7a1deb73 | 237 | |
Hendrikvg | 15:80b3ac2b8448 | 238 | // main |
Hendrikvg | 21:394a7a1deb73 | 239 | int main(){ |
RobertoO | 0:67c50348f842 | 240 | pc.baud(115200); |
Hendrikvg | 17:cacf9e75eda7 | 241 | pc.printf("Hello World!\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 242 | button_1.fall(go_next_1); |
Hendrikvg | 21:394a7a1deb73 | 243 | button_2.fall(go_next_2); |
Hendrikvg | 21:394a7a1deb73 | 244 | sinus_time.start(); |
Hendrikvg | 21:394a7a1deb73 | 245 | PWM_motor_1.period_ms(10); |
Hendrikvg | 21:394a7a1deb73 | 246 | ControlMotor1.attach(&Motor_1, Ts); |
Hendrikvg | 21:394a7a1deb73 | 247 | RW_scope.attach(&WriteScope, 0.1); |
Hendrikvg | 21:394a7a1deb73 | 248 | //TickerStateMachine.attach(StateMachine,1.00f); |
Hendrikvg | 21:394a7a1deb73 | 249 | while(true) { |
Hendrikvg | 21:394a7a1deb73 | 250 | StateMachine(); |
Hendrikvg | 21:394a7a1deb73 | 251 | } |
Hendrikvg | 2:d9b0ebf3fcca | 252 | } |