Kod___r__d__owy.rar

[C] Serwonapęd - silnik DC, Atmega32, PWM, PID, Enkoder

Witam wszystkich po przerwie. Wrzucam tutaj moją pracę mgr gdyż wiele osób pytało o nią w wiadomościach prywatnych. Po zainteresowaniu dotyczącym pracy inż. widzę, że na pewno informacje zawarte w pracy mgr, która jest rozwinięciem pierwotnego serwonapędu, przydadzą się wielu osobom. Dla początkujących znów będzie to kompedium wiedzy, dla zaawansowanych pewnie nic ciekawego. Serwonapęd można zastosować z dowolnym silnikiem prądu stałego i enkoderem w dowolnym urządzeniu. Chciałbym jeszcze raz podziękować wszystkim za dobre rady podczas pisania pracy mgr. https://www.youtube.com/watch?v=pbICAofwSSA https://www.youtube.com/watch?v=XJ3ORRuegBI Plik arkusza kalkulacyjnego, który jest w załączniku służył mi do wykreślania trajektorii i odpowiedzi regulatora PID. Obliczenia PID były wykonywane w arkuszu oprócz członu I.

Download file - link to post

• Kod___r__d__owy.rar
  • qei.o
  • Quadrature_Encoder_Interface.elf
  • rprintf.o
  • Quadrature_Encoder_Interface.eep
  • Quadrature_Encoder_Interface.map
  • Makefile
  • lcd_lib.o
  • rprintfLCD.o
  • Quadrature_Encoder_Interface.hex
  • Quadrature_Encoder_Interface.lss
  • led7seg.o
  • dep
    • led7seg.o.d
    • qei.o.d
    • rprintfLCD.o.d
    • lcd_lib.o.d
    • rprintf.o.d

Kod___r__d__owy.rar > rprintf.h

/*! \file rprintf.h \brief printf routine and associated routines. */
//****************************************************************************
//
// File Name : 'rprintf.h'
// Title : printf routine and associated routines
// Author : Pascal Stang - Copyright (C) 2000-2002
// Created : 2000.12.26
// Revised : 2003.5.1
// Version : 1.0
// Target MCU : Atmel AVR series and other targets
// Editor Tabs : 4
//
// NOTE: This code is currently below version 1.0, and therefore is considered
// to be lacking in some functionality or documentation, or may not be fully
// tested. Nonetheless, you can expect most functions to work.
//
// This code is distributed under the GNU Public License
// which can be found at http://www.gnu.org/licenses/gpl.txt
//
/// \ingroup general
/// \defgroup rprintf printf() Function Library (rprintf.c)
/// \code #include " rprintf.h " \endcode
/// \par Overview
/// The rprintf function library provides a simplified (reduced) version of
/// the common C printf() function.  See the code files for details about
/// which printf features are supported.  Also in this library are a
/// variety of functions for fast printing of certain common data types
/// (variable types).  Functions include print string from RAM, print
/// string from ROM, print string snippet, print hex byte/short/long, and
/// a custom-formatted number print, as well as an optional floating-point
/// print routine.
///
/// \note All output from the rprintf library can be directed to any device
/// or software which accepts characters.  This means that rprintf output
/// can be sent to the UART (serial port) or can be used with the LCD
/// display libraries to print formatted text on the screen.
//
//****************************************************************************
//@{

#ifndef RPRINTF_H
#define RPRINTF_H

// needed for use of PSTR below
#include & lt; avr/pgmspace.h & gt;

// configuration
// defining RPRINTF_SIMPLE will compile a smaller, simpler, and faster printf() function
// defining RPRINTF_COMPLEX will compile a larger, more capable, and slower printf() function
#ifndef RPRINTF_COMPLEX
#define RPRINTF_SIMPLE
#endif

// Define RPRINTF_FLOAT to enable the floating-point printf function: rprintfFloat()
// (adds +4600bytes or 2.2Kwords of code)

// defines/constants
#define STRING_IN_RAM 0
#define STRING_IN_ROM 1

// make a putchar for those that are used to using it
//#define putchar(c) rprintfChar(c);

// functions

//! Initializes the rprintf library for an output stream.
/// You must call this initializer once before using any other rprintf function.
/// The argument must be a character stream output function.
void rprintfInitRS(void (*putchar_func)(unsigned char c));

//! prints a single character to the current output device
void rprintfChar(unsigned char c);

//! prints a null-terminated string stored in RAM
void rprintfStr(char str[]);

//! Prints a section of a string stored in RAM.
/// Begins printing at position indicated by & lt; start & gt; ,
/// and prints number of characters indicated by & lt; len & gt; .
void rprintfStrLen(char str[], unsigned int start, unsigned int len);

//! prints a string stored in program rom
/// \note This function does not actually store your string in
/// program rom, but merely reads it assuming you stored it properly.
void rprintfProgStr(const prog_char str[]);

//! Using the function rprintfProgStrM(...) automatically causes
/// your string to be stored in ROM, thereby not wasting precious RAM.
/// Example usage:
/// \code
/// rprintfProgStrM( " Hello, this string is stored in program rom " );
/// \endcode
#define rprintfProgStrM(string) (rprintfProgStr(PSTR(string)))

//! Prints a carriage-return and line-feed.
/// Useful when printing to serial ports/terminals.
void rprintfCRLF(void);

// Prints the number contained in " data " in hex format
// u04,u08,u16,and u32 functions handle 4,8,16,or 32 bits respectively
void rprintfu04(unsigned char data); /// & lt; Print 4-bit hex number. Outputs a single hex character.
void rprintfu08(unsigned char data); /// & lt; Print 8-bit hex number. Outputs two hex characters.
void rprintfu16(unsigned short data); /// & lt; Print 16-bit hex number. Outputs four hex characters.
void rprintfu32(unsigned long data); /// & lt; Print 32-bit hex number. Outputs eight hex characters.

//! A flexible integer-number printing routine.
/// Print the number " n " in the given " base " , using exactly " numDigits " .
/// Print +/- if signed flag " isSigned " is TRUE.
/// The character specified in " padchar " will be used to pad extra characters.
///
/// Examples:
/// \code
/// uartPrintfNum(10, 6, TRUE, ' ', 1234); -- & gt; " +1234 "
/// uartPrintfNum(10, 6, FALSE, '0', 1234); -- & gt; " 001234 "
/// uartPrintfNum(16, 6, FALSE, '.', 0x5AA5); -- & gt; " ..5AA5 "
/// \endcode
void rprintfNum(char base, char numDigits, char isSigned, char padchar, long n);

#ifdef RPRINTF_FLOAT
//! floating-point print routine
void rprintfFloat(char numDigits, double x);
#endif

// NOTE: Below you'll see the function prototypes of rprintf1RamRom and
// rprintf2RamRom. rprintf1RamRom and rprintf2RamRom are both reduced versions
// of the regular C printf() command. However, they are modified to be able
// to read their text/format strings from RAM or ROM in the Atmel microprocessors.
// Unless you really intend to, do not use the " RamRom " versions of the functions
// directly. Instead use the #defined function versions:
//
// printfx( " text/format " ,args) ...to keep your text/format string stored in RAM
// - or -
// printfxROM( " text/format " ,args) ...to keep your text/format string stored in ROM
//
// where x is either 1 or 2 for the simple or more powerful version of printf()
//
// Since there is much more ROM than RAM available in the Atmel microprocessors,
// and nearly all text/format strings are constant (never change in the course
// of the program), you should try to use the ROM printf version exclusively.
// This will ensure you leave as much RAM as possible for program variables and
// data.

//! \fn int rprintf(const char *format, ...);
/// A reduced substitute for the usual C printf() function.
/// This function actually points to either rprintf1RamRom or rprintf2RamRom
/// depending on the user's selection. Rprintf1 is a simple small fast print
/// routine while rprintf2 is larger and slower but more capable. To choose
/// the routine you would like to use, define either RPRINTF_SIMPLE or
/// RPRINTF_COMPLEX in global.h.

#ifdef RPRINTF_SIMPLE
//! A simple printf routine.
/// Called by rprintf() - does a simple printf (supports %d, %x, %c).
/// Supports:
/// - %d - decimal
/// - %x - hex
/// - %c - character
int rprintf1RamRom(unsigned char stringInRom, const char *format, ...);
// #defines for RAM or ROM operation
#define rprintf1(format, args...) rprintf1RamRom(STRING_IN_ROM, PSTR(format), ## args)
#define rprintf1RAM(format, args...) rprintf1RamRom(STRING_IN_RAM, format, ## args)

// *** Default rprintf(...) ***
// this next line determines what the the basic rprintf() defaults to:
#define rprintf(format, args...) rprintf1RamRom(STRING_IN_ROM, PSTR(format), ## args)
#endif

#ifdef RPRINTF_COMPLEX
//! A more powerful printf routine.
/// Called by rprintf() - does a more powerful printf (supports %d, %u, %o, %x, %c, %s).
/// Supports:
/// - %d - decimal
/// - %u - unsigned decimal
/// - %o - octal
/// - %x - hex
/// - %c - character
/// - %s - strings
/// - and the width,precision,padding modifiers
/// \note This printf does not support floating point numbers.
int rprintf2RamRom(unsigned char stringInRom, const char *sfmt, ...);
// #defines for RAM or ROM operation
#define rprintf2(format, args...) rprintf2RamRom(STRING_IN_ROM, format, ## args)
#define rprintf2RAM(format, args...) rprintf2RamRom(STRING_IN_RAM, format, ## args)

// *** Default rprintf(...) ***
// this next line determines what the the basic rprintf() defaults to:
#define rprintf(format, args...) rprintf2RamRom(STRING_IN_ROM, PSTR(format), ## args)
#endif

#endif
//@}

Kod___r__d__owy.rar > rprintf.c

/*! \file rprintf.c \brief printf routine and associated routines. */
//*****************************************************************************
//
// File Name : 'rprintf.c'
// Title : printf routine and associated routines
// Author : Pascal Stang - Copyright (C) 2000-2002
// Created : 2000.12.26
// Revised : 2003.5.1
// Version : 1.0
// Target MCU : Atmel AVR series and other targets
// Editor Tabs : 4
//
// NOTE: This code is currently below version 1.0, and therefore is considered
// to be lacking in some functionality or documentation, or may not be fully
// tested. Nonetheless, you can expect most functions to work.
//
// This code is distributed under the GNU Public License
// which can be found at http://www.gnu.org/licenses/gpl.txt
//
//*****************************************************************************

#include & lt; avr/pgmspace.h & gt;
//#include & lt; string-avr.h & gt;
//#include & lt; stdlib.h & gt;
#include & lt; stdarg.h & gt;
#include " conf/global.h "
#include " rprintf.h "

#ifndef TRUE
#define TRUE -1
#define FALSE 0
#endif

#define INF 32766 // maximum field size to print
#define READMEMBYTE(a,char_ptr) ((a)?(pgm_read_byte(char_ptr)):(*char_ptr))

#ifdef RPRINTF_COMPLEX
static unsigned char buf[128];
#endif

// use this to store hex conversion in RAM
//static char HexChars[] = " 0123456789ABCDEF " ;
// use this to store hex conversion in program memory
//static prog_char HexChars[] = " 0123456789ABCDEF " ;
static char __attribute__ ((progmem)) HexChars[] = " 0123456789ABCDEF " ;

#define hexchar(x) pgm_read_byte( HexChars+((x) & 0x0f) )
//#define hexchar(x) ((((x) & 0x0F) & gt; 9)?((x)+'A'-10):((x)+'0'))

// function pointer to single character output routine
static void (*rputchar)(unsigned char c);

// *** rprintf initialization ***
// you must call this function once and supply the character output
// routine before using other functions in this library
void rprintfInitRS(void (*putchar_func)(unsigned char c))
{
rputchar = putchar_func;
}

// *** rprintfChar ***
// send a character/byte to the current output device
void rprintfChar(unsigned char c)
{
// do LF - & gt; CR/LF translation
if(c == '\n')
rputchar('\r');
// send character
rputchar(c);
}

// *** rprintfStr ***
// prints a null-terminated string stored in RAM
void rprintfStr(char str[])
{
// send a string stored in RAM
// check to make sure we have a good pointer
if (!str) return;

// print the string until a null-terminator
while (*str)
rprintfChar(*str++);
}

// *** rprintfStrLen ***
// prints a section of a string stored in RAM
// begins printing at position indicated by & lt; start & gt;
// prints number of characters indicated by & lt; len & gt;
void rprintfStrLen(char str[], unsigned int start, unsigned int len)
{
register int i=0;

// check to make sure we have a good pointer
if (!str) return;
// spin through characters up to requested start
// keep going as long as there's no null
while((i++ & lt; start) & & (*str++));
// for(i=0; i & lt; start; i++)
// {
// // keep steping through string as long as there's no null
// if(*str) str++;
// }

// then print exactly len characters
for(i=0; i & lt; len; i++)
{
// print data out of the string as long as we haven't reached a null yet
// at the null, start printing spaces
if(*str)
rprintfChar(*str++);
else
rprintfChar(' ');
}

}

