From 528d38d82ff62aaa282812ae8b7a9fcc30691213 Mon Sep 17 00:00:00 2001 From: Dimitri Sokolyuk Date: Sun, 3 Sep 2017 00:54:03 +0200 Subject: Add fsm --- fsm.go | 57 +++++++++++++++++++++++++++++++++++++++++++++++++ main.go | 75 +++-------------------------------------------------------------- 2 files changed, 60 insertions(+), 72 deletions(-) create mode 100644 fsm.go diff --git a/fsm.go b/fsm.go new file mode 100644 index 0000000..772a73d --- /dev/null +++ b/fsm.go @@ -0,0 +1,57 @@ +package main + +import ( + "bufio" + "io" + "log" +) + +type stateFn func() stateFn + +type FSM struct { + events chan *Events + command chan *Command +} + +func NewFSM(rw io.ReadWriter) *FSM { + events := make(chan *Events) + command := make(chan *Command) + go readEvents(rw, events) + go writeCommands(rw, command) + return &FSM{events: events, command: command} +} + +func readEvents(r io.Reader, ch chan<- *Events) { + buf := bufio.NewReader(r) + for { + event := new(Events) + if err := Recv(buf, event); err != nil { + if err == io.ErrUnexpectedEOF { + continue + } + log.Println(err) + continue + } + ch <- event + } +} + +func writeCommands(w io.Writer, ch <-chan *Command) { + for command := range ch { + if err := Send(w, command); err != nil { + if err == io.ErrUnexpectedEOF { + continue + } + log.Println(err) + } + } +} + +func (f *FSM) Start() { + for state := f.initalState; state != nil; state = state() { + } +} + +func (f *FSM) initalState() stateFn { + return nil +} diff --git a/main.go b/main.go index ce4ea93..166b336 100644 --- a/main.go +++ b/main.go @@ -6,14 +6,13 @@ import ( "bufio" "io" "log" - "time" "github.com/dim13/cobs" "github.com/golang/protobuf/proto" "github.com/tarm/serial" ) -func Write(w io.Writer, pb proto.Message) error { +func Send(w io.Writer, pb proto.Message) error { buf := new(proto.Buffer) if err := buf.EncodeMessage(pb); err != nil { return err @@ -22,7 +21,7 @@ func Write(w io.Writer, pb proto.Message) error { return err } -func Read(buf *bufio.Reader, pb proto.Message) error { +func Recv(buf *bufio.Reader, pb proto.Message) error { block, err := buf.ReadBytes(0) if err != nil { return err @@ -30,43 +29,6 @@ func Read(buf *bufio.Reader, pb proto.Message) error { return proto.NewBuffer(cobs.Decode(block)).DecodeMessage(pb) } -func Reader(r io.Reader) <-chan *Events { - c := make(chan *Events) - buf := bufio.NewReader(r) - go func() { - for { - event := &Events{} - if err := Read(buf, event); err != nil { - if err == io.ErrUnexpectedEOF { - continue - } - log.Println("ERR", err) - return - } - log.Println("<-", event) - c <- event - } - }() - return c -} - -func Writer(w io.Writer) chan<- *Command { - c := make(chan *Command) - go func() { - for command := range c { - log.Println("->", command) - if err := Write(w, command); err != nil { - if err == io.ErrUnexpectedEOF { - continue - } - log.Println("ERR", err) - return - } - } - }() - return c -} - func main() { conf := &serial.Config{ //Name: "/dev/tty.usbmodem1421", @@ -80,36 +42,5 @@ func main() { } defer s.Close() - w := Writer(s) - - go func() { - for i := 30; i < 140; i += 10 { - w <- &Command{Direction: uint32(i)} - time.Sleep(100 * time.Millisecond) - } - for i := 140; i > 30; i -= 10 { - w <- &Command{Direction: uint32(i)} - time.Sleep(100 * time.Millisecond) - } - w <- &Command{Direction: 85} - }() - - for event := range Reader(s) { - if event.SensorC || event.Distance < 20 { - //w <- &Command{Stop: true} - } - } - - /* 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}) - */ + NewFSM(s).Start() } -- cgit v1.2.3