From 4255e0ad78e0f7cc8694df77e585ca02a8113bb3 Mon Sep 17 00:00:00 2001 From: Dimitri Sokolyuk Date: Mon, 21 Aug 2017 23:52:53 +0200 Subject: ... --- car/car.ino | 33 +++++++++++---------------------- 1 file changed, 11 insertions(+), 22 deletions(-) (limited to 'car/car.ino') diff --git a/car/car.ino b/car/car.ino index 474d543..0bcc293 100644 --- a/car/car.ino +++ b/car/car.ino @@ -4,7 +4,6 @@ #include #include #include -//#include #include #include @@ -14,7 +13,6 @@ PacketSerial serial; Servo servo; -int trim; IRrecv irrecv(IR); decode_results ir; @@ -32,13 +30,10 @@ void motor(int e, int a, int b, int v) { analogWrite(e, v); } -void motorR(int v) { - motor(ENA, IN1, IN2, v); -} - -void motorL(int v) { - motor(ENB, IN3, IN4, v); -} +#define motorR(v) motor(ENA, IN1, IN2, v) +#define motorL(v) motor(ENB, IN3, IN4, v) +#define look(deg) servo.write(deg + 90) +#define looking() (servo.read() - 90) int distance() { digitalWrite(Trig, LOW); @@ -49,12 +44,6 @@ int distance() { return pulseIn(Echo, HIGH, TimeOut) / ToCM; } -// + left -// - right -void look(int deg) { - servo.write(90 + deg + trim); -} - void onPacket(const uint8_t* buf, size_t size) { Command cmd = Command_init_zero; @@ -71,16 +60,12 @@ void onPacket(const uint8_t* buf, size_t size) { motorR(0); motorL(0); } - if (cmd.has_Trim) { - trim = cmd.Trim; - } - if (cmd.has_TurnHead) { - look(cmd.TurnHead); + if (cmd.has_Direction) { + look(cmd.Direction); } if (cmd.has_Center) { look(0); } - } void env() { @@ -106,6 +91,9 @@ void env() { irrecv.resume(); } + evt.Direction = looking(); + evt.has_Direction = true; + pb_ostream_t ostream = pb_ostream_from_buffer(buf, sizeof(buf)); pb_encode_delimited(&ostream, Event_fields, &evt); @@ -115,7 +103,7 @@ void env() { void loop() { env(); serial.update(); - delay(200); + delay(100); } void setup() { @@ -142,4 +130,5 @@ void setup() { irrecv.enableIRIn(); servo.attach(SRV); + servo.write(90); } -- cgit v1.2.3