control.c 3.24 KB
Newer Older
Quirin Apfel's avatar
Quirin Apfel committed
1
#include "control.h"
2
#include <math.h>
Quirin Apfel's avatar
Quirin Apfel committed
3
#include "ezs_io.h"
Quirin Apfel's avatar
blub  
Quirin Apfel committed
4

5
6
7
//defines
#define OUT_MAX 100 //maximum output in percent
#define OUT_MIN -100
Quirin Apfel's avatar
blub  
Quirin Apfel committed
8

Quirin Apfel's avatar
Quirin Apfel committed
9
#ifndef F_CONTROL
10
#define F_CONTROL 200 //noch festzulegen
Quirin Apfel's avatar
Quirin Apfel committed
11
#endif
12
13
14
15
16
#define INITIAL_OFFSET 2.8

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

Quirin Apfel's avatar
Quirin Apfel committed
17
18
19
20
21
22
23
/*
 * Implements two-point-controller.
 * Outputs either OUT_MAX, OUT_MIN depending on current error,
 * using hysteresis.
 */
float two_point_control(float e)
{
Quirin Apfel's avatar
Quirin Apfel committed
24
    float hysteresis = 1;
Quirin Apfel's avatar
Quirin Apfel committed
25
    static float new_out = 0;
Quirin Apfel's avatar
Quirin Apfel committed
26
27
    
    if(e < -hysteresis){
Quirin Apfel's avatar
Quirin Apfel committed
28
        new_out = OUT_MIN;
Quirin Apfel's avatar
Quirin Apfel committed
29
    }else if(e > hysteresis){
Quirin Apfel's avatar
Quirin Apfel committed
30
31
        new_out = OUT_MAX;
    }
Quirin Apfel's avatar
Quirin Apfel committed
32
33
    float a = OUT_MAX;
    ezs_printf("out_max %f",a);
Quirin Apfel's avatar
Quirin Apfel committed
34
35
36
    return new_out;
}

37
38
/*
 * Implements PI-controller.
Quirin Apfel's avatar
Quirin Apfel committed
39
40
 * Feeds back correction for system input depending on system error e,
 * consisting of proportional and integral component.
41
42
 * Limiting integral when maximal output is reached to avoid integrator wind-up.
 */
Quirin Apfel's avatar
Quirin Apfel committed
43
float pi_control(float e)
Quirin Apfel's avatar
blub  
Quirin Apfel committed
44
{
45
46
    const float K = 20.0f;
    const float T_i = 0.18f;
Quirin Apfel's avatar
blub  
Quirin Apfel committed
47

48
    sum_error += e/F_CONTROL;
Quirin Apfel's avatar
blub  
Quirin Apfel committed
49

50
    float new_out = K*(e + sum_error/T_i);
Quirin Apfel's avatar
blub  
Quirin Apfel committed
51
52
53

    if(new_out>OUT_MAX){
        new_out = OUT_MAX;
54
        sum_error -= e;
Quirin Apfel's avatar
blub  
Quirin Apfel committed
55
56
    }else if(new_out<OUT_MIN){
        new_out = OUT_MIN;
57
        sum_error -= e;
Quirin Apfel's avatar
blub  
Quirin Apfel committed
58
    } 
Quirin Apfel's avatar
Quirin Apfel committed
59
   
Quirin Apfel's avatar
blub  
Quirin Apfel committed
60
61
62
    return new_out;
}

63
/*
Quirin Apfel's avatar
Quirin Apfel committed
64
65
66
67
 * 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.
68
 */
Quirin Apfel's avatar
Quirin Apfel committed
69
float pid_control(float e)
Quirin Apfel's avatar
blub  
Quirin Apfel committed
70
{
71
72
73
    const float K = 20.0f;
    const float T_i = 0.18f;
    const float T_d = 0.00;
Quirin Apfel's avatar
Quirin Apfel committed
74

75

Quirin Apfel's avatar
Quirin Apfel committed
76
77
    static float e_old = 0;

78
    sum_error += e/F_CONTROL;
Quirin Apfel's avatar
Quirin Apfel committed
79
    float derivative = (e - e_old)*F_CONTROL;
Quirin Apfel's avatar
Quirin Apfel committed
80

81
    float new_out = K*(e + sum_error/T_i + T_d*derivative);
Quirin Apfel's avatar
Quirin Apfel committed
82
83

    if(new_out>OUT_MAX){
84
        new_out = OUT_MAX;
85
        sum_error -= e/F_CONTROL;
Quirin Apfel's avatar
Quirin Apfel committed
86
87
    }else if(new_out<OUT_MIN){
        new_out = OUT_MIN;
88
        sum_error -= e/F_CONTROL;
Quirin Apfel's avatar
Quirin Apfel committed
89
90
    } 

91
    return new_out;
Quirin Apfel's avatar
blub  
Quirin Apfel committed
92
93
}

Stefan's avatar
Stefan committed
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
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;
}

Quirin Apfel's avatar
Quirin Apfel committed
112
113
114
115
116
117
/*
 * Implements Offset-controller.
 * Feeds back correction control_output to correct offset.
 */
float offset_control(float motor_signal)
{
Quirin Apfel's avatar
Quirin Apfel committed
118
119
120
    const float K = 0.001f;//0.001
    float max_offset = 6.0;
    float min_offset = 0.0;
Quirin Apfel's avatar
blub  
Quirin Apfel committed
121

122
123
    // sum_offset += K*motor_signal/F_CONTROL;

Stefan's avatar
Stefan committed
124
125
126
    float new_offset = iir_filter(K*motor_signal);
    // sum_offset = sum_offset + iir_filter(K*motor_signal); 
    // float new_offset = sum_offset
Quirin Apfel's avatar
Quirin Apfel committed
127
128
    if(new_offset > max_offset){
        new_offset = max_offset;
129
        sum_offset -= motor_signal/F_CONTROL;
Florian Zapf's avatar
Florian Zapf committed
130
131
    }else if(new_offset < min_offset){
        new_offset = min_offset;
132
        sum_offset -= motor_signal/F_CONTROL;
Quirin Apfel's avatar
Quirin Apfel committed
133
    }
Quirin Apfel's avatar
Quirin Apfel committed
134

Quirin Apfel's avatar
Quirin Apfel committed
135
136
    return new_offset;
}
Quirin Apfel's avatar
Quirin Apfel committed
137

138
139
140
141
142
143
144
/*
 * Resets integrators in controllers.
 */
void reset_control(void){
    sum_error = 0;
    sum_offset = INITIAL_OFFSET; 
}