From 22bec7ba538aa7619bd4f742684c2007d1fe9e7c Mon Sep 17 00:00:00 2001 From: Dimitri Sokolyuk Date: Mon, 21 Aug 2017 23:06:34 +0200 Subject: motor commands --- car/car.ino | 43 +++++++++++++++++++++++------------- car/elegoo.pb.c | 15 +++++++------ car/elegoo.pb.h | 33 +++++++++++++++------------- car/elegoo/elegoo.pb.go | 58 ++++++++++++++++++++++++++++--------------------- car/elegoo/elegoo.proto | 11 +++++----- car/elegoo/main.go | 30 +++++++++++++++++++++++-- 6 files changed, 121 insertions(+), 69 deletions(-) (limited to 'car') diff --git a/car/car.ino b/car/car.ino index b048db1..ee5c84f 100644 --- a/car/car.ino +++ b/car/car.ino @@ -14,7 +14,9 @@ PacketSerial serial; Servo servo; + IRrecv irrecv(IR); +decode_results ir; void motor(int e, int a, int b, int v) { if (v > 0) { @@ -29,6 +31,14 @@ 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); +} + void move(int l, int r) { motor(ENA, IN1, IN2, l); motor(ENB, IN3, IN4, r); @@ -57,20 +67,22 @@ void look(int deg) { #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 onPacket(const uint8_t* buf, size_t size) { Command cmd = Command_init_zero; pb_istream_t istream = pb_istream_from_buffer(buf, size); pb_decode_delimited(&istream, Command_fields, &cmd); + if (cmd.has_SpeedR) { + motorR(cmd.SpeedR); + } + if (cmd.has_SpeedL) { + motorL(cmd.SpeedL); + } + if (cmd.has_Stop && cmd.Stop) { + motorR(0); + motorL(0); + } if (cmd.has_TurnHead) { servo.write(90 + cmd.TurnHead); } @@ -87,17 +99,20 @@ void env() { evt.Distance = distance(); evt.has_Distance = evt.Distance > 0; - evt.SensorR = !digitalRead(SR); + evt.SensorR = digitalRead(SR); evt.has_SensorR = true; - evt.SensorC = !digitalRead(SC); + evt.SensorC = digitalRead(SC); evt.has_SensorC = true; - evt.SensorL = !digitalRead(SL); + evt.SensorL = digitalRead(SL); evt.has_SensorL = true; - evt.KeyPress = ir(); - evt.has_KeyPress = evt.KeyPress != 0; + if (irrecv.decode(&ir)) { + evt.KeyPress = ir.value; + evt.has_KeyPress = ir.bits > 0; + irrecv.resume(); + } pb_ostream_t ostream = pb_ostream_from_buffer(buf, sizeof(buf)); pb_encode_delimited(&ostream, Event_fields, &evt); @@ -114,8 +129,6 @@ void loop() { void setup() { serial.begin(57600); serial.setPacketHandler(&onPacket); - //pb_istream_from_stream(Serial, istream); - //pb_ostream_from_stream(Serial, ostream); pinMode(Echo, INPUT); pinMode(Trig, OUTPUT); diff --git a/car/elegoo.pb.c b/car/elegoo.pb.c index 570d496..5af0f71 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 Mon Aug 21 21:08:59 2017. */ +/* Generated by nanopb-0.3.6 at Mon Aug 21 22:55:02 2017. */ #include "elegoo.pb.h" @@ -10,16 +10,17 @@ -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, SINT32 , OPTIONAL, STATIC , OTHER, Command, TurnHead, RightSpeed, 0), - PB_FIELD( 4, BOOL , OPTIONAL, STATIC , OTHER, Command, Center, TurnHead, 0), +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, SINT32 , OPTIONAL, STATIC , OTHER, Command, TurnHead, Stop, 0), + PB_FIELD( 5, 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( 1, UINT32 , OPTIONAL, STATIC , FIRST, Event, Distance, Distance, 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), diff --git a/car/elegoo.pb.h b/car/elegoo.pb.h index 29865ad..4a0e918 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 Mon Aug 21 21:08:59 2017. */ +/* Generated by nanopb-0.3.6 at Mon Aug 21 22:55:02 2017. */ #ifndef PB_ELEGOO_PB_H_INCLUDED #define PB_ELEGOO_PB_H_INCLUDED @@ -16,10 +16,12 @@ extern "C" { /* Struct definitions */ typedef struct _Command { - bool has_LeftSpeed; - int32_t LeftSpeed; - bool has_RightSpeed; - int32_t RightSpeed; + bool has_SpeedR; + int32_t SpeedR; + bool has_SpeedL; + int32_t SpeedL; + bool has_Stop; + bool Stop; bool has_TurnHead; int32_t TurnHead; bool has_Center; @@ -29,7 +31,7 @@ typedef struct _Command { typedef struct _Event { bool has_Distance; - int32_t Distance; + uint32_t Distance; bool has_SensorR; bool SensorR; bool has_SensorC; @@ -44,16 +46,17 @@ typedef struct _Event { /* Default values for struct fields */ /* Initializer values for message structs */ -#define Command_init_default {false, 0, false, 0, false, 0, false, 0} +#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} -#define Command_init_zero {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} /* 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 Command_SpeedR_tag 1 +#define Command_SpeedL_tag 2 +#define Command_Stop_tag 3 +#define Command_TurnHead_tag 4 +#define Command_Center_tag 5 #define Event_Distance_tag 1 #define Event_SensorR_tag 2 #define Event_SensorC_tag 3 @@ -61,12 +64,12 @@ typedef struct _Event { #define Event_KeyPress_tag 5 /* Struct field encoding specification for nanopb */ -extern const pb_field_t Command_fields[5]; +extern const pb_field_t Command_fields[6]; extern const pb_field_t Event_fields[6]; /* Maximum encoded size of messages (where known) */ -#define Command_size 30 -#define Event_size 23 +#define Command_size 22 +#define Event_size 18 /* 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 b29cb8f..a0e9704 100644 --- a/car/elegoo/elegoo.pb.go +++ b/car/elegoo/elegoo.pb.go @@ -29,10 +29,11 @@ var _ = math.Inf 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:"zigzag32,3,opt,name=TurnHead" json:"TurnHead,omitempty"` - Center bool `protobuf:"varint,4,opt,name=Center" json:"Center,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"` + TurnHead int32 `protobuf:"zigzag32,4,opt,name=TurnHead" json:"TurnHead,omitempty"` + Center bool `protobuf:"varint,5,opt,name=Center" json:"Center,omitempty"` } func (m *Command) Reset() { *m = Command{} } @@ -40,20 +41,27 @@ func (m *Command) String() string { return proto.CompactTextString(m) func (*Command) ProtoMessage() {} func (*Command) Descriptor() ([]byte, []int) { return fileDescriptor0, []int{0} } -func (m *Command) GetLeftSpeed() int32 { +func (m *Command) GetSpeedR() int32 { if m != nil { - return m.LeftSpeed + return m.SpeedR } return 0 } -func (m *Command) GetRightSpeed() int32 { +func (m *Command) GetSpeedL() int32 { if m != nil { - return m.RightSpeed + return m.SpeedL } return 0 } +func (m *Command) GetStop() bool { + if m != nil { + return m.Stop + } + return false +} + func (m *Command) GetTurnHead() int32 { if m != nil { return m.TurnHead @@ -69,7 +77,7 @@ func (m *Command) GetCenter() bool { } type Event struct { - Distance int32 `protobuf:"varint,1,opt,name=Distance" json:"Distance,omitempty"` + Distance uint32 `protobuf:"varint,1,opt,name=Distance" json:"Distance,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"` @@ -81,7 +89,7 @@ func (m *Event) String() string { return proto.CompactTextString(m) } func (*Event) ProtoMessage() {} func (*Event) Descriptor() ([]byte, []int) { return fileDescriptor0, []int{1} } -func (m *Event) GetDistance() int32 { +func (m *Event) GetDistance() uint32 { if m != nil { return m.Distance } @@ -124,19 +132,19 @@ func init() { func init() { proto.RegisterFile("elegoo.proto", fileDescriptor0) } var fileDescriptor0 = []byte{ - // 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, + // 212 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, 0x6a, 0x64, 0xe4, 0x62, 0x77, 0xce, 0xcf, + 0xcd, 0x4d, 0xcc, 0x4b, 0x11, 0x12, 0xe3, 0x62, 0x0b, 0x2e, 0x48, 0x4d, 0x4d, 0x09, 0x92, 0x60, + 0x54, 0x60, 0xd4, 0x10, 0x0c, 0x82, 0xf2, 0xe0, 0xe2, 0x3e, 0x12, 0x4c, 0x48, 0xe2, 0x3e, 0x42, + 0x42, 0x5c, 0x2c, 0xc1, 0x25, 0xf9, 0x05, 0x12, 0xcc, 0x0a, 0x8c, 0x1a, 0x1c, 0x41, 0x60, 0xb6, + 0x90, 0x14, 0x17, 0x47, 0x48, 0x69, 0x51, 0x9e, 0x47, 0x6a, 0x62, 0x8a, 0x04, 0x0b, 0x58, 0x35, + 0x9c, 0x0f, 0x32, 0xc7, 0x39, 0x35, 0xaf, 0x24, 0xb5, 0x48, 0x82, 0x15, 0xac, 0x03, 0xca, 0x53, + 0xea, 0x65, 0xe4, 0x62, 0x75, 0x2d, 0x4b, 0xcd, 0x2b, 0x01, 0xe9, 0x76, 0xc9, 0x2c, 0x2e, 0x49, + 0xcc, 0x4b, 0x4e, 0x05, 0xbb, 0x81, 0x37, 0x08, 0xce, 0x17, 0x92, 0xe0, 0x62, 0x0f, 0x4e, 0xcd, + 0x2b, 0xce, 0x2f, 0x0a, 0x02, 0x3b, 0x83, 0x23, 0x08, 0xc6, 0x45, 0xc8, 0x38, 0x43, 0x9d, 0x02, + 0xe3, 0x22, 0x64, 0x7c, 0xc0, 0x8e, 0x81, 0xcb, 0xf8, 0x80, 0x6c, 0xf2, 0x4e, 0xad, 0x0c, 0x28, + 0x4a, 0x2d, 0x2e, 0x06, 0xbb, 0x86, 0x37, 0x08, 0xce, 0x77, 0x62, 0x8b, 0x62, 0xc9, 0x4d, 0xcc, + 0xcc, 0x4b, 0x62, 0x03, 0x07, 0x91, 0x31, 0x20, 0x00, 0x00, 0xff, 0xff, 0x8f, 0x9d, 0x88, 0x7d, + 0x32, 0x01, 0x00, 0x00, } diff --git a/car/elegoo/elegoo.proto b/car/elegoo/elegoo.proto index bb72aac..f310b62 100644 --- a/car/elegoo/elegoo.proto +++ b/car/elegoo/elegoo.proto @@ -3,14 +3,15 @@ syntax = "proto3"; option go_package = "main"; message Command { - int32 LeftSpeed = 1; - int32 RightSpeed = 2; - sint32 TurnHead = 3; // ±90 deg - bool Center = 4; + sint32 SpeedR = 1; + sint32 SpeedL = 2; + bool Stop = 3; + sint32 TurnHead = 4; // ±90 deg + bool Center = 5; } message Event { - int32 Distance = 1; + uint32 Distance = 1; bool SensorR = 2; bool SensorC = 3; bool SensorL = 4; diff --git a/car/elegoo/main.go b/car/elegoo/main.go index e73b731..900b584 100644 --- a/car/elegoo/main.go +++ b/car/elegoo/main.go @@ -20,7 +20,6 @@ func Write(w io.Writer, pb proto.Message) error { if err != nil { return err } - log.Printf("write: % x", buf.Bytes()) // DEBUG block := cobs.Encode(buf.Bytes()) _, err = w.Write(block) return err @@ -37,10 +36,11 @@ func Read(buf *bufio.Reader, pb proto.Message) error { // /dev/cu.Elegoo-DevB // /dev/cu.usbmodem1421 +// /dev/cu.usbmodem1411 func main() { c := &serial.Config{ - Name: "/dev/tty.usbmodem1411", + Name: "/dev/tty.usbmodem1421", Baud: 57600, } s, err := serial.OpenPort(c) @@ -59,6 +59,32 @@ func main() { cmd.TurnHead = 0 cmd.Center = true Write(s, cmd) + + /* MOTOR + cmd.SpeedL = 200 + cmd.SpeedR = 0 + cmd.Stop = false + Write(s, cmd) + time.Sleep(3 * time.Second) + + cmd.SpeedL = 0 + cmd.SpeedR = 0 + cmd.Stop = true + Write(s, cmd) + time.Sleep(3 * time.Second) + + cmd.SpeedL = 0 + cmd.SpeedR = 200 + cmd.Stop = false + Write(s, cmd) + time.Sleep(3 * time.Second) + + cmd.SpeedL = 0 + cmd.SpeedR = 0 + cmd.Stop = true + Write(s, cmd) + */ + }() buf := bufio.NewReader(s) -- cgit v1.2.3