Skip to content

Commit a4f9e91

Browse files
sogwmssogwms
committed
add controller(algorithm) && rename
1 parent 8dde45d commit a4f9e91

File tree

14 files changed

+463
-21
lines changed

14 files changed

+463
-21
lines changed

‎controller/controller.c‎

Lines changed: 90 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,90 @@
1+
#include "controller.h"
2+
3+
#define DBG_SECTION_NAME "controller"
4+
#define DBG_LEVEL DBG_LOG
5+
#include <rtdbg.h>
6+
7+
// single intput and single output system
8+
9+
controller_t controller_create(rt_size_t size)
10+
{
11+
// TODO
12+
// Malloc memory and initialize PID
13+
controller_t new_controller = (controller_t)rt_malloc(size);
14+
if(new_controller == RT_NULL)
15+
{
16+
LOG_E("Failed to malloc memory for automatic controller\n");
17+
return RT_NULL;
18+
}
19+
20+
new_controller->enable = RT_FALSE;
21+
22+
return new_controller;
23+
}
24+
25+
rt_err_t controller_update(controller_t controller, float current_point)
26+
{
27+
RT_ASSERT(controller != RT_NULL);
28+
29+
if (controller->enable)
30+
{
31+
return controller->update(controller, current_point);
32+
}
33+
else
34+
{
35+
controller->output = controller->target;
36+
}
37+
38+
return RT_EOK;
39+
}
40+
41+
rt_err_t controller_destroy(controller_t controller)
42+
{
43+
RT_ASSERT(controller != RT_NULL);
44+
45+
return controller->destroy(controller);
46+
}
47+
48+
rt_err_t controller_enable(controller_t controller)
49+
{
50+
// TODO
51+
RT_ASSERT(controller != RT_NULL);
52+
53+
controller->enable = RT_TRUE;
54+
55+
return RT_EOK;
56+
}
57+
58+
rt_err_t controller_disable(controller_t controller)
59+
{
60+
// TODO
61+
RT_ASSERT(controller != RT_NULL);
62+
63+
controller->enable = RT_FALSE;
64+
65+
return RT_EOK;
66+
}
67+
68+
rt_err_t controller_reset(controller_t controller)
69+
{
70+
// TODO
71+
RT_ASSERT(controller != RT_NULL);
72+
73+
return controller->reset(controller);
74+
}
75+
76+
rt_err_t controller_set_target(controller_t controller, rt_int16_t target)
77+
{
78+
RT_ASSERT(controller != RT_NULL);
79+
80+
controller->target = target;
81+
return RT_EOK;
82+
}
83+
84+
rt_err_t controller_set_sample_time(controller_t controller, rt_uint16_t sample_time)
85+
{
86+
RT_ASSERT(controller != RT_NULL);
87+
88+
controller->sample_time = sample_time;
89+
return RT_EOK;
90+
}

‎controller/controller.h‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
#ifndef __CONTROLLER_H__
2+
#define __CONTROLLER_H__
3+
4+
#include <rtthread.h>
5+
6+
struct controller
7+
{
8+
float target;
9+
float output;
10+
rt_uint16_t sample_time; // unit:ms
11+
int enable;
12+
13+
rt_err_t (*update)(void *controller, float current_point);
14+
rt_err_t (*reset)(void *controller);
15+
rt_err_t (*destroy)(void *controller);
16+
};
17+
18+
typedef struct controller *controller_t;
19+
20+
controller_t controller_create(rt_size_t size);
21+
rt_err_t controller_update(controller_t controller, float current_point);
22+
rt_err_t controller_destroy(controller_t controller);
23+
rt_err_t controller_enable(controller_t controller);
24+
rt_err_t controller_disable(controller_t controller);
25+
rt_err_t controller_reset(controller_t controller);
26+
rt_err_t controller_set_target(controller_t controller, rt_int16_t target);
27+
rt_err_t controller_set_sample_time(controller_t controller, rt_uint16_t sample_time);
28+
29+
#endif // __CONTROLLER_H__

