aboutsummaryrefslogtreecommitdiff
path: root/car/car.ino
blob: a0c6f3c45604f03e6a1f7ed12e9b7499bbabcc5a (plain)
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
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
// Dimitri Sokolyuk
// 01.01.2017

//#include <PacketSerial.h>
#include <Servo.h>
#include <IRremote.h>
//#include <os48.h>

#include "config.h"
#include "pb_stream.h"
#include "elegoo.pb.h"

//PacketSerial serial;
Servo head;
IRrecv irrecv(IR);

pb_istream_t istream;
pb_ostream_t ostream;

void motor(int e, int a, int b, int v) {
  if (v > 0) {
    digitalWrite(a, LOW);
    digitalWrite(b, HIGH);
  }
  if (v < 0) {
    digitalWrite(a, HIGH);
    digitalWrite(b, LOW);
    v = -v;
  }
  analogWrite(e, v);
}

void move(int l, int r) {
  motor(ENA, IN1, IN2, l);
  motor(ENB, IN3, IN4, r);
}

#define forward(v)  do move( v,  v); while (0)
#define backward(v) do move(-v, -v); while (0)
#define toleft(v)   do move( v, -v); while (0)
#define toright(v)  do move(-v,  v); while (0)
#define stop()      do move( 0,  0); while (0)

int distance() {
  digitalWrite(Trig, LOW);
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);
  delayMicroseconds(18);
  digitalWrite(Trig, LOW);
  return pulseIn(Echo, HIGH, TimeOut) / ToCM;
}

void look(int deg) {
  head.write(90 + deg);
  delay(1000); // wait to finish
}

#define lookahead()     do look(   0); while (0)
#define lookleft(deg)   do look( deg); while (0)
#define lookright(deg)  do look(-deg); while (0)


int ir() {
  decode_results results = {};
  if (irrecv.decode(&results)) {
    irrecv.resume();
  }
  return results.value;
}

void setup() {
  Serial.begin(57600);
  pb_istream_from_stream(Serial, istream);
  pb_ostream_from_stream(Serial, ostream);

  pinMode(Echo, INPUT);
  pinMode(Trig, OUTPUT);

  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);
  pinMode(LED, OUTPUT);

  pinMode(S1, INPUT);
  pinMode(S2, INPUT);
  pinMode(S3, INPUT);

  pinMode(IR, INPUT);
  irrecv.enableIRIn();

  head.attach(SRV);
  lookahead();
  stop();
}

void loop() {
  Command cmd = Command_init_zero;
  pb_decode_delimited(&istream, Command_fields, &cmd);

  Event evt = Event_init_zero;

  evt.Distance = distance();
  evt.has_Distance = evt.Distance > 0;

  evt.SensorA = digitalRead(S1);
  evt.has_SensorA = true;

  evt.SensorB = digitalRead(S2);
  evt.has_SensorB = true;

  evt.SensorC = digitalRead(S3);
  evt.has_SensorC = true;

  evt.KeyPress = ir();
  evt.has_KeyPress = evt.KeyPress != 0;

  pb_encode_delimited(&ostream, Event_fields, &evt);

  delay(1000);
}