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; }