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