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

Committer:
shimniok
Date:
2013-06-06
Revision:
2:fbc6e3cf3ed8
Parent:
0:a6a169de725f
Child:
3:42f3821c4e54

File content as of revision 2:fbc6e3cf3ed8:

/** 
 * Clamp a value (angle) between min (non-inclusive) and max (inclusive)
 * e.g. clamp(v, 0, 360) or clamp(v, -180, 180)
 */
float clamp(float v, float min, float max) 
{
    float mod = (max - min);
    if (v >= max) float -= mod;
    if (v < min) float += mod;
}

// convert character to an int
//
int ctoi(char c)
{
  int i=-1;
  
  if (c >= '0' && c <= '9') {
    i = c - '0';
  }

  //printf("char: %c  int %d\n", c, i); 
 
  return i;
}


// convert string to floating point
//
double cvstof(char *s)
{
  double f=0.0;
  double mult = 0.1;
  bool neg = false;
  //char dec = 1;
  
  // leading spaces
  while (*s == ' ' || *s == '\t') {
    s++;
    if (*s == 0) break;
  }

  // What about negative numbers?
  if (*s == '-') {
    neg = true;
    s++;
  }

  // before the decimal
  //
  while (*s != 0) {
    if (*s == '.') {
      s++;
      break;
    }
    f = (f * 10.0) + (double) ctoi(*s);
    s++;
  }
  // after the decimal
  while (*s != 0 && *s >= '0' && *s <= '9') {
    f += (double) ctoi(*s) * mult;
    mult /= 10;
    s++;
  }
  
  // if we were negative...
  if (neg) f = -f;
  
  return f;
}

// 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;

  while (*t != 0 && *t != '\n' && *t != delim && i < max) {
    *s++ = *t++;
    i++;
  }
  *s = 0;
    
  return t+1;
}