Diff: pid_controller.cpp

Revision:

0:7a97ebb833eb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pid_controller.cpp	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,112 @@
+#include "pid_controller.h"
+#include "globals.h"
+
+PIDController::PIDController() {
+    this->reset();
+}
+
+void PIDController::reset() volatile {    
+    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;
+    
+    m_doneCounter = 0;
+    
+    encoders.reset();
+}
+
+void PIDController::update() volatile {
+     getSensorFeedback();
+     x_controller();
+     w_controller();
+     updateMotorPwm();
+}
+
+void PIDController::setXGoal(int counts) {
+     m_goalX = counts;
+}
+
+void PIDController::setWGoal(int counts) {
+     m_goalW = counts;
+}
+
+bool PIDController::isDone() volatile {
+    return m_doneCounter > done_limit;
+}
+
+char* PIDController::getData() {
+    sprintf(buf, "goalx: %d\tgoalw: %d\tpwmr: %.3f\tpwml: %.3f\tpwmx: %.3f\tpwmw: %.3f\terrorx: %d\terrorw: %d\n",
+        m_goalX,
+        m_goalW,
+        m_pwmX + m_pwmW,
+        m_pwmX - m_pwmW,
+        m_pwmX,
+        m_pwmW,
+        m_errorX,
+        m_errorW);
+    return buf;
+}
+
+/**
+ * Private functions to do the internal work for PID.
+ **/
+void PIDController::getSensorFeedback() volatile {
+     int right, left;
+     right = encoders.right();
+     left = encoders.left();
+     
+     m_countsW = right - left;
+     m_countsX = (right + left) / 2;
+}
+
+void PIDController::x_controller() volatile {
+     m_errorX = m_goalX - m_countsX;
+     m_pwmX = KpX * m_errorX + KdX * (m_errorX - m_errorX_old);
+     m_errorX_old = m_errorX;
+}
+
+void PIDController::w_controller() volatile {
+     m_errorW = m_goalW - m_countsW;
+     m_pwmW = KpW * m_errorW + KdW * (m_errorW - m_errorW_old);
+     m_errorW_old = m_errorW;
+}
+
+float limit(float val, float min, float max) {
+    if (abs(val) > max) {
+        if (val > 0) val = max;
+        else if (val < 0) val = -max;
+    }
+    else if (abs(val) < min) {
+        if (val > 0) val = min;
+        else if (val < 0) val = -min;
+    }
+    
+    return val;
+}
+
+void PIDController::updateMotorPwm() volatile {
+     m_pwmX = limit(m_pwmX, 0, max_speed);
+     m_pwmW = limit(m_pwmW, 0, max_speed);
+     
+     float right = m_pwmX + m_pwmW;
+     float left = m_pwmX - m_pwmW;
+     
+     if (abs(right) < min_speed && abs(left) < min_speed) {
+         m_doneCounter += 1;
+     }
+     else { 
+        motors.setRightPwm(m_pwmX + m_pwmW);
+        motors.setLeftPwm(m_pwmX - m_pwmW);
+     }
+}
+