// *** rprintfProgStr ***
// prints a null-terminated string stored in program ROM
void rprintfProgStr(const prog_char str[])
{
// print a string stored in program memory
register char c;

// check to make sure we have a good pointer
if (!str) return;

// print the string until the null-terminator
while((c = pgm_read_byte(str++)))
rprintfChar(c);
}

// *** rprintfCRLF ***
// prints carriage return and line feed
void rprintfCRLF(void)
{
// print CR/LF
//rprintfChar('\r');
// LF - & gt; CR/LF translation built-in to rprintfChar()
rprintfChar('\n');
}

// *** rprintfu04 ***
// prints an unsigned 4-bit number in hex (1 digit)
void rprintfu04(unsigned char data)
{
// print 4-bit hex value
// char Character = data & 0x0f;
// if (Character & gt; 9)
// Character+='A'-10;
// else
// Character+='0';
rprintfChar(hexchar(data));
}

// *** rprintfu08 ***
// prints an unsigned 8-bit number in hex (2 digits)
void rprintfu08(unsigned char data)
{
// print 8-bit hex value
rprintfu04(data & gt; & gt; 4);
rprintfu04(data);
}

// *** rprintfu16 ***
// prints an unsigned 16-bit number in hex (4 digits)
void rprintfu16(unsigned short data)
{
// print 16-bit hex value
rprintfu08(data & gt; & gt; 8);
rprintfu08(data);
}

// *** rprintfu32 ***
// prints an unsigned 32-bit number in hex (8 digits)
void rprintfu32(unsigned long data)
{
// print 32-bit hex value
rprintfu16(data & gt; & gt; 16);
rprintfu16(data);
}

// *** rprintfNum ***
// special printf for numbers only
// see formatting information below
// Print the number " n " in the given " base "
// using exactly " numDigits "
// print +/- if signed flag " isSigned " is TRUE
// use the character specified in " padchar " to pad extra characters
//
// Examples:
// uartPrintfNum(10, 6, TRUE, ' ', 1234); -- & gt; " +1234 "
// uartPrintfNum(10, 6, FALSE, '0', 1234); -- & gt; " 001234 "
// uartPrintfNum(16, 6, FALSE, '.', 0x5AA5); -- & gt; " ..5AA5 "
void rprintfNum(char base, char numDigits, char isSigned, char padchar, long n)
{
// define a global HexChars or use line below
//static char HexChars[16] = " 0123456789ABCDEF " ;
char *p, buf[32];
unsigned long x;
unsigned char count;

// prepare negative number
if( isSigned & & (n & lt; 0) )
{
x = -n;
}
else
{
x = n;
}

// setup little string buffer
count = (numDigits-1)-(isSigned?1:0);
p = buf + sizeof (buf);
*--p = '\0';

// force calculation of first digit
// (to prevent zero from not printing at all!!!)
*--p = hexchar(x%base); x /= base;
// calculate remaining digits
while(count--)
{
if(x != 0)
{
// calculate next digit
*--p = hexchar(x%base); x /= base;
}
else
{
// no more digits left, pad out to desired length
*--p = padchar;
}
}

// apply signed notation if requested
if( isSigned )
{
if(n & lt; 0)
{
*--p = '-';
}
else if(n & gt; 0)
{
*--p = '+';
}
else
{
*--p = ' ';
}
}

// print the string right-justified
count = numDigits;
while(count--)
{
rprintfChar(*p++);
}
}

//#ifdef RPRINTF_FLOAT
// *** rprintfFloat ***
// floating-point print
void rprintfFloat(char numDigits, double x)
{
unsigned char firstplace = FALSE;
unsigned char negative;
unsigned char i, digit;
double place = 1.0;

// save sign
negative = (x & lt; 0);
// convert to absolute value
x = (x & gt; 0)?(x):(-x);

// find starting digit place
for(i=0; i & lt; 15; i++)
{
if((x/place) & lt; 10.0)
break;
else
place *= 10.0;
}
// print polarity character
if(negative)
rprintfChar('-');
else
rprintfChar('+');

// print digits
for(i=0; i & lt; numDigits; i++)
{
digit = (x/place);

if(digit | firstplace | (place == 1.0))
{
firstplace = TRUE;
rprintfChar(digit+0x30);
}
else
rprintfChar(' ');

if(place == 1.0)
{
rprintfChar('.');
}

x -= (digit*place);
place /= 10.0;
}
}
//#endif

#ifdef RPRINTF_SIMPLE
// *** rprintf1RamRom ***
// called by rprintf() - does a simple printf (supports %d, %x, %c)
// Supports:
// %d - decimal
// %x - hex
// %c - character
int rprintf1RamRom(unsigned char stringInRom, const char *format, ...)
{
// simple printf routine
// define a global HexChars or use line below
//static char HexChars[16] = " 0123456789ABCDEF " ;
char format_flag;
unsigned int u_val, div_val, base;
va_list ap;

va_start(ap, format);
for (;;)
{
while ((format_flag = READMEMBYTE(stringInRom,format++) ) != '%')
{ // Until '%' or '\0'
if (!format_flag)
{
va_end(ap);
return(0);
}
rprintfChar(format_flag);
}

switch (format_flag = READMEMBYTE(stringInRom,format++) )
{
case 'c': format_flag = va_arg(ap,int);
default: rprintfChar(format_flag); continue;
case 'd': base = 10; div_val = 10000; goto CONVERSION_LOOP;
// case 'x': base = 16; div_val = 0x10;
case 'x': base = 16; div_val = 0x1000;

CONVERSION_LOOP:
u_val = va_arg(ap,int);
if (format_flag == 'd')
{
if (((int)u_val) & lt; 0)
{
u_val = - u_val;
rprintfChar('-');
}
while (div_val & gt; 1 & & div_val & gt; u_val) div_val /= 10;
}
do
{
//rprintfChar(pgm_read_byte(HexChars+(u_val/div_val)));
rprintfu04(u_val/div_val);
u_val %= div_val;
div_val /= base;
} while (div_val);
}
}
va_end(ap);
}
#endif


#ifdef RPRINTF_COMPLEX
// *** rprintf2RamRom ***
// called by rprintf() - does a more powerful printf (supports %d, %u, %o, %x, %c, %s)
// Supports:
// %d - decimal
// %u - unsigned decimal
// %o - octal
// %x - hex
// %c - character
// %s - strings
// and the width,precision,padding modifiers
// **this printf does not supporting point numbers
int rprintf2RamRom(unsigned char stringInRom, const char *sfmt, ...)
{
register unsigned char *f, *bp;
register long l;
register unsigned long u;
register int i;
register int fmt;
register unsigned char pad = ' ';
int flush_left = 0, f_width = 0, prec = INF, hash = 0, do_long = 0;
int sign = 0;

va_list ap;
va_start(ap, sfmt);

f = (unsigned char *) sfmt;

for (; READMEMBYTE(stringInRom,f); f++)
{
if (READMEMBYTE(stringInRom,f) != '%')
{ // not a format character
// then just output the char
rprintfChar(READMEMBYTE(stringInRom,f));
}
else
{
f++; // if we have a " % " then skip it
if (READMEMBYTE(stringInRom,f) == '-')
{
flush_left = 1; // minus: flush left
f++;
}
if (READMEMBYTE(stringInRom,f) == '0'
|| READMEMBYTE(stringInRom,f) == '.')
{
// padding with 0 rather than blank
pad = '0';
f++;
}
if (READMEMBYTE(stringInRom,f) == '*')
{ // field width
f_width = va_arg(ap, int);
f++;
}
else if (Isdigit(READMEMBYTE(stringInRom,f)))
{
f_width = atoiRamRom(stringInRom, (char *) f);
while (Isdigit(READMEMBYTE(stringInRom,f)))
f++; // skip the digits
}
if (READMEMBYTE(stringInRom,f) == '.')
{ // precision
f++;
if (READMEMBYTE(stringInRom,f) == '*')
{
prec = va_arg(ap, int);
f++;
}
else if (Isdigit(READMEMBYTE(stringInRom,f)))
{
prec = atoiRamRom(stringInRom, (char *) f);
while (Isdigit(READMEMBYTE(stringInRom,f)))
f++; // skip the digits
}
}
if (READMEMBYTE(stringInRom,f) == '#')
{ // alternate form
hash = 1;
f++;
}
if (READMEMBYTE(stringInRom,f) == 'l')
{ // long format
do_long = 1;
f++;
}

fmt = READMEMBYTE(stringInRom,f);
bp = buf;
switch (fmt) { // do the formatting
case 'd': // 'd' signed decimal
if (do_long)
l = va_arg(ap, long);
else
l = (long) (va_arg(ap, int));
if (l & lt; 0)
{
sign = 1;
l = -l;
}
do {
*bp++ = l % 10 + '0';
} while ((l /= 10) & gt; 0);
if (sign)
*bp++ = '-';
f_width = f_width - (bp - buf);
if (!flush_left)
while (f_width-- & gt; 0)
rprintfChar(pad);
for (bp--; bp & gt; = buf; bp--)
rprintfChar(*bp);
if (flush_left)
while (f_width-- & gt; 0)
rprintfChar(' ');
break;
case 'o': // 'o' octal number
case 'x': // 'x' hex number
case 'u': // 'u' unsigned decimal
if (do_long)
u = va_arg(ap, unsigned long);
else
u = (unsigned long) (va_arg(ap, unsigned));
if (fmt == 'u')
{ // unsigned decimal
do {
*bp++ = u % 10 + '0';
} while ((u /= 10) & gt; 0);
}
else if (fmt == 'o')
{ // octal
do {
*bp++ = u % 8 + '0';
} while ((u /= 8) & gt; 0);
if (hash)
*bp++ = '0';
}
else if (fmt == 'x')
{ // hex
do {
i = u % 16;
if (i & lt; 10)
*bp++ = i + '0';
else
*bp++ = i - 10 + 'a';
} while ((u /= 16) & gt; 0);
if (hash)
{
*bp++ = 'x';
*bp++ = '0';
}
}
i = f_width - (bp - buf);
if (!flush_left)
while (i-- & gt; 0)
rprintfChar(pad);
for (bp--; bp & gt; = buf; bp--)
rprintfChar((int) (*bp));
if (flush_left)
while (i-- & gt; 0)
rprintfChar(' ');
break;
case 'c': // 'c' character
i = va_arg(ap, int);
rprintfChar((int) (i));
break;
case 's': // 's' string
bp = va_arg(ap, unsigned char *);
if (!bp)
bp = (unsigned char *) " (nil) " ;
f_width = f_width - strlen((char *) bp);
if (!flush_left)
while (f_width-- & gt; 0)
rprintfChar(pad);
for (i = 0; *bp & & i & lt; prec; i++)
{
rprintfChar(*bp);
bp++;
}
if (flush_left)
while (f_width-- & gt; 0)
rprintfChar(' ');
break;
case '%': // '%' character
rprintfChar('%');
break;
}
flush_left = 0, f_width = 0, prec = INF, hash = 0, do_long = 0;
sign = 0;
pad = ' ';
}
}

va_end(ap);
return 0;
}

unsigned char Isdigit(char c)
{
if((c & gt; = 0x30) & & (c & lt; = 0x39))
return TRUE;
else
return FALSE;
}

int atoiRamRom(unsigned char stringInRom, char *str)
{
int num = 0;;

while(Isdigit(READMEMBYTE(stringInRom,str)))
{
num *= 10;
num += ((READMEMBYTE(stringInRom,str++)) - 0x30);
}
return num;
}

#endif

