BaeUtilities.cpp

/********************************************************************************
*  Project              : FIRST Motor Controller
*  File Name            : BaeUtilities.cpp        
*  Contributors         : JDG, ELF, EMF
*  Creation Date        : July 20, 2008
*  Revision History     : Source code & revision history maintained at sourceforge.WPI.edu    
*  File Description     : Open source utility extensions for FIRST Vision API.
*/                            
/*----------------------------------------------------------------------------*/
/*        Copyright (c) FIRST 2008.  All Rights Reserved.                     */
/*  Open Source Software - may be modified and shared by FRC teams. The code  */
/*  must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <string.h>
#include <math.h>
#include <stdioLib.h> 
 
#include "BaeUtilities.h"
#include "Servo.h"
#include "Timer.h"

static DebugOutputType  dprintfFlag = DEBUG_OFF;  

void SetDebugFlag ( DebugOutputType flag  )  
{ dprintfFlag = flag; }

void dprintf ( char * tempString, ...  )  /* Variable argument list */
{
  va_list       args;                     /* Input argument list */
  int           line_number;      /* Line number passed in argument */
  int           type;
  char          *functionName;    /* Format passed in argument */
  char          *fmt;             /* Format passed in argument */
  char          text[512];        /* Text string */
  char          outtext[512];     /* Text string */
  FILE          *outfile_fd;      /* Output file pointer */
  char          filepath[128];    /* Text string */
  int           fatalFlag=0;
  char          *filename;
  int           index;
  int           tempStringLen;

  if (dprintfFlag == DEBUG_OFF) { return; }
  
  va_start (args, tempString);
  
  tempStringLen = strlen(tempString);
  filename = tempString;
  for (index=0;index<tempStringLen;index++){
          if (tempString[index] == ' ') {
                  printf( "ERROR in dprintf: malformed calling sequence (%s)\n",tempString);return;
                }
          if (tempString[index] == '\\' || tempString[index] == '/')
                  filename = tempString + index + 1;
  }
  
  /* Extract function name */
  functionName = va_arg (args, char *);
 
 /* Extract line number from argument list */
  line_number = va_arg (args, int);

 /* Extract information type from argument list */
  type = va_arg (args, int);

 /* Extract format from argument list */
  fmt = va_arg (args, char *);

  vsprintf (text, fmt, args);

  va_end (args);

  /* Format output statement */
  switch (type)
   {
   case DEBUG_TYPE:
     sprintf (outtext, "[%s:%s@%04d] DEBUG %s\n",
              filename, functionName, line_number, text);
     break;
   case INFO_TYPE:
     sprintf (outtext, "[%s:%s@%04d] INFO %s\n",
              filename, functionName, line_number, text);
     break;
   case ERROR_TYPE:
     sprintf (outtext, "[%s:%s@%04d] ERROR %s\n",
              filename, functionName, line_number, text);
     break;
   case CRITICAL_TYPE:
     sprintf (outtext, "[%s:%s@%04d] CRITICAL %s\n",
              filename, functionName, line_number, text);
     break;
   case FATAL_TYPE:
     fatalFlag = 1;
     sprintf (outtext, "[%s:%s@%04d] FATAL %s\n",
              filename, functionName, line_number, text);
     break;
   default:
     printf( "ERROR in dprintf: malformed calling sequence\n");
     return;
     break;
   }

  sprintf (filepath, "%s.debug", filename);

  /* Write output statement */
  switch (dprintfFlag)
  {
  default:
  case DEBUG_OFF: 
        break;
  case DEBUG_MOSTLY_OFF: 
        if (fatalFlag)  {
          if ((outfile_fd = fopen (filepath, "a+")) != NULL)      {
            fwrite (outtext, sizeof (char), strlen (outtext), outfile_fd);
            fclose (outfile_fd);
          }
        }
        break;
  case DEBUG_SCREEN_ONLY: 
        printf ("%s", outtext);
        break;
  case DEBUG_FILE_ONLY: 
        if ((outfile_fd = fopen (filepath, "a+")) != NULL)      {
          fwrite (outtext, sizeof (char), strlen (outtext), outfile_fd);
          fclose (outfile_fd);
        }
        break;
  case DEBUG_SCREEN_AND_FILE: // BOTH
        printf ("%s", outtext);
        if ((outfile_fd = fopen (filepath, "a+")) != NULL)      {
          fwrite (outtext, sizeof (char), strlen (outtext), outfile_fd);
          fclose (outfile_fd);
        }
        break;
  }
}

double RangeToNormalized(double position, int range){
        return(((position*2.0)/(double)range)-1.0);
}

