// C++ code
//
/*
  Sweep

  by BARRAGAN <http://barraganstudio.com>
  This example code is in the public domain.

  modified 8 Nov 2013  by Scott Fitzgerald
  http://www.arduino.cc/en/Tutorial/Sweep
*/

#include <Servo.h>
uint8_t counter = 0;
uint16_t counter16 = 0;
uint8_t imp = 10;

int pos = 0;

Servo servo_9;

void setup()
{
  //
  DDRB |= (1 << PB1);
  tim2_ctc_oc_init();
}

void loop()
{

  //TCCR2A = (1<<WGM21) | (0<<WGM20);


  //TCCR2B = (0<<WGM22) | (1<<CS22) | (0<<);

}

ISR(TIMER2_COMPA_vect)
{
  if(counter==199){

    //20ms period 0,1 - 200
    //+90, 0, -90
    //2 1,5     1   20  -    15    -   10
    counter=0;
    PORTB |= (1<<PB1);
  }
  else counter++;
  if(counter16==9999){
    counter16=0;
    if(imp==20){
      imp=10;
    } else {
      imp++;
    }
  } else {
    counter16++;
  }
  if(counter==imp){
    PORTB &= ~(1<<PB1);
  }


}

void tim2_ctc_oc_init(){
     TCCR2A = (1<<WGM21) | (0<<WGM20);


    TCCR2B = (0<<WGM22) | (1<<CS22) | (0<<CS21) | (0<<CS20);
  OCR2A = 24;
  TIMSK2 = (1<<OCIE2A);
  sei();

}