From 6c2152ff558a974e5d27eab21a80f9871ad1856f Mon Sep 17 00:00:00 2001 From: Thomas Kohler Date: Mon, 12 Feb 2024 17:55:26 +0100 Subject: [PATCH] megapi: use serialport adaptor and move driver to drivers/serial --- README.md | 1 + drivers/MIGRATION.md | 33 ++++- drivers/serial/README.md | 1 + drivers/serial/megapi/motor_driver.go | 130 ++++++++++++++++++ ...pi_motor.go => serialport_megapi_motor.go} | 9 +- platforms/megapi/README.md | 17 ++- platforms/megapi/megapi_adaptor.go | 88 ------------ platforms/megapi/motor_driver.go | 97 ------------- platforms/serialport/adaptor.go | 12 +- platforms/serialport/adaptor_test.go | 12 +- 10 files changed, 190 insertions(+), 210 deletions(-) create mode 100644 drivers/serial/megapi/motor_driver.go rename examples/{megapi_motor.go => serialport_megapi_motor.go} (74%) delete mode 100644 platforms/megapi/megapi_adaptor.go delete mode 100644 platforms/megapi/motor_driver.go diff --git a/README.md b/README.md index 0af24c298..55fe6232c 100644 --- a/README.md +++ b/README.md @@ -391,6 +391,7 @@ the `gobot/drivers/serial` package: - [UART](https://en.wikipedia.org/wiki/Serial_port) <=> [Drivers](https://github.com/hybridgroup/gobot/tree/master/drivers/serial) - Sphero: Sphero - Neurosky: MindWave + - MegaPi: MotorDriver Support for devices that use Serial Peripheral Interface (SPI) have a shared set of drivers provided using the `gobot/drivers/spi` package: diff --git a/drivers/MIGRATION.md b/drivers/MIGRATION.md index 02b12da32..2eb43e50c 100644 --- a/drivers/MIGRATION.md +++ b/drivers/MIGRATION.md @@ -115,7 +115,7 @@ import( ### Neurosky adaptor split off -The Neurosky adaptor now us the generic serial adaptor. The driver part was moved. With this, the imports needs to be +The Neurosky adaptor now use the generic serial adaptor. The driver part was moved. With this, the imports needs to be adjusted. In addition all events now have a suffix "Event", see below. ```go @@ -148,6 +148,37 @@ import( ... ``` +### MegaPi adaptor split off + +The MegaPi adaptor now use the generic serial adaptor. The driver part was moved. With this, the imports needs to be +adjusted. + +```go +// old +import( + ... + "gobot.io/x/gobot/v2/platforms/megapi" + ... +) + +... + megaPiAdaptor := megapi.NewAdaptor("/dev/ttyS0") + motor := megapi.NewMotorDriver(megaPiAdaptor, 1) +... + +// new +import( + ... + "gobot.io/x/gobot/v2/drivers/serial/megapi" + "gobot.io/x/gobot/v2/platforms/serialport" + ... +) +... + adaptor := serialport.NewAdaptor("/dev/ttyS0", serialport.WithName("MegaPi")) + motor := megapi.NewMotorDriver(adaptor, 1) +... +``` + ## Switch from version 2.2.0 (gpio drivers affected) ### gpio.ButtonDriver, gpio.PIRMotionDriver: substitute parameter "v time.duration" diff --git a/drivers/serial/README.md b/drivers/serial/README.md index a8ec27b0c..207c83b11 100644 --- a/drivers/serial/README.md +++ b/drivers/serial/README.md @@ -14,3 +14,4 @@ Gobot has a extensible system for connecting to hardware devices. The following - Sphero: Sphero - Neurosky: MindWave +- MegaPi: MotorDriver diff --git a/drivers/serial/megapi/motor_driver.go b/drivers/serial/megapi/motor_driver.go new file mode 100644 index 000000000..371ee6c68 --- /dev/null +++ b/drivers/serial/megapi/motor_driver.go @@ -0,0 +1,130 @@ +package megapi + +import ( + "bytes" + "encoding/binary" + "log" + "sync" + "time" + + "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/serial" +) + +var _ gobot.Driver = (*MotorDriver)(nil) + +type megapiMotorSerialAdaptor interface { + gobot.Adaptor + serial.SerialWriter +} + +// MotorDriver represents a motor +type MotorDriver struct { + *serial.Driver + port byte + halted bool + writeBytesChannel chan []byte + finalizeChannel chan struct{} + syncRoot *sync.Mutex +} + +// NewMotorDriver creates a new MotorDriver at the given port +func NewMotorDriver(a megapiMotorSerialAdaptor, port byte, opts ...serial.OptionApplier) *MotorDriver { + d := &MotorDriver{ + port: port, + halted: true, + syncRoot: &sync.Mutex{}, + writeBytesChannel: make(chan []byte), + finalizeChannel: make(chan struct{}), + } + d.Driver = serial.NewDriver(a, "MegaPiMotor", d.initialize, d.shutdown, opts...) + + return d +} + +// Speed sets the motors speed to the specified value +func (d *MotorDriver) Speed(speed int16) error { + d.syncRoot.Lock() + defer d.syncRoot.Unlock() + + if d.halted { + return nil + } + return d.speedHelper(speed) +} + +// initialize implements the Driver interface +func (d *MotorDriver) initialize() error { + d.syncRoot.Lock() + defer d.syncRoot.Unlock() + + // sleeping is required to give the board a chance to reset after connection is done + time.Sleep(2 * time.Second) + + // kick off thread to send bytes to the board + go func() { + for { + select { + case bytes := <-d.writeBytesChannel: + if _, err := d.adaptor().SerialWrite(bytes); err != nil { + panic(err) + } + time.Sleep(10 * time.Millisecond) + case <-d.finalizeChannel: + d.finalizeChannel <- struct{}{} + return + default: + time.Sleep(10 * time.Millisecond) + } + } + }() + + d.halted = false + return d.speedHelper(0) +} + +// Halt terminates the Driver interface +func (d *MotorDriver) shutdown() error { + d.syncRoot.Lock() + defer d.syncRoot.Unlock() + + d.finalizeChannel <- struct{}{} + <-d.finalizeChannel + + d.halted = true + return d.speedHelper(0) +} + +// 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 (d *MotorDriver) speedHelper(speed int16) error { + if err := d.sendSpeed(speed - 1); err != nil { + return err + } + return d.sendSpeed(speed) +} + +// sendSpeed sets the motors speed to the specified value +func (d *MotorDriver) sendSpeed(speed int16) error { + bufOut := new(bytes.Buffer) + + // byte sequence: 0xff, 0x55, id, action, device, port + bufOut.Write([]byte{0xff, 0x55, 0x6, 0x0, 0x2, 0xa, d.port}) + if err := binary.Write(bufOut, binary.LittleEndian, speed); err != nil { + return err + } + bufOut.Write([]byte{0xa}) + d.writeBytesChannel <- bufOut.Bytes() + + return nil +} + +func (d *MotorDriver) adaptor() megapiMotorSerialAdaptor { + if a, ok := d.Connection().(megapiMotorSerialAdaptor); ok { + return a + } + + log.Printf("%s has no MegaPi serial connector\n", d.Name()) + return nil +} diff --git a/examples/megapi_motor.go b/examples/serialport_megapi_motor.go similarity index 74% rename from examples/megapi_motor.go rename to examples/serialport_megapi_motor.go index fba251446..54fbd3191 100644 --- a/examples/megapi_motor.go +++ b/examples/serialport_megapi_motor.go @@ -11,14 +11,15 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/megapi" + "gobot.io/x/gobot/v2/drivers/serial/megapi" + "gobot.io/x/gobot/v2/platforms/serialport" ) func main() { // use "/dev/ttyUSB0" if connecting with USB cable // use "/dev/ttyAMA0" on devices older than Raspberry Pi 3 Model B - megaPiAdaptor := megapi.NewAdaptor("/dev/ttyS0") - motor := megapi.NewMotorDriver(megaPiAdaptor, 1) + adaptor := serialport.NewAdaptor("/dev/ttyS0", serialport.WithName("MegaPi")) + motor := megapi.NewMotorDriver(adaptor, 1) work := func() { speed := int16(0) @@ -36,7 +37,7 @@ func main() { } robot := gobot.NewRobot("megaPiBot", - []gobot.Connection{megaPiAdaptor}, + []gobot.Connection{adaptor}, []gobot.Device{motor}, work, ) diff --git a/platforms/megapi/README.md b/platforms/megapi/README.md index 29db58c1b..c95aaaff9 100644 --- a/platforms/megapi/README.md +++ b/platforms/megapi/README.md @@ -15,16 +15,19 @@ Please refer to the main [README.md](https://github.com/hybridgroup/gobot/blob/r package main import ( - "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/megapi" + "fmt" "time" + + "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/serial/megapi" + "gobot.io/x/gobot/v2/platforms/serialport" ) func main() { // use "/dev/ttyUSB0" if connecting with USB cable // use "/dev/ttyAMA0" on devices older than Raspberry Pi 3 Model B - megaPiAdaptor := megapi.NewAdaptor("/dev/ttyS0") - motor := megapi.NewMotorDriver(megaPiAdaptor, 1) + adaptor := serialport.NewAdaptor("/dev/ttyS0", serialport.WithName("MegaPi")) + motor := megapi.NewMotorDriver(adaptor, 1) work := func() { speed := int16(0) @@ -40,13 +43,13 @@ func main() { } robot := gobot.NewRobot("megaPiBot", - []gobot.Connection{megaPiAdaptor}, + []gobot.Connection{adaptor}, []gobot.Device{motor}, work, ) if err := robot.Start(); err != nil { - panic(err) - } + panic(err) + } } ``` diff --git a/platforms/megapi/megapi_adaptor.go b/platforms/megapi/megapi_adaptor.go deleted file mode 100644 index fb8767e44..000000000 --- a/platforms/megapi/megapi_adaptor.go +++ /dev/null @@ -1,88 +0,0 @@ -package megapi - -import ( - "io" - "time" - - "go.bug.st/serial" - - "gobot.io/x/gobot/v2" -) - -var _ gobot.Adaptor = (*Adaptor)(nil) - -// Adaptor is the Gobot adaptor for the MakeBlock MegaPi board -type Adaptor struct { - name string - port string - connection io.ReadWriteCloser - serialMode *serial.Mode - writeBytesChannel chan []byte - finalizeChannel chan struct{} -} - -// NewAdaptor returns a new Adaptor with specified serial port used to talk to the MegaPi with a baud rate of 115200 -func NewAdaptor(device string) *Adaptor { - c := &serial.Mode{BaudRate: 115200} - return &Adaptor{ - name: "MegaPi", - connection: nil, - port: device, - serialMode: c, - writeBytesChannel: make(chan []byte), - finalizeChannel: make(chan struct{}), - } -} - -// Name returns the name of this adaptor -func (megaPi *Adaptor) Name() string { - return megaPi.name -} - -// SetName sets the name of this adaptor -func (megaPi *Adaptor) SetName(n string) { - megaPi.name = n -} - -// Connect starts a connection to the board -func (megaPi *Adaptor) Connect() error { - if megaPi.connection == nil { - sp, err := serial.Open(megaPi.port, megaPi.serialMode) - if err != nil { - return 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: - if _, err := megaPi.connection.Write(bytes); err != nil { - panic(err) - } - time.Sleep(10 * time.Millisecond) - case <-megaPi.finalizeChannel: - megaPi.finalizeChannel <- struct{}{} - return - default: - time.Sleep(10 * time.Millisecond) - } - } - }() - return nil -} - -// Finalize terminates the connection to the board -func (megaPi *Adaptor) Finalize() error { - megaPi.finalizeChannel <- struct{}{} - <-megaPi.finalizeChannel - if err := megaPi.connection.Close(); err != nil { - return err - } - return nil -} diff --git a/platforms/megapi/motor_driver.go b/platforms/megapi/motor_driver.go deleted file mode 100644 index ff6709cdf..000000000 --- a/platforms/megapi/motor_driver.go +++ /dev/null @@ -1,97 +0,0 @@ -package megapi - -import ( - "bytes" - "encoding/binary" - "sync" - - "gobot.io/x/gobot/v2" -) - -var _ gobot.Driver = (*MotorDriver)(nil) - -// MotorDriver represents a motor -type MotorDriver struct { - name string - megaPi *Adaptor - port byte - halted bool - syncRoot *sync.Mutex -} - -// NewMotorDriver creates a new MotorDriver at the given port -func NewMotorDriver(megaPi *Adaptor, port byte) *MotorDriver { - return &MotorDriver{ - name: "MegaPiMotor", - megaPi: megaPi, - port: port, - halted: true, - syncRoot: &sync.Mutex{}, - } -} - -// Name returns the name of this motor -func (d *MotorDriver) Name() string { - return d.name -} - -// SetName sets the name of this motor -func (d *MotorDriver) SetName(n string) { - d.name = n -} - -// Start implements the Driver interface -func (d *MotorDriver) Start() error { - d.syncRoot.Lock() - defer d.syncRoot.Unlock() - d.halted = false - return d.speedHelper(0) -} - -// Halt terminates the Driver interface -func (d *MotorDriver) Halt() error { - d.syncRoot.Lock() - defer d.syncRoot.Unlock() - d.halted = true - return d.speedHelper(0) -} - -// Connection returns the Connection associated with the Driver -func (d *MotorDriver) Connection() gobot.Connection { - return gobot.Connection(d.megaPi) -} - -// Speed sets the motors speed to the specified value -func (d *MotorDriver) Speed(speed int16) error { - d.syncRoot.Lock() - defer d.syncRoot.Unlock() - if d.halted { - return nil - } - return d.speedHelper(speed) -} - -// 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 (d *MotorDriver) speedHelper(speed int16) error { - if err := d.sendSpeed(speed - 1); err != nil { - return err - } - return d.sendSpeed(speed) -} - -// sendSpeed sets the motors speed to the specified value -func (d *MotorDriver) sendSpeed(speed int16) error { - bufOut := new(bytes.Buffer) - - // byte sequence: 0xff, 0x55, id, action, device, port - bufOut.Write([]byte{0xff, 0x55, 0x6, 0x0, 0x2, 0xa, d.port}) - if err := binary.Write(bufOut, binary.LittleEndian, speed); err != nil { - return err - } - bufOut.Write([]byte{0xa}) - d.megaPi.writeBytesChannel <- bufOut.Bytes() - - return nil -} diff --git a/platforms/serialport/adaptor.go b/platforms/serialport/adaptor.go index 04901f505..8af566039 100644 --- a/platforms/serialport/adaptor.go +++ b/platforms/serialport/adaptor.go @@ -21,7 +21,6 @@ type Adaptor struct { cfg *configuration sp io.ReadWriteCloser - connected bool connectFunc func(string, int) (io.ReadWriteCloser, error) } @@ -70,7 +69,7 @@ func (a *Adaptor) SetName(n string) { // Connect initiates a connection to the serial port. func (a *Adaptor) Connect() error { - if a.connected { + if a.sp != nil { return fmt.Errorf("serial port is already connected, try reconnect or run disconnect first") } @@ -80,7 +79,6 @@ func (a *Adaptor) Connect() error { } a.sp = sp - a.connected = true return nil } @@ -91,11 +89,11 @@ func (a *Adaptor) Finalize() error { // Disconnect terminates the connection to the port. func (a *Adaptor) Disconnect() error { - if a.connected { + if a.sp != nil { if err := a.sp.Close(); err != nil { return err } - a.connected = false + a.sp = nil } return nil } @@ -103,7 +101,7 @@ func (a *Adaptor) Disconnect() error { // Reconnect attempts to reconnect to the port. If the port is connected it will first close // that connection and then establish a new connection. func (a *Adaptor) Reconnect() error { - if a.connected { + if a.sp != nil { if err := a.Disconnect(); err != nil { return err } @@ -116,7 +114,7 @@ func (a *Adaptor) Port() string { return a.port } // IsConnected returns the connection state func (a *Adaptor) IsConnected() bool { - return a.connected + return a.sp != nil } // SerialRead reads from the port to the given reference diff --git a/platforms/serialport/adaptor_test.go b/platforms/serialport/adaptor_test.go index 47ebea684..972011415 100644 --- a/platforms/serialport/adaptor_test.go +++ b/platforms/serialport/adaptor_test.go @@ -117,7 +117,7 @@ func TestConnect(t *testing.T) { // act & assert require.EqualError(t, a.Connect(), "serial port is already connected, try reconnect or run disconnect first") // re-arrange error - a.connected = false + a.sp = nil a.connectFunc = func(string, int) (io.ReadWriteCloser, error) { return nil, errors.New("connect error") } @@ -129,16 +129,16 @@ func TestConnect(t *testing.T) { func TestReconnect(t *testing.T) { // arrange a, _ := initTestAdaptor() - require.True(t, a.connected) + require.NotNil(t, a.sp) // act & assert require.NoError(t, a.Reconnect()) - assert.True(t, a.connected) + require.NotNil(t, a.sp) // act & assert require.NoError(t, a.Disconnect()) - assert.False(t, a.connected) + require.Nil(t, a.sp) // act & assert require.NoError(t, a.Reconnect()) - assert.True(t, a.connected) + require.NotNil(t, a.sp) } func TestFinalize(t *testing.T) { @@ -149,7 +149,7 @@ func TestFinalize(t *testing.T) { assert.False(t, a.IsConnected()) // re-arrange error rwc.simulateCloseErr = true - a.connected = true + require.NoError(t, a.Connect()) // act & assert require.ErrorContains(t, a.Finalize(), "close error") }