diff --git a/examples/megapi_motor.go b/examples/megapi_motor.go new file mode 100644 index 000000000..322599812 --- /dev/null +++ b/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() +} diff --git a/platforms/megapi/README.md b/platforms/megapi/README.md new file mode 100644 index 000000000..637057679 --- /dev/null +++ b/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() +} +``` diff --git a/platforms/megapi/megapi_adaptor.go b/platforms/megapi/megapi_adaptor.go new file mode 100644 index 000000000..21e7af48c --- /dev/null +++ b/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 +} diff --git a/platforms/megapi/motor_driver.go b/platforms/megapi/motor_driver.go new file mode 100644 index 000000000..dd298df79 --- /dev/null +++ b/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() +}