//******************************************************************************
// code below this line is commented out and can be ignored
//******************************************************************************
/*
char* sprintf(const char *sfmt, ...)
{
register unsigned char *f, *bp, *str;
register long l;
register unsigned long u;
register int i;
register int fmt;
register unsigned char pad = ' ';
int flush_left = 0, f_width = 0, prec = INF, hash = 0, do_long = 0;
int sign = 0;

va_list ap;
va_start(ap, sfmt);

str = bufstring;
f = (unsigned char *) sfmt;

for (; *f; f++)
{
if (*f != '%')
{ // not a format character
*str++ = (*f); // then just output the char
}
else
{
f++; // if we have a " % " then skip it
if (*f == '-')
{
flush_left = 1; // minus: flush left
f++;
}
if (*f == '0' || *f == '.')
{
// padding with 0 rather than blank
pad = '0';
f++;
}
if (*f == '*')
{ // field width
f_width = va_arg(ap, int);
f++;
}
else if (Isdigit(*f))
{
f_width = atoi((char *) f);
while (Isdigit(*f))
f++; // skip the digits
}
if (*f == '.')
{ // precision
f++;
if (*f == '*')
{
prec = va_arg(ap, int);
f++;
}
else if (Isdigit(*f))
{
prec = atoi((char *) f);
while (Isdigit(*f))
f++; // skip the digits
}
}
if (*f == '#')
{ // alternate form
hash = 1;
f++;
}
if (*f == 'l')
{ // long format
do_long = 1;
f++;
}

fmt = *f;
bp = buf;
switch (fmt) { // do the formatting
case 'd': // 'd' signed decimal
if (do_long)
l = va_arg(ap, long);
else
l = (long) (va_arg(ap, int));
if (l & lt; 0)
{
sign = 1;
l = -l;
}
do {
*bp++ = l % 10 + '0';
} while ((l /= 10) & gt; 0);
if (sign)
*bp++ = '-';
f_width = f_width - (bp - buf);
if (!flush_left)
while (f_width-- & gt; 0)
*str++ = (pad);
for (bp--; bp & gt; = buf; bp--)
*str++ = (*bp);
if (flush_left)
while (f_width-- & gt; 0)
*str++ = (' ');
break;
case 'o': // 'o' octal number
case 'x': // 'x' hex number
case 'u': // 'u' unsigned decimal
if (do_long)
u = va_arg(ap, unsigned long);
else
u = (unsigned long) (va_arg(ap, unsigned));
if (fmt == 'u')
{ // unsigned decimal
do {
*bp++ = u % 10 + '0';
} while ((u /= 10) & gt; 0);
}
else if (fmt == 'o')
{ // octal
do {
*bp++ = u % 8 + '0';
} while ((u /= 8) & gt; 0);
if (hash)
*bp++ = '0';
}
else if (fmt == 'x')
{ // hex
do {
i = u % 16;
if (i & lt; 10)
*bp++ = i + '0';
else
*bp++ = i - 10 + 'a';
} while ((u /= 16) & gt; 0);
if (hash)
{
*bp++ = 'x';
*bp++ = '0';
}
}
i = f_width - (bp - buf);
if (!flush_left)
while (i-- & gt; 0)
*str++ = (pad);
for (bp--; bp & gt; = buf; bp--)
*str++ = ((int) (*bp));
if (flush_left)
while (i-- & gt; 0)
*str++ = (' ');
break;
case 'c': // 'c' character
i = va_arg(ap, int);
*str++ = ((int) (i));
break;
case 's': // 's' string
bp = va_arg(ap, unsigned char *);
if (!bp)
bp = (unsigned char *) " (nil) " ;
f_width = f_width - strlen((char *) bp);
if (!flush_left)
while (f_width-- & gt; 0)
*str++ = (pad);
for (i = 0; *bp & & i & lt; prec; i++)
{
*str++ = (*bp);
bp++;
}
if (flush_left)
while (f_width-- & gt; 0)
*str++ = (' ');
break;
case '%': // '%' character
*str++ = ('%');
break;
}
flush_left = 0, f_width = 0, prec = INF, hash = 0, do_long = 0;
sign = 0;
pad = ' ';
}
}

va_end(ap);
// terminate string with null
*str++ = '\0';
return bufstring;
}

*/

Kod___r__d__owy.rar > qei.c

#include & lt; avr/io.h & gt;
#include & lt; stdint.h & gt;
#include & lt; stddef.h & gt;
#include & lt; stdlib.h & gt;
#include & lt; util/delay.h & gt;
#include & lt; inttypes.h & gt;
#include & lt; avr/interrupt.h & gt;
#include " rprintf.h "
#include " rprintfLCD.h "
#include & lt; math.h & gt;
#include & lt; string.h & gt;
#include " qei.h "
#include & lt; avr/pgmspace.h & gt;

#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#define cbi(sfr, bit) (_SFR_BYTE(sfr) & = ~_BV(bit))

#define ENC_PORT PIND // Port enkodera
#define CAP_PORT PIND // Port wejœcia steruj¹cego STEP/DIR (tylko DIR)
#define SET_ENB PORTB |= _BV(0) // Makro steruj¹ce pinem ENABLE
#define SET_B1 PORTB |= _BV(1) // Makro steruj¹ce pinem B1
#define CLR_ENB PORTB & = ~_BV(0)
#define CLR_B1 PORTB & = ~_BV(1)
#define SAT_LED_ON PORTC & = ~_BV(6)
#define SAT_LED_OFF PORTC |= _BV(6)
#define READY_LED_ON PORTC & = ~_BV(7)
#define READY_LED_OFF PORTC |= _BV(7)
#define ENC_LED_OFF PORTC & = ~_BV(5)
#define ENC_LED_ON PORTC |= _BV(5)
#define LED4_PULSE_ON PORTC |= _BV(4)
#define LED4_PULSE_OFF PORTC & = ~_BV(4)
#define OVERLO_LED_OFF PORTC |= _BV(3)
#define OVERLO_LED_ON PORTC & = ~_BV(3)
#define LED2_PULSE_ON PORTC |= _BV(2)
#define LED2_PULSE_OFF PORTC & = ~_BV(2)

#define USART_BAUDRATE 9600 // Prêdkoœæ transmisji szeregowej
#define BAUD_PRESCALE (((F_CPU / (USART_BAUDRATE * 16UL))) - 1)
//#define RESP_TIME_MS 500 // Czas nagrywania odpowiedzi serwa

const char PROGMEM ready_str[] = " \n\nREADY & gt; " ;

/*************** PID variables *******************/
volatile struct Servo_struct
{
float gainP; // Parametr: przyrost proporcjonalny
float gainI; // Parametr: przyrost integralny
float gainD; // Parametr: przyrost rozniczkowy
float feedF0; // Parametr: przyrost FeedForward pozycja
float feedF1; // Parametr: przyrost FeedForward ró¿niczka pozycji
float maxPosError; // Parametr: maksymalny blad pozycji
float maxError; // Parametr: ograniczenie wielkoœci b³êdu
float maxErrorI; // Parametr: maksymalna wartoœæ ca³kowania
float maxErrorD; // Parametr: maksymalna wartoœæ ró¿niczkowania
float maxPosD; // Parametr: maksymalna wartoœæ ró¿niczkowanej pozycji
float maxPosDD;
float maxOutput; // Parametr: limit mocy motoru
float deadBand; // Parametr: nieczu³oœæ serwa
float error; // b³¹d
float prevError; // poprzedni b³¹d
float errorI; // ca³ka b³êdu
float errorD; // ró¿niczka b³êdu
float positionD; // ró¿niczka pozycji
float positionDD;
long int position; // rejestr pozycji zadanej
long int prevPos; // poprzednia pozycja zadana
long int prevPosD;
long int feedback; // rejestr pozycji aktualnej
float outputI; // zmienna wyjœciowa cz³onu ca³kuj¹cego w opcji skaluj i ca³kuj
float maxOutputI; // maksymalna wartoœæ cz³onu ca³kuj¹cego
float output; // motor output
float offset; // przesuniêcie punktu pracy generatora PWM w stosunku do PIDa
int multiplierCap; // mno¿nik wejœcia
int multiplierEnc; // mno¿nik wejœcia enkodera
int saturated; // PID w nasyceniu
int overLoadCnt; // licznik przeci¹¿enia silnika

} Servo;

volatile struct
{
unsigned enable:1; // Bit w³¹czenia serwa
unsigned dir:1; // Bit kierunku
unsigned dirLast:1; // Poprzedni kierunek
unsigned pidOn:1; // Bit w³¹czenia kalkulacji serwa
unsigned pidOff:1; // Bit wy³¹czenia serwa u¿ywany do komendy wy³¹czenia silnika
unsigned testOn:1; // Bit w³¹czenia trybu testowania odpowiedzi uk³adu
unsigned testOff:1; // Bit
unsigned motorOn:1; // Bit w³¹czenia silnika
unsigned motorOff:1; // Bit wy³¹czenia silnika
unsigned overLoad:1; // Bit przeci¹¿enia silnika
unsigned ovrLoadMsg:1; // Bit pomocniczy - dziêki niemu wiadomoœæ o przeci¹¿eniu wyœwietlana jest 1 krotnie.
} stat;

char inpBuf[8];
unsigned char i, parameter, comCount, udata;

int positionTable[200];
int feedbackTable[200];
float outputTable[200];
//float ff1Table[120];

uint8_t a=0;
long int encoderPos;
long int commandPos;
uint16_t curveLength;
//static short gear = 0;
unsigned short new_cmd,last_cmd, new_fb,last_fb = 0;
short last_state = 0; // last servo cycle enable/disable state


/*********** Encoder, capture variables ***********/

void IOinit(void)
{
DDRD = 0x30; // b00110000 PD5, PD4 out
PORTD = 0xFF; // Podci¹ga PORTD do 1

DDRA = 0xFF; // Ustawia jako wejscie/wyjscie
PORTA = 0xFF; // Podciaga PORTA do 1

DDRB = 0xDB; // b11011011 INT2 / in button in
PORTB = 0xFE; // Podciaga PORTB do 1, PB0 = 0

DDRC = 0xFF; // b11111111 out
PORTC = 0xFF; // Podciaga PORTC do 1

TIMSK = 0x02;

// Ustawienie timer0 do przerwania z f 1kHz
OCR0 = 0xFC;
TCCR0 = 0x0B;

// Timer1 jako fast PWM
TCCR1A = 0xB3; // Konfiguracja z CodeVision
TCCR1B = 0x09;
TCNT1H = 0x00;
TCNT1L = 0x00;
ICR1H = 0x00;
ICR1L = 0x00;
OCR1AH = 0x00;
OCR1AL = 0x00;
OCR1BH = 0x00;
OCR1BL = 0x00;
}

/**************** Encoder Init ******************/
void encoderAInit(void)
{
sbi(MCUCR,ISC00); // Ustawienie INT0
cbi(MCUCR,ISC01); // na zbocze rosnace
sbi(GICR,INT0); // Wlaczenie przerwania INT0
}

/**************** Capture Init ******************/
void encoderBInit(void)
{
sbi(MCUCR,ISC10); // Ustawienie INT1
cbi(MCUCR,ISC11); // na zbocze rosnace
sbi(GICR,INT1); // Wlaczenie przerwania INT1
}

void captureInit(void)
{
sbi(MCUCSR,ISC2); // Ustawienie INT0 na zbocze rosnace
sbi(GICR,INT2); // Wlaczenie przerwania INT2
}

/**************** PID Init **********************/
void pidInit(void)
{
Servo.position = 0;
Servo.feedback = 0;
Servo.maxPosError = 0;
Servo.error = 0;
Servo.output = 0;
Servo.outputI = 0;
Servo.deadBand = 0;
Servo.maxError = 100;
Servo.maxErrorI = 0;
Servo.maxErrorD = 500;
Servo.maxOutput = 511;
Servo.maxOutputI = 511;
Servo.offset = 512;
Servo.maxPosD = 0;
Servo.maxPosDD = 0;
Servo.errorI = 0;
Servo.errorD = 0;
Servo.prevError = 0;
Servo.positionD = 0;
Servo.positionDD = 0;
Servo.prevPos = 0;
Servo.prevPosD = 0;
Servo.gainP = 25;
Servo.gainI = 10;
Servo.gainD = 50;
Servo.feedF0 = 8;
Servo.feedF1 = 10;
Servo.multiplierCap = 1;
Servo.multiplierEnc = 1;
Servo.saturated = 0;
Servo.overLoadCnt = 0;
}

void pidStartUp(void)
{
//new_cmd = last_cmd = new_fb = last_fb = 0;
commandPos = 0;
encoderPos = 0;
Servo.position = 0;
Servo.feedback = 0;
Servo.error = 0.0;
Servo.output = 0.0;
Servo.offset = 512;
Servo.outputI = 0.0;
Servo.errorD = 0.0;
Servo.positionD = 0.0;
Servo.positionDD = 0.0;
Servo.prevPos = 0;
Servo.prevPosD = 0;
Servo.overLoadCnt = 0;
stat.overLoad = 0;
}

void moveInit(void)
{
stat.enable = 1; // Flaga w³¹czenia (nieu¿ywane)
stat.dir = 0; // Flaga kierunku
stat.dirLast = 0; // Flaga ostatniego kierunku
stat.pidOn = 1; // Flaga w³¹czenia PIDa
stat.pidOff = 0; // Flaga wy³¹czenia PIDa
stat.testOn = 0; // Flaga w³¹czenia tryby rejestrowania
stat.testOff = 0;
stat.motorOn = 1; // Flaga w³¹czenia silnika
stat.motorOff = 0; // Flaga wy³¹czenia silnika
stat.overLoad = 0; // Flaga przeci¹¿enia silnika
stat.ovrLoadMsg = 0; // Flaga pomocnicza wiadomoœci o przeci¹¿eniu
}

void usartInit(void)
{
UCSRB |= (1 & lt; & lt; RXEN) | (1 & lt; & lt; TXEN); // Turn on the transmission and reception circuitry
UCSRC |= (1 & lt; & lt; URSEL) | (1 & lt; & lt; UCSZ0) | (1 & lt; & lt; UCSZ1); // Use 8-bit character sizes
UBRRL = BAUD_PRESCALE; // Load lower 8-bits of the baud rate value into the low byte of the UBRR register
UBRRH = (BAUD_PRESCALE & gt; & gt; 8); // Load upper 8-bits of the baud rate value into the high byte of the UBRR register
UCSRB |= (1 & lt; & lt; RXCIE); // Enable the USART Recieve Complete interrupt (USART_RXC)
}

