#include <avr/io.h>

#include <avr/interrupt.h>

#include "debounce_key.h"

#include "rc_servo.h"

#include <util/delay.h>

// uses mode 15, timer: OC1C on PB7, 4x4 keyboard: S13/S9 for control, KEY1 turns on the timer

ISR(INT4_vect) {

   if (OCR1C < MAX) {

       OCR1C += 0x01;

   }

}

ISR(INT5_vect) {

   if (OCR1C > MIN) {

       OCR1C -= 0x01;

   }

}

int main(void)

{

   init_rc_servo();

   init_debounce_key_interrupt(SENSE_RAISING);

   sei();

   int8_t i = 0;

   while(1)

   {

       for (i = -100; i < 100; i += 1) {

           rc_servo(i);                // move to the right (percentage function)

           _delay_ms(1);

       }

       _delay_ms(1500);

       

       for (i = 100; i > -100; i -= 1) {

           rc_servo(i);                // move to the left

           _delay_ms(1);

       }

       

       _delay_ms(1500);

   }

}

/*! @file rc_servo.c

@brief Implementation of library to control the Parallax Servo.

@author Pawel Kramarz

@defgroup servo_module Parallax Servo Driver */

#include <avr/io.h>

#include "rc_servo.h"

void setPrescaler(uint16_t speed) {

   switch (speed)

   {

       // no clock

       case 0:

       TCCR1B &= 0xf8;

       break;

       case 1:

       TCCR1B &= ~(1 << CS12) & ~(1 << CS11);

       TCCR1B |= (1 << CS10);

       break;

       

       case 8:

       TCCR1B &= ~(1 << CS12) & ~(1 << CS10);

       TCCR1B |= (1 << CS11);

       break;

       

       case 64:

       TCCR1B &= ~(1 << CS12);

       TCCR1B |= (1 << CS11) | (1 << CS10);

       break;

       case 256:

       TCCR1B |= (1 << CS12);

       TCCR1B &= ~(1 << CS11) & ~(1 << CS10);

       break;

       case 1024:

       TCCR1B |= (1 << CS12) | (1 << CS10);

       TCCR1B &= ~(1 << CS11);

       break;

       

       default:

       TCCR1B &= 0xf8;

       break;

   }

}

void init_rc_servo() {

   // OC1C as output

   DDRB |= (1 << PB7);

   

   // Toggle OC1C on Compare Match

   TCCR1A |= (1 << COM1C1);

   // page 167 in atmega1280_datasheet.pdf

   TCCR1A &= ~(1 << COM1C0);

   

   // mode 15, p. 148

   TCCR1B |= (1 << WGM13);

   TCCR1B |= (1 << WGM12);

   TCCR1A |= (1 << WGM11);

   TCCR1A |= (1 << WGM10);

   

   setPrescaler(PRESCALER);

   // period: 50Hz for prescaler 64, 19.2 Description p. 172

   OCR1A = TOP;

   // set the rotor to the center position

   OCR1C = CENTER;

   // start value

   TCNT1 = 0x0000;

}

void rc_servo(int8_t percent) {

   // value range between -100 and +100

   if((percent > -101) && (percent < 101)) {

       uint16_t value = ((((CENTER - MIN) * percent)/100) + CENTER);

       OCR1C = value;

   }

}