Diff: src/pid_controller.cpp

Revision:

3:35deb5c21b33

Parent:

1:6f18bb7a77a5

--- a/src/pid_controller.cpp	Thu Nov 15 17:22:50 2018 +0000
+++ b/src/pid_controller.cpp	Mon Nov 26 23:50:58 2018 +0000
@@ -2,137 +2,88 @@
 #include "globals.h"
 
 PIDController::PIDController() {
-    this->reset();
+  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;
 }
 
-/***
- * Assignment 3
- * 
- * Implement PID!
- ***/
 
-void PIDController::reset() volatile {    
-    /**
-     * You'll have a lot to keep track of. Make sure to reset all 
-     * those variables here.
-     **/
-
-    m_goalW = 0;
-    m_goalX = 0;
-
-    m_pwmW = 0;
-    m_pwmX = 0;
-
-    m_errorW = 0;
-    m_errorX = 0;
-    m_errorW_old = 0;
-    m_errorX_old = 0;
-
-    m_countsW = 0;
-    m_countsX = 0;
+void PIDController::update()  {
+  get_sensor_feedback();
+  x_controller();
+  w_controller();
+  update_motor_pwm();
+  if (is_done()) {
+  	reset();
+  }
 }
 
-void PIDController::update() volatile {
-    /**
-     * Update your PID controller. This is what should be called
-     * every systick, so you should call all your important
-     * functions here.
-     **/
-    getSensorFeedback();
-    
-    x_controller();
-    w_controller();
-    
-    updateMotorPwm();
+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::setXGoal(int counts) {
-    /**
-     * Set goal for X controller.
-     **/
-     m_goalX = counts;
-}
-
-void PIDController::setWGoal(int counts) {
-    /**
-     * Set goal for W controller. Make sure to associate a turning
-     * direction with a sign, e.g., positive is clockwise, negative
-     * is counter-clockwise.
-     **/
-     m_goalW = counts;
+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_));
 }
 
-bool PIDController::isDone() volatile {
-    /**
-     * When is the PID done? Well, probably when you've reached
-     * your goals...
-     **/
-     if ( (m_goalW == m_countsW) && (m_goalX == m_countsX) ) {
-        return true;
-    }
-    return false;
+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_));
 }
 
-char* PIDController::getData() {
-    /**
-     * Use this function to print data about your PID,
-     * because you can't print directly in systick!
-     **/
-    sprintf(buf, "goalx: %d\tgoalw: %d\tpwmx: %.3f\tpwmw: %.3f\terrorx: %d\terrorw: %d\n",
-        m_goalX,
-        m_goalW,
-        m_pwmX,
-        m_pwmW,
-        m_errorX,
-        m_errorW);
-    return buf;
+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);
 }
 
-/**
- * Private functions to do the internal work for PID.
- **/
-void PIDController::getSensorFeedback() volatile {
-    /**
-     * Update sensor values, from encoders
-     **/
-     
-     int encL = encoders.getPulsesL();
-     int encR = encoders.getPulsesR();
-     
-     m_countsX = (encL + encR) / 2;
-     m_countsW = encL - encR;
-     
-     // update m_countsX, m_countsW
+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;
 }
 
-void PIDController::x_controller() volatile {
-    /**
-     * Your X PID controller, sets m_pwmX
-     **/
-
-    m_errorX_old = m_errorX;
-    m_errorX = m_goalX - m_countsX;
-//    m_pwmX_raw = (KpX * m_errorX) + (KdX * (m_errorX_old - m_errorX));
-    m_pwmX = 10;
+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_;
 }
 
-void PIDController::w_controller() volatile {
-    /**
-     * Your W PID controller, sets m_pwmW
-     **/
 
-    m_errorW_old = m_errorW;
-    m_errorW = m_goalW - m_countsW;
-    m_pwmW = (KpW * m_errorW) + (KdW * (m_errorW_old - m_errorW));
+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;
 }
 
-void PIDController::updateMotorPwm() volatile {
-    /**
-     * Takes m_pwmX, m_pwmW, and adjusts the motors based on those values.
-     **/
-     
-    // if m_goalW > 0, rat is turning to the right
-    // set pwm based off of X (amplitude) and W (angle)
-    motors.setRightPwm((m_pwmX - m_pwmW)/20.0);
-    motors.setLeftPwm((m_pwmX + m_pwmW)/20.0);
-}