File content as of revision 3:35deb5c21b33:

#include "pid_controller.h"
#include "globals.h"

PIDController::PIDController() {
  reset();
}

void PIDController::reset()  {
  encoders.reset();
  goal_x_ = 0;
  goal_w_ = 0;
  counts_x_ = 0;
  counts_w_ = 0;
  error_x_ = 0;
  error_w_ = 0;
  pwm_x_ = 0;
  pwm_w_ = 0;
}


void PIDController::update()  {
  get_sensor_feedback();
  x_controller();
  w_controller();
  update_motor_pwm();
  if (is_done()) {
  	reset();
  }
}

void PIDController::get_sensor_feedback()  {
  const float k_counts_r = (float)encoders.getPulsesR();
  const float k_counts_l = (float)encoders.getPulsesL();
	
  counts_x_ = (k_counts_r + k_counts_l) / 2.0f;
  counts_w_ = k_counts_r - 360.0f/357.0f*k_counts_l;
}

void PIDController::x_controller()  {
  error_prev_x_ = error_x_;
  error_x_ = goal_x_ - counts_x_;
  pwm_x_ = limit_pwm(g_KPX * error_x_ + g_KDX * (error_x_ - error_prev_x_));
}

void PIDController::w_controller()  {
  error_prev_w_ = error_w_;
  error_w_ = goal_w_ - counts_w_;
  pwm_w_ = limit_pwm(g_KPW * error_w_ + g_KDW * (error_w_ - error_prev_w_));
}

void PIDController::update_motor_pwm() {
  const float k_pwm_r = pwm_x_ + pwm_w_;
  const float k_pwm_l = pwm_x_ - pwm_w_;

  if (abs(k_pwm_r) < g_MIN_SPEED && abs(k_pwm_l) < g_MIN_SPEED)
    stop_cnt_++;

  motors.setRightPwm(k_pwm_r);
  motors.setLeftPwm(k_pwm_l);
}

void PIDController::set_goal_x(int counts) { goal_x_ = counts; stop_cnt_ = 0; }

void PIDController::set_goal_w(int counts) { goal_w_ = counts; stop_cnt_ = 0; }

bool PIDController::is_done() const {
  return stop_cnt_ > g_DONE_THRESHOLD;
}

char* PIDController::get_data() {
  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",
	  goal_x_,
	  goal_w_,
	  counts_x_,
	  counts_w_,
	  error_x_,
	  error_w_,
	  pwm_x_,
	  pwm_w_);
  return data_buf_;
}


float PIDController::limit_pwm(float pwm) const {
  if (abs(pwm) > g_MAX_SPEED){
    return (pwm > 0) ? g_MAX_SPEED : -g_MAX_SPEED;
  } else return pwm;
}