From 846afdd48d11d5bae24013f0cea969f6b6f47d49 Mon Sep 17 00:00:00 2001 From: Dimitri Sokolyuk Date: Sat, 26 Aug 2017 12:16:49 +0200 Subject: Add timer --- car/car.ino | 29 ++++++++++++----- car/elegoo.pb.c | 20 ++++++------ car/elegoo.pb.h | 42 ++++++++++++++----------- car/elegoo/elegoo.pb.go | 83 +++++++++++++++++++++++++++++-------------------- car/elegoo/elegoo.proto | 6 +++- car/elegoo/main.go | 58 ++++++++++++++++++++++++---------- 6 files changed, 153 insertions(+), 85 deletions(-) (limited to 'car') diff --git a/car/car.ino b/car/car.ino index 96f9c32..b33cd8d 100644 --- a/car/car.ino +++ b/car/car.ino @@ -3,6 +3,7 @@ #include #include +#include #include #include @@ -13,7 +14,7 @@ PacketSerial serial; Servo servo; - +Timer timer; IRrecv irrecv(IR); decode_results ir; @@ -46,6 +47,11 @@ int distance() { return pulseIn(Echo, HIGH, TimeOut) / ToCM; } +void stop() { + motorR(0); + motorL(0); +} + void onPacket(const uint8_t* buf, size_t size) { Command cmd = Command_init_zero; @@ -59,8 +65,7 @@ void onPacket(const uint8_t* buf, size_t size) { motorL(cmd.SpeedL); } if (cmd.has_Stop) { - motorR(0); - motorL(0); + stop(); } if (cmd.has_Direction) { look(cmd.Direction); @@ -68,12 +73,15 @@ void onPacket(const uint8_t* buf, size_t size) { if (cmd.has_Center) { look(0); } + if (cmd.has_StopAfter) { + timer.after(cmd.StopAfter, stop); + } } void events() { - uint8_t buf[256]; + uint8_t buf[64]; - Event evt = Event_init_zero; + Events evt = Events_init_zero; evt.Distance = distance(); evt.has_Distance = evt.Distance > 0; @@ -96,16 +104,18 @@ void events() { evt.Direction = looking(); evt.has_Direction = true; + evt.Time = millis(); + evt.has_Time = true; + pb_ostream_t ostream = pb_ostream_from_buffer(buf, sizeof(buf)); - pb_encode_delimited(&ostream, Event_fields, &evt); + pb_encode_delimited(&ostream, Events_fields, &evt); serial.send(buf, ostream.bytes_written); } void loop() { - events(); + timer.update(); serial.update(); - delay(100); } void setup() { @@ -133,4 +143,7 @@ void setup() { servo.attach(SRV); servo.write(90); + + timer.every(100, events); + timer.oscillate(LED, 500, LOW); } diff --git a/car/elegoo.pb.c b/car/elegoo.pb.c index 12d123d..a4e7ad4 100644 --- a/car/elegoo.pb.c +++ b/car/elegoo.pb.c @@ -1,5 +1,5 @@ /* Automatically generated nanopb constant definitions */ -/* Generated by nanopb-0.3.6 at Tue Aug 22 00:23:16 2017. */ +/* Generated by nanopb-0.3.6 at Sat Aug 26 10:18:35 2017. */ #include "elegoo.pb.h" @@ -10,22 +10,24 @@ -const pb_field_t Command_fields[6] = { +const pb_field_t Command_fields[7] = { PB_FIELD( 1, SINT32 , OPTIONAL, STATIC , FIRST, Command, SpeedR, SpeedR, 0), PB_FIELD( 2, SINT32 , OPTIONAL, STATIC , OTHER, Command, SpeedL, SpeedR, 0), PB_FIELD( 3, BOOL , OPTIONAL, STATIC , OTHER, Command, Stop, SpeedL, 0), PB_FIELD( 4, BOOL , OPTIONAL, STATIC , OTHER, Command, Center, Stop, 0), PB_FIELD( 5, SINT32 , OPTIONAL, STATIC , OTHER, Command, Direction, Center, 0), + PB_FIELD( 6, UINT32 , OPTIONAL, STATIC , OTHER, Command, StopAfter, Direction, 0), PB_LAST_FIELD }; -const pb_field_t Event_fields[7] = { - PB_FIELD( 1, UINT32 , OPTIONAL, STATIC , FIRST, Event, Distance, Distance, 0), - PB_FIELD( 2, SINT32 , OPTIONAL, STATIC , OTHER, Event, Direction, Distance, 0), - PB_FIELD( 3, BOOL , OPTIONAL, STATIC , OTHER, Event, SensorR, Direction, 0), - PB_FIELD( 4, BOOL , OPTIONAL, STATIC , OTHER, Event, SensorC, SensorR, 0), - PB_FIELD( 5, BOOL , OPTIONAL, STATIC , OTHER, Event, SensorL, SensorC, 0), - PB_FIELD( 6, UINT32 , OPTIONAL, STATIC , OTHER, Event, KeyPress, SensorL, 0), +const pb_field_t Events_fields[8] = { + PB_FIELD( 1, UINT32 , OPTIONAL, STATIC , FIRST, Events, Distance, Distance, 0), + PB_FIELD( 2, SINT32 , OPTIONAL, STATIC , OTHER, Events, Direction, Distance, 0), + PB_FIELD( 3, BOOL , OPTIONAL, STATIC , OTHER, Events, SensorR, Direction, 0), + PB_FIELD( 4, BOOL , OPTIONAL, STATIC , OTHER, Events, SensorC, SensorR, 0), + PB_FIELD( 5, BOOL , OPTIONAL, STATIC , OTHER, Events, SensorL, SensorC, 0), + PB_FIELD( 6, UINT32 , OPTIONAL, STATIC , OTHER, Events, KeyPress, SensorL, 0), + PB_FIELD( 7, UINT32 , OPTIONAL, STATIC , OTHER, Events, Time, KeyPress, 0), PB_LAST_FIELD }; diff --git a/car/elegoo.pb.h b/car/elegoo.pb.h index 1490445..661bbbc 100644 --- a/car/elegoo.pb.h +++ b/car/elegoo.pb.h @@ -1,5 +1,5 @@ /* Automatically generated nanopb header */ -/* Generated by nanopb-0.3.6 at Tue Aug 22 00:23:16 2017. */ +/* Generated by nanopb-0.3.6 at Sat Aug 26 10:18:35 2017. */ #ifndef PB_ELEGOO_PB_H_INCLUDED #define PB_ELEGOO_PB_H_INCLUDED @@ -26,10 +26,12 @@ typedef struct _Command { bool Center; bool has_Direction; int32_t Direction; + bool has_StopAfter; + uint32_t StopAfter; /* @@protoc_insertion_point(struct:Command) */ } Command; -typedef struct _Event { +typedef struct _Events { bool has_Distance; uint32_t Distance; bool has_Direction; @@ -42,16 +44,18 @@ typedef struct _Event { bool SensorL; bool has_KeyPress; uint32_t KeyPress; -/* @@protoc_insertion_point(struct:Event) */ -} Event; + bool has_Time; + uint32_t Time; +/* @@protoc_insertion_point(struct:Events) */ +} Events; /* Default values for struct fields */ /* Initializer values for message structs */ -#define Command_init_default {false, 0, false, 0, false, 0, false, 0, false, 0} -#define Event_init_default {false, 0, false, 0, false, 0, false, 0, false, 0, false, 0} -#define Command_init_zero {false, 0, false, 0, false, 0, false, 0, false, 0} -#define Event_init_zero {false, 0, false, 0, false, 0, false, 0, false, 0, false, 0} +#define Command_init_default {false, 0, false, 0, false, 0, false, 0, false, 0, false, 0} +#define Events_init_default {false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, false, 0} +#define Command_init_zero {false, 0, false, 0, false, 0, false, 0, false, 0, false, 0} +#define Events_init_zero {false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, false, 0} /* Field tags (for use in manual encoding/decoding) */ #define Command_SpeedR_tag 1 @@ -59,20 +63,22 @@ typedef struct _Event { #define Command_Stop_tag 3 #define Command_Center_tag 4 #define Command_Direction_tag 5 -#define Event_Distance_tag 1 -#define Event_Direction_tag 2 -#define Event_SensorR_tag 3 -#define Event_SensorC_tag 4 -#define Event_SensorL_tag 5 -#define Event_KeyPress_tag 6 +#define Command_StopAfter_tag 6 +#define Events_Distance_tag 1 +#define Events_Direction_tag 2 +#define Events_SensorR_tag 3 +#define Events_SensorC_tag 4 +#define Events_SensorL_tag 5 +#define Events_KeyPress_tag 6 +#define Events_Time_tag 7 /* Struct field encoding specification for nanopb */ -extern const pb_field_t Command_fields[6]; -extern const pb_field_t Event_fields[7]; +extern const pb_field_t Command_fields[7]; +extern const pb_field_t Events_fields[8]; /* Maximum encoded size of messages (where known) */ -#define Command_size 22 -#define Event_size 24 +#define Command_size 28 +#define Events_size 30 /* Message IDs (where set with "msgid" option) */ #ifdef PB_MSGID diff --git a/car/elegoo/elegoo.pb.go b/car/elegoo/elegoo.pb.go index 0d779ee..f453f44 100644 --- a/car/elegoo/elegoo.pb.go +++ b/car/elegoo/elegoo.pb.go @@ -9,7 +9,7 @@ It is generated from these files: It has these top-level messages: Command - Event + Events */ package main @@ -29,11 +29,12 @@ var _ = math.Inf const _ = proto.ProtoPackageIsVersion2 // please upgrade the proto package type Command struct { - SpeedR int32 `protobuf:"zigzag32,1,opt,name=SpeedR" json:"SpeedR,omitempty"` - SpeedL int32 `protobuf:"zigzag32,2,opt,name=SpeedL" json:"SpeedL,omitempty"` - Stop bool `protobuf:"varint,3,opt,name=Stop" json:"Stop,omitempty"` - Center bool `protobuf:"varint,4,opt,name=Center" json:"Center,omitempty"` - Direction int32 `protobuf:"zigzag32,5,opt,name=Direction" json:"Direction,omitempty"` + SpeedR int32 `protobuf:"zigzag32,1,opt,name=SpeedR" json:"SpeedR,omitempty"` + SpeedL int32 `protobuf:"zigzag32,2,opt,name=SpeedL" json:"SpeedL,omitempty"` + Stop bool `protobuf:"varint,3,opt,name=Stop" json:"Stop,omitempty"` + Center bool `protobuf:"varint,4,opt,name=Center" json:"Center,omitempty"` + Direction int32 `protobuf:"zigzag32,5,opt,name=Direction" json:"Direction,omitempty"` + StopAfter uint32 `protobuf:"varint,6,opt,name=StopAfter" json:"StopAfter,omitempty"` } func (m *Command) Reset() { *m = Command{} } @@ -76,83 +77,99 @@ func (m *Command) GetDirection() int32 { return 0 } -type Event struct { +func (m *Command) GetStopAfter() uint32 { + if m != nil { + return m.StopAfter + } + return 0 +} + +type Events struct { Distance uint32 `protobuf:"varint,1,opt,name=Distance" json:"Distance,omitempty"` Direction int32 `protobuf:"zigzag32,2,opt,name=Direction" json:"Direction,omitempty"` SensorR bool `protobuf:"varint,3,opt,name=SensorR" json:"SensorR,omitempty"` SensorC bool `protobuf:"varint,4,opt,name=SensorC" json:"SensorC,omitempty"` SensorL bool `protobuf:"varint,5,opt,name=SensorL" json:"SensorL,omitempty"` KeyPress uint32 `protobuf:"varint,6,opt,name=KeyPress" json:"KeyPress,omitempty"` + Time uint32 `protobuf:"varint,7,opt,name=Time" json:"Time,omitempty"` } -func (m *Event) Reset() { *m = Event{} } -func (m *Event) String() string { return proto.CompactTextString(m) } -func (*Event) ProtoMessage() {} -func (*Event) Descriptor() ([]byte, []int) { return fileDescriptor0, []int{1} } +func (m *Events) Reset() { *m = Events{} } +func (m *Events) String() string { return proto.CompactTextString(m) } +func (*Events) ProtoMessage() {} +func (*Events) Descriptor() ([]byte, []int) { return fileDescriptor0, []int{1} } -func (m *Event) GetDistance() uint32 { +func (m *Events) GetDistance() uint32 { if m != nil { return m.Distance } return 0 } -func (m *Event) GetDirection() int32 { +func (m *Events) GetDirection() int32 { if m != nil { return m.Direction } return 0 } -func (m *Event) GetSensorR() bool { +func (m *Events) GetSensorR() bool { if m != nil { return m.SensorR } return false } -func (m *Event) GetSensorC() bool { +func (m *Events) GetSensorC() bool { if m != nil { return m.SensorC } return false } -func (m *Event) GetSensorL() bool { +func (m *Events) GetSensorL() bool { if m != nil { return m.SensorL } return false } -func (m *Event) GetKeyPress() uint32 { +func (m *Events) GetKeyPress() uint32 { if m != nil { return m.KeyPress } return 0 } +func (m *Events) GetTime() uint32 { + if m != nil { + return m.Time + } + return 0 +} + func init() { proto.RegisterType((*Command)(nil), "Command") - proto.RegisterType((*Event)(nil), "Event") + proto.RegisterType((*Events)(nil), "Events") } func init() { proto.RegisterFile("elegoo.proto", fileDescriptor0) } var fileDescriptor0 = []byte{ - // 217 bytes of a gzipped FileDescriptorProto - 0x1f, 0x8b, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0xff, 0x5c, 0x90, 0xb1, 0x4a, 0xc5, 0x30, - 0x14, 0x86, 0xc9, 0xb5, 0x37, 0xad, 0x07, 0x1d, 0xcc, 0x20, 0x41, 0x1c, 0x4a, 0xa7, 0x4e, 0x2e, - 0xbe, 0x81, 0xad, 0x93, 0x19, 0x24, 0xdd, 0xdc, 0x6a, 0x7b, 0x90, 0x80, 0xcd, 0x29, 0x49, 0x10, - 0x9c, 0x7d, 0x1c, 0x5f, 0x52, 0x1a, 0xd3, 0x56, 0xef, 0x96, 0xef, 0xff, 0xc3, 0xcf, 0xc7, 0x81, - 0x0b, 0x7c, 0xc7, 0x37, 0xa2, 0xbb, 0xd9, 0x51, 0xa0, 0xea, 0x8b, 0x41, 0xde, 0xd0, 0x34, 0xf5, - 0x76, 0x14, 0xd7, 0xc0, 0xbb, 0x19, 0x71, 0xd4, 0x92, 0x95, 0xac, 0xbe, 0xd2, 0x89, 0xb6, 0x5c, - 0xc9, 0xc3, 0x9f, 0x5c, 0x09, 0x01, 0x59, 0x17, 0x68, 0x96, 0x67, 0x25, 0xab, 0x0b, 0x1d, 0xdf, - 0xcb, 0xdf, 0x06, 0x6d, 0x40, 0x27, 0xb3, 0x98, 0x26, 0x12, 0xb7, 0x70, 0xde, 0x1a, 0x87, 0x43, - 0x30, 0x64, 0xe5, 0x31, 0xce, 0xec, 0x41, 0xf5, 0xcd, 0xe0, 0xf8, 0xf8, 0x81, 0x36, 0x88, 0x1b, - 0x28, 0x5a, 0xe3, 0x43, 0x6f, 0x07, 0x8c, 0x16, 0x97, 0x7a, 0xe3, 0xff, 0x1b, 0x87, 0x93, 0x0d, - 0x21, 0x21, 0xef, 0xd0, 0x7a, 0x72, 0x3a, 0x09, 0xad, 0xb8, 0x37, 0x4d, 0x92, 0x5a, 0x71, 0x6f, - 0x54, 0x74, 0xda, 0x1a, 0xb5, 0x78, 0x3c, 0xe1, 0xe7, 0xb3, 0x43, 0xef, 0x25, 0xff, 0xf5, 0x58, - 0xf9, 0x81, 0xbf, 0x64, 0x53, 0x6f, 0xec, 0x2b, 0x8f, 0x27, 0xbc, 0xff, 0x09, 0x00, 0x00, 0xff, - 0xff, 0x78, 0x67, 0x1b, 0x88, 0x52, 0x01, 0x00, 0x00, + // 240 bytes of a gzipped FileDescriptorProto + 0x1f, 0x8b, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0xff, 0x5c, 0x90, 0xb1, 0x4e, 0xc3, 0x30, + 0x10, 0x86, 0xe5, 0x12, 0x9c, 0x72, 0xa2, 0x03, 0x1e, 0x90, 0x85, 0x18, 0xa2, 0x4e, 0x99, 0x58, + 0x78, 0x02, 0x48, 0x99, 0xc8, 0x80, 0x1c, 0x26, 0xb6, 0xd0, 0x1e, 0xc8, 0x12, 0xf1, 0x45, 0xb6, + 0x85, 0xc4, 0xe3, 0xf0, 0x26, 0x3c, 0x1a, 0xf2, 0xc9, 0x49, 0xa0, 0x9b, 0xff, 0xef, 0xcf, 0xe5, + 0x3e, 0x1d, 0x9c, 0xe3, 0x07, 0xbe, 0x13, 0xdd, 0x8c, 0x9e, 0x22, 0x6d, 0xbf, 0x05, 0x94, 0x0d, + 0x0d, 0x43, 0xef, 0x0e, 0xea, 0x12, 0x64, 0x37, 0x22, 0x1e, 0x8c, 0x16, 0x95, 0xa8, 0x2f, 0x4c, + 0x4e, 0x33, 0x6f, 0xf5, 0xea, 0x0f, 0x6f, 0x95, 0x82, 0xa2, 0x8b, 0x34, 0xea, 0x93, 0x4a, 0xd4, + 0x6b, 0xc3, 0xef, 0xf4, 0x6d, 0x83, 0x2e, 0xa2, 0xd7, 0x05, 0xd3, 0x9c, 0xd4, 0x35, 0x9c, 0xed, + 0xac, 0xc7, 0x7d, 0xb4, 0xe4, 0xf4, 0x29, 0xff, 0x66, 0x01, 0xa9, 0x4d, 0xd3, 0x77, 0x6f, 0x69, + 0x50, 0x56, 0xa2, 0xde, 0x98, 0x05, 0x6c, 0x7f, 0x04, 0xc8, 0x87, 0x4f, 0x74, 0x31, 0xa8, 0x2b, + 0x58, 0xef, 0x6c, 0x88, 0xbd, 0xdb, 0x23, 0x4b, 0x6e, 0xcc, 0x9c, 0xff, 0xaf, 0x58, 0x1d, 0xaf, + 0xd0, 0x50, 0x76, 0xe8, 0x02, 0x79, 0x93, 0x7d, 0xa7, 0xb8, 0x34, 0x4d, 0x76, 0x9e, 0xe2, 0xd2, + 0xb4, 0xac, 0x3c, 0x37, 0x6d, 0xf2, 0x78, 0xc4, 0xaf, 0x27, 0x8f, 0x21, 0x64, 0xdf, 0x39, 0xa7, + 0xb3, 0x3c, 0xdb, 0x01, 0x75, 0xc9, 0x9c, 0xdf, 0xf7, 0xf2, 0xa5, 0x18, 0x7a, 0xeb, 0x5e, 0x25, + 0x5f, 0xfd, 0xf6, 0x37, 0x00, 0x00, 0xff, 0xff, 0x0f, 0x34, 0x8b, 0xa3, 0x85, 0x01, 0x00, 0x00, } diff --git a/car/elegoo/elegoo.proto b/car/elegoo/elegoo.proto index f9b13bf..a25f592 100644 --- a/car/elegoo/elegoo.proto +++ b/car/elegoo/elegoo.proto @@ -1,5 +1,7 @@ syntax = "proto3"; + + option go_package = "main"; message Command { @@ -8,13 +10,15 @@ message Command { bool Stop = 3; bool Center = 4; sint32 Direction = 5; // +90 left; -90 right + uint32 StopAfter = 6; // milliseconds } -message Event { +message Events { uint32 Distance = 1; sint32 Direction = 2; bool SensorR = 3; bool SensorC = 4; bool SensorL = 5; uint32 KeyPress = 6; + uint32 Time = 7; } diff --git a/car/elegoo/main.go b/car/elegoo/main.go index 40fffdb..46d3058 100644 --- a/car/elegoo/main.go +++ b/car/elegoo/main.go @@ -39,7 +39,9 @@ func Read(buf *bufio.Reader, pb proto.Message) error { func main() { c := &serial.Config{ - Name: "/dev/tty.usbmodem1421", + //Name: "/dev/tty.usbmodem1421", + //Name: "/dev/tty.usbmodem1411", + Name: "/dev/tty.Elegoo-DevB", Baud: 57600, } s, err := serial.OpenPort(c) @@ -47,18 +49,50 @@ func main() { log.Fatal(err) } defer s.Close() + log.Println("connected") go func() { - time.Sleep(5 * time.Second) + log.Println("recv") + buf := bufio.NewReader(s) + for { + evt := &Events{} + if err := Read(buf, evt); err != nil { + log.Println("ERR", err) + } + log.Println(evt) + } + }() - Write(s, &Command{Direction: -45}) - time.Sleep(time.Second) + log.Println("send -45") + time.Sleep(time.Second) + Write(s, &Command{Direction: -45}) - Write(s, &Command{Direction: 45}) - time.Sleep(time.Second) + log.Println("send +45") + time.Sleep(time.Second) + Write(s, &Command{Direction: 45}) - Write(s, &Command{Center: true}) - }() + log.Println("send center") + time.Sleep(time.Second) + Write(s, &Command{Center: true}) + + log.Println("send -5") + time.Sleep(time.Second) + Write(s, &Command{Direction: -5}) + + /* log.Println("send motor") + Write(s, &Command{SpeedL: 200, SpeedR: 200, StopAfter: 1000}) + time.Sleep(time.Second) + */ + + log.Println("send motor turn") + time.Sleep(time.Second) + Write(s, &Command{SpeedL: -250, SpeedR: 250, StopAfter: 500}) + + log.Println("send motor turn") + time.Sleep(time.Second) + Write(s, &Command{SpeedL: 250, SpeedR: -250, StopAfter: 500}) + + time.Sleep(time.Minute) /* MOTOR cmd.SpeedL = 200 @@ -85,12 +119,4 @@ func main() { Write(s, cmd) */ - buf := bufio.NewReader(s) - for { - evt := &Event{} - if err := Read(buf, evt); err != nil { - log.Println("ERR", err) - } - log.Println(evt) - } } -- cgit v1.2.3