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@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?

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