From 3adc147fc0a70f18888949ac90441f24b9369b15 Mon Sep 17 00:00:00 2001 From: Dimitri Sokolyuk Date: Mon, 21 Aug 2017 21:10:36 +0200 Subject: wip --- car/car.ino | 28 ++++++++++++++++--------- car/config.h | 6 +++--- car/elegoo.pb.c | 15 ++++++------- car/elegoo.pb.h | 27 +++++++++++++----------- car/elegoo/elegoo.pb.go | 56 +++++++++++++++++++++++++++++-------------------- car/elegoo/elegoo.proto | 9 ++++---- car/elegoo/main.go | 42 +++++++++++++++++++++++-------------- 7 files changed, 108 insertions(+), 75 deletions(-) (limited to 'car') diff --git a/car/car.ino b/car/car.ino index 66205dd..b048db1 100644 --- a/car/car.ino +++ b/car/car.ino @@ -68,8 +68,15 @@ int ir() { void onPacket(const uint8_t* buf, size_t size) { Command cmd = Command_init_zero; - pb_istream_t istream = pb_istream_from_buffer(buf, sizeof(buf)); + pb_istream_t istream = pb_istream_from_buffer(buf, size); pb_decode_delimited(&istream, Command_fields, &cmd); + + if (cmd.has_TurnHead) { + servo.write(90 + cmd.TurnHead); + } + if (cmd.has_Center) { + servo.write(90); + } } void env() { @@ -80,15 +87,15 @@ void env() { evt.Distance = distance(); evt.has_Distance = evt.Distance > 0; - evt.SensorA = digitalRead(S1); - evt.has_SensorA = true; + evt.SensorR = !digitalRead(SR); + evt.has_SensorR = true; - evt.SensorB = digitalRead(S2); - evt.has_SensorB = true; - - evt.SensorC = digitalRead(S3); + evt.SensorC = !digitalRead(SC); evt.has_SensorC = true; + evt.SensorL = !digitalRead(SL); + evt.has_SensorL = true; + evt.KeyPress = ir(); evt.has_KeyPress = evt.KeyPress != 0; @@ -101,6 +108,7 @@ void env() { void loop() { env(); serial.update(); + delay(200); } void setup() { @@ -121,9 +129,9 @@ void setup() { pinMode(ENB, OUTPUT); pinMode(LED, OUTPUT); - pinMode(S1, INPUT); - pinMode(S2, INPUT); - pinMode(S3, INPUT); + pinMode(SR, INPUT); + pinMode(SC, INPUT); + pinMode(SL, INPUT); pinMode(IR, INPUT); irrecv.enableIRIn(); diff --git a/car/config.h b/car/config.h index ae99c2a..af497b1 100644 --- a/car/config.h +++ b/car/config.h @@ -17,9 +17,9 @@ #define IN4 10 // 6 // Pinout sensors -#define S1 2 -#define S2 4 -#define S3 11 +#define SR 2 // right +#define SC 4 // center +#define SL 11 // left // Servo #define SRV 3 diff --git a/car/elegoo.pb.c b/car/elegoo.pb.c index 9c450b9..570d496 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 Sun Aug 20 22:42:38 2017. */ +/* Generated by nanopb-0.3.6 at Mon Aug 21 21:08:59 2017. */ #include "elegoo.pb.h" @@ -10,19 +10,20 @@ -const pb_field_t Command_fields[4] = { +const pb_field_t Command_fields[5] = { PB_FIELD( 1, INT32 , OPTIONAL, STATIC , FIRST, Command, LeftSpeed, LeftSpeed, 0), PB_FIELD( 2, INT32 , OPTIONAL, STATIC , OTHER, Command, RightSpeed, LeftSpeed, 0), - PB_FIELD( 3, INT32 , OPTIONAL, STATIC , OTHER, Command, TurnHead, RightSpeed, 0), + PB_FIELD( 3, SINT32 , OPTIONAL, STATIC , OTHER, Command, TurnHead, RightSpeed, 0), + PB_FIELD( 4, BOOL , OPTIONAL, STATIC , OTHER, Command, Center, TurnHead, 0), PB_LAST_FIELD }; const pb_field_t Event_fields[6] = { PB_FIELD( 1, INT32 , OPTIONAL, STATIC , FIRST, Event, Distance, Distance, 0), - PB_FIELD( 2, BOOL , OPTIONAL, STATIC , OTHER, Event, SensorA, Distance, 0), - PB_FIELD( 3, BOOL , OPTIONAL, STATIC , OTHER, Event, SensorB, SensorA, 0), - PB_FIELD( 4, BOOL , OPTIONAL, STATIC , OTHER, Event, SensorC, SensorB, 0), - PB_FIELD( 5, UINT32 , OPTIONAL, STATIC , OTHER, Event, KeyPress, SensorC, 0), + PB_FIELD( 2, BOOL , OPTIONAL, STATIC , OTHER, Event, SensorR, Distance, 0), + PB_FIELD( 3, BOOL , OPTIONAL, STATIC , OTHER, Event, SensorC, SensorR, 0), + PB_FIELD( 4, BOOL , OPTIONAL, STATIC , OTHER, Event, SensorL, SensorC, 0), + PB_FIELD( 5, UINT32 , OPTIONAL, STATIC , OTHER, Event, KeyPress, SensorL, 0), PB_LAST_FIELD }; diff --git a/car/elegoo.pb.h b/car/elegoo.pb.h index d353a56..29865ad 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 Sun Aug 20 22:42:38 2017. */ +/* Generated by nanopb-0.3.6 at Mon Aug 21 21:08:59 2017. */ #ifndef PB_ELEGOO_PB_H_INCLUDED #define PB_ELEGOO_PB_H_INCLUDED @@ -22,18 +22,20 @@ typedef struct _Command { int32_t RightSpeed; bool has_TurnHead; int32_t TurnHead; + bool has_Center; + bool Center; /* @@protoc_insertion_point(struct:Command) */ } Command; typedef struct _Event { bool has_Distance; int32_t Distance; - bool has_SensorA; - bool SensorA; - bool has_SensorB; - bool SensorB; + bool has_SensorR; + bool SensorR; bool has_SensorC; bool SensorC; + bool has_SensorL; + bool SensorL; bool has_KeyPress; uint32_t KeyPress; /* @@protoc_insertion_point(struct:Event) */ @@ -42,27 +44,28 @@ typedef struct _Event { /* Default values for struct fields */ /* Initializer values for message structs */ -#define Command_init_default {false, 0, false, 0, false, 0} +#define Command_init_default {false, 0, false, 0, false, 0, false, 0} #define Event_init_default {false, 0, false, 0, false, 0, false, 0, false, 0} -#define Command_init_zero {false, 0, false, 0, false, 0} +#define Command_init_zero {false, 0, false, 0, false, 0, false, 0} #define Event_init_zero {false, 0, false, 0, false, 0, false, 0, false, 0} /* Field tags (for use in manual encoding/decoding) */ #define Command_LeftSpeed_tag 1 #define Command_RightSpeed_tag 2 #define Command_TurnHead_tag 3 +#define Command_Center_tag 4 #define Event_Distance_tag 1 -#define Event_SensorA_tag 2 -#define Event_SensorB_tag 3 -#define Event_SensorC_tag 4 +#define Event_SensorR_tag 2 +#define Event_SensorC_tag 3 +#define Event_SensorL_tag 4 #define Event_KeyPress_tag 5 /* Struct field encoding specification for nanopb */ -extern const pb_field_t Command_fields[4]; +extern const pb_field_t Command_fields[5]; extern const pb_field_t Event_fields[6]; /* Maximum encoded size of messages (where known) */ -#define Command_size 33 +#define Command_size 30 #define Event_size 23 /* Message IDs (where set with "msgid" option) */ diff --git a/car/elegoo/elegoo.pb.go b/car/elegoo/elegoo.pb.go index e07627e..b29cb8f 100644 --- a/car/elegoo/elegoo.pb.go +++ b/car/elegoo/elegoo.pb.go @@ -31,7 +31,8 @@ const _ = proto.ProtoPackageIsVersion2 // please upgrade the proto package type Command struct { LeftSpeed int32 `protobuf:"varint,1,opt,name=LeftSpeed" json:"LeftSpeed,omitempty"` RightSpeed int32 `protobuf:"varint,2,opt,name=RightSpeed" json:"RightSpeed,omitempty"` - TurnHead int32 `protobuf:"varint,3,opt,name=TurnHead" json:"TurnHead,omitempty"` + TurnHead int32 `protobuf:"zigzag32,3,opt,name=TurnHead" json:"TurnHead,omitempty"` + Center bool `protobuf:"varint,4,opt,name=Center" json:"Center,omitempty"` } func (m *Command) Reset() { *m = Command{} } @@ -60,11 +61,18 @@ func (m *Command) GetTurnHead() int32 { return 0 } +func (m *Command) GetCenter() bool { + if m != nil { + return m.Center + } + return false +} + type Event struct { Distance int32 `protobuf:"varint,1,opt,name=Distance" json:"Distance,omitempty"` - SensorA bool `protobuf:"varint,2,opt,name=SensorA" json:"SensorA,omitempty"` - SensorB bool `protobuf:"varint,3,opt,name=SensorB" json:"SensorB,omitempty"` - SensorC bool `protobuf:"varint,4,opt,name=SensorC" json:"SensorC,omitempty"` + SensorR bool `protobuf:"varint,2,opt,name=SensorR" json:"SensorR,omitempty"` + SensorC bool `protobuf:"varint,3,opt,name=SensorC" json:"SensorC,omitempty"` + SensorL bool `protobuf:"varint,4,opt,name=SensorL" json:"SensorL,omitempty"` KeyPress uint32 `protobuf:"varint,5,opt,name=KeyPress" json:"KeyPress,omitempty"` } @@ -80,23 +88,23 @@ func (m *Event) GetDistance() int32 { return 0 } -func (m *Event) GetSensorA() bool { +func (m *Event) GetSensorR() bool { if m != nil { - return m.SensorA + return m.SensorR } return false } -func (m *Event) GetSensorB() bool { +func (m *Event) GetSensorC() bool { if m != nil { - return m.SensorB + return m.SensorC } return false } -func (m *Event) GetSensorC() bool { +func (m *Event) GetSensorL() bool { if m != nil { - return m.SensorC + return m.SensorL } return false } @@ -116,17 +124,19 @@ func init() { func init() { proto.RegisterFile("elegoo.proto", fileDescriptor0) } var fileDescriptor0 = []byte{ - // 192 bytes of a gzipped FileDescriptorProto - 0x1f, 0x8b, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0xff, 0xe2, 0xe2, 0x49, 0xcd, 0x49, 0x4d, - 0xcf, 0xcf, 0xd7, 0x2b, 0x28, 0xca, 0x2f, 0xc9, 0x57, 0x4a, 0xe6, 0x62, 0x77, 0xce, 0xcf, 0xcd, - 0x4d, 0xcc, 0x4b, 0x11, 0x92, 0xe1, 0xe2, 0xf4, 0x49, 0x4d, 0x2b, 0x09, 0x2e, 0x48, 0x4d, 0x4d, - 0x91, 0x60, 0x54, 0x60, 0xd4, 0x60, 0x0d, 0x42, 0x08, 0x08, 0xc9, 0x71, 0x71, 0x05, 0x65, 0xa6, - 0x67, 0x40, 0xa5, 0x99, 0xc0, 0xd2, 0x48, 0x22, 0x42, 0x52, 0x5c, 0x1c, 0x21, 0xa5, 0x45, 0x79, - 0x1e, 0xa9, 0x89, 0x29, 0x12, 0xcc, 0x60, 0x59, 0x38, 0x5f, 0xa9, 0x97, 0x91, 0x8b, 0xd5, 0xb5, - 0x2c, 0x35, 0xaf, 0x04, 0xa4, 0xca, 0x25, 0xb3, 0xb8, 0x24, 0x31, 0x2f, 0x39, 0x15, 0x6a, 0x05, - 0x9c, 0x2f, 0x24, 0xc1, 0xc5, 0x1e, 0x9c, 0x9a, 0x57, 0x9c, 0x5f, 0xe4, 0x08, 0x36, 0x9e, 0x23, - 0x08, 0xc6, 0x45, 0xc8, 0x38, 0x81, 0x8d, 0x86, 0xcb, 0x38, 0x21, 0x64, 0x9c, 0x25, 0x58, 0x90, - 0x65, 0x9c, 0x41, 0x36, 0x79, 0xa7, 0x56, 0x06, 0x14, 0xa5, 0x16, 0x17, 0x4b, 0xb0, 0x2a, 0x30, - 0x6a, 0xf0, 0x06, 0xc1, 0xf9, 0x4e, 0x6c, 0x51, 0x2c, 0xb9, 0x89, 0x99, 0x79, 0x49, 0x6c, 0xe0, - 0x30, 0x30, 0x06, 0x04, 0x00, 0x00, 0xff, 0xff, 0xb3, 0x2f, 0x3b, 0x79, 0x13, 0x01, 0x00, 0x00, + // 209 bytes of a gzipped FileDescriptorProto + 0x1f, 0x8b, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0xff, 0x4c, 0x90, 0xbd, 0x4a, 0xc0, 0x30, + 0x14, 0x46, 0x89, 0xf6, 0xcf, 0x8b, 0x0e, 0x66, 0x90, 0x20, 0x22, 0xa1, 0x53, 0x26, 0x17, 0xdf, + 0xc0, 0x28, 0x08, 0x76, 0x90, 0xd4, 0xc9, 0x2d, 0xda, 0x6b, 0x2d, 0xd8, 0xa4, 0x24, 0x51, 0x10, + 0x9f, 0xc1, 0x77, 0x96, 0x86, 0x34, 0x75, 0x3c, 0xdf, 0xe1, 0x72, 0xe0, 0xc2, 0x31, 0x7e, 0xe0, + 0x68, 0xed, 0xd5, 0xe2, 0x6c, 0xb0, 0xed, 0x0f, 0xd4, 0xd2, 0xce, 0xb3, 0x36, 0x03, 0xbd, 0x80, + 0xa3, 0x0e, 0xdf, 0x42, 0xbf, 0x20, 0x0e, 0x8c, 0x70, 0x22, 0x4a, 0xb5, 0x0f, 0xf4, 0x12, 0x40, + 0x4d, 0xe3, 0x7b, 0xd2, 0x07, 0x51, 0xff, 0x5b, 0xe8, 0x39, 0x34, 0x4f, 0x9f, 0xce, 0xdc, 0xa3, + 0x1e, 0xd8, 0x21, 0x27, 0xe2, 0x54, 0x65, 0xa6, 0x67, 0x50, 0x49, 0x34, 0x01, 0x1d, 0x2b, 0x38, + 0x11, 0x8d, 0x4a, 0xd4, 0xfe, 0x12, 0x28, 0xef, 0xbe, 0xd0, 0x84, 0xf5, 0xfa, 0x76, 0xf2, 0x41, + 0x9b, 0x57, 0x4c, 0xe9, 0xcc, 0x94, 0x41, 0xdd, 0xa3, 0xf1, 0xd6, 0xa9, 0x98, 0x6d, 0xd4, 0x86, + 0xbb, 0x91, 0x31, 0x99, 0x8d, 0xdc, 0x4d, 0x97, 0x92, 0x1b, 0xae, 0xa5, 0x07, 0xfc, 0x7e, 0x74, + 0xe8, 0x3d, 0x2b, 0x39, 0x11, 0x27, 0x2a, 0xf3, 0x4d, 0xf5, 0x5c, 0xcc, 0x7a, 0x32, 0x2f, 0x55, + 0xfc, 0xcd, 0xf5, 0x5f, 0x00, 0x00, 0x00, 0xff, 0xff, 0x81, 0x6c, 0x06, 0xb3, 0x2b, 0x01, 0x00, + 0x00, } diff --git a/car/elegoo/elegoo.proto b/car/elegoo/elegoo.proto index 2915a11..bb72aac 100644 --- a/car/elegoo/elegoo.proto +++ b/car/elegoo/elegoo.proto @@ -5,13 +5,14 @@ option go_package = "main"; message Command { int32 LeftSpeed = 1; int32 RightSpeed = 2; - int32 TurnHead = 3; + sint32 TurnHead = 3; // ±90 deg + bool Center = 4; } message Event { int32 Distance = 1; - bool SensorA = 2; - bool SensorB = 3; - bool SensorC = 4; + bool SensorR = 2; + bool SensorC = 3; + bool SensorL = 4; uint32 KeyPress = 5; } diff --git a/car/elegoo/main.go b/car/elegoo/main.go index b750922..e73b731 100644 --- a/car/elegoo/main.go +++ b/car/elegoo/main.go @@ -4,10 +4,12 @@ package main //go:generate sh -c "protoc --nanopb_out=.. *.proto" import ( + "bufio" "io" "log" "time" + "github.com/dim13/cobs" "github.com/golang/protobuf/proto" "github.com/tarm/serial" ) @@ -18,19 +20,19 @@ func Write(w io.Writer, pb proto.Message) error { if err != nil { return err } - log.Println("write: % x", buf.Bytes()) // DEBUG - _, err = w.Write(buf.Bytes()) + log.Printf("write: % x", buf.Bytes()) // DEBUG + block := cobs.Encode(buf.Bytes()) + _, err = w.Write(block) return err } -func Read(r io.Reader, pb proto.Message) error { - buf := make([]byte, 80) - n, err := r.Read(buf) +func Read(buf *bufio.Reader, pb proto.Message) error { + block, err := buf.ReadBytes(0) if err != nil { return err } - log.Println("read: % x", buf[:n]) // DEBUG - return proto.NewBuffer(buf[:n]).DecodeMessage(pb) + block = cobs.Decode(block) + return proto.NewBuffer(block).DecodeMessage(pb) } // /dev/cu.Elegoo-DevB @@ -38,7 +40,7 @@ func Read(r io.Reader, pb proto.Message) error { func main() { c := &serial.Config{ - Name: "/dev/cu.usbmodem1421", + Name: "/dev/tty.usbmodem1411", Baud: 57600, } s, err := serial.OpenPort(c) @@ -47,16 +49,24 @@ func main() { } defer s.Close() - //cmd := &Command{} - //Write(s, cmd) - go func() { - for { - evt := &Event{} - Read(s, evt) - log.Println(evt) + cmd := &Command{} + for i := -45; i < 45; i += 5 { + cmd.TurnHead = int32(i) + Write(s, cmd) + time.Sleep(time.Second / 2) } + cmd.TurnHead = 0 + cmd.Center = true + Write(s, cmd) }() - time.Sleep(time.Minute) + 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