2 * Copyright Droids Corporation (2009)
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_ax12.c,v 1.3 2009-04-24 19:30:42 zer0 Exp $
20 * Olivier MATZ <zer0@droids-corp.org>
26 #include <aversive/pgmspace.h>
27 #include <aversive/wait.h>
28 #include <aversive/error.h>
33 #include <clock_time.h>
37 #include <control_system_manager.h>
38 #include <blocking_detection_manager.h>
42 #include <parse_string.h>
43 #include <parse_num.h>
46 #include "ax12_user.h"
48 uint8_t addr_from_string(const char *s)
51 if (!strcmp_P(s, PSTR("cw_angle_limit")))
52 return AA_CW_ANGLE_LIMIT_L;
53 if (!strcmp_P(s, PSTR("ccw_angle_limit")))
54 return AA_CCW_ANGLE_LIMIT_L;
55 if (!strcmp_P(s, PSTR("max_torque")))
56 return AA_MAX_TORQUE_L;
57 if (!strcmp_P(s, PSTR("down_calibration")))
58 return AA_DOWN_CALIBRATION_L;
59 if (!strcmp_P(s, PSTR("up_calibration")))
60 return AA_UP_CALIBRATION_L;
61 if (!strcmp_P(s, PSTR("torque_limit")))
62 return AA_TORQUE_LIMIT_L;
63 if (!strcmp_P(s, PSTR("position")))
64 return AA_PRESENT_POSITION_L;
65 if (!strcmp_P(s, PSTR("speed")))
66 return AA_PRESENT_SPEED_L;
67 if (!strcmp_P(s, PSTR("load")))
68 return AA_PRESENT_LOAD_L;
69 if (!strcmp_P(s, PSTR("moving_speed")))
70 return AA_MOVING_SPEED_L;
71 if (!strcmp_P(s, PSTR("model")))
72 return AA_MODEL_NUMBER_L;
73 if (!strcmp_P(s, PSTR("goal_pos")))
74 return AA_GOAL_POSITION_L;
75 if (!strcmp_P(s, PSTR("punch")))
79 if (!strcmp_P(s, PSTR("firmware")))
81 if (!strcmp_P(s, PSTR("id")))
83 if (!strcmp_P(s, PSTR("baudrate")))
85 if (!strcmp_P(s, PSTR("delay")))
87 if (!strcmp_P(s, PSTR("high_lim_temp")))
88 return AA_HIGHEST_LIMIT_TEMP;
89 if (!strcmp_P(s, PSTR("low_lim_volt")))
90 return AA_LOWEST_LIMIT_VOLTAGE;
91 if (!strcmp_P(s, PSTR("high_lim_volt")))
92 return AA_HIGHEST_LIMIT_VOLTAGE;
93 if (!strcmp_P(s, PSTR("status_return")))
94 return AA_STATUS_RETURN_LEVEL;
95 if (!strcmp_P(s, PSTR("alarm_led")))
97 if (!strcmp_P(s, PSTR("alarm_shutdown")))
98 return AA_ALARM_SHUTDOWN;
99 if (!strcmp_P(s, PSTR("torque_enable")))
100 return AA_TORQUE_ENABLE;
101 if (!strcmp_P(s, PSTR("led")))
103 if (!strcmp_P(s, PSTR("cw_comp_margin")))
104 return AA_CW_COMPLIANCE_MARGIN;
105 if (!strcmp_P(s, PSTR("ccw_comp_margin")))
106 return AA_CCW_COMPLIANCE_MARGIN;
107 if (!strcmp_P(s, PSTR("cw_comp_slope")))
108 return AA_CW_COMPLIANCE_SLOPE;
109 if (!strcmp_P(s, PSTR("ccw_comp_slope")))
110 return AA_CCW_COMPLIANCE_SLOPE;
111 if (!strcmp_P(s, PSTR("voltage")))
112 return AA_PRESENT_VOLTAGE;
113 if (!strcmp_P(s, PSTR("temp")))
114 return AA_PRESENT_TEMP;
115 if (!strcmp_P(s, PSTR("reginst")))
116 return AA_PRESENT_REGINST;
117 if (!strcmp_P(s, PSTR("moving")))
119 if (!strcmp_P(s, PSTR("lock")))
125 /**********************************************************/
128 /* this structure is filled when cmd_ax12_stress is parsed successfully */
129 struct cmd_ax12_stress_result {
134 /* function called when cmd_ax12_stress is parsed successfully */
135 static void cmd_ax12_stress_parsed(void *parsed_result, __attribute__((unused)) void *data)
137 struct cmd_ax12_stress_result *res = parsed_result;
144 for (i=0; i<1000; i++) {
145 if (AX12_read_byte(&gen.ax12, res->id, AA_ID, &val) != 0)
149 printf_P(PSTR("%d errors / 1000\r\n"), nb_errs);
150 t = (time_get_us2() - t) / 1000;
151 printf_P(PSTR("Test done in %d ms\r\n"), (int)t);
155 for (i=0; i<1000; i++) {
156 if (AX12_write_int(&gen.ax12, res->id, AA_GOAL_POSITION_L, 500))
160 printf_P(PSTR("%d errors / 1000\r\n"), nb_errs);
161 t = (time_get_us2() - t) / 1000;
162 printf_P(PSTR("Test done in %d ms\r\n"), (int)t);
168 for (i=0; i<100; i++) {
169 if (AX12_write_int(&gen.ax12, id, AA_GOAL_POSITION_L, 500))
176 printf_P(PSTR("%d errors / 100\r\n"), nb_errs);
177 t = (time_get_us2() - t) / 1000;
178 printf_P(PSTR("Test done in %d ms\r\n"), (int)t);
183 prog_char str_ax12_stress_arg0[] = "ax12_stress";
184 parse_pgm_token_string_t cmd_ax12_stress_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ax12_stress_result, arg0, str_ax12_stress_arg0);
185 parse_pgm_token_num_t cmd_ax12_stress_id = TOKEN_NUM_INITIALIZER(struct cmd_ax12_stress_result, id, UINT8);
187 prog_char help_ax12_stress[] = "Stress an AX12 with 1000 'read id' commands (id)";
188 parse_pgm_inst_t cmd_ax12_stress = {
189 .f = cmd_ax12_stress_parsed, /* function to call */
190 .data = NULL, /* 2nd arg of func */
191 .help_str = help_ax12_stress,
192 .tokens = { /* token list, NULL terminated */
193 (prog_void *)&cmd_ax12_stress_arg0,
194 (prog_void *)&cmd_ax12_stress_id,
199 /**********************************************************/
200 /* Ax12_Dump_Stats */
202 /* this structure is filled when cmd_ax12_dump_stats is parsed successfully */
203 struct cmd_ax12_dump_stats_result {
207 /* function called when cmd_ax12_dump_stats is parsed successfully */
208 static void cmd_ax12_dump_stats_parsed(__attribute__((unused)) void *parsed_result,
209 __attribute__((unused)) void *data)
214 prog_char str_ax12_dump_stats_arg0[] = "ax12_dump_stats";
215 parse_pgm_token_string_t cmd_ax12_dump_stats_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ax12_dump_stats_result, arg0, str_ax12_dump_stats_arg0);
217 prog_char help_ax12_dump_stats[] = "Dump AX12 stats";
218 parse_pgm_inst_t cmd_ax12_dump_stats = {
219 .f = cmd_ax12_dump_stats_parsed, /* function to call */
220 .data = NULL, /* 2nd arg of func */
221 .help_str = help_ax12_dump_stats,
222 .tokens = { /* token list, NULL terminated */
223 (prog_void *)&cmd_ax12_dump_stats_arg0,
228 /**********************************************************/
230 /* this structure is filled when cmd_baudrate is parsed successfully */
231 struct cmd_baudrate_result {
236 /* function called when cmd_baudrate is parsed successfully */
237 static void cmd_baudrate_parsed(void * parsed_result, __attribute__((unused)) void *data)
239 struct cmd_baudrate_result *res = parsed_result;
240 struct uart_config c;
242 uart_getconf(UART_AX12_NUM, &c);
243 c.baudrate = res->arg1;
244 uart_setconf(UART_AX12_NUM, &c);
247 prog_char str_baudrate_arg0[] = "baudrate";
248 parse_pgm_token_string_t cmd_baudrate_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_baudrate_result, arg0, str_baudrate_arg0);
249 parse_pgm_token_num_t cmd_baudrate_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_baudrate_result, arg1, UINT32);
251 prog_char help_baudrate[] = "Change ax12 baudrate";
252 parse_pgm_inst_t cmd_baudrate = {
253 .f = cmd_baudrate_parsed, /* function to call */
254 .data = NULL, /* 2nd arg of func */
255 .help_str = help_baudrate,
256 .tokens = { /* token list, NULL terminated */
257 (prog_void *)&cmd_baudrate_arg0,
258 (prog_void *)&cmd_baudrate_arg1,
263 /**********************************************************/
267 /* this structure is filled when cmd_uint16_read is parsed successfully */
268 struct cmd_uint16_result {
275 /* function called when cmd_uint16_read is parsed successfully */
276 static void cmd_uint16_read_parsed(void * parsed_result, __attribute__((unused)) void *data)
278 struct cmd_uint16_result *res = parsed_result;
281 uint8_t addr = addr_from_string(res->arg1);
282 ret = ax12_user_read_int(&gen.ax12, res->num, addr, &val);
284 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
285 printf_P(PSTR("%s: %d [0x%.4x]\r\n"), res->arg1, val, val);
288 prog_char str_uint16_arg0[] = "read";
289 parse_pgm_token_string_t cmd_uint16_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0);
290 prog_char str_uint16_arg1[] = "moving_speed#model#goal_pos#cw_angle_limit#ccw_angle_limit#"
291 "max_torque#down_calibration#up_calibration#torque_limit#"
292 "position#speed#load#punch";
293 parse_pgm_token_string_t cmd_uint16_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1);
294 parse_pgm_token_num_t cmd_uint16_num = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, num, UINT8);
296 prog_char help_uint16_read[] = "Read uint16 value (type, num)";
297 parse_pgm_inst_t cmd_uint16_read = {
298 .f = cmd_uint16_read_parsed, /* function to call */
299 .data = NULL, /* 2nd arg of func */
300 .help_str = help_uint16_read,
301 .tokens = { /* token list, NULL terminated */
302 (prog_void *)&cmd_uint16_arg0,
303 (prog_void *)&cmd_uint16_arg1,
304 (prog_void *)&cmd_uint16_num,
309 /* function called when cmd_uint16_write is parsed successfully */
310 static void cmd_uint16_write_parsed(void * parsed_result, __attribute__((unused)) void *data)
312 struct cmd_uint16_result *res = parsed_result;
314 uint8_t addr = addr_from_string(res->arg1);
315 printf_P(PSTR("writing %s: %d [0x%.4x]\r\n"), res->arg1,
317 ret = ax12_user_write_int(&gen.ax12, res->num, addr, res->val);
319 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
322 prog_char str_uint16_arg0_w[] = "write";
323 parse_pgm_token_string_t cmd_uint16_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0_w);
324 prog_char str_uint16_arg1_w[] = "moving_speed#goal_pos#cw_angle_limit#ccw_angle_limit#"
325 "max_torque#torque_limit#punch";
326 parse_pgm_token_string_t cmd_uint16_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1_w);
327 parse_pgm_token_num_t cmd_uint16_val = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, val, UINT16);
329 prog_char help_uint16_write[] = "Write uint16 value (write, num, val)";
330 parse_pgm_inst_t cmd_uint16_write = {
331 .f = cmd_uint16_write_parsed, /* function to call */
332 .data = NULL, /* 2nd arg of func */
333 .help_str = help_uint16_write,
334 .tokens = { /* token list, NULL terminated */
335 (prog_void *)&cmd_uint16_arg0_w,
336 (prog_void *)&cmd_uint16_arg1_w,
337 (prog_void *)&cmd_uint16_num,
338 (prog_void *)&cmd_uint16_val,
343 /**********************************************************/
347 /* this structure is filled when cmd_uint8_read is parsed successfully */
348 struct cmd_uint8_result {
355 /* function called when cmd_uint8_read is parsed successfully */
356 static void cmd_uint8_read_parsed(void * parsed_result, __attribute__((unused)) void *data)
358 struct cmd_uint8_result *res = parsed_result;
361 uint8_t addr = addr_from_string(res->arg1);
363 ret = ax12_user_read_byte(&gen.ax12, res->num, addr, &val);
365 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
366 printf_P(PSTR("%s: %d [0x%.2x]\r\n"), res->arg1, val, val);
369 prog_char str_uint8_arg0[] = "read";
370 parse_pgm_token_string_t cmd_uint8_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0);
371 prog_char str_uint8_arg1[] = "id#firmware#baudrate#delay#high_lim_temp#"
372 "low_lim_volt#high_lim_volt#status_return#alarm_led#"
373 "alarm_shutdown#torque_enable#led#cw_comp_margin#"
374 "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
375 "voltage#temp#reginst#moving#lock";
376 parse_pgm_token_string_t cmd_uint8_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1);
377 parse_pgm_token_num_t cmd_uint8_num = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, num, UINT8);
379 prog_char help_uint8_read[] = "Read uint8 value (type, num)";
380 parse_pgm_inst_t cmd_uint8_read = {
381 .f = cmd_uint8_read_parsed, /* function to call */
382 .data = NULL, /* 2nd arg of func */
383 .help_str = help_uint8_read,
384 .tokens = { /* token list, NULL terminated */
385 (prog_void *)&cmd_uint8_arg0,
386 (prog_void *)&cmd_uint8_arg1,
387 (prog_void *)&cmd_uint8_num,
392 /* function called when cmd_uint8_write is parsed successfully */
393 static void cmd_uint8_write_parsed(void * parsed_result, __attribute__((unused)) void *data)
395 struct cmd_uint8_result *res = parsed_result;
396 uint8_t addr = addr_from_string(res->arg1);
398 printf_P(PSTR("writing %s: %d [0x%.2x]\r\n"), res->arg1,
400 ret = ax12_user_write_byte(&gen.ax12, res->num, addr, res->val);
402 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
405 prog_char str_uint8_arg0_w[] = "write";
406 parse_pgm_token_string_t cmd_uint8_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0_w);
407 prog_char str_uint8_arg1_w[] = "id#baudrate#delay#high_lim_temp#"
408 "low_lim_volt#high_lim_volt#status_return#alarm_led#"
409 "alarm_shutdown#torque_enable#led#cw_comp_margin#"
410 "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
412 parse_pgm_token_string_t cmd_uint8_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1_w);
413 parse_pgm_token_num_t cmd_uint8_val = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, val, UINT8);
415 prog_char help_uint8_write[] = "Write uint8 value (write, num, val)";
416 parse_pgm_inst_t cmd_uint8_write = {
417 .f = cmd_uint8_write_parsed, /* function to call */
418 .data = NULL, /* 2nd arg of func */
419 .help_str = help_uint8_write,
420 .tokens = { /* token list, NULL terminated */
421 (prog_void *)&cmd_uint8_arg0_w,
422 (prog_void *)&cmd_uint8_arg1_w,
423 (prog_void *)&cmd_uint8_num,
424 (prog_void *)&cmd_uint8_val,