// Funkcja wylaczajaca przerwanie enkodera
void encoderBOff(void)
{
cbi(GICR,INT1);
}

// Funkcja wlaczajaca przerwanie enkodera
void encoderBOn(void)
{
sbi(GICR,INT1);
}

// Obracaj silnikiem w prawo
void go_right(void)
{
TCCR1A = 0xB3; // odwrócenie fazy sygna³u PWM
PORTC |= _BV(1);
PORTC & = ~_BV(0);
}

// Obracaj silnikiem w lewo
void go_left(void)
{
TCCR1A = 0xE3; // odwrócenie fazy sygna³u PWM
PORTC |= _BV(0);
PORTC & = ~_BV(1);
}

// Zewrzyj styki motoru, fast stop
void stop_motor(void)
{
CLR_ENB;
PORTC |= _BV(0);
PORTC |= _BV(1);
}

// Funkcja wypisujaca dane na RS232
void print_byte_to_usart(uint8_t dana)
{
while ((UCSRA & (1 & lt; & lt; UDRE)) == 0) {}; // Nic nie rób dopóki rejestr UDR nie bêdzie gotowy na zapis kolejnych danych
UDR = dana; // Wyœlij otrzymane dane z powrotem do komputera
}

// Funkcja wypisujaca dane na LCD
void print_byte_to_lcd(uint8_t dana)
{
LCDsendChar(dana);
}

// Funkcja limituj¹ca wartoœæ i zwacaj¹ca wartoœæ ogr. zmiennej
float limitVal(float variable, float maxValue)
{
if(variable & gt; maxValue)
{
variable = maxValue;
return variable;
}
else if(variable & lt; -maxValue)
{
variable = -maxValue;
return variable;
}
else return variable;
}

//Wykrywanie nasycenia napêdu
int satLimit(float variable, float maxValue)
{
if(variable & gt; maxValue)
{
return 1;
}
else if(variable & lt; -maxValue)
{
return -1;
}
else
{
return 0;
}
}

// Przetwarzanie poleceñ wydanych przez RS232
void DoCommand(void)
{
if (comCount == 0)
{
switch(inpBuf[0])
{
case 'P': parameter = 'P';
break;
case 'I': parameter = 'I';
break;
case 'D': parameter = 'D';
break;
case 'M': parameter = 'M';
break;
case 'N': parameter = 'N';
break;
case 'O': parameter = 'O';
break;
case 'R': parameter = 'R';
break;
case 'B': parameter = 'B';
break;
case 'X': parameter = 'X';
break;
case 'T': parameter = 'T';
break;
case 'F': parameter = 'F';
break;
case 'G': parameter = 'G';
break;
case 'C': parameter = 'C';
break;
case 'E': parameter = 'E';
break;
case 'H': rprintfProgStr(PSTR( " \nAvailable commands: " ));
rprintfProgStr(PSTR( " \n? - current status of ServoDrive " ));
rprintfProgStr(PSTR( " \nX,x - go to x position as fast as possible " ));
rprintfProgStr(PSTR( " \nP,x - change of Proportional Gain parameter " ));
rprintfProgStr(PSTR( " \nI,x - change of Integral Gain parameter " ));
rprintfProgStr(PSTR( " \nD,x - change of Derivative Gain parameter " ));
rprintfProgStr(PSTR( " \nM,x - change of Max Error parameter " ));
rprintfProgStr(PSTR( " \nN,x - change of Max I Output parameter " ));
rprintfProgStr(PSTR( " \nO,x - change of Max D Error parameter " ));
rprintfProgStr(PSTR( " \nR,x - change of Max Output parameter " ));
rprintfProgStr(PSTR( " \nB,x - change of Dead Band parameter " ));
rprintfProgStr(PSTR( " \nF,x - change of FF0 parameter " ));
rprintfProgStr(PSTR( " \nG,x - change of FF1 parameter " ));
rprintfProgStr(PSTR( " \nC,x - change of multiplierCap parameter " ));
rprintfProgStr(PSTR( " \nE,x - change of multiplierEnc parameter " ));
rprintfProgStr(PSTR( " \nT,x - go to x position for step response test " ));
rprintfProgStr(PSTR( " \nS - current status of PID calculations " ));
rprintfProgStr(PSTR( " \n0 - turns the motor off " ));
rprintfProgStr(PSTR( " \n1 - turns the motor on " ));
rprintfProgStr(PSTR( " \n2 - 2x encoder decoding " ));
rprintfProgStr(PSTR( " \n4 - 4x encoder decoding " ));
break;
case '0': stat.motorOn = 0;
stat.motorOff = 1;
parameter = 0;
rprintfProgStr(PSTR( " \nMotor is turned OFF " ));
break;
case '1': stat.motorOn = 1;
parameter = 0;
rprintfProgStr(PSTR( " \nMotor is turned ON " ));
break;
case '2': encoderBOff();
rprintfProgStr(PSTR( " \n2x encoder decoding is ON " ));
break;
case '4': encoderBInit();
encoderBOn();
rprintfProgStr(PSTR( " \n4x encoder decoding is ON " ));
break;
case 'Q': Servo.maxPosError = 0;
rprintfProgStr(PSTR( " \nError counter is cleared " ));
break;
case 'W': pidStartUp();
rprintfProgStr(PSTR( " \nPID has been initialized " ));
break;
case '?': rprintfProgStr(PSTR( " \nP gain = " ));
rprintfFloat(4,Servo.gainP);
rprintfProgStr(PSTR( " \nI gain = " ));
rprintfFloat(4,Servo.gainI);
rprintfProgStr(PSTR( " \nD gain = " ));
rprintfFloat(4,Servo.gainD);
rprintfProgStr(PSTR( " \nFF0 gain = " ));
rprintfFloat(4,Servo.feedF0);
rprintfProgStr(PSTR( " \nFF1 gain = " ));
rprintfFloat(4,Servo.feedF1);
rprintfProgStr(PSTR( " \nEncoder position = " ));
rprintfNum(10, 6, 1, ' ',Servo.feedback);
rprintfProgStr(PSTR( " \nCom. position = " ));
rprintfNum(10, 6, 1, ' ',Servo.position);
rprintfProgStr(PSTR( " \nMax. detect. err.= " ));
rprintfFloat(4,Servo.maxPosError);
rprintfProgStr(PSTR( " \nMax error = " ));
rprintfFloat(4,Servo.maxError);
rprintfProgStr(PSTR( " \nMax I output = " ));
rprintfFloat(4,Servo.maxOutputI);
rprintfProgStr(PSTR( " \nMax D error = " ));
rprintfFloat(4,Servo.maxErrorD);
rprintfProgStr(PSTR( " \nMax output = " ));
rprintfFloat(4,Servo.maxOutput);
rprintfProgStr(PSTR( " \nDead band = " ));
rprintfFloat(4,Servo.deadBand);
rprintfProgStr(PSTR( " \nMultiplierCapture= " ));
rprintfNum(10, 6, 1, ' ',Servo.multiplierCap);
rprintfProgStr(PSTR( " \nMultiplierEncoder= " ));
rprintfNum(10, 6, 1, ' ',Servo.multiplierEnc);

parameter = 0;
break;
case 'S': rprintfProgStr(PSTR( " \rServo Loop Internal Calcs:\r\n " ));
rprintfProgStr(PSTR( " Servo.errorP= " ));
rprintfFloat(4,(Servo.error * Servo.gainP));
rprintfProgStr(PSTR( " \nServo.outputI= " ));
rprintfFloat(4,Servo.outputI);
rprintfProgStr(PSTR( " \nServo.errorD= " ));
rprintfFloat(4,(Servo.error * Servo.gainD));
rprintfProgStr(PSTR( " \nServo.feedF0= " ));
rprintfFloat(4,(Servo.feedF0 * Servo.positionD));
rprintfProgStr(PSTR( " \nServo.feedF1= " ));
rprintfFloat(4,(Servo.feedF1 * Servo.positionDD));
rprintfProgStr(PSTR( " \nServo.output= " ));
rprintfFloat(4,Servo.output);
rprintfProgStr(PSTR( " \nMax. detect. err.= " ));
rprintfFloat(4,Servo.maxPosError);
rprintfProgStr(PSTR( " \nsaturated= " ));
rprintfNum(10, 2, 1, ' ',Servo.saturated);
rprintfProgStr(PSTR( " \nstat.enable= " ));
rprintfNum(10, 2, 0, ' ',stat.enable);
rprintfProgStr(PSTR( " \nstat.pidOn= " ));
rprintfNum(10, 2, 0, ' ',stat.pidOn);
rprintfProgStr(PSTR( " \nstat.motorOn= " ));
rprintfNum(10, 2, 0, ' ',stat.motorOn);
rprintfProgStr(PSTR( " \nOverLoad? " ));
rprintfNum(10, 5, 1, ' ',Servo.overLoadCnt);
rprintfProgStr(PSTR( " \nWynik I ? " ));
rprintfNum(10, 2, 0, ' ',((Servo.saturated & lt; 0) & & (Servo.error & lt; 0)) || ((Servo.saturated & gt; 0) & & (Servo.error & gt; 0)));
parameter = 0;
break;
default: if (inpBuf[0] != '\0')
{
rprintfProgStr(PSTR( " \nUnknown command " ));
}
parameter = 0;
break;
}
}
else if (comCount == 1)
{
switch(parameter)
{
case 'P': Servo.gainP = atof(inpBuf);
rprintfProgStr(PSTR( " \nP gain = " ));
rprintfFloat(4,Servo.gainP);
break;
case 'I': Servo.gainI = atof(inpBuf);
rprintfProgStr(PSTR( " \nI gain = " ));
rprintfFloat(4,Servo.gainI);
break;
case 'D': Servo.gainD = atof(inpBuf);
rprintfProgStr(PSTR( " \nD gain = " ));
rprintfFloat(4,Servo.gainD);
break;
case 'M': Servo.maxError = atof(inpBuf);
rprintfProgStr(PSTR( " \nMax Error = " ));
rprintfFloat(4,Servo.maxError);
break;
case 'N': Servo.maxOutputI = atof(inpBuf);
rprintfProgStr(PSTR( " \nMax I Output = " ));
rprintfFloat(4,Servo.maxOutputI);
break;
case 'O': Servo.maxErrorD = atof(inpBuf);
rprintfProgStr(PSTR( " \nMax D Error = " ));
rprintfFloat(4,Servo.maxErrorD);
break;
case 'R': Servo.maxOutput = atof(inpBuf);
rprintfProgStr(PSTR( " \nMax Output = " ));
rprintfFloat(4,Servo.maxOutput);
break;
case 'B': Servo.deadBand = atof(inpBuf);
rprintfProgStr(PSTR( " \nDead Band = " ));
rprintfFloat(4,Servo.deadBand);
break;
case 'X': commandPos = atoi(inpBuf);
rprintfProgStr(PSTR( " \nCommanded pos. = " ));
rprintfNum(10, 6, 1, ' ',commandPos);
break;
case 'T': commandPos = atoi(inpBuf);
rprintfProgStr(PSTR( " \nTest jump = " ));
rprintfNum(10, 6, 1, ' ',commandPos);
stat.testOn = 1;
break;
case 'F': Servo.feedF0 = atof(inpBuf);
rprintfProgStr(PSTR( " \nFeedForward0 gain= " ));
rprintfFloat(4,Servo.feedF0);
break;
case 'G': Servo.feedF1 = atof(inpBuf);
rprintfProgStr(PSTR( " \nFeedForward1 gain= " ));
rprintfFloat(4,Servo.feedF1);
break;
case 'C': Servo.multiplierCap = atoi(inpBuf);
if(Servo.multiplierCap & gt; 4)
Servo.multiplierCap = 4;
if(Servo.multiplierCap & lt; 1)
Servo.multiplierCap = 1;
rprintfProgStr(PSTR( " \nMultiplierCap= " ));
rprintfNum(10, 6, 1, ' ',Servo.multiplierCap);
break;
case 'E': Servo.multiplierEnc = atoi(inpBuf);
if(Servo.multiplierEnc & gt; 4)
Servo.multiplierEnc = 4;
if(Servo.multiplierEnc & lt; 1)
Servo.multiplierEnc = 1;
rprintfProgStr(PSTR( " \nMultiplierEnc= " ));
rprintfNum(10, 6, 1, ' ',Servo.multiplierEnc);
break;
default: break;
}
}
else;
}

