//include files
#include <Mega32.h>
#include <delay.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

//servo 0 PD5 (turntable)
//servo 1 PB3 (shoulder)
//servo 2 PD7 (elbow)
//servo 3 PD4 (wrist)
//servo 4 PD3 (gripper)

//integer definitions for X and O
#define xmove 1
#define omove -1
#define nmove 0

//2D 3x3 integer array representing tic-tac-toe board
int board[3][3] = {
// row1
nmove,
nmove,
nmove,
// row2
nmove,
nmove,
nmove,
//row3
nmove,
nmove,
nmove};

//variables for receiving input from serial
unsigned char r_char; //received character
unsigned char r_buffer[16]; //receive buffer
unsigned char r_index, r_ready; //receive flags

//function declarations
void move(int,int,int);
void printboard(void);
int checkblock(void);
int checkwin(void);
int trytowin(void);
void moveServo(int, int);
void initialize(void);
void delay_usv(unsigned int);
void pickup(int,int);
void neutral(void);
void drop(int, int);

//integer flags
int gameOver = 0; //signals end of game
int lockkeypad = 0; //turns over control from user to robot and vice versa

int i; //used by program's internal delay function (delay_usv)
int turn = 1; //variable to hold number of turns played in the current tic-tac-toe game


//integer values for servo PWM control
unsigned int servo0, servo1, servo2, servo3, servo4;

/**************************************************************************
USART receive interrupt, activates when a character is received from serial
**/
interrupt [USART_RXC] void uart_rec(void)
{
    //PORTC = PORTC ^ 0xff;
    r_char=UDR;    //get a char
    UDR=r_char;    //then print it
    //build the input string
    if (r_char != '\r') r_buffer[r_index++]=r_char;
    else
    {
        putchar('\n');              //use putchar to avoid overwrite
        r_buffer[r_index]=0x00;                //zero terminate
        r_ready=1;                //signal cmd processor
    }
}

/***************************************
Function to print current board position
**/
void printboard(void){
  int i, j;
  //iterate through the 2-D board array, and print x's, o's and dots according to the integer stored there
  for(i = 0; i < 3; i++){
    for(j = 0; j < 3; j++){
      if(board[i][j] == nmove) printf(". ");
      else if(board[i][j] == xmove) printf("x ");
      else if(board[i][j] == omove) printf("o ");
    }
    printf("\r\n");
  }
}

/**************************************************************************
Function to move a given piece onto a given square on the tic-tac-toe board
@param id The type of piece, whether x or o
@param rpos The row number of the square
@param cpos The column number of the square
**/
void move(int id, int rpos, int cpos){
     
        //if an o is to be moved
        if(id == omove){
            board[rpos][cpos] = omove; //set the corresponding array position to -1
            printboard(); //print the current board layout
            pickup(0,turn); //pickup the corresponding 'o' piece depending on the turn number
            neutral(); //move back to a neutral position
            drop(rpos,cpos); //place the piece on the given square
            neutral(); //move back to a neutral position
            moveServo(4,180); //open the gripper wide for the next pickup
            delay_ms(30);
            printf("\r\n");
        }
        //if an x is to moved
        else if(id == xmove){
            board[rpos][cpos] = xmove; //set the corresponding array position to -1
            printboard(); //print the current board layout
            pickup(1,6-turn); //pickup the corresponding 'x' piece depending on the turn number
            neutral(); //move back to a neutral position
            drop(rpos,cpos); //place the piece on the given square
            neutral(); //move back to a neutral position
            moveServo(4,180); //open the gripper wide for the next pickup
            delay_ms(30);
            printf("\r\n");
        }

}