float NormalizeToRange(float normalizedValue, float minRange, float maxRange) {
        float range = maxRange-minRange;
        float temp = (float)((normalizedValue / 2.0)+ 0.5)*range;
        return (temp + minRange);
}       
float NormalizeToRange(float normalizedValue) {
        return (float)((normalizedValue / 2.0) + 0.5);
}       

void ShowActivity (char *fmt, ...)
{
  static char   activity_indication_string[] = "|/-\\";
  static int    ai = 3;
  va_list       args;
  char          text[1024];

  va_start (args, fmt);

  vsprintf (text, fmt, args);

  ai = ai == 3 ? 0 : ai + 1;

  printf ("%c %s \r", activity_indication_string[ai], text);
  fflush (stdout);

  va_end (args);
}

#define PI 3.14159265358979

double SinPosition (double *period, double sinStart)
{
  double rtnVal;
  static double sinePeriod=0.0;
  static double timestamp;
  double sinArg;

  //1st call
  if (period != NULL) {
    sinePeriod = *period;
        timestamp = GetTime();
        return 0.0;
  }

  //Multiplying by 2*pi to the time difference makes sinePeriod work if it's measured in seconds.
  //Adding sinStart to the part multiplied by PI, but not by 2, allows it to work as described in the comments.
  sinArg = PI *((2.0 * (GetTime() - timestamp)) + sinStart) / sinePeriod;
  rtnVal = sin (sinArg);  
  return (rtnVal);
}


double ElapsedTime ( double startTime )  
{
        double realTime = GetTime();    
        return (realTime-startTime);
}

void panInit()  {
        double period = 3.0;    // number of seconds for one complete pan
        SinPosition(&period, 0.0);      // initial call to set up time
}

void panInit(double period)     {
        if (period < 0.0) period=3.0;
        SinPosition(&period, 0.0);      // initial call to set up time
}

void panForTarget(Servo *panServo)      { panForTarget(panServo, 0.0); }

void panForTarget(Servo *panServo, double sinStart)     {
        float normalizedSinPosition = (float)SinPosition(NULL, sinStart);
        float newServoPosition = NormalizeToRange(normalizedSinPosition);
        panServo->Set( newServoPosition );
        //ShowActivity ("pan x: normalized %f newServoPosition = %f" , 
        //              normalizedSinPosition, newServoPosition );
}


int processFile(char *inputFile, char *outputString, int lineNumber)
{
        FILE *infile;
        int stringSize = 80;            // max size of one line in file 
        char inputStr[stringSize];
        struct stat fileStatus;
        int fileSize=0;
        int lineCount=0;
          
        if (lineNumber < 0)
                  return (-1);

        if ((infile = fopen (inputFile, "r")) == NULL) {
            printf ("Fatal error opening file %s\n",inputFile);
            return (0);
        }
    memset (&fileStatus, 0, sizeof(fileStatus));
    if (!stat(inputFile, &fileStatus)) {
      if (S_ISREG(fileStatus.st_mode)) {
        fileSize = fileStatus.st_size;
      }
    }

  while (!feof(infile)) {
    if (fgets (inputStr, stringSize, infile) != NULL) {
      // Skip empty lines
      if (emptyString(inputStr))
        continue;
      // Skip comment lines
      if (inputStr[0] == '#' || inputStr[0] == '!')
        continue;

      lineCount++;
      if (lineNumber == 0)
        continue;
      else
      {
          if (lineCount == lineNumber)
                  break;
      }
    }
  }

  // close file 
  fclose (infile);
  // if number lines requested return the count
  if (lineNumber == 0)
          return (lineCount);
  // check for input out of range
  if (lineNumber > lineCount)
          return (-1);
  // return the line selected
  if (lineCount) {
    stripString(inputStr);
    strcpy(outputString, inputStr);
    return(lineCount);
  } 
  else {
    return(-1);
  }
}

int emptyString(char *string)
{
  int i,len;

  if(string == NULL)
    return(1);

  len = strlen(string);
  for(i=0; i<len; i++) {
    // Ignore the following characters
    if (string[i] == '\n' || string[i] == '\r' ||
        string[i] == '\t' || string[i] == ' ')
      continue;
    return(0);
  }
  return(1);
}

void stripString(char *string)
{
  int i,j,len;

  if(string == NULL)
    return;

  len = strlen(string);
  for(i=0,j=0; i<len; i++) {
    // Remove the following characters from the string
    if (string[i] == '\n' || string[i] == '\r' || string[i] == '\"')
      continue;
    // Copy anything else
    string[j++] = string[i];
  }
  string[j] = '\0'; 
}