Diff: src/pid_controller.cpp

Revision:

1:6f18bb7a77a5

Child:

3:35deb5c21b33

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/pid_controller.cpp	Thu Nov 15 17:19:20 2018 +0000
@@ -0,0 +1,138 @@
+#include "pid_controller.h"
+#include "globals.h"
+
+PIDController::PIDController() {
+    this->reset();
+}
+
+/***
+ * 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() 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::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;
+}
+
+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;
+}
+
+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;
+}
+
+/**
+ * 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::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;
+}
+
+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));
+}
+
+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);
+}