Rob Griffith / Mbed 2 deprecated rat_code

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed QEI

src/pid_controller.cpp@3:35deb5c21b33, 2018-11-26 (annotated)

Committer:
rwgriffithv
Date:
Mon Nov 26 23:50:58 2018 +0000
Revision:
3:35deb5c21b33
Parent:
1:6f18bb7a77a5 
pid controller updated

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dionigi 1:6f18bb7a77a5 1 #include "pid_controller.h"
dionigi 1:6f18bb7a77a5 2 #include "globals.h"
dionigi 1:6f18bb7a77a5 3
dionigi 1:6f18bb7a77a5 4 PIDController::PIDController() {
rwgriffithv 3:35deb5c21b33 5 reset();
rwgriffithv 3:35deb5c21b33 6 }
rwgriffithv 3:35deb5c21b33 7
rwgriffithv 3:35deb5c21b33 8 void PIDController::reset() {
rwgriffithv 3:35deb5c21b33 9 encoders.reset();
rwgriffithv 3:35deb5c21b33 10 goal_x_ = 0;
rwgriffithv 3:35deb5c21b33 11 goal_w_ = 0;
rwgriffithv 3:35deb5c21b33 12 counts_x_ = 0;
rwgriffithv 3:35deb5c21b33 13 counts_w_ = 0;
rwgriffithv 3:35deb5c21b33 14 error_x_ = 0;
rwgriffithv 3:35deb5c21b33 15 error_w_ = 0;
rwgriffithv 3:35deb5c21b33 16 pwm_x_ = 0;
rwgriffithv 3:35deb5c21b33 17 pwm_w_ = 0;
dionigi 1:6f18bb7a77a5 18 }
dionigi 1:6f18bb7a77a5 19
dionigi 1:6f18bb7a77a5 20
rwgriffithv 3:35deb5c21b33 21 void PIDController::update() {
rwgriffithv 3:35deb5c21b33 22 get_sensor_feedback();
rwgriffithv 3:35deb5c21b33 23 x_controller();
rwgriffithv 3:35deb5c21b33 24 w_controller();
rwgriffithv 3:35deb5c21b33 25 update_motor_pwm();
rwgriffithv 3:35deb5c21b33 26 if (is_done()) {
rwgriffithv 3:35deb5c21b33 27 reset();
rwgriffithv 3:35deb5c21b33 28 }
dionigi 1:6f18bb7a77a5 29 }
dionigi 1:6f18bb7a77a5 30
rwgriffithv 3:35deb5c21b33 31 void PIDController::get_sensor_feedback() {
rwgriffithv 3:35deb5c21b33 32 const float k_counts_r = (float)encoders.getPulsesR();
rwgriffithv 3:35deb5c21b33 33 const float k_counts_l = (float)encoders.getPulsesL();
rwgriffithv 3:35deb5c21b33 34
rwgriffithv 3:35deb5c21b33 35 counts_x_ = (k_counts_r + k_counts_l) / 2.0f;
rwgriffithv 3:35deb5c21b33 36 counts_w_ = k_counts_r - 360.0f/357.0f*k_counts_l;
dionigi 1:6f18bb7a77a5 37 }
dionigi 1:6f18bb7a77a5 38
rwgriffithv 3:35deb5c21b33 39 void PIDController::x_controller() {
rwgriffithv 3:35deb5c21b33 40 error_prev_x_ = error_x_;
rwgriffithv 3:35deb5c21b33 41 error_x_ = goal_x_ - counts_x_;
rwgriffithv 3:35deb5c21b33 42 pwm_x_ = limit_pwm(g_KPX * error_x_ + g_KDX * (error_x_ - error_prev_x_));
dionigi 1:6f18bb7a77a5 43 }
dionigi 1:6f18bb7a77a5 44
rwgriffithv 3:35deb5c21b33 45 void PIDController::w_controller() {
rwgriffithv 3:35deb5c21b33 46 error_prev_w_ = error_w_;
rwgriffithv 3:35deb5c21b33 47 error_w_ = goal_w_ - counts_w_;
rwgriffithv 3:35deb5c21b33 48 pwm_w_ = limit_pwm(g_KPW * error_w_ + g_KDW * (error_w_ - error_prev_w_));
dionigi 1:6f18bb7a77a5 49 }
dionigi 1:6f18bb7a77a5 50
rwgriffithv 3:35deb5c21b33 51 void PIDController::update_motor_pwm() {
rwgriffithv 3:35deb5c21b33 52 const float k_pwm_r = pwm_x_ + pwm_w_;
rwgriffithv 3:35deb5c21b33 53 const float k_pwm_l = pwm_x_ - pwm_w_;
rwgriffithv 3:35deb5c21b33 54
rwgriffithv 3:35deb5c21b33 55 if (abs(k_pwm_r) < g_MIN_SPEED && abs(k_pwm_l) < g_MIN_SPEED)
rwgriffithv 3:35deb5c21b33 56 stop_cnt_++;
rwgriffithv 3:35deb5c21b33 57
rwgriffithv 3:35deb5c21b33 58 motors.setRightPwm(k_pwm_r);
rwgriffithv 3:35deb5c21b33 59 motors.setLeftPwm(k_pwm_l);
dionigi 1:6f18bb7a77a5 60 }
dionigi 1:6f18bb7a77a5 61
rwgriffithv 3:35deb5c21b33 62 void PIDController::set_goal_x(int counts) { goal_x_ = counts; stop_cnt_ = 0; }
rwgriffithv 3:35deb5c21b33 63
rwgriffithv 3:35deb5c21b33 64 void PIDController::set_goal_w(int counts) { goal_w_ = counts; stop_cnt_ = 0; }
rwgriffithv 3:35deb5c21b33 65
rwgriffithv 3:35deb5c21b33 66 bool PIDController::is_done() const {
rwgriffithv 3:35deb5c21b33 67 return stop_cnt_ > g_DONE_THRESHOLD;
dionigi 1:6f18bb7a77a5 68 }
dionigi 1:6f18bb7a77a5 69
rwgriffithv 3:35deb5c21b33 70 char* PIDController::get_data() {
rwgriffithv 3:35deb5c21b33 71 sprintf(data_buf_, "goal_x: %d\tgoal_w: %d\tcounts_x: %.3f\tcounts_w: %.3f\terror_x: %.3f\terror_w: %.3f\tpwm_x: %.3f\tpwm_w: %.3f\n",
rwgriffithv 3:35deb5c21b33 72 goal_x_,
rwgriffithv 3:35deb5c21b33 73 goal_w_,
rwgriffithv 3:35deb5c21b33 74 counts_x_,
rwgriffithv 3:35deb5c21b33 75 counts_w_,
rwgriffithv 3:35deb5c21b33 76 error_x_,
rwgriffithv 3:35deb5c21b33 77 error_w_,
rwgriffithv 3:35deb5c21b33 78 pwm_x_,
rwgriffithv 3:35deb5c21b33 79 pwm_w_);
rwgriffithv 3:35deb5c21b33 80 return data_buf_;
dionigi 1:6f18bb7a77a5 81 }
dionigi 1:6f18bb7a77a5 82
dionigi 1:6f18bb7a77a5 83
rwgriffithv 3:35deb5c21b33 84 float PIDController::limit_pwm(float pwm) const {
rwgriffithv 3:35deb5c21b33 85 if (abs(pwm) > g_MAX_SPEED){
rwgriffithv 3:35deb5c21b33 86 return (pwm > 0) ? g_MAX_SPEED : -g_MAX_SPEED;
rwgriffithv 3:35deb5c21b33 87 } else return pwm;
dionigi 1:6f18bb7a77a5 88 }
dionigi 1:6f18bb7a77a5 89