/*****************************************
Function to check for victory
@return 1 if a winning result has occurred
**/
int checkwin(void){
   //sums along rows, columns, and diagonals of the board array
   int sumrow1, sumrow2, sumrow3, sumcol1, sumcol2, sumcol3, sumdiag1, sumdiag2;
   sumrow1 = board[0][0]+board[0][1]+board[0][2];
   sumrow2 = board[1][0]+board[1][1]+board[1][2];
   sumrow3 = board[2][0]+board[2][1]+board[2][2];
   sumcol1 = board[0][0]+board[1][0]+board[2][0];
   sumcol2 = board[0][1]+board[1][1]+board[2][1];
   sumcol3 = board[0][2]+board[1][2]+board[2][2];
   sumdiag1 = board[0][0]+board[1][1]+board[2][2];
   sumdiag2 = board[0][2]+board[1][1]+board[2][0];
   
   //if o wins, print the appropriate message and do a victory wave by moving servo 2 which controls the elbow
   if(sumrow1 == -3 | sumrow2 == -3 | sumrow3 == -3 | sumcol1 == -3 | sumcol2 == -3 | sumcol3 == -3 | sumdiag1 == -3
   | sumdiag2 == -3){
    printf("I won!!\r\n");
    moveServo(2,90);
    delay_ms(500);
    moveServo(2,50);
    delay_ms(500);
    moveServo(2,90);
    delay_ms(500);
    moveServo(2,50);
    return 1;
   }
   
   //if x wins, print the appropriate message
   else if(sumrow1 == 3 | sumrow2 == 3 | sumrow3 == 3 | sumcol1 == 3 | sumcol2 == 3 | sumcol3 == 3 | sumdiag1 == 3
   | sumdiag2 == 3){
    printf("You won!!\r\n");
    return 1;
   }
   
   //if no one wins, return 0
   else return 0;
}

/***************************************************************************************
Function that checks if the robot needs to block the user from winning: if so, it blocks
@return 1 if a block was needed, and was therefore executed
**/
int checkblock(void){
   //check all possibilities for which a block might be needed; if any possibility is true, move to the corresponding position,
   //and return 1
   if(board[0][0] == xmove && board[0][1] == xmove && board[0][2] == nmove) {move(omove,0,2); return 1;}
   else if(board[0][0] == xmove && board[1][0] == xmove && board[2][0] == nmove) {move(omove,2,0); return 1;}
   else if(board[0][2] == xmove && board[0][1] == xmove && board[0][0] == nmove) {move(omove,0,0); return 1;}
   else if(board[0][2] == xmove && board[1][2] == xmove && board[2][2] == nmove) {move(omove,2,2); return 1;}
   else if(board[2][0] == xmove && board[1][0] == xmove && board[0][0] == nmove) {move(omove,0,0); return 1;}
   else if(board[2][0] == xmove && board[2][1] == xmove && board[2][2] == nmove) {move(omove,2,2); return 1;}
   else if(board[2][2] == xmove && board[2][1] == xmove && board[2][0] == nmove) {move(omove,2,0); return 1;}
   else if(board[2][2] == xmove && board[1][2] == xmove && board[0][2] == nmove) {move(omove,0,2); return 1;}
   else if(board[0][0] == xmove && board[0][2] == xmove && board[0][1] == nmove) {move(omove,0,1); return 1;}
   else if(board[0][0] == xmove && board[2][0] == xmove && board[1][0] == nmove) {move(omove,1,0); return 1;}
   else if(board[2][0] == xmove && board[2][2] == xmove && board[2][1] == nmove) {move(omove,2,1); return 1;}
   else if(board[2][2] == xmove && board[0][2] == xmove && board[1][2] == nmove) {move(omove,1,2); return 1;}
   else if(board[0][0] == xmove && board[1][1] == xmove && board[2][2] == nmove) {move(omove,2,2); return 1;}
   else if(board[0][2] == xmove && board[1][1] == xmove && board[2][0] == nmove) {move(omove,2,0); return 1;}
   else if(board[2][0] == xmove && board[1][1] == xmove && board[0][2] == nmove) {move(omove,0,2); return 1;}
   else if(board[2][2] == xmove && board[1][1] == xmove && board[0][0] == nmove) {move(omove,0,0); return 1;}
   else if(board[0][1] == xmove && board[1][1] == xmove && board[2][1] == nmove) {move(omove,2,1); return 1;}
   else if(board[2][1] == xmove && board[1][1] == xmove && board[0][1] == nmove) {move(omove,0,1); return 1;}
   else if(board[1][0] == xmove && board[1][1] == xmove && board[1][2] == nmove) {move(omove,1,2); return 1;}
   else if(board[1][2] == xmove && board[1][1] == xmove && board[1][0] == nmove) {move(omove,1,0); return 1;}
   //if none of the possibilities is true, i.e. if no block is needed, return 0
   else return 0;
}

