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
Diff: util.cpp
- Revision:
- 18:c2f3df4ef5fe
- Parent:
- 3:42f3821c4e54
--- a/util.cpp Thu Nov 29 17:17:52 2018 +0000 +++ b/util.cpp Thu Nov 29 17:21:37 2018 +0000 @@ -1,4 +1,8 @@ #include <math.h> +#include <string.h> + +#define MAXBUF 32 +#define MAXDIGITS 10 /** * Clamp a value (angle) between min (non-inclusive) and max (inclusive) @@ -81,13 +85,112 @@ return f; } + +char *cvntos(unsigned long n) +{ + static char buf[MAXBUF+1]; // +1 null termination + char *str = buf+MAXBUF; + int i; + + // ensure null termination + *str-- = '\0'; + + for (i = 0; i < MAXBUF; i++) { + unsigned long m = n; + n /= 10; + char c = m - 10 * n; + *--str = c + '0'; + if (n == 0) break; + } + + return str; +} + + + +char *cvitos(long n) +{ + static char buf[MAXBUF+2]; // +1 for sign, +1 for null termination + char *str = buf; + + *str = '\0'; + if (n < 0) { + *str++ = '-'; + *str = '\0'; + n = -n; + } + strcat(str, cvntos(n)); + + return buf; +} + + + +char *cvftos(double number, int digits) +{ + static char buf[MAXBUF+3+MAXDIGITS]; // +1 for termination, +1 for sign, +1 for ., +MAXDIGITS for digits + char *str = buf; + int i; + + // ensure proper null termination + *str = '\0'; + + // Limited buffer space for decimals + if (digits > MAXDIGITS) + digits = MAXDIGITS; + + if (isnan(number)) { + strcpy(buf, "nan"); + } else if (isinf(number)) { + strcpy(buf, "inf"); + } else if (number > 4294967040.0 || number < -4294967040.0) { // constant determined empirically + strcpy(buf, "ovf"); + } else { + + // Handle negative numbers + if (number < 0.0) { + // Add the sign + strcat(str, "-"); + number = -number; + } + + // Round correctly so that print(1.999, 2) prints as "2.00" + double rounding = 0.5; + for (i=0; i < digits; i++) + rounding /= 10.0; + number += rounding; + + // Extract the integer part of the number and print it + unsigned long int_part = (unsigned long)number; + double remainder = number - (double)int_part; + + // Add the integer part + strcat(str, cvntos(int_part)); + // Add the decimal point + char *s = str + strlen(str); + *s++ = '.'; + + // Extract digits from the remainder one at a time + while (digits-- > 0) { + remainder *= 10.0; + int toPrint = (int) remainder; + *s++ = toPrint + '0'; + remainder -= (double) toPrint; + } + *s = '\0'; + } + + return str; +} + + // copy t to s until delimiter is reached // return location of delimiter+1 in t // if s or t null, return null char *split(char *s, char *t, int max, char delim) { int i = 0; - + if (s == 0 || t == 0) return 0; @@ -96,6 +199,6 @@ i++; } *s = 0; - + return t+1; }