File content as of revision 0:7a97ebb833eb:

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