revert test serpi
[aversive.git] / projects / microb2010 / sensorboard / commands_ax12.c
1 /*
2  *  Copyright Droids Corporation (2009)
3  * 
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.
8  *
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.
13  *
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
17  *
18  *  Revision : $Id: commands_ax12.c,v 1.1 2009-03-29 18:44:54 zer0 Exp $
19  *
20  *  Olivier MATZ <zer0@droids-corp.org> 
21  */
22
23 #include <stdio.h>
24 #include <string.h>
25
26 #include <aversive/pgmspace.h>
27 #include <aversive/wait.h>
28 #include <aversive/error.h>
29
30 #include <ax12.h>
31 #include <uart.h>
32 #include <pwm_ng.h>
33 #include <time.h>
34
35 #include <pid.h>
36 #include <quadramp.h>
37 #include <control_system_manager.h>
38 #include <blocking_detection_manager.h>
39
40 #include <rdline.h>
41 #include <parse.h>
42 #include <parse_string.h>
43 #include <parse_num.h>
44
45 #include "main.h"
46
47 uint8_t addr_from_string(const char *s)
48 {
49         /* 16 bits */
50         if (!strcmp_P(s, PSTR("cw_angle_limit")))
51                 return AA_CW_ANGLE_LIMIT_L;
52         if (!strcmp_P(s, PSTR("ccw_angle_limit")))
53                 return AA_CCW_ANGLE_LIMIT_L;
54         if (!strcmp_P(s, PSTR("max_torque")))
55                 return AA_MAX_TORQUE_L;
56         if (!strcmp_P(s, PSTR("down_calibration")))
57                 return AA_DOWN_CALIBRATION_L;
58         if (!strcmp_P(s, PSTR("up_calibration")))
59                 return AA_UP_CALIBRATION_L;
60         if (!strcmp_P(s, PSTR("torque_limit")))
61                 return AA_TORQUE_LIMIT_L;
62         if (!strcmp_P(s, PSTR("position")))
63                 return AA_PRESENT_POSITION_L;
64         if (!strcmp_P(s, PSTR("speed")))
65                 return AA_PRESENT_SPEED_L;
66         if (!strcmp_P(s, PSTR("load")))
67                 return AA_PRESENT_LOAD_L;
68         if (!strcmp_P(s, PSTR("moving_speed")))
69                 return AA_MOVING_SPEED_L;
70         if (!strcmp_P(s, PSTR("model")))
71                 return AA_MODEL_NUMBER_L;
72         if (!strcmp_P(s, PSTR("goal_pos")))
73                 return AA_GOAL_POSITION_L;
74         if (!strcmp_P(s, PSTR("punch")))
75                 return AA_PUNCH_L;
76
77         /* 8 bits */
78         if (!strcmp_P(s, PSTR("firmware")))
79                 return AA_FIRMWARE;
80         if (!strcmp_P(s, PSTR("id")))
81                 return AA_ID;
82         if (!strcmp_P(s, PSTR("baudrate")))
83                 return AA_BAUD_RATE;
84         if (!strcmp_P(s, PSTR("delay")))
85                 return AA_DELAY_TIME;
86         if (!strcmp_P(s, PSTR("high_lim_temp")))
87                 return AA_HIGHEST_LIMIT_TEMP;
88         if (!strcmp_P(s, PSTR("low_lim_volt")))
89                 return AA_LOWEST_LIMIT_VOLTAGE;
90         if (!strcmp_P(s, PSTR("high_lim_volt")))
91                 return AA_HIGHEST_LIMIT_VOLTAGE;
92         if (!strcmp_P(s, PSTR("status_return")))
93                 return AA_STATUS_RETURN_LEVEL;
94         if (!strcmp_P(s, PSTR("alarm_led")))
95                 return AA_ALARM_LED;
96         if (!strcmp_P(s, PSTR("alarm_shutdown")))
97                 return AA_ALARM_SHUTDOWN;
98         if (!strcmp_P(s, PSTR("torque_enable")))
99                 return AA_TORQUE_ENABLE;
100         if (!strcmp_P(s, PSTR("led")))
101                 return AA_LED;
102         if (!strcmp_P(s, PSTR("cw_comp_margin")))
103                 return AA_CW_COMPLIANCE_MARGIN;
104         if (!strcmp_P(s, PSTR("ccw_comp_margin")))
105                 return AA_CCW_COMPLIANCE_MARGIN;
106         if (!strcmp_P(s, PSTR("cw_comp_slope")))
107                 return AA_CW_COMPLIANCE_SLOPE;
108         if (!strcmp_P(s, PSTR("ccw_comp_slope")))
109                 return AA_CCW_COMPLIANCE_SLOPE;
110         if (!strcmp_P(s, PSTR("voltage")))
111                 return AA_PRESENT_VOLTAGE;
112         if (!strcmp_P(s, PSTR("temp")))
113                 return AA_PRESENT_TEMP;
114         if (!strcmp_P(s, PSTR("reginst")))
115                 return AA_PRESENT_REGINST;
116         if (!strcmp_P(s, PSTR("moving")))
117                 return AA_MOVING;
118         if (!strcmp_P(s, PSTR("lock")))
119                 return AA_LOCK;
120         
121         return 0;
122 }
123
124 /**********************************************************/
125 /* Ax12_Stress */
126
127 /* this structure is filled when cmd_ax12_stress is parsed successfully */
128 struct cmd_ax12_stress_result {
129         fixed_string_t arg0;
130         uint8_t id;
131 };
132
133 /* function called when cmd_ax12_stress is parsed successfully */
134 static void cmd_ax12_stress_parsed(void *parsed_result, void *data)
135 {
136         struct cmd_ax12_stress_result *res = parsed_result;
137         int i, nb_errs = 0;
138         uint8_t val;
139         microseconds t = time_get_us2();
140
141         for (i=0; i<1000; i++) {
142                 if (AX12_read_byte(&gen.ax12, res->id, AA_ID, &val) != 0)
143                         nb_errs ++;
144         }
145
146         printf_P(PSTR("%d errors / 1000\r\n"), nb_errs);
147         t = (time_get_us2() - t) / 1000;
148         printf_P(PSTR("Test done in %d ms\r\n"), (int)t);
149 }
150
151 prog_char str_ax12_stress_arg0[] = "ax12_stress";
152 parse_pgm_token_string_t cmd_ax12_stress_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ax12_stress_result, arg0, str_ax12_stress_arg0);
153 parse_pgm_token_num_t cmd_ax12_stress_id = TOKEN_NUM_INITIALIZER(struct cmd_ax12_stress_result, id, UINT8);
154
155 prog_char help_ax12_stress[] = "Stress an AX12 with 1000 'read id' commands";
156 parse_pgm_inst_t cmd_ax12_stress = {
157         .f = cmd_ax12_stress_parsed,  /* function to call */
158         .data = NULL,      /* 2nd arg of func */
159         .help_str = help_ax12_stress,
160         .tokens = {        /* token list, NULL terminated */
161                 (prog_void *)&cmd_ax12_stress_arg0, 
162                 (prog_void *)&cmd_ax12_stress_id, 
163                 NULL,
164         },
165 };
166
167 /**********************************************************/
168
169 /* this structure is filled when cmd_baudrate is parsed successfully */
170 struct cmd_baudrate_result {
171         fixed_string_t arg0;
172         uint32_t arg1;
173 };
174
175 /* function called when cmd_baudrate is parsed successfully */
176 static void cmd_baudrate_parsed(void * parsed_result, void * data)
177 {
178         struct cmd_baudrate_result *res = parsed_result;
179         struct uart_config c;
180
181         printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L);
182         uart_getconf(1, &c);
183         c.baudrate = res->arg1;
184         uart_setconf(1, &c);
185         printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L);
186 }
187
188 prog_char str_baudrate_arg0[] = "baudrate";
189 parse_pgm_token_string_t cmd_baudrate_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_baudrate_result, arg0, str_baudrate_arg0);
190 parse_pgm_token_num_t cmd_baudrate_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_baudrate_result, arg1, UINT32);
191
192 prog_char help_baudrate[] = "Change ax12 baudrate";
193 parse_pgm_inst_t cmd_baudrate = {
194         .f = cmd_baudrate_parsed,  /* function to call */
195         .data = NULL,      /* 2nd arg of func */
196         .help_str = help_baudrate,
197         .tokens = {        /* token list, NULL terminated */
198                 (prog_void *)&cmd_baudrate_arg0, 
199                 (prog_void *)&cmd_baudrate_arg1, 
200                 NULL,
201         },
202 };
203
204 /**********************************************************/
205 /* Uint16 */
206
207
208 /* this structure is filled when cmd_uint16_read is parsed successfully */
209 struct cmd_uint16_result {
210         fixed_string_t arg0;
211         fixed_string_t arg1;
212         uint8_t num;
213         uint16_t val;
214 };
215
216 /* function called when cmd_uint16_read is parsed successfully */
217 static void cmd_uint16_read_parsed(void * parsed_result, void * data)
218 {
219         struct cmd_uint16_result *res = parsed_result;
220         uint8_t ret;
221         uint16_t val;
222         uint8_t addr = addr_from_string(res->arg1);
223         ret = AX12_read_int(&gen.ax12, res->num, addr, &val);
224         if (ret)
225                 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
226         printf_P(PSTR("%s: %d [0x%.4x]\r\n"), res->arg1, val, val);
227 }
228
229 prog_char str_uint16_arg0[] = "read";
230 parse_pgm_token_string_t cmd_uint16_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0);
231 prog_char str_uint16_arg1[] = "moving_speed#model#goal_pos#cw_angle_limit#ccw_angle_limit#"
232                 "max_torque#down_calibration#up_calibration#torque_limit#"
233                 "position#speed#load#punch";
234 parse_pgm_token_string_t cmd_uint16_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1);
235 parse_pgm_token_num_t cmd_uint16_num = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, num, UINT8);
236
237 prog_char help_uint16_read[] = "Read uint16 value (type, num)";
238 parse_pgm_inst_t cmd_uint16_read = {
239         .f = cmd_uint16_read_parsed,  /* function to call */
240         .data = NULL,      /* 2nd arg of func */
241         .help_str = help_uint16_read,
242         .tokens = {        /* token list, NULL terminated */
243                 (prog_void *)&cmd_uint16_arg0,
244                 (prog_void *)&cmd_uint16_arg1,
245                 (prog_void *)&cmd_uint16_num,
246                 NULL,
247         },
248 };
249
250 /* function called when cmd_uint16_write is parsed successfully */
251 static void cmd_uint16_write_parsed(void * parsed_result, void * data)
252 {
253         struct cmd_uint16_result *res = parsed_result;
254         uint8_t ret;
255         uint8_t addr = addr_from_string(res->arg1);
256         printf_P(PSTR("writing %s: %d [0x%.4x]\r\n"), res->arg1,
257                  res->val, res->val);
258         ret = AX12_write_int(&gen.ax12, res->num, addr, res->val);
259         if (ret)
260                 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
261 }
262
263 prog_char str_uint16_arg0_w[] = "write";
264 parse_pgm_token_string_t cmd_uint16_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0_w);
265 prog_char str_uint16_arg1_w[] = "moving_speed#goal_pos#cw_angle_limit#ccw_angle_limit#"
266                 "max_torque#torque_limit#punch";
267 parse_pgm_token_string_t cmd_uint16_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1_w);
268 parse_pgm_token_num_t cmd_uint16_val = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, val, UINT16);
269
270 prog_char help_uint16_write[] = "Write uint16 value (write, num, val)";
271 parse_pgm_inst_t cmd_uint16_write = {
272         .f = cmd_uint16_write_parsed,  /* function to call */
273         .data = NULL,      /* 2nd arg of func */
274         .help_str = help_uint16_write,
275         .tokens = {        /* token list, NULL terminated */
276                 (prog_void *)&cmd_uint16_arg0_w,
277                 (prog_void *)&cmd_uint16_arg1_w,
278                 (prog_void *)&cmd_uint16_num,
279                 (prog_void *)&cmd_uint16_val,
280                 NULL,
281         },
282 };
283
284 /**********************************************************/
285 /* Uint8 */
286
287
288 /* this structure is filled when cmd_uint8_read is parsed successfully */
289 struct cmd_uint8_result {
290         fixed_string_t arg0;
291         fixed_string_t arg1;
292         uint8_t num;
293         uint8_t val;
294 };
295
296 /* function called when cmd_uint8_read is parsed successfully */
297 static void cmd_uint8_read_parsed(void * parsed_result, void * data)
298 {
299         struct cmd_uint8_result *res = parsed_result;
300         uint8_t ret;
301         uint8_t val;
302         uint8_t addr = addr_from_string(res->arg1);
303
304         ret = AX12_read_byte(&gen.ax12, res->num, addr, &val);
305         if (ret)
306                 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
307         printf_P(PSTR("%s: %d [0x%.2x]\r\n"), res->arg1, val, val);
308 }
309
310 prog_char str_uint8_arg0[] = "read";
311 parse_pgm_token_string_t cmd_uint8_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0);
312 prog_char str_uint8_arg1[] = "id#firmware#baudrate#delay#high_lim_temp#"
313                 "low_lim_volt#high_lim_volt#status_return#alarm_led#"
314                 "alarm_shutdown#torque_enable#led#cw_comp_margin#"
315                 "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
316                 "voltage#temp#reginst#moving#lock";
317 parse_pgm_token_string_t cmd_uint8_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1);
318 parse_pgm_token_num_t cmd_uint8_num = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, num, UINT8);
319
320 prog_char help_uint8_read[] = "Read uint8 value (type, num)";
321 parse_pgm_inst_t cmd_uint8_read = {
322         .f = cmd_uint8_read_parsed,  /* function to call */
323         .data = NULL,      /* 2nd arg of func */
324         .help_str = help_uint8_read,
325         .tokens = {        /* token list, NULL terminated */
326                 (prog_void *)&cmd_uint8_arg0,
327                 (prog_void *)&cmd_uint8_arg1,
328                 (prog_void *)&cmd_uint8_num,
329                 NULL,
330         },
331 };
332
333 /* function called when cmd_uint8_write is parsed successfully */
334 static void cmd_uint8_write_parsed(void * parsed_result, void * data)
335 {
336         struct cmd_uint8_result *res = parsed_result;
337         uint8_t addr = addr_from_string(res->arg1);
338         uint8_t ret;
339         printf_P(PSTR("writing %s: %d [0x%.2x]\r\n"), res->arg1, 
340                  res->val, res->val);
341         ret = AX12_write_byte(&gen.ax12, res->num, addr, res->val);
342         if (ret)
343                 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
344 }
345
346 prog_char str_uint8_arg0_w[] = "write";
347 parse_pgm_token_string_t cmd_uint8_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0_w);
348 prog_char str_uint8_arg1_w[] = "id#baudrate#delay#high_lim_temp#"
349                 "low_lim_volt#high_lim_volt#status_return#alarm_led#"
350                 "alarm_shutdown#torque_enable#led#cw_comp_margin#"
351                 "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
352                 "reginst#lock";
353 parse_pgm_token_string_t cmd_uint8_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1_w);
354 parse_pgm_token_num_t cmd_uint8_val = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, val, UINT8);
355
356 prog_char help_uint8_write[] = "Write uint8 value (write, num, val)";
357 parse_pgm_inst_t cmd_uint8_write = {
358         .f = cmd_uint8_write_parsed,  /* function to call */
359         .data = NULL,      /* 2nd arg of func */
360         .help_str = help_uint8_write,
361         .tokens = {        /* token list, NULL terminated */
362                 (prog_void *)&cmd_uint8_arg0_w,
363                 (prog_void *)&cmd_uint8_arg1_w,
364                 (prog_void *)&cmd_uint8_num,
365                 (prog_void *)&cmd_uint8_val,
366                 NULL,
367         },
368 };