void calcPID(void)
{
float tmp1;
float periodfp;//, periodrecip;

periodfp = 0.01; // 0,001

// Oblicz blad
tmp1 = (float)(Servo.position - Servo.feedback);
Servo.error = tmp1;

// Aktualizuj zmienn¹ wykorzystywan¹ do monitorowania maksymalnego b³êdu podczas ruchu
if(abs(Servo.error) & gt; abs(Servo.maxPosError))
Servo.maxPosError = abs(Servo.error);

// Zastosuj maksima dla b³êdu
if(Servo.maxError != 0)
{
Servo.error = limitVal(Servo.error, Servo.maxError);
}

// Ogranicz czu³oœæ serwonapêdu przez zakres nieczu³oœci
if(Servo.error & gt; Servo.deadBand)
Servo.error -= Servo.deadBand;
else if(Servo.error & lt; Servo.deadBand)
Servo.error += Servo.deadBand;
else
Servo.error = 0;

// Licz calkowanie tylko gdy pid w³¹czony
if((Servo.gainI & gt; 0.0) & & (stat.enable != 0))
{
// Jeœli wyjœcie jest na pe³nej/zerowej mocy nie pozwól ca³kowaniu odp³yn¹æ
if(((Servo.saturated & lt; 0) & & (Servo.error & lt; 0)) ||
((Servo.saturated & gt; 0) & & (Servo.error & gt; 0)))
{
//Nie ca³kuj gdy wyjœcie jest nasycone
}
else
// Ca³kuj
//Servo.errorI += Servo.error * periodfp;
Servo.outputI += (Servo.error * Servo.gainI * periodfp);

// Saturacja cz³ony ca³kuj¹cego
Servo.saturated = satLimit(Servo.outputI, Servo.maxOutputI); // Jeœli cz³on ca³kuj¹cy siê nasyci to wy³¹cz ca³kowanie
// Zastosuj maksima dla ca³kowania
if(Servo.maxOutputI != 0.0)
{
Servo.outputI = limitVal(Servo.outputI, Servo.maxOutputI);
}
}
else
{
// Jeœli PID nie w³¹czony zresetuj ca³kowanie
Servo.outputI = 0;
}

// Oblicz akcjê ró¿niczkuj¹c¹
Servo.errorD = (float)(Servo.error - Servo.prevError);
Servo.prevError = Servo.error;

// Zastosuj maksima dla akcji ró¿niczkuj¹cej
if(Servo.maxErrorD != 0)
{
Servo.errorD = limitVal(Servo.errorD, Servo.maxErrorD);
}

// Oblicz ró¿niczkê pozycji - prêdkoœæ
Servo.positionD = (float)(Servo.position - Servo.prevPos);
// Oblicz ró¿niczkê prêdkoœci - przyspieszenie
Servo.positionDD = (float)(Servo.positionD - Servo.prevPosD);
Servo.prevPos = Servo.position;
Servo.prevPosD = Servo.positionD;

// Zastosuj maksima dla akcji ró¿niczkuj¹cej
if(Servo.maxPosD != 0)
{
Servo.positionD = limitVal(Servo.positionD, Servo.maxPosD);
}

if(Servo.maxPosDD != 0)
{
Servo.positionDD = limitVal(Servo.positionDD, Servo.maxPosDD);
}

// Dodaj b³êdy
Servo.output = (Servo.gainP * Servo.error);
Servo.output += Servo.outputI;
Servo.output += (Servo.gainD * Servo.errorD);
//Feed forward
Servo.output += (Servo.feedF0 * Servo.positionD);
Servo.output += (Servo.feedF1 * Servo.positionDD);

// SprawdŸ czy napêd jest nasycony
Servo.saturated = satLimit(Servo.output, Servo.maxOutput);

// SprawdŸ czy napêd jest przeci¹¿ony
if(Servo.saturated & & stat.motorOn)
{
Servo.overLoadCnt = Servo.overLoadCnt + 3;
}
else if(stat.motorOn) // zmniejsz wartoœæ rejestru przeci¹¿eniowego
{ // gdy napêd pracuje normalnie
Servo.overLoadCnt--;
if(Servo.overLoadCnt & lt; 0)
Servo.overLoadCnt = 0;
}

// Jeœli przeci¹¿ony przez 20ms to wy³¹cz silnik
if((Servo.overLoadCnt & gt; 60) & & stat.motorOn)
{
stat.overLoad = 1; // flaga przeci¹¿onego serwonapêdu
stat.motorOn = 0; // wy³¹cz silnik
stat.motorOff = 1; // wy³¹cz silnik
stat.ovrLoadMsg = 1; // bit pomocniczy
}
// Zastosuj maksima dla wyjœcia PIDa
Servo.output = limitVal(Servo.output, Servo.maxOutput);
}

void setPWM(float amps)
{
OCR1A = abs(amps); //ENABLE PD5
OCR1B = abs(amps); //ENABLE PD4
}


/*************** Obsluga enkodera *********************/
ISR (INT0_vect, ISR_BLOCK) //obs³uga przerwania INT0
{
ENC_LED_OFF;
//unsigned char encoderDir;

// Odczyt portu i przesuniecie bitow na prawo
// Sprawdzenie kierunku obrotu - dzialanie XOR pin 7654 3210
//encoderDir = ((ENC_PORT & 0x08) & gt; & gt; 3) ^ ((ENC_PORT & 0x04) & gt; & gt; 2); // 0000 1000 0x08 B
// 0000 0100 0x04 A
if((((ENC_PORT & 0x08) & gt; & gt; 3) ^ ((ENC_PORT & 0x04) & gt; & gt; 2)) == 0)
// Kierunek obrotu w prawo: dodaj
{
encoderPos++;
}
else
// Kierunek obrotu w lewo: odejmij
{
encoderPos--;
}
ENC_LED_ON;
}

/*************** Obsluga enkodera B **********************/
ISR (INT1_vect, ISR_BLOCK) //obs³uga przerwania INT1 na sygna³ B
{
ENC_LED_OFF;
//unsigned char encoderDir;
// Sprawdzenie kierunku rozkazu - dzialanie XOR pin 7654 3210
//encoderDir = ((ENC_PORT & 0x08) & gt; & gt; 3) ^ ((ENC_PORT & 0x04) & gt; & gt; 2); // 0000 1000 0x08 B
// 0000 0100 0x04 A
if((((ENC_PORT & 0x08) & gt; & gt; 3) ^ ((ENC_PORT & 0x04) & gt; & gt; 2)) == 0)
// Kierunek rozkazu w prawo: dodaj
{
encoderPos--;
}
else
// Kierunek rozkazu w lewo: odejmij
{
encoderPos++;
}
ENC_LED_ON;
}



/*************** Obsluga rozkazów **********************/
ISR (INT2_vect, ISR_BLOCK) //obs³uga przerwania INT2 na sygna³ STEP
{
LED2_PULSE_OFF;
//unsigned char captureDir;
// Sprawdzenie kierunku rozkazu - dzialanie XOR pin 7654 3210
//captureDir = ((CAP_PORT & 0x08) & gt; & gt; 3) ^ ((CAP_PORT & 0x40) & gt; & gt; 6); // 0000 1000 0x08 STEP
// 0100 0000 0x40 DIR
if((CAP_PORT & 0x40)) // Sprawdza wartoϾ DIR
// Kierunek rozkazu w prawo: dodaj
{
commandPos -= Servo.multiplierCap;
}
else
// Kierunek rozkazu w lewo: odejmij
{
commandPos += Servo.multiplierCap;
}
LED2_PULSE_ON;
}

/*************** Przerwanie 1kHz PID ******************/
ISR (TIMER0_COMP_vect, ISR_NOBLOCK)
{

LED4_PULSE_ON; // sprawdzamy jak d³ugo tutaj jesteœmy

if(stat.pidOn) // W³¹czenie wy³¹czenie Pida
{
//gear = 0;
if(stat.pidOn & & (last_state == 0))
{
pidStartUp();
}

Servo.position = commandPos; // uaktualnij zadan¹ pozycjê
Servo.feedback = encoderPos; // uaktualnij pozycjê z enkodera

calcPID(); // Oblicz odpowiedŸ regulatora PID
if(stat.motorOn){
setPWM(Servo.output + Servo.offset);
SET_ENB;
}
else{
CLR_ENB;
}

last_state = stat.pidOn;
// Ustaw PWM
if(stat.testOn) // Tryb rejestrowania trajektorii
{

if(a & lt; 200) // Rejestrujemy tylko pierwsze 200ms ruchu
{
positionTable[a] = Servo.position;
feedbackTable[a] = Servo.feedback;
outputTable[a] = Servo.output;
//ff1Table[a] = (Servo.feedF1 * Servo.positionDD);
}
else
{
//stat.pidOn = 0;
//stat.pidOff = 1;
stat.testOn = 0; // Po rejestracji wy³¹cz tryb rejestrowania

for(int b=0 ; b & lt; a ; b++) // Wydrukuj przez RS wyniki
{
rprintf( " \n " );
rprintfNum(10, 3, 0, ' ',b);
rprintf( " ; " );
rprintfNum(10, 4, 0, ' ',positionTable[b]);
rprintf( " ; " );
rprintfNum(10, 4, 0, ' ',feedbackTable[b]);
rprintf( " ; " );
rprintfFloat(4,outputTable[b]);
//rprintf( " ; " );
//rprintfFloat(4,ff1Table[b]);
}
rprintfProgStr(ready_str);
a = 0;
}
a++; // licznik 200ms
}
}
else
{
CLR_ENB;
}
LED4_PULSE_OFF;
}


/*************** Przerwanie USART ******************/
ISR(USART_RXC_vect, ISR_BLOCK)
{
stat.motorOn = 0; // Wy³¹cz silnik (pwm) na czas transmisji
switch(udata = UDR)
{
case ',': DoCommand(); // Przetwórz polecenie
memset(inpBuf,0,8); // kasowanie bufora
i=0;
comCount++; // Licznik przecinka
UDR = udata;
break;

case 0x0d:DoCommand(); // Odebrano CR czyli enter
memset(inpBuf,0,8); // kasowanie bufora
i=0;
comCount=0; // reset licznika przecinka
rprintfProgStr(ready_str); // drukuj READY
if(!stat.motorOff) // Wy³¹czanie w³¹czanie
{
stat.motorOn = 1;
}
stat.motorOff = 0;
break;

default: inpBuf[i] = udata; // Odczyt 8 znaków z RS
i++;
if (i & gt; 7)
{
rprintfProgStr(ready_str);
memset(inpBuf,0,8);
i=0;
}
else UDR = udata;
break;
}
}

int main (void)
{
CLR_ENB;
cli();
usartInit(); // Inicjalizacja USARTa
IOinit(); // Inicjalizacja portów
rprintfInitRS(print_byte_to_usart); // Inicjalizacja funkcji rprintf dla RSa
rprintfInitLCD(print_byte_to_lcd); // Inicjalizacja funkcji rprintf dla LCDka

// Inicjalizacja LCD
LCDinit();
LCDcursorOFF();

LCDclr();
rprintfProgStrMLCD( " ServoMechanism " );
LCDGotoXY(0,1);
rprintfProgStrMLCD( " ver. 1.2 " );

rprintf( " \nServoMechanism " );
rprintf( " \nver. 1.2 " );
rprintf( " \nType 'H' to see help\n " );
rprintfProgStr(ready_str);
_delay_ms(1000);
LCDclr();

encoderAInit(); // Inicjalizacja kana³u A enkodera
encoderBInit(); // Inicjalizacja kana³u B enkodera
captureInit(); // Inicjalizacja kana³u STEP rozkazu
pidInit(); // Inicjalizacja PIDa
moveInit(); // Inicjalizacja parametrów Serwa
setPWM(0.0);
_delay_ms(100);
sei(); // W³¹czenie przerwañ
encoderBOff(); // Wy³¹czenie przerwañ kana³u B enkodera (dekodowanie x2)

while(1)
{
LCDhome();
rprintfProgStrMLCD( " Er " );

LCDGotoXY(2,0);
rprintfNumLCD(10,3,1,' ',Servo.error);

LCDGotoXY(0,1);
rprintfProgStrMLCD( " O " );

LCDGotoXY(1,1);
rprintfFloatLCD(4,Servo.output);

LCDGotoXY(8,0);
rprintfProgStrMLCD( " Pos " );

LCDGotoXY(11,0);
rprintfNumLCD(10,4,1,' ',Servo.position);

LCDGotoXY(11,1);
rprintfFloatLCD(3,Servo.maxPosError);


// wskaŸnik nasyconego napêdu
if(Servo.saturated) SAT_LED_ON; // Dioda nasyconego serwonapêdu
else SAT_LED_OFF;
if(stat.pidOn & & stat.motorOn) READY_LED_ON; // Dioda gotowoœci serwonapêdu
else READY_LED_OFF;
if(stat.overLoad & & stat.ovrLoadMsg) // wyœwietl jednokrotnie informacjê o przeci¹¿eniu
{
rprintf( " \nMotor overload, type W and 1 to restart the motor " );
stat.ovrLoadMsg = 0; // zresetuj bit pomocniczy, zabezpieczenie przed wielokrotnym komunikatem
}
if(stat.overLoad)
OVERLO_LED_ON; // zapal diodê przeci¹¿enia
else
OVERLO_LED_OFF; // zgaœ diodê przeci¹¿enia

if((PINB & (1 & lt; & lt; PB5)) == 0) //w³¹cz rejestrowanie ruchu po naciœniêciu przycisku.
//if(stat.testOff == 0)
stat.testOn = 1;

}
}

Kod___r__d__owy.rar > cmdlineconf.h

