File content as of revision 0:8f5db5085df7:

#include "PIDController.hpp"

PIDController::PIDController(Motor* motor, PwmOut* servo, FXAS21002* gyro) {
    this->motor = motor;
    this->servo = servo;
    this->gyro = gyro;

    initializeController();

    target_angle = 0;
    saved_velocity = 0;
    gyro_reference = 0;

    interrupt_time = 0.02;
    Ts = interrupt_time*1000;
}

void PIDController::initializeController() {
    P = INITIAL_P;
    I = INITIAL_I;
    D = INITIAL_D;
    N = INITIAL_N;

    for (int i = 0; i < 2; i++)
	{
		e[i] = 0;
		ui[i] = 0;
		ud[i] = 0;
	}
}

void PIDController::PIDTrigger() {
    printf("PIDCntroller \r\n");
    PIDController::RunPID();
}

void PIDController::PIDAttach() {
    
    pid_trigger.attach(std::bind(&PIDController::PIDTrigger, this), 1.0);
}

void PIDController::PIDDetach() {
    pid_trigger.detach();
}

void PIDController::setInterruptTime(float interrupt_time) {
	this->interrupt_time = interrupt_time;
}

void PIDController::setPIDConstants(float P, float I, float D, float N) {
    this->P = P;
    this->I = I;
    this->D = D;
    this->N = N;
}

void PIDController::setTargetAngle(float target_angle) {
    this->target_angle = target_angle;
    if (target_angle > PI)
        this->target_angle = target_angle - 2 * PI;
    else if (target_angle < -PI)
        this->target_angle = target_angle + 2 * PI;
}

void PIDController::addTargetAngle(float add_angle) {
    this->target_angle = this->target_angle + add_angle;
}

void PIDController::Control() {
	setServoPWM(u*100/(PI/8), *servo);
    if(abs(e[0]) >= ERROR_THRESH && motor->getVelocity() > MINIMUM_CURVE_VELOCITY) {
        saved_velocity = motor->getVelocity();
        motor->setVelocity(MINIMUM_CURVE_VELOCITY);
    }
    else if(abs(e[0]) < ERROR_THRESH && motor->getVelocity() != saved_velocity) {
        motor->setVelocity(saved_velocity);
    }
}
/* PID controller for angular position */
void PIDController::RunPID() {
    float feedback = gyro->get_angle() - gyro_reference;
    	e[0]= e[0] - 2*PI;
    if(e[0] < -PI)
    	e[0] = e[0] + 2*PI;

    /* Proportinal Part */
    up[0] = e[0]*P;  

    /* Integral Part */
    ui[1] = ui[0];
    if(abs(u) < PI/8){
    	ui[0] = (P*I*Ts)*e[1] + ui[1];    
    }
    else if(u > 0)
    	ui[0] = PI/8 - up[0];
    else if(u < 0)
    	ui[0] = -PI/8 - up[0]; 

    /* Derivative Part */
    ud[1] = ud[0];
    ud[0] = P*D*N*(e[0] - e[1]) - ud[1]*(N*Ts -1); 
    
    /** Sum of all parts **/ 
    u = up[0] + ud[0] + ui[0];

    Control();
}

void PIDController::setGyroReference(float gyro_ref) {
	this->gyro_reference = gyro_ref;
}