15 #include <sys/types.h>
22 #include "BaeUtilities.h"
34 static DebugOutputType dprintfFlag = DEBUG_OFF;
40 void SetDebugFlag ( DebugOutputType flag )
41 { dprintfFlag = flag; }
50 void dprintf (
const char * tempString, ... )
55 const char *functionName;
66 if (dprintfFlag == DEBUG_OFF) {
return; }
68 va_start (args, tempString);
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;
76 if (tempString[index] ==
'\\' || tempString[index] ==
'/')
77 filename = tempString + index + 1;
81 functionName = va_arg (args,
const char *);
84 line_number = va_arg (args,
int);
87 type = va_arg (args,
int);
90 fmt = va_arg (args,
const char *);
92 vsprintf (text, fmt, args);
100 sprintf (outtext,
"[%s:%s@%04d] DEBUG %s\n",
101 filename, functionName, line_number, text);
104 sprintf (outtext,
"[%s:%s@%04d] INFO %s\n",
105 filename, functionName, line_number, text);
108 sprintf (outtext,
"[%s:%s@%04d] ERROR %s\n",
109 filename, functionName, line_number, text);
112 sprintf (outtext,
"[%s:%s@%04d] CRITICAL %s\n",
113 filename, functionName, line_number, text);
117 sprintf (outtext,
"[%s:%s@%04d] FATAL %s\n",
118 filename, functionName, line_number, text);
121 printf(
"ERROR in dprintf: malformed calling sequence\n");
126 sprintf (filepath,
"%s.debug", filename);
134 case DEBUG_MOSTLY_OFF:
136 if ((outfile_fd = fopen (filepath,
"a+")) != NULL) {
137 fwrite (outtext,
sizeof (
char), strlen (outtext), outfile_fd);
142 case DEBUG_SCREEN_ONLY:
143 printf (
"%s", outtext);
145 case DEBUG_FILE_ONLY:
146 if ((outfile_fd = fopen (filepath,
"a+")) != NULL) {
147 fwrite (outtext,
sizeof (
char), strlen (outtext), outfile_fd);
151 case DEBUG_SCREEN_AND_FILE:
152 printf (
"%s", outtext);
153 if ((outfile_fd = fopen (filepath,
"a+")) != NULL) {
154 fwrite (outtext,
sizeof (
char), strlen (outtext), outfile_fd);
167 double RangeToNormalized(
double position,
int range){
168 return(((position*2.0)/(
double)range)-1.0);
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);
184 float NormalizeToRange(
float normalizedValue) {
185 return (
float)((normalizedValue / 2.0) + 0.5);
193 void ShowActivity (
char *fmt, ...)
195 static char activity_indication_string[] =
"|/-\\";
200 va_start (args, fmt);
202 vsprintf (text, fmt, args);
204 ai = ai == 3 ? 0 : ai + 1;
206 printf (
"%c %s \r", activity_indication_string[ai], text);
212 #define PI 3.14159265358979
222 double SinPosition (
double *period,
double sinStart)
225 static double sinePeriod=0.0;
226 static double timestamp;
230 if (period != NULL) {
231 sinePeriod = *period;
232 timestamp = GetTime();
238 sinArg = PI *((2.0 * (GetTime() - timestamp)) + sinStart) / sinePeriod;
239 rtnVal = sin (sinArg);
249 double ElapsedTime (
double startTime )
251 double realTime = GetTime();
252 return (realTime-startTime);
261 SinPosition(&period, 0.0);
264 void panInit(
double period) {
265 if (period < 0.0) period=3.0;
266 SinPosition(&period, 0.0);
274 void panForTarget(
Servo *panServo) { panForTarget(panServo, 0.0); }
276 void panForTarget(
Servo *panServo,
double sinStart) {
277 float normalizedSinPosition = (float)SinPosition(NULL, sinStart);
278 float newServoPosition = NormalizeToRange(normalizedSinPosition);
279 panServo->
Set( newServoPosition );
296 int processFile(
char *inputFile,
char *outputString,
int lineNumber)
300 char inputStr[stringSize];
306 if ((infile = fopen (inputFile,
"r")) == NULL) {
307 printf (
"Fatal error opening file %s\n",inputFile);
311 while (!feof(infile)) {
312 if (fgets (inputStr, stringSize, infile) != NULL) {
314 if (emptyString(inputStr))
317 if (inputStr[0] ==
'#' || inputStr[0] ==
'!')
325 if (lineCount == lineNumber)
337 if (lineNumber > lineCount)
341 stripString(inputStr);
342 strcpy(outputString, inputStr);
353 int emptyString(
char *
string)
360 len = strlen(
string);
361 for(i=0; i<len; i++) {
363 if (
string[i] ==
'\n' ||
string[i] ==
'\r' ||
364 string[i] ==
'\t' ||
string[i] ==
' ')
374 void stripString(
char *
string)
381 len = strlen(
string);
382 for(i=0,j=0; i<len; i++) {
384 if (
string[i] ==
'\n' ||
string[i] ==
'\r' ||
string[i] ==
'\"')
387 string[j++] =
string[i];