Tristan Vlogman / errorFetch

Updates error values

Dependents:   locomotion_pid_action_refactor_EMG

errorFetch.cpp

Committer:
tvlogman
Date:
2017-10-22
Revision:
2:d22c458a8a78
Parent:
1:9e2c9237d88b
Child:
3:71a7dd98fb2c

File content as of revision 2:d22c458a8a78:

#include "errorFetch.h"

#include "mbed.h"
#include "QEI.h"
#include "refGen.h"

// Member function definitions
errorFetch::errorFetch(float N, float Ts):gearRatio(N), Ts(Ts), e_pos(0), e_int(0), e_der(0), e_prev(0){

    }
    
void errorFetch::fetchError(int counts, float ref){
    // Calculating motor position based on encoder counts
    double motorPosition = 2*3.14*(counts/(gearRatio*64.0f));
    
    // Computing position error
    e_pos = ref - motorPosition;
    
    // Limiting the integral error to prevent integrator saturation
    if(fabs(e_int) <= 5){
        e_int = e_int + Ts*e_pos;    
        }
    
    // Derivative error   
    e_der = (e_pos - e_prev)/Ts;
    e_prev = e_pos; // Store current position error as we'll need it to compute the next derivative error
    }