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@3:42f3821c4e54, 2013-06-07 (annotated)

Committer:
shimniok
Date:
Fri Jun 07 14:45:46 2013 +0000
Revision:
3:42f3821c4e54
Parent:
2:fbc6e3cf3ed8
Child:
18:c2f3df4ef5fe
Working version 6/6/2013. Heading estimation may not be quite perfect yet but seems the major estimation bugs are fixed now.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 3:42f3821c4e54 1 #include <math.h>
shimniok 3:42f3821c4e54 2
shimniok 2:fbc6e3cf3ed8 3 /**
shimniok 2:fbc6e3cf3ed8 4 * Clamp a value (angle) between min (non-inclusive) and max (inclusive)
shimniok 2:fbc6e3cf3ed8 5 * e.g. clamp(v, 0, 360) or clamp(v, -180, 180)
shimniok 2:fbc6e3cf3ed8 6 */
shimniok 3:42f3821c4e54 7 float clamp(float v, float min, float max, bool flip)
shimniok 2:fbc6e3cf3ed8 8 {
shimniok 3:42f3821c4e54 9 float i;
shimniok 3:42f3821c4e54 10 float f;
shimniok 2:fbc6e3cf3ed8 11 float mod = (max - min);
shimniok 3:42f3821c4e54 12
shimniok 3:42f3821c4e54 13 f = modff((v/mod), &i) * mod;
shimniok 3:42f3821c4e54 14 if (flip) {
shimniok 3:42f3821c4e54 15 if (f > max) f -= mod;
shimniok 3:42f3821c4e54 16 if (f <= min) f += mod;
shimniok 3:42f3821c4e54 17 } else {
shimniok 3:42f3821c4e54 18 if (f < min) f += mod;
shimniok 3:42f3821c4e54 19 if (f >= max) f -= mod;
shimniok 3:42f3821c4e54 20 }
shimniok 3:42f3821c4e54 21 return f;
shimniok 2:fbc6e3cf3ed8 22 }
shimniok 2:fbc6e3cf3ed8 23
shimniok 0:a6a169de725f 24 // convert character to an int
shimniok 0:a6a169de725f 25 //
shimniok 0:a6a169de725f 26 int ctoi(char c)
shimniok 0:a6a169de725f 27 {
shimniok 0:a6a169de725f 28 int i=-1;
shimniok 0:a6a169de725f 29
shimniok 0:a6a169de725f 30 if (c >= '0' && c <= '9') {
shimniok 0:a6a169de725f 31 i = c - '0';
shimniok 0:a6a169de725f 32 }
shimniok 0:a6a169de725f 33
shimniok 0:a6a169de725f 34 //printf("char: %c int %d\n", c, i);
shimniok 0:a6a169de725f 35
shimniok 0:a6a169de725f 36 return i;
shimniok 0:a6a169de725f 37 }
shimniok 0:a6a169de725f 38
shimniok 0:a6a169de725f 39
shimniok 0:a6a169de725f 40 // convert string to floating point
shimniok 0:a6a169de725f 41 //
shimniok 0:a6a169de725f 42 double cvstof(char *s)
shimniok 0:a6a169de725f 43 {
shimniok 0:a6a169de725f 44 double f=0.0;
shimniok 0:a6a169de725f 45 double mult = 0.1;
shimniok 0:a6a169de725f 46 bool neg = false;
shimniok 0:a6a169de725f 47 //char dec = 1;
shimniok 0:a6a169de725f 48
shimniok 0:a6a169de725f 49 // leading spaces
shimniok 0:a6a169de725f 50 while (*s == ' ' || *s == '\t') {
shimniok 0:a6a169de725f 51 s++;
shimniok 0:a6a169de725f 52 if (*s == 0) break;
shimniok 0:a6a169de725f 53 }
shimniok 0:a6a169de725f 54
shimniok 0:a6a169de725f 55 // What about negative numbers?
shimniok 0:a6a169de725f 56 if (*s == '-') {
shimniok 0:a6a169de725f 57 neg = true;
shimniok 0:a6a169de725f 58 s++;
shimniok 0:a6a169de725f 59 }
shimniok 0:a6a169de725f 60
shimniok 0:a6a169de725f 61 // before the decimal
shimniok 0:a6a169de725f 62 //
shimniok 0:a6a169de725f 63 while (*s != 0) {
shimniok 0:a6a169de725f 64 if (*s == '.') {
shimniok 0:a6a169de725f 65 s++;
shimniok 0:a6a169de725f 66 break;
shimniok 0:a6a169de725f 67 }
shimniok 0:a6a169de725f 68 f = (f * 10.0) + (double) ctoi(*s);
shimniok 0:a6a169de725f 69 s++;
shimniok 0:a6a169de725f 70 }
shimniok 0:a6a169de725f 71 // after the decimal
shimniok 0:a6a169de725f 72 while (*s != 0 && *s >= '0' && *s <= '9') {
shimniok 0:a6a169de725f 73 f += (double) ctoi(*s) * mult;
shimniok 0:a6a169de725f 74 mult /= 10;
shimniok 0:a6a169de725f 75 s++;
shimniok 0:a6a169de725f 76 }
shimniok 0:a6a169de725f 77
shimniok 0:a6a169de725f 78 // if we were negative...
shimniok 0:a6a169de725f 79 if (neg) f = -f;
shimniok 0:a6a169de725f 80
shimniok 0:a6a169de725f 81 return f;
shimniok 0:a6a169de725f 82 }
shimniok 0:a6a169de725f 83
shimniok 0:a6a169de725f 84 // copy t to s until delimiter is reached
shimniok 0:a6a169de725f 85 // return location of delimiter+1 in t
shimniok 0:a6a169de725f 86 // if s or t null, return null
shimniok 0:a6a169de725f 87 char *split(char *s, char *t, int max, char delim)
shimniok 0:a6a169de725f 88 {
shimniok 0:a6a169de725f 89 int i = 0;
shimniok 0:a6a169de725f 90
shimniok 0:a6a169de725f 91 if (s == 0 || t == 0)
shimniok 0:a6a169de725f 92 return 0;
shimniok 0:a6a169de725f 93
shimniok 0:a6a169de725f 94 while (*t != 0 && *t != '\n' && *t != delim && i < max) {
shimniok 0:a6a169de725f 95 *s++ = *t++;
shimniok 0:a6a169de725f 96 i++;
shimniok 0:a6a169de725f 97 }
shimniok 0:a6a169de725f 98 *s = 0;
shimniok 0:a6a169de725f 99
shimniok 0:a6a169de725f 100 return t+1;
shimniok 0:a6a169de725f 101 }