2 * Copyright Droids Corporation (2008)
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 * Revision : $Id: commands.c,v 1.1.2.3 2008-12-27 16:29:08 zer0 Exp $
20 * Olivier MATZ <zer0@droids-corp.org>
26 #include <aversive/pgmspace.h>
27 #include <aversive/wait.h>
31 #include <parse_num.h>
32 #include <parse_string.h>
38 uint8_t addr_from_string(const char *s)
41 if (!strcmp_P(s, PSTR("cw_angle_limit")))
42 return AA_CW_ANGLE_LIMIT_L;
43 if (!strcmp_P(s, PSTR("ccw_angle_limit")))
44 return AA_CCW_ANGLE_LIMIT_L;
45 if (!strcmp_P(s, PSTR("max_torque")))
46 return AA_MAX_TORQUE_L;
47 if (!strcmp_P(s, PSTR("down_calibration")))
48 return AA_DOWN_CALIBRATION_L;
49 if (!strcmp_P(s, PSTR("up_calibration")))
50 return AA_UP_CALIBRATION_L;
51 if (!strcmp_P(s, PSTR("torque_limit")))
52 return AA_TORQUE_LIMIT_L;
53 if (!strcmp_P(s, PSTR("position")))
54 return AA_PRESENT_POSITION_L;
55 if (!strcmp_P(s, PSTR("speed")))
56 return AA_PRESENT_SPEED_L;
57 if (!strcmp_P(s, PSTR("load")))
58 return AA_PRESENT_LOAD_L;
59 if (!strcmp_P(s, PSTR("moving_speed")))
60 return AA_MOVING_SPEED_L;
61 if (!strcmp_P(s, PSTR("model")))
62 return AA_MODEL_NUMBER_L;
63 if (!strcmp_P(s, PSTR("goal_pos")))
64 return AA_GOAL_POSITION_L;
65 if (!strcmp_P(s, PSTR("punch")))
69 if (!strcmp_P(s, PSTR("firmware")))
71 if (!strcmp_P(s, PSTR("id")))
73 if (!strcmp_P(s, PSTR("baudrate")))
75 if (!strcmp_P(s, PSTR("delay")))
77 if (!strcmp_P(s, PSTR("high_lim_temp")))
78 return AA_HIGHEST_LIMIT_TEMP;
79 if (!strcmp_P(s, PSTR("low_lim_volt")))
80 return AA_LOWEST_LIMIT_VOLTAGE;
81 if (!strcmp_P(s, PSTR("high_lim_volt")))
82 return AA_HIGHEST_LIMIT_VOLTAGE;
83 if (!strcmp_P(s, PSTR("status_return")))
84 return AA_STATUS_RETURN_LEVEL;
85 if (!strcmp_P(s, PSTR("alarm_led")))
87 if (!strcmp_P(s, PSTR("alarm_shutdown")))
88 return AA_ALARM_SHUTDOWN;
89 if (!strcmp_P(s, PSTR("torque_enable")))
90 return AA_TORQUE_ENABLE;
91 if (!strcmp_P(s, PSTR("led")))
93 if (!strcmp_P(s, PSTR("cw_comp_margin")))
94 return AA_CW_COMPLIANCE_MARGIN;
95 if (!strcmp_P(s, PSTR("ccw_comp_margin")))
96 return AA_CCW_COMPLIANCE_MARGIN;
97 if (!strcmp_P(s, PSTR("cw_comp_slope")))
98 return AA_CW_COMPLIANCE_SLOPE;
99 if (!strcmp_P(s, PSTR("ccw_comp_slope")))
100 return AA_CCW_COMPLIANCE_SLOPE;
101 if (!strcmp_P(s, PSTR("voltage")))
102 return AA_PRESENT_VOLTAGE;
103 if (!strcmp_P(s, PSTR("temp")))
104 return AA_PRESENT_TEMP;
105 if (!strcmp_P(s, PSTR("reginst")))
106 return AA_PRESENT_REGINST;
107 if (!strcmp_P(s, PSTR("moving")))
109 if (!strcmp_P(s, PSTR("lock")))
117 /**********************************************************/
120 /* this structure is filled when cmd_reset is parsed successfully */
121 struct cmd_reset_result {
125 /* function called when cmd_reset is parsed successfully */
126 static void cmd_reset_parsed(void * parsed_result, void * data)
131 prog_char str_reset_arg0[] = "reset";
132 parse_pgm_token_string_t cmd_reset_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_reset_result, arg0, str_reset_arg0);
134 prog_char help_reset[] = "Reset the board";
135 parse_pgm_inst_t cmd_reset = {
136 .f = cmd_reset_parsed, /* function to call */
137 .data = NULL, /* 2nd arg of func */
138 .help_str = help_reset,
139 .tokens = { /* token list, NULL terminated */
140 (prog_void *)&cmd_reset_arg0,
145 /**********************************************************/
148 /* this structure is filled when cmd_test is parsed successfully */
149 struct cmd_test_result {
153 /* function called when cmd_test is parsed successfully */
154 static void cmd_test_parsed(void * parsed_result, void * data)
156 /* Set some AX12 parameters */
157 AX12_write_int(&ax12,0xFE,AA_PUNCH_L,0x20);
158 AX12_write_int(&ax12,0xFE,AA_TORQUE_LIMIT_L,0x3FF);
159 AX12_write_int(&ax12,0xFE,AA_MOVING_SPEED_L,0x2FF);
160 AX12_write_byte(&ax12,0xFE,AA_ALARM_LED,0xEF);
162 AX12_set_position(&ax12,0xFE,0x1FF - 0x130);
164 AX12_set_position(&ax12,0xFE,0x1FF + 0x130);
168 prog_char str_test_arg0[] = "test";
169 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
171 prog_char help_test[] = "Test func";
172 parse_pgm_inst_t cmd_test = {
173 .f = cmd_test_parsed, /* function to call */
174 .data = NULL, /* 2nd arg of func */
175 .help_str = help_test,
176 .tokens = { /* token list, NULL terminated */
177 (prog_void *)&cmd_test_arg0,
182 /**********************************************************/
184 /* this structure is filled when cmd_test is parsed successfully */
185 struct cmd_baudrate_result {
190 /* function called when cmd_baudrate is parsed successfully */
191 static void cmd_baudrate_parsed(void * parsed_result, void * data)
193 struct cmd_baudrate_result *res = parsed_result;
194 struct uart_config c;
197 c.baudrate = res->arg1;
199 printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L);
202 prog_char str_baudrate_arg0[] = "baudrate";
203 parse_pgm_token_string_t cmd_baudrate_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_baudrate_result, arg0, str_baudrate_arg0);
204 parse_pgm_token_num_t cmd_baudrate_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_baudrate_result, arg1, UINT32);
206 prog_char help_baudrate[] = "Change ax12 baudrate";
207 parse_pgm_inst_t cmd_baudrate = {
208 .f = cmd_baudrate_parsed, /* function to call */
209 .data = NULL, /* 2nd arg of func */
210 .help_str = help_baudrate,
211 .tokens = { /* token list, NULL terminated */
212 (prog_void *)&cmd_baudrate_arg0,
213 (prog_void *)&cmd_baudrate_arg1,
218 /**********************************************************/
222 /* this structure is filled when cmd_uint16_read is parsed successfully */
223 struct cmd_uint16_result {
230 /* function called when cmd_uint16_read is parsed successfully */
231 static void cmd_uint16_read_parsed(void * parsed_result, void * data)
233 struct cmd_uint16_result *res = parsed_result;
236 uint8_t addr = addr_from_string(res->arg1);
237 ret = AX12_read_int(&ax12, res->num, addr, &val);
239 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
240 printf_P(PSTR("%s: %d [0x%.4x]\r\n"), res->arg1, val, val);
243 prog_char str_uint16_arg0[] = "read";
244 parse_pgm_token_string_t cmd_uint16_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0);
245 prog_char str_uint16_arg1[] = "moving_speed#model#goal_pos#cw_angle_limit#ccw_angle_limit#"
246 "max_torque#down_calibration#up_calibration#torque_limit#"
247 "position#speed#load#punch";
248 parse_pgm_token_string_t cmd_uint16_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1);
249 parse_pgm_token_num_t cmd_uint16_num = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, num, UINT8);
251 prog_char help_uint16_read[] = "Read uint16 value (type, num)";
252 parse_pgm_inst_t cmd_uint16_read = {
253 .f = cmd_uint16_read_parsed, /* function to call */
254 .data = NULL, /* 2nd arg of func */
255 .help_str = help_uint16_read,
256 .tokens = { /* token list, NULL terminated */
257 (prog_void *)&cmd_uint16_arg0,
258 (prog_void *)&cmd_uint16_arg1,
259 (prog_void *)&cmd_uint16_num,
264 /* function called when cmd_uint16_write is parsed successfully */
265 static void cmd_uint16_write_parsed(void * parsed_result, void * data)
267 struct cmd_uint16_result *res = parsed_result;
269 uint8_t addr = addr_from_string(res->arg1);
270 printf_P(PSTR("writing %s: %d [0x%.4x]\r\n"), res->arg1,
272 ret = AX12_write_int(&ax12, res->num, addr, res->val);
274 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
277 prog_char str_uint16_arg0_w[] = "write";
278 parse_pgm_token_string_t cmd_uint16_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0_w);
279 prog_char str_uint16_arg1_w[] = "moving_speed#goal_pos#cw_angle_limit#ccw_angle_limit#"
280 "max_torque#torque_limit#punch";
281 parse_pgm_token_string_t cmd_uint16_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1_w);
282 parse_pgm_token_num_t cmd_uint16_val = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, val, UINT16);
284 prog_char help_uint16_write[] = "Write uint16 value (write, num, val)";
285 parse_pgm_inst_t cmd_uint16_write = {
286 .f = cmd_uint16_write_parsed, /* function to call */
287 .data = NULL, /* 2nd arg of func */
288 .help_str = help_uint16_write,
289 .tokens = { /* token list, NULL terminated */
290 (prog_void *)&cmd_uint16_arg0_w,
291 (prog_void *)&cmd_uint16_arg1_w,
292 (prog_void *)&cmd_uint16_num,
293 (prog_void *)&cmd_uint16_val,
298 /**********************************************************/
302 /* this structure is filled when cmd_uint8_read is parsed successfully */
303 struct cmd_uint8_result {
310 /* function called when cmd_uint8_read is parsed successfully */
311 static void cmd_uint8_read_parsed(void * parsed_result, void * data)
313 struct cmd_uint8_result *res = parsed_result;
316 uint8_t addr = addr_from_string(res->arg1);
318 ret = AX12_read_byte(&ax12, res->num, addr, &val);
320 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
321 printf_P(PSTR("%s: %d [0x%.2x]\r\n"), res->arg1, val, val);
324 prog_char str_uint8_arg0[] = "read";
325 parse_pgm_token_string_t cmd_uint8_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0);
326 prog_char str_uint8_arg1[] = "id#firmware#baudrate#delay#high_lim_temp#"
327 "low_lim_volt#high_lim_volt#status_return#alarm_led#"
328 "alarm_shutdown#torque_enable#led#cw_comp_margin#"
329 "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
330 "voltage#temp#reginst#moving#lock";
331 parse_pgm_token_string_t cmd_uint8_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1);
332 parse_pgm_token_num_t cmd_uint8_num = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, num, UINT8);
334 prog_char help_uint8_read[] = "Read uint8 value (type, num)";
335 parse_pgm_inst_t cmd_uint8_read = {
336 .f = cmd_uint8_read_parsed, /* function to call */
337 .data = NULL, /* 2nd arg of func */
338 .help_str = help_uint8_read,
339 .tokens = { /* token list, NULL terminated */
340 (prog_void *)&cmd_uint8_arg0,
341 (prog_void *)&cmd_uint8_arg1,
342 (prog_void *)&cmd_uint8_num,
347 /* function called when cmd_uint8_write is parsed successfully */
348 static void cmd_uint8_write_parsed(void * parsed_result, void * data)
350 struct cmd_uint8_result *res = parsed_result;
351 uint8_t addr = addr_from_string(res->arg1);
353 printf_P(PSTR("writing %s: %d [0x%.2x]\r\n"), res->arg1,
355 ret = AX12_write_byte(&ax12, res->num, addr, res->val);
357 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
360 prog_char str_uint8_arg0_w[] = "write";
361 parse_pgm_token_string_t cmd_uint8_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0_w);
362 prog_char str_uint8_arg1_w[] = "id#baudrate#delay#high_lim_temp#"
363 "low_lim_volt#high_lim_volt#status_return#alarm_led#"
364 "alarm_shutdown#torque_enable#led#cw_comp_margin#"
365 "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
367 parse_pgm_token_string_t cmd_uint8_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1_w);
368 parse_pgm_token_num_t cmd_uint8_val = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, val, UINT8);
370 prog_char help_uint8_write[] = "Write uint8 value (write, num, val)";
371 parse_pgm_inst_t cmd_uint8_write = {
372 .f = cmd_uint8_write_parsed, /* function to call */
373 .data = NULL, /* 2nd arg of func */
374 .help_str = help_uint8_write,
375 .tokens = { /* token list, NULL terminated */
376 (prog_void *)&cmd_uint8_arg0_w,
377 (prog_void *)&cmd_uint8_arg1_w,
378 (prog_void *)&cmd_uint8_num,
379 (prog_void *)&cmd_uint8_val,
386 parse_pgm_ctx_t main_ctx[] = {
387 (parse_pgm_inst_t *)&cmd_reset,
388 (parse_pgm_inst_t *)&cmd_test,
389 (parse_pgm_inst_t *)&cmd_baudrate,
390 (parse_pgm_inst_t *)&cmd_uint16_read,
391 (parse_pgm_inst_t *)&cmd_uint16_write,
392 (parse_pgm_inst_t *)&cmd_uint8_read,
393 (parse_pgm_inst_t *)&cmd_uint8_write,