Statistics
| Revision:

root / trunk / code / projects / diagnostic_station / station / comm_robot.c @ 1298

History | View | Annotate | Download (9.37 KB)

1
#include <dragonfly_lib.h>
2
#include <wireless.h>
3
#include <string.h>
4

    
5
#include "global.h"
6
#include "comm_robot.h"
7
#include "../common/comm_station_robot.h"
8

    
9

    
10
#define received_state_waiting 0
11
#define received_state_pre_timeout 1
12
#define received_state_timeout 2
13
#define received_state_received 3
14
#define received_state_uninitialized 4
15

    
16
#define MAX_PACKET_LENGTH 32
17

    
18
uint8_t received_buffer[MAX_PACKET_LENGTH]; // Maximum length of a packet to be handled
19
uint8_t received_length;
20
uint8_t received_state;
21

    
22

    
23

    
24
// *********************
25
// ** Motor direction **
26
// *********************
27

    
28
/**
29
  * All the functions in this file use these motor settings: a pair (direction/velocity) maps to a pair motor_direction/
30
  * motor_velocity. motor_direction and motor_velocity are the values passed to the library functions and also sent
31
  * to the robot. direction also includes an "off" direction. If the direction is off, the motor_velocity is always 0.\
32
  **/
33

    
34
char *motor_direction_string (uint8_t direction)
35
{
36
        if (direction==motor_direction_forward)
37
                return "forward";
38
        else if (direction==motor_direction_backward)
39
                return "backward";
40
        else if (direction==motor_direction_off)
41
                return "off";
42
        else
43
                return "?";
44
}
45

    
46
static uint8_t motor_direction (uint8_t direction, uint8_t velocity)
47
{
48
        if (direction==motor_direction_backward)
49
                return BACKWARD;
50
        else
51
                return FORWARD;
52
}
53

    
54
static uint8_t motor_velocity (uint8_t direction, uint8_t velocity)
55
{
56
        if (direction==motor_direction_off)
57
                return 0;
58
        else
59
                return velocity;
60
}
61

    
62

    
63
// ***************************
64
// ** Common comm functions        **
65
// ***************************
66

    
67
static void send_packet (char type, char *data, int len)
68
{
69
        // Right now we only wait until we receive *any* packet (with the appropriate group), so the functionality for
70
        // sending command packets and request packets is the same.
71
        // Note that this will horribly fail if other people are using the same channel/group.
72
        
73
        // Keep sending the packet until we get a reply (retry on timeout)
74
        do
75
        {
76
                received_state=received_state_waiting;
77
        
78
                // Send the request packet
79
                wl_send_global_packet (station_robot_group, type, data, len, 0);
80
                
81
                // When the reply is received, the receive handler will set received_state to recieved_state_received. Wait
82
                // until this happens, periodically calling the wl_do function to handle incoming messages.
83
                
84
                // Keep polling the wireless until the state is either "received" or "timeout" (as set by the callbacks)
85
                while (received_state!=received_state_received && received_state!=received_state_timeout)
86
                        wl_do();
87
                        
88
                if (received_state==received_state_timeout)
89
                        usb_puts ("# Timeout, retrying" NL);
90
        }
91
        while (received_state==received_state_timeout);
92
}
93

    
94
// A packet that is followed by a "done" packet from the robot.
95
static void send_command_packet (char type, char *data, int len)
96
{
97
        send_packet (type, data, len);
98
}
99

    
100
// A packet that is followed by a data packet from the robot
101
static void send_request_packet(char type, char *data, int len)
102
{
103
        send_packet (type, data, len);
104
}
105

    
106

    
107
// *************************
108
// ** Individual commands **
109
// *************************
110

    
111
void robot_set_orbs (uint8_t red1, uint8_t green1, uint8_t blue1, uint8_t red2, uint8_t green2, uint8_t blue2)
112
{
113
        char data[6];
114

    
115
        data[0]=red1;
116
        data[1]=green1;
117
        data[2]=blue1;
118
        
119
        data[3]=red2;
120
        data[4]=green2;
121
        data[5]=blue2;
122

    
123
        send_command_packet (station_robot_set_orbs, data, 6);
124
}
125

    
126
/** Direction is motor_direction_off/forward/backward, not FORWARD/BACKWARD */
127
void robot_set_motors (uint8_t direction1, uint8_t speed1, uint8_t direction2, uint8_t speed2)
128
{
129
        char data[4];
130
        
131
        data[0]=motor_direction (direction1, speed1);
132
        data[1]=motor_velocity  (direction1, speed1);
133
        data[2]=motor_direction (direction2, speed2);
134
        data[3]=motor_velocity  (direction2, speed2);
135
        
136
        send_command_packet (station_robot_set_motors, data, 4);
137
}
138

    
139
/** Direction is direction_off/forward/backward, not FORWARD/BACKWARD */
140
void robot_set_motors_time (uint8_t direction1, uint8_t speed1, uint8_t direction2, uint8_t speed2, uint16_t time_ms)
141
{
142
        char data[6];
143
        
144
        data[0]=motor_direction (direction1, speed1);
145
        data[1]=motor_velocity  (direction1, speed1);
146
        data[2]=motor_direction (direction2, speed1);
147
        data[3]=motor_velocity  (direction2, speed2);
148
        data[4]=WORD_BYTE_0(time_ms);
149
        data[5]=WORD_BYTE_1(time_ms);
150
        
151
        send_command_packet (station_robot_set_motors_time, data, 6);
152
}
153

    
154
void robot_set_motors_off (void)
155
{
156
        send_command_packet (station_robot_set_motors_off, NULL, 0);
157
}
158

    
159
void robot_set_bom (uint16_t bitmask)
160
{
161
        char data[2];
162
        
163
        data[0]=WORD_BYTE_0(bitmask);
164
        data[1]=WORD_BYTE_1(bitmask);
165
        
166
        send_command_packet (station_robot_set_bom, data, 2);
167
}
168

    
169
void robot_reset_encoders (void)
170
{
171
        send_command_packet (station_robot_reset_encoders, NULL, 0);
172
}
173

    
174

    
175
// *************************
176
// ** Individual requests **
177
// *************************
178

    
179
bool robot_read_encoders (int16_t *left, int16_t *right)
180
{
181
        send_request_packet (station_robot_read_encoders, NULL, 0);
182
        
183
        // Now the received data is in received_buffer/received_length
184
        if (received_length>=4)
185
        {
186
                *left =WORD(received_buffer[0], received_buffer[1]);
187
                *right=WORD(received_buffer[2], received_buffer[3]);
188
                return true;
189
        }
190
        else
191
        {
192
                return false;
193
        }
194
}
195

    
196
bool robot_read_bom (uint8_t num, uint16_t *value)
197
{
198
        send_request_packet(station_robot_read_bom, (char *)&num, 1);
199
        
200
        // Now the received data is in received_buffer/received_length
201
        if(received_length >= 2)
202
        {
203
                *value = WORD(received_buffer[0], received_buffer[1]);
204
                return true;
205
        }
206
        else
207
        {
208
                return false;
209
        }
210

    
211
}
212

    
213
bool robot_read_rangefinder (uint8_t num, uint16_t *value)
214
{
215
        send_request_packet(station_robot_read_rangefinder, (char *)&num, 1);
216
        
217
        // Now the received data is in received_buffer/received_length
218
        if(received_length >= 2)
219
        {
220
                *value = WORD(received_buffer[0], received_buffer[1]);
221
                return true;
222
        }
223
        else
224
        {
225
                return false;
226
        }
227
}
228

    
229
bool robot_read_bom_all (int16_t *value)
230
{
231
        send_request_packet(station_robot_read_bom_all, NULL, 0);
232
        
233
        // Now the received data is in received_buffer/received_length
234
        if(received_length >= 32)
235
        {
236
                value[0] = WORD(received_buffer[0], received_buffer[1]);
237
                value[1] = WORD(received_buffer[2], received_buffer[3]);
238
                value[2] = WORD(received_buffer[4], received_buffer[5]);
239
                value[3] = WORD(received_buffer[6], received_buffer[7]);
240
                value[4] = WORD(received_buffer[8], received_buffer[9]);
241
                value[5] = WORD(received_buffer[10], received_buffer[11]);
242
                value[6] = WORD(received_buffer[12], received_buffer[13]);
243
                value[7] = WORD(received_buffer[14], received_buffer[15]);
244
                value[8] = WORD(received_buffer[16], received_buffer[17]);
245
                value[9] = WORD(received_buffer[18], received_buffer[19]);
246
                value[10] = WORD(received_buffer[20], received_buffer[21]);
247
                value[11] = WORD(received_buffer[22], received_buffer[23]);
248
                value[12] = WORD(received_buffer[24], received_buffer[25]);
249
                value[13] = WORD(received_buffer[26], received_buffer[27]);
250
                value[14] = WORD(received_buffer[28], received_buffer[29]);
251
                value[15] = WORD(received_buffer[30], received_buffer[31]);
252
                return true;
253
        }
254
        else
255
        {
256
                return false;
257
        }
258
}
259

    
260

    
261
// ***********************
262
// ** Receive callbacks **
263
// ***********************
264

    
265
static void robot_station_receive (char type, int source, unsigned char* packet, int length)
266
{
267
        // Dump the raw packet
268
        //usb_puts ("# [");
269
        //for (uint8_t i=0; i<length; ++i)
270
        //{
271
        //        if (i!=0) usb_putc (' ');
272
        //        usb_puti (packet[i]);
273
        //}
274
        //usb_putc (']');
275
        
276
        // We received some data from the robot. It is not handled immediately as some function is waiting for it.
277
        
278
        // Note the fact that we received some data
279
        received_state=received_state_received;
280
        
281
        // The packet buffer might be overwritten after this function returns, so we have to make a copy of the data.
282
        
283
        // First, determine the number of bytes to copy. It is the minimum of the received length and the size of the data
284
        // buffer the data is copied to. If the received length is greater than the buffer size, print an error message.
285
        // We could stop here (while (1);), but then any long packet would stop the program, even if it's unrelated to
286
        // what we're doing.
287
        received_length=length;
288
        if (sizeof (received_buffer)<received_length)
289
        {
290
                received_length=sizeof (received_buffer);
291
                usb_puts ("# Error: received packet is too long for allocated buffer." NL);
292
        }
293
        
294
        // Now copy the data
295
        memcpy (received_buffer, packet, received_length);
296
}
297

    
298
static void robot_station_timeout (void)
299
{
300
        if      (received_state==received_state_waiting    ) { received_state=received_state_pre_timeout; }
301
        else if (received_state==received_state_pre_timeout) { received_state=received_state_timeout    ; }
302
}
303

    
304
// ********************
305
// ** Initialization **
306
// ********************
307

    
308
// Must not be a local variable because the library doesn't make a copy of it.
309
PacketGroupHandler receive_handler={robot_station_group, robot_station_timeout, NULL, &robot_station_receive, NULL};
310

    
311
void comm_robot_init ()
312
{
313
        received_state=received_state_uninitialized;
314
        received_length=0;
315

    
316
        usb_puts("# Initializing robot communication" NL);
317
        xbee_init ();
318
        wl_init();
319
        usb_puts("# Done" NL);
320
        
321
        //PacketGroupHandler xxx_receive_handler = {robot_station_group, NULL, NULL, &robot_station_receive, NULL};
322
        wl_register_packet_group(&receive_handler);
323
}