Revision 3:35deb5c21b33, committed 2018-11-26

Comitter:

rwgriffithv

Date:

Mon Nov 26 23:50:58 2018 +0000

Parent:

2:14f83a308b82

Commit message:

pid controller updated

Changed in this revision

--- a/headers/assignment3.h	Thu Nov 15 17:22:50 2018 +0000
+++ b/headers/assignment3.h	Mon Nov 26 23:50:58 2018 +0000
@@ -5,9 +5,9 @@
 void drive_straight();
 
 // Part B
-void test_turns1();
-void test_turns2();
-void test_turns3();
+void do_turns1();
+void do_turns2();
+void do_turns3();
 
 void move_cells();
 

--- a/headers/motors.h	Thu Nov 15 17:22:50 2018 +0000
+++ b/headers/motors.h	Mon Nov 26 23:50:58 2018 +0000
@@ -1,10 +1,10 @@
 #pragma once
 
-const int PERIOD_US = 10000;
+const int PERIOD_US = 1000;
 
 // Feel free to change these parameters as needed
 const float MIN_SPEED = 0.08f;
-const float MAX_SPEED = 0.3f;
+const float MAX_SPEED = 0.2;
 
 const int RIGHT_MOTOR = 0;
 const int LEFT_MOTOR = 1;

--- a/headers/pid_controller.h	Thu Nov 15 17:22:50 2018 +0000
+++ b/headers/pid_controller.h	Mon Nov 26 23:50:58 2018 +0000
@@ -1,44 +1,52 @@
 #pragma once
-
 #include "globals.h"
 
-const float KpX = 1;
-const float KdX = 0;
+const float g_KPX = 0.01;
+const float g_KDX = 0.03;
+const float g_KPW = 0.1;
+const float g_KDW = 0.1;
 
-const float KpW = 1;
-const float KdW = 0;
+const float g_MAX_SPEED = 0.3;
+const float g_MIN_SPEED = 0.08;
+const float g_DONE_THRESHOLD = 100;
 
 class PIDController {
-public:
-    PIDController();
-    
-    void reset() volatile;
-    void update() volatile;
+ public:
+  PIDController();
+
+  void reset();
+  void update();
+
+  void set_goal_x(int counts);
+  void set_goal_w(int counts);
 
-    void setXGoal(int counts);
-    void setWGoal(int counts);
+  bool is_done() const;
+  char* get_data();
+
+ private:
+  void get_sensor_feedback();
+  void x_controller();
+  void w_controller();
+  void update_motor_pwm();
+  float limit_pwm(float pwm) const;
 
-    bool isDone() volatile;
-    char* getData();
-private:
-    void getSensorFeedback() volatile;
-    void x_controller() volatile;
-    void w_controller() volatile;
-    void updateMotorPwm() volatile;
-    
-    int m_goalW;
-    int m_goalX;
-    
-    float m_pwmW;
-    float m_pwmX;
-    
-    int m_errorW;
-    int m_errorX;
-    int m_errorW_old;
-    int m_errorX_old;
-    
-    int m_countsW;
-    int m_countsX;
-    
-    char buf[200];
+  int goal_x_;
+  int goal_w_;
+
+  float counts_x_;
+  float counts_w_;
+
+  float error_x_;
+  float error_w_;
+
+  float error_prev_x_;
+  float error_prev_w_;
+
+  float pwm_x_;
+  float pwm_w_;
+
+  int stop_cnt_;
+
+  char data_buf_[200];
 };
+

--- a/src/assignment3.cpp	Thu Nov 15 17:22:50 2018 +0000
+++ b/src/assignment3.cpp	Mon Nov 26 23:50:58 2018 +0000
@@ -86,17 +86,27 @@
     systick.start();
 
     // STAGE 3: The real (optional) test.
+    wait(0.25);
     mainController.turn(90);
+    wait(0.25);
     mainController.turn(90);
+    wait(0.25);
     mainController.turn(180);
+    wait(0.25);
 
     mainController.turn(-90);
+    wait(0.25);
     mainController.turn(-90);
+    wait(0.25);
     mainController.turn(-180);
+    wait(0.25);
 
     mainController.turn(90);
+    wait(0.25);
     mainController.turn(90);
+    wait(0.25);
     mainController.turn(-90);
+    wait(0.25);
     mainController.turn(-90);
 }
 

--- a/src/main.cpp	Thu Nov 15 17:22:50 2018 +0000
+++ b/src/main.cpp	Mon Nov 26 23:50:58 2018 +0000
@@ -1,4 +1,4 @@
-#include "assignment2.h"
+#include "assignment3.h"
 
 int main() {
     /***
@@ -24,5 +24,6 @@
     basic_motor_movement();
     assignment2*/
     
-    //drive_straight(); // assignment3 test1
+    drive_straight(); // assignment3 test1
+    do_turns3();
 }

--- a/src/main_controller.cpp	Thu Nov 15 17:22:50 2018 +0000
+++ b/src/main_controller.cpp	Mon Nov 26 23:50:58 2018 +0000
@@ -32,9 +32,10 @@
      * Set an X goal and W goal correctly! This is discussed in the lecture slides.
      * For help, see example code: pid/drive-straight.cpp
      **/
-    m_pid->setXGoal(10);
-    m_pid->setWGoal(0);
+    m_pid->set_goal_x(3600);
+    m_pid->set_goal_w(0);
     pc.printf("Driving straight\n");
+    while(!m_pid->is_done());
 }
 
 /***
@@ -43,10 +44,10 @@
  * Get your mouse to turn, and drive a specific distance.
  ***/
 void MainController::turn(int deg) {
-    /**
-     * For help, see example code: pid/turning.cpp
-     **/
+    m_pid->set_goal_x(0);
+    m_pid->set_goal_w(4.65*deg);
     pc.printf("Turning %d degrees\n", deg);
+    while(!m_pid->is_done());
 }
 
 void MainController::moveCells(float n) {

--- 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);
-}