/*! \file cmdlineconf.h \brief Command-Line Interface Library Configuration. */
//*****************************************************************************
//
// File Name : 'cmdlineconf.h'
// Title : Command-Line Interface Library Configuration
// Author : Pascal Stang - Copyright (C) 2003
// Created : 2003.07.16
// Revised : 2003.07.21
// Version : 0.1
// Target MCU : Atmel AVR Series
// Editor Tabs : 4
//
// NOTE: This code is currently below version 1.0, and therefore is considered
// to be lacking in some functionality or documentation, or may not be fully
// tested. Nonetheless, you can expect most functions to work.
//
// This code is distributed under the GNU Public License
// which can be found at http://www.gnu.org/licenses/gpl.txt
//
//*****************************************************************************

#ifndef CMDLINECONF_H
#define CMDLINECONF_H

#include " global.h "

// constants/macros/typdefs

// size of command database
// (maximum number of commands the cmdline system can handle)
#define CMDLINE_MAX_COMMANDS 10

// maximum length (number of characters) of each command string
// (quantity must include one additional byte for a null terminator)
#define CMDLINE_MAX_CMD_LENGTH 15

// allotted buffer size for command entry
// (must be enough chars for typed commands and the arguments that follow)
#define CMDLINE_BUFFERSIZE 80

// number of lines of command history to keep
// (each history buffer is CMDLINE_BUFFERSIZE in size)
// ***** ONLY ONE LINE OF COMMAND HISTORY IS CURRENTLY SUPPORTED
#define CMDLINE_HISTORYSIZE 1

#endif

Kod___r__d__owy.rar > i2cconf.h

/*! \file i2cconf.h \brief I2C (TWI) interface configuration. */
//*****************************************************************************
//
// File Name : 'i2cconf.h'
// Title : I2C (TWI) interface configuration
// Author : Pascal Stang - Copyright (C) 2002-2003
// Created : 2002.06.25
// Revised : 2003.03.02
// Version : 0.7
// Target MCU : Atmel AVR series
// Editor Tabs : 4
//
// This code is distributed under the GNU Public License
// which can be found at http://www.gnu.org/licenses/gpl.txt
//
//*****************************************************************************

#ifndef I2CCONF_H
#define I2CCONF_H

// define I2C data buffer sizes
// These buffers are used in interrupt-driven Master sending and receiving,
// and in slave sending and receiving. They must be large enough to store
// the largest I2C packet you expect to send and receive, respectively.
#define I2C_SEND_DATA_BUFFER_SIZE 0x20
#define I2C_RECEIVE_DATA_BUFFER_SIZE 0x20

#endif

Kod___r__d__owy.rar > mmcconf.h

/*! \file mmcconf.h \brief MultiMedia and SD Flash Card Interface Configuration. */
//*****************************************************************************
//
// File Name : 'mmc.h'
// Title : MultiMedia and SD Flash Card Interface Configuration
// Author : Pascal Stang - Copyright (C) 2004
// Created : 2004.09.22
// Revised : 2004.09.22
// Version : 0.1
// Target MCU : Atmel AVR Series
// Editor Tabs : 4
//
// NOTE: This code is currently below version 1.0, and therefore is considered
// to be lacking in some functionality or documentation, or may not be fully
// tested. Nonetheless, you can expect most functions to work.
//
// This code is distributed under the GNU Public License
// which can be found at http://www.gnu.org/licenses/gpl.txt
//
//*****************************************************************************

#ifndef MMCCONF_H
#define MMCCONF_H

// define to enable debugging print statements
//#define MMC_DEBUG

// MMC card chip select pin defines
#define MMC_CS_PORT PORTB
#define MMC_CS_DDR DDRB
#define MMC_CS_PIN 0

#endif

Kod___r__d__owy.rar > sta013conf.h

/*! \file sta013conf.h \brief STA013 MP3 player driver configuration. */
//*****************************************************************************
//
// File Name : 'sta013.h'
// Title : STMicroelectronics STA013 MP3 player driver
// Author : Pascal Stang - Copyright (C) 2000-2002
// Created : 10/22/2000
// Revised : 12/04/2000
// Version : 0.3
// Target MCU : ATmega103 (should work for Atmel AVR Series)
// Editor Tabs : 4
//
// NOTE: This code is currently below version 1.0, and therefore is considered
// to be lacking in some functionality or documentation, or may not be fully
// tested. Nonetheless, you can expect most functions to work.
//
// This code is distributed under the GNU Public License
// which can be found at http://www.gnu.org/licenses/gpl.txt
//
//*****************************************************************************


#ifndef STA013CONF_H
#define STA013CONF_H

// STA013 Configuration

// STA013 demand line
#define STA013_DEMAND_PORT PORTE // port to which DEMAND line is connected
#define STA013_DEMAND_PORTIN PINE // input port to which DEMAND line is connected
#define STA013_DEMAND_PIN PE4 // port pin to which DEMAND line is connected
#define STA013_DEMAND_INTR SIG_INTERRUPT4 // interrupt to which DEMAND line is connected

#endif

Kod___r__d__owy.rar > avrlibtypes.h

/*! \file avrlibtypes.h \brief AVRlib global types and typedefines. */
//*****************************************************************************
//
// File Name : 'avrlibtypes.h'
// Title : AVRlib global types and typedefines include file
// Author : Pascal Stang
// Created : 7/12/2001
// Revised : 9/30/2002
// Version : 1.0
// Target MCU : Atmel AVR series
// Editor Tabs : 4
//
// Description : Type-defines required and used by AVRlib. Most types are also
// generally useful.
//
// This code is distributed under the GNU Public License
// which can be found at http://www.gnu.org/licenses/gpl.txt
//
//*****************************************************************************


#ifndef AVRLIBTYPES_H
#define AVRLIBTYPES_H

#ifndef WIN32
// true/false defines
#define FALSE 0
#define TRUE -1
#endif

// datatype definitions macros
typedef unsigned char u08;
typedef signed char s08;
typedef unsigned short u16;
typedef signed short s16;
typedef unsigned long u32;
typedef signed long s32;
typedef unsigned long long u64;
typedef signed long long s64;

/* use inttypes.h instead
// C99 standard integer type definitions
typedef unsigned char uint8_t;
typedef signed char int8_t;
typedef unsigned short uint16_t;
typedef signed short int16_t;
typedef unsigned long uint32_t;
typedef signed long int32_t;
typedef unsigned long uint64_t;
typedef signed long int64_t;
*/
// maximum value that can be held
// by unsigned data types (8,16,32bits)
#define MAX_U08 255
#define MAX_U16 65535
#define MAX_U32 4294967295

// maximum values that can be held
// by signed data types (8,16,32bits)
#define MIN_S08 -128
#define MAX_S08 127
#define MIN_S16 -32768
#define MAX_S16 32767
#define MIN_S32 -2147483648
#define MAX_S32 2147483647

#ifndef WIN32
// more type redefinitions
typedef unsigned char BOOL;
typedef unsigned char BYTE;
typedef unsigned int WORD;
typedef unsigned long DWORD;

typedef unsigned char UCHAR;
typedef unsigned int UINT;
typedef unsigned short USHORT;
typedef unsigned long ULONG;

typedef char CHAR;
typedef int INT;
typedef long LONG;
#endif

#endif

Kod___r__d__owy.rar > rprintfLCD.h

/*! \file rprintfLCD.h \brief printf routine and associated routines. */
//****************************************************************************
//
// File Name : 'rprintfLCD.h'
// Title : printf routine and associated routines
// Author : Pascal Stang - Copyright (C) 2000-2002
// Created : 2000.12.26
// Revised : 2003.5.1
// Version : 1.0
// Target MCU : Atmel AVR series and other targets
// Editor Tabs : 4
//
// NOTE: This code is currently below version 1.0, and therefore is considered
// to be lacking in some functionality or documentation, or may not be fully
// tested. Nonetheless, you can expect most functions to work.
//
// This code is distributed under the GNU Public License
// which can be found at http://www.gnu.org/licenses/gpl.txt
//
/// \ingroup general
/// \defgroup rprintf printf() Function Library (rprintf.c)
/// \code #include " rprintfLCD.h " \endcode
/// \par Overview
/// The rprintfLCD function library provides a simplified (reduced) version of
/// the common C printf() function.  See the code files for details about
/// which printf features are supported.  Also in this library are a
/// variety of functions for fast printing of certain common data types
/// (variable types).  Functions include print string from RAM, print
/// string from ROM, print string snippet, print hex byte/short/long, and
/// a custom-formatted number print, as well as an optional floating-point
/// print routine.
///
/// \note All output from the rprintfLCD library can be directed to any device
/// or software which accepts characters.  This means that rprintfLCD output
/// can be sent to the UART (serial port) or can be used with the LCD
/// display libraries to print formatted text on the screen.
//
//****************************************************************************
//@{

#ifndef RPRINTFLCD_H
#define RPRINTFLCD_H

// needed for use of PSTR below
#include & lt; avr/pgmspace.h & gt;

// configuration
// defining RPRINTF_SIMPLE will compile a smaller, simpler, and faster printf() function
// defining RPRINTF_COMPLEX will compile a larger, more capable, and slower printf() function
#ifndef RPRINTFLCD_COMPLEX
#define RPRINTFLCD_SIMPLE
#endif

// Define RPRINTF_FLOAT to enable the floating-point printf function: rprintfFloat()
// (adds +4600bytes or 2.2Kwords of code)

// defines/constants
#define STRING_IN_RAM 0
#define STRING_IN_ROM 1

// make a putchar for those that are used to using it
//#define putchar(c) rprintfChar(c);

// functions

//! Initializes the rprintf library for an output stream.
/// You must call this initializer once before using any other rprintf function.
/// The argument must be a character stream output function.
void rprintfInitLCD(void (*putchar_func)(unsigned char c));

//! prints a single character to the current output device
void rprintfCharLCD(unsigned char c);

//! prints a null-terminated string stored in RAM
void rprintfStrLCD(char str[]);

//! Prints a section of a string stored in RAM.
/// Begins printing at position indicated by & lt; start & gt; ,
/// and prints number of characters indicated by & lt; len & gt; .
void rprintfStrLenLCD(char str[], unsigned int start, unsigned int len);

//! prints a string stored in program rom
/// \note This function does not actually store your string in
/// program rom, but merely reads it assuming you stored it properly.
void rprintfProgStrLCD(const prog_char str[]);

//! Using the function rprintfProgStrM(...) automatically causes
/// your string to be stored in ROM, thereby not wasting precious RAM.
/// Example usage:
/// \code
/// rprintfProgStrM( " Hello, this string is stored in program rom " );
/// \endcode
#define rprintfProgStrMLCD(string) (rprintfProgStrLCD(PSTR(string)))

//! Prints a carriage-return and line-feed.
/// Useful when printing to serial ports/terminals.
void rprintfCRLFLCD(void);

// Prints the number contained in " data " in hex format
// u04,u08,u16,and u32 functions handle 4,8,16,or 32 bits respectively
void rprintfu04LCD(unsigned char data); /// & lt; Print 4-bit hex number. Outputs a single hex character.
void rprintfu08LCD(unsigned char data); /// & lt; Print 8-bit hex number. Outputs two hex characters.
void rprintfu16LCD(unsigned short data); /// & lt; Print 16-bit hex number. Outputs four hex characters.
void rprintfu32LCD(unsigned long data); /// & lt; Print 32-bit hex number. Outputs eight hex characters.

//! A flexible integer-number printing routine.
/// Print the number " n " in the given " base " , using exactly " numDigits " .
/// Print +/- if signed flag " isSigned " is TRUE.
/// The character specified in " padchar " will be used to pad extra characters.
///
/// Examples:
/// \code
/// uartPrintfNum(10, 6, TRUE, ' ', 1234); -- & gt; " +1234 "
/// uartPrintfNum(10, 6, FALSE, '0', 1234); -- & gt; " 001234 "
/// uartPrintfNum(16, 6, FALSE, '.', 0x5AA5); -- & gt; " ..5AA5 "
/// \endcode
void rprintfNumLCD(char base, char numDigits, char isSigned, char padchar, long n);

#ifdef RPRINTFLCD_FLOAT
//! floating-point print routine
void rprintfFloatLCD(char numDigits, double x);
#endif

// NOTE: Below you'll see the function prototypes of rprintf1RamRom and
// rprintf2RamRom. rprintf1RamRom and rprintf2RamRom are both reduced versions
// of the regular C printf() command. However, they are modified to be able
// to read their text/format strings from RAM or ROM in the Atmel microprocessors.
// Unless you really intend to, do not use the " RamRom " versions of the functions
// directly. Instead use the #defined function versions:
//
// printfx( " text/format " ,args) ...to keep your text/format string stored in RAM
// - or -
// printfxROM( " text/format " ,args) ...to keep your text/format string stored in ROM
//
// where x is either 1 or 2 for the simple or more powerful version of printf()
//
// Since there is much more ROM than RAM available in the Atmel microprocessors,
// and nearly all text/format strings are constant (never change in the course
// of the program), you should try to use the ROM printf version exclusively.
// This will ensure you leave as much RAM as possible for program variables and
// data.

//! \fn int rprintf(const char *format, ...);
/// A reduced substitute for the usual C printf() function.
/// This function actually points to either rprintf1RamRom or rprintf2RamRom
/// depending on the user's selection. Rprintf1 is a simple small fast print
/// routine while rprintf2 is larger and slower but more capable. To choose
/// the routine you would like to use, define either RPRINTF_SIMPLE or
/// RPRINTF_COMPLEX in global.h.

#ifdef RPRINTFLCD_SIMPLE
//! A simple printf routine.
/// Called by rprintf() - does a simple printf (supports %d, %x, %c).
/// Supports:
/// - %d - decimal
/// - %x - hex
/// - %c - character
int rprintf1RamRomLCD(unsigned char stringInRom, const char *format, ...);
// #defines for RAM or ROM operation
#define rprintf1LCD(format, args...) rprintf1RamRomLCD(STRING_IN_ROM, PSTR(format), ## args)
#define rprintf1RAMLCD(format, args...) rprintf1RamRomLCD(STRING_IN_RAM, format, ## args)

// *** Default rprintf(...) ***
// this next line determines what the the basic rprintf() defaults to:
#define rprintfLCD(format, args...) rprintf1RamRomLCD(STRING_IN_ROM, PSTR(format), ## args)
#endif

#ifdef RPRINTFLCD_COMPLEX
//! A more powerful printf routine.
/// Called by rprintf() - does a more powerful printf (supports %d, %u, %o, %x, %c, %s).
/// Supports:
/// - %d - decimal
/// - %u - unsigned decimal
/// - %o - octal
/// - %x - hex
/// - %c - character
/// - %s - strings
/// - and the width,precision,padding modifiers
/// \note This printf does not support floating point numbers.
int rprintf2RamRomLCD(unsigned char stringInRom, const char *sfmt, ...);
// #defines for RAM or ROM operation
#define rprintf2LCD(format, args...) rprintf2RamRomLCD(STRING_IN_ROM, format, ## args)
#define rprintf2RAMLCD(format, args...) rprintf2RamRomLCD(STRING_IN_RAM, format, ## args)

// *** Default rprintfLCD(...) ***
// this next line determines what the the basic rprintfLCD() defaults to:
#define rprintfLCD(format, args...) rprintf2RamRomLCD(STRING_IN_ROM, PSTR(format), ## args)
#endif

#endif
//@}

Kod___r__d__owy.rar > lcd_lib.h

//*****************************************************************************
//
// File Name : 'lcd_lib.h'
// Title : 4 bit LCd interface header file
// Author : Scienceprog.com - Copyright (C) 2007
// Created : 2007-06-18
// Revised : 2007-06-18
// Version : 1.0
// Target MCU : Atmel AVR series
//
// This code is distributed under the GNU Public License
// which can be found at http://www.gnu.org/licenses/gpl.txt
//
//*****************************************************************************
#ifndef LCD_LIB
#define LCD_LIB

#include & lt; inttypes.h & gt;

#define LCD_RS 2 //define MCU pin connected to LCD RS
#define LCD_RW 1 //define MCU pin connected to LCD R/W
#define LCD_E 3 //define MCU pin connected to LCD E
#define LCD_D4 4 //define MCU pin connected to LCD D3
#define LCD_D5 5 //define MCU pin connected to LCD D4
#define LCD_D6 6 //define MCU pin connected to LCD D5
#define LCD_D7 7 //define MCU pin connected to LCD D6
#define LDP PORTA //define MCU port connected to LCD data pins
#define LCP PORTA //define MCU port connected to LCD control pins
#define LDDR DDRA //define MCU direction register for port connected to LCD data pins
#define LCDR DDRA //define MCU direction register for port connected to LCD control pins

#define LCD_CLR 0 //DB0: clear display
#define LCD_HOME 1 //DB1: return to home position
#define LCD_ENTRY_MODE 2 //DB2: set entry mode
#define LCD_ENTRY_INC 1 //DB1: increment
#define LCD_ENTRY_SHIFT 0 //DB2: shift
#define LCD_ON_CTRL 3 //DB3: turn lcd/cursor on
#define LCD_ON_DISPLAY 2 //DB2: turn display on
#define LCD_ON_CURSOR 1 //DB1: turn cursor on
#define LCD_ON_BLINK 0 //DB0: blinking cursor
#define LCD_MOVE 4 //DB4: move cursor/display
#define LCD_MOVE_DISP 3 //DB3: move display (0- & gt; move cursor)
#define LCD_MOVE_RIGHT 2 //DB2: move right (0- & gt; left)
#define LCD_FUNCTION 5 //DB5: function set
#define LCD_FUNCTION_8BIT 4 //DB4: set 8BIT mode (0- & gt; 4BIT mode)
#define LCD_FUNCTION_2LINES 3 //DB3: two lines (0- & gt; one line)
#define LCD_FUNCTION_10DOTS 2 //DB2: 5x10 font (0- & gt; 5x7 font)
#define LCD_CGRAM 6 //DB6: set CG RAM address
#define LCD_DDRAM 7 //DB7: set DD RAM address
// reading:
#define LCD_BUSY 7 //DB7: LCD is busy
#define LCD_LINES 2 //visible lines
#define LCD_LINE_LENGTH 16 //line length (in characters)
// cursor position to DDRAM mapping
#define LCD_LINE0_DDRAMADDR 0x00
#define LCD_LINE1_DDRAMADDR 0x40
#define LCD_LINE2_DDRAMADDR 0x14
#define LCD_LINE3_DDRAMADDR 0x54

void LCDsendChar(uint8_t); //forms data ready to send to 74HC164
void LCDsendCommand(uint8_t); //forms data ready to send to 74HC164
void LCDinit(void); //Initializes LCD
void LCDclr(void); //Clears LCD
void LCDhome(void); //LCD cursor home
void LCDstring(uint8_t*, uint8_t); //Outputs string to LCD
void LCDGotoXY(uint8_t, uint8_t); //Cursor to X Y position
void CopyStringtoLCD(const uint8_t*, uint8_t, uint8_t);//copies flash string to LCD at x,y
void LCDdefinechar(const uint8_t *,uint8_t);//write char to LCD CGRAM
void LCDshiftRight(uint8_t); //shift by n characters Right
void LCDshiftLeft(uint8_t); //shift by n characters Left
void LCDcursorOn(void); //Underline cursor ON
void LCDcursorOnBlink(void); //Underline blinking cursor ON
void LCDcursorOFF(void); //Cursor OFF
void LCDblank(void); //LCD blank but not cleared
void LCDvisible(void); //LCD visible
void LCDcursorLeft(uint8_t); //Shift cursor left by n
void LCDcursorRight(uint8_t); //shif cursor right by n

#endif

Kod___r__d__owy.rar > rprintfLCD.c

/*! \file rprintfLCD.c \brief printf routine and associated routines. */
//*****************************************************************************
//
// File Name : 'rprintfLCD.c'
// Title : printf routine and associated routines
// Author : Pascal Stang - Copyright (C) 2000-2002
// Created : 2000.12.26
// Revised : 2003.5.1
// Version : 1.0
// Target MCU : Atmel AVR series and other targets
// Editor Tabs : 4
//
// NOTE: This code is currently below version 1.0, and therefore is considered
// to be lacking in some functionality or documentation, or may not be fully
// tested. Nonetheless, you can expect most functions to work.
//
// This code is distributed under the GNU Public License
// which can be found at http://www.gnu.org/licenses/gpl.txt
//
//*****************************************************************************

#include & lt; avr/pgmspace.h & gt;
//#include & lt; string-avr.h & gt;
//#include & lt; stdlib.h & gt;
#include & lt; stdarg.h & gt;
#include " conf/global.h "
#include " rprintfLCD.h "

#ifndef TRUE
#define TRUE -1
#define FALSE 0
#endif

#define INF 32766 // maximum field size to print
#define READMEMBYTE(a,char_ptr) ((a)?(pgm_read_byte(char_ptr)):(*char_ptr))

#ifdef RPRINTFLCD_COMPLEX
static unsigned char buf[128];
#endif

// use this to store hex conversion in RAM
//static char HexChars[] = " 0123456789ABCDEF " ;
// use this to store hex conversion in program memory
//static prog_char HexChars[] = " 0123456789ABCDEF " ;
static char __attribute__ ((progmem)) HexChars[] = " 0123456789ABCDEF " ;

#define hexchar(x) pgm_read_byte( HexChars+((x) & 0x0f) )
//#define hexchar(x) ((((x) & 0x0F) & gt; 9)?((x)+'A'-10):((x)+'0'))

// function pointer to single character output routine
static void (*rputchar)(unsigned char c);

// *** rprintfLCD initialization ***
// you must call this function once and supply the character output
// routine before using other functions in this library
void rprintfInitLCD(void (*putchar_func)(unsigned char c))
{
rputchar = putchar_func;
}

// *** rprintfCharLCD ***
// send a character/byte to the current output device
void rprintfCharLCD(unsigned char c)
{
// do LF - & gt; CR/LF translation
if(c == '\n')
rputchar('\r');
// send character
rputchar(c);
}

// *** rprintfStrLCD ***
// prints a null-terminated string stored in RAM
void rprintfStrLCD(char str[])
{
// send a string stored in RAM
// check to make sure we have a good pointer
if (!str) return;

// print the string until a null-terminator
while (*str)
rprintfCharLCD(*str++);
}

// *** rprintfStrLenLCD ***
// prints a section of a string stored in RAM
// begins printing at position indicated by & lt; start & gt;
// prints number of characters indicated by & lt; len & gt;
void rprintfStrLenLCD(char str[], unsigned int start, unsigned int len)
{
register int i=0;

// check to make sure we have a good pointer
if (!str) return;
// spin through characters up to requested start
// keep going as long as there's no null
while((i++ & lt; start) & & (*str++));
// for(i=0; i & lt; start; i++)
// {
// // keep steping through string as long as there's no null
// if(*str) str++;
// }

// then print exactly len characters
for(i=0; i & lt; len; i++)
{
// print data out of the string as long as we haven't reached a null yet
// at the null, start printing spaces
if(*str)
rprintfCharLCD(*str++);
else
rprintfCharLCD(' ');
}

}

// *** rprintfProgStrLCD ***
// prints a null-terminated string stored in program ROM
void rprintfProgStrLCD(const prog_char str[])
{
// print a string stored in program memory
register char c;

// check to make sure we have a good pointer
if (!str) return;

// print the string until the null-terminator
while((c = pgm_read_byte(str++)))
rprintfCharLCD(c);
}

// *** rprintfCRLFLCD ***
// prints carriage return and line feed
void rprintfCRLFLCD(void)
{
// print CR/LF
//rprintfCharLCD('\r');
// LF - & gt; CR/LF translation built-in to rprintfCharLCD()
rprintfCharLCD('\n');
}

// *** rprintfu04LCD ***
// prints an unsigned 4-bit number in hex (1 digit)
void rprintfu04LCD(unsigned char data)
{
// print 4-bit hex value
// char Character = data & 0x0f;
// if (Character & gt; 9)
// Character+='A'-10;
// else
// Character+='0';
rprintfCharLCD(hexchar(data));
}

// *** rprintfu08LCD ***
// prints an unsigned 8-bit number in hex (2 digits)
void rprintfu08LCD(unsigned char data)
{
// print 8-bit hex value
rprintfu04LCD(data & gt; & gt; 4);
rprintfu04LCD(data);
}

// *** rprintfu16LCD ***
// prints an unsigned 16-bit number in hex (4 digits)
void rprintfu16LCD(unsigned short data)
{
// print 16-bit hex value
rprintfu08LCD(data & gt; & gt; 8);
rprintfu08LCD(data);
}

// *** rprintfu32LCD ***
// prints an unsigned 32-bit number in hex (8 digits)
void rprintfu32LCD(unsigned long data)
{
// print 32-bit hex value
rprintfu16LCD(data & gt; & gt; 16);
rprintfu16LCD(data);
}

// *** rprintfNumLCD ***
// special printf for numbers only
// see formatting information below
// Print the number " n " in the given " base "
// using exactly " numDigits "
// print +/- if signed flag " isSigned " is TRUE
// use the character specified in " padchar " to pad extra characters
//
// Examples:
// uartPrintfNum(10, 6, TRUE, ' ', 1234); -- & gt; " +1234 "
// uartPrintfNum(10, 6, FALSE, '0', 1234); -- & gt; " 001234 "
// uartPrintfNum(16, 6, FALSE, '.', 0x5AA5); -- & gt; " ..5AA5 "
void rprintfNumLCD(char base, char numDigits, char isSigned, char padchar, long n)
{
// define a global HexChars or use line below
//static char HexChars[16] = " 0123456789ABCDEF " ;
char *p, buf[32];
unsigned long x;
unsigned char count;

// prepare negative number
if( isSigned & & (n & lt; 0) )
{
x = -n;
}
else
{
x = n;
}

// setup little string buffer
count = (numDigits-1)-(isSigned?1:0);
p = buf + sizeof (buf);
*--p = '\0';

// force calculation of first digit
// (to prevent zero from not printing at all!!!)
*--p = hexchar(x%base); x /= base;
// calculate remaining digits
while(count--)
{
if(x != 0)
{
// calculate next digit
*--p = hexchar(x%base); x /= base;
}
else
{
// no more digits left, pad out to desired length
*--p = padchar;
}
}

// apply signed notation if requested
if( isSigned )
{
if(n & lt; 0)
{
*--p = '-';
}
else if(n & gt; 0)
{
*--p = '+';
}
else
{
*--p = ' ';
}
}

// print the string right-justified
count = numDigits;
while(count--)
{
rprintfCharLCD(*p++);
}
}

