/
BaeUtilities.cpp
407 lines (357 loc) · 11.4 KB
/
BaeUtilities.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
/********************************************************************************
* 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 "stdafx.h"
#include "BaeUtilities.h"
// #include "Servo.h"
// #include "Timer.h"
/**
* Utility functions
*/
/**
* debug output flag options:
* DEBUG_OFF, DEBUG_MOSTLY_OFF, DEBUG_SCREEN_ONLY, DEBUG_FILE_ONLY, DEBUG_SCREEN_AND_FILE
*/
static DebugOutputType dprintfFlag = DEBUG_OFF;
/**
* Set the debug flag to print to screen, file on cRIO, both or neither
* @param tempString The format string.
*/
void SetDebugFlag ( DebugOutputType flag )
{ dprintfFlag = flag; }
/**
* Debug print to a file and/or a terminal window.
* Call like you would call printf.
* Set functionName in the function if you want the correct function name to print out.
* The file line number will also be printed.
* @param tempString The format string.
*/
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;
}
}
/**
* @brief Normalizes a value in a range, used for drive input
* @param position The position in the range, starting at 0
* @param range The size of the range that position is in
* @return The normalized position from -1 to +1
*/
double RangeToNormalized(double position, int range){
return(((position*2.0)/(double)range)-1.0);
}
/**
* @brief Convert a normalized value to the corresponding value in a range.
* This is used to convert normalized values to the servo command range.
* @param normalizedValue The normalized value (in the -1 to +1 range)
* @param minRange The minimum of the range (0 is default)
* @param maxRange The maximum of the range (1 is default)
* @return The value in the range corresponding to the input normalized value
*/
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);
}
/**
* @brief Displays an activity indicator to console.
* Call this function like you would call printf.
* @param fmt The format string
*/
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
/**
* @brief Calculate sine wave increments (-1.0 to 1.0).
* The first time this is called, it sets up the time increment. Subsequent calls
* will give values along the sine wave depending on current time. If the wave is
* stopped and restarted, it must be reinitialized with a new "first call".
*
* @param period length of time to complete a complete wave
* @param sinStart Where to start the sine wave (0.0 = 2 pi, pi/2 = 1.0, etc.)
*/
#if 0
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);
}
#endif
/**
* @brief Find the elapsed time since a specified time.
* @param startTime The starting time
* @return How long it has been since the starting time
*/
#if 0
double ElapsedTime ( double startTime )
{
double realTime = GetTime();
return (realTime-startTime);
}
#endif
/**
* @brief Initialize pan parameters
* @param period The number of seconds to complete one pan
*/
#if 0
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
}
#endif
/**
* @brief Move the horizontal servo back and forth.
* @param panServo The servo object to move
* @param sinStart The position on the sine wave to begin the pan
*/
#if 0
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 );
}
#endif
/** @brief Read a file and return non-comment output string
Call the first time with 0 lineNumber to get the number of lines to read
Then call with each lineNumber to get one camera parameter. There should
be one property=value entry on each line, i.e. "exposure=auto"
* @param inputFile filename to read
* @param outputString one string
* @param lineNumber if 0, return number of lines; else return that line number
* @return int number of lines or -1 if error
**/
#if 0
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);
}
}
#endif
/** Ignore empty string
* @param string to check if empty
**/
#if 0
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);
}
#endif
/** Remove special characters from string
* @param string to process
**/
#if 0
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';
}
#endif