‎controller/inc_pid_controller.c‎

Lines changed: 117 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,117 @@
1+
#include "inc_pid_controller.h"
2+
3+
#define DBG_SECTION_NAME "inc_pid_controller"
4+
#define DBG_LEVEL DBG_LOG
5+
#include <rtdbg.h>
6+
7+
static rt_err_t inc_pid_controller_reset(void *pid)
8+
{
9+
rt_memset(pid, 0, sizeof(struct inc_pid_controller));
10+
return RT_EOK;
11+
}
12+
13+
static rt_err_t inc_pid_controller_destroy(void *pid)
14+
{
15+
rt_free(pid);
16+
return RT_EOK;
17+
}
18+
19+
static rt_err_t inc_pid_controller_update(void *pid, float current_point)
20+
{
21+
inc_pid_controller_t inc_pid = (inc_pid_controller_t)pid;
22+
// TODO
23+
if((rt_tick_get() - inc_pid->last_time) < rt_tick_from_millisecond(inc_pid->controller.sample_time))
24+
{
25+
LOG_D("PID waiting ... ");
26+
return RT_EBUSY;
27+
}
28+
inc_pid->last_time = rt_tick_get();
29+
30+
inc_pid->error = inc_pid->controller.target - current_point;
31+
32+
inc_pid->p_error = inc_pid->kp * (inc_pid->error - inc_pid->error_l);
33+
inc_pid->i_error = inc_pid->ki * inc_pid->error;
34+
inc_pid->d_error = inc_pid->kd * (inc_pid->error - 2 * inc_pid->error_l + inc_pid->error_ll);
35+
36+
inc_pid->last_out += inc_pid->p_error + inc_pid->i_error + inc_pid->d_error;
37+
38+
if (inc_pid->last_out > inc_pid->maximum)
39+
{
40+
inc_pid->last_out = inc_pid->maximum;
41+
}
42+
if (inc_pid->last_out < inc_pid->minimum)
43+
{
44+
inc_pid->last_out = inc_pid->minimum;
45+
}
46+
47+
inc_pid->error_ll = inc_pid->error_l;
48+
inc_pid->error_l = inc_pid->error;
49+
50+
inc_pid->controller.output = inc_pid->last_out;
51+
52+
// rt_kprintf("%d - %d\n", current_point, pid->set_point);
53+
// LOG_D("PID current: %d : setpoint %d - P%d I%d D%d - [%d]", current_point, pid->set_point, (int)(pid->p_error + 0.5f), (int)(pid->i_error + 0.5f), (int)(pid->d_error + 0.5f), (int)(pid->out + 0.5f));
54+
// LOG_D("PID P Error: %d", (int)(pid->p_error + 0.5f));
55+
// LOG_D("PID I Error: %d", (int)(pid->i_error + 0.5f));
56+
// LOG_D("PID D Error: %d", (int)(pid->d_error + 0.5f));
57+
// LOG_D("PID Last Out: %d", (int)(pid->last_out + 0.5f));
58+
// LOG_D("PID Out: %d", (int)(pid->out + 0.5f));
59+
60+
return RT_EOK;
61+
}
62+
63+
inc_pid_controller_t inc_pid_controller_create(float kp, float ki, float kd)
64+
{
65+
inc_pid_controller_t new_pid = (inc_pid_controller_t)controller_create(sizeof(struct inc_pid_controller));
66+
if(new_pid == RT_NULL)
67+
{
68+
return RT_NULL;
69+
}
70+
71+
new_pid->kp = kp;
72+
new_pid->ki = ki;
73+
new_pid->kd = kd;
74+
75+
new_pid->maximum = +1000;
76+
new_pid->minimum = -1000;
77+
78+
new_pid->p_error = 0.0f;
79+
new_pid->i_error = 0.0f;
80+
new_pid->d_error = 0.0f;
81+
82+
new_pid->error = 0.0f;
83+
new_pid->error_l = 0.0f;
84+
new_pid->error_ll = 0.0f;
85+
86+
new_pid->last_out = 0.0f;
87+
88+
new_pid->controller.reset = inc_pid_controller_reset;
89+
new_pid->controller.destroy = inc_pid_controller_destroy;
90+
new_pid->controller.update = inc_pid_controller_update;
91+
92+
return new_pid;
93+
}
94+
95+
rt_err_t inc_pid_controller_set_kp(inc_pid_controller_t pid, float kp)
96+
{
97+
RT_ASSERT(pid != RT_NULL);
98+
99+
pid->kp = kp;
100+
return RT_EOK;
101+
}
102+
103+
rt_err_t inc_pid_controller_set_ki(inc_pid_controller_t pid, float ki)
104+
{
105+
RT_ASSERT(pid != RT_NULL);
106+
107+
pid->ki = ki;
108+
return RT_EOK;
109+
}
110+
111+
rt_err_t inc_pid_controller_set_kd(inc_pid_controller_t pid, float kd)
112+
{
113+
RT_ASSERT(pid != RT_NULL);
114+
115+
pid->kd = kd;
116+
return RT_EOK;
117+
}

‎controller/inc_pid_controller.h‎

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
#ifndef __INC_PID_CONTROLLER_H__
2+
#define __INC_PID_CONTROLLER_H__
3+
4+
#include <rtthread.h>
5+
#include "controller.h"
6+
7+
typedef struct inc_pid_controller *inc_pid_controller_t;
8+
9+
struct inc_pid_controller
10+
{
11+
struct controller controller;
12+
13+
float kp;
14+
float ki;
15+
float kd;
16+
17+
float minimum;
18+
float maximum;
19+
20+
float p_error;
21+
float i_error;
22+
float d_error;
23+
24+
float error;
25+
float error_l;
26+
float error_ll;
27+
28+
float last_out;
29+
rt_tick_t last_time;
30+
};
31+
32+
inc_pid_controller_t inc_pid_controller_create(float kp, float ki, float kd);
33+
rt_err_t inc_pid_controller_set_kp(inc_pid_controller_t pid, float kp);
34+
rt_err_t inc_pid_controller_set_ki(inc_pid_controller_t pid, float ki);
35+
rt_err_t inc_pid_controller_set_kd(inc_pid_controller_t pid, float kd);
36+
37+
#endif

‎controller/pos_pid_controller.c‎

Lines changed: 120 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,120 @@
1+
#include "pos_pid_controller.h"
2+
3+
#define DBG_SECTION_NAME "pos_pid_controller"
4+
#define DBG_LEVEL DBG_LOG
5+
#include <rtdbg.h>
6+
7+
static rt_err_t pos_pid_controller_reset(void *pid)
8+
{
9+
rt_memset(pid, 0, sizeof(struct pos_pid_controller));
10+
return RT_EOK;
11+
}
12+
13+
static rt_err_t pos_pid_controller_destroy(void *pid)
14+
{
15+
rt_free(pid);
16+
return RT_EOK;
17+
}
18+
19+
static rt_err_t pos_pid_controller_update(void *pid, float current_point)
20+
{
21+
pos_pid_controller_t pos_pid = (pos_pid_controller_t)pid;
22+
23+
if((rt_tick_get() - pos_pid->last_time) < rt_tick_from_millisecond(pos_pid->controller.sample_time))
24+
{
25+
LOG_D("PID waiting ... ");
26+
return RT_EBUSY;
27+
}
28+
pos_pid->last_time = rt_tick_get();
29+
30+
pos_pid->error = pos_pid->controller.target - current_point;
31+
32+
pos_pid->integral += pos_pid->error;
33+
34+
//Perform integral value capping to avoid internal PID state to blows up
35+
//when controllertuators saturate:
36+
if(pos_pid->integral > pos_pid->anti_windup_value) {
37+
pos_pid->integral = pos_pid->anti_windup_value;
38+
} else if (pos_pid->integral < -pos_pid->anti_windup_value) {
39+
pos_pid->integral = -pos_pid->anti_windup_value;
40+
}
41+
42+
pos_pid->p_error = pos_pid->kp * pos_pid->error;
43+
pos_pid->i_error = pos_pid->ki * pos_pid->integral;
44+
pos_pid->d_error = pos_pid->kd * (pos_pid->error - pos_pid->error_l);
45+
46+
pos_pid->last_out = pos_pid->p_error + pos_pid->i_error + pos_pid->d_error;
47+
if (pos_pid->last_out > pos_pid->maximum)
48+
{
49+
pos_pid->last_out = pos_pid->maximum;
50+
}
51+
if (pos_pid->last_out < pos_pid->minimum)
52+
{
53+
pos_pid->last_out = pos_pid->minimum;
54+
}
55+
56+
pos_pid->error_l = pos_pid->error;
57+
58+
pos_pid->controller.output = pos_pid->last_out;
59+
60+
// rt_kprintf("%d - %d\n", current_point, pid->set_point);
61+
// LOG_D("PID current: %d : setpoint %d - P%d I%d D%d - [%d]", current_point, pid->set_point, (int)(pid->p_error + 0.5f), (int)(pid->i_error + 0.5f), (int)(pid->d_error + 0.5f), (int)(pid->out + 0.5f));
62+
// LOG_D("PID P Error: %d", (int)(pid->p_error + 0.5f));
63+
// LOG_D("PID I Error: %d", (int)(pid->i_error + 0.5f));
64+
// LOG_D("PID D Error: %d", (int)(pid->d_error + 0.5f));
65+
// LOG_D("PID Last Out: %d", (int)(pid->last_out + 0.5f));
66+
// LOG_D("PID Out: %d", (int)(pid->out + 0.5f));
67+
68+
return RT_EOK;
69+
}
70+
71+
pos_pid_controller_t pos_pid_controller_create(float kp, float ki, float kd)
72+
{
73+
pos_pid_controller_t new_pid = (pos_pid_controller_t)controller_create(sizeof(struct pos_pid_controller));
74+
if(new_pid == RT_NULL)
75+
{
76+
return RT_NULL;
77+
}
78+
79+
new_pid->kp = kp;
80+
new_pid->ki = ki;
81+
new_pid->kd = kd;
82+
83+
new_pid->maximum = +1000;
84+
new_pid->minimum = -1000;
85+
new_pid->anti_windup_value = 0.0f;
86+
87+
new_pid->integral = 0.0f;
88+
new_pid->p_error = 0.0f;
89+
new_pid->i_error = 0.0f;
90+
new_pid->d_error = 0.0f;
91+
92+
new_pid->error = 0.0f;
93+
new_pid->error_l = 0.0f;
94+
95+
new_pid->last_out = 0.0f;
96+
97+
new_pid->controller.reset = pos_pid_controller_reset;
98+
new_pid->controller.destroy = pos_pid_controller_destroy;
99+
new_pid->controller.update = pos_pid_controller_update;
100+
101+
return new_pid;
102+
}
103+
104+
rt_err_t pos_pid_controller_set_kp(pos_pid_controller_t pid, float kp)
105+
{
106+
pid->kp = kp;
107+
return RT_EOK;
108+
}
109+
110+
rt_err_t pos_pid_controller_set_ki(pos_pid_controller_t pid, float ki)
111+
{
112+
pid->ki = ki;
113+
return RT_EOK;
114+
}
115+
116+
rt_err_t pos_pid_controller_set_kd(pos_pid_controller_t pid, float kd)
117+
{
118+
pid->kd = kd;
119+
return RT_EOK;
120+
}

0 commit comments

Comments
 (0)