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: i2c_protocol.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
25 #include <aversive/pgmspace.h>
26 #include <aversive/wait.h>
27 #include <aversive/error.h>
33 #include <clock_time.h>
37 #include <control_system_manager.h>
38 #include <trajectory_manager.h>
39 #include <vect_base.h>
42 #include <obstacle_avoidance.h>
43 #include <blocking_detection_manager.h>
44 #include <robot_system.h>
45 #include <position_manager.h>
49 #include <parse_string.h>
50 #include <parse_num.h>
52 #include "../common/i2c_commands.h"
55 #include "i2c_protocol.h"
57 #define I2C_STATE_MAX 4
59 #define I2C_TIMEOUT 100 /* ms */
60 #define I2C_MAX_ERRORS 40
62 static volatile uint8_t i2c_poll_num = 0;
63 static volatile uint8_t i2c_state = 0;
64 static volatile uint16_t i2c_errors = 0;
66 #define OP_READY 0 /* no i2c op running */
67 #define OP_POLL 1 /* a user command is running */
68 #define OP_CMD 2 /* a polling (req / ans) is running */
70 static volatile uint8_t running_op = OP_READY;
74 static uint8_t error_log = 0;
76 static int8_t i2c_req_cobboard_status(void);
77 static int8_t i2c_req_ballboard_status(void);
81 /* used for commands */
82 uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
83 volatile int8_t command_dest=-1;
84 volatile uint8_t command_size=0;
86 #define I2C_ERROR(args...) do { \
87 if (error_log < I2C_MAX_LOG) { \
88 ERROR(E_USER_I2C_PROTO, args); \
90 if (error_log == I2C_MAX_LOG) { \
91 ERROR(E_USER_I2C_PROTO, \
92 "i2c logs are now warnings"); \
96 WARNING(E_USER_I2C_PROTO, args); \
99 void i2c_protocol_init(void)
103 void i2c_protocol_debug(void)
108 printf_P(PSTR("I2C protocol debug infos:\r\n"));
109 printf_P(PSTR(" i2c_state=%d\r\n"), i2c_state);
110 printf_P(PSTR(" i2c_errors=%d\r\n"), i2c_errors);
111 printf_P(PSTR(" running_op=%d\r\n"), running_op);
112 printf_P(PSTR(" command_size=%d\r\n"), command_size);
113 printf_P(PSTR(" command_dest=%d\r\n"), command_dest);
114 printf_P(PSTR(" i2c_status=%x\r\n"), i2c_status());
119 static void i2cproto_next_state(uint8_t inc)
122 if (i2c_state >= I2C_STATE_MAX) {
128 void i2cproto_wait_update(void)
131 poll_num = i2c_poll_num;
132 WAIT_COND_OR_TIMEOUT((i2c_poll_num-poll_num) > 1, 150);
135 /* called periodically : the goal of this 'thread' is to send requests
136 * and read answers on i2c slaves in the correct order. */
137 void i2c_poll_slaves(void *dummy)
141 static uint8_t a = 0;
147 /* already running */
149 if (running_op != OP_READY) {
154 /* if a command is ready to be sent, so send it */
157 err = i2c_send(command_dest, command_buf, command_size,
165 /* no command, so do the polling */
166 running_op = OP_POLL;
170 /* poll status of cobboard */
171 #define I2C_REQ_COBBOARD 0
172 case I2C_REQ_COBBOARD:
173 if ((err = i2c_req_cobboard_status()))
177 #define I2C_ANS_COBBOARD 1
178 case I2C_ANS_COBBOARD:
179 if ((err = i2c_recv(I2C_COBBOARD_ADDR,
180 sizeof(struct i2c_ans_cobboard_status),
185 /* poll status of ballboard */
186 #define I2C_REQ_BALLBOARD 2
187 case I2C_REQ_BALLBOARD:
188 if ((err = i2c_req_ballboard_status()))
192 #define I2C_ANS_BALLBOARD 3
193 case I2C_ANS_BALLBOARD:
194 if ((err = i2c_recv(I2C_BALLBOARD_ADDR,
195 sizeof(struct i2c_ans_ballboard_status),
200 /* nothing, go to the first request */
203 running_op = OP_READY;
210 running_op = OP_READY;
213 if (i2c_errors > I2C_MAX_ERRORS) {
214 I2C_ERROR("I2C send is_cmd=%d proto_state=%d "
215 "err=%d i2c_status=%x", !!command_size, i2c_state, err, i2c_status());
221 /* called when the xmit is finished */
222 void i2c_sendevent(int8_t size)
225 if (running_op == OP_POLL) {
226 i2cproto_next_state(1);
233 NOTICE(E_USER_I2C_PROTO, "send error state=%d size=%d "
234 "op=%d", i2c_state, size, running_op);
235 if (i2c_errors > I2C_MAX_ERRORS) {
236 I2C_ERROR("I2C error, slave not ready");
241 if (running_op == OP_POLL) {
242 /* skip associated answer */
243 i2cproto_next_state(2);
246 running_op = OP_READY;
249 /* called rx event */
250 void i2c_recvevent(uint8_t * buf, int8_t size)
252 if (running_op == OP_POLL)
253 i2cproto_next_state(1);
255 /* recv is only trigged after a poll */
256 running_op = OP_READY;
264 case I2C_ANS_COBBOARD_STATUS: {
265 struct i2c_ans_cobboard_status * ans =
266 (struct i2c_ans_cobboard_status *)buf;
268 if (size != sizeof (*ans))
272 cobboard.mode = ans->mode;
273 cobboard.status = ans->status;
274 cobboard.left_cobroller_speed = ans->left_cobroller_speed;
275 cs_set_consign(&mainboard.left_cobroller.cs, cobboard.left_cobroller_speed);
276 cobboard.right_cobroller_speed = ans->right_cobroller_speed;
277 cs_set_consign(&mainboard.right_cobroller.cs, cobboard.right_cobroller_speed);
282 case I2C_ANS_BALLBOARD_STATUS: {
283 struct i2c_ans_ballboard_status * ans =
284 (struct i2c_ans_ballboard_status *)buf;
286 if (size != sizeof (*ans))
288 ballboard.mode = ans->mode;
289 ballboard.status = ans->status;
290 ballboard.ball_count = ans->ball_count;
301 NOTICE(E_USER_I2C_PROTO, "recv error state=%d op=%d",
302 i2c_state, running_op);
303 if (i2c_errors > I2C_MAX_ERRORS) {
304 I2C_ERROR("I2C error, slave not ready");
310 void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c)
313 #endif /* !HOST_VERSION */
315 /* ******** ******** ******** ******** */
317 /* ******** ******** ******** ******** */
321 i2c_send_command(uint8_t addr, uint8_t * buf, uint8_t size)
327 microseconds us = time_get_us2();
329 while ((time_get_us2() - us) < (I2C_TIMEOUT)*1000L) {
331 if (command_size == 0) {
332 memcpy(command_buf, buf, size);
340 /* this should not happen... except if we are called from an
341 * interrupt context, but it's forbidden */
342 I2C_ERROR("I2C command send failed");
348 static int8_t i2c_req_cobboard_status(void)
350 struct i2c_req_cobboard_status buf;
353 buf.hdr.cmd = I2C_REQ_COBBOARD_STATUS;
354 err = i2c_send(I2C_COBBOARD_ADDR, (uint8_t*)&buf,
355 sizeof(buf), I2C_CTRL_GENERIC);
360 static int8_t i2c_req_ballboard_status(void)
362 struct i2c_req_ballboard_status buf;
364 buf.hdr.cmd = I2C_REQ_BALLBOARD_STATUS;
365 return i2c_send(I2C_BALLBOARD_ADDR, (uint8_t*)&buf,
366 sizeof(buf), I2C_CTRL_GENERIC);
368 #endif /* !HOST_VERSION */
370 int8_t i2c_set_color(uint8_t addr, uint8_t color)
372 struct i2c_cmd_generic_color buf;
374 if (addr == I2C_BALLBOARD_ADDR)
375 return 0; /* XXX disabled for now */
376 buf.hdr.cmd = I2C_CMD_GENERIC_SET_COLOR;
378 return i2c_send_command(addr, (uint8_t*)&buf, sizeof(buf));
381 int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state)
383 struct i2c_cmd_led_control buf;
384 buf.hdr.cmd = I2C_CMD_GENERIC_LED_CONTROL;
387 return i2c_send_command(addr, (uint8_t*)&buf, sizeof(buf));
390 int8_t i2c_cobboard_mode_eject(void)
392 struct i2c_cmd_cobboard_set_mode buf;
393 buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
394 buf.mode = cobboard.mode | I2C_COBBOARD_MODE_EJECT;
395 return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
398 int8_t i2c_cobboard_mode_harvest(uint8_t side)
400 struct i2c_cmd_cobboard_set_mode buf;
401 uint8_t mode = cobboard.mode;
403 buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
404 if (side == I2C_LEFT_SIDE) {
405 mode |= I2C_COBBOARD_MODE_L_DEPLOY;
406 mode |= I2C_COBBOARD_MODE_L_HARVEST;
409 mode |= I2C_COBBOARD_MODE_R_DEPLOY;
410 mode |= I2C_COBBOARD_MODE_R_HARVEST;
413 return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
416 int8_t i2c_cobboard_mode_deploy(uint8_t side)
418 struct i2c_cmd_cobboard_set_mode buf;
419 uint8_t mode = cobboard.mode;
421 buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
422 if (side == I2C_LEFT_SIDE) {
423 mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST);
424 mode |= I2C_COBBOARD_MODE_L_DEPLOY;
427 mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST);
428 mode |= I2C_COBBOARD_MODE_R_DEPLOY;
431 return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
434 int8_t i2c_cobboard_mode_pack(uint8_t side)
436 struct i2c_cmd_cobboard_set_mode buf;
437 uint8_t mode = cobboard.mode;
439 buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
440 if (side == I2C_LEFT_SIDE)
441 mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST);
443 mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST);
445 return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
448 int8_t i2c_cobboard_mode_init(void)
450 struct i2c_cmd_cobboard_set_mode buf;
451 buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
452 buf.mode = I2C_COBBOARD_MODE_INIT;
453 return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
456 int8_t i2c_ballboard_set_mode(uint8_t mode)
458 struct i2c_cmd_ballboard_set_mode buf;
459 buf.hdr.cmd = I2C_CMD_BALLBOARD_SET_MODE;
461 return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));