/* Quick interactive demo running servo with Timer 1 */

// ------- Preamble -------- //

#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>

#include "pinDefines.h"
#include "USART.h"

#define PULSE_MIN 1000 /* experiment with these values */

#define PULSE_MAX 2000 /* to match your own servo */
#define PULSE_MID 1500

static inline uint16_t getNumber16(void);

static inline void initTimer1Servo(void) {

/* Set up Timer1 (16bit) to give a pulse every 20ms */
/* Use Fast PWM mode, counter max in ICR1 */
TCCR1A |= (1 << WGM11);
TCCR1B |= (1 << WGM12) | (1 << WGM13);
TCCR1B |= (1 << CS10); /* /1 prescaling -- counting in microseconds */
ICR1 = 20000; /* TOP value = 20ms */
TCCR1A |= (1 << COM1A1); /* Direct output on PB1 / OC1A */
DDRB |= (1 << PB1); /* set pin for output */
}

static inline void showOff(void) {

printString("Center\r\n");
OCR1A = PULSE_MID;
_delay_ms(1500);
printString("Clockwise Max\r\n");
OCR1A = PULSE_MIN;
_delay_ms(1500);
printString("Counterclockwise Max\r\n");
OCR1A = PULSE_MAX;
_delay_ms(1500);
printString("Center\r\n");
OCR1A = PULSE_MID;
_delay_ms(1500);
}

int main(void) {

// -------- Inits --------- //

uint16_t servoPulseLength;
OCR1A = PULSE_MID; /* set it to middle position initially */
initTimer1Servo();
initUSART();
printString("\r\nWelcome to the Servo Demo\r\n");
showOff();

// ------ Event loop ------ //

while (1) {

printString("\r\nEnter a four-digit pulse length:\r\n");

servoPulseLength = getNumber16();

printString("On my way....\r\n");
 OCR1A = servoPulseLength;
DDRB |= (1 << PB1); /* re-enable output pin */

_delay_ms(1000);
printString("Releasing...\r\n");
while (TCNT1 < 3000) {;
} /* delay until pulse part of cycle done */
DDRB &= ~(1 << PB1); /* disable output pin */

} /* End event loop */

return 0; /* This line is never reached */
}

static inline uint16_t getNumber16(void) {

// Gets a PWM value from the serial port.
// Reads in characters, turns them into a number
char thousands = '0';
char hundreds = '0';
char tens = '0';
char ones = '0';
char thisChar = '0';

do {
thousands = hundreds; /* shift numbers over */
hundreds = tens;
tens = ones;
ones = thisChar;
thisChar = receiveByte(); /* get a new character */
transmitByte(thisChar); /* echo */
} while (thisChar != '\r');

transmitByte('\n'); /* newline */
return (1000 * (thousands - '0') + 100 * (hundreds - '0') +
10 * (tens - '0') + ones - '0');
}