Serial Servo Controller Part-2

Hello ,
        I've blah blah'd a lot in my previous blog regarding servo controller more than 2 year ago .
http://miniprojects-be-btech.blogspot.in/2011/08/serial-servo-controller.html

Now I would like to share the code , incidentally its my Btech final year project .
schedular based serial servo controller based on AVR & GUI has been developed using VB ( will share the GUI details in next blog ) ,  GUI can read command from a file something like a sequencer .

Code goes like this .

#include
#include
#include
#define F_CPU 16000000UL
#include
#include


#define SETBIT(ADDRESS, BIT) (ADDRESS |= (1 << BIT))
#define CLEARBIT(ADDRESS, BIT) (ADDRESS &= ~(1 << BIT))


/// Data Types /////////
typedef unsigned char u8;
typedef unsigned int u16;
u8 prev;
u8 position[8],count=0;

typedef struct task
{
   // pointer to a function
   void (*pfunc) (void);
   // delay before the first call
   u16 delay;
   // interval between subsequent runs
   u16 period;
   // flag indicating time to run
   u8 run;
}task;

/// Defines ///////////
// 25msec period 256-180
// 7.3728MHz and /1024 prescaler
#define StartFrom       200
// maximum number of tasks
#define MAXnTASKS       3

/// Globals ///////////
volatile task TaskArray[MAXnTASKS];
void InitScheduler (void);
void DeleteTask (u8 index);
void AddTask (void (*taskfunc)(void), u16 taskdelay, u16 taskperiod);
void DispatchTask (void);
void PWM (void);
void get1 (void);



void USARTWriteChar(unsigned char data)
{
//Wait untill the transmitter is ready
while(!(UCSRA & (1< {
//Do nothing
}

//Now write the data to USART buffer

UDR=data;
}
char USARTReadChar(void)
{


while ( !(UCSRA & (1<{

}

return UDR;

}

void USARTInit(uint16_t ubrr_value)
{      
   
   //Set Baud rate
   UBRRL = ubrr_value;
   UBRRH = (ubrr_value>>8);
   DDRD=1<   DDRA=255;
 
   /*Set Frame Format


   >> Asynchronous mode
   >> No Parity
   >> 1 StopBit
   >> char size 8

   */

   UCSRC=(1<

   //Enable The receiver and transmitter
   UCSRB=(1<

}




int main()
{


 InitScheduler();
    DDRA=0xff;
DDRB=0;
PORTB=255;
USARTInit(103);
_delay_ms(10);
   // populate task array  
   position[7]=1;  
    position[6]=1;
position[5]=1;
 position[4]=1;
  position[3]=1;
   position[2]=1;
position[1]=1;
 position[0]=1;  
   AddTask (PWM, 0, 8);
   AddTask (get1, 1, 8);
   // enable interrupts
  // sei();
    cli();  
   while (1)
   {
      get1 ();
   }          


}
void InitScheduler (void)
{
   u8 i;
 
   // timer prescaler clock/1024
   TCCR0 |= (1<   // clear pending interrupts
   TIFR = 1<   // enable timer0 overflow interrupt
   TIMSK |= 1<    // load timer0
   TCNT0 = StartFrom;

   // clear task array
   for (i=0; i}

void DeleteTask (u8 j)
{
   TaskArray[j].pfunc = 0x0000;
   TaskArray[j].delay = 0;
   TaskArray[j].period = 0;
   TaskArray[j].run = 0;
}

void AddTask (void (*taskfunc)(void), u16 taskdelay, u16 taskperiod)
{
   u8 n=0;

   // find next available position
   while ((TaskArray[n].pfunc != 0) && (n < MAXnTASKS)) n++;

   // place task
   if (n < MAXnTASKS)
   {
      TaskArray[n].pfunc = taskfunc;
      TaskArray[n].delay = taskdelay;
      TaskArray[n].period = taskperiod;
      TaskArray[n].run = 0;  
   }
}

SIGNAL(SIG_OVERFLOW0)
{
   u8 m;
   // testing
 
    // load timer
   TCNT0 = StartFrom;
 
   for (m=0; m   {
      if (TaskArray[m].pfunc)
      {  
         if (TaskArray[m].delay == 0)
         {
            TaskArray[m].run = 1;
            TaskArray[m].delay = TaskArray[m].period;
         }
         else TaskArray[m].delay--;
      }
   }



}

void DispatchTask (void)
{
   u8 k;
 
   for (k=0; k   {
      if (TaskArray[k].run == 1)
      {
         // run task
         (*TaskArray[k].pfunc)();
         // clear run flag
         TaskArray[k].run = 0;
      }
   }
}


void PWM (void)
{
cli();
   int tnow;
      if(1)
   {
  SETBIT(PORTA, 0);
   }
   if(1)
   {
   SETBIT(PORTA, 1);
   }
   if(1)
   {
   SETBIT(PORTA, 2);
   }
   if(1)
   {
   SETBIT(PORTA, 3);
   }
   if(1)
   {
   SETBIT(PORTA, 4);
   }
   if(1)
   {
    SETBIT(PORTA, 5);
   }
   if(1)
   {
    SETBIT(PORTA, 6);
   }
      if(1)
   {
   SETBIT(PORTA, 7);
   }

 _delay_us(597);

for(tnow=0;tnow<128 p="" tnow="">{
if(position[7]==tnow)
{
CLEARBIT(PORTA, 0);
}
if(position[6]==tnow)
{
CLEARBIT(PORTA, 1);
}
if(position[5]==tnow)
{
CLEARBIT(PORTA, 2);
}
if(position[4]==tnow)
{
CLEARBIT(PORTA, 3);
}
if(position[3]==tnow)
{
CLEARBIT(PORTA, 4);
}
if(position[2]==tnow)
{
CLEARBIT(PORTA, 5);
}
if(position[1]==tnow)
{
CLEARBIT(PORTA, 6);
}
if(position[0]==tnow)
{
CLEARBIT(PORTA, 7);
}
_delay_us(5);
}// end of tnow
 
sei();
}


void get1 (void)
{
int i;
unsigned int send=position[0];
send=send+10;
USARTWriteChar(send);
//USARTWriteChar('\');
//USARTWriteChar('n');
for(i=0;i<1 i="" p="">position[i]=USARTReadChar();

}