/***************************************************************************************************************************
Function that checks if a win is possible in this turn, and if so, moves to the corresponding position, making the robot win
@return 1 if the win was executed
**/
int trytowin(void){
   //check all possibilities which might lead to a win, and if any of the possibilities is true, move to the corresponding
   //square, returning 1
   if(board[0][0] == omove && board[0][1] == omove && board[0][2] == nmove) {move(omove,0,2); return 1;}
   else if(board[0][0] == omove && board[1][0] == omove && board[2][0] == nmove) {move(omove,2,0); return 1;}
   else if(board[0][2] == omove && board[0][1] == omove && board[0][0] == nmove) {move(omove,0,0); return 1;}
   else if(board[0][2] == omove && board[1][2] == omove && board[2][2] == nmove) {move(omove,2,2); return 1;}
   else if(board[2][0] == omove && board[1][0] == omove && board[0][0] == nmove) {move(omove,0,0); return 1;}
   else if(board[2][0] == omove && board[2][1] == omove && board[2][2] == nmove) {move(omove,2,2); return 1;}
   else if(board[2][2] == omove && board[2][1] == omove && board[2][0] == nmove) {move(omove,2,0); return 1;}
   else if(board[2][2] == omove && board[1][2] == omove && board[0][2] == nmove) {move(omove,0,2); return 1;}
   else if(board[0][0] == omove && board[0][2] == omove && board[0][1] == nmove) {move(omove,0,1); return 1;}
   else if(board[0][0] == omove && board[2][0] == omove && board[1][0] == nmove) {move(omove,1,0); return 1;}
   else if(board[2][0] == omove && board[2][2] == omove && board[2][1] == nmove) {move(omove,2,1); return 1;}
   else if(board[2][2] == omove && board[0][2] == omove && board[1][2] == nmove) {move(omove,1,2); return 1;}
   else if(board[0][0] == omove && board[1][1] == omove && board[2][2] == nmove) {move(omove,2,2); return 1;}
   else if(board[0][2] == omove && board[1][1] == omove && board[2][0] == nmove) {move(omove,2,0); return 1;}
   else if(board[2][0] == omove && board[1][1] == omove && board[0][2] == nmove) {move(omove,0,2); return 1;}
   else if(board[2][2] == omove && board[1][1] == omove && board[0][0] == nmove) {move(omove,0,0); return 1;}
   else if(board[0][1] == omove && board[1][1] == omove && board[2][1] == nmove) {move(omove,2,1); return 1;}
   else if(board[2][1] == omove && board[1][1] == omove && board[0][1] == nmove) {move(omove,0,1); return 1;}
   else if(board[1][0] == omove && board[1][1] == omove && board[1][2] == nmove) {move(omove,1,2); return 1;}
   else if(board[1][2] == omove && board[1][1] == omove && board[1][0] == nmove) {move(omove,1,0); return 1;}
   //if a win is not possible, return 0
   else return 0;
}

/***********************************************************************************************
Function for picking up a given piece
@param piece 0 for o, 1 for x
@param num The position of the piece to be picked up (pieces are arranged in a line on the base)
**/
void pickup(int piece, int num){
  if(piece == 0){
  switch(num){
    case 1:
    moveServo(0,0);
    moveServo(2,20);
    moveServo(3,10);
    moveServo(1,70);
    moveServo(4,20);
    break;
    case 2:
    moveServo(0,0);
    moveServo(2,28);
    moveServo(3,5);
    moveServo(1,65);
    moveServo(4,20);
    break;
    case 3:
    moveServo(0,0);
    moveServo(2,37);
    moveServo(3,15);
    moveServo(1,63);
    moveServo(4,20);
    break;
    case 4:
    moveServo(0,5);
    moveServo(2,45);
    moveServo(3,45);
    moveServo(1,60);
    moveServo(4,20);
    break;
   }
  }
  else if(piece == 1){
    switch(num){
     case 1:
    moveServo(0,30);
    moveServo(2,20);
    moveServo(3,25);
    moveServo(1,70);
    moveServo(4,20);
    break;
    case 2:
    moveServo(0,30);
    moveServo(2,28);
    moveServo(3,15);
    moveServo(1,65);
    moveServo(4,20);
    break;
    case 3:
    moveServo(0,28);
    moveServo(2,38);
    moveServo(3,50);
    moveServo(1,63);
    moveServo(4,20);
    break;
    case 4:
    moveServo(0,28);
    moveServo(2,45);
    moveServo(3,50);
    moveServo(1,60);
    moveServo(4,20);
    break;
    case 5:
    moveServo(0,33);
    moveServo(2,50);
    moveServo(3,25);
    moveServo(1,57);
    moveServo(4,20);
    break;
    }
  }
}

/*********************************************
Function to move the arm to a neutral position
**/
void neutral(void){
  delay_ms(1500);
  moveServo(1,90);
}

