Revision 1198
Added server communication parameter parsing
Misc. fixes
| trunk/code/projects/diagnostic_station/station/comm_robot.c | ||
|---|---|---|
| 24 | 24 |
// A packet that is followed by a data packet from the robot |
| 25 | 25 |
static void send_request_packet(char type, char *data, int len) |
| 26 | 26 |
{
|
| 27 |
wl_Send_global_packet (station_robot_group, type, data, len, 0);
|
|
| 27 |
wl_send_global_packet (station_robot_group, type, data, len, 0);
|
|
| 28 | 28 |
while(!new_data){
|
| 29 | 29 |
wl_do(); |
| 30 | 30 |
} |
| ... | ... | |
| 92 | 92 |
|
| 93 | 93 |
char* robot_read_encoders () |
| 94 | 94 |
{
|
| 95 |
send_request_packet (station_robot_read_enocders, NULL, 0);
|
|
| 95 |
send_request_packet (station_robot_read_encoders, NULL, 0);
|
|
| 96 | 96 |
return data_received; |
| 97 | 97 |
} |
| 98 | 98 |
|
| 99 |
void receive_encoders(int* data, int length){
|
|
| 100 |
((int*)data_received)[0] = data[0]; |
|
| 101 |
((int*)data_received)[1] = data[1]; |
|
| 102 |
} |
|
| 103 |
|
|
| 99 | 104 |
void robot_station_receive(char type, int source, unsigned char* packet, int length) |
| 100 | 105 |
{
|
| 101 | 106 |
switch (type) |
| 102 | 107 |
{
|
| 103 |
case robot_station_finished: new_data = 1; break;
|
|
| 104 |
case robot_station_data_encoders: receive_encoders((int*)packet, length); break;
|
|
| 108 |
case robot_station_finished: new_data = 1; break;
|
|
| 109 |
case robot_station_data_encoders: receive_encoders((int*)packet, length); break;
|
|
| 105 | 110 |
} |
| 106 | 111 |
new_data = 1; |
| 107 | 112 |
} |
| 108 | 113 |
|
| 109 |
void receive_encoders(int* data; int length){
|
|
| 110 |
((int*)data_received)[0] = data[0]; |
|
| 111 |
((int*)data_received)[1] = data[1]; |
|
| 112 |
} |
|
| 114 |
|
|
| 115 |
void comm_robot_init () |
|
| 116 |
{
|
|
| 117 |
PacketGroupHandler receive_handler = {robot_station_group, NULL, NULL, &robot_station_receive, NULL};
|
|
| 118 |
wl_register_packet_group(&receive_handler); |
|
| 119 |
} |
|
| trunk/code/projects/diagnostic_station/station/comm_robot.h | ||
|---|---|---|
| 1 | 1 |
#ifndef _robot_comm_h |
| 2 | 2 |
#define _robot_comm_h |
| 3 | 3 |
|
| 4 |
void comm_robot_init (void); |
|
| 5 |
|
|
| 6 |
|
|
| 4 | 7 |
void robot_set_orbs (uint8_t red1, uint8_t green1, uint8_t blue1, uint8_t red2, uint8_t green2, uint8_t blue2); |
| 5 | 8 |
void robot_set_motors (uint8_t direction1, uint8_t speed1, uint8_t direction2, uint8_t speed2); |
| 6 | 9 |
void robot_set_motors_off (void); |
| trunk/code/projects/diagnostic_station/station/hardware_rbom.c | ||
|---|---|---|
| 1 | 1 |
#include "hardware_rbom.h" |
| 2 | 2 |
|
| 3 |
static void pDataDump(int startPort, int stopPort, unsigned int delay); |
|
| 4 |
static void pUsr(int startPort, int stopPort, unsigned int delay); |
|
| 5 |
|
|
| 3 | 6 |
void rbom_init (void) |
| 4 | 7 |
{
|
| 5 | 8 |
// FIXME implement |
| trunk/code/projects/diagnostic_station/station/hardware_rbom.h | ||
|---|---|---|
| 8 | 8 |
* Inverse BOM sensor macros for (N)orth, |
| 9 | 9 |
* (S)outh, (E)ast, and (W)est positions. |
| 10 | 10 |
*/ |
| 11 |
#define N_SENSOR |
|
| 12 |
#define E_SENSOR |
|
| 13 |
#define S_SENSOR |
|
| 14 |
#define W_SENSOR |
|
| 11 |
#define N_SENSOR AN3
|
|
| 12 |
#define E_SENSOR AN4
|
|
| 13 |
#define S_SENSOR AN5
|
|
| 14 |
#define W_SENSOR AN6
|
|
| 15 | 15 |
|
| 16 | 16 |
void rbom_init (void); |
| 17 | 17 |
|
| ... | ... | |
| 20 | 20 |
void rbom_update (void); |
| 21 | 21 |
void rbom_debug (void); |
| 22 | 22 |
|
| 23 |
static void pUsr(int startPort, int stopPort, unsigned int delay); |
|
| 24 |
static void pDataDump(int startPort, int stopPort, unsigned int delay); |
|
| 25 |
|
|
| 26 | 23 |
#endif |
| trunk/code/projects/diagnostic_station/station/comm_server.c | ||
|---|---|---|
| 1 | 1 |
#include "comm_server.h" |
| 2 | 2 |
|
| 3 | 3 |
#include <avr/pgmspace.h> |
| 4 |
#include <string.h> |
|
| 4 | 5 |
|
| 5 | 6 |
#include "global.h" |
| 6 | 7 |
#include "tests.h" |
| ... | ... | |
| 9 | 10 |
// ## Settings ## |
| 10 | 11 |
// ############## |
| 11 | 12 |
|
| 12 |
#define comm_server_debug |
|
| 13 |
//#define comm_server_debug
|
|
| 13 | 14 |
|
| 14 | 15 |
// ############### |
| 15 | 16 |
// ## Constants ## |
| ... | ... | |
| 17 | 18 |
|
| 18 | 19 |
#define buffer_size 120 |
| 19 | 20 |
|
| 21 |
// Put all symbolic string constants into the program memory because else all of them would be copied into the RAM (see |
|
| 22 |
// the avr-libc documentation, chapter "Data in Program Space"). |
|
| 20 | 23 |
|
| 24 |
// Commands |
|
| 25 |
const char command_ping [] PROGMEM = "ping" ; |
|
| 26 |
const char command_start_test [] PROGMEM = "start_test" ; |
|
| 27 |
const char command_all [] PROGMEM = "all" ; |
|
| 28 |
const char command_comm [] PROGMEM = "comm" ; |
|
| 29 |
const char command_bom [] PROGMEM = "bom" ; |
|
| 30 |
const char command_rangefinder [] PROGMEM = "rangefinder"; |
|
| 31 |
const char command_motor [] PROGMEM = "motor" ; |
|
| 32 |
const char command_encoder [] PROGMEM = "encoder" ; |
|
| 33 |
const char command_help [] PROGMEM = "help" ; |
|
| 34 |
|
|
| 35 |
// Help |
|
| 36 |
const char help_text [] PROGMEM = |
|
| 37 |
"# Available commands:" NL |
|
| 38 |
"# - help" NL |
|
| 39 |
"# - ping" NL |
|
| 40 |
"# - start_test all" NL |
|
| 41 |
"# - start_test comm" NL |
|
| 42 |
"# - start_test bom all|<num> (0..15)" NL |
|
| 43 |
"# - start_test rangefinder all|<num> (?..?)" NL |
|
| 44 |
"# - start_test motor all|<num> (1..2)" NL |
|
| 45 |
"# - start_test encoder all|<num> (1..2)" NL |
|
| 46 |
; |
|
| 47 |
|
|
| 48 |
|
|
| 21 | 49 |
// #################### |
| 22 | 50 |
// ## Initialization ## |
| 23 | 51 |
// #################### |
| ... | ... | |
| 32 | 60 |
// ## Message processing ## |
| 33 | 61 |
// ######################## |
| 34 | 62 |
|
| 35 |
// This function is copied from XNS
|
|
| 36 |
//static void serial_send_string_P (const char *s)/*{{{*/
|
|
| 37 |
//{
|
|
| 38 |
// char buf;
|
|
| 39 |
// while (memcpy_P (&buf, s, sizeof (char)), buf!=0)
|
|
| 40 |
// {
|
|
| 41 |
// serial_transmit (buf);
|
|
| 42 |
// s++;
|
|
| 43 |
// }
|
|
| 44 |
//}
|
|
| 63 |
// TODO move this to the library
|
|
| 64 |
static void serial_send_string_P (PGM_P s)
|
|
| 65 |
{
|
|
| 66 |
char buf; |
|
| 67 |
while (memcpy_P (&buf, s, sizeof (char)), buf!=0) |
|
| 68 |
{
|
|
| 69 |
usb_putc (buf);
|
|
| 70 |
s++; |
|
| 71 |
} |
|
| 72 |
} |
|
| 45 | 73 |
|
| 74 |
static char *find_next_parameter (char *p) |
|
| 75 |
{
|
|
| 76 |
// Advance p until a blank or the terminating zero is found. |
|
| 77 |
while ((*p)!=' ' && (*p)!=0) p++; |
|
| 78 |
|
|
| 79 |
// Advance p until a non-blank (including the terminating zero) is found. |
|
| 80 |
while (*p==' ') p++; |
|
| 81 |
|
|
| 82 |
// We have found the next parameter unless p points to the terminating zero. |
|
| 83 |
if (*p==0) |
|
| 84 |
return NULL; |
|
| 85 |
else |
|
| 86 |
return p; |
|
| 87 |
} |
|
| 88 |
|
|
| 46 | 89 |
/** Check if the buffer starts with the given text, followed by a space */ |
| 47 |
// This function is copied from XNS |
|
| 48 |
static bool serial_match (PGM_P text, uint8_t *buffer, uint8_t size) |
|
| 90 |
static bool serial_match (PGM_P text, const char *buffer) |
|
| 49 | 91 |
{
|
| 50 | 92 |
uint8_t text_len=strlen_P (text); |
| 93 |
uint8_t buffer_len=strlen (buffer); |
|
| 51 | 94 |
|
| 52 | 95 |
// If the buffer is shorter than the text, there is no match |
| 53 |
if (size<text_len) return false;
|
|
| 96 |
if (buffer_len<text_len) return false;
|
|
| 54 | 97 |
|
| 55 | 98 |
// If the buffer is longer than the text, it must have a space at position after the text |
| 56 |
if (size>text_len && buffer[text_len]!=' ') return false;
|
|
| 99 |
if (buffer_len>text_len && buffer[text_len]!=' ') return false;
|
|
| 57 | 100 |
|
| 58 | 101 |
// Test for match |
| 59 | 102 |
if (strncmp_P ((char *)buffer, text, text_len)==0) return true; |
| ... | ... | |
| 80 | 123 |
// ## Message handlers ## |
| 81 | 124 |
// ###################### |
| 82 | 125 |
|
| 83 |
static void handle_message_ping (uint8_t *buffer, uint8_t size)
|
|
| 126 |
static void handle_message_ping (char *buffer)
|
|
| 84 | 127 |
{
|
| 85 | 128 |
usb_puts ("pong" NL);
|
| 86 | 129 |
orbs_set (255, 127, 0, 0, 255, 0); |
| ... | ... | |
| 88 | 131 |
orbs_set (0, 255, 0, 255, 127, 0); |
| 89 | 132 |
} |
| 90 | 133 |
|
| 91 |
static void handle_message_start_test (uint8_t *buffer, uint8_t size)
|
|
| 134 |
static void handle_message_start_test (char *buffer)
|
|
| 92 | 135 |
{
|
| 93 | 136 |
// FIXME parameter handling |
| 94 | 137 |
// Parameters: all/rangefinder/motor/encoder all/<num> |
| 95 |
test_all (); |
|
| 138 |
|
|
| 139 |
buffer=find_next_parameter (buffer); |
|
| 140 |
|
|
| 141 |
if (!buffer) |
|
| 142 |
usb_puts ("# Parameter missing" NL);
|
|
| 143 |
else if (serial_match (command_all, buffer)) |
|
| 144 |
{
|
|
| 145 |
usb_puts ("# Doing all tests" NL);
|
|
| 146 |
test_all (); |
|
| 147 |
} |
|
| 148 |
else |
|
| 149 |
{
|
|
| 150 |
usb_puts ("# Warning: unhandled parameters: [");
|
|
| 151 |
usb_puts (buffer); |
|
| 152 |
usb_puts ("]" NL);
|
|
| 153 |
} |
|
| 96 | 154 |
} |
| 97 | 155 |
|
| 156 |
static void handle_message_help (char *buffer) |
|
| 157 |
{
|
|
| 158 |
serial_send_string_P (help_text); |
|
| 159 |
} |
|
| 98 | 160 |
|
| 161 |
|
|
| 99 | 162 |
// ########################## |
| 100 | 163 |
// ## Messages (receiving) ## |
| 101 | 164 |
// ########################## |
| 102 | 165 |
|
| 103 |
|
|
| 104 |
// Put all symbolic string constants into the program memory because else all of them would be copied into the RAM (see |
|
| 105 |
// the avr-libc documentation, chapter "Data in Program Space"). |
|
| 106 |
const char command_ping [] PROGMEM = "ping" ; |
|
| 107 |
const char command_start_test [] PROGMEM = "start_test"; |
|
| 108 |
|
|
| 109 |
static void handle_message (uint8_t *message, uint8_t size) |
|
| 166 |
/** message is 0 terminated */ |
|
| 167 |
static void handle_message (char *message) |
|
| 110 | 168 |
{
|
| 111 | 169 |
#ifdef comm_server_debug |
| 112 | 170 |
// Output: "# Received a message, size <size>: [<message>]" |
| ... | ... | |
| 118 | 176 |
#endif |
| 119 | 177 |
|
| 120 | 178 |
bool handled=false; |
| 121 |
if (serial_match (command_ping , message, size)) { handle_message_ping (message, size); handled=true; }
|
|
| 122 |
if (serial_match (command_start_test, message, size)) { handle_message_start_test (message, size); handled=true; }
|
|
| 179 |
if (serial_match (command_ping , message)) { handle_message_ping (message); handled=true; }
|
|
| 180 |
if (serial_match (command_start_test, message)) { handle_message_start_test (message); handled=true; }
|
|
| 181 |
if (serial_match (command_help , message)) { handle_message_help (message); handled=true; }
|
|
| 123 | 182 |
// More messages go here |
| 124 | 183 |
|
| 125 | 184 |
if (!handled) |
| ... | ... | |
| 155 | 214 |
|
| 156 | 215 |
void server_main (void) |
| 157 | 216 |
{
|
| 158 |
uint8_t buffer[buffer_size];
|
|
| 217 |
char buffer[buffer_size];
|
|
| 159 | 218 |
uint8_t c; |
| 160 | 219 |
uint8_t buffer_fill=0; |
| 161 | 220 |
|
| ... | ... | |
| 181 | 240 |
buffer[buffer_fill]=0; |
| 182 | 241 |
|
| 183 | 242 |
// Handle the message. |
| 184 |
handle_message (buffer, buffer_fill);
|
|
| 243 |
handle_message (buffer); |
|
| 185 | 244 |
|
| 186 | 245 |
// Reset the buffer |
| 187 | 246 |
buffer_fill=0; |
| trunk/code/projects/diagnostic_station/station/main.c | ||
|---|---|---|
| 9 | 9 |
#include "tests.h" |
| 10 | 10 |
#include "self_test.h" |
| 11 | 11 |
#include "comm_server.h" |
| 12 |
#include "comm_robot.h" |
|
| 12 | 13 |
|
| 13 | 14 |
void interactive_main (void) |
| 14 | 15 |
{
|
| ... | ... | |
| 40 | 41 |
dragonfly_init (0); |
| 41 | 42 |
orb_init (); |
| 42 | 43 |
orbs_set (0, 0,255,0,0,0); |
| 44 |
while (1); |
|
| 45 |
return 0; |
|
| 43 | 46 |
} |
| 44 | 47 |
|
| 45 | 48 |
int main_john (void) |
| ... | ... | |
| 53 | 56 |
|
| 54 | 57 |
rbom_debug(); |
| 55 | 58 |
|
| 59 |
return 0; |
|
| 56 | 60 |
} |
| 57 | 61 |
|
| 58 | 62 |
|
| 59 | 63 |
int main_evan (void) |
| 60 | 64 |
{
|
| 61 |
#include <dragonfly_lib.h> |
|
| 62 |
#include <wireless.h> |
|
| 63 |
|
|
| 64 | 65 |
dragonfly_init(0); |
| 65 | 66 |
|
| 66 | 67 |
usb_init (); |
| ... | ... | |
| 71 | 72 |
orb_init_pwm (); |
| 72 | 73 |
hardware_init (); |
| 73 | 74 |
comm_server_init (); |
| 75 |
comm_robot_init (); |
|
| 74 | 76 |
|
| 75 | 77 |
orb1_set (255, 0, 0); usb_puts("# Initializing wireless" NL);
|
| 76 | 78 |
xbee_init (); |
| 77 | 79 |
wl_init(); |
| 78 | 80 |
orb2_set (255, 0, 0); usb_puts("# Done" NL);
|
| 79 | 81 |
|
| 80 |
PacketGroupHandler receive_handler = {robot_station_group, NULL, NULL, &robot_station_receive, NULL};
|
|
| 81 |
wl_register_packet_group(&receive_handler); |
|
| 82 | 82 |
|
| 83 | 83 |
// If button 1 is pressed after initialization of the wireless (which takes about 1s), run all tests. |
| 84 | 84 |
if (button1_read ()) |
| ... | ... | |
| 96 | 96 |
} |
| 97 | 97 |
|
| 98 | 98 |
while (1); |
| 99 |
return 0; |
|
| 99 | 100 |
} |
| 100 | 101 |
|
| 101 | 102 |
int main_default (void) |
| 102 | 103 |
{
|
| 103 |
#include <dragonfly_lib.h> |
|
| 104 |
#include <wireless.h> |
|
| 105 |
|
|
| 106 | 104 |
dragonfly_init(0); |
| 107 | 105 |
|
| 108 | 106 |
usb_init (); |
| ... | ... | |
| 135 | 133 |
} |
| 136 | 134 |
|
| 137 | 135 |
while (1); |
| 138 |
} |
|
| 136 |
} |
|
| trunk/code/projects/diagnostic_station/station/comm_server.h | ||
|---|---|---|
| 8 | 8 |
void comm_server_init (void); |
| 9 | 9 |
void server_send_finished (void); |
| 10 | 10 |
|
| 11 |
|
|
| 12 |
|
|
| 13 |
|
|
| 14 | 11 |
void server_main (void); |
| 15 | 12 |
|
| 16 | 13 |
|
| trunk/code/projects/diagnostic_station/robot/main.c | ||
|---|---|---|
| 71 | 71 |
case station_robot_set_bom: message_set_bom (length, packet); break; |
| 72 | 72 |
case station_robot_set_motors_off: message_set_motors_off (length, packet); break; |
| 73 | 73 |
case station_robot_set_motors_time: message_set_motors_time (length, packet); break; |
| 74 |
case station_robot_read_encoders: message_Send_econders (length, packet); break;
|
|
| 74 |
case station_robot_read_encoders: message_send_encoders (length, packet); break;
|
|
| 75 | 75 |
} |
| 76 | 76 |
} |
| 77 | 77 |
|
| trunk/code/projects/diagnostic_station/common/comm_station_robot.h | ||
|---|---|---|
| 17 | 17 |
#define station_robot_set_motors_time 5 |
| 18 | 18 |
#define station_robot_read_encoders 6 |
| 19 | 19 |
|
| 20 |
|
|
| 21 | 20 |
#define robot_station_group 2 |
| 22 | 21 |
|
| 23 | 22 |
#define robot_station_finished 1 |
Also available in: Unified diff