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