WPILibC++  trunk
BaeUtilities.cpp
1 /********************************************************************************
2 * Project : FIRST Motor Controller
3 * File Name : BaeUtilities.cpp
4 * Contributors : JDG, ELF, EMF
5 * Creation Date : July 20, 2008
6 * Revision History : Source code & revision history maintained at sourceforge.WPI.edu
7 * File Description : Open source utility extensions for FIRST Vision API.
8 */
9 /*----------------------------------------------------------------------------*/
10 /* Copyright (c) FIRST 2008. All Rights Reserved. */
11 /* Open Source Software - may be modified and shared by FRC teams. The code */
12 /* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
13 /*----------------------------------------------------------------------------*/
14 #include <stdio.h>
15 #include <sys/types.h>
16 #include <sys/stat.h>
17 #include <unistd.h>
18 #include <string.h>
19 #include <math.h>
20 #include <stdioLib.h>
21 
22 #include "BaeUtilities.h"
23 #include "Servo.h"
24 #include "Timer.h"
25 
34 static DebugOutputType dprintfFlag = DEBUG_OFF;
35 
40 void SetDebugFlag ( DebugOutputType flag )
41 { dprintfFlag = flag; }
42 
50 void dprintf ( const char * tempString, ... ) /* Variable argument list */
51 {
52  va_list args; /* Input argument list */
53  int line_number; /* Line number passed in argument */
54  int type;
55  const char *functionName; /* Format passed in argument */
56  const char *fmt; /* Format passed in argument */
57  char text[512]; /* Text string */
58  char outtext[512]; /* Text string */
59  FILE *outfile_fd; /* Output file pointer */
60  char filepath[128]; /* Text string */
61  int fatalFlag=0;
62  const char *filename;
63  int index;
64  int tempStringLen;
65 
66  if (dprintfFlag == DEBUG_OFF) { return; }
67 
68  va_start (args, tempString);
69 
70  tempStringLen = strlen(tempString);
71  filename = tempString;
72  for (index=0;index<tempStringLen;index++){
73  if (tempString[index] == ' ') {
74  printf( "ERROR in dprintf: malformed calling sequence (%s)\n",tempString);return;
75  }
76  if (tempString[index] == '\\' || tempString[index] == '/')
77  filename = tempString + index + 1;
78  }
79 
80  /* Extract function name */
81  functionName = va_arg (args, const char *);
82 
83  /* Extract line number from argument list */
84  line_number = va_arg (args, int);
85 
86  /* Extract information type from argument list */
87  type = va_arg (args, int);
88 
89  /* Extract format from argument list */
90  fmt = va_arg (args, const char *);
91 
92  vsprintf (text, fmt, args);
93 
94  va_end (args);
95 
96  /* Format output statement */
97  switch (type)
98  {
99  case DEBUG_TYPE:
100  sprintf (outtext, "[%s:%s@%04d] DEBUG %s\n",
101  filename, functionName, line_number, text);
102  break;
103  case INFO_TYPE:
104  sprintf (outtext, "[%s:%s@%04d] INFO %s\n",
105  filename, functionName, line_number, text);
106  break;
107  case ERROR_TYPE:
108  sprintf (outtext, "[%s:%s@%04d] ERROR %s\n",
109  filename, functionName, line_number, text);
110  break;
111  case CRITICAL_TYPE:
112  sprintf (outtext, "[%s:%s@%04d] CRITICAL %s\n",
113  filename, functionName, line_number, text);
114  break;
115  case FATAL_TYPE:
116  fatalFlag = 1;
117  sprintf (outtext, "[%s:%s@%04d] FATAL %s\n",
118  filename, functionName, line_number, text);
119  break;
120  default:
121  printf( "ERROR in dprintf: malformed calling sequence\n");
122  return;
123  break;
124  }
125 
126  sprintf (filepath, "%s.debug", filename);
127 
128  /* Write output statement */
129  switch (dprintfFlag)
130  {
131  default:
132  case DEBUG_OFF:
133  break;
134  case DEBUG_MOSTLY_OFF:
135  if (fatalFlag) {
136  if ((outfile_fd = fopen (filepath, "a+")) != NULL) {
137  fwrite (outtext, sizeof (char), strlen (outtext), outfile_fd);
138  fclose (outfile_fd);
139  }
140  }
141  break;
142  case DEBUG_SCREEN_ONLY:
143  printf ("%s", outtext);
144  break;
145  case DEBUG_FILE_ONLY:
146  if ((outfile_fd = fopen (filepath, "a+")) != NULL) {
147  fwrite (outtext, sizeof (char), strlen (outtext), outfile_fd);
148  fclose (outfile_fd);
149  }
150  break;
151  case DEBUG_SCREEN_AND_FILE: // BOTH
152  printf ("%s", outtext);
153  if ((outfile_fd = fopen (filepath, "a+")) != NULL) {
154  fwrite (outtext, sizeof (char), strlen (outtext), outfile_fd);
155  fclose (outfile_fd);
156  }
157  break;
158  }
159 }
160 
167 double RangeToNormalized(double position, int range){
168  return(((position*2.0)/(double)range)-1.0);
169 }
170 
179 float NormalizeToRange(float normalizedValue, float minRange, float maxRange) {
180  float range = maxRange-minRange;
181  float temp = (float)((normalizedValue / 2.0)+ 0.5)*range;
182  return (temp + minRange);
183 }
184 float NormalizeToRange(float normalizedValue) {
185  return (float)((normalizedValue / 2.0) + 0.5);
186 }
187 
193 void ShowActivity (char *fmt, ...)
194 {
195  static char activity_indication_string[] = "|/-\\";
196  static int ai = 3;
197  va_list args;
198  char text[1024];
199 
200  va_start (args, fmt);
201 
202  vsprintf (text, fmt, args);
203 
204  ai = ai == 3 ? 0 : ai + 1;
205 
206  printf ("%c %s \r", activity_indication_string[ai], text);
207  fflush (stdout);
208 
209  va_end (args);
210 }
211 
212 #define PI 3.14159265358979
213 
222 double SinPosition (double *period, double sinStart)
223 {
224  double rtnVal;
225  static double sinePeriod=0.0;
226  static double timestamp;
227  double sinArg;
228 
229  //1st call
230  if (period != NULL) {
231  sinePeriod = *period;
232  timestamp = GetTime();
233  return 0.0;
234  }
235 
236  //Multiplying by 2*pi to the time difference makes sinePeriod work if it's measured in seconds.
237  //Adding sinStart to the part multiplied by PI, but not by 2, allows it to work as described in the comments.
238  sinArg = PI *((2.0 * (GetTime() - timestamp)) + sinStart) / sinePeriod;
239  rtnVal = sin (sinArg);
240  return (rtnVal);
241 }
242 
243 
249 double ElapsedTime ( double startTime )
250 {
251  double realTime = GetTime();
252  return (realTime-startTime);
253 }
254 
259 void panInit() {
260  double period = 3.0; // number of seconds for one complete pan
261  SinPosition(&period, 0.0); // initial call to set up time
262 }
263 
264 void panInit(double period) {
265  if (period < 0.0) period=3.0;
266  SinPosition(&period, 0.0); // initial call to set up time
267 }
268 
274 void panForTarget(Servo *panServo) { panForTarget(panServo, 0.0); }
275 
276 void panForTarget(Servo *panServo, double sinStart) {
277  float normalizedSinPosition = (float)SinPosition(NULL, sinStart);
278  float newServoPosition = NormalizeToRange(normalizedSinPosition);
279  panServo->Set( newServoPosition );
280  //ShowActivity ("pan x: normalized %f newServoPosition = %f" ,
281  // normalizedSinPosition, newServoPosition );
282 }
283 
284 
296 int processFile(char *inputFile, char *outputString, int lineNumber)
297 {
298  FILE *infile;
299  int stringSize = 80; // max size of one line in file
300  char inputStr[stringSize];
301  int lineCount=0;
302 
303  if (lineNumber < 0)
304  return (-1);
305 
306  if ((infile = fopen (inputFile, "r")) == NULL) {
307  printf ("Fatal error opening file %s\n",inputFile);
308  return (0);
309  }
310 
311  while (!feof(infile)) {
312  if (fgets (inputStr, stringSize, infile) != NULL) {
313  // Skip empty lines
314  if (emptyString(inputStr))
315  continue;
316  // Skip comment lines
317  if (inputStr[0] == '#' || inputStr[0] == '!')
318  continue;
319 
320  lineCount++;
321  if (lineNumber == 0)
322  continue;
323  else
324  {
325  if (lineCount == lineNumber)
326  break;
327  }
328  }
329  }
330 
331  // close file
332  fclose (infile);
333  // if number lines requested return the count
334  if (lineNumber == 0)
335  return (lineCount);
336  // check for input out of range
337  if (lineNumber > lineCount)
338  return (-1);
339  // return the line selected
340  if (lineCount) {
341  stripString(inputStr);
342  strcpy(outputString, inputStr);
343  return(lineCount);
344  }
345  else {
346  return(-1);
347  }
348 }
349 
353 int emptyString(char *string)
354 {
355  int i,len;
356 
357  if(string == NULL)
358  return(1);
359 
360  len = strlen(string);
361  for(i=0; i<len; i++) {
362  // Ignore the following characters
363  if (string[i] == '\n' || string[i] == '\r' ||
364  string[i] == '\t' || string[i] == ' ')
365  continue;
366  return(0);
367  }
368  return(1);
369 }
370 
374 void stripString(char *string)
375 {
376  int i,j,len;
377 
378  if(string == NULL)
379  return;
380 
381  len = strlen(string);
382  for(i=0,j=0; i<len; i++) {
383  // Remove the following characters from the string
384  if (string[i] == '\n' || string[i] == '\r' || string[i] == '\"')
385  continue;
386  // Copy anything else
387  string[j++] = string[i];
388  }
389  string[j] = '\0';
390 }
391 
392 
void Set(float value)
Definition: Servo.cpp:64
Definition: Servo.h:19