/* ========================================================================== */
/*                RC Servo Motor Control Program                           */
/* ========================================================================== */
/*           Designed and programmed by Duck-Yong Yoon in 2004.  */

#include <avr/io.h>
#include "c:\AvrEdit\Ok128c\Ok128.h"

void LCD_s2d(signed char number)        /* display signed 2-digit decimal number */
{ unsigned char i;

  if(number >= 0)                               // sign
    { LCD_data('+');
      i = number;
    }
  else
    { LCD_data('-');
      i = -number;
    }

  LCD_data(i/10 + '0');                         // 10^1
  LCD_data(i%10 + '0');                         // 10^0
}

void Motor(signed char angle)           /* output pulse for RC servo motor */
{ unsigned int i;

  i = 375 - 200*angle/90;                       // convert angle to OCR value

  OCR1AH = (i >> 8);                            // set OCR1A
  OCR1AL = i & 0xFF;

  LCD_command(0xC6);                            // display motor angle
  LCD_s2d(angle);
}

int main(void)
{ signed char angle;

  MCU_initialize();                             // initialize MCU
  Delay_ms(50);                                 // wait for system stabilization
  LCD_initialize();                             // initialize text LCD module

  LCD_string(0x80," RC Servo Motor ");          // display motor angle
  LCD_string(0xC0,"      +00       ");

  TCCR1A = 0xAA;                                // Fast PWM mode(14)
  TCCR1B = 0x1B;
  TCCR1C = 0x00;
  ICR1H = (4999 >> 8);                          // f(PWM) = 16MHz/64/5000 = 50Hz
  ICR1L = 4999 & 0xFF;                          // T(PWM) = 20ms

  while(1)
    { Motor(0);                                 // sequential motor angle control
      Delay_ms(500);
      Motor(-90);
      Delay_ms(500);
      Motor(+90);
      Delay_ms(500);
      Motor(0);
      Delay_ms(1000);

      for(angle=0; angle>=-90; angle--)         // continuous motor anglr control
        { Motor(angle);
          Delay_ms(40);
        }
    }
}