//#ifdef RPRINTF_FLOAT
// *** rprintfFloatLCD ***
// floating-point print
void rprintfFloatLCD(char numDigits, double x)
{
unsigned char firstplace = FALSE;
unsigned char negative;
unsigned char i, digit;
double place = 1.0;

// save sign
negative = (x & lt; 0);
// convert to absolute value
x = (x & gt; 0)?(x):(-x);

// find starting digit place
for(i=0; i & lt; 15; i++)
{
if((x/place) & lt; 10.0)
break;
else
place *= 10.0;
}
// print polarity character
if(negative)
rprintfCharLCD('-');
else
rprintfCharLCD('+');

// print digits
for(i=0; i & lt; numDigits; i++)
{
digit = (x/place);

if(digit | firstplace | (place == 1.0))
{
firstplace = TRUE;
rprintfCharLCD(digit+0x30);
}
else
rprintfCharLCD(' ');

if(place == 1.0)
{
rprintfCharLCD('.');
}

x -= (digit*place);
place /= 10.0;
}
}
//#endif

#ifdef RPRINTF_SIMPLELCD
// *** rprintf1RamRomLCD ***
// called by rprintf() - does a simple printf (supports %d, %x, %c)
// Supports:
// %d - decimal
// %x - hex
// %c - character
int rprintf1RamRomLCD(unsigned char stringInRom, const char *format, ...)
{
// simple printf routine
// define a global HexChars or use line below
//static char HexChars[16] = " 0123456789ABCDEF " ;
char format_flag;
unsigned int u_val, div_val, base;
va_list ap;

va_start(ap, format);
for (;;)
{
while ((format_flag = READMEMBYTE(stringInRom,format++) ) != '%')
{ // Until '%' or '\0'
if (!format_flag)
{
va_end(ap);
return(0);
}
rprintfCharLCD(format_flag);
}

switch (format_flag = READMEMBYTE(stringInRom,format++) )
{
case 'c': format_flag = va_arg(ap,int);
default: rprintfCharLCD(format_flag); continue;
case 'd': base = 10; div_val = 10000; goto CONVERSION_LOOP;
// case 'x': base = 16; div_val = 0x10;
case 'x': base = 16; div_val = 0x1000;

CONVERSION_LOOP:
u_val = va_arg(ap,int);
if (format_flag == 'd')
{
if (((int)u_val) & lt; 0)
{
u_val = - u_val;
rprintfCharLCD('-');
}
while (div_val & gt; 1 & & div_val & gt; u_val) div_val /= 10;
}
do
{
//rprintfCharLCD(pgm_read_byte(HexChars+(u_val/div_val)));
rprintfu04LCD(u_val/div_val);
u_val %= div_val;
div_val /= base;
} while (div_val);
}
}
va_end(ap);
}
#endif


#ifdef RPRINTFLCD_COMPLEX
// *** rprintf2RamRomLCD ***
// called by rprintf() - does a more powerful printf (supports %d, %u, %o, %x, %c, %s)
// Supports:
// %d - decimal
// %u - unsigned decimal
// %o - octal
// %x - hex
// %c - character
// %s - strings
// and the width,precision,padding modifiers
// **this printf does not supporting point numbers
int rprintf2RamRomLCD(unsigned char stringInRom, const char *sfmt, ...)
{
register unsigned char *f, *bp;
register long l;
register unsigned long u;
register int i;
register int fmt;
register unsigned char pad = ' ';
int flush_left = 0, f_width = 0, prec = INF, hash = 0, do_long = 0;
int sign = 0;

va_list ap;
va_start(ap, sfmt);

f = (unsigned char *) sfmt;

for (; READMEMBYTE(stringInRom,f); f++)
{
if (READMEMBYTE(stringInRom,f) != '%')
{ // not a format character
// then just output the char
rprintfCharLCD(READMEMBYTE(stringInRom,f));
}
else
{
f++; // if we have a " % " then skip it
if (READMEMBYTE(stringInRom,f) == '-')
{
flush_left = 1; // minus: flush left
f++;
}
if (READMEMBYTE(stringInRom,f) == '0'
|| READMEMBYTE(stringInRom,f) == '.')
{
// padding with 0 rather than blank
pad = '0';
f++;
}
if (READMEMBYTE(stringInRom,f) == '*')
{ // field width
f_width = va_arg(ap, int);
f++;
}
else if (Isdigit(READMEMBYTE(stringInRom,f)))
{
f_width = atoiRamRom(stringInRom, (char *) f);
while (Isdigit(READMEMBYTE(stringInRom,f)))
f++; // skip the digits
}
if (READMEMBYTE(stringInRom,f) == '.')
{ // precision
f++;
if (READMEMBYTE(stringInRom,f) == '*')
{
prec = va_arg(ap, int);
f++;
}
else if (Isdigit(READMEMBYTE(stringInRom,f)))
{
prec = atoiRamRom(stringInRom, (char *) f);
while (Isdigit(READMEMBYTE(stringInRom,f)))
f++; // skip the digits
}
}
if (READMEMBYTE(stringInRom,f) == '#')
{ // alternate form
hash = 1;
f++;
}
if (READMEMBYTE(stringInRom,f) == 'l')
{ // long format
do_long = 1;
f++;
}

fmt = READMEMBYTE(stringInRom,f);
bp = buf;
switch (fmt) { // do the formatting
case 'd': // 'd' signed decimal
if (do_long)
l = va_arg(ap, long);
else
l = (long) (va_arg(ap, int));
if (l & lt; 0)
{
sign = 1;
l = -l;
}
do {
*bp++ = l % 10 + '0';
} while ((l /= 10) & gt; 0);
if (sign)
*bp++ = '-';
f_width = f_width - (bp - buf);
if (!flush_left)
while (f_width-- & gt; 0)
rprintfCharLCD(pad);
for (bp--; bp & gt; = buf; bp--)
rprintfCharLCD(*bp);
if (flush_left)
while (f_width-- & gt; 0)
rprintfCharLCD(' ');
break;
case 'o': // 'o' octal number
case 'x': // 'x' hex number
case 'u': // 'u' unsigned decimal
if (do_long)
u = va_arg(ap, unsigned long);
else
u = (unsigned long) (va_arg(ap, unsigned));
if (fmt == 'u')
{ // unsigned decimal
do {
*bp++ = u % 10 + '0';
} while ((u /= 10) & gt; 0);
}
else if (fmt == 'o')
{ // octal
do {
*bp++ = u % 8 + '0';
} while ((u /= 8) & gt; 0);
if (hash)
*bp++ = '0';
}
else if (fmt == 'x')
{ // hex
do {
i = u % 16;
if (i & lt; 10)
*bp++ = i + '0';
else
*bp++ = i - 10 + 'a';
} while ((u /= 16) & gt; 0);
if (hash)
{
*bp++ = 'x';
*bp++ = '0';
}
}
i = f_width - (bp - buf);
if (!flush_left)
while (i-- & gt; 0)
rprintfCharLCD(pad);
for (bp--; bp & gt; = buf; bp--)
rprintfCharLCD((int) (*bp));
if (flush_left)
while (i-- & gt; 0)
rprintfCharLCD(' ');
break;
case 'c': // 'c' character
i = va_arg(ap, int);
rprintfCharLCD((int) (i));
break;
case 's': // 's' string
bp = va_arg(ap, unsigned char *);
if (!bp)
bp = (unsigned char *) " (nil) " ;
f_width = f_width - strlen((char *) bp);
if (!flush_left)
while (f_width-- & gt; 0)
rprintfCharLCD(pad);
for (i = 0; *bp & & i & lt; prec; i++)
{
rprintfCharLCD(*bp);
bp++;
}
if (flush_left)
while (f_width-- & gt; 0)
rprintfCharLCD(' ');
break;
case '%': // '%' character
rprintfCharLCD('%');
break;
}
flush_left = 0, f_width = 0, prec = INF, hash = 0, do_long = 0;
sign = 0;
pad = ' ';
}
}

va_end(ap);
return 0;
}

unsigned char Isdigit(char c)
{
if((c & gt; = 0x30) & & (c & lt; = 0x39))
return TRUE;
else
return FALSE;
}

int atoiRamRom(unsigned char stringInRom, char *str)
{
int num = 0;;

while(Isdigit(READMEMBYTE(stringInRom,str)))
{
num *= 10;
num += ((READMEMBYTE(stringInRom,str++)) - 0x30);
}
return num;
}

#endif

//******************************************************************************
// code below this line is commented out and can be ignored
//******************************************************************************
/*
char* sprintf(const char *sfmt, ...)
{
register unsigned char *f, *bp, *str;
register long l;
register unsigned long u;
register int i;
register int fmt;
register unsigned char pad = ' ';
int flush_left = 0, f_width = 0, prec = INF, hash = 0, do_long = 0;
int sign = 0;

va_list ap;
va_start(ap, sfmt);

str = bufstring;
f = (unsigned char *) sfmt;

for (; *f; f++)
{
if (*f != '%')
{ // not a format character
*str++ = (*f); // then just output the char
}
else
{
f++; // if we have a " % " then skip it
if (*f == '-')
{
flush_left = 1; // minus: flush left
f++;
}
if (*f == '0' || *f == '.')
{
// padding with 0 rather than blank
pad = '0';
f++;
}
if (*f == '*')
{ // field width
f_width = va_arg(ap, int);
f++;
}
else if (Isdigit(*f))
{
f_width = atoi((char *) f);
while (Isdigit(*f))
f++; // skip the digits
}
if (*f == '.')
{ // precision
f++;
if (*f == '*')
{
prec = va_arg(ap, int);
f++;
}
else if (Isdigit(*f))
{
prec = atoi((char *) f);
while (Isdigit(*f))
f++; // skip the digits
}
}
if (*f == '#')
{ // alternate form
hash = 1;
f++;
}
if (*f == 'l')
{ // long format
do_long = 1;
f++;
}

fmt = *f;
bp = buf;
switch (fmt) { // do the formatting
case 'd': // 'd' signed decimal
if (do_long)
l = va_arg(ap, long);
else
l = (long) (va_arg(ap, int));
if (l & lt; 0)
{
sign = 1;
l = -l;
}
do {
*bp++ = l % 10 + '0';
} while ((l /= 10) & gt; 0);
if (sign)
*bp++ = '-';
f_width = f_width - (bp - buf);
if (!flush_left)
while (f_width-- & gt; 0)
*str++ = (pad);
for (bp--; bp & gt; = buf; bp--)
*str++ = (*bp);
if (flush_left)
while (f_width-- & gt; 0)
*str++ = (' ');
break;
case 'o': // 'o' octal number
case 'x': // 'x' hex number
case 'u': // 'u' unsigned decimal
if (do_long)
u = va_arg(ap, unsigned long);
else
u = (unsigned long) (va_arg(ap, unsigned));
if (fmt == 'u')
{ // unsigned decimal
do {
*bp++ = u % 10 + '0';
} while ((u /= 10) & gt; 0);
}
else if (fmt == 'o')
{ // octal
do {
*bp++ = u % 8 + '0';
} while ((u /= 8) & gt; 0);
if (hash)
*bp++ = '0';
}
else if (fmt == 'x')
{ // hex
do {
i = u % 16;
if (i & lt; 10)
*bp++ = i + '0';
else
*bp++ = i - 10 + 'a';
} while ((u /= 16) & gt; 0);
if (hash)
{
*bp++ = 'x';
*bp++ = '0';
}
}
i = f_width - (bp - buf);
if (!flush_left)
while (i-- & gt; 0)
*str++ = (pad);
for (bp--; bp & gt; = buf; bp--)
*str++ = ((int) (*bp));
if (flush_left)
while (i-- & gt; 0)
*str++ = (' ');
break;
case 'c': // 'c' character
i = va_arg(ap, int);
*str++ = ((int) (i));
break;
case 's': // 's' string
bp = va_arg(ap, unsigned char *);
if (!bp)
bp = (unsigned char *) " (nil) " ;
f_width = f_width - strlen((char *) bp);
if (!flush_left)
while (f_width-- & gt; 0)
*str++ = (pad);
for (i = 0; *bp & & i & lt; prec; i++)
{
*str++ = (*bp);
bp++;
}
if (flush_left)
while (f_width-- & gt; 0)
*str++ = (' ');
break;
case '%': // '%' character
*str++ = ('%');
break;
}
flush_left = 0, f_width = 0, prec = INF, hash = 0, do_long = 0;
sign = 0;
pad = ' ';
}
}

va_end(ap);
// terminate string with null
*str++ = '\0';
return bufstring;
}

*/