SHOW:
|
|
- or go back to the newest paste.
| 1 | // PID parameters | |
| 2 | int const Kp = (int)( 10.0000 * (1 << 16) ) ; | |
| 3 | int const Ki = (int)( 1.0000 * (1 << 16) ) ; | |
| 4 | int const Kd = (int)( 0.0000 * (1 << 16) ) ; | |
| 5 | ||
| 6 | uint32_t const reference_position = *position_var; // initial reading from decoder | |
| 7 | ||
| 8 | - | int control_signal = 0; |
| 8 | + | int error_accum = 0; |
| 9 | int prev_error = 0; | |
| 10 | - | int prev_error_change = 0; |
| 10 | + | |
| 11 | ||
| 12 | while (true) {
| |
| 13 | // wait for and receive load cell measurement | |
| 14 | int16_t force = receive_measurement(); | |
| 15 | ||
| 16 | // sample motor position decoder | |
| 17 | uint32_t position = *position_var; | |
| 18 | int relative_position = position - reference_position; | |
| 19 | ||
| 20 | // get target position | |
| 21 | int input_signal = shmem.signal[ signal_index++ ]; | |
| 22 | if( signal_index == sizeof(shmem.signal)/sizeof(shmem.signal[0]) ) | |
| 23 | signal_index = 0; | |
| 24 | ||
| 25 | // PID control loop | |
| 26 | int error = input_signal - relative_position; | |
| 27 | error_accum += error; | |
| 28 | int error_change = error - prev_error; | |
| 29 | - | int error_change_change = error_change - prev_error_change; |
| 29 | + | int control_signal = Ki * error_accum + Kp * error + Kd * error_change; |
| 30 | - | control_signal += Ki * error + Kp * error_change + Kd * error_change_change; |
| 30 | + | |
| 31 | ||
| 32 | - | prev_error_change = error_change; |
| 32 | + | |
| 33 | if (control_signal < 0) {
| |
| 34 | // place lower bound on control signal | |
| 35 | if( control_signal < -(4000 << 16) ) | |
| 36 | control_signal = -(4000 << 16); | |
| 37 | EPWM1A = (-control_signal) >> 16; | |
| 38 | gpio_set_one_low( &GPIO0, 31 ); | |
| 39 | } else {
| |
| 40 | // place upper bound on control signal | |
| 41 | if (control_signal > (4000 << 16)) | |
| 42 | control_signal = (4000 << 16); | |
| 43 | EPWM1A = control_signal >> 16; | |
| 44 | gpio_set_one_high(&GPIO0,31); | |
| 45 | } | |
| 46 | ||
| 47 | // report to python | |
| 48 | send_message( ++id, force, position, input_signal, control_signal ); | |
| 49 | } |
