/*
 * sevoclass.cpp
 *
 *  Created on: 27. 6. 2014
 *      Author: martin.vitek
 */

#include "servo.h"
#include <avr/io.h>
#include <stdint.h>

servo_class::servo_class(volatile uint8_t *ddr, 
								  uint8_t  bit,
						 volatile uint16_t *ocr,
						 servo_data &data):data(data)
{
	this->ddr  = ddr;
	this->bit  = bit;
	this->ocr  = ocr;

	*ddr |= (1<<bit);
	
	this->data = data;
	
	one_step = data.pulse_duration / data.ICR_value;
	one_degree_pulse = (data.max_duration - data.min_duration) / 180;
}

servo_class::~servo_class()
{
	*ddr &= ~(1<<bit);
}

void servo_class::set_angle(uint8_t angle)
{
		 if (angle < 0)   current_angle = 0;
	else if (angle > 180) current_angle = 180;
	else				  current_angle = angle;

	*ocr = (data.min_duration + (one_degree_pulse * current_angle) ) / one_step;
}