root / trunk / code / projects / diagnostic_station / station / comm_robot.c @ 1195
History | View | Annotate | Download (2.52 KB)
1 |
#include <dragonfly_lib.h> |
---|---|
2 |
#include <wireless.h> |
3 |
|
4 |
#include "comm_robot.h" |
5 |
#include "../common/comm_station_robot.h" |
6 |
|
7 |
char new_data = 0; |
8 |
char finished = 0; |
9 |
char data_received[10]; |
10 |
|
11 |
// A packet that is followed by a "done" packet from the robot.
|
12 |
static void send_command_packet (char type, char *data, int len) |
13 |
{ |
14 |
wl_send_global_packet (station_robot_group, type, data, len, 0);
|
15 |
|
16 |
// TODO: is it guaranteed that the package will be sent on the next call to wl_do, or
|
17 |
// do we have to call wl_do in a loop?
|
18 |
|
19 |
// TODO: wait for "done" from robot.
|
20 |
|
21 |
wl_do (); |
22 |
} |
23 |
|
24 |
// A packet that is followed by a data packet from the robot
|
25 |
static void send_request_packet(char type, char *data, int len) |
26 |
{ |
27 |
wl_Send_global_packet (station_robot_group, type, data, len, 0);
|
28 |
while(!new_data){
|
29 |
wl_do(); |
30 |
} |
31 |
new_data = 0;
|
32 |
} |
33 |
|
34 |
|
35 |
|
36 |
|
37 |
void robot_set_orbs (uint8_t red1, uint8_t green1, uint8_t blue1, uint8_t red2, uint8_t green2, uint8_t blue2)
|
38 |
{ |
39 |
char data[6]; |
40 |
|
41 |
data[0]=red1;
|
42 |
data[1]=green1;
|
43 |
data[2]=blue1;
|
44 |
|
45 |
data[3]=red2;
|
46 |
data[4]=green2;
|
47 |
data[5]=blue2;
|
48 |
|
49 |
send_command_packet (station_robot_set_orbs, data, 6);
|
50 |
} |
51 |
|
52 |
void robot_set_motors (uint8_t direction1, uint8_t speed1, uint8_t direction2, uint8_t speed2)
|
53 |
{ |
54 |
char data[4]; |
55 |
|
56 |
data[0]=direction1;
|
57 |
data[1]=speed1;
|
58 |
data[2]=direction2;
|
59 |
data[3]=speed2;
|
60 |
|
61 |
send_command_packet (station_robot_set_motors, data, 4);
|
62 |
} |
63 |
|
64 |
void robot_set_motors_time (uint8_t direction1, uint8_t speed1, uint8_t direction2, uint8_t speed2, uint16_t time_ms)
|
65 |
{ |
66 |
char data[6]; |
67 |
|
68 |
data[0]=direction1;
|
69 |
data[1]=speed1;
|
70 |
data[2]=direction2;
|
71 |
data[3]=speed2;
|
72 |
data[4]=WORD_BYTE_0(time_ms);
|
73 |
data[5]=WORD_BYTE_1(time_ms);
|
74 |
|
75 |
send_command_packet (station_robot_set_motors, data, 6);
|
76 |
} |
77 |
|
78 |
void robot_set_motors_off (void) |
79 |
{ |
80 |
send_command_packet (station_robot_set_motors_off, NULL, 0); |
81 |
} |
82 |
|
83 |
void robot_set_bom (uint16_t bitmask)
|
84 |
{ |
85 |
char data[2]; |
86 |
|
87 |
data[0]=WORD_BYTE_0(bitmask);
|
88 |
data[1]=WORD_BYTE_1(bitmask);
|
89 |
|
90 |
send_command_packet (station_robot_set_bom, data, 2);
|
91 |
} |
92 |
|
93 |
char* robot_read_encoders ()
|
94 |
{ |
95 |
send_request_packet (station_robot_read_enocders, NULL, 0); |
96 |
return data_received;
|
97 |
} |
98 |
|
99 |
void robot_station_receive(char type, int source, unsigned char* packet, int length) |
100 |
{ |
101 |
switch (type)
|
102 |
{ |
103 |
case robot_station_finished: new_data = 1; break; |
104 |
case robot_station_data_encoders: receive_encoders((int*)packet, length); break; |
105 |
} |
106 |
new_data = 1;
|
107 |
} |
108 |
|
109 |
void receive_encoders(int* data; int length){ |
110 |
((int*)data_received)[0] = data[0]; |
111 |
((int*)data_received)[1] = data[1]; |
112 |
} |