aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorDimitri Sokolyuk <demon@dim13.org>2017-01-25 02:50:39 +0100
committerDimitri Sokolyuk <demon@dim13.org>2017-01-25 02:50:39 +0100
commit427998e3506be484caeec94ab720cefbfebf35f2 (patch)
tree2c7a05f7b4cec72f20b8473d9f8dba5daea8f7f3
parent5ba39ddb1385043ed29cb913597e4e285e457273 (diff)
wip
-rw-r--r--car/car.ino19
-rw-r--r--car/common.cpp13
-rw-r--r--car/common.h10
-rw-r--r--car/config.h2
-rw-r--r--car/elegoo.pb.c58
-rw-r--r--car/elegoo.pb.h89
-rw-r--r--car/elegoo.proto20
-rw-r--r--car/elegoo/elegoo.pb.go143
-rw-r--r--car/elegoo/gen.go4
-rw-r--r--car/elegoo/main.go78
10 files changed, 431 insertions, 5 deletions
diff --git a/car/car.ino b/car/car.ino
index c36e0c3..68c07f1 100644
--- a/car/car.ino
+++ b/car/car.ino
@@ -6,6 +6,8 @@
//#include <os48.h>
#include "config.h"
#include "ir.h"
+#include "common.h"
+#include "elegoo.pb.h"
Servo head;
IRrecv irrecv(IR);
@@ -117,6 +119,7 @@ void setup() {
pinMode(ENA, OUTPUT);
pinMode(ENB, OUTPUT);
+ pinMode(LED, OUTPUT);
pinMode(S1, INPUT);
pinMode(S2, INPUT);
@@ -131,7 +134,21 @@ void setup() {
}
void loop() {
+ pb_istream_t input = pb_istream_from_serial();
+ Command cmd = {};
+ //if (Serial.available())
+ pb_decode_delimited(&input, Command_fields, &cmd);
+
+ pb_ostream_t output = pb_ostream_from_serial();
+ Event evt = {};
+ int d = distance();
+ if (d > 0) {
+ evt.Distance = d;
+ pb_encode_delimited(&output, Event_fields, &evt);
+ }
+
+ delay(1000);
//ultra();
- ir();
+ //ir();
}
diff --git a/car/common.cpp b/car/common.cpp
index 90f08d2..7b5522f 100644
--- a/car/common.cpp
+++ b/car/common.cpp
@@ -3,31 +3,36 @@
#include "pb_encode.h"
#include "pb_decode.h"
+#define MAXSZ 32
+
bool
write_callback(pb_ostream_t *stream, const uint8_t *buf, size_t count)
{
- return Serial.write(buf, count) == count;
+ int result = Serial.write(buf, count);
+// Serial.flush();
+ return result == count;
}
bool
read_callback(pb_istream_t *stream, uint8_t *buf, size_t count)
{
int result = Serial.readBytes(buf, count);
- if (result <= 0)
+ if (result == 0) {
stream->bytes_left = 0; // EOF
+ }
return result == count;
}
pb_ostream_t
pb_ostream_from_serial()
{
- pb_ostream_t stream = {&write_callback, NULL, SIZE_MAX, 0};
+ pb_ostream_t stream = {write_callback, NULL, MAXSZ, 0};
return stream;
}
pb_istream_t
pb_istream_from_serial()
{
- pb_istream_t stream = {&read_callback, NULL, SIZE_MAX};
+ pb_istream_t stream = {read_callback, NULL, MAXSZ};
return stream;
}
diff --git a/car/common.h b/car/common.h
new file mode 100644
index 0000000..2192c1c
--- /dev/null
+++ b/car/common.h
@@ -0,0 +1,10 @@
+#ifndef COMMON_H
+#define COMMON_H
+
+#include "pb_encode.h"
+#include "pb_decode.h"
+
+pb_istream_t pb_istream_from_serial();
+pb_ostream_t pb_ostream_from_serial();
+
+#endif
diff --git a/car/config.h b/car/config.h
index 78e542b..10cfefa 100644
--- a/car/config.h
+++ b/car/config.h
@@ -1,6 +1,8 @@
#ifndef CONFIG_H
#define CONFIG_H
+#define LED 13
+
// Settings
#define dist 20
#define velo 130
diff --git a/car/elegoo.pb.c b/car/elegoo.pb.c
new file mode 100644
index 0000000..1a268f4
--- /dev/null
+++ b/car/elegoo.pb.c
@@ -0,0 +1,58 @@
+/* Automatically generated nanopb constant definitions */
+/* Generated by nanopb-0.3.6 at Tue Jan 24 21:09:08 2017. */
+
+#include "elegoo.pb.h"
+
+/* @@protoc_insertion_point(includes) */
+#if PB_PROTO_HEADER_VERSION != 30
+#error Regenerate this file with the current version of nanopb generator.
+#endif
+
+
+
+const pb_field_t Command_fields[3] = {
+ PB_FIELD( 1, MESSAGE , OPTIONAL, STATIC , FIRST, Command, SetSpeed, SetSpeed, &Speed_fields),
+ PB_FIELD( 2, BOOL , OPTIONAL, STATIC , OTHER, Command, Stop, SetSpeed, 0),
+ PB_LAST_FIELD
+};
+
+const pb_field_t Speed_fields[3] = {
+ PB_FIELD( 1, INT32 , OPTIONAL, STATIC , FIRST, Speed, Left, Left, 0),
+ PB_FIELD( 2, INT32 , OPTIONAL, STATIC , OTHER, Speed, Right, Left, 0),
+ PB_LAST_FIELD
+};
+
+const pb_field_t Event_fields[5] = {
+ 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_LAST_FIELD
+};
+
+
+/* Check that field information fits in pb_field_t */
+#if !defined(PB_FIELD_32BIT)
+/* If you get an error here, it means that you need to define PB_FIELD_32BIT
+ * compile-time option. You can do that in pb.h or on compiler command line.
+ *
+ * The reason you need to do this is that some of your messages contain tag
+ * numbers or field sizes that are larger than what can fit in 8 or 16 bit
+ * field descriptors.
+ */
+PB_STATIC_ASSERT((pb_membersize(Command, SetSpeed) < 65536), YOU_MUST_DEFINE_PB_FIELD_32BIT_FOR_MESSAGES_Command_Speed_Event)
+#endif
+
+#if !defined(PB_FIELD_16BIT) && !defined(PB_FIELD_32BIT)
+/* If you get an error here, it means that you need to define PB_FIELD_16BIT
+ * compile-time option. You can do that in pb.h or on compiler command line.
+ *
+ * The reason you need to do this is that some of your messages contain tag
+ * numbers or field sizes that are larger than what can fit in the default
+ * 8 bit descriptors.
+ */
+PB_STATIC_ASSERT((pb_membersize(Command, SetSpeed) < 256), YOU_MUST_DEFINE_PB_FIELD_16BIT_FOR_MESSAGES_Command_Speed_Event)
+#endif
+
+
+/* @@protoc_insertion_point(eof) */
diff --git a/car/elegoo.pb.h b/car/elegoo.pb.h
new file mode 100644
index 0000000..c162007
--- /dev/null
+++ b/car/elegoo.pb.h
@@ -0,0 +1,89 @@
+/* Automatically generated nanopb header */
+/* Generated by nanopb-0.3.6 at Tue Jan 24 21:09:08 2017. */
+
+#ifndef PB_ELEGOO_PB_H_INCLUDED
+#define PB_ELEGOO_PB_H_INCLUDED
+#include "pb.h"
+
+/* @@protoc_insertion_point(includes) */
+#if PB_PROTO_HEADER_VERSION != 30
+#error Regenerate this file with the current version of nanopb generator.
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Struct definitions */
+typedef struct _Event {
+ bool has_Distance;
+ int32_t Distance;
+ bool has_SensorA;
+ bool SensorA;
+ bool has_SensorB;
+ bool SensorB;
+ bool has_SensorC;
+ bool SensorC;
+/* @@protoc_insertion_point(struct:Event) */
+} Event;
+
+typedef struct _Speed {
+ bool has_Left;
+ int32_t Left;
+ bool has_Right;
+ int32_t Right;
+/* @@protoc_insertion_point(struct:Speed) */
+} Speed;
+
+typedef struct _Command {
+ bool has_SetSpeed;
+ Speed SetSpeed;
+ bool has_Stop;
+ bool Stop;
+/* @@protoc_insertion_point(struct:Command) */
+} Command;
+
+/* Default values for struct fields */
+
+/* Initializer values for message structs */
+#define Command_init_default {false, Speed_init_default, false, 0}
+#define Speed_init_default {false, 0, false, 0}
+#define Event_init_default {false, 0, false, 0, false, 0, false, 0}
+#define Command_init_zero {false, Speed_init_zero, false, 0}
+#define Speed_init_zero {false, 0, false, 0}
+#define Event_init_zero {false, 0, false, 0, false, 0, false, 0}
+
+/* Field tags (for use in manual encoding/decoding) */
+#define Event_Distance_tag 1
+#define Event_SensorA_tag 2
+#define Event_SensorB_tag 3
+#define Event_SensorC_tag 4
+#define Speed_Left_tag 1
+#define Speed_Right_tag 2
+#define Command_SetSpeed_tag 1
+#define Command_Stop_tag 2
+
+/* Struct field encoding specification for nanopb */
+extern const pb_field_t Command_fields[3];
+extern const pb_field_t Speed_fields[3];
+extern const pb_field_t Event_fields[5];
+
+/* Maximum encoded size of messages (where known) */
+#define Command_size 26
+#define Speed_size 22
+#define Event_size 17
+
+/* Message IDs (where set with "msgid" option) */
+#ifdef PB_MSGID
+
+#define ELEGOO_MESSAGES \
+
+
+#endif
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+/* @@protoc_insertion_point(eof) */
+
+#endif
diff --git a/car/elegoo.proto b/car/elegoo.proto
new file mode 100644
index 0000000..0e56d17
--- /dev/null
+++ b/car/elegoo.proto
@@ -0,0 +1,20 @@
+syntax = "proto3";
+
+option go_package = "main";
+
+message Command {
+ Speed SetSpeed = 1;
+ bool Stop = 2;
+}
+
+message Speed {
+ int32 Left = 1;
+ int32 Right = 2;
+}
+
+message Event {
+ int32 Distance = 1;
+ bool SensorA = 2;
+ bool SensorB = 3;
+ bool SensorC = 4;
+}
diff --git a/car/elegoo/elegoo.pb.go b/car/elegoo/elegoo.pb.go
new file mode 100644
index 0000000..5854d75
--- /dev/null
+++ b/car/elegoo/elegoo.pb.go
@@ -0,0 +1,143 @@
+// Code generated by protoc-gen-go.
+// source: elegoo.proto
+// DO NOT EDIT!
+
+/*
+Package main is a generated protocol buffer package.
+
+It is generated from these files:
+ elegoo.proto
+
+It has these top-level messages:
+ Command
+ Speed
+ Event
+*/
+package main
+
+import proto "github.com/golang/protobuf/proto"
+import fmt "fmt"
+import math "math"
+
+// Reference imports to suppress errors if they are not otherwise used.
+var _ = proto.Marshal
+var _ = fmt.Errorf
+var _ = math.Inf
+
+// This is a compile-time assertion to ensure that this generated file
+// is compatible with the proto package it is being compiled against.
+// A compilation error at this line likely means your copy of the
+// proto package needs to be updated.
+const _ = proto.ProtoPackageIsVersion2 // please upgrade the proto package
+
+type Command struct {
+ SetSpeed *Speed `protobuf:"bytes,1,opt,name=SetSpeed" json:"SetSpeed,omitempty"`
+ Stop bool `protobuf:"varint,2,opt,name=Stop" json:"Stop,omitempty"`
+}
+
+func (m *Command) Reset() { *m = Command{} }
+func (m *Command) String() string { return proto.CompactTextString(m) }
+func (*Command) ProtoMessage() {}
+func (*Command) Descriptor() ([]byte, []int) { return fileDescriptor0, []int{0} }
+
+func (m *Command) GetSetSpeed() *Speed {
+ if m != nil {
+ return m.SetSpeed
+ }
+ return nil
+}
+
+func (m *Command) GetStop() bool {
+ if m != nil {
+ return m.Stop
+ }
+ return false
+}
+
+type Speed struct {
+ Left int32 `protobuf:"varint,1,opt,name=Left" json:"Left,omitempty"`
+ Right int32 `protobuf:"varint,2,opt,name=Right" json:"Right,omitempty"`
+}
+
+func (m *Speed) Reset() { *m = Speed{} }
+func (m *Speed) String() string { return proto.CompactTextString(m) }
+func (*Speed) ProtoMessage() {}
+func (*Speed) Descriptor() ([]byte, []int) { return fileDescriptor0, []int{1} }
+
+func (m *Speed) GetLeft() int32 {
+ if m != nil {
+ return m.Left
+ }
+ return 0
+}
+
+func (m *Speed) GetRight() int32 {
+ if m != nil {
+ return m.Right
+ }
+ return 0
+}
+
+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"`
+}
+
+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{2} }
+
+func (m *Event) GetDistance() int32 {
+ if m != nil {
+ return m.Distance
+ }
+ return 0
+}
+
+func (m *Event) GetSensorA() bool {
+ if m != nil {
+ return m.SensorA
+ }
+ return false
+}
+
+func (m *Event) GetSensorB() bool {
+ if m != nil {
+ return m.SensorB
+ }
+ return false
+}
+
+func (m *Event) GetSensorC() bool {
+ if m != nil {
+ return m.SensorC
+ }
+ return false
+}
+
+func init() {
+ proto.RegisterType((*Command)(nil), "Command")
+ proto.RegisterType((*Speed)(nil), "Speed")
+ proto.RegisterType((*Event)(nil), "Event")
+}
+
+func init() { proto.RegisterFile("elegoo.proto", fileDescriptor0) }
+
+var fileDescriptor0 = []byte{
+ // 186 bytes of a gzipped FileDescriptorProto
+ 0x1f, 0x8b, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0xff, 0x4c, 0x8f, 0xb1, 0xce, 0x82, 0x30,
+ 0x14, 0x46, 0xc3, 0xff, 0x53, 0x20, 0x57, 0xa7, 0xc6, 0xa1, 0x71, 0x22, 0x9d, 0x98, 0x48, 0xd4,
+ 0x27, 0x00, 0x74, 0x73, 0x6a, 0x37, 0x37, 0x94, 0x2b, 0x92, 0x48, 0x8b, 0x70, 0xe3, 0xf3, 0x9b,
+ 0x54, 0x04, 0xb6, 0xef, 0xf4, 0xe4, 0xa4, 0xb9, 0xb0, 0xc6, 0x27, 0xd6, 0xd6, 0xa6, 0x5d, 0x6f,
+ 0xc9, 0xca, 0x0c, 0xc2, 0xc2, 0xb6, 0x6d, 0x69, 0x2a, 0x2e, 0x21, 0xd2, 0x48, 0xba, 0x43, 0xac,
+ 0x84, 0x17, 0x7b, 0xc9, 0x6a, 0x1f, 0xa4, 0x8e, 0xd4, 0xf4, 0xce, 0x39, 0xf8, 0x9a, 0x6c, 0x27,
+ 0xfe, 0x62, 0x2f, 0x89, 0x94, 0xdb, 0x72, 0x07, 0x6c, 0x92, 0x67, 0xbc, 0x93, 0x8b, 0x99, 0x72,
+ 0x9b, 0x6f, 0x80, 0xa9, 0xa6, 0x7e, 0x90, 0x2b, 0x98, 0xfa, 0x82, 0x7c, 0x01, 0x3b, 0xbd, 0xd1,
+ 0x10, 0xdf, 0x42, 0x74, 0x6c, 0x06, 0x2a, 0xcd, 0x0d, 0xc7, 0x6c, 0x62, 0x2e, 0x20, 0xd4, 0x68,
+ 0x06, 0xdb, 0x67, 0xe3, 0x77, 0x3f, 0x9c, 0x4d, 0x2e, 0xfe, 0x97, 0x26, 0x9f, 0x4d, 0x21, 0xfc,
+ 0xa5, 0x29, 0xf2, 0xe0, 0xe2, 0xb7, 0x65, 0x63, 0xae, 0x81, 0xbb, 0xfb, 0xf0, 0x09, 0x00, 0x00,
+ 0xff, 0xff, 0x87, 0x0b, 0xf3, 0xb3, 0x07, 0x01, 0x00, 0x00,
+}
diff --git a/car/elegoo/gen.go b/car/elegoo/gen.go
new file mode 100644
index 0000000..76a8669
--- /dev/null
+++ b/car/elegoo/gen.go
@@ -0,0 +1,4 @@
+package main
+
+//go:generate sh -c "protoc -I.. --go_out=. ../*.proto"
+//go:generate sh -c "protoc -I.. --nanopb_out=.. ../*.proto"
diff --git a/car/elegoo/main.go b/car/elegoo/main.go
new file mode 100644
index 0000000..9171102
--- /dev/null
+++ b/car/elegoo/main.go
@@ -0,0 +1,78 @@
+package main
+
+import (
+ "io"
+ "log"
+ "time"
+
+ "github.com/golang/protobuf/proto"
+ "github.com/tarm/serial"
+)
+
+func Write(w io.Writer, buf []byte) {
+ w.Write([]byte{byte(len(buf))})
+ w.Write(buf)
+}
+
+func Read(r io.Reader) []byte {
+ buf := [32]byte{}
+ n, _ := r.Read(buf[:])
+ return buf[:n]
+ /*
+ sz := [1]byte{}
+ r.Read(sz[:])
+ log.Println(sz)
+ if l := int(sz[0]); l > 0 {
+ buf := make([]byte, l)
+ r.Read(buf)
+ return buf
+ }
+ return nil
+ */
+}
+
+func varint(b []byte) []byte {
+ return append([]byte{byte(len(b))}, b...)
+}
+
+// /dev/cu.Elegoo-DevB
+// /dev/cu.usbmodem1421
+
+func Wait(d time.Duration) {
+ log.Println("Wait", d)
+ time.Sleep(d)
+}
+
+func main() {
+ c := &serial.Config{
+ Name: "/dev/cu.usbmodem1421",
+ Baud: 57600,
+ }
+ s, err := serial.OpenPort(c)
+ if err != nil {
+ log.Fatal(err)
+ }
+ defer s.Close()
+
+ Wait(3 * time.Second)
+ cmd := &Command{
+ SetSpeed: &Speed{
+ Left: 100,
+ Right: 120,
+ },
+ }
+ buf, err := proto.Marshal(cmd)
+ if err != nil {
+ log.Fatal(err)
+ }
+ log.Printf("Send: %v", buf)
+ Write(s, buf)
+
+ go func() {
+ for {
+ log.Printf("Got: %v", Read(s))
+ }
+ }()
+
+ time.Sleep(30 * time.Second)
+}