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@2:fbc6e3cf3ed8, 2013-06-06 (annotated)
- Committer:
- shimniok
- Date:
- Thu Jun 06 13:40:23 2013 +0000
- Revision:
- 2:fbc6e3cf3ed8
- Parent:
- 0:a6a169de725f
- Child:
- 3:42f3821c4e54
Sort-of working version, still some errors with estimation. Added clamp() function.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimniok | 2:fbc6e3cf3ed8 | 1 | /** |
shimniok | 2:fbc6e3cf3ed8 | 2 | * Clamp a value (angle) between min (non-inclusive) and max (inclusive) |
shimniok | 2:fbc6e3cf3ed8 | 3 | * e.g. clamp(v, 0, 360) or clamp(v, -180, 180) |
shimniok | 2:fbc6e3cf3ed8 | 4 | */ |
shimniok | 2:fbc6e3cf3ed8 | 5 | float clamp(float v, float min, float max) |
shimniok | 2:fbc6e3cf3ed8 | 6 | { |
shimniok | 2:fbc6e3cf3ed8 | 7 | float mod = (max - min); |
shimniok | 2:fbc6e3cf3ed8 | 8 | if (v >= max) float -= mod; |
shimniok | 2:fbc6e3cf3ed8 | 9 | if (v < min) float += mod; |
shimniok | 2:fbc6e3cf3ed8 | 10 | } |
shimniok | 2:fbc6e3cf3ed8 | 11 | |
shimniok | 0:a6a169de725f | 12 | // convert character to an int |
shimniok | 0:a6a169de725f | 13 | // |
shimniok | 0:a6a169de725f | 14 | int ctoi(char c) |
shimniok | 0:a6a169de725f | 15 | { |
shimniok | 0:a6a169de725f | 16 | int i=-1; |
shimniok | 0:a6a169de725f | 17 | |
shimniok | 0:a6a169de725f | 18 | if (c >= '0' && c <= '9') { |
shimniok | 0:a6a169de725f | 19 | i = c - '0'; |
shimniok | 0:a6a169de725f | 20 | } |
shimniok | 0:a6a169de725f | 21 | |
shimniok | 0:a6a169de725f | 22 | //printf("char: %c int %d\n", c, i); |
shimniok | 0:a6a169de725f | 23 | |
shimniok | 0:a6a169de725f | 24 | return i; |
shimniok | 0:a6a169de725f | 25 | } |
shimniok | 0:a6a169de725f | 26 | |
shimniok | 0:a6a169de725f | 27 | |
shimniok | 0:a6a169de725f | 28 | // convert string to floating point |
shimniok | 0:a6a169de725f | 29 | // |
shimniok | 0:a6a169de725f | 30 | double cvstof(char *s) |
shimniok | 0:a6a169de725f | 31 | { |
shimniok | 0:a6a169de725f | 32 | double f=0.0; |
shimniok | 0:a6a169de725f | 33 | double mult = 0.1; |
shimniok | 0:a6a169de725f | 34 | bool neg = false; |
shimniok | 0:a6a169de725f | 35 | //char dec = 1; |
shimniok | 0:a6a169de725f | 36 | |
shimniok | 0:a6a169de725f | 37 | // leading spaces |
shimniok | 0:a6a169de725f | 38 | while (*s == ' ' || *s == '\t') { |
shimniok | 0:a6a169de725f | 39 | s++; |
shimniok | 0:a6a169de725f | 40 | if (*s == 0) break; |
shimniok | 0:a6a169de725f | 41 | } |
shimniok | 0:a6a169de725f | 42 | |
shimniok | 0:a6a169de725f | 43 | // What about negative numbers? |
shimniok | 0:a6a169de725f | 44 | if (*s == '-') { |
shimniok | 0:a6a169de725f | 45 | neg = true; |
shimniok | 0:a6a169de725f | 46 | s++; |
shimniok | 0:a6a169de725f | 47 | } |
shimniok | 0:a6a169de725f | 48 | |
shimniok | 0:a6a169de725f | 49 | // before the decimal |
shimniok | 0:a6a169de725f | 50 | // |
shimniok | 0:a6a169de725f | 51 | while (*s != 0) { |
shimniok | 0:a6a169de725f | 52 | if (*s == '.') { |
shimniok | 0:a6a169de725f | 53 | s++; |
shimniok | 0:a6a169de725f | 54 | break; |
shimniok | 0:a6a169de725f | 55 | } |
shimniok | 0:a6a169de725f | 56 | f = (f * 10.0) + (double) ctoi(*s); |
shimniok | 0:a6a169de725f | 57 | s++; |
shimniok | 0:a6a169de725f | 58 | } |
shimniok | 0:a6a169de725f | 59 | // after the decimal |
shimniok | 0:a6a169de725f | 60 | while (*s != 0 && *s >= '0' && *s <= '9') { |
shimniok | 0:a6a169de725f | 61 | f += (double) ctoi(*s) * mult; |
shimniok | 0:a6a169de725f | 62 | mult /= 10; |
shimniok | 0:a6a169de725f | 63 | s++; |
shimniok | 0:a6a169de725f | 64 | } |
shimniok | 0:a6a169de725f | 65 | |
shimniok | 0:a6a169de725f | 66 | // if we were negative... |
shimniok | 0:a6a169de725f | 67 | if (neg) f = -f; |
shimniok | 0:a6a169de725f | 68 | |
shimniok | 0:a6a169de725f | 69 | return f; |
shimniok | 0:a6a169de725f | 70 | } |
shimniok | 0:a6a169de725f | 71 | |
shimniok | 0:a6a169de725f | 72 | // copy t to s until delimiter is reached |
shimniok | 0:a6a169de725f | 73 | // return location of delimiter+1 in t |
shimniok | 0:a6a169de725f | 74 | // if s or t null, return null |
shimniok | 0:a6a169de725f | 75 | char *split(char *s, char *t, int max, char delim) |
shimniok | 0:a6a169de725f | 76 | { |
shimniok | 0:a6a169de725f | 77 | int i = 0; |
shimniok | 0:a6a169de725f | 78 | |
shimniok | 0:a6a169de725f | 79 | if (s == 0 || t == 0) |
shimniok | 0:a6a169de725f | 80 | return 0; |
shimniok | 0:a6a169de725f | 81 | |
shimniok | 0:a6a169de725f | 82 | while (*t != 0 && *t != '\n' && *t != delim && i < max) { |
shimniok | 0:a6a169de725f | 83 | *s++ = *t++; |
shimniok | 0:a6a169de725f | 84 | i++; |
shimniok | 0:a6a169de725f | 85 | } |
shimniok | 0:a6a169de725f | 86 | *s = 0; |
shimniok | 0:a6a169de725f | 87 | |
shimniok | 0:a6a169de725f | 88 | return t+1; |
shimniok | 0:a6a169de725f | 89 | } |