Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- // PID parameters
- int const Kp = (int)( 10.0000 * (1 << 16) ) ;
- int const Ki = (int)( 1.0000 * (1 << 16) ) ;
- int const Kd = (int)( 0.0000 * (1 << 16) ) ;
- uint32_t const reference_position = *position_var; // initial reading from decoder
- int error_accum = 0;
- int prev_error = 0;
- unsigned signal_index = 0;
- while (true) {
- // wait for and receive load cell measurement
- int16_t force = receive_measurement();
- // sample motor position decoder
- uint32_t position = *position_var;
- int relative_position = position - reference_position;
- // get target position
- int input_signal = shmem.signal[ signal_index++ ];
- if( signal_index == sizeof(shmem.signal)/sizeof(shmem.signal[0]) )
- signal_index = 0;
- // PID control loop
- int error = input_signal - relative_position;
- error_accum += error;
- int error_change = error - prev_error;
- int control_signal = Ki * error_accum + Kp * error + Kd * error_change;
- prev_error = error;
- // update motor output
- if (control_signal < 0) {
- // place lower bound on control signal
- if( control_signal < -(4000 << 16) )
- control_signal = -(4000 << 16);
- EPWM1A = (-control_signal) >> 16;
- gpio_set_one_low( &GPIO0, 31 );
- } else {
- // place upper bound on control signal
- if (control_signal > (4000 << 16))
- control_signal = (4000 << 16);
- EPWM1A = control_signal >> 16;
- gpio_set_one_high(&GPIO0,31);
- }
- // report to python
- send_message( ++id, force, position, input_signal, control_signal );
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement