// servosquirter.c
// for NerdKits with ATmega168
// mrobbins@mit.edu

#define F_CPU 14745600

#include <stdio.h>

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <inttypes.h>

#include "../libnerdkits/delay.h"
#include "../libnerdkits/lcd.h"
#include "../libnerdkits/uart.h"

// PIN DEFINITIONS:
//
// PB3 - pump control
// PB2 - servo signal (OC1B)


void pwm_set(uint16_t x) {
  OCR1B = x;
}


#define PWM_MIN 1300
#define PWM_MAX 4450
#define PWM_START 2765
void pwm_init() {
  // setup Timer1 for Fast PWM mode, 16-bit
  // COM1B1 -- for non-inverting output
  // WGM13, WGM12, WGM11, WGM10 -- for Fast PWM with OCR1A as TOP value
  // CS11 -- for CLK/8 prescaling
  
  OCR1A = 36864;	// sets PWM to repeat pulse every 20.0ms
  pwm_set(PWM_START);
  TCCR1A = (1<<COM1B1) | (1<<WGM11) | (1<<WGM10);
  TCCR1B = (1<<WGM13) | (1<<WGM12) | (1<<CS11);
  
  // each count is 8/14745600 = 0.5425us.
  // so 1.0ms = 1843.2
  //    1.5ms = 2764.8
  //    2.0ms = 3686.4
  //   20.0ms = 36864
}



int main() {
  // init LCD
  lcd_init();
  FILE lcd_stream = FDEV_SETUP_STREAM(lcd_putchar, 0, _FDEV_SETUP_WRITE);
  lcd_write_string(PSTR(" NerdKits ServoSquirter "));
  
  // init serial port
  uart_init();
  FILE uart_stream = FDEV_SETUP_STREAM(uart_putchar, uart_getchar, _FDEV_SETUP_RW);
  stdin = stdout = &uart_stream;

  // set PB2,PB3 as output
  DDRB |= (1<<PB2) | (1<<PB3);

  // init PWM
  pwm_init();

  uint16_t pos = PWM_START;
  char tc;
  while(1) {
    pwm_set(pos);
    // Print the current servo position to the LCD.
    lcd_line_two();
    fprintf_P(&lcd_stream, PSTR(" servo position: %d "), pos);


    // Wait for a character to be sent to the serial port.
    tc = uart_read();
    if(tc==']') pos+= 25;
    if(tc=='[') pos-= 25;
    if(tc=='0') PORTB &= ~(1<<PB3);
    if(tc=='1') PORTB |= (1<<PB3);
    
    // bounds checking
    if(pos > PWM_MAX) pos = PWM_MAX;
    if(pos < PWM_MIN) pos = PWM_MIN;

    // Print the current servo position to the serial port.
    printf_P(PSTR("%d\r\n"), pos);
  } 

  return 0;
}