From acaf7f81d910a60275d6d6fb77511085892b35e7 Mon Sep 17 00:00:00 2001 From: Dimitri Sokolyuk Date: Sat, 26 Aug 2017 18:06:51 +0200 Subject: ... --- car/car.ino | 7 ++----- car/elegoo.pb.c | 9 ++++----- car/elegoo.pb.h | 19 ++++++++----------- car/elegoo/elegoo.pb.go | 44 ++++++++++++++++++-------------------------- car/elegoo/elegoo.proto | 15 +++++++-------- car/elegoo/main.go | 24 ++++++++++-------------- 6 files changed, 49 insertions(+), 69 deletions(-) diff --git a/car/car.ino b/car/car.ino index b33cd8d..b5d7209 100644 --- a/car/car.ino +++ b/car/car.ino @@ -35,8 +35,8 @@ void motor(int e, int a, int b, int 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) +#define look(v) servo.write(v) +#define looking() (servo.read()) int distance() { digitalWrite(Trig, LOW); @@ -70,9 +70,6 @@ void onPacket(const uint8_t* buf, size_t size) { if (cmd.has_Direction) { look(cmd.Direction); } - if (cmd.has_Center) { - look(0); - } if (cmd.has_StopAfter) { timer.after(cmd.StopAfter, stop); } diff --git a/car/elegoo.pb.c b/car/elegoo.pb.c index a4e7ad4..7aa3b5e 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 Sat Aug 26 10:18:35 2017. */ +/* Generated by nanopb-0.3.6 at Sat Aug 26 18:04:10 2017. */ #include "elegoo.pb.h" @@ -10,13 +10,12 @@ -const pb_field_t Command_fields[7] = { +const pb_field_t Command_fields[6] = { 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_FIELD( 4, UINT32 , OPTIONAL, STATIC , OTHER, Command, Direction, Stop, 0), + PB_FIELD( 5, UINT32 , OPTIONAL, STATIC , OTHER, Command, StopAfter, Direction, 0), PB_LAST_FIELD }; diff --git a/car/elegoo.pb.h b/car/elegoo.pb.h index 661bbbc..0f4c4e1 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 Sat Aug 26 10:18:35 2017. */ +/* Generated by nanopb-0.3.6 at Sat Aug 26 18:04:10 2017. */ #ifndef PB_ELEGOO_PB_H_INCLUDED #define PB_ELEGOO_PB_H_INCLUDED @@ -22,10 +22,8 @@ typedef struct _Command { int32_t SpeedL; bool has_Stop; bool Stop; - bool has_Center; - bool Center; bool has_Direction; - int32_t Direction; + uint32_t Direction; bool has_StopAfter; uint32_t StopAfter; /* @@protoc_insertion_point(struct:Command) */ @@ -52,18 +50,17 @@ typedef struct _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, false, 0} +#define Command_init_default {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 Command_init_zero {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 #define Command_SpeedL_tag 2 #define Command_Stop_tag 3 -#define Command_Center_tag 4 -#define Command_Direction_tag 5 -#define Command_StopAfter_tag 6 +#define Command_Direction_tag 4 +#define Command_StopAfter_tag 5 #define Events_Distance_tag 1 #define Events_Direction_tag 2 #define Events_SensorR_tag 3 @@ -73,11 +70,11 @@ typedef struct _Events { #define Events_Time_tag 7 /* Struct field encoding specification for nanopb */ -extern const pb_field_t Command_fields[7]; +extern const pb_field_t Command_fields[6]; extern const pb_field_t Events_fields[8]; /* Maximum encoded size of messages (where known) */ -#define Command_size 28 +#define Command_size 26 #define Events_size 30 /* Message IDs (where set with "msgid" option) */ diff --git a/car/elegoo/elegoo.pb.go b/car/elegoo/elegoo.pb.go index f453f44..3a367e0 100644 --- a/car/elegoo/elegoo.pb.go +++ b/car/elegoo/elegoo.pb.go @@ -32,9 +32,8 @@ 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"` - StopAfter uint32 `protobuf:"varint,6,opt,name=StopAfter" json:"StopAfter,omitempty"` + Direction uint32 `protobuf:"varint,4,opt,name=Direction" json:"Direction,omitempty"` + StopAfter uint32 `protobuf:"varint,5,opt,name=StopAfter" json:"StopAfter,omitempty"` } func (m *Command) Reset() { *m = Command{} } @@ -63,14 +62,7 @@ func (m *Command) GetStop() bool { return false } -func (m *Command) GetCenter() bool { - if m != nil { - return m.Center - } - return false -} - -func (m *Command) GetDirection() int32 { +func (m *Command) GetDirection() uint32 { if m != nil { return m.Direction } @@ -156,20 +148,20 @@ func init() { func init() { proto.RegisterFile("elegoo.proto", fileDescriptor0) } var fileDescriptor0 = []byte{ - // 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, + // 234 bytes of a gzipped FileDescriptorProto + 0x1f, 0x8b, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0xff, 0x54, 0x90, 0xb1, 0x4e, 0xc3, 0x30, + 0x10, 0x86, 0x65, 0x08, 0x4e, 0x39, 0xd1, 0x01, 0x0f, 0xe8, 0x84, 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, + 0x85, 0xc4, 0x23, 0xf0, 0x56, 0x3c, 0x1a, 0xb2, 0xd3, 0x3a, 0x61, 0xbb, 0xff, 0xff, 0x2c, 0xdd, + 0xe7, 0x83, 0x0b, 0xfa, 0xa4, 0x0f, 0xe6, 0xdb, 0xd1, 0x71, 0xe0, 0xcd, 0x8f, 0x80, 0xb2, 0xe1, + 0x61, 0xe8, 0xed, 0x5e, 0x5d, 0x81, 0xec, 0x46, 0xa2, 0xbd, 0x46, 0x51, 0x89, 0xfa, 0x52, 0x1f, + 0x52, 0xee, 0x5b, 0x3c, 0x59, 0xf4, 0xad, 0x52, 0x50, 0x74, 0x81, 0x47, 0x3c, 0xad, 0x44, 0xbd, + 0xd2, 0x69, 0x56, 0x37, 0x70, 0xbe, 0x35, 0x8e, 0x76, 0xc1, 0xb0, 0xc5, 0xa2, 0x12, 0xf5, 0x5a, + 0xcf, 0x45, 0xa4, 0xf1, 0xd5, 0xfd, 0x7b, 0x20, 0x87, 0x67, 0x13, 0xcd, 0xc5, 0xe6, 0x57, 0x80, + 0x7c, 0xfc, 0x22, 0x1b, 0xbc, 0xba, 0x86, 0xd5, 0xd6, 0xf8, 0xd0, 0xdb, 0x1d, 0x25, 0x99, 0xb5, + 0xce, 0xf9, 0xff, 0x8a, 0xc9, 0x68, 0xb1, 0x02, 0xa1, 0xec, 0xc8, 0x7a, 0x76, 0xfa, 0xe0, 0x75, + 0x8c, 0x33, 0x69, 0x92, 0x58, 0x26, 0xcd, 0x4c, 0xda, 0x24, 0x95, 0x49, 0x1b, 0x3d, 0x9e, 0xe8, + 0xfb, 0xd9, 0x91, 0xf7, 0x28, 0x27, 0x8f, 0x63, 0x8e, 0xdf, 0x7f, 0x31, 0x03, 0x61, 0x99, 0xfa, + 0x34, 0x3f, 0xc8, 0xd7, 0x62, 0xe8, 0x8d, 0x7d, 0x93, 0xe9, 0xba, 0x77, 0x7f, 0x01, 0x00, 0x00, + 0xff, 0xff, 0x22, 0x73, 0x73, 0x8b, 0x6d, 0x01, 0x00, 0x00, } diff --git a/car/elegoo/elegoo.proto b/car/elegoo/elegoo.proto index a25f592..7cfd826 100644 --- a/car/elegoo/elegoo.proto +++ b/car/elegoo/elegoo.proto @@ -5,20 +5,19 @@ syntax = "proto3"; option go_package = "main"; message Command { - sint32 SpeedR = 1; - sint32 SpeedL = 2; + sint32 SpeedR = 1; // -255 .. +255 + sint32 SpeedL = 2; // -255 .. +255 bool Stop = 3; - bool Center = 4; - sint32 Direction = 5; // +90 left; -90 right - uint32 StopAfter = 6; // milliseconds + uint32 Direction = 4; // deg: 0 right .. 180 left + uint32 StopAfter = 5; // milliseconds } message Events { - uint32 Distance = 1; - sint32 Direction = 2; + uint32 Distance = 1; // cm + sint32 Direction = 2; // deg bool SensorR = 3; bool SensorC = 4; bool SensorL = 5; uint32 KeyPress = 6; - uint32 Time = 7; + uint32 Time = 7; // milliseconds } diff --git a/car/elegoo/main.go b/car/elegoo/main.go index 46d3058..3d7fcde 100644 --- a/car/elegoo/main.go +++ b/car/elegoo/main.go @@ -39,9 +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", + //Name: "/dev/tty.Elegoo-DevB", Baud: 57600, } s, err := serial.OpenPort(c) @@ -64,25 +64,20 @@ func main() { }() log.Println("send -45") - time.Sleep(time.Second) - Write(s, &Command{Direction: -45}) + time.Sleep(3 * time.Second) + Write(s, &Command{Direction: 5}) log.Println("send +45") - time.Sleep(time.Second) - Write(s, &Command{Direction: 45}) - - log.Println("send center") - time.Sleep(time.Second) - Write(s, &Command{Center: true}) + time.Sleep(3 * time.Second) + Write(s, &Command{Direction: 175}) - log.Println("send -5") - time.Sleep(time.Second) - Write(s, &Command{Direction: -5}) + log.Println("send +0") + time.Sleep(3 * time.Second) + Write(s, &Command{Direction: 90}) /* 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) @@ -91,6 +86,7 @@ func main() { log.Println("send motor turn") time.Sleep(time.Second) Write(s, &Command{SpeedL: 250, SpeedR: -250, StopAfter: 500}) + */ time.Sleep(time.Minute) -- cgit v1.2.3