root / trunk / code / projects / diagnostic_station / station / comm_robot.c @ 1226
History | View | Annotate | Download (7.3 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 |
uint8_t received_buffer[10]; // Maximum length of a packet to be handled |
17 |
uint8_t received_length; |
18 |
uint8_t received_state; |
19 |
|
20 |
|
21 |
|
22 |
// *********************
|
23 |
// ** Motor direction **
|
24 |
// *********************
|
25 |
|
26 |
/**
|
27 |
* All the functions in this file use these motor settings: a pair (direction/velocity) maps to a pair motor_direction/
|
28 |
* motor_velocity. motor_direction and motor_velocity are the values passed to the library functions and also sent
|
29 |
* to the robot. direction also includes an "off" direction. If the direction is off, the motor_velocity is always 0.\
|
30 |
**/
|
31 |
|
32 |
char *motor_direction_string (uint8_t direction)
|
33 |
{ |
34 |
if (direction==motor_direction_forward)
|
35 |
return "forward"; |
36 |
else if (direction==motor_direction_backward) |
37 |
return "backward"; |
38 |
else if (direction==motor_direction_off) |
39 |
return "off"; |
40 |
else
|
41 |
return "?"; |
42 |
} |
43 |
|
44 |
static uint8_t motor_direction (uint8_t direction, uint8_t velocity)
|
45 |
{ |
46 |
if (direction==motor_direction_backward)
|
47 |
return BACKWARD;
|
48 |
else
|
49 |
return FORWARD;
|
50 |
} |
51 |
|
52 |
static uint8_t motor_velocity (uint8_t direction, uint8_t velocity)
|
53 |
{ |
54 |
if (direction==motor_direction_off)
|
55 |
return 0; |
56 |
else
|
57 |
return velocity;
|
58 |
} |
59 |
|
60 |
|
61 |
// ***************************
|
62 |
// ** Common comm functions **
|
63 |
// ***************************
|
64 |
|
65 |
static void send_packet (char type, char *data, int len) |
66 |
{ |
67 |
// Right now we only wait until we receive *any* packet (with the appropriate group), so the functionality for
|
68 |
// sending command packets and request packets is the same.
|
69 |
// Note that this will horribly fail if other people are using the same channel/group.
|
70 |
|
71 |
// Keep sending the packet until we get a reply (retry on timeout)
|
72 |
do
|
73 |
{ |
74 |
received_state=received_state_waiting; |
75 |
|
76 |
// Send the request packet
|
77 |
wl_send_global_packet (station_robot_group, type, data, len, 0);
|
78 |
|
79 |
// When the reply is received, the receive handler will set received_state to recieved_state_received. Wait
|
80 |
// until this happens, periodically calling the wl_do function to handle incoming messages.
|
81 |
|
82 |
// Keep polling the wireless until the state is either "received" or "timeout" (as set by the callbacks)
|
83 |
while (received_state!=received_state_received && received_state!=received_state_timeout)
|
84 |
wl_do(); |
85 |
|
86 |
if (received_state==received_state_timeout)
|
87 |
usb_puts ("# Timeout, retrying" NL);
|
88 |
} |
89 |
while (received_state==received_state_timeout);
|
90 |
} |
91 |
|
92 |
// A packet that is followed by a "done" packet from the robot.
|
93 |
static void send_command_packet (char type, char *data, int len) |
94 |
{ |
95 |
send_packet (type, data, len); |
96 |
} |
97 |
|
98 |
// A packet that is followed by a data packet from the robot
|
99 |
static void send_request_packet(char type, char *data, int len) |
100 |
{ |
101 |
send_packet (type, data, len); |
102 |
} |
103 |
|
104 |
|
105 |
// *************************
|
106 |
// ** Individual commands **
|
107 |
// *************************
|
108 |
|
109 |
void robot_set_orbs (uint8_t red1, uint8_t green1, uint8_t blue1, uint8_t red2, uint8_t green2, uint8_t blue2)
|
110 |
{ |
111 |
char data[6]; |
112 |
|
113 |
data[0]=red1;
|
114 |
data[1]=green1;
|
115 |
data[2]=blue1;
|
116 |
|
117 |
data[3]=red2;
|
118 |
data[4]=green2;
|
119 |
data[5]=blue2;
|
120 |
|
121 |
send_command_packet (station_robot_set_orbs, data, 6);
|
122 |
} |
123 |
|
124 |
/** Direction is motor_direction_off/forward/backward, not FORWARD/BACKWARD */
|
125 |
void robot_set_motors (uint8_t direction1, uint8_t speed1, uint8_t direction2, uint8_t speed2)
|
126 |
{ |
127 |
char data[4]; |
128 |
|
129 |
data[0]=motor_direction (direction1, speed1);
|
130 |
data[1]=motor_velocity (direction1, speed1);
|
131 |
data[2]=motor_direction (direction2, speed2);
|
132 |
data[3]=motor_velocity (direction2, speed2);
|
133 |
|
134 |
send_command_packet (station_robot_set_motors, data, 4);
|
135 |
} |
136 |
|
137 |
/** Direction is direction_off/forward/backward, not FORWARD/BACKWARD */
|
138 |
void robot_set_motors_time (uint8_t direction1, uint8_t speed1, uint8_t direction2, uint8_t speed2, uint16_t time_ms)
|
139 |
{ |
140 |
char data[6]; |
141 |
|
142 |
data[0]=motor_direction (direction1, speed1);
|
143 |
data[1]=motor_velocity (direction1, speed1);
|
144 |
data[2]=motor_direction (direction2, speed1);
|
145 |
data[3]=motor_velocity (direction2, speed2);
|
146 |
data[4]=WORD_BYTE_0(time_ms);
|
147 |
data[5]=WORD_BYTE_1(time_ms);
|
148 |
|
149 |
send_command_packet (station_robot_set_motors_time, data, 6);
|
150 |
} |
151 |
|
152 |
void robot_set_motors_off (void) |
153 |
{ |
154 |
send_command_packet (station_robot_set_motors_off, NULL, 0); |
155 |
} |
156 |
|
157 |
void robot_set_bom (uint16_t bitmask)
|
158 |
{ |
159 |
char data[2]; |
160 |
|
161 |
data[0]=WORD_BYTE_0(bitmask);
|
162 |
data[1]=WORD_BYTE_1(bitmask);
|
163 |
|
164 |
send_command_packet (station_robot_set_bom, data, 2);
|
165 |
} |
166 |
|
167 |
void robot_reset_encoders (void) |
168 |
{ |
169 |
send_command_packet (station_robot_reset_encoders, NULL, 0); |
170 |
} |
171 |
|
172 |
|
173 |
// *************************
|
174 |
// ** Individual requests **
|
175 |
// *************************
|
176 |
|
177 |
bool robot_read_encoders (int16_t *left, int16_t *right)
|
178 |
{ |
179 |
send_request_packet (station_robot_read_encoders, NULL, 0); |
180 |
|
181 |
// Now the received data is in received_buffer/received_length
|
182 |
if (received_length>=4) |
183 |
{ |
184 |
*left =WORD(received_buffer[0], received_buffer[1]); |
185 |
*right=WORD(received_buffer[2], received_buffer[3]); |
186 |
return true; |
187 |
} |
188 |
else
|
189 |
{ |
190 |
return false; |
191 |
} |
192 |
} |
193 |
|
194 |
uint16_t robot_read_bom (uint8_t num) |
195 |
{ |
196 |
// FIXME implement
|
197 |
return 0; |
198 |
} |
199 |
|
200 |
uint16_t robot_read_rangefinder (uint8_t num) |
201 |
{ |
202 |
// FIXME implement
|
203 |
return 0; |
204 |
} |
205 |
|
206 |
|
207 |
// ***********************
|
208 |
// ** Receive callbacks **
|
209 |
// ***********************
|
210 |
|
211 |
static void robot_station_receive (char type, int source, unsigned char* packet, int length) |
212 |
{ |
213 |
// Dump the raw packet
|
214 |
//usb_puts ("# [");
|
215 |
//for (uint8_t i=0; i<length; ++i)
|
216 |
//{
|
217 |
// if (i!=0) usb_putc (' ');
|
218 |
// usb_puti (packet[i]);
|
219 |
//}
|
220 |
//usb_putc (']');
|
221 |
|
222 |
// We received some data from the robot. It is not handled immediately as some function is waiting for it.
|
223 |
|
224 |
// Note the fact that we received some data
|
225 |
received_state=received_state_received; |
226 |
|
227 |
// The packet buffer might be overwritten after this function returns, so we have to make a copy of the data.
|
228 |
|
229 |
// First, determine the number of bytes to copy. It is the minimum of the received length and the size of the data
|
230 |
// buffer the data is copied to.
|
231 |
received_length=length; |
232 |
if (sizeof (received_buffer)<received_length) received_length=sizeof (received_buffer); |
233 |
|
234 |
// Now copy the data
|
235 |
memcpy (received_buffer, packet, received_length); |
236 |
} |
237 |
|
238 |
static void robot_station_timeout (void) |
239 |
{ |
240 |
if (received_state==received_state_waiting ) { received_state=received_state_pre_timeout; }
|
241 |
else if (received_state==received_state_pre_timeout) { received_state=received_state_timeout ; } |
242 |
} |
243 |
|
244 |
// ********************
|
245 |
// ** Initialization **
|
246 |
// ********************
|
247 |
|
248 |
// Must not be a local variable because the library doesn't make a copy of it.
|
249 |
PacketGroupHandler receive_handler={robot_station_group, robot_station_timeout, NULL, &robot_station_receive, NULL}; |
250 |
|
251 |
void comm_robot_init ()
|
252 |
{ |
253 |
received_state=received_state_uninitialized; |
254 |
received_length=0;
|
255 |
|
256 |
usb_puts("# Initializing robot communication" NL);
|
257 |
xbee_init (); |
258 |
wl_init(); |
259 |
usb_puts("# Done" NL);
|
260 |
|
261 |
//PacketGroupHandler xxx_receive_handler = {robot_station_group, NULL, NULL, &robot_station_receive, NULL};
|
262 |
wl_register_packet_group(&receive_handler); |
263 |
} |