Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
/*
* 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;
}