File content as of revision 0:40cfee72b03a:

#include "pid.h"

//position
void PositionPid::period(float control_cycle){
        dt = control_cycle;
}

void PositionPid::gain(float kp_, float ki_, float kd_) {
    kp = kp_;
    ki = ki_;
    kd = kd_;
}

void PositionPid::reset(){
    for(int i=NOW;i<=OLD;i++){
        distance[i] = 0;
        difference[i] = 0;
    }
    
    for(int i=P; i<=D; i++){
        result[i] = 0;
    }
}

void PositionPid::cal(float current,float target){
    distance[NOW] = current;
    
    difference[OLD] = difference[NOW];
    difference[NOW] = target - distance[NOW];

    result[P]  = difference[NOW];
    result[I] += (difference[NOW]+difference[OLD])/2 * dt;
    result[D]  = (distance[NOW] - distance[OLD])/dt;
    
    result[OUTPUT] = kp*result[P] + ki*result[I] + kd*result[D];
    
    if(result[OUTPUT] >  pid_constant::UPPER_LIMIT){
         result[OUTPUT] =  pid_constant::UPPER_LIMIT;
    } else if(result[OUTPUT] < pid_constant::LOWER_LIMIT){
         result[OUTPUT] = pid_constant::LOWER_LIMIT;
    }
    
    distance[OLD] = distance[NOW];
}

float PositionPid::getState(int channel){
    float output;
    switch(channel){
        case P:
            output = result[P];
        case I:
            output = result[I];
        case D:
            output = result[D];
        case OUTPUT:
            output = result[OUTPUT];
    }
    return output;
}


//speed
void SpeedPid::cal(float target_speed, float now_speed){
        diffelence[TWO_OLD] = diffelence[OLD];
        diffelence[OLD]     = diffelence[NOW];
        diffelence[NOW]     = target_speed - now_speed;
            
        result[P]  = diffelence[NOW] - diffelence[OLD];
        result[I]  = diffelence[NOW] * dt;
        result[D]  = diffelence[NOW] - diffelence[OLD]*2 + diffelence[TWO_OLD];
            
        result[OUTPUT] += (result[P]*gain[P] + result[I]*gain[I] + result[D]*gain[D]);
            
        if(result[OUTPUT] >  1.0f){ 
                result[OUTPUT] =  1.0f;
            } else if(result[OUTPUT] < -1.0f){
                result[OUTPUT] = -1.0f;
            }
}
            
float SpeedPid::getState(int channel){
        float output;
        switch(channel){
            case P:
                output = result[P];
            case I:
                output = result[I];
            case D:
                output = result[D];
            case OUTPUT:
                output = result[OUTPUT];
        }
        return output;
}
        
void SpeedPid::init(float kp_, float ki_, float kd_, float dt_){
        gain[P] = kp_;
        gain[I] = ki_;
        gain[D] = kd_;
        dt = dt_;
}