/*******************************************************************
Function to place a picked up piece onto a given square on the board
@param x The row number of the square
@param y The column number of the square
**/
void drop(int x, int y){
    if(x == 0 && y == 0){
     moveServo(0,70);
     moveServo(2,26);
     moveServo(3,30);
     moveServo(1,75);
     moveServo(4,70);
    }
    else if(x == 1 && y == 0){
     moveServo(0,72);
     moveServo(2,33);
     moveServo(3,30);
     moveServo(1,71);
     moveServo(4,70);
    }
    else if(x == 2 && y == 0){
     moveServo(0,80);
     moveServo(2,43);
     moveServo(3,45);
     moveServo(1,66);
     moveServo(4,70);
    }
    else if(x == 0 && y == 1){
     moveServo(0,90);
     moveServo(2,26);
     moveServo(3,25);
     moveServo(1,70);
     moveServo(4,70);
    }
    else if(x == 1 && y == 1){
       moveServo(0,90);
     moveServo(2,33);
     moveServo(3,30);
     moveServo(1,66);
     moveServo(4,70);
   }
   else if(x == 2 && y == 1){
      moveServo(0,90);
     moveServo(2,43);
     moveServo(3,40);
     moveServo(1,65);
     moveServo(4,70);
   }
   else if(x == 0 && y == 2){
      moveServo(0,112);
     moveServo(2,26);
     moveServo(3,45);
     moveServo(1,75);
     moveServo(4,70);
   }
   else if(x == 1 && y == 2){
      moveServo(0,112);
     moveServo(2,33);
     moveServo(3,30);
     moveServo(1,70);
     moveServo(4,70);
   }
   else if(x == 2 && y == 2){
      moveServo(0,112);
     moveServo(2,43);
     moveServo(3,30);
     moveServo(1,66);
     moveServo(4,70);
   }
}

/*****************************************************************
Function to move a given servo by a given angle
@param i The servo to be moved, i.e., 0, 1, 2, 3 or 4
@param angle The angle in degrees by which the servo must be moved
**/
void moveServo(int i, int angle){   
   int count = 0; //integer to count the duration of the PWM signal supplied to servo 4
   
   //For servos 0, 1, 2 and 3, set the corresponding timer output-compare value to a number
   //approximately between 69 and 118
   //For servo 4, create a manual PWM waveform for about 1 s
   switch(i){
    case 0:
           servo0 = (int)(-0.27*(angle) + 118); //the OCR value for servo 0
        while(OCR1A != servo0){
           //increment or decrement the OCR value by 1, with an 80-ms gap between each iteration
            if(OCR1A < servo0) OCR1A = OCR1A + 1;
           if(OCR1A > servo0) OCR1A = OCR1A - 1;
           delay_ms(80);
        }
    break;
    case 1:
        servo1 = (int)(-0.27*(angle+80) + 118); //the OCR value for servo 1
           while(OCR0 != servo1){
           //increment or decrement the OCR value by 1, with an 80-ms gap between each iteration                  
            if(OCR0 < servo1) OCR0 = OCR0 + 1;
           if(OCR0 > servo1) OCR0 = OCR0 - 1;
           delay_ms(80);
        }
    break;
    case 2:
        servo2 = (int)(0.27*(angle-15) + 69); //the OCR value for servo 2
        while(OCR2 != servo2){
           //increment or decrement the OCR value by 1, with an 80-ms gap between each iteration                   
            if(OCR2 < servo2) OCR2 = OCR2 + 1;
           if(OCR2 > servo2) OCR2 = OCR2 - 1;
           delay_ms(80);
        }
    break;
    case 3:
        servo3 = (int)(0.27*(angle+88) + 69); //the OCR value for servo 3
        while(OCR1B != servo3){
           //increment or decrement the OCR value by 1, with an 80-ms gap between each iteration                    
            if(OCR1B < servo3) OCR1B = OCR1B + 1;
           if(OCR1B > servo3) OCR1B = OCR1B - 1;
           delay_ms(80);
        }
    break;
    case 4:
        servo4 = -5*(angle) + 1610; //the time of the PWM signal supplied to servo 4, measured in 1.1 us
        //the PWM is generated for approximately 1 s
        while(++count < 20){
           //the manually generated waveform appears on PORTD.3            
             PORTD.3 = 1; //assert PORTD.3
           delay_usv(servo4); //delay for the given time (measured in 1.1 us)
           PORTD.3 = 0; //negate PORTD.3
           delay_ms(20); //delay for 20 ms
        }
    break;
   }
}

/**************************************************
The main function of the program: controls the game
**/
void main(void){
    //variables for holding the angle and servo number received from terminal
    unsigned char aAngle[4];
    unsigned char aIndex[2];
     unsigned int i;

    initialize();
    
    //run the game for 5 turns maximum
    while(turn <= 5){
      if(gameOver == 1) break; //end the game if over  
      if(lockkeypad == 0){//user's turn
      if(r_ready == 1){//if a character is received
      
          //set the receive flags back to zero
           r_ready = 0;
          r_index = 0;          
          
          //depending on the character written, move an x to the corresponding position on the board
           if(r_buffer[0] == 'a'){
            move(xmove,0,0);
            lockkeypad = 1;
          }
          else if(r_buffer[0] == 'b'){
            move(xmove,0,1);
            lockkeypad = 1;
          }
          else if(r_buffer[0] == 'c'){
            move(xmove,0,2);
            lockkeypad = 1;
          }
          else if(r_buffer[0] == 'd'){
            move(xmove,1,0);
            lockkeypad = 1;
          }
          else if(r_buffer[0] == 'e'){
            move(xmove,1,1);
            lockkeypad = 1;
          }
          else if(r_buffer[0] == 'f'){
            move(xmove,1,2);
            lockkeypad = 1;
          }
          else if(r_buffer[0] == 'g'){
            move(xmove,2,0);
            lockkeypad = 1;
          }
          else if(r_buffer[0] == 'h'){
            move(xmove,2,1);
            lockkeypad = 1;
          }
          else if(r_buffer[0] == 'i'){
            move(xmove,2,2);
            lockkeypad = 1;
          }
          //otherwise if a command is given to move a specific servo, move it by the given angle
          else{
            for(i = 0; i<1; i++){
                      aIndex[i] = r_buffer[i];
            }
               aIndex[1] = 0x00;
             for(i = 2; i<5; i++){
                     aAngle[i-2] = r_buffer[i];
             }
             aAngle[3] = 0x00; //null terminate
            moveServo(atoi(aIndex),atoi(aAngle));
          }
        }
      }
      else if(lockkeypad == 1){//robot's turn
                         if(turn == 1){//try moving to the center for the first turn, if not available, move to square 'a'
                                if(board[1][1] == nmove){move(omove,1,1); lockkeypad = 0;}
                                else{move(omove,0,0); lockkeypad = 0;}
                         }
                         else if(turn == 2){//second turn
                                if(checkblock() == 1) lockkeypad = 0; //check if there is a need to block
                                else{//if not, move to an available corner square
                                 if(board[0][0] == nmove) {move(omove,0,0); lockkeypad = 0;}
                                 else if(board[0][2] == nmove) {move(omove,0,2); lockkeypad = 0;}
                                 else if(board[2][0] == nmove) {move(omove,2,0); lockkeypad = 0;}
                                 else if(board[2][2] == nmove) {move(omove,2,2); lockkeypad = 0;}
                                 else if(board[0][1] == nmove) {move(omove,0,1); lockkeypad = 0;}
                                 else if(board[1][0] == nmove) {move(omove,1,0); lockkeypad = 0;}
                                 else if(board[1][2] == nmove) {move(omove,1,2); lockkeypad = 0;}
                                 else if(board[2][1] == nmove) {move(omove,2,1); lockkeypad = 0;}
                                 else if(board[1][1] == nmove) {move(omove,1,1); lockkeypad = 0;}
                                }
                         }
                         else if(turn == 3){//third turn
                            if(checkwin() == 1) gameOver = 1; //if someone has won, signal game over
                            //else, see if a win is possible, if so, win
                            else if(trytowin() == 1){lockkeypad = 0; gameOver = 1;}
                            //else, see if a block is needed, if so, block
                            else if(checkblock() == 1){
                              lockkeypad = 0;
                            }
                            //else, move to an available square, preferably a corner square, followed by a side square
                            else{
                                 if(board[0][0] == nmove) {move(omove,0,0); lockkeypad = 0;}
                                 else if(board[0][2] == nmove) {move(omove,0,2); lockkeypad = 0;}
                                 else if(board[2][0] == nmove) {move(omove,2,0); lockkeypad = 0;}
                                 else if(board[2][2] == nmove) {move(omove,2,2); lockkeypad = 0;}
                                 else if(board[0][1] == nmove) {move(omove,0,1); lockkeypad = 0;}
                                 else if(board[1][0] == nmove) {move(omove,1,0); lockkeypad = 0;}
                                 else if(board[1][2] == nmove) {move(omove,1,2); lockkeypad = 0;}
                                 else if(board[2][1] == nmove) {move(omove,2,1); lockkeypad = 0;}
                                 else if(board[1][1] == nmove) {move(omove,1,1); lockkeypad = 0;}
                            }
                            if(checkwin() == 1) gameOver = 1; //if someone has won, signal game over
                         }
                         else if(turn == 4){//fourth turn
                            //everything else same as above
                            if(checkwin() == 1) gameOver = 1;
                            else if(trytowin() == 1){ lockkeypad = 0; gameOver = 1;}
                            else if(checkblock() == 1){
                              lockkeypad = 0;
                            }
                            else{
                                 if(board[0][0] == nmove) {move(omove,0,0); lockkeypad = 0;}
                                 else if(board[0][2] == nmove) {move(omove,0,2); lockkeypad = 0;}
                                 else if(board[2][0] == nmove) {move(omove,2,0); lockkeypad = 0;}
                                 else if(board[2][2] == nmove) {move(omove,2,2); lockkeypad = 0;}
                                 else if(board[0][1] == nmove) {move(omove,0,1); lockkeypad = 0;}
                                 else if(board[1][0] == nmove) {move(omove,1,0); lockkeypad = 0;}
                                 else if(board[1][2] == nmove) {move(omove,1,2); lockkeypad = 0;}
                                 else if(board[2][1] == nmove) {move(omove,2,1); lockkeypad = 0;}
                                 else if(board[1][1] == nmove) {move(omove,1,1); lockkeypad = 0;}
                            }
                            if(checkwin() == 1) gameOver = 1;
                         }
                         else if(turn == 5){//fifth turn
                            //everything else same as above
                            if(checkwin() == 1) gameOver = 1;
                            else if(trytowin() == 1){ lockkeypad = 0; gameOver = 1;}
                            else if(checkblock() == 1){
                              lockkeypad = 0;
                            }
                            else{
                                 if(board[0][0] == nmove) {move(omove,0,0); lockkeypad = 0;}
                                 else if(board[0][2] == nmove) {move(omove,0,2); lockkeypad = 0;}
                                 else if(board[2][0] == nmove) {move(omove,2,0); lockkeypad = 0;}
                                 else if(board[2][2] == nmove) {move(omove,2,2); lockkeypad = 0;}
                                 else if(board[0][1] == nmove) {move(omove,0,1); lockkeypad = 0;}
                                 else if(board[1][0] == nmove) {move(omove,1,0); lockkeypad = 0;}
                                 else if(board[1][2] == nmove) {move(omove,1,2); lockkeypad = 0;}
                                 else if(board[2][1] == nmove) {move(omove,2,1); lockkeypad = 0;}
                                 else if(board[1][1] == nmove) {move(omove,1,1); lockkeypad = 0;}
                            }
                            if(checkwin() == 1) gameOver = 1;
                         }
                         turn++; //increment the number of turns
          }
    }
}

/******************************************************************************************************************
Function for delaying by the given duration
@param duration The duration by which the program must delay, measured in 1.1 us
@note The function was originally intended to delay for 1 us, but the following code structure was closest in terms
of simple assembly instructions
**/
void delay_usv(unsigned int duration){
    duration /= 2;
    for(i = 0; i < duration; i++){
        delay_us(1);
    }
}

/*****************************
Function for setting it all up
**/
void initialize(void){
     //set up the USART control for the receive interrupt and so on
     UCSRB = 0x18 ;
     UBRRL = 103;
     UCSRB.7 = 1; //set the receive interrupt enable flag

     //set up the ports B and D as outputs for the PWM waveforms
     DDRB = 0xff;
     DDRD = 0xff;

     printf("Welcome!\n\r"); //welcome message

     //set up all timers in Fast PWM mode, with 1 PWM cycle = 4096 us
     TCCR0 = 0b01101100;
     TCCR2 = 0b01101110;
     TCCR1A = 0b10100001;
     TCCR1B = 0b00001100;

     //set up the OCR values for the timers to initialize the servos into position
     OCR1A = 115;
     OCR0 = 69;
     OCR2 = 69;
     OCR1B = 93;

     //set the receive ready flag to zero
     r_ready = 0;
     
     //initialize the moveServo values
     servo0 = 0;
     servo1 = 0;
     servo2 = 0;
     servo3 = 0;
     moveServo(4,180);     

     #asm
      sei
     #endasm
}