Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.
Dependencies: mbed Watchdog SDFileSystem DigoleSerialDisp
util.cpp@0:a6a169de725f, 2013-05-27 (annotated)
- Committer:
- shimniok
- Date:
- Mon May 27 13:26:03 2013 +0000
- Revision:
- 0:a6a169de725f
- Child:
- 2:fbc6e3cf3ed8
Working version with priorities set and update time display
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimniok | 0:a6a169de725f | 1 | // convert character to an int |
shimniok | 0:a6a169de725f | 2 | // |
shimniok | 0:a6a169de725f | 3 | int ctoi(char c) |
shimniok | 0:a6a169de725f | 4 | { |
shimniok | 0:a6a169de725f | 5 | int i=-1; |
shimniok | 0:a6a169de725f | 6 | |
shimniok | 0:a6a169de725f | 7 | if (c >= '0' && c <= '9') { |
shimniok | 0:a6a169de725f | 8 | i = c - '0'; |
shimniok | 0:a6a169de725f | 9 | } |
shimniok | 0:a6a169de725f | 10 | |
shimniok | 0:a6a169de725f | 11 | //printf("char: %c int %d\n", c, i); |
shimniok | 0:a6a169de725f | 12 | |
shimniok | 0:a6a169de725f | 13 | return i; |
shimniok | 0:a6a169de725f | 14 | } |
shimniok | 0:a6a169de725f | 15 | |
shimniok | 0:a6a169de725f | 16 | |
shimniok | 0:a6a169de725f | 17 | // convert string to floating point |
shimniok | 0:a6a169de725f | 18 | // |
shimniok | 0:a6a169de725f | 19 | double cvstof(char *s) |
shimniok | 0:a6a169de725f | 20 | { |
shimniok | 0:a6a169de725f | 21 | double f=0.0; |
shimniok | 0:a6a169de725f | 22 | double mult = 0.1; |
shimniok | 0:a6a169de725f | 23 | bool neg = false; |
shimniok | 0:a6a169de725f | 24 | //char dec = 1; |
shimniok | 0:a6a169de725f | 25 | |
shimniok | 0:a6a169de725f | 26 | // leading spaces |
shimniok | 0:a6a169de725f | 27 | while (*s == ' ' || *s == '\t') { |
shimniok | 0:a6a169de725f | 28 | s++; |
shimniok | 0:a6a169de725f | 29 | if (*s == 0) break; |
shimniok | 0:a6a169de725f | 30 | } |
shimniok | 0:a6a169de725f | 31 | |
shimniok | 0:a6a169de725f | 32 | // What about negative numbers? |
shimniok | 0:a6a169de725f | 33 | if (*s == '-') { |
shimniok | 0:a6a169de725f | 34 | neg = true; |
shimniok | 0:a6a169de725f | 35 | s++; |
shimniok | 0:a6a169de725f | 36 | } |
shimniok | 0:a6a169de725f | 37 | |
shimniok | 0:a6a169de725f | 38 | // before the decimal |
shimniok | 0:a6a169de725f | 39 | // |
shimniok | 0:a6a169de725f | 40 | while (*s != 0) { |
shimniok | 0:a6a169de725f | 41 | if (*s == '.') { |
shimniok | 0:a6a169de725f | 42 | s++; |
shimniok | 0:a6a169de725f | 43 | break; |
shimniok | 0:a6a169de725f | 44 | } |
shimniok | 0:a6a169de725f | 45 | f = (f * 10.0) + (double) ctoi(*s); |
shimniok | 0:a6a169de725f | 46 | s++; |
shimniok | 0:a6a169de725f | 47 | } |
shimniok | 0:a6a169de725f | 48 | // after the decimal |
shimniok | 0:a6a169de725f | 49 | while (*s != 0 && *s >= '0' && *s <= '9') { |
shimniok | 0:a6a169de725f | 50 | f += (double) ctoi(*s) * mult; |
shimniok | 0:a6a169de725f | 51 | mult /= 10; |
shimniok | 0:a6a169de725f | 52 | s++; |
shimniok | 0:a6a169de725f | 53 | } |
shimniok | 0:a6a169de725f | 54 | |
shimniok | 0:a6a169de725f | 55 | // if we were negative... |
shimniok | 0:a6a169de725f | 56 | if (neg) f = -f; |
shimniok | 0:a6a169de725f | 57 | |
shimniok | 0:a6a169de725f | 58 | return f; |
shimniok | 0:a6a169de725f | 59 | } |
shimniok | 0:a6a169de725f | 60 | |
shimniok | 0:a6a169de725f | 61 | // copy t to s until delimiter is reached |
shimniok | 0:a6a169de725f | 62 | // return location of delimiter+1 in t |
shimniok | 0:a6a169de725f | 63 | // if s or t null, return null |
shimniok | 0:a6a169de725f | 64 | char *split(char *s, char *t, int max, char delim) |
shimniok | 0:a6a169de725f | 65 | { |
shimniok | 0:a6a169de725f | 66 | int i = 0; |
shimniok | 0:a6a169de725f | 67 | |
shimniok | 0:a6a169de725f | 68 | if (s == 0 || t == 0) |
shimniok | 0:a6a169de725f | 69 | return 0; |
shimniok | 0:a6a169de725f | 70 | |
shimniok | 0:a6a169de725f | 71 | while (*t != 0 && *t != '\n' && *t != delim && i < max) { |
shimniok | 0:a6a169de725f | 72 | *s++ = *t++; |
shimniok | 0:a6a169de725f | 73 | i++; |
shimniok | 0:a6a169de725f | 74 | } |
shimniok | 0:a6a169de725f | 75 | *s = 0; |
shimniok | 0:a6a169de725f | 76 | |
shimniok | 0:a6a169de725f | 77 | return t+1; |
shimniok | 0:a6a169de725f | 78 | } |