Merge pull request hybridgroup#305 from yurigorokhov/megapi_motors

Adding support for MakeBlock megapi

examples/megapi_motor.go

@@ -0,0 +1,39 @@
+package main
+
+import (
+	"github.com/hybridgroup/gobot"
+	"github.com/hybridgroup/gobot/platforms/megapi"
+	"time"
+)
+
+func main() {
+	gbot := gobot.NewGobot()
+
+	// use "/dev/ttyUSB0" if connecting with USB cable
+	// use "/dev/ttyAMA0" on devices older than Raspberry Pi 3 Model B
+	megaPiAdaptor := megapi.NewMegaPiAdaptor("megapi", "/dev/ttyS0")
+	motor := megapi.NewMotorDriver(megaPiAdaptor, "motor1", 1)
+
+	work := func() {
+		speed := int16(0)
+		fadeAmount := int16(30)
+
+		gobot.Every(100*time.Millisecond, func() {
+			motor.Speed(speed)
+			speed = speed + fadeAmount
+			if speed == 0 || speed == 300 {
+				fadeAmount = -fadeAmount
+			}
+		})
+	}
+
+	robot := gobot.NewRobot("megaPiBot",
+		[]gobot.Connection{megaPiAdaptor},
+		[]gobot.Device{motor},
+		work,
+	)
+
+	gbot.AddRobot(robot)
+
+	gbot.Start()
+}

platforms/megapi/README.md

@@ -0,0 +1,55 @@
+# MegaPi
+
+The [MegaPi](http://learn.makeblock.com/en/megapi/) is a motor controller by MakeBlock that is compatible with the Raspberry Pi.
+
+The code is based on a python implementation that can be found [here](https://github.com/Makeblock-official/PythonForMegaPi).
+
+## How to Install
+
+```
+go get -d -u github.com/hybridgroup/gobot/... && go install github.com/hybridgroup/gobot/platforms/megapi
+```
+
+## How to Use
+
+```go
+package main
+
+import (
+	"github.com/hybridgroup/gobot"
+	"github.com/hybridgroup/gobot/platforms/megapi"
+	"time"
+)
+
+func main() {
+	gbot := gobot.NewGobot()
+
+	// use "/dev/ttyUSB0" if connecting with USB cable
+	// use "/dev/ttyAMA0" on devices older than Raspberry Pi 3 Model B
+	megaPiAdaptor := megapi.NewMegaPiAdaptor("megapi", "/dev/ttyS0")
+	motor := megapi.NewMotorDriver(megaPiAdaptor, "motor1", 1)
+
+	work := func() {
+		speed := int16(0)
+		fadeAmount := int16(30)
+
+		gobot.Every(100*time.Millisecond, func() {
+			motor.Speed(speed)
+			speed = speed + fadeAmount
+			if speed == 0 || speed == 300 {
+				fadeAmount = -fadeAmount
+			}
+		})
+	}
+
+	robot := gobot.NewRobot("megaPiBot",
+		[]gobot.Connection{megaPiAdaptor},
+		[]gobot.Device{motor},
+		work,
+	)
+
+	gbot.AddRobot(robot)
+
+	gbot.Start()
+}
+```

platforms/megapi/megapi_adaptor.go

@@ -0,0 +1,77 @@
+package megapi
+
+import (
+	"github.com/hybridgroup/gobot"
+	"github.com/tarm/serial"
+	"io"
+	"time"
+)
+
+var _ gobot.Adaptor = (*MegaPiAdaptor)(nil)
+
+// MegaPiAdaptor is the Gobot adaptor for the MakeBlock MegaPi board
+type MegaPiAdaptor struct {
+	name              string
+	connection        io.ReadWriteCloser
+	serialConfig      *serial.Config
+	writeBytesChannel chan []byte
+	finalizeChannel   chan struct{}
+}
+
+// NewMegaPiAdaptor returns a new MegaPiAdaptor with specified name and specified serial port used to talk to the MegaPi with a baud rate of 115200
+func NewMegaPiAdaptor(name string, device string) *MegaPiAdaptor {
+	c := &serial.Config{Name: device, Baud: 115200}
+	return &MegaPiAdaptor{
+		name:              name,
+		connection:        nil,
+		serialConfig:      c,
+		writeBytesChannel: make(chan []byte),
+		finalizeChannel:   make(chan struct{}),
+	}
+}
+
+// Name returns the name of this adaptor
+func (megaPi *MegaPiAdaptor) Name() string {
+	return megaPi.name
+}
+
+// Connect starts a connection to the board
+func (megaPi *MegaPiAdaptor) Connect() (errs []error) {
+	if megaPi.connection == nil {
+		sp, err := serial.OpenPort(megaPi.serialConfig)
+		if err != nil {
+			return []error{err}
+		}
+
+		// sleeping is required to give the board a chance to reset
+		time.Sleep(2 * time.Second)
+		megaPi.connection = sp
+	}
+
+	// kick off thread to send bytes to the board
+	go func() {
+		for {
+			select {
+			case bytes := <-megaPi.writeBytesChannel:
+				megaPi.connection.Write(bytes)
+				time.Sleep(10 * time.Millisecond)
+			case <-megaPi.finalizeChannel:
+				megaPi.finalizeChannel <- struct{}{}
+				return
+			default:
+				time.Sleep(10 * time.Millisecond)
+			}
+		}
+	}()
+	return
+}
+
+// Finalize terminates the connection to the board
+func (megaPi *MegaPiAdaptor) Finalize() (errs []error) {
+	megaPi.finalizeChannel <- struct{}{}
+	<-megaPi.finalizeChannel
+	if err := megaPi.connection.Close(); err != nil {
+		return []error{err}
+	}
+	return
+}

platforms/megapi/motor_driver.go

@@ -0,0 +1,88 @@
+package megapi
+
+import (
+	"bytes"
+	"encoding/binary"
+	"github.com/hybridgroup/gobot"
+	"sync"
+)
+
+var _ gobot.Driver = (*MotorDriver)(nil)
+
+// MotorDriver represents a motor
+type MotorDriver struct {
+	name     string
+	megaPi   *MegaPiAdaptor
+	port     byte
+	halted   bool
+	syncRoot *sync.Mutex
+}
+
+// NewMotorDriver creates a new MotorDriver using the provided name, and at the given port
+func NewMotorDriver(megaPi *MegaPiAdaptor, name string, port byte) *MotorDriver {
+	return &MotorDriver{
+		name:     name,
+		megaPi:   megaPi,
+		port:     port,
+		halted:   true,
+		syncRoot: &sync.Mutex{},
+	}
+}
+
+// Name returns the name of this motor
+func (m *MotorDriver) Name() string {
+	return m.name
+}
+
+// Start implements the Driver interface
+func (m *MotorDriver) Start() []error {
+	m.syncRoot.Lock()
+	defer m.syncRoot.Unlock()
+	m.halted = false
+	m.speedHelper(0)
+	return []error{}
+}
+
+// Halt terminates the Driver interface
+func (m *MotorDriver) Halt() []error {
+	m.syncRoot.Lock()
+	defer m.syncRoot.Unlock()
+	m.halted = true
+	m.speedHelper(0)
+	return []error{}
+}
+
+// Connection returns the Connection associated with the Driver
+func (m *MotorDriver) Connection() gobot.Connection {
+	return gobot.Connection(m.megaPi)
+}
+
+// Speed sets the motors speed to the specified value
+func (m *MotorDriver) Speed(speed int16) error {
+	m.syncRoot.Lock()
+	defer m.syncRoot.Unlock()
+	if m.halted {
+		return nil
+	}
+	m.speedHelper(speed)
+	return nil
+}
+
+// there is some sort of bug on the hardware such that you cannot
+// send the exact same speed to 2 different motors consecutively
+// hence we ensure we always alternate speeds
+func (m *MotorDriver) speedHelper(speed int16) {
+	m.sendSpeed(speed - 1)
+	m.sendSpeed(speed)
+}
+
+// sendSpeed sets the motors speed to the specified value
+func (m *MotorDriver) sendSpeed(speed int16) {
+	bufOut := new(bytes.Buffer)
+
+	// byte sequence: 0xff, 0x55, id, action, device, port
+	bufOut.Write([]byte{0xff, 0x55, 0x6, 0x0, 0x2, 0xa, m.port})
+	binary.Write(bufOut, binary.LittleEndian, speed)
+	bufOut.Write([]byte{0xa})
+	m.megaPi.writeBytesChannel <- bufOut.Bytes()
+}