void DEBUG_printf(const char* Str, ...)
{
va_list ap;
va_start(ap, Str);
while(*Str) {
if(*Str == '%') {
*Str++;
switch(*Str) {
/*** characters ***/
case 'c': {
const char c = (const char) va_arg (ap, const char);
DEBUG_putch(c);
*Str++;
break;
}
/*** integers ***/
case 'd':
case 'i':
{
uint32_t c = va_arg (ap, uint32_t);
uint8_t s[32]={0};
itoa_s (c, 10, s);
DEBUG_print((const char*) s);
*Str++; // go to next character
break;
}
/*** display in hex ***/
case 'X':
case 'x':
{
uint32_t c = va_arg (ap, uint32_t);
uint8_t s[32]={0};
itoa_s (c,16,s);
DEBUG_print((const char*) s);
*Str++; // go to next character
break;
}
/*** strings ***/
case 's':
{
const char *s = va_arg (ap, const char*);
DEBUG_print((const char*) s);
*Str++; // go to next character
break;
}
case '%':
{
DEBUG_putch('%');
*Str++;
break;
}
}
} else {
/* itoa_s: a step of itoa */
int itoa_s(int n, char s[], int i) {
if (n < 0) {
s[i++] = '-';
n = -n;
}
if (n / 10)
i = itoa_s(n / 10, s, i);
s[i++] = n % 10 + '0';
return i;
}
static inline size_t snprint_int(char *buf, size_t size, int i,
unsigned int base, bool left,
char padding, size_t width)
{
char str[33];
size_t len = itoa_s(i, base, str, sizeof(str));
if (len == 0)
return 0;
return copy_with_padding(buf, size, str, len, left, padding, width);
}
int vfprintf(FILE* stream, const char* format, va_list args)
{
size_t i;
for (i=0; i<strlen(format);i++)
{
switch (format[i])
{
case '%':
switch (format[i+1]) {
/*** characters ***/
case 'c':
{
char c = va_arg(args, char);
fputc(c, stream);
i++; // go to next character
break;
}
/*** address of ***/
case 's':
{
int c = (int&)va_arg(args, char);
char str[64];
strcpy(str,(const char*)c);
fputs(str, stream);
i++; // go to next character
break;
}
/*** integers ***/
case 'd':
case 'i':
{
int c = va_arg (args, int);
char str[32]={0};
itoa_s(c, 10, str);
fputs(str, stream);
i++; // go to next character
break;
}
/*** display in hex ***/
case 'X':
case 'x':
{
int c = va_arg(args, int);
char str[32]={0};
itoa_s(c,16,str);
fputs(str, stream);
i++; // go to next character
break;
}
default:
va_end(args);
return 1;
}
break;
default:
fputc(format[i], stream);
break;
}
}
va_end(args);
return (int)i;
}
int DebugPrintf (const char* str, ...) {
if(!str)
return 0;
va_list args;
size_t i;
va_start (args, str);
for (i=0; i<strlen(str);i++) {
switch (str[i]) {
case '%':
switch (str[i+1]) {
case 'c': {
char c = va_arg (args, char);
DebugPutc (c);
i++;
break;
}
case 's': {
int c = (int) va_arg (args, char);
char str[32]={0};
itoa_s (c, 16, str);
DebugPuts (str);
i++;
break;
}
case 'd':
case 'i': {
int c = va_arg (args, int);
char str[32]={0};
itoa_s (c, 10, str);
DebugPuts (str);
i++;
break;
}
case 'X':
case 'x': {
int c = va_arg (args, int);
char str[32]={0};
itoa_s (c,16,str);
DebugPuts (str);
i++;
break;
}
default:
va_end (args);
return 1;
}
break;
default:
DebugPutc (str[i]);
break;
}
}
va_end (args);
}
int kernelPrintf (const char* str, ...) {
if(!str)
return 0;
va_list args;
va_start (args, str);
size_t i=0;
for (i=0; i<strlen(str);i++) {
switch (str[i]) {
case '%':
switch (str[i+1]) {
/*** characters ***/
case 'c': {
char c =(char) va_arg (args, int);
kernelPutc (c);
i++; // go to next character
break;
}
/*** address of ***/
case 's': {
int c = (int) va_arg (args, char);//(int&) FIXME
char str[32]={0};
itoa_s (c, 16, str);
kernelPuts (str);
i++; // go to next character
break;
}
/*** integers ***/
case 'd':
case 'i': {
int c = va_arg (args, int);
char str[32]={0};
itoa_s (c, 10, str);
kernelPuts (str);
i++; // go to next character
break;
}
/*** display in hex ***/
case 'X':
case 'x': {
int c = va_arg (args, int);
char str[32]={0};
itoa_s (c,16,str);
kernelPuts (str);
i++; // go to next character
break;
}
default:
va_end (args);
return 1;
}
break;
default:
kernelPutc (str[i]);
break;
}
}
va_end (args);
return i;
}
/* itoa: convert n to characters in s */
void itoa(int n, char s[]) {
int i = itoa_s(n, s, 0);
s[i] = '\0';
}
void LocalRobot::update()
{
RobotConsole::update();
// Only one thread can access *this now.
SYNC;
if(mode == SystemCall::logfileReplay)
{
if(logAcknowledged)
{
if(logPlayer.replay())
logAcknowledged = false;
}
}
else if(mode == SystemCall::physicalRobot)
{
int duration = SystemCall::getTimeSince(imageTimeStamp);
if(duration >= 33)
{
imageTimeStamp = duration >= 67 ? SystemCall::getCurrentSystemTime() : imageTimeStamp + 33;
image.timeStamp = SystemCall::getCurrentSystemTime();
}
}
else if(mode == SystemCall::simulatedRobot)
{
if(moveOp != noMove)
{
if(moveOp == moveBoth)
ctrl->moveObject(oracle.getRobot(), movePos * 0.001, moveRot * (pi / 180));
else if(moveOp == movePosition)
ctrl->moveObject(oracle.getRobot(), movePos * 0.001);
else if(moveOp == moveBallPosition)
ctrl->moveObject(oracle.getBall(), movePos * 0.001);
moveOp = noMove;
}
const int imageDelay = 33;
int duration = SystemCall::getTimeSince(imageTimeStamp);
if(duration >= imageDelay)
{
imageTimeStamp = duration >= 2 * imageDelay ? SystemCall::getCurrentSystemTime() : imageTimeStamp + imageDelay;
if(calculateImage) // If there is a PDA camera
oracle.getImage(image);
else
image.timeStamp = SystemCall::getCurrentSystemTime();
oracle.getRobotPose(robotPose);
oracle.getBallModel(robotPose, ballModel);
}
else
oracle.getRobotPose(robotPose);
oracle.getOdometryData(robotPose, odometryData);
oracle.getSensorData(sensorData);
oracle.getAndSetJointData(jointRequest, jointData);
}
std::string statusText;
if(mode == SystemCall::logfileReplay)
{
statusText = std::string("replaying ") + logFile + " ";
if(logPlayer.currentFrameNumber != -1)
{
char buf[10];
statusText += itoa_s(logPlayer.currentFrameNumber + 1, buf, sizeof(buf), 10);
}
else
statusText += "finished";
}
if(mode != SystemCall::logfileReplay && logPlayer.numberOfFrames != 0)
{
if(statusText != "")
statusText += ", ";
char buf[10];
statusText += std::string("recorded ");
statusText += itoa_s(logPlayer.numberOfFrames, buf, sizeof(buf), 10);
}
if(pollingFor)
{
statusText += statusText != "" ? ", polling for " : "polling for ";
statusText += pollingFor;
}
if(!statusText.empty())
{
statusText = robotName.substr(robotName.find_last_of(".") + 1) + ": " + statusText;
ctrl->printStatusText(statusText);
}
}
//! writes formatted string to buffer
int vsprintf(char *str, const char *format, va_list ap) {
if (!str)
return 0;
if (!format)
return 0;
size_t loc=0;
size_t i;
for (i=0 ; i<=strlen(format);i++, loc++) {
switch (format[i]) {
case '%':
switch (format[i+1]) {
/*** characters ***/
case 'c': {
char c = va_arg (ap, char);
str[loc] = c;
i++;
break;
}
/*** integers ***/
case 'd':
case 'i': {
int c = va_arg (ap, int);
char s[32]={0};
itoa_s (c, 10, s);
strcpy (&str[loc], s);
loc+= strlen(s) - 2;
i++; // go to next character
break;
}
/*** display in hex ***/
case 'X':
case 'x': {
int c = va_arg (ap, int);
char s[32]={0};
itoa_s (c,16,s);
strcpy (&str[loc], s);
i++; // go to next character
loc+=strlen(s) - 2;
break;
}
/*** strings ***/
case 's': {
int c = (int&) va_arg (ap, char);
char s[32]={0};
strcpy (s,(const char*)c);
strcpy (&str[loc], s);
i++; // go to next character
loc+=strlen(s) - 2;
break;
}
}
break;
default:
str[loc] = format[i];
break;
}
}
return i;
}
