Skip to content

Commit 0e56f41

Browse files
sogwmssogwms
committed
add ASSERT && update some log && update command and pos_pid and reset-function
1 parent 5c90cf7 commit 0e56f41

29 files changed

+314
-221
lines changed

‎chassis/chassis.c‎

Lines changed: 61 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -5,10 +5,6 @@
55
#define DBG_LEVEL DBG_LOG
66
#include <rtdbg.h>
77

8-
#define DEFAULT_VELOCITY_LINEAR_X 0.2
9-
#define DEFAULT_VELOCITY_LINEAR_Y 0.2
10-
#define DEFAULT_VELOCITY_ANGULAR_Z 1
11-
128
chassis_t chassis_create(wheel_t* c_wheels, kinematics_t c_kinematics)
139
{
1410
// Malloc memory for new chassis
@@ -25,8 +21,10 @@ chassis_t chassis_create(wheel_t* c_wheels, kinematics_t c_kinematics)
2521
return new_chassis;
2622
}
2723

28-
void chassis_destroy(chassis_t chas)
24+
rt_err_t chassis_destroy(chassis_t chas)
2925
{
26+
RT_ASSERT(chas != RT_NULL);
27+
3028
for(int i = 0; i < chas->c_kinematics->total_wheels; i++)
3129
{
3230
LOG_I("Free wheel %d", i);
@@ -35,10 +33,14 @@ void chassis_destroy(chassis_t chas)
3533
kinematics_destroy(chas->c_kinematics);
3634

3735
rt_free(chas);
36+
37+
return RT_EOK;
3838
}
3939

4040
rt_err_t chassis_enable(chassis_t chas)
4141
{
42+
RT_ASSERT(chas != RT_NULL);
43+
4244
for(int i = 0; i < chas->c_kinematics->total_wheels; i++)
4345
{
4446
LOG_I("Enabling wheel %d", i);
@@ -50,20 +52,35 @@ rt_err_t chassis_enable(chassis_t chas)
5052

5153
rt_err_t chassis_disable(chassis_t chas)
5254
{
53-
// TODO
55+
RT_ASSERT(chas != RT_NULL);
56+
57+
for(int i = 0; i < chas->c_kinematics->total_wheels; i++)
58+
{
59+
LOG_I("Disabling wheel %d", i);
60+
wheel_disable(chas->c_wheels[i]);
61+
}
5462

5563
return RT_EOK;
5664
}
5765

5866
rt_err_t chassis_reset(chassis_t chas)
5967
{
60-
// TODO
68+
RT_ASSERT(chas != RT_NULL);
69+
70+
for(int i = 0; i < chas->c_kinematics->total_wheels; i++)
71+
{
72+
LOG_I("Reset wheel %d", i);
73+
wheel_reset(chas->c_wheels[i]);
74+
}
75+
kinematics_reset(chas->c_kinematics);
6176

6277
return RT_EOK;
6378
}
6479

6580
rt_err_t chassis_set_velocity(chassis_t chas, struct velocity target_velocity)
6681
{
82+
RT_ASSERT(chas != RT_NULL);
83+
6784
rt_int16_t* res_rpm = kinematics_get_rpm(*chas->c_kinematics, target_velocity);
6885
chassis_set_rpm(chas, res_rpm);
6986

@@ -72,24 +89,22 @@ rt_err_t chassis_set_velocity(chassis_t chas, struct velocity target_velocity)
7289

7390
rt_err_t chassis_set_rpm(chassis_t chas, rt_int16_t target_rpm[])
7491
{
92+
RT_ASSERT(chas != RT_NULL);
93+
7594
// Set new speed
7695
for(int i = 0; i < chas->c_kinematics->total_wheels; i++)
7796
{
7897
LOG_I("Set wheel %d speed %d rpm", i, target_rpm[i]);
7998
wheel_set_rpm(chas->c_wheels[i], target_rpm[i]);
8099
}
81100

82-
for(int i = 0; i < chas->c_kinematics->total_wheels; i++)
83-
{
84-
wheel_update(chas->c_wheels[i]);
85-
}
86-
87101
return RT_EOK;
88102
}
89103

90104
rt_err_t chassis_update(chassis_t chas)
91105
{
92-
// TODO
106+
RT_ASSERT(chas != RT_NULL);
107+
93108
for(int i = 0; i < chas->c_kinematics->total_wheels; i++)
94109
{
95110
wheel_update(chas->c_wheels[i]);
@@ -99,6 +114,8 @@ rt_err_t chassis_update(chassis_t chas)
99114

100115
rt_err_t chassis_straight(chassis_t chas, float linear_x)
101116
{
117+
RT_ASSERT(chas != RT_NULL);
118+
102119
struct velocity target_velocity = {
103120
.linear_x = linear_x,
104121
.linear_y = 0.0f,
@@ -110,6 +127,8 @@ rt_err_t chassis_straight(chassis_t chas, float linear_x)
110127

111128
rt_err_t chassis_move(chassis_t chas, float linear_y)
112129
{
130+
RT_ASSERT(chas != RT_NULL);
131+
113132
struct velocity target_velocity = {
114133
.linear_x = 0.0f,
115134
.linear_y = linear_y,
@@ -121,6 +140,8 @@ rt_err_t chassis_move(chassis_t chas, float linear_y)
121140

122141
rt_err_t chassis_rotate(chassis_t chas, float angular_z)
123142
{
143+
RT_ASSERT(chas != RT_NULL);
144+
124145
struct velocity target_velocity = {
125146
.linear_x = 0.0f,
126147
.linear_y = 0.0f,
@@ -129,3 +150,30 @@ rt_err_t chassis_rotate(chassis_t chas, float angular_z)
129150
rt_int16_t* res_rpm = kinematics_get_rpm(*chas->c_kinematics, target_velocity);
130151
return chassis_set_rpm(chas, res_rpm);
131152
}
153+
154+
rt_err_t chassis_set_velocity_x(chassis_t chas, float linear_x)
155+
{
156+
RT_ASSERT(chas != RT_NULL);
157+
158+
chas->c_velocity.linear_x = linear_x;
159+
rt_int16_t* res_rpm = kinematics_get_rpm(*chas->c_kinematics, chas->c_velocity);
160+
return chassis_set_rpm(chas, res_rpm);
161+
}
162+
163+
rt_err_t chassis_set_velocity_y(chassis_t chas, float linear_y)
164+
{
165+
RT_ASSERT(chas != RT_NULL);
166+
167+
chas->c_velocity.linear_y = linear_y;
168+
rt_int16_t* res_rpm = kinematics_get_rpm(*chas->c_kinematics, chas->c_velocity);
169+
return chassis_set_rpm(chas, res_rpm);
170+
}
171+
172+
rt_err_t chassis_set_velocity_z(chassis_t chas, float angular_z)
173+
{
174+
RT_ASSERT(chas != RT_NULL);
175+
176+
chas->c_velocity.angular_z = angular_z;
177+
rt_int16_t* res_rpm = kinematics_get_rpm(*chas->c_kinematics, chas->c_velocity);
178+
return chassis_set_rpm(chas, res_rpm);
179+
}

‎chassis/chassis.h‎

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -4,8 +4,8 @@
44
#include <kinematics.h>
55
#include <wheel.h>
66

7-
#define CHASSIS_VELOCITY_LINEAR_MAXIMUM 1.0f
8-
#define CHASSIS_VELOCITY_ANGULAR_MAXIMUM 5.0f
7+
#define CHASSIS_VELOCITY_LINEAR_MAXIMUM 0.1f // m/s
8+
#define CHASSIS_VELOCITY_ANGULAR_MAXIMUM 5.0f * CHASSIS_VELOCITY_LINEAR_MAXIMUM // rad/s
99

1010
typedef struct chassis *chassis_t;
1111

@@ -17,7 +17,7 @@ struct chassis
1717
};
1818

1919
chassis_t chassis_create(wheel_t* c_wheel, kinematics_t c_kinematics);
20-
void chassis_destroy(chassis_t chas);
20+
rt_err_t chassis_destroy(chassis_t chas);
2121

2222
rt_err_t chassis_enable(chassis_t chas);
2323
rt_err_t chassis_disable(chassis_t chas);
@@ -32,4 +32,8 @@ rt_err_t chassis_straight(chassis_t chas, float linear_x);
3232
rt_err_t chassis_move(chassis_t chas, float linear_y);
3333
rt_err_t chassis_rotate(chassis_t chas, float angular_z);
3434

35+
rt_err_t chassis_set_velocity_x(chassis_t chas, float linear_x);
36+
rt_err_t chassis_set_velocity_y(chassis_t chas, float linear_y);
37+
rt_err_t chassis_set_velocity_z(chassis_t chas, float angular_z);
38+
3539
#endif

‎controller/controller.c‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -8,12 +8,11 @@
88

99
controller_t controller_create(rt_size_t size, rt_uint16_t sample_time)
1010
{
11-
// TODO
1211
// Malloc memory and initialize PID
1312
controller_t new_controller = (controller_t)rt_malloc(size);
1413
if(new_controller == RT_NULL)
1514
{
16-
LOG_E("Failed to malloc memory for automatic controller\n");
15+
LOG_E("Failed to malloc memory for new controller\n");
1716
return RT_NULL;
1817
}
1918

@@ -43,32 +42,33 @@ rt_err_t controller_destroy(controller_t controller)
4342
{
4443
RT_ASSERT(controller != RT_NULL);
4544

45+
LOG_D("Free controller");
46+
4647
return controller->destroy(controller);
4748
}
4849

4950
rt_err_t controller_enable(controller_t controller)
5051
{
51-
// TODO
5252
RT_ASSERT(controller != RT_NULL);
5353

54+
LOG_D("Enabling controller");
5455
controller->enable = RT_TRUE;
5556

5657
return RT_EOK;
5758
}
5859

5960
rt_err_t controller_disable(controller_t controller)
6061
{
61-
// TODO
6262
RT_ASSERT(controller != RT_NULL);
6363

64+
LOG_D("Disabling controller");
6465
controller->enable = RT_FALSE;
6566

6667
return RT_EOK;
6768
}
6869

6970
rt_err_t controller_reset(controller_t controller)
7071
{
71-
// TODO
7272
RT_ASSERT(controller != RT_NULL);
7373

7474
return controller->reset(controller);

‎controller/controller.h‎

Lines changed: 10 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -36,15 +36,15 @@ struct controller
3636

3737
typedef struct controller *controller_t;
3838

39-
controller_t controller_create(rt_size_t size, rt_uint16_t sample_time);
40-
rt_err_t controller_update(controller_t controller, float current_point);
41-
rt_err_t controller_destroy(controller_t controller);
42-
rt_err_t controller_enable(controller_t controller);
43-
rt_err_t controller_disable(controller_t controller);
44-
rt_err_t controller_reset(controller_t controller);
45-
rt_err_t controller_set_target(controller_t controller, rt_int16_t target);
46-
rt_err_t controller_set_sample_time(controller_t controller, rt_uint16_t sample_time);
47-
rt_err_t controller_set_param(controller_t controller, controller_param_t param);
48-
rt_err_t controller_get_param(controller_t controller, controller_param_t param);
39+
controller_t controller_create(rt_size_t size, rt_uint16_t sample_time);
40+
rt_err_t controller_update(controller_t controller, float current_point);
41+
rt_err_t controller_destroy(controller_t controller);
42+
rt_err_t controller_enable(controller_t controller);
43+
rt_err_t controller_disable(controller_t controller);
44+
rt_err_t controller_reset(controller_t controller);
45+
rt_err_t controller_set_target(controller_t controller, rt_int16_t target);
46+
rt_err_t controller_set_sample_time(controller_t controller, rt_uint16_t sample_time);
47+
rt_err_t controller_set_param(controller_t controller, controller_param_t param);
48+
rt_err_t controller_get_param(controller_t controller, controller_param_t param);
4949

5050
#endif // __CONTROLLER_H__

‎controller/inc_pid_controller.c‎

Lines changed: 7 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,13 @@
66

77
static rt_err_t inc_pid_controller_reset(void *pid)
88
{
9-
rt_memset(pid, 0, sizeof(struct inc_pid_controller));
9+
inc_pid_controller_t inc_pid = (inc_pid_controller_t)pid;
10+
11+
inc_pid->error = 0.0f;
12+
inc_pid->error_l = 0.0f;
13+
inc_pid->error_ll = 0.0f;
14+
inc_pid->last_out = 0.0f;
15+
1016
return RT_EOK;
1117
}
1218

@@ -71,14 +77,6 @@ static rt_err_t inc_pid_controller_update(void *pid, float current_point)
7177

7278
inc_pid->controller.output = inc_pid->last_out;
7379

74-
// rt_kprintf("%d - %d\n", current_point, pid->set_point);
75-
// 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));
76-
// LOG_D("PID P Error: %d", (int)(pid->p_error + 0.5f));
77-
// LOG_D("PID I Error: %d", (int)(pid->i_error + 0.5f));
78-
// LOG_D("PID D Error: %d", (int)(pid->d_error + 0.5f));
79-
// LOG_D("PID Last Out: %d", (int)(pid->last_out + 0.5f));
80-
// LOG_D("PID Out: %d", (int)(pid->out + 0.5f));
81-
8280
return RT_EOK;
8381
}
8482

‎controller/inc_pid_controller.h‎

Lines changed: 15 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -8,25 +8,25 @@ typedef struct inc_pid_controller *inc_pid_controller_t;
88

99
struct inc_pid_controller
1010
{
11-
struct controller controller;
11+
struct controller controller;
1212

13-
float kp;
14-
float ki;
15-
float kd;
13+
float kp;
14+
float ki;
15+
float kd;
1616

17-
float minimum;
18-
float maximum;
17+
float minimum;
18+
float maximum;
1919

20-
float p_error;
21-
float i_error;
22-
float d_error;
20+
float p_error;
21+
float i_error;
22+
float d_error;
2323

24-
float error;
25-
float error_l;
26-
float error_ll;
27-
28-
float last_out;
29-
rt_tick_t last_time;
24+
float error;
25+
float error_l;
26+
float error_ll;
27+
28+
float last_out;
29+
rt_tick_t last_time;
3030
};
3131

3232
inc_pid_controller_t inc_pid_controller_create(float kp, float ki, float kd, rt_uint16_t sample_time);

0 commit comments

Comments
 (0)