Skip to content

Commit 7a17b25

Browse files
sogwmssogwms
committed
update command (4
1 parent dc09cc2 commit 7a17b25

File tree

5 files changed

+270
-104
lines changed

5 files changed

+270
-104
lines changed

‎protocol/ano.c‎

Lines changed: 17 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,7 @@ static rt_sem_t rx_sem = RT_NULL;
2323
// ABOUT COMMAND
2424
static void *ano_target;
2525

26-
static rt_err_t ano_sender_send(rt_int16_t cmd, void *param, rt_uint16_t size)
26+
static rt_err_t ano_sender_send(rt_uint16_t cmd, void *param, rt_uint16_t size)
2727
{
2828
switch (cmd)
2929
{
@@ -161,15 +161,25 @@ static void ano_parse_frame(uint8_t *buffer, uint8_t length)
161161
{
162162
if (*(buffer + 4) == 0X01)
163163
{
164-
command_handle(COMMAND_REQUEST_PID, RT_NULL, 0, &ano_sender, ano_target);
164+
struct cmd_dt_pid pid[4];
165+
if (RT_EOK == command_handle(COMMAND_GET_WHEELS_PID, pid, 4*sizeof(struct cmd_dt_pid), ano_target))
166+
{
167+
ano_send_pid(1,
168+
pid[0].kp, pid[0].ki, pid[0].kd,
169+
pid[1].kp, pid[1].ki, pid[1].kd,
170+
pid[2].kp, pid[2].ki, pid[2].kd);
171+
ano_send_pid(2,
172+
pid[3].kp, pid[3].ki, pid[3].kd,
173+
0.0f,0.0f,0.0f,0.0f,0.0f,0.0f);
174+
}
165175
}
166176
else if (*(buffer + 4) == 0XA0)
167177
{
168178
// request version info
169179
}
170180
else if (*(buffer + 4) == 0XA1)
171181
{
172-
command_handle(COMMAND_RESET_PID, RT_NULL, 0, &ano_sender, ano_target);
182+
command_handle(COMMAND_SET_DEFAULT_PID, RT_NULL, 0, ano_target);
173183
}
174184
}
175185
else if (*(buffer + 2) == 0X10) //PID1
@@ -182,17 +192,17 @@ static void ano_parse_frame(uint8_t *buffer, uint8_t length)
182192
pid.kp = kpid[0];
183193
pid.ki = kpid[1];
184194
pid.kd = kpid[2];
185-
command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), &ano_sender, ano_target);
195+
command_handle(COMMAND_SET_WHEEL0_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
186196
pid.id = PID_ID_WHEEL_1;
187197
pid.kp = kpid[3];
188198
pid.ki = kpid[4];
189199
pid.kd = kpid[5];
190-
command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), &ano_sender, ano_target);
200+
command_handle(COMMAND_SET_WHEEL1_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
191201
pid.id = PID_ID_WHEEL_2;
192202
pid.kp = kpid[6];
193203
pid.ki = kpid[7];
194204
pid.kd = kpid[8];
195-
command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), &ano_sender, ano_target);
205+
command_handle(COMMAND_SET_WHEEL2_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
196206

197207
ano_send_check(*(buffer + 2), sum);
198208
}
@@ -206,7 +216,7 @@ static void ano_parse_frame(uint8_t *buffer, uint8_t length)
206216
pid.kp = kpid[0];
207217
pid.ki = kpid[1];
208218
pid.kd = kpid[2];
209-
command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), &ano_sender, ano_target);
219+
command_handle(COMMAND_SET_WHEEL3_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
210220
// pid.id = 5;
211221
// pid.kp = kpid[3];
212222
// pid.ki = kpid[4];

‎protocol/command.c‎

Lines changed: 191 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
#include <command.h>
2+
#include <chassis.h>
23

34
#define DBG_SECTION_NAME "command"
45
#define DBG_LEVEL DBG_LOG
@@ -10,6 +11,15 @@
1011
#define THREAD_TIMESLICE 10
1112
static rt_thread_t cmd_tid = RT_NULL;
1213

14+
//
15+
struct command_info
16+
{
17+
rt_uint16_t cmd;
18+
void *target;
19+
};
20+
21+
typedef struct command_info *command_info_t;
22+
1323
// Message Queue
1424
struct command_msg
1525
{
@@ -20,59 +30,127 @@ struct command_msg
2030
#define MAX_MSGS 32
2131
static rt_mq_t cmd_mq = RT_NULL;
2232

23-
// Table
24-
#define TABLE_MAX_SIZE 32
25-
struct command
26-
{
27-
rt_int16_t robot_cmd;
28-
void (*handler)(command_info_t info, void *param, rt_uint16_t size);
29-
};
30-
static struct command command_table[TABLE_MAX_SIZE];
31-
static uint16_t command_table_size = 0;
32-
33-
rt_err_t command_register(rt_int16_t cmd, void (*handler)(command_info_t info, void *param, rt_uint16_t size))
33+
static rt_err_t command_handle_get(struct command_msg *msg)
3434
{
35-
if (command_table_size > TABLE_MAX_SIZE - 1)
35+
switch (msg->info.cmd)
3636
{
37-
LOG_E("Command table voerflow");
38-
return RT_ERROR;
39-
}
40-
command_table[command_table_size].robot_cmd = cmd;
41-
command_table[command_table_size].handler = handler;
42-
command_table_size++;
37+
case COMMAND_GET_WHEEL0_PID:
38+
if (msg->size == sizeof(struct cmd_dt_pid))
39+
{
40+
struct cmd_dt_pid *ppid = msg->param;
41+
chassis_t chas = (chassis_t)msg->info.target;
42+
struct controller_param parameter;
4343

44-
return RT_EOK;
45-
}
44+
controller_get_param(chas->c_wheels[0]->w_controller, &parameter);
45+
ppid->kp = parameter.data.pid.kp;
46+
ppid->ki = parameter.data.pid.ki;
47+
ppid->kd = parameter.data.pid.kd;
48+
}
49+
else
50+
{
51+
return RT_ERROR;
52+
}
53+
54+
break;
55+
case COMMAND_GET_WHEEL1_PID:
56+
if (msg->size == sizeof(struct cmd_dt_pid))
57+
{
58+
struct cmd_dt_pid *ppid = msg->param;
59+
chassis_t chas = (chassis_t)msg->info.target;
60+
struct controller_param parameter;
4661

47-
rt_err_t command_unregister(rt_int16_t cmd)
48-
{
49-
for (int i = 0; i < command_table_size; i++)
50-
{
51-
if (command_table[i].robot_cmd == cmd)
62+
controller_get_param(chas->c_wheels[1]->w_controller, &parameter);
63+
ppid->kp = parameter.data.pid.kp;
64+
ppid->ki = parameter.data.pid.ki;
65+
ppid->kd = parameter.data.pid.kd;
66+
}
67+
else
5268
{
53-
for (int j = i; j < command_table_size - 1; j++)
69+
return RT_ERROR;
70+
}
71+
72+
break;
73+
case COMMAND_GET_WHEEL2_PID:
74+
if (msg->size == sizeof(struct cmd_dt_pid))
75+
{
76+
struct cmd_dt_pid *ppid = msg->param;
77+
chassis_t chas = (chassis_t)msg->info.target;
78+
struct controller_param parameter;
79+
80+
controller_get_param(chas->c_wheels[2]->w_controller, &parameter);
81+
ppid->kp = parameter.data.pid.kp;
82+
ppid->ki = parameter.data.pid.ki;
83+
ppid->kd = parameter.data.pid.kd;
84+
}
85+
else
86+
{
87+
return RT_ERROR;
88+
}
89+
90+
break;
91+
case COMMAND_GET_WHEEL3_PID:
92+
if (msg->size == sizeof(struct cmd_dt_pid))
93+
{
94+
struct cmd_dt_pid *ppid = msg->param;
95+
chassis_t chas = (chassis_t)msg->info.target;
96+
struct controller_param parameter;
97+
98+
controller_get_param(chas->c_wheels[3]->w_controller, &parameter);
99+
ppid->kp = parameter.data.pid.kp;
100+
ppid->ki = parameter.data.pid.ki;
101+
ppid->kd = parameter.data.pid.kd;
102+
}
103+
else
104+
{
105+
return RT_ERROR;
106+
}
107+
108+
break;
109+
case COMMAND_GET_WHEELS_PID:
110+
if (msg->size >= 2 * sizeof(struct cmd_dt_pid))
111+
{
112+
struct cmd_dt_pid *ppid = msg->param;
113+
chassis_t chas = (chassis_t)msg->info.target;
114+
struct controller_param parameter;
115+
116+
for (int i = 0; (i < chas->c_kinematics->total_wheels) && (i < (msg->size / sizeof(struct cmd_dt_pid))); i++)
54117
{
55-
rt_memcpy(&command_table[j], &command_table[j+1], sizeof(struct command));
118+
controller_get_param(chas->c_wheels[i]->w_controller, &parameter);
119+
ppid[i].id = COMMAND_GET_WHEEL0_PID + i;
120+
ppid[i].kp = parameter.data.pid.kp;
121+
ppid[i].ki = parameter.data.pid.ki;
122+
ppid[i].kd = parameter.data.pid.kd;
56123
}
57-
command_table_size--;
58-
break;
59124
}
125+
else
126+
{
127+
return RT_ERROR;
128+
}
129+
130+
break;
131+
132+
default: return RT_ERROR;
60133
}
61134

62135
return RT_EOK;
63136
}
64137

65-
rt_err_t command_handle(rt_int16_t cmd, void *param, rt_uint16_t size, command_sender_t sender, void *target)
138+
rt_err_t command_handle(rt_uint16_t cmd, void *param, rt_uint16_t size, void *target)
66139
{
67140
struct command_msg msg = {
68141
.param = param,
69142
.size = size,
70143
.info = {
71-
.sender = sender,
72144
.target = target,
73145
.cmd = cmd,
74146
}
75147
};
148+
149+
if (cmd > COMMAND_GET_START && cmd < COMMAND_GET_END)
150+
{
151+
return command_handle_get(&msg);
152+
}
153+
76154
return rt_mq_send(cmd_mq, &msg, sizeof(struct command_msg));
77155
}
78156

@@ -93,14 +171,91 @@ static void command_thread_entry(void *param)
93171
while (1)
94172
{
95173
rt_mq_recv(cmd_mq, &msg, sizeof(struct command_msg), RT_WAITING_FOREVER);
96-
// look-up table and call callback
97-
for (int i = 0; i < command_table_size; i++)
174+
175+
// TODO
176+
switch (msg.info.cmd)
98177
{
99-
if (command_table[i].robot_cmd == msg.info.cmd)
178+
case COMMAND_SET_WHEELS_PID:
179+
if (msg.size >= 2 * sizeof(struct cmd_dt_pid))
180+
{
181+
chassis_t chas = (chassis_t)msg.info.target;
182+
struct cmd_dt_pid *ppid = msg.param;
183+
struct controller_param parameter;
184+
for (int i = 0; i < chas->c_kinematics->total_wheels; i++)
185+
{
186+
parameter.data.pid.kp = ppid[i].kp;
187+
parameter.data.pid.ki = ppid[i].ki;
188+
parameter.data.pid.kd = ppid[i].kd;
189+
controller_set_param(chas->c_wheels[ppid[i].id]->w_controller, &parameter);
190+
}
191+
}
192+
break;
193+
case COMMAND_SET_WHEEL0_PID:
194+
if (msg.size == sizeof(struct cmd_dt_pid))
100195
{
101-
command_table[i].handler(&msg.info, msg.param, msg.size);
102-
break;
196+
chassis_t chas = (chassis_t)msg.info.target;
197+
if (chas->c_kinematics->total_wheels > 0)
198+
{
199+
struct cmd_dt_pid *ppid = msg.param;
200+
struct controller_param parameter = {
201+
.data.pid.kp = ppid->kp,
202+
.data.pid.ki = ppid->ki,
203+
.data.pid.kd = ppid->kd
204+
};
205+
controller_set_param(chas->c_wheels[0]->w_controller, &parameter);
206+
}
103207
}
208+
break;
209+
case COMMAND_SET_WHEEL1_PID:
210+
if (msg.size == sizeof(struct cmd_dt_pid))
211+
{
212+
chassis_t chas = (chassis_t)msg.info.target;
213+
if (chas->c_kinematics->total_wheels > 1)
214+
{
215+
struct cmd_dt_pid *ppid = msg.param;
216+
struct controller_param parameter = {
217+
.data.pid.kp = ppid->kp,
218+
.data.pid.ki = ppid->ki,
219+
.data.pid.kd = ppid->kd
220+
};
221+
controller_set_param(chas->c_wheels[1]->w_controller, &parameter);
222+
}
223+
}
224+
break;
225+
case COMMAND_SET_WHEEL2_PID:
226+
if (msg.size == sizeof(struct cmd_dt_pid))
227+
{
228+
chassis_t chas = (chassis_t)msg.info.target;
229+
if (chas->c_kinematics->total_wheels > 2)
230+
{
231+
struct cmd_dt_pid *ppid = msg.param;
232+
struct controller_param parameter = {
233+
.data.pid.kp = ppid->kp,
234+
.data.pid.ki = ppid->ki,
235+
.data.pid.kd = ppid->kd
236+
};
237+
controller_set_param(chas->c_wheels[2]->w_controller, &parameter);
238+
}
239+
}
240+
break;
241+
case COMMAND_SET_WHEEL3_PID:
242+
if (msg.size == sizeof(struct cmd_dt_pid))
243+
{
244+
chassis_t chas = (chassis_t)msg.info.target;
245+
if (chas->c_kinematics->total_wheels > 3)
246+
{
247+
struct cmd_dt_pid *ppid = msg.param;
248+
struct controller_param parameter = {
249+
.data.pid.kp = ppid->kp,
250+
.data.pid.ki = ppid->ki,
251+
.data.pid.kd = ppid->kd
252+
};
253+
controller_set_param(chas->c_wheels[3]->w_controller, &parameter);
254+
}
255+
}
256+
break;
257+
default:
258+
break;
104259
}
105260
}
106261
}

0 commit comments

Comments
 (0)