Michael Shimniok / Mbed 2 deprecated DataBus

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@25:bb5356402687, 2018-11-30 (annotated)

Committer:
shimniok
Date:
Fri Nov 30 16:11:53 2018 +0000
Revision:
25:bb5356402687
Parent:
18:c2f3df4ef5fe 
Initial publish of revised version.

Who changed what in which revision?

UserRevisionLine numberNew 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 }