Contiki 2.6

stepper-process.c

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