control.c

#include "control.h"
#include <math.h>
#include "ezs_io.h"

//defines
#define OUT_MAX 100 //maximum output in percent
#define OUT_MIN -100

#ifndef F_CONTROL
#define F_CONTROL 200 //noch festzulegen
#endif
#define INITIAL_OFFSET 2.8

static float sum_error = 0;
static float sum_offset = INITIAL_OFFSET;

/*
 * Implements two-point-controller.
 * Outputs either OUT_MAX, OUT_MIN depending on current error,
 * using hysteresis.
 */
float two_point_control(float e)
{
    float hysteresis = 1;
    static float new_out = 0;
    
    if(e < -hysteresis){
        new_out = OUT_MIN;
    }else if(e > hysteresis){
        new_out = OUT_MAX;
    }
    float a = OUT_MAX;
    ezs_printf("out_max %f",a);
    return new_out;
}

/*
 * Implements PI-controller.
 * Feeds back correction for system input depending on system error e,
 * consisting of proportional and integral component.
 * Limiting integral when maximal output is reached to avoid integrator wind-up.
 */
float pi_control(float e)
{
    const float K = 20.0f;
    const float T_i = 0.18f;

    sum_error += e/F_CONTROL;

    float new_out = K*(e + sum_error/T_i);

    if(new_out>OUT_MAX){
        new_out = OUT_MAX;
        sum_error -= e;
    }else if(new_out<OUT_MIN){
        new_out = OUT_MIN;
        sum_error -= e;
    } 
   
    return new_out;
}

/*
 * Implements PID-controller.
 * Feeds back correction for system input depending on system error e,
 * consisting of proportional, integral and derivative component.
 * Limiting integral when maximal output is reached to avoid integrator wind-up.
 */
float pid_control(float e)
{
    const float K = 20.0f;
    const float T_i = 0.18f;
    const float T_d = 0.00;


    static float e_old = 0;

    sum_error += e/F_CONTROL;
    float derivative = (e - e_old)*F_CONTROL;

    float new_out = K*(e + sum_error/T_i + T_d*derivative);

    if(new_out>OUT_MAX){
        new_out = OUT_MAX;
        sum_error -= e/F_CONTROL;
    }else if(new_out<OUT_MIN){
        new_out = OUT_MIN;
        sum_error -= e/F_CONTROL;
    } 

    return new_out;
}

static float iir_filter(float phi_acc)
{
    static float in_1 = 0;
    static float in_2 = 0;
    static float out_1 = 0;
    static float out_2 = 0;
    const float a[2] = {-1.9112,0.915};
    const float b[3] = {9.4469e-4,0.0019,9.4469e-4};

    float new_out = b[0]*phi_acc + b[1]*in_1 + b[2]*in_2- a[0]*out_1 - a[1]*out_2;

    out_2 = out_1;
    out_1 = new_out;
    in_2 = in_1;
    in_1 = phi_acc;    
    return new_out;
}

/*
 * Implements Offset-controller.
 * Feeds back correction control_output to correct offset.
 */
float offset_control(float motor_signal)
{
    const float K = 0.001f;//0.001
    float max_offset = 6.0;
    float min_offset = 0.0;

    // sum_offset += K*motor_signal/F_CONTROL;

    float new_offset = iir_filter(K*motor_signal);
    // sum_offset = sum_offset + iir_filter(K*motor_signal); 
    // float new_offset = sum_offset
    if(new_offset > max_offset){
        new_offset = max_offset;
        sum_offset -= motor_signal/F_CONTROL;
    }else if(new_offset < min_offset){
        new_offset = min_offset;
        sum_offset -= motor_signal/F_CONTROL;
    }

    return new_offset;
}

/*
 * Resets integrators in controllers.
 */
void reset_control(void){
    sum_error = 0;
    sum_offset = INITIAL_OFFSET; 
}