![](/media/cache/profiles/2012_sf_avc_095.jpg.50x50_q85.jpg)
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@18:c2f3df4ef5fe, 2018-11-29 (annotated)
- Committer:
- shimniok
- Date:
- Thu Nov 29 17:21:37 2018 +0000
- Revision:
- 18:c2f3df4ef5fe
- Parent:
- 3:42f3821c4e54
Added Telemetry, updated util files
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimniok | 3:42f3821c4e54 | 1 | #include <math.h> |
shimniok | 18:c2f3df4ef5fe | 2 | #include <string.h> |
shimniok | 18:c2f3df4ef5fe | 3 | |
shimniok | 18:c2f3df4ef5fe | 4 | #define MAXBUF 32 |
shimniok | 18:c2f3df4ef5fe | 5 | #define MAXDIGITS 10 |
shimniok | 3:42f3821c4e54 | 6 | |
shimniok | 2:fbc6e3cf3ed8 | 7 | /** |
shimniok | 2:fbc6e3cf3ed8 | 8 | * Clamp a value (angle) between min (non-inclusive) and max (inclusive) |
shimniok | 2:fbc6e3cf3ed8 | 9 | * e.g. clamp(v, 0, 360) or clamp(v, -180, 180) |
shimniok | 2:fbc6e3cf3ed8 | 10 | */ |
shimniok | 3:42f3821c4e54 | 11 | float clamp(float v, float min, float max, bool flip) |
shimniok | 2:fbc6e3cf3ed8 | 12 | { |
shimniok | 3:42f3821c4e54 | 13 | float i; |
shimniok | 3:42f3821c4e54 | 14 | float f; |
shimniok | 2:fbc6e3cf3ed8 | 15 | float mod = (max - min); |
shimniok | 3:42f3821c4e54 | 16 | |
shimniok | 3:42f3821c4e54 | 17 | f = modff((v/mod), &i) * mod; |
shimniok | 3:42f3821c4e54 | 18 | if (flip) { |
shimniok | 3:42f3821c4e54 | 19 | if (f > max) f -= mod; |
shimniok | 3:42f3821c4e54 | 20 | if (f <= min) f += mod; |
shimniok | 3:42f3821c4e54 | 21 | } else { |
shimniok | 3:42f3821c4e54 | 22 | if (f < min) f += mod; |
shimniok | 3:42f3821c4e54 | 23 | if (f >= max) f -= mod; |
shimniok | 3:42f3821c4e54 | 24 | } |
shimniok | 3:42f3821c4e54 | 25 | return f; |
shimniok | 2:fbc6e3cf3ed8 | 26 | } |
shimniok | 2:fbc6e3cf3ed8 | 27 | |
shimniok | 0:a6a169de725f | 28 | // convert character to an int |
shimniok | 0:a6a169de725f | 29 | // |
shimniok | 0:a6a169de725f | 30 | int ctoi(char c) |
shimniok | 0:a6a169de725f | 31 | { |
shimniok | 0:a6a169de725f | 32 | int i=-1; |
shimniok | 0:a6a169de725f | 33 | |
shimniok | 0:a6a169de725f | 34 | if (c >= '0' && c <= '9') { |
shimniok | 0:a6a169de725f | 35 | i = c - '0'; |
shimniok | 0:a6a169de725f | 36 | } |
shimniok | 0:a6a169de725f | 37 | |
shimniok | 0:a6a169de725f | 38 | //printf("char: %c int %d\n", c, i); |
shimniok | 0:a6a169de725f | 39 | |
shimniok | 0:a6a169de725f | 40 | return i; |
shimniok | 0:a6a169de725f | 41 | } |
shimniok | 0:a6a169de725f | 42 | |
shimniok | 0:a6a169de725f | 43 | |
shimniok | 0:a6a169de725f | 44 | // convert string to floating point |
shimniok | 0:a6a169de725f | 45 | // |
shimniok | 0:a6a169de725f | 46 | double cvstof(char *s) |
shimniok | 0:a6a169de725f | 47 | { |
shimniok | 0:a6a169de725f | 48 | double f=0.0; |
shimniok | 0:a6a169de725f | 49 | double mult = 0.1; |
shimniok | 0:a6a169de725f | 50 | bool neg = false; |
shimniok | 0:a6a169de725f | 51 | //char dec = 1; |
shimniok | 0:a6a169de725f | 52 | |
shimniok | 0:a6a169de725f | 53 | // leading spaces |
shimniok | 0:a6a169de725f | 54 | while (*s == ' ' || *s == '\t') { |
shimniok | 0:a6a169de725f | 55 | s++; |
shimniok | 0:a6a169de725f | 56 | if (*s == 0) break; |
shimniok | 0:a6a169de725f | 57 | } |
shimniok | 0:a6a169de725f | 58 | |
shimniok | 0:a6a169de725f | 59 | // What about negative numbers? |
shimniok | 0:a6a169de725f | 60 | if (*s == '-') { |
shimniok | 0:a6a169de725f | 61 | neg = true; |
shimniok | 0:a6a169de725f | 62 | s++; |
shimniok | 0:a6a169de725f | 63 | } |
shimniok | 0:a6a169de725f | 64 | |
shimniok | 0:a6a169de725f | 65 | // before the decimal |
shimniok | 0:a6a169de725f | 66 | // |
shimniok | 0:a6a169de725f | 67 | while (*s != 0) { |
shimniok | 0:a6a169de725f | 68 | if (*s == '.') { |
shimniok | 0:a6a169de725f | 69 | s++; |
shimniok | 0:a6a169de725f | 70 | break; |
shimniok | 0:a6a169de725f | 71 | } |
shimniok | 0:a6a169de725f | 72 | f = (f * 10.0) + (double) ctoi(*s); |
shimniok | 0:a6a169de725f | 73 | s++; |
shimniok | 0:a6a169de725f | 74 | } |
shimniok | 0:a6a169de725f | 75 | // after the decimal |
shimniok | 0:a6a169de725f | 76 | while (*s != 0 && *s >= '0' && *s <= '9') { |
shimniok | 0:a6a169de725f | 77 | f += (double) ctoi(*s) * mult; |
shimniok | 0:a6a169de725f | 78 | mult /= 10; |
shimniok | 0:a6a169de725f | 79 | s++; |
shimniok | 0:a6a169de725f | 80 | } |
shimniok | 0:a6a169de725f | 81 | |
shimniok | 0:a6a169de725f | 82 | // if we were negative... |
shimniok | 0:a6a169de725f | 83 | if (neg) f = -f; |
shimniok | 0:a6a169de725f | 84 | |
shimniok | 0:a6a169de725f | 85 | return f; |
shimniok | 0:a6a169de725f | 86 | } |
shimniok | 0:a6a169de725f | 87 | |
shimniok | 18:c2f3df4ef5fe | 88 | |
shimniok | 18:c2f3df4ef5fe | 89 | char *cvntos(unsigned long n) |
shimniok | 18:c2f3df4ef5fe | 90 | { |
shimniok | 18:c2f3df4ef5fe | 91 | static char buf[MAXBUF+1]; // +1 null termination |
shimniok | 18:c2f3df4ef5fe | 92 | char *str = buf+MAXBUF; |
shimniok | 18:c2f3df4ef5fe | 93 | int i; |
shimniok | 18:c2f3df4ef5fe | 94 | |
shimniok | 18:c2f3df4ef5fe | 95 | // ensure null termination |
shimniok | 18:c2f3df4ef5fe | 96 | *str-- = '\0'; |
shimniok | 18:c2f3df4ef5fe | 97 | |
shimniok | 18:c2f3df4ef5fe | 98 | for (i = 0; i < MAXBUF; i++) { |
shimniok | 18:c2f3df4ef5fe | 99 | unsigned long m = n; |
shimniok | 18:c2f3df4ef5fe | 100 | n /= 10; |
shimniok | 18:c2f3df4ef5fe | 101 | char c = m - 10 * n; |
shimniok | 18:c2f3df4ef5fe | 102 | *--str = c + '0'; |
shimniok | 18:c2f3df4ef5fe | 103 | if (n == 0) break; |
shimniok | 18:c2f3df4ef5fe | 104 | } |
shimniok | 18:c2f3df4ef5fe | 105 | |
shimniok | 18:c2f3df4ef5fe | 106 | return str; |
shimniok | 18:c2f3df4ef5fe | 107 | } |
shimniok | 18:c2f3df4ef5fe | 108 | |
shimniok | 18:c2f3df4ef5fe | 109 | |
shimniok | 18:c2f3df4ef5fe | 110 | |
shimniok | 18:c2f3df4ef5fe | 111 | char *cvitos(long n) |
shimniok | 18:c2f3df4ef5fe | 112 | { |
shimniok | 18:c2f3df4ef5fe | 113 | static char buf[MAXBUF+2]; // +1 for sign, +1 for null termination |
shimniok | 18:c2f3df4ef5fe | 114 | char *str = buf; |
shimniok | 18:c2f3df4ef5fe | 115 | |
shimniok | 18:c2f3df4ef5fe | 116 | *str = '\0'; |
shimniok | 18:c2f3df4ef5fe | 117 | if (n < 0) { |
shimniok | 18:c2f3df4ef5fe | 118 | *str++ = '-'; |
shimniok | 18:c2f3df4ef5fe | 119 | *str = '\0'; |
shimniok | 18:c2f3df4ef5fe | 120 | n = -n; |
shimniok | 18:c2f3df4ef5fe | 121 | } |
shimniok | 18:c2f3df4ef5fe | 122 | strcat(str, cvntos(n)); |
shimniok | 18:c2f3df4ef5fe | 123 | |
shimniok | 18:c2f3df4ef5fe | 124 | return buf; |
shimniok | 18:c2f3df4ef5fe | 125 | } |
shimniok | 18:c2f3df4ef5fe | 126 | |
shimniok | 18:c2f3df4ef5fe | 127 | |
shimniok | 18:c2f3df4ef5fe | 128 | |
shimniok | 18:c2f3df4ef5fe | 129 | char *cvftos(double number, int digits) |
shimniok | 18:c2f3df4ef5fe | 130 | { |
shimniok | 18:c2f3df4ef5fe | 131 | static char buf[MAXBUF+3+MAXDIGITS]; // +1 for termination, +1 for sign, +1 for ., +MAXDIGITS for digits |
shimniok | 18:c2f3df4ef5fe | 132 | char *str = buf; |
shimniok | 18:c2f3df4ef5fe | 133 | int i; |
shimniok | 18:c2f3df4ef5fe | 134 | |
shimniok | 18:c2f3df4ef5fe | 135 | // ensure proper null termination |
shimniok | 18:c2f3df4ef5fe | 136 | *str = '\0'; |
shimniok | 18:c2f3df4ef5fe | 137 | |
shimniok | 18:c2f3df4ef5fe | 138 | // Limited buffer space for decimals |
shimniok | 18:c2f3df4ef5fe | 139 | if (digits > MAXDIGITS) |
shimniok | 18:c2f3df4ef5fe | 140 | digits = MAXDIGITS; |
shimniok | 18:c2f3df4ef5fe | 141 | |
shimniok | 18:c2f3df4ef5fe | 142 | if (isnan(number)) { |
shimniok | 18:c2f3df4ef5fe | 143 | strcpy(buf, "nan"); |
shimniok | 18:c2f3df4ef5fe | 144 | } else if (isinf(number)) { |
shimniok | 18:c2f3df4ef5fe | 145 | strcpy(buf, "inf"); |
shimniok | 18:c2f3df4ef5fe | 146 | } else if (number > 4294967040.0 || number < -4294967040.0) { // constant determined empirically |
shimniok | 18:c2f3df4ef5fe | 147 | strcpy(buf, "ovf"); |
shimniok | 18:c2f3df4ef5fe | 148 | } else { |
shimniok | 18:c2f3df4ef5fe | 149 | |
shimniok | 18:c2f3df4ef5fe | 150 | // Handle negative numbers |
shimniok | 18:c2f3df4ef5fe | 151 | if (number < 0.0) { |
shimniok | 18:c2f3df4ef5fe | 152 | // Add the sign |
shimniok | 18:c2f3df4ef5fe | 153 | strcat(str, "-"); |
shimniok | 18:c2f3df4ef5fe | 154 | number = -number; |
shimniok | 18:c2f3df4ef5fe | 155 | } |
shimniok | 18:c2f3df4ef5fe | 156 | |
shimniok | 18:c2f3df4ef5fe | 157 | // Round correctly so that print(1.999, 2) prints as "2.00" |
shimniok | 18:c2f3df4ef5fe | 158 | double rounding = 0.5; |
shimniok | 18:c2f3df4ef5fe | 159 | for (i=0; i < digits; i++) |
shimniok | 18:c2f3df4ef5fe | 160 | rounding /= 10.0; |
shimniok | 18:c2f3df4ef5fe | 161 | number += rounding; |
shimniok | 18:c2f3df4ef5fe | 162 | |
shimniok | 18:c2f3df4ef5fe | 163 | // Extract the integer part of the number and print it |
shimniok | 18:c2f3df4ef5fe | 164 | unsigned long int_part = (unsigned long)number; |
shimniok | 18:c2f3df4ef5fe | 165 | double remainder = number - (double)int_part; |
shimniok | 18:c2f3df4ef5fe | 166 | |
shimniok | 18:c2f3df4ef5fe | 167 | // Add the integer part |
shimniok | 18:c2f3df4ef5fe | 168 | strcat(str, cvntos(int_part)); |
shimniok | 18:c2f3df4ef5fe | 169 | // Add the decimal point |
shimniok | 18:c2f3df4ef5fe | 170 | char *s = str + strlen(str); |
shimniok | 18:c2f3df4ef5fe | 171 | *s++ = '.'; |
shimniok | 18:c2f3df4ef5fe | 172 | |
shimniok | 18:c2f3df4ef5fe | 173 | // Extract digits from the remainder one at a time |
shimniok | 18:c2f3df4ef5fe | 174 | while (digits-- > 0) { |
shimniok | 18:c2f3df4ef5fe | 175 | remainder *= 10.0; |
shimniok | 18:c2f3df4ef5fe | 176 | int toPrint = (int) remainder; |
shimniok | 18:c2f3df4ef5fe | 177 | *s++ = toPrint + '0'; |
shimniok | 18:c2f3df4ef5fe | 178 | remainder -= (double) toPrint; |
shimniok | 18:c2f3df4ef5fe | 179 | } |
shimniok | 18:c2f3df4ef5fe | 180 | *s = '\0'; |
shimniok | 18:c2f3df4ef5fe | 181 | } |
shimniok | 18:c2f3df4ef5fe | 182 | |
shimniok | 18:c2f3df4ef5fe | 183 | return str; |
shimniok | 18:c2f3df4ef5fe | 184 | } |
shimniok | 18:c2f3df4ef5fe | 185 | |
shimniok | 18:c2f3df4ef5fe | 186 | |
shimniok | 0:a6a169de725f | 187 | // copy t to s until delimiter is reached |
shimniok | 0:a6a169de725f | 188 | // return location of delimiter+1 in t |
shimniok | 0:a6a169de725f | 189 | // if s or t null, return null |
shimniok | 0:a6a169de725f | 190 | char *split(char *s, char *t, int max, char delim) |
shimniok | 0:a6a169de725f | 191 | { |
shimniok | 0:a6a169de725f | 192 | int i = 0; |
shimniok | 18:c2f3df4ef5fe | 193 | |
shimniok | 0:a6a169de725f | 194 | if (s == 0 || t == 0) |
shimniok | 0:a6a169de725f | 195 | return 0; |
shimniok | 0:a6a169de725f | 196 | |
shimniok | 0:a6a169de725f | 197 | while (*t != 0 && *t != '\n' && *t != delim && i < max) { |
shimniok | 0:a6a169de725f | 198 | *s++ = *t++; |
shimniok | 0:a6a169de725f | 199 | i++; |
shimniok | 0:a6a169de725f | 200 | } |
shimniok | 0:a6a169de725f | 201 | *s = 0; |
shimniok | 18:c2f3df4ef5fe | 202 | |
shimniok | 0:a6a169de725f | 203 | return t+1; |
shimniok | 0:a6a169de725f | 204 | } |