Contiki 2.6
|
00001 #include <stepper-process.h> 00002 #include <stepper-steps.h> 00003 #include <stepper.h> 00004 #include <stepper-move.h> 00005 #include <string.h> 00006 #include <interrupt-utils.h> 00007 #include <stdio.h> 00008 #include <unistd.h> 00009 #include <net/uip.h> 00010 #include <dev/cc2420.h> 00011 00012 00013 #undef putchar 00014 00015 00016 static unsigned int 00017 parse_uint_hex(const char **pp, const char *end) 00018 { 00019 unsigned int v = 0; 00020 while(*pp < end) { 00021 char ch; 00022 if ((ch = **pp) >= '0' && ch <= '9') { 00023 v = v* 16 + (ch - '0'); 00024 } else if (ch >= 'A' && ch <= 'F') { 00025 v = v* 16 + (ch - 'A') + 10; 00026 } else break; 00027 (*pp)++; 00028 } 00029 return v; 00030 } 00031 00032 static int 00033 parse_int_hex(const char **pp, const char *end) 00034 { 00035 if (*pp == end) return 0; 00036 if (**pp == '-') { 00037 (*pp)++; 00038 return -parse_uint_hex(pp, end); 00039 } else { 00040 return parse_uint_hex(pp, end); 00041 } 00042 } 00043 00044 static void 00045 skip_white(const char **pp, const char *end) 00046 { 00047 char ch; 00048 while(*pp < end && ((ch = **pp) == ' ' || ch == '\t')) (*pp)++; 00049 } 00050 00051 static const char hex_chars[] = 00052 {'0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F'}; 00053 00054 static void 00055 format_uint_hex(char **str, char *end, unsigned int v) 00056 { 00057 char buffer[10]; 00058 char *p = buffer+10; 00059 if (*str == end) return; 00060 if (v == 0) { 00061 *(*str)++ = '0'; 00062 return; 00063 } 00064 while(v > 0) { 00065 *--p = hex_chars[v&0xf]; 00066 v >>= 4; 00067 } 00068 while((p < buffer+10) && (*str < end)) { 00069 *(*str)++ = *p++; 00070 } 00071 } 00072 00073 static void 00074 format_int_hex(char **str, char *end, int v) 00075 { 00076 if (v < 0) { 00077 if (*str == end) return; 00078 *(*str)++ = '-'; 00079 v = -v; 00080 } 00081 format_uint_hex(str, end, v); 00082 } 00083 00084 static void 00085 format_ull_hex(char **str, char *end, unsigned long long int v) 00086 { 00087 char buffer[16]; 00088 char *p = buffer+10; 00089 if (*str == end) return; 00090 if (v == 0) { 00091 *(*str)++ = '0'; 00092 return; 00093 } 00094 while(v > 0) { 00095 *--p = hex_chars[v&0xf]; 00096 v >>= 4; 00097 } 00098 while((p < buffer+10) && (*str < end)) { 00099 *(*str)++ = *p++; 00100 } 00101 } 00102 static void 00103 format_ll_hex(char **str, char *end, long long v) 00104 { 00105 if (v < 0) { 00106 if (*str == end) return; 00107 *(*str)++ = '-'; 00108 v = -v; 00109 } 00110 format_ull_hex(str, end, v); 00111 } 00112 00113 typedef struct _ReplyBuffer ReplyBuffer; 00114 00115 struct _ReplyBuffer 00116 { 00117 char buffer[70]; /* Should be small enough to fit in one packet */ 00118 char *write; 00119 }; 00120 00121 static ReplyBuffer tcp_reply; 00122 static ReplyBuffer udp_reply; 00123 00124 #define REPLY_BUFFER_END(reply) ((reply)->buffer+sizeof((reply)->buffer)) 00125 #define REPLY_BUFFER_LEFT(reply) \ 00126 ((reply)->buffer+sizeof((reply)->buffer) - (reply)->write) 00127 00128 static void 00129 reply_char(ReplyBuffer *reply, char c) 00130 { 00131 if (REPLY_BUFFER_LEFT(reply) > 0) { 00132 *reply->write++ = c; 00133 } 00134 } 00135 00136 static void 00137 reply_str(ReplyBuffer *reply, char *str) 00138 { 00139 while(reply->write < REPLY_BUFFER_END(reply) && *str != '\0') 00140 *reply->write++ = *str++; 00141 } 00142 00143 static void 00144 stepper_reply(ReplyBuffer *reply, StepperResult res) 00145 { 00146 switch(res) { 00147 case STEPPER_OK: 00148 reply_str(reply, "OK"); 00149 break; 00150 case STEPPER_ERR_MEM: 00151 reply_str(reply, "ERR MEM"); 00152 break; 00153 case STEPPER_ERR_TOO_LATE: 00154 reply_str(reply, "ERR LATE"); 00155 break; 00156 case STEPPER_ERR_INDEX: /* Sholdn't happen here */ 00157 reply_str(reply, "ERR INDEX"); 00158 break; 00159 default: 00160 reply_str(reply, "ERR"); 00161 } 00162 reply_char(reply, '\n'); 00163 } 00164 00165 #define CHECK_INPUT_LEFT(x) \ 00166 do {\ 00167 if ((x) > inend - input_line) {reply_str(reply, "ERR\n");return 0;}\ 00168 } while(0) 00169 00170 static int 00171 handle_line(const char *input_line, const char *inend, ReplyBuffer *reply) 00172 { 00173 unsigned long when; 00174 #if 0 00175 { 00176 const char *p = input_line; 00177 printf("Got line: '"); 00178 while(p < inend) { 00179 putchar(*p++); 00180 } 00181 printf("'\n"); 00182 fsync(1); 00183 } 00184 #endif 00185 skip_white(&input_line, inend); 00186 CHECK_INPUT_LEFT(1); 00187 if (*input_line == '#') { 00188 input_line++; 00189 reply_char(reply, '#'); 00190 while (input_line < inend &&*input_line != ' ') { 00191 reply_char(reply, *input_line++); 00192 } 00193 reply_char(reply, ' '); 00194 } 00195 skip_white(&input_line, inend); 00196 00197 if (*input_line == '@') { 00198 input_line++; 00199 when = parse_uint_hex(&input_line, inend); 00200 } else { 00201 when = stepper_current_period() + 3; 00202 } 00203 skip_white(&input_line, inend); 00204 CHECK_INPUT_LEFT(1); 00205 if (input_line[0] == 'L' || input_line[0] == 'R') { 00206 unsigned int stepper_index = (input_line[0] == 'R' ? 1 : 0); 00207 CHECK_INPUT_LEFT(1); 00208 input_line++; 00209 if (input_line[0] == 'S') { 00210 int speed; 00211 input_line++; 00212 if (input_line == inend) { 00213 /* printf("Speed: %ld\n", 00214 stepper_current_velocity(stepper_index)/VEL_SCALE);*/ 00215 reply_char(reply, input_line[-2]); 00216 reply_char(reply, 'S'); 00217 format_int_hex(&reply->write, REPLY_BUFFER_END(reply), 00218 stepper_current_velocity(stepper_index)/VEL_SCALE); 00219 reply_char(reply, '\n'); 00220 } else { 00221 speed = parse_int_hex(&input_line, inend); 00222 if (*input_line == ',') { 00223 StepperResult res; 00224 unsigned int acc; 00225 input_line++; 00226 acc = parse_uint_hex(&input_line, inend); 00227 /* printf("Speed=%d, Acc=%u\n", speed, acc); */ 00228 res = stepper_set_velocity(stepper_index, &when, 00229 acc, speed*VEL_SCALE); 00230 00231 stepper_reply(reply, res); 00232 } else { 00233 reply_str(reply, "ERR\n"); 00234 } 00235 } 00236 } else if (input_line[0] == 'C') { 00237 reply_char(reply, input_line[-1]); 00238 reply_char(reply, 'C'); 00239 format_ll_hex(&reply->write, REPLY_BUFFER_END(reply), 00240 stepper_current_step(stepper_index)); 00241 reply_char(reply, '\n'); 00242 } else if (input_line[0] == 'M') { 00243 unsigned int speed; 00244 unsigned int acc; 00245 int move; 00246 input_line++; 00247 speed = parse_uint_hex(&input_line, inend); 00248 CHECK_INPUT_LEFT(1); 00249 if (*input_line == ',') { 00250 input_line++; 00251 acc = parse_uint_hex(&input_line, inend); 00252 if (*input_line == ',') { 00253 StepperResult res; 00254 input_line++; 00255 move = parse_int_hex(&input_line, inend); 00256 /*printf("Speed=%u, Acc=%u, Move=%d\n", speed, acc, move);*/ 00257 res = stepper_move(stepper_index, &when, 00258 acc,speed*VEL_SCALE,move*DIST_SCALE); 00259 stepper_reply(reply, res); 00260 } else { 00261 reply_str(reply, "ERR\n"); 00262 } 00263 } else { 00264 reply_str(reply, "ERR\n"); 00265 } 00266 } else { 00267 reply_str(reply, "ERR\n"); 00268 } 00269 } else if (input_line[0] == 'E') { 00270 STEPPER_ENABLE(); 00271 printf("Stepper enabled\n"); 00272 reply_str(reply, "OK\n"); 00273 } else if (input_line[0] == 'D') { 00274 STEPPER_DISABLE(); 00275 printf("Stepper disabled\n"); 00276 reply_str(reply, "OK\n"); 00277 } else if (input_line[0] == 'p') { 00278 reply_char(reply, 'p'); 00279 format_int_hex(&reply->write, REPLY_BUFFER_END(reply), 00280 cc2420_last_rssi); 00281 reply_char(reply, ','); 00282 format_uint_hex(&reply->write, REPLY_BUFFER_END(reply), 00283 cc2420_last_correlation); 00284 reply_char(reply, '\n'); 00285 } else if (input_line[0] == 'T') { 00286 reply_char(reply, 'T'); 00287 format_int_hex(&reply->write, REPLY_BUFFER_END(reply), 00288 stepper_current_period()); 00289 reply_char(reply, '\n'); 00290 } else if (input_line[0] == 'q') { 00291 return 1; 00292 } else { 00293 reply_str(reply, "ERR\n"); 00294 } 00295 return 0; 00296 } 00297 static unsigned int transmit_len = 0; 00298 00299 static void 00300 send_reply() 00301 { 00302 if (transmit_len == 0) { 00303 transmit_len = tcp_reply.write - tcp_reply.buffer; 00304 if (transmit_len > 0) { 00305 /* printf("Sending len = %d\n", transmit_len); */ 00306 uip_send(tcp_reply.buffer, transmit_len); 00307 } 00308 } 00309 } 00310 00311 00312 static void 00313 handle_connection() 00314 { 00315 static char exiting = 0; 00316 static char line_buffer[100]; 00317 static char *line_end; 00318 if (uip_connected()) { 00319 exiting = 0; 00320 transmit_len = 0; 00321 line_end = line_buffer; 00322 tcp_reply.write = tcp_reply.buffer; 00323 reply_str(&tcp_reply, "Ready\n"); 00324 send_reply(); 00325 } 00326 if (uip_acked()) { 00327 if (tcp_reply.write - tcp_reply.buffer > transmit_len) { 00328 memmove(tcp_reply.buffer, tcp_reply.buffer + transmit_len, 00329 tcp_reply.write - tcp_reply.buffer - transmit_len); 00330 } 00331 tcp_reply.write -= transmit_len; 00332 /* printf("Acked: %d left\n", reply_buffer.write-reply_buffer.buffer); */ 00333 transmit_len = 0; 00334 if (exiting && tcp_reply.write == tcp_reply.buffer) { 00335 uip_close(); 00336 exiting = 0; 00337 } 00338 } 00339 if (uip_newdata()) { 00340 const char *read_pos = uip_appdata; 00341 const char *read_end = read_pos + uip_len; 00342 /* printf("Got data\n"); */ 00343 while(read_pos < read_end) { 00344 if (line_end == line_buffer+sizeof(line_buffer)) { 00345 /* Buffer too small, just discard everything */ 00346 line_end = line_buffer; 00347 } 00348 *line_end++ = *read_pos++; 00349 if (line_end[-1] == '\n' || line_end[-1] == '\r' || line_end[-1] == ';'){ 00350 if (line_end - 1 != line_buffer) { 00351 if (handle_line(line_buffer, line_end - 1, &tcp_reply)) { 00352 send_reply(); 00353 /* Postpone closing if there's reply data left to be sent. */ 00354 if (transmit_len == 0) 00355 uip_close(); 00356 else 00357 exiting = 1; 00358 break; 00359 } 00360 } 00361 line_end = line_buffer; 00362 } 00363 } 00364 send_reply(); 00365 } 00366 00367 if (uip_poll()) { 00368 send_reply(); 00369 } 00370 if(uip_rexmit()) { 00371 printf("Retransmit\n"); 00372 if (transmit_len > 0) 00373 uip_send(tcp_reply.buffer, transmit_len); 00374 } 00375 00376 } 00377 00378 PROCESS(udp_stepper_process, "UDP stepper process"); 00379 00380 PROCESS_THREAD(udp_stepper_process, ev, data) 00381 { 00382 static struct etimer timer; 00383 static struct uip_udp_conn *conn; 00384 static char listening = 1; /* Listen for connections from anyone */ 00385 static uip_ipaddr_t any; 00386 PROCESS_EXITHANDLER(goto exit); 00387 PROCESS_BEGIN(); 00388 00389 printf("udp_stepper_process starting\n"); 00390 00391 uip_ipaddr(&any, 0,0,0,0); 00392 conn = udp_new(&any, UIP_HTONS(0), NULL); 00393 if (!conn) goto exit; 00394 uip_udp_bind(conn, UIP_HTONS(1010)); 00395 etimer_set(&timer, CLOCK_SECOND*2); 00396 while(1) { 00397 PROCESS_YIELD(); 00398 00399 if(ev == tcpip_event) { 00400 if (uip_newdata()) { 00401 struct uip_udpip_hdr *header = (struct uip_udpip_hdr *)uip_buf; 00402 const char *line_start = uip_appdata; 00403 const char *line_end = line_start; 00404 const char *packet_end = line_start + uip_len; 00405 udp_reply.write = udp_reply.buffer; 00406 while(line_end < packet_end) { 00407 if (*line_end == '\n' || *line_end == '\r' || *line_end == ';' ) { 00408 if (line_end != line_start) { 00409 handle_line(line_start, line_end, &udp_reply); 00410 } 00411 line_start = line_end+1; 00412 } 00413 line_end++; 00414 } 00415 /* Check if we are connected to a client, if not reconnect */ 00416 if (listening) { 00417 uip_udp_remove(conn); 00418 conn = udp_new(&header->srcipaddr, header->srcport, &conn); 00419 if (!conn) goto exit; 00420 uip_udp_bind(conn, UIP_HTONS(1010)); 00421 listening = 0; 00422 } 00423 etimer_reset(&timer); 00424 tcpip_poll_udp(conn); 00425 } else if (uip_poll()) { 00426 if (data == &conn) { 00427 uip_send(udp_reply.buffer, udp_reply.write - udp_reply.buffer); 00428 /* printf("sent %ld\n", udp_reply.write - udp_reply.buffer); */ 00429 } 00430 } 00431 } else if (ev == PROCESS_EVENT_TIMER) { 00432 uip_udp_remove(conn); 00433 conn = udp_new(&any, UIP_HTONS(0), NULL); 00434 if (!conn) goto exit; 00435 uip_udp_bind(conn, UIP_HTONS(1010)); 00436 listening = 1; 00437 } 00438 } 00439 00440 exit: 00441 /* Contiki does automatic garbage collection of uIP state and we 00442 * need not worry about that. */ 00443 printf("udprecv_process exiting\n"); 00444 PROCESS_END(); 00445 } 00446 00447 static const uint32_t stepper0_steps_acc[] = MICRO_STEP(0,3); 00448 static const uint32_t stepper0_steps_run[] = MICRO_STEP(0,2); 00449 static const uint32_t stepper0_steps_hold[] = MICRO_STEP(0,1); 00450 00451 static const uint32_t stepper1_steps_acc[] = MICRO_STEP(1,3); 00452 static const uint32_t stepper1_steps_run[] = MICRO_STEP(1,2); 00453 static const uint32_t stepper1_steps_hold[] = MICRO_STEP(1,1); 00454 00455 static StepperAccSeq seq_heap[40]; 00456 00457 static void 00458 init_seq_heap() 00459 { 00460 unsigned int i; 00461 for(i = 0; i < sizeof(seq_heap)/sizeof(seq_heap[0]); i++) { 00462 seq_heap[i].next = NULL; 00463 stepper_free_seq(&seq_heap[i]); 00464 } 00465 } 00466 00467 static void 00468 robot_stepper_init() 00469 { 00470 disableIRQ(); 00471 init_seq_heap(); 00472 stepper_init(AT91C_BASE_TC0, AT91C_ID_TC0); 00473 *AT91C_PIOA_OER = STEPPER_INHIBIT; 00474 *AT91C_PIOA_MDER = STEPPER_INHIBIT; /* | STEPPER0_IOMASK; */ 00475 *AT91C_PIOA_CODR = STEPPER_INHIBIT; 00476 stepper_init_io(1, STEPPER_IOMASK(0), stepper0_steps_acc, 00477 stepper0_steps_run, stepper0_steps_hold, 00478 (sizeof(stepper0_steps_run) / sizeof(stepper0_steps_run[0]))); 00479 stepper_init_io(0, STEPPER_IOMASK(1), stepper1_steps_acc, 00480 stepper1_steps_run, stepper1_steps_hold, 00481 (sizeof(stepper1_steps_run) / sizeof(stepper1_steps_run[0]))); 00482 enableIRQ(); 00483 } 00484 00485 00486 PROCESS(stepper_process, "Stepper control process"); 00487 00488 PROCESS_THREAD(stepper_process, ev, data) 00489 { 00490 PROCESS_EXITHANDLER(goto exit); 00491 PROCESS_BEGIN(); 00492 robot_stepper_init(); 00493 tcp_listen(UIP_HTONS(1010)); 00494 00495 process_start(&udp_stepper_process, NULL); 00496 printf("Stepper starting\n"); 00497 00498 while(1) { 00499 PROCESS_WAIT_EVENT_UNTIL(ev == tcpip_event); 00500 if(uip_connected()) { 00501 /* printf("connected\n"); */ 00502 handle_connection(); /* Initialise parser */ 00503 while(!(uip_aborted() || uip_closed() || uip_timedout())) { 00504 PROCESS_WAIT_EVENT_UNTIL(ev == tcpip_event); 00505 handle_connection(); 00506 } 00507 } 00508 printf("disconnected\n"); 00509 } 00510 00511 exit: 00512 /* Contiki does automatic garbage collection of uIP state and we 00513 * need not worry about that. */ 00514 printf("Stepper exiting\n"); 00515 PROCESS_END(); 00516 } 00517