better cobboard commands in strat_event
[aversive.git] / projects / microb2010 / cobboard / i2c_protocol.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: i2c_protocol.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $
19  *
20  */
21
22 #include <string.h>
23
24 #include <aversive.h>
25 #include <aversive/list.h>
26 #include <aversive/error.h>
27
28 #include <i2c.h>
29 #include <ax12.h>
30 #include <uart.h>
31 #include <pwm_ng.h>
32 #include <clock_time.h>
33 #include <spi.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 "../common/i2c_commands.h"
46 #include "main.h"
47 #include "sensor.h"
48 #include "state.h"
49
50 void i2c_protocol_init(void)
51 {
52 }
53
54 /*** LED CONTROL ***/
55 void i2c_led_control(uint8_t l, uint8_t state)
56 {
57         switch(l) {
58         case 1:
59                 state? LED1_ON():LED1_OFF();
60                 break;
61         case 2:
62                 state? LED2_ON():LED2_OFF();
63                 break;
64         case 3:
65                 state? LED3_ON():LED3_OFF();
66                 break;
67         case 4:
68                 state? LED4_ON():LED4_OFF();
69                 break;
70         default:
71                 break;
72         }
73 }
74
75 #ifdef notyet
76 static void i2c_test(uint16_t val)
77 {
78         static uint16_t prev=0;
79
80         if ( (val-prev) != 1 ) {
81                 WARNING(E_USER_I2C_PROTO, "Dupplicated msg %d", val);
82         }
83         prev = val;
84         ext.nb_test_cmd ++;
85 }
86 #endif
87
88 static void i2c_send_status(void)
89 {
90         struct i2c_ans_cobboard_status ans;
91         i2c_flush();
92         ans.hdr.cmd =  I2C_ANS_COBBOARD_STATUS;
93
94         /* status */
95         ans.mode = state_get_mode();
96         ans.status = state_get_status();
97
98         ans.left_cobroller_speed = cobboard.left_cobroller_speed;
99         ans.right_cobroller_speed = cobboard.right_cobroller_speed;
100
101         ans.cob_count = state_get_cob_count();
102
103         i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans,
104                  sizeof(ans), I2C_CTRL_GENERIC);
105 }
106
107 void i2c_recvevent(uint8_t * buf, int8_t size)
108 {
109         void *void_cmd = buf;
110
111         static uint8_t a = 0;
112
113         a++;
114         if (a & 0x10)
115                 LED2_TOGGLE();
116
117         if (size <= 0) {
118                 goto error;
119         }
120
121         switch (buf[0]) {
122
123         /* Commands (no answer needed) */
124         case I2C_CMD_GENERIC_LED_CONTROL:
125                 {
126                         struct i2c_cmd_led_control *cmd = void_cmd;
127                         if (size != sizeof (*cmd))
128                                 goto error;
129                         i2c_led_control(cmd->led_num, cmd->state);
130                         break;
131                 }
132
133         case I2C_CMD_COBBOARD_SET_MODE:
134                 {
135                         struct i2c_cmd_cobboard_set_mode *cmd = void_cmd;
136                         if (size != sizeof(struct i2c_cmd_cobboard_set_mode))
137                                 goto error;
138                         if (!state_get_i2c_ignore())
139                                 state_set_mode(cmd->mode);
140                         break;
141                 }
142
143         case I2C_CMD_GENERIC_SET_COLOR:
144                 {
145                         struct i2c_cmd_generic_color *cmd = void_cmd;
146                         if (size != sizeof (*cmd))
147                                 goto error;
148                         cobboard.our_color = cmd->color;
149                         break;
150                 }
151
152 #ifdef notyet
153         case I2C_CMD_EXTENSION_TEST:
154                 {
155                         struct i2c_cmd_extension_test *cmd = void_cmd;
156                         if (size != sizeof (*cmd))
157                                 goto error;
158                         i2c_test(cmd->val);
159                         break;
160                 }
161 #endif
162
163         /* Add other commands here ...*/
164
165         case I2C_REQ_COBBOARD_STATUS:
166                 {
167                         struct i2c_req_cobboard_status *cmd = void_cmd;
168                         if (size != sizeof (struct i2c_req_cobboard_status))
169                                 goto error;
170
171                         /* mode is in req */
172                         if (state_get_status() != I2C_COBBOARD_STATUS_OFF &&
173                             !state_get_i2c_ignore()) {
174                                 state_set_spickle(I2C_LEFT_SIDE, cmd->lspickle);
175                                 state_set_spickle(I2C_RIGHT_SIDE, cmd->rspickle);
176                         }
177                         i2c_send_status();
178                         break;
179                 }
180
181         default:
182                 goto error;
183         }
184
185  error:
186         /* log error on a led ? */
187         return;
188 }
189
190 void i2c_recvbyteevent(__attribute__((unused)) uint8_t hwstatus,
191                        __attribute__((unused)) uint8_t i, 
192                        __attribute__((unused)) int8_t c)
193 {
194 }
195
196 void i2c_sendevent(__attribute__((unused)) int8_t size)
197 {
198 }
199
200