mag niet van hendrik D:
Dependencies: mbed MatrixMath QEI HIDScope Matrix biquadFilter MODSERIAL FastPWM
main.cpp@22:6cc93216b323, 2019-10-14 (annotated)
- Committer:
- Hendrikvg
- Date:
- Mon Oct 14 13:26:46 2019 +0000
- Revision:
- 22:6cc93216b323
- Parent:
- 21:394a7a1deb73
- Child:
- 23:78898ddfb103
calculate error algemener gemaakt zodat motor 2 makkelijker toegevoegd kan worden
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 | 22:6cc93216b323 | 29 | volatile double theta_1; |
Hendrikvg | 22:6cc93216b323 | 30 | volatile float theta_error_1; |
Hendrikvg | 22:6cc93216b323 | 31 | volatile float theta_reference_1; |
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 | 22:6cc93216b323 | 41 | float CalculateError(float theta_reference,float theta) |
Hendrikvg | 21:394a7a1deb73 | 42 | { |
Hendrikvg | 22:6cc93216b323 | 43 | float 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 | 22:6cc93216b323 | 71 | void CalculateDirectionMotor() |
Hendrikvg | 21:394a7a1deb73 | 72 | { |
Hendrikvg | 22:6cc93216b323 | 73 | direction_motor_1 = Controller(CalculateError(theta_reference_1,theta_1)) <= 0.0f; |
Hendrikvg | 22:6cc93216b323 | 74 | //direction_motor_2 = Controller(CalculateError(theta_reference_2,theta_2)) <= 0.0f; |
Hendrikvg | 21:394a7a1deb73 | 75 | } |
Hendrikvg | 20:ac1b4ffa3323 | 76 | |
Hendrikvg | 21:394a7a1deb73 | 77 | void WriteScope() |
Hendrikvg | 21:394a7a1deb73 | 78 | { |
Hendrikvg | 22:6cc93216b323 | 79 | scope.set(0,theta_1); |
Hendrikvg | 21:394a7a1deb73 | 80 | //scope.set(1,Controller(CalculateError())); |
Hendrikvg | 22:6cc93216b323 | 81 | scope.set(1,CalculateError(theta_reference_1,theta_1)); |
Hendrikvg | 22:6cc93216b323 | 82 | scope.set(2,theta_reference_1); |
Hendrikvg | 21:394a7a1deb73 | 83 | scope.send(); |
Hendrikvg | 21:394a7a1deb73 | 84 | } |
Hendrikvg | 16:40183eeadb6d | 85 | |
Hendrikvg | 21:394a7a1deb73 | 86 | void ReadEncoder(){ |
Hendrikvg | 22:6cc93216b323 | 87 | theta_1 = ((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 | 88 | } |
Hendrikvg | 16:40183eeadb6d | 89 | |
Hendrikvg | 21:394a7a1deb73 | 90 | void Motor_1() { |
Hendrikvg | 21:394a7a1deb73 | 91 | ReadEncoder(); |
Hendrikvg | 22:6cc93216b323 | 92 | theta_reference_1 = 360*sin(0.1f*sinus_time.read()*2*3.14f); |
Hendrikvg | 22:6cc93216b323 | 93 | CalculateDirectionMotor(); |
Hendrikvg | 22:6cc93216b323 | 94 | PWM_motor_1.write(fabs(Controller(CalculateError(theta_reference_1,theta_1))/360.0f)); |
Hendrikvg | 21:394a7a1deb73 | 95 | } |
Hendrikvg | 21:394a7a1deb73 | 96 | |
Hendrikvg | 21:394a7a1deb73 | 97 | void go_next_1() |
Hendrikvg | 21:394a7a1deb73 | 98 | { |
Hendrikvg | 21:394a7a1deb73 | 99 | demo = !demo; |
Hendrikvg | 21:394a7a1deb73 | 100 | } |
Hendrikvg | 21:394a7a1deb73 | 101 | |
Hendrikvg | 21:394a7a1deb73 | 102 | void go_next_2() |
Hendrikvg | 21:394a7a1deb73 | 103 | { |
Hendrikvg | 21:394a7a1deb73 | 104 | emg = !emg; |
Hendrikvg | 21:394a7a1deb73 | 105 | next = emg; |
Hendrikvg | 21:394a7a1deb73 | 106 | } |
Hendrikvg | 21:394a7a1deb73 | 107 | |
Hendrikvg | 21:394a7a1deb73 | 108 | void while_start() |
Hendrikvg | 21:394a7a1deb73 | 109 | { |
Hendrikvg | 21:394a7a1deb73 | 110 | // Do startup stuff |
Hendrikvg | 21:394a7a1deb73 | 111 | CurrentState = motor_calibration; |
Hendrikvg | 21:394a7a1deb73 | 112 | StateChanged = true; |
Hendrikvg | 21:394a7a1deb73 | 113 | } |
Hendrikvg | 21:394a7a1deb73 | 114 | |
Hendrikvg | 21:394a7a1deb73 | 115 | void while_motor_calibration() |
Hendrikvg | 21:394a7a1deb73 | 116 | { |
Hendrikvg | 21:394a7a1deb73 | 117 | // Do motor calibration stuff |
Hendrikvg | 21:394a7a1deb73 | 118 | if (demo) // bool aanmaken voor demo (switch oid aanmaken) |
Hendrikvg | 21:394a7a1deb73 | 119 | { |
Hendrikvg | 21:394a7a1deb73 | 120 | CurrentState = demo_mode; |
Hendrikvg | 21:394a7a1deb73 | 121 | StateChanged = true; |
Hendrikvg | 21:394a7a1deb73 | 122 | } |
Hendrikvg | 21:394a7a1deb73 | 123 | if (emg) // bool aanmaken voor EMG (switch oid aanmaken) |
Hendrikvg | 21:394a7a1deb73 | 124 | { |
Hendrikvg | 21:394a7a1deb73 | 125 | CurrentState = emg_calibration; |
Hendrikvg | 21:394a7a1deb73 | 126 | StateChanged = true; |
Hendrikvg | 21:394a7a1deb73 | 127 | } |
Hendrikvg | 21:394a7a1deb73 | 128 | } |
Hendrikvg | 21:394a7a1deb73 | 129 | |
Hendrikvg | 21:394a7a1deb73 | 130 | void while_demo_mode() |
Hendrikvg | 21:394a7a1deb73 | 131 | { |
Hendrikvg | 21:394a7a1deb73 | 132 | // Do demo mode stuff |
Hendrikvg | 21:394a7a1deb73 | 133 | if (!demo) // bool aanmaken voor demo (switch oid) |
Hendrikvg | 21:394a7a1deb73 | 134 | { |
Hendrikvg | 21:394a7a1deb73 | 135 | emg = true; |
Hendrikvg | 21:394a7a1deb73 | 136 | } |
Hendrikvg | 21:394a7a1deb73 | 137 | if (emg) // bool aanmaken voor EMG (switch oid aanmaken) |
Hendrikvg | 21:394a7a1deb73 | 138 | { |
Hendrikvg | 21:394a7a1deb73 | 139 | CurrentState = emg_calibration; |
Hendrikvg | 21:394a7a1deb73 | 140 | StateChanged = true; |
Hendrikvg | 21:394a7a1deb73 | 141 | } |
Hendrikvg | 21:394a7a1deb73 | 142 | } |
Hendrikvg | 21:394a7a1deb73 | 143 | |
Hendrikvg | 21:394a7a1deb73 | 144 | void while_emg_calibration() |
Hendrikvg | 21:394a7a1deb73 | 145 | { |
Hendrikvg | 21:394a7a1deb73 | 146 | // Do emg calibration stuff |
Hendrikvg | 21:394a7a1deb73 | 147 | if (!emg) // bool aanmaken voor EMG (switch) |
Hendrikvg | 21:394a7a1deb73 | 148 | { |
Hendrikvg | 21:394a7a1deb73 | 149 | CurrentState = vertical_movement; |
Hendrikvg | 21:394a7a1deb73 | 150 | StateChanged = true; |
Hendrikvg | 21:394a7a1deb73 | 151 | } |
Hendrikvg | 21:394a7a1deb73 | 152 | } |
Hendrikvg | 21:394a7a1deb73 | 153 | |
Hendrikvg | 21:394a7a1deb73 | 154 | void while_vertical_movement() |
Hendrikvg | 21:394a7a1deb73 | 155 | { |
Hendrikvg | 21:394a7a1deb73 | 156 | // Do vertical movement stuff |
Hendrikvg | 21:394a7a1deb73 | 157 | if (next) // EMG gebaseerde threshold aanmaken |
Hendrikvg | 21:394a7a1deb73 | 158 | { |
Hendrikvg | 21:394a7a1deb73 | 159 | CurrentState = horizontal_movement; |
Hendrikvg | 21:394a7a1deb73 | 160 | StateChanged = true; |
Hendrikvg | 21:394a7a1deb73 | 161 | } |
Hendrikvg | 21:394a7a1deb73 | 162 | } |
Hendrikvg | 17:cacf9e75eda7 | 163 | |
Hendrikvg | 21:394a7a1deb73 | 164 | void while_horizontal_movement() |
Hendrikvg | 21:394a7a1deb73 | 165 | { |
Hendrikvg | 21:394a7a1deb73 | 166 | // Do horizontal movement stuff |
Hendrikvg | 21:394a7a1deb73 | 167 | if (!next) // EMG gebaseerde threshold aanmaken |
Hendrikvg | 21:394a7a1deb73 | 168 | { |
Hendrikvg | 21:394a7a1deb73 | 169 | CurrentState = vertical_movement; |
Hendrikvg | 21:394a7a1deb73 | 170 | StateChanged = true; |
Hendrikvg | 21:394a7a1deb73 | 171 | } |
Hendrikvg | 21:394a7a1deb73 | 172 | } |
Hendrikvg | 21:394a7a1deb73 | 173 | |
Hendrikvg | 21:394a7a1deb73 | 174 | void StateTransition() |
Hendrikvg | 21:394a7a1deb73 | 175 | { |
Hendrikvg | 21:394a7a1deb73 | 176 | if (StateChanged) |
Hendrikvg | 21:394a7a1deb73 | 177 | { |
Hendrikvg | 21:394a7a1deb73 | 178 | if (CurrentState == start) |
Hendrikvg | 21:394a7a1deb73 | 179 | { |
Hendrikvg | 21:394a7a1deb73 | 180 | pc.printf("Initiating start.\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 181 | } |
Hendrikvg | 21:394a7a1deb73 | 182 | if (CurrentState == motor_calibration) |
Hendrikvg | 21:394a7a1deb73 | 183 | { |
Hendrikvg | 21:394a7a1deb73 | 184 | pc.printf("Initiating motor_calibration.\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 185 | } |
Hendrikvg | 21:394a7a1deb73 | 186 | if (CurrentState == demo_mode) |
Hendrikvg | 21:394a7a1deb73 | 187 | { |
Hendrikvg | 21:394a7a1deb73 | 188 | pc.printf("Initiating demo_mode.\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 189 | } |
Hendrikvg | 21:394a7a1deb73 | 190 | if (CurrentState == emg_calibration) |
Hendrikvg | 21:394a7a1deb73 | 191 | { |
Hendrikvg | 21:394a7a1deb73 | 192 | pc.printf("Initiating emg_calibration.\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 193 | } |
Hendrikvg | 21:394a7a1deb73 | 194 | if (CurrentState == vertical_movement) |
Hendrikvg | 21:394a7a1deb73 | 195 | { |
Hendrikvg | 21:394a7a1deb73 | 196 | pc.printf("Initiating vertical_movement.\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 197 | } |
Hendrikvg | 21:394a7a1deb73 | 198 | if (CurrentState == horizontal_movement) |
Hendrikvg | 21:394a7a1deb73 | 199 | { |
Hendrikvg | 21:394a7a1deb73 | 200 | pc.printf("Initiating horizontal_movement.\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 201 | } |
Hendrikvg | 21:394a7a1deb73 | 202 | StateChanged = false; |
Hendrikvg | 21:394a7a1deb73 | 203 | } |
Hendrikvg | 21:394a7a1deb73 | 204 | } |
Hendrikvg | 21:394a7a1deb73 | 205 | |
Hendrikvg | 21:394a7a1deb73 | 206 | void StateMachine() |
Hendrikvg | 21:394a7a1deb73 | 207 | { |
Hendrikvg | 21:394a7a1deb73 | 208 | switch(CurrentState) |
Hendrikvg | 21:394a7a1deb73 | 209 | { |
Hendrikvg | 21:394a7a1deb73 | 210 | case start: |
Hendrikvg | 21:394a7a1deb73 | 211 | StateTransition(); |
Hendrikvg | 21:394a7a1deb73 | 212 | while_start(); |
Hendrikvg | 21:394a7a1deb73 | 213 | break; |
Hendrikvg | 21:394a7a1deb73 | 214 | case motor_calibration: |
Hendrikvg | 21:394a7a1deb73 | 215 | StateTransition(); |
Hendrikvg | 21:394a7a1deb73 | 216 | while_motor_calibration(); |
Hendrikvg | 21:394a7a1deb73 | 217 | break; |
Hendrikvg | 21:394a7a1deb73 | 218 | case demo_mode: |
Hendrikvg | 21:394a7a1deb73 | 219 | StateTransition(); |
Hendrikvg | 21:394a7a1deb73 | 220 | while_demo_mode(); |
Hendrikvg | 21:394a7a1deb73 | 221 | break; |
Hendrikvg | 21:394a7a1deb73 | 222 | case emg_calibration: |
Hendrikvg | 21:394a7a1deb73 | 223 | StateTransition(); |
Hendrikvg | 21:394a7a1deb73 | 224 | while_emg_calibration(); |
Hendrikvg | 21:394a7a1deb73 | 225 | break; |
Hendrikvg | 21:394a7a1deb73 | 226 | case vertical_movement: |
Hendrikvg | 21:394a7a1deb73 | 227 | StateTransition(); |
Hendrikvg | 21:394a7a1deb73 | 228 | while_vertical_movement(); |
Hendrikvg | 21:394a7a1deb73 | 229 | break; |
Hendrikvg | 21:394a7a1deb73 | 230 | case horizontal_movement: |
Hendrikvg | 21:394a7a1deb73 | 231 | StateTransition(); |
Hendrikvg | 21:394a7a1deb73 | 232 | while_horizontal_movement(); |
Hendrikvg | 21:394a7a1deb73 | 233 | break; |
Hendrikvg | 21:394a7a1deb73 | 234 | default: |
Hendrikvg | 21:394a7a1deb73 | 235 | pc.printf("Unknown or unimplemented state reached!\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 236 | } |
Hendrikvg | 21:394a7a1deb73 | 237 | } |
Hendrikvg | 21:394a7a1deb73 | 238 | |
Hendrikvg | 15:80b3ac2b8448 | 239 | // main |
Hendrikvg | 21:394a7a1deb73 | 240 | int main(){ |
RobertoO | 0:67c50348f842 | 241 | pc.baud(115200); |
Hendrikvg | 17:cacf9e75eda7 | 242 | pc.printf("Hello World!\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 243 | button_1.fall(go_next_1); |
Hendrikvg | 21:394a7a1deb73 | 244 | button_2.fall(go_next_2); |
Hendrikvg | 21:394a7a1deb73 | 245 | sinus_time.start(); |
Hendrikvg | 21:394a7a1deb73 | 246 | PWM_motor_1.period_ms(10); |
Hendrikvg | 21:394a7a1deb73 | 247 | ControlMotor1.attach(&Motor_1, Ts); |
Hendrikvg | 21:394a7a1deb73 | 248 | RW_scope.attach(&WriteScope, 0.1); |
Hendrikvg | 21:394a7a1deb73 | 249 | //TickerStateMachine.attach(StateMachine,1.00f); |
Hendrikvg | 21:394a7a1deb73 | 250 | while(true) { |
Hendrikvg | 21:394a7a1deb73 | 251 | StateMachine(); |
Hendrikvg | 21:394a7a1deb73 | 252 | } |
Hendrikvg | 2:d9b0ebf3fcca | 253 | } |