root / trunk / code / projects / diagnostic_station / station / comm_robot.c @ 1195
History | View | Annotate | Download (2.52 KB)
1 | 1159 | deffi | #include <dragonfly_lib.h> |
---|---|---|---|
2 | #include <wireless.h> |
||
3 | |||
4 | 1182 | deffi | #include "comm_robot.h" |
5 | 1159 | deffi | #include "../common/comm_station_robot.h" |
6 | |||
7 | 1192 | emullini | char new_data = 0; |
8 | char finished = 0; |
||
9 | char data_received[10]; |
||
10 | |||
11 | 1164 | deffi | // 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 | 1159 | deffi | { |
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 | 1164 | deffi | |
19 | // TODO: wait for "done" from robot.
|
||
20 | |||
21 | 1159 | deffi | wl_do (); |
22 | } |
||
23 | |||
24 | 1164 | deffi | // A packet that is followed by a data packet from the robot
|
25 | 1192 | emullini | 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 | 1164 | deffi | |
34 | |||
35 | |||
36 | |||
37 | 1159 | deffi | 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 | 1164 | deffi | send_command_packet (station_robot_set_orbs, data, 6);
|
50 | 1159 | deffi | } |
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 | 1164 | deffi | send_command_packet (station_robot_set_motors, data, 4);
|
62 | 1159 | deffi | } |
63 | |||
64 | 1164 | deffi | 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 | 1181 | deffi | void robot_set_motors_off (void) |
79 | 1164 | deffi | { |
80 | send_command_packet (station_robot_set_motors_off, NULL, 0); |
||
81 | } |
||
82 | |||
83 | 1159 | deffi | 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 | 1164 | deffi | send_command_packet (station_robot_set_bom, data, 2);
|
91 | 1159 | deffi | } |
92 | 1170 | emullini | |
93 | 1192 | emullini | 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 | 1195 | emullini | case robot_station_data_encoders: receive_encoders((int*)packet, length); break; |
105 | 1192 | emullini | } |
106 | 1195 | emullini | 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 | 1192 | emullini | } |