-
Notifications
You must be signed in to change notification settings - Fork 0
/
control.c
70 lines (54 loc) · 1.53 KB
/
control.c
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
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
// \author Benjamin Franzke
#include <avr/io.h>
#include "control.h"
#include "sensor.h"
void init_control() {
// set engine pins to output
DDR_ENGINE |= (1 << ENGINE_LEFT) | (1 << ENGINE_RIGHT)|
(1 << ENGINE_ENABLE);
PORT_ENGINE &= ~(1 << ENGINE_ENABLE);
// set direction pins to output
DDR_DIRECTION |= (1 << DIR_LEFT) | (1 << DIR_RIGHT) |
(1 << DIR_EN);
DIRECTION &= ~(1 << DIR_EN);
// PWM configuration
OCR1A = 0;
TCCR1A = (1 << COM1A1) | (1 << WGM12) |
(1 << WGM11) | (1 << WGM10);
TCCR1B = (1 << CS10);
}
void control_cmd (uint8_t _action, int8_t _param) {
if (_action == 'S') {
PORTC ^= (1 << PC1);
if (_param == 0) {
OCR1A = 0;
PORT_ENGINE &= ~(1 << ENGINE_RIGHT);
PORT_ENGINE &= ~(1 << ENGINE_LEFT);
} else if (_param > 0) {
rwd = 1;
OCR1A = ((uint16_t) _param) << 3;
PORT_ENGINE &= ~(1 << ENGINE_RIGHT);
PORT_ENGINE |= (1 << ENGINE_LEFT);
} else if ((_param < 0) & !hinderniss) {
rwd = 0;
OCR1A = ((uint16_t) (- _param)) << 3;
PORT_ENGINE &= ~(1 << ENGINE_LEFT);
PORT_ENGINE |= (1 << ENGINE_RIGHT);
}
} else if (_action == 'D') {
if (_param >= -70 && _param <= 70) {
DIRECTION &= ~(1 << DIR_LEFT);
DIRECTION &= ~(1 << DIR_RIGHT);
DIRECTION &= ~(1 << DIR_EN);
} else if (_param > 70) {
DIRECTION &= ~(1 << DIR_RIGHT);
DIRECTION |= (1 << DIR_LEFT);
DIRECTION |= (1 << DIR_EN);
} else if (_param < -70) {
DIRECTION &= ~(1 << DIR_LEFT);
DIRECTION |= (1 << DIR_RIGHT);
DIRECTION |= (1 << DIR_EN);
}
}
}
/* vim: set sts=0 fenc=utf-8: */