/
main.cpp
84 lines (67 loc) · 1.35 KB
/
main.cpp
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
71
72
73
74
75
76
77
78
79
80
81
82
83
/*
G - ground
D - distance
M - motor
----------------------
| G2 D1 D2 D3 G3 |
| |
| |
| M1 M2 |
| |
| |
| G5 D4 D5 D6 G7 |
----------------------
*/
#include "lib/sumo.h"
#include "lib/motor.h"
#include "lib/queue.h"
#define ITIME 50
#define MAX_POWER 50 // tak zeby nie zjarac serw :p
#define DEBUG 1
Queue queue;
Move move;
Motor motor1;
Motor motor2;
// debug itp
unsigned char progressVal = 1;
void progress(){
led_send(progressVal);
if (progressVal*2 > 63) progressVal = 1;
else progressVal *= 2;
}
void debug(){
progress();
}
void init(){
led_init();
motor_init();
switch_init();
ground_init();
dist_init();
servo_init();
if(DEBUG) usart_init();
motor1 = Motor(&OCR1A, &MOTOR1_DIR_PORT, MOTOR1_DIR_PIN, MAX_POWER);
motor2 = Motor(&OCR1B, &MOTOR2_DIR_PORT, MOTOR2_DIR_PIN, MAX_POWER);
}
int main() {
init();
leds_on();
wait_s(5); // regulaminowy czas
for(;;){
if(DEBUG) debug();
if(switch1_pressed()){
leds_negate();
}
if(queue.head){
move = queue.pop(ITIME);
motor1.set_power(move.m1);
motor2.set_power(move.m2);
} else {
leds_off();
// szukanie
motor1.set_power(0);
motor2.set_power(0);
}
wait_ms(ITIME);
}
}