diff --git a/Makefile b/Makefile index e3c7883f0..0b95c4346 100644 --- a/Makefile +++ b/Makefile @@ -12,7 +12,7 @@ including_except := $(shell go list ./... | grep -v platforms/opencv) # Run tests on nearly all directories without test cache, with race detection test_race: - go test -failfast -count=1 -v -race $(including_except) + go test -failfast -count=1 -race $(including_except) # Run tests on nearly all directories without test cache test: diff --git a/README.md b/README.md index a9e48b4c4..c8b524320 100644 --- a/README.md +++ b/README.md @@ -101,12 +101,13 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/sphero" + "gobot.io/x/gobot/v2/drivers/serial" + "gobot.io/x/gobot/v2/platforms/serialport" ) func main() { - adaptor := sphero.NewAdaptor("/dev/rfcomm0") - driver := sphero.NewSpheroDriver(adaptor) + adaptor := serialport.NewAdaptor("/dev/rfcomm0") + driver := serial.NewSpheroDriver(adaptor) work := func() { gobot.Every(3*time.Second, func() { @@ -166,18 +167,20 @@ import ( "gobot.io/x/gobot/v2" "gobot.io/x/gobot/v2/api" - "gobot.io/x/gobot/v2/platforms/sphero" + "gobot.io/x/gobot/v2/drivers/common/sphero" + "gobot.io/x/gobot/v2/drivers/serial" + "gobot.io/x/gobot/v2/platforms/serialport" ) func NewSwarmBot(port string) *gobot.Robot { - spheroAdaptor := sphero.NewAdaptor(port) - spheroDriver := sphero.NewSpheroDriver(spheroAdaptor) + spheroAdaptor := serialport.NewAdaptor(port) + spheroDriver := serial.NewSpheroDriver(spheroAdaptor) spheroDriver.SetName("Sphero" + port) work := func() { spheroDriver.Stop() - spheroDriver.On(sphero.Collision, func(data interface{}) { + spheroDriver.On(sphero.CollisionEvent, func(data interface{}) { fmt.Println("Collision Detected!") }) @@ -229,7 +232,7 @@ platforms are currently supported: - Audio <=> [Package](https://github.com/hybridgroup/gobot/tree/master/platforms/audio) - [Beaglebone Black](http://beagleboard.org/boards) <=> [Package](https://github.com/hybridgroup/gobot/tree/master/platforms/beaglebone) - [Beaglebone PocketBeagle](http://beagleboard.org/pocket/) <=> [Package](https://github.com/hybridgroup/gobot/tree/master/platforms/beaglebone) -- [Bluetooth LE](https://www.bluetooth.com/what-is-bluetooth-technology/bluetooth-technology-basics/low-energy) <=> [Package](https://github.com/hybridgroup/gobot/tree/master/platforms/ble) +- [Bluetooth LE](https://www.bluetooth.com/what-is-bluetooth-technology/bluetooth-technology-basics/low-energy) <=> [Package](https://github.com/hybridgroup/gobot/tree/master/platforms/bleclient) - [C.H.I.P](http://www.nextthing.co/pages/chip) <=> [Package](https://github.com/hybridgroup/gobot/tree/master/platforms/chip) - [C.H.I.P Pro](https://docs.getchip.com/chip_pro.html) <=> [Package](https://github.com/hybridgroup/gobot/tree/master/platforms/chip) - [Digispark](http://digistump.com/products/1) <=> [Package](https://github.com/hybridgroup/gobot/tree/master/platforms/digispark) @@ -259,15 +262,47 @@ platforms are currently supported: - [Pebble](https://www.getpebble.com/) <=> [Package](https://github.com/hybridgroup/gobot/tree/master/platforms/pebble) - [Radxa Rock Pi 4](https://wiki.radxa.com/Rock4/) <=> [Package](https://github.com/hybridgroup/gobot/tree/master/platforms/rockpi) - [Raspberry Pi](http://www.raspberrypi.org/) <=> [Package](https://github.com/hybridgroup/gobot/tree/master/platforms/raspi) -- [Sphero](http://www.sphero.com/) <=> [Package](https://github.com/hybridgroup/gobot/tree/master/platforms/sphero) +- [Serial Port](https://en.wikipedia.org/wiki/Serial_port) <=> [Package](https://github.com/hybridgroup/gobot/tree/master/platforms/serialport) +- [Sphero](http://www.sphero.com/) <=> [Package](https://github.com/hybridgroup/gobot/tree/master/platforms/sphero/sphero) - [Sphero BB-8](http://www.sphero.com/bb8) <=> [Package](https://github.com/hybridgroup/gobot/tree/master/platforms/sphero/bb8) - [Sphero Ollie](http://www.sphero.com/ollie) <=> [Package](https://github.com/hybridgroup/gobot/tree/master/platforms/sphero/ollie) - [Sphero SPRK+](http://www.sphero.com/sprk-plus) <=> [Package](https://github.com/hybridgroup/gobot/tree/master/platforms/sphero/sprkplus) - [Tinker Board](https://www.asus.com/us/Single-Board-Computer/Tinker-Board/) <=> [Package](https://github.com/hybridgroup/gobot/tree/master/platforms/tinkerboard) - [UP2](http://www.up-board.org/upsquared/) <=> [Package](https://github.com/hybridgroup/gobot/tree/master/platforms/upboard/up2) -Support for many devices that use General Purpose Input/Output (GPIO) have -a shared set of drivers provided using the `gobot/drivers/gpio` package: +Support for many devices that use Analog Input/Output (AIO) have a shared set of drivers provided using +the `gobot/drivers/aio` package: + +- [AIO](https://en.wikipedia.org/wiki/Analog-to-digital_converter) <=> [Drivers](https://github.com/hybridgroup/gobot/tree/master/drivers/aio) + - Analog Actuator + - Analog Sensor + - Grove Light Sensor + - Grove Piezo Vibration Sensor + - Grove Rotary Dial + - Grove Sound Sensor + - Grove Temperature Sensor + - Temperature Sensor (supports linear and NTC thermistor in normal and inverse mode) + - Thermal Zone Temperature Sensor + +Support for many devices that use Bluetooth LE (BLE) have a shared set of drivers provided using +the `gobot/drivers/ble` package: + +- [BLE](http://en.wikipedia.org/wiki/Bluetooth_low_energy) <=> [Drivers](https://github.com/hybridgroup/gobot/tree/master/drivers/ble) + - Battery Service + - Device Information Service + - Generic Access Service + - Microbit: AccelerometerDriver + - Microbit: ButtonDriver + - Microbit: IOPinDriver + - Microbit: LEDDriver + - Microbit: MagnetometerDriver + - Microbit: TemperatureDriver + - Sphero: BB8 + - Sphero: Ollie + - Sphero: SPRK+ + +Support for many devices that use General Purpose Input/Output (GPIO) have a shared set of drivers provided using +the `gobot/drivers/gpio` package: - [GPIO](https://en.wikipedia.org/wiki/General_Purpose_Input/Output) <=> [Drivers](https://github.com/hybridgroup/gobot/tree/master/drivers/gpio) - AIP1640 LED Dot Matrix/7 Segment Controller @@ -294,22 +329,8 @@ a shared set of drivers provided using the `gobot/drivers/gpio` package: - Stepper Motor - TM1638 LED Controller -Support for many devices that use Analog Input/Output (AIO) have -a shared set of drivers provided using the `gobot/drivers/aio` package: - -- [AIO](https://en.wikipedia.org/wiki/Analog-to-digital_converter) <=> [Drivers](https://github.com/hybridgroup/gobot/tree/master/drivers/aio) - - Analog Actuator - - Analog Sensor - - Grove Light Sensor - - Grove Piezo Vibration Sensor - - Grove Rotary Dial - - Grove Sound Sensor - - Grove Temperature Sensor - - Temperature Sensor (supports linear and NTC thermistor in normal and inverse mode) - - Thermal Zone Temperature Sensor - -Support for devices that use Inter-Integrated Circuit (I2C) have a shared set of -drivers provided using the `gobot/drivers/i2c` package: +Support for devices that use Inter-Integrated Circuit (I2C) have a shared set of drivers provided using +the `gobot/drivers/i2c` package: - [I2C](https://en.wikipedia.org/wiki/I%C2%B2C) <=> [Drivers](https://github.com/hybridgroup/gobot/tree/master/drivers/i2c) - Adafruit 1109 2x16 RGB-LCD with 5 keys @@ -351,6 +372,12 @@ drivers provided using the `gobot/drivers/i2c` package: - Wii Nunchuck Controller - YL-40 Brightness/Temperature sensor, Potentiometer, analog input, analog output Driver +Support for many devices that use Serial communication (UART) have a shared set of drivers provided using +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 + Support for devices that use Serial Peripheral Interface (SPI) have a shared set of drivers provided using the `gobot/drivers/spi` package: @@ -366,8 +393,6 @@ a shared set of drivers provided using the `gobot/drivers/spi` package: - MFRC522 RFID Card Reader - SSD1306 OLED Display Controller -More platforms and drivers are coming soon... - ## API Gobot includes a RESTful API to query the status of any robot running within a group, including the connection and diff --git a/adaptor.go b/adaptor.go index 5456136f7..6eedb50cb 100644 --- a/adaptor.go +++ b/adaptor.go @@ -225,6 +225,20 @@ type Adaptor interface { Finalize() error } +// BLEConnector is the interface that a BLE ClientAdaptor must implement +type BLEConnector interface { + Adaptor + + Reconnect() error + Disconnect() error + Address() string + + ReadCharacteristic(cUUID string) ([]byte, error) + WriteCharacteristic(cUUID string, data []byte) error + Subscribe(cUUID string, f func([]byte, error)) error + WithoutResponses(use bool) +} + // Porter is the interface that describes an adaptor's port type Porter interface { Port() string diff --git a/appveyor.yml b/appveyor.yml index dd9d21df6..3031663a6 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -15,10 +15,14 @@ before_test: build_script: - go test -v -cpu=2 . - go test -v -cpu=2 ./drivers/aio/... - - go test -v -cpu=2 ./platforms/ble/... + - go test -v -cpu=2 ./drivers/ble/. + - go test -v -cpu=2 ./drivers/ble/parrot/. + - go test -v -cpu=2 ./drivers/ble/sphero/. + - go test -v -cpu=2 ./drivers/serial/... + - go test -v -cpu=2 ./platforms/bleclient/... - go test -v -cpu=2 ./platforms/dji/... - go test -v -cpu=2 ./platforms/firmata/... - go test -v -cpu=2 ./platforms/joystick/... - go test -v -cpu=2 ./platforms/parrot/... - - go test -v -cpu=2 ./platforms/sphero/... + - go test -v -cpu=2 ./platforms/serialport/... - cd .. diff --git a/doc.go b/doc.go index acd5e0d58..2f1eee92a 100644 --- a/doc.go +++ b/doc.go @@ -70,66 +70,68 @@ pure idiomatic Golang code. For example: Finally, you can use Master Gobot to add the complete Gobot API or control swarms of Robots: - package main - - import ( - "fmt" - "time" - - "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/api" - "gobot.io/x/gobot/v2/platforms/sphero" - ) - - func NewSwarmBot(port string) *gobot.Robot { - spheroAdaptor := sphero.NewAdaptor(port) - spheroDriver := sphero.NewSpheroDriver(spheroAdaptor) - spheroDriver.SetName("Sphero" + port) - - work := func() { - spheroDriver.Stop() - - spheroDriver.On(sphero.Collision, func(data interface{}) { - fmt.Println("Collision Detected!") - }) - - gobot.Every(1*time.Second, func() { - spheroDriver.Roll(100, uint16(gobot.Rand(360))) - }) - gobot.Every(3*time.Second, func() { - spheroDriver.SetRGB(uint8(gobot.Rand(255)), - uint8(gobot.Rand(255)), - uint8(gobot.Rand(255)), - ) - }) - } - - robot := gobot.NewRobot("sphero", - []gobot.Connection{spheroAdaptor}, - []gobot.Device{spheroDriver}, - work, - ) - - return robot - } - - func main() { - master := gobot.NewMaster() - api.NewAPI(master).Start() - - spheros := []string{ - "/dev/rfcomm0", - "/dev/rfcomm1", - "/dev/rfcomm2", - "/dev/rfcomm3", - } - - for _, port := range spheros { - master.AddRobot(NewSwarmBot(port)) - } - - master.Start() - } + package main + + import ( + "fmt" + "time" + + "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/api" + "gobot.io/x/gobot/v2/drivers/common/sphero" + "gobot.io/x/gobot/v2/drivers/serial" + "gobot.io/x/gobot/v2/platforms/serialport" + ) + + func NewSwarmBot(port string) *gobot.Robot { + spheroAdaptor := serialport.NewAdaptor(port) + spheroDriver := serial.NewSpheroDriver(spheroAdaptor) + spheroDriver.SetName("Sphero" + port) + + work := func() { + spheroDriver.Stop() + + spheroDriver.On(sphero.CollisionEvent, func(data interface{}) { + fmt.Println("Collision Detected!") + }) + + gobot.Every(1*time.Second, func() { + spheroDriver.Roll(100, uint16(gobot.Rand(360))) + }) + gobot.Every(3*time.Second, func() { + spheroDriver.SetRGB(uint8(gobot.Rand(255)), + uint8(gobot.Rand(255)), + uint8(gobot.Rand(255)), + ) + }) + } + + robot := gobot.NewRobot("sphero", + []gobot.Connection{spheroAdaptor}, + []gobot.Device{spheroDriver}, + work, + ) + + return robot + } + + func main() { + master := gobot.NewMaster() + api.NewAPI(master).Start() + + spheros := []string{ + "/dev/rfcomm0", + "/dev/rfcomm1", + "/dev/rfcomm2", + "/dev/rfcomm3", + } + + for _, port := range spheros { + master.AddRobot(NewSwarmBot(port)) + } + + master.Start() + } Copyright (c) 2013-2018 The Hybrid Group. Licensed under the Apache 2.0 license. */ diff --git a/drivers/gpio/MIGRATION.md b/drivers/MIGRATION.md similarity index 68% rename from drivers/gpio/MIGRATION.md rename to drivers/MIGRATION.md index c493e1b4a..2d80d434d 100644 --- a/drivers/gpio/MIGRATION.md +++ b/drivers/MIGRATION.md @@ -1,15 +1,83 @@ -# Migration of GPIO drivers +# Migration of drivers From time to time a breaking change of API can happen. Following to [SemVer](https://semver.org/), the gobot main version should be increased. In such case all users needs to adjust there projects for the next update, although they not using a driver with changed API. -To prevent this scenario for most users, the main version will not always increased, but affected GPIO drivers are listed +To prevent this scenario for most users, the main version will not always increased, but affected drivers are listed here and a migration strategy is provided. -## Switch from version 2.2.0 +## Switch from version 2.3.0 (ble and sphero adaptors affected) -### ButtonDriver, PIRMotionDriver: substitute parameter "v time.duration" +### BLE drivers and client adaptor + +All BLE drivers now can be found in the folder "drivers/ble". Formerly the drivers are located below "platforms/ble". +In addition the location of the BLE client adaptor was changed to "platforms/bleclient". Therefore a change for the +import paths is needed. The constructor function was also renamed, see below. + +```go +// old +import( + ... + "gobot.io/x/gobot/v2/platforms/ble" + ... +) + +... + bleAdaptor := ble.NewClientAdaptor(os.Args[1]) +... + +// new +import( + ... + "gobot.io/x/gobot/v2/drivers/ble" + "gobot.io/x/gobot/v2/platforms/bleclient" + ... +) +... + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) +... +``` + +### Sphero adaptor split off + +The Serial Based Sphero adaptor was split off into a generic serial adaptor and the driver part. With this, the imports +needs to be adjusted. In addition all events now have a postfix "Event", see below. + +```go +// old +import( + ... + "gobot.io/x/gobot/v2/platforms/sphero" + ... +) + +... + adaptor := sphero.NewAdaptor("/dev/rfcomm0") + spheroDriver := sphero.NewSpheroDriver(adaptor) +... + spheroDriver.On(sphero.Collision, func(data interface{}) { +... + +// new +import( + ... + "gobot.io/x/gobot/v2/drivers/common/sphero" + "gobot.io/x/gobot/v2/drivers/serial" + "gobot.io/x/gobot/v2/platforms/serialport" + ... +) +... + adaptor := serialport.NewAdaptor("/dev/rfcomm0") + spheroDriver := serial.NewSpheroDriver(adaptor) +... + spheroDriver.On(sphero.CollisionEvent, func(data interface{}) { +... +``` + +## Switch from version 2.2.0 (gpio drivers affected) + +### gpio.ButtonDriver, gpio.PIRMotionDriver: substitute parameter "v time.duration" A backward compatible case is still included, but it is recommended to use "WithButtonPollInterval" instead, see example below. @@ -22,7 +90,7 @@ d := gpio.NewButtonDriver(adaptor, "1", 50*time.Millisecond) d := gpio.NewButtonDriver(adaptor, "1", gpio.WithButtonPollInterval(50*time.Millisecond)) ``` -### EasyDriver: optional pins +### gpio.EasyDriver: optional pins There is no need to use the direction, enable or sleep feature of the driver. Therefore the parameters are removed from constructor. Please migrate according to the examples below. The order of the optional functions does not matter. @@ -42,7 +110,7 @@ d3 := gpio.NewEasyDriver(adaptor, 0.83, "31", gpio.WithEasyDirectionPin("32"), g gpio.WithEasySleepPin("34")) ``` -### BuzzerDriver: unexport 'BPM' attribute +### gpio.BuzzerDriver: unexport 'BPM' attribute ```go d := gpio.NewBuzzerDriver(adaptor, "1") @@ -55,7 +123,7 @@ d.SetBPM(120.0) fmt.Println("BPM:", d.BPM()) ``` -### RelayDriver: unexport 'Inverted' attribute +### gpio.RelayDriver: unexport 'Inverted' attribute Usually the relay is inverted or not, except be rewired. From now on the inverted behavior can only be changed on initialization. If there is really a different use case, please file a new issue. @@ -71,7 +139,7 @@ d := gpio.NewRelayDriver(adaptor, "1", gpio.WithRelayInverted()) fmt.Println("is inverted:", d.IsInverted()) ``` -### HD44780Driver: make 'SetRWPin()' an option +### gpio.HD44780Driver: make 'SetRWPin()' an option ```go // old @@ -82,7 +150,7 @@ d.SetRWPin("10") d := gpio.NewHD44780Driver(adaptor, ..., gpio.WithHD44780RWPin("10")) ``` -### ServoDriver: unexport 'CurrentAngle' and rename functions 'Min()', 'Max()', 'Center()' +### gpio.ServoDriver: unexport 'CurrentAngle' and rename functions 'Min()', 'Max()', 'Center()' ```go d := gpio.NewServoDriver(adaptor, "1") @@ -99,7 +167,7 @@ d.ToCenter() d.ToMax() ``` -### MotorDriver: unexport pin and state attributes, rename functions +### gpio.MotorDriver: unexport pin and state attributes, rename functions The motor driver was heavily revised - sorry for the inconveniences. diff --git a/drivers/ble/LICENSE b/drivers/ble/LICENSE new file mode 100644 index 000000000..257d22e97 --- /dev/null +++ b/drivers/ble/LICENSE @@ -0,0 +1,13 @@ +Copyright (c) 2014-2018 The Hybrid Group + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. diff --git a/drivers/ble/README.md b/drivers/ble/README.md new file mode 100644 index 000000000..a45b41f98 --- /dev/null +++ b/drivers/ble/README.md @@ -0,0 +1,28 @@ +# BLE + +This package provides drivers for [Bluetooth LE (BLE)](http://en.wikipedia.org/wiki/Bluetooth_low_energy) +devices. It is normally used by connecting an [BLE client adaptor](https://github.com/hybridgroup/gobot/tree/release/platforms/bleclient) +that supports the needed interfaces for BLE devices. + +## Getting Started + +Please refer to the main [README.md](https://github.com/hybridgroup/gobot/blob/release/README.md) and the README files +in the subfolders. + +## Hardware Support + +Gobot has a extensible system for connecting to hardware devices. The following BLE devices are currently supported: + +- Battery Service +- Device Information Service +- Generic Access Service +- Microbit: Accelerometer +- Microbit: Button +- Microbit: IO Pin +- Microbit: LED +- Microbit: Magnetometer +- Microbit: Temperature +- Serial Port over BLE +- Sphero: BB8 +- Sphero: Ollie +- Sphero: SPRK+ diff --git a/drivers/ble/battery_driver.go b/drivers/ble/battery_driver.go new file mode 100644 index 000000000..f97ecafef --- /dev/null +++ b/drivers/ble/battery_driver.go @@ -0,0 +1,39 @@ +package ble + +import ( + "bytes" + "log" + + "gobot.io/x/gobot/v2" +) + +const batteryCharaShort = "2a19" + +// BatteryDriver represents the battery service for a BLE peripheral +type BatteryDriver struct { + *Driver + gobot.Eventer +} + +// NewBatteryDriver creates a new driver +func NewBatteryDriver(a gobot.BLEConnector) *BatteryDriver { + d := &BatteryDriver{ + Driver: NewDriver(a, "Battery", nil, nil), + Eventer: gobot.NewEventer(), + } + + return d +} + +// GetBatteryLevel reads and returns the current battery level +func (d *BatteryDriver) GetBatteryLevel() uint8 { + c, err := d.Adaptor().ReadCharacteristic(batteryCharaShort) + if err != nil { + log.Println(err) + return 0 + } + buf := bytes.NewBuffer(c) + val, _ := buf.ReadByte() + level := val + return level +} diff --git a/drivers/ble/battery_driver_test.go b/drivers/ble/battery_driver_test.go new file mode 100644 index 000000000..6495317ff --- /dev/null +++ b/drivers/ble/battery_driver_test.go @@ -0,0 +1,33 @@ +package ble + +import ( + "strings" + "testing" + + "github.com/stretchr/testify/assert" + + "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/ble/testutil" +) + +var _ gobot.Driver = (*BatteryDriver)(nil) + +func TestNewBatteryDriver(t *testing.T) { + d := NewBatteryDriver(testutil.NewBleTestAdaptor()) + assert.True(t, strings.HasPrefix(d.Name(), "Battery")) + assert.NotNil(t, d.Eventer) +} + +func TestBatteryDriverRead(t *testing.T) { + a := testutil.NewBleTestAdaptor() + d := NewBatteryDriver(a) + a.SetReadCharacteristicTestFunc(func(cUUID string) ([]byte, error) { + if cUUID == "2a19" { + return []byte{20}, nil + } + + return nil, nil + }) + + assert.Equal(t, uint8(20), d.GetBatteryLevel()) +} diff --git a/drivers/ble/ble_driver.go b/drivers/ble/ble_driver.go new file mode 100644 index 000000000..e1b766b36 --- /dev/null +++ b/drivers/ble/ble_driver.go @@ -0,0 +1,118 @@ +package ble + +import ( + "log" + "sync" + + "gobot.io/x/gobot/v2" +) + +// optionApplier needs to be implemented by each configurable option type +type optionApplier interface { + apply(cfg *configuration) +} + +// configuration contains all changeable attributes of the driver. +type configuration struct { + name string +} + +// nameOption is the type for applying another name to the configuration +type nameOption string + +// Driver implements the interface gobot.Driver. +type Driver struct { + gobot.Commander + connection interface{} + driverCfg *configuration + afterStart func() error + beforeHalt func() error + mutex *sync.Mutex +} + +// NewDriver creates a new basic BLE gobot driver. +func NewDriver(a interface{}, name string, afterStart func() error, beforeHalt func() error) *Driver { + if afterStart == nil { + afterStart = func() error { return nil } + } + + if beforeHalt == nil { + beforeHalt = func() error { return nil } + } + + d := Driver{ + driverCfg: &configuration{name: gobot.DefaultName(name)}, + connection: a, + afterStart: afterStart, + beforeHalt: beforeHalt, + Commander: gobot.NewCommander(), + mutex: &sync.Mutex{}, + } + + return &d +} + +// WithName is used to replace the default name of the driver. +func WithName(name string) optionApplier { + return nameOption(name) +} + +// Name returns the name of the driver. +func (d *Driver) Name() string { + return d.driverCfg.name +} + +// SetName sets the name of the driver. +// Deprecated: Please use option [aio.WithName] instead. +func (d *Driver) SetName(name string) { + WithName(name).apply(d.driverCfg) +} + +// Connection returns the connection of the driver. +func (d *Driver) Connection() gobot.Connection { + if conn, ok := d.connection.(gobot.Connection); ok { + return conn + } + + log.Printf("%s has no gobot connection\n", d.driverCfg.name) + return nil +} + +// Start initializes the driver. +func (d *Driver) Start() error { + d.mutex.Lock() + defer d.mutex.Unlock() + + // currently there is nothing to do here for the driver + + return d.afterStart() +} + +// Halt halts the driver. +func (d *Driver) Halt() error { + d.mutex.Lock() + defer d.mutex.Unlock() + + // currently there is nothing to do after halt for the driver + + return d.beforeHalt() +} + +// Adaptor returns the BLE adaptor +func (d *Driver) Adaptor() gobot.BLEConnector { + if a, ok := d.connection.(gobot.BLEConnector); ok { + return a + } + + log.Printf("%s has no BLE connector\n", d.driverCfg.name) + return nil +} + +func (d *Driver) Mutex() *sync.Mutex { + return d.mutex +} + +// apply change the name in the configuration. +func (o nameOption) apply(c *configuration) { + c.name = string(o) +} diff --git a/drivers/ble/ble_driver_test.go b/drivers/ble/ble_driver_test.go new file mode 100644 index 000000000..5e40f313b --- /dev/null +++ b/drivers/ble/ble_driver_test.go @@ -0,0 +1,70 @@ +package ble + +import ( + "fmt" + "strings" + "testing" + + "github.com/stretchr/testify/assert" + "github.com/stretchr/testify/require" + + "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/ble/testutil" +) + +var _ gobot.Driver = (*Driver)(nil) + +func initTestDriver() *Driver { + a := testutil.NewBleTestAdaptor() + d := NewDriver(a, "BLE_BASIC", nil, nil) + return d +} + +func TestNewDriver(t *testing.T) { + // arrange + const name = "mybot" + a := testutil.NewBleTestAdaptor() + // act + d := NewDriver(a, name, nil, nil) + // assert + assert.IsType(t, &Driver{}, d) + assert.NotNil(t, d.driverCfg) + assert.True(t, strings.HasPrefix(d.Name(), name)) + assert.Equal(t, a, d.Connection()) + require.NoError(t, d.afterStart()) + require.NoError(t, d.beforeHalt()) + assert.NotNil(t, d.Commander) + assert.NotNil(t, d.mutex) +} + +func Test_applyWithName(t *testing.T) { + // arrange + const name = "mybot" + cfg := configuration{name: "oldname"} + // act + WithName(name).apply(&cfg) + // assert + assert.Equal(t, name, cfg.name) +} + +func TestStart(t *testing.T) { + // arrange + d := initTestDriver() + // act, assert + require.NoError(t, d.Start()) + // arrange after start function + d.afterStart = func() error { return fmt.Errorf("after start error") } + // act, assert + require.EqualError(t, d.Start(), "after start error") +} + +func TestHalt(t *testing.T) { + // arrange + d := initTestDriver() + // act, assert + require.NoError(t, d.Halt()) + // arrange after start function + d.beforeHalt = func() error { return fmt.Errorf("before halt error") } + // act, assert + require.EqualError(t, d.Halt(), "before halt error") +} diff --git a/drivers/ble/device_information_driver.go b/drivers/ble/device_information_driver.go new file mode 100644 index 000000000..8eca3c21b --- /dev/null +++ b/drivers/ble/device_information_driver.go @@ -0,0 +1,92 @@ +package ble + +import ( + "bytes" + "log" + + "gobot.io/x/gobot/v2" +) + +const ( + deviceInformationModelNumberCharaShort = "2a24" + deviceInformationFirmwareRevisionCharaShort = "2a26" + deviceInformationHardwareRevisionCharaShort = "2a27" + deviceInformationManufacturerNameCharaShort = "2a29" + deviceInformationPnPIdCharaShort = "2a50" +) + +// DeviceInformationDriver represents the device information service for a BLE peripheral +type DeviceInformationDriver struct { + *Driver + gobot.Eventer +} + +// NewDeviceInformationDriver creates a new driver +func NewDeviceInformationDriver(a gobot.BLEConnector) *DeviceInformationDriver { + n := &DeviceInformationDriver{ + Driver: NewDriver(a, "DeviceInformation", nil, nil), + Eventer: gobot.NewEventer(), + } + + return n +} + +// GetModelNumber returns the model number for the BLE Peripheral +func (d *DeviceInformationDriver) GetModelNumber() string { + c, err := d.Adaptor().ReadCharacteristic(deviceInformationModelNumberCharaShort) + if err != nil { + log.Println(err) + return "" + } + buf := bytes.NewBuffer(c) + model := buf.String() + return model +} + +// GetFirmwareRevision returns the firmware revision for the BLE Peripheral +func (d *DeviceInformationDriver) GetFirmwareRevision() string { + c, err := d.Adaptor().ReadCharacteristic(deviceInformationFirmwareRevisionCharaShort) + if err != nil { + log.Println(err) + return "" + } + buf := bytes.NewBuffer(c) + val := buf.String() + return val +} + +// GetHardwareRevision returns the hardware revision for the BLE Peripheral +func (d *DeviceInformationDriver) GetHardwareRevision() string { + c, err := d.Adaptor().ReadCharacteristic(deviceInformationHardwareRevisionCharaShort) + if err != nil { + log.Println(err) + return "" + } + buf := bytes.NewBuffer(c) + val := buf.String() + return val +} + +// GetManufacturerName returns the manufacturer name for the BLE Peripheral +func (d *DeviceInformationDriver) GetManufacturerName() string { + c, err := d.Adaptor().ReadCharacteristic(deviceInformationManufacturerNameCharaShort) + if err != nil { + log.Println(err) + return "" + } + buf := bytes.NewBuffer(c) + val := buf.String() + return val +} + +// GetPnPId returns the PnP ID for the BLE Peripheral +func (d *DeviceInformationDriver) GetPnPId() string { + c, err := d.Adaptor().ReadCharacteristic(deviceInformationPnPIdCharaShort) + if err != nil { + log.Println(err) + return "" + } + buf := bytes.NewBuffer(c) + val := buf.String() + return val +} diff --git a/drivers/ble/device_information_driver_test.go b/drivers/ble/device_information_driver_test.go new file mode 100644 index 000000000..976849e6c --- /dev/null +++ b/drivers/ble/device_information_driver_test.go @@ -0,0 +1,44 @@ +package ble + +import ( + "strings" + "testing" + + "github.com/stretchr/testify/assert" + + "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/ble/testutil" +) + +var _ gobot.Driver = (*DeviceInformationDriver)(nil) + +func TestNewDeviceInformationDriver(t *testing.T) { + d := NewDeviceInformationDriver(testutil.NewBleTestAdaptor()) + assert.True(t, strings.HasPrefix(d.Name(), "DeviceInformation")) + assert.NotNil(t, d.Eventer) +} + +func TestDeviceInformationGetModelNumber(t *testing.T) { + d := NewDeviceInformationDriver(testutil.NewBleTestAdaptor()) + assert.Equal(t, "2a24", d.GetModelNumber()) +} + +func TestDeviceInformationGetFirmwareRevision(t *testing.T) { + d := NewDeviceInformationDriver(testutil.NewBleTestAdaptor()) + assert.Equal(t, "2a26", d.GetFirmwareRevision()) +} + +func TestDeviceInformationGetHardwareRevision(t *testing.T) { + d := NewDeviceInformationDriver(testutil.NewBleTestAdaptor()) + assert.Equal(t, "2a27", d.GetHardwareRevision()) +} + +func TestDeviceInformationGetManufacturerName(t *testing.T) { + d := NewDeviceInformationDriver(testutil.NewBleTestAdaptor()) + assert.Equal(t, "2a29", d.GetManufacturerName()) +} + +func TestDeviceInformationGetPnPId(t *testing.T) { + d := NewDeviceInformationDriver(testutil.NewBleTestAdaptor()) + assert.Equal(t, "2a50", d.GetPnPId()) +} diff --git a/drivers/ble/doc.go b/drivers/ble/doc.go new file mode 100644 index 000000000..45a0afc11 --- /dev/null +++ b/drivers/ble/doc.go @@ -0,0 +1,7 @@ +/* +Package ble provides the Gobot drivers for several Bluetooth LE Services. + +For more information refer to the README: +https://github.com/hybridgroup/gobot/blob/release/drivers/ble/README.md +*/ +package ble // import "gobot.io/x/gobot/v2/drivers/ble" diff --git a/platforms/ble/generic_access_driver.go b/drivers/ble/generic_access_driver.go similarity index 65% rename from platforms/ble/generic_access_driver.go rename to drivers/ble/generic_access_driver.go index 3b3f40c08..776db09ce 100644 --- a/platforms/ble/generic_access_driver.go +++ b/drivers/ble/generic_access_driver.go @@ -8,48 +8,30 @@ import ( "gobot.io/x/gobot/v2" ) +const ( + genericAccessDeviceNameCharaShort = "2a00" + genericAccessAppearanceCharaShort = "2a01" +) + // GenericAccessDriver represents the Generic Access Service for a BLE Peripheral type GenericAccessDriver struct { - name string - connection gobot.Connection + *Driver gobot.Eventer } // NewGenericAccessDriver creates a GenericAccessDriver -func NewGenericAccessDriver(a BLEConnector) *GenericAccessDriver { - n := &GenericAccessDriver{ - name: gobot.DefaultName("GenericAccess"), - connection: a, - Eventer: gobot.NewEventer(), +func NewGenericAccessDriver(a gobot.BLEConnector) *GenericAccessDriver { + d := &GenericAccessDriver{ + Driver: NewDriver(a, "GenericAccess", nil, nil), + Eventer: gobot.NewEventer(), } - return n + return d } -// Connection returns the Driver's Connection to the associated Adaptor -func (b *GenericAccessDriver) Connection() gobot.Connection { return b.connection } - -// Name returns the Driver name -func (b *GenericAccessDriver) Name() string { return b.name } - -// SetName sets the Driver name -func (b *GenericAccessDriver) SetName(n string) { b.name = n } - -// adaptor returns BLE adaptor for this device -func (b *GenericAccessDriver) adaptor() BLEConnector { - //nolint:forcetypeassert // ok here - return b.Connection().(BLEConnector) -} - -// Start tells driver to get ready to do work -func (b *GenericAccessDriver) Start() error { return nil } - -// Halt stops driver (void) -func (b *GenericAccessDriver) Halt() error { return nil } - // GetDeviceName returns the device name for the BLE Peripheral -func (b *GenericAccessDriver) GetDeviceName() string { - c, err := b.adaptor().ReadCharacteristic("2a00") +func (d *GenericAccessDriver) GetDeviceName() string { + c, err := d.Adaptor().ReadCharacteristic(genericAccessDeviceNameCharaShort) if err != nil { log.Println(err) return "" @@ -61,8 +43,8 @@ func (b *GenericAccessDriver) GetDeviceName() string { } // GetAppearance returns the appearance string for the BLE Peripheral -func (b *GenericAccessDriver) GetAppearance() string { - c, err := b.adaptor().ReadCharacteristic("2a01") +func (d *GenericAccessDriver) GetAppearance() string { + c, err := d.Adaptor().ReadCharacteristic(genericAccessAppearanceCharaShort) if err != nil { log.Println(err) return "" diff --git a/drivers/ble/generic_access_driver_test.go b/drivers/ble/generic_access_driver_test.go new file mode 100644 index 000000000..845cc5881 --- /dev/null +++ b/drivers/ble/generic_access_driver_test.go @@ -0,0 +1,37 @@ +package ble + +import ( + "strings" + "testing" + + "github.com/stretchr/testify/assert" + + "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/ble/testutil" +) + +var _ gobot.Driver = (*GenericAccessDriver)(nil) + +func TestNewGenericAccessDriver(t *testing.T) { + d := NewGenericAccessDriver(testutil.NewBleTestAdaptor()) + assert.True(t, strings.HasPrefix(d.Name(), "GenericAccess")) + assert.NotNil(t, d.Eventer) +} + +func TestGenericAccessDriverGetDeviceName(t *testing.T) { + d := NewGenericAccessDriver(testutil.NewBleTestAdaptor()) + assert.Equal(t, "2a00", d.GetDeviceName()) +} + +func TestGenericAccessDriverGetAppearance(t *testing.T) { + a := testutil.NewBleTestAdaptor() + d := NewGenericAccessDriver(a) + a.SetReadCharacteristicTestFunc(func(cUUID string) ([]byte, error) { + if cUUID == "2a01" { + return []byte{128, 0}, nil + } + return nil, nil + }) + + assert.Equal(t, "Generic Computer", d.GetAppearance()) +} diff --git a/drivers/ble/microbit/LICENSE b/drivers/ble/microbit/LICENSE new file mode 100644 index 000000000..257d22e97 --- /dev/null +++ b/drivers/ble/microbit/LICENSE @@ -0,0 +1,13 @@ +Copyright (c) 2014-2018 The Hybrid Group + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. diff --git a/drivers/ble/microbit/accelerometer_driver.go b/drivers/ble/microbit/accelerometer_driver.go new file mode 100644 index 000000000..634bee816 --- /dev/null +++ b/drivers/ble/microbit/accelerometer_driver.go @@ -0,0 +1,67 @@ +package microbit + +import ( + "bytes" + "encoding/binary" + + "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/ble" +) + +const ( + // accelerometerService = "e95d0753251d470aa062fa1922dfa9a8" + accelerometerChara = "e95dca4b251d470aa062fa1922dfa9a8" + + AccelerometerEvent = "accelerometer" +) + +// AccelerometerDriver is the Gobot driver for the Microbit's built-in accelerometer +type AccelerometerDriver struct { + *ble.Driver + gobot.Eventer +} + +type AccelerometerData struct { + X float32 + Y float32 + Z float32 +} + +// NewAccelerometerDriver creates a AccelerometerDriver +func NewAccelerometerDriver(a gobot.BLEConnector) *AccelerometerDriver { + d := &AccelerometerDriver{ + Eventer: gobot.NewEventer(), + } + d.Driver = ble.NewDriver(a, "Microbit Accelerometer", d.initialize, nil) + + d.AddEvent(AccelerometerEvent) + + return d +} + +// initialize tells driver to get ready to do work +func (d *AccelerometerDriver) initialize() error { + // subscribe to accelerometer notifications + return d.Adaptor().Subscribe(accelerometerChara, func(data []byte, e error) { + a := struct{ x, y, z int16 }{x: 0, y: 0, z: 0} + + buf := bytes.NewBuffer(data) + if err := binary.Read(buf, binary.LittleEndian, &a.x); err != nil { + panic(err) + } + if err := binary.Read(buf, binary.LittleEndian, &a.y); err != nil { + panic(err) + } + if err := binary.Read(buf, binary.LittleEndian, &a.z); err != nil { + panic(err) + } + + result := &AccelerometerData{ + X: float32(a.x) / 1000.0, + Y: float32(a.y) / 1000.0, + Z: float32(a.z) / 1000.0, + } + + d.Publish(d.Event(AccelerometerEvent), result) + }) +} diff --git a/platforms/microbit/accelerometer_driver_test.go b/drivers/ble/microbit/accelerometer_driver_test.go similarity index 57% rename from platforms/microbit/accelerometer_driver_test.go rename to drivers/ble/microbit/accelerometer_driver_test.go index 33ea547a5..fd59c2528 100644 --- a/platforms/microbit/accelerometer_driver_test.go +++ b/drivers/ble/microbit/accelerometer_driver_test.go @@ -10,41 +10,40 @@ import ( "github.com/stretchr/testify/require" "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/ble/testutil" ) var _ gobot.Driver = (*AccelerometerDriver)(nil) -func initTestAccelerometerDriver() *AccelerometerDriver { - d := NewAccelerometerDriver(NewBleTestAdaptor()) - return d -} - -func TestAccelerometerDriver(t *testing.T) { - d := initTestAccelerometerDriver() +func TestNewAccelerometerDriver(t *testing.T) { + d := NewAccelerometerDriver(testutil.NewBleTestAdaptor()) + assert.IsType(t, &AccelerometerDriver{}, d) assert.True(t, strings.HasPrefix(d.Name(), "Microbit Accelerometer")) - d.SetName("NewName") - assert.Equal(t, "NewName", d.Name()) + assert.NotNil(t, d.Eventer) } -func TestAccelerometerDriverStartAndHalt(t *testing.T) { - d := initTestAccelerometerDriver() +func TestAccelerometerStartAndHalt(t *testing.T) { + d := NewAccelerometerDriver(testutil.NewBleTestAdaptor()) require.NoError(t, d.Start()) require.NoError(t, d.Halt()) } -func TestAccelerometerDriverReadData(t *testing.T) { +func TestAccelerometerReadData(t *testing.T) { sem := make(chan bool) - a := NewBleTestAdaptor() + a := testutil.NewBleTestAdaptor() d := NewAccelerometerDriver(a) - _ = d.Start() - _ = d.On(Accelerometer, func(data interface{}) { + require.NoError(t, d.Start()) + + err := d.On("accelerometer", func(data interface{}) { assert.InDelta(t, float32(8.738), data.(*AccelerometerData).X, 0.0) assert.InDelta(t, float32(8.995), data.(*AccelerometerData).Y, 0.0) assert.InDelta(t, float32(9.252), data.(*AccelerometerData).Z, 0.0) sem <- true }) - a.TestReceiveNotification([]byte{0x22, 0x22, 0x23, 0x23, 0x24, 0x24}, nil) + require.NoError(t, err) + + a.SendTestDataToSubscriber([]byte{0x22, 0x22, 0x23, 0x23, 0x24, 0x24}, nil) select { case <-sem: diff --git a/drivers/ble/microbit/button_driver.go b/drivers/ble/microbit/button_driver.go new file mode 100644 index 000000000..a095f3828 --- /dev/null +++ b/drivers/ble/microbit/button_driver.go @@ -0,0 +1,50 @@ +package microbit + +import ( + "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/ble" +) + +const ( + // buttonService = "e95d9882251d470aa062fa1922dfa9a8" + buttonAChara = "e95dda90251d470aa062fa1922dfa9a8" + buttonBChara = "e95dda91251d470aa062fa1922dfa9a8" + + ButtonAEvent = "buttonA" + ButtonBEvent = "buttonB" +) + +// ButtonDriver is the Gobot driver for the Microbit's built-in buttons +type ButtonDriver struct { + *ble.Driver + gobot.Eventer +} + +// NewButtonDriver creates a new driver +func NewButtonDriver(a gobot.BLEConnector) *ButtonDriver { + d := &ButtonDriver{ + Eventer: gobot.NewEventer(), + } + + d.Driver = ble.NewDriver(a, "Microbit Button", d.initialize, nil) + + d.AddEvent(ButtonAEvent) + d.AddEvent(ButtonBEvent) + + return d +} + +// initialize tells driver to get ready to do work +func (d *ButtonDriver) initialize() error { + // subscribe to button A notifications + if err := d.Adaptor().Subscribe(buttonAChara, func(data []byte, e error) { + d.Publish(d.Event(ButtonAEvent), data) + }); err != nil { + return err + } + + // subscribe to button B notifications + return d.Adaptor().Subscribe(buttonBChara, func(data []byte, e error) { + d.Publish(d.Event(ButtonBEvent), data) + }) +} diff --git a/platforms/microbit/button_driver_test.go b/drivers/ble/microbit/button_driver_test.go similarity index 50% rename from platforms/microbit/button_driver_test.go rename to drivers/ble/microbit/button_driver_test.go index be92c0ed7..886bc05e9 100644 --- a/platforms/microbit/button_driver_test.go +++ b/drivers/ble/microbit/button_driver_test.go @@ -9,38 +9,36 @@ import ( "github.com/stretchr/testify/require" "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/ble/testutil" ) var _ gobot.Driver = (*ButtonDriver)(nil) -func initTestButtonDriver() *ButtonDriver { - d := NewButtonDriver(NewBleTestAdaptor()) - return d -} - -func TestButtonDriver(t *testing.T) { - d := initTestButtonDriver() +func TestNewButtonDriver(t *testing.T) { + d := NewButtonDriver(testutil.NewBleTestAdaptor()) + assert.IsType(t, &ButtonDriver{}, d) assert.True(t, strings.HasPrefix(d.Name(), "Microbit Button")) - d.SetName("NewName") - assert.Equal(t, "NewName", d.Name()) + assert.NotNil(t, d.Eventer) } -func TestButtonDriverStartAndHalt(t *testing.T) { - d := initTestButtonDriver() +func TestButtonStartAndHalt(t *testing.T) { + d := NewButtonDriver(testutil.NewBleTestAdaptor()) require.NoError(t, d.Start()) require.NoError(t, d.Halt()) } -func TestButtonDriverReadData(t *testing.T) { +func TestButtonReadData(t *testing.T) { sem := make(chan bool) - a := NewBleTestAdaptor() + a := testutil.NewBleTestAdaptor() d := NewButtonDriver(a) - _ = d.Start() - _ = d.On(ButtonB, func(data interface{}) { + require.NoError(t, d.Start()) + + err := d.On("buttonB", func(data interface{}) { sem <- true }) + require.NoError(t, err) - a.TestReceiveNotification([]byte{1}, nil) + a.SendTestDataToSubscriber([]byte{1}, nil) select { case <-sem: diff --git a/drivers/ble/microbit/doc.go b/drivers/ble/microbit/doc.go new file mode 100644 index 000000000..285a0ca6d --- /dev/null +++ b/drivers/ble/microbit/doc.go @@ -0,0 +1,7 @@ +/* +Package microbit contains the Gobot drivers for the Microbit platform. + +For more information refer to the microbit README: +https://github.com/hybridgroup/gobot/blob/release/platforms/microbit/README.md +*/ +package microbit // import "gobot.io/x/gobot/v2/drivers/ble/microbit" diff --git a/drivers/ble/microbit/io_pin_driver.go b/drivers/ble/microbit/io_pin_driver.go new file mode 100644 index 000000000..883568fc3 --- /dev/null +++ b/drivers/ble/microbit/io_pin_driver.go @@ -0,0 +1,250 @@ +package microbit + +import ( + "bytes" + "encoding/binary" + "errors" + "strconv" + + "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/ble" + "gobot.io/x/gobot/v2/drivers/common/bit" +) + +const ( + // ioPinService = "e95d127b251d470aa062fa1922dfa9a8" + pinDataChara = "e95d8d00251d470aa062fa1922dfa9a8" + pinADConfigChara = "e95d5899251d470aa062fa1922dfa9a8" + pinIOConfigChara = "e95db9fe251d470aa062fa1922dfa9a8" +) + +// IOPinDriver is the Gobot driver for the Microbit's built-in digital and analog I/O +type IOPinDriver struct { + *ble.Driver + adMask int + ioMask int + gobot.Eventer +} + +// pinData has the read data for a specific digital pin +type pinData struct { + pin uint8 + value uint8 +} + +// NewIOPinDriver creates a new driver +func NewIOPinDriver(a gobot.BLEConnector) *IOPinDriver { + d := &IOPinDriver{ + Eventer: gobot.NewEventer(), + } + + d.Driver = ble.NewDriver(a, "Microbit IO Pins", d.initialize, nil) + + return d +} + +// initialize tells driver to get ready to do work +func (d *IOPinDriver) initialize() error { + if _, err := d.ReadPinADConfig(); err != nil { + return err + } + _, err := d.ReadPinIOConfig() + return err +} + +// WritePinData writes the pin data for a single pin +func (d *IOPinDriver) WritePinData(pin string, data byte) error { + i, err := strconv.Atoi(pin) + if err != nil { + return err + } + + buf := []byte{byte(i), data} + err = d.Adaptor().WriteCharacteristic(pinDataChara, buf) + return err +} + +// ReadPinADConfig reads and returns the pin A/D config mask for all pins +func (d *IOPinDriver) ReadPinADConfig() (int, error) { + c, err := d.Adaptor().ReadCharacteristic(pinADConfigChara) + if err != nil { + return 0, err + } + var result byte + for i := 0; i < 4; i++ { + result |= c[i] << uint(i) + } + + d.adMask = int(result) + return int(result), nil +} + +// WritePinADConfig writes the pin A/D config mask for all pins +func (d *IOPinDriver) WritePinADConfig(config int) error { + d.adMask = config + data := &bytes.Buffer{} + if err := binary.Write(data, binary.LittleEndian, uint32(config)); err != nil { + return err + } + + return d.Adaptor().WriteCharacteristic(pinADConfigChara, data.Bytes()) +} + +// ReadPinIOConfig reads and returns the pin IO config mask for all pins +func (d *IOPinDriver) ReadPinIOConfig() (int, error) { + c, err := d.Adaptor().ReadCharacteristic(pinIOConfigChara) + if err != nil { + return 0, err + } + + var result byte + for i := 0; i < 4; i++ { + result |= c[i] << uint(i) + } + + d.ioMask = int(result) + return int(result), nil +} + +// WritePinIOConfig writes the pin I/O config mask for all pins +func (d *IOPinDriver) WritePinIOConfig(config int) error { + d.ioMask = config + data := &bytes.Buffer{} + if err := binary.Write(data, binary.LittleEndian, uint32(config)); err != nil { + return err + } + + return d.Adaptor().WriteCharacteristic(pinIOConfigChara, data.Bytes()) +} + +// DigitalRead reads from a pin +func (d *IOPinDriver) DigitalRead(pin string) (int, error) { + p, err := validatedPin(pin) + if err != nil { + return 0, err + } + + if err := d.ensureDigital(p); err != nil { + return 0, err + } + if err := d.ensureInput(p); err != nil { + return 0, err + } + + pins, err := d.readAllPinData() + if err != nil { + return 0, err + } + + return int(pins[p].value), nil +} + +// DigitalWrite writes to a pin +func (d *IOPinDriver) DigitalWrite(pin string, level byte) error { + p, err := validatedPin(pin) + if err != nil { + return err + } + + if err := d.ensureDigital(p); err != nil { + return err + } + if err := d.ensureOutput(p); err != nil { + return err + } + + return d.WritePinData(pin, level) +} + +// AnalogRead reads from a pin +func (d *IOPinDriver) AnalogRead(pin string) (int, error) { + p, err := validatedPin(pin) + if err != nil { + return 0, err + } + + if err := d.ensureAnalog(p); err != nil { + return 0, err + } + if err := d.ensureInput(p); err != nil { + return 0, err + } + + pins, err := d.readAllPinData() + if err != nil { + return 0, err + } + + return int(pins[p].value), nil +} + +func (d *IOPinDriver) ensureDigital(pin int) error { + if bit.IsSet(d.adMask, uint8(pin)) { + return d.WritePinADConfig(bit.Clear(d.adMask, uint8(pin))) + } + + return nil +} + +func (d *IOPinDriver) ensureAnalog(pin int) error { + if !bit.IsSet(d.adMask, uint8(pin)) { + return d.WritePinADConfig(bit.Set(d.adMask, uint8(pin))) + } + + return nil +} + +func (d *IOPinDriver) ensureInput(pin int) error { + if !bit.IsSet(d.ioMask, uint8(pin)) { + return d.WritePinIOConfig(bit.Set(d.ioMask, uint8(pin))) + } + + return nil +} + +func (d *IOPinDriver) ensureOutput(pin int) error { + if bit.IsSet(d.ioMask, uint8(pin)) { + return d.WritePinIOConfig(bit.Clear(d.ioMask, uint8(pin))) + } + + return nil +} + +func (d *IOPinDriver) readAllPinData() ([]pinData, error) { + c, _ := d.Adaptor().ReadCharacteristic(pinDataChara) + buf := bytes.NewBuffer(c) + pinsData := make([]pinData, buf.Len()/2) + + for i := 0; i < buf.Len()/2; i++ { + pin, err := buf.ReadByte() + if err != nil { + return nil, err + } + + value, err := buf.ReadByte() + if err != nil { + return nil, err + } + + pinData := pinData{ + pin: pin, + value: value, + } + pinsData[i] = pinData + } + + return pinsData, nil +} + +func validatedPin(pin string) (int, error) { + i, err := strconv.Atoi(pin) + if err != nil { + return 0, err + } + + if i < 0 || i > 2 { + return 0, errors.New("Invalid pin.") + } + + return i, nil +} diff --git a/drivers/ble/microbit/io_pin_driver_test.go b/drivers/ble/microbit/io_pin_driver_test.go new file mode 100644 index 000000000..f73c1d27e --- /dev/null +++ b/drivers/ble/microbit/io_pin_driver_test.go @@ -0,0 +1,165 @@ +package microbit + +import ( + "errors" + "strings" + "testing" + + "github.com/stretchr/testify/assert" + "github.com/stretchr/testify/require" + + "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/aio" + "gobot.io/x/gobot/v2/drivers/ble/testutil" + "gobot.io/x/gobot/v2/drivers/gpio" +) + +// the MicrobitIOPinDriver is a Driver +var _ gobot.Driver = (*IOPinDriver)(nil) + +// that supports the DigitalReader, DigitalWriter, & AnalogReader interfaces +var ( + _ gpio.DigitalReader = (*IOPinDriver)(nil) + _ gpio.DigitalWriter = (*IOPinDriver)(nil) + _ aio.AnalogReader = (*IOPinDriver)(nil) +) + +func TestNewIOPinDriver(t *testing.T) { + d := NewIOPinDriver(testutil.NewBleTestAdaptor()) + assert.IsType(t, &IOPinDriver{}, d) + assert.True(t, strings.HasPrefix(d.Name(), "Microbit IO Pin")) + assert.NotNil(t, d.Eventer) +} + +func TestIOPinStartAndHalt(t *testing.T) { + a := testutil.NewBleTestAdaptor() + d := NewIOPinDriver(a) + a.SetReadCharacteristicTestFunc(func(cUUID string) ([]byte, error) { + return []byte{0, 1, 1, 0}, nil + }) + require.NoError(t, d.Start()) + require.NoError(t, d.Halt()) +} + +func TestIOPinStartError(t *testing.T) { + a := testutil.NewBleTestAdaptor() + d := NewIOPinDriver(a) + a.SetReadCharacteristicTestFunc(func(cUUID string) ([]byte, error) { + return nil, errors.New("read error") + }) + require.ErrorContains(t, d.Start(), "read error") +} + +func TestIOPinDigitalRead(t *testing.T) { + a := testutil.NewBleTestAdaptor() + d := NewIOPinDriver(a) + a.SetReadCharacteristicTestFunc(func(cUUID string) ([]byte, error) { + return []byte{0, 1, 1, 0, 2, 1}, nil + }) + + val, err := d.DigitalRead("0") + require.NoError(t, err) + assert.Equal(t, 1, val) + + val, err = d.DigitalRead("1") + require.NoError(t, err) + assert.Equal(t, 0, val) +} + +func TestIOPinDigitalReadInvalidPin(t *testing.T) { + a := testutil.NewBleTestAdaptor() + d := NewIOPinDriver(a) + + _, err := d.DigitalRead("A3") + require.Error(t, err) + + _, err = d.DigitalRead("6") + require.ErrorContains(t, err, "Invalid pin.") +} + +func TestIOPinDigitalWrite(t *testing.T) { + a := testutil.NewBleTestAdaptor() + d := NewIOPinDriver(a) + + // TODO: a better test + require.NoError(t, d.DigitalWrite("0", 1)) +} + +func TestIOPinDigitalWriteInvalidPin(t *testing.T) { + a := testutil.NewBleTestAdaptor() + d := NewIOPinDriver(a) + + require.Error(t, d.DigitalWrite("A3", 1)) + require.ErrorContains(t, d.DigitalWrite("6", 1), "Invalid pin.") +} + +func TestIOPinAnalogRead(t *testing.T) { + a := testutil.NewBleTestAdaptor() + d := NewIOPinDriver(a) + a.SetReadCharacteristicTestFunc(func(cUUID string) ([]byte, error) { + return []byte{0, 0, 1, 128, 2, 1}, nil + }) + + val, err := d.AnalogRead("0") + require.NoError(t, err) + assert.Equal(t, 0, val) + + val, err = d.AnalogRead("1") + require.NoError(t, err) + assert.Equal(t, 128, val) +} + +func TestIOPinAnalogReadInvalidPin(t *testing.T) { + a := testutil.NewBleTestAdaptor() + d := NewIOPinDriver(a) + + _, err := d.AnalogRead("A3") + require.Error(t, err) + + _, err = d.AnalogRead("6") + require.ErrorContains(t, err, "Invalid pin.") +} + +func TestIOPinDigitalAnalogRead(t *testing.T) { + a := testutil.NewBleTestAdaptor() + d := NewIOPinDriver(a) + a.SetReadCharacteristicTestFunc(func(cUUID string) ([]byte, error) { + return []byte{0, 0, 1, 128, 2, 1}, nil + }) + + val, err := d.DigitalRead("0") + require.NoError(t, err) + assert.Equal(t, 0, val) + + val, err = d.AnalogRead("0") + require.NoError(t, err) + assert.Equal(t, 0, val) +} + +func TestIOPinDigitalWriteAnalogRead(t *testing.T) { + a := testutil.NewBleTestAdaptor() + d := NewIOPinDriver(a) + a.SetReadCharacteristicTestFunc(func(cUUID string) ([]byte, error) { + return []byte{0, 0, 1, 128, 2, 1}, nil + }) + + require.NoError(t, d.DigitalWrite("1", 0)) + + val, err := d.AnalogRead("1") + require.NoError(t, err) + assert.Equal(t, 128, val) +} + +func TestIOPinAnalogReadDigitalWrite(t *testing.T) { + a := testutil.NewBleTestAdaptor() + d := NewIOPinDriver(a) + a.SetReadCharacteristicTestFunc(func(cUUID string) ([]byte, error) { + return []byte{0, 0, 1, 128, 2, 1}, nil + }) + + val, err := d.AnalogRead("1") + require.NoError(t, err) + assert.Equal(t, 128, val) + + require.NoError(t, d.DigitalWrite("1", 0)) +} diff --git a/platforms/microbit/led_driver.go b/drivers/ble/microbit/led_driver.go similarity index 62% rename from platforms/microbit/led_driver.go rename to drivers/ble/microbit/led_driver.go index 83a42afe4..3de8a5b30 100644 --- a/platforms/microbit/led_driver.go +++ b/drivers/ble/microbit/led_driver.go @@ -2,71 +2,46 @@ package microbit import ( "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" + "gobot.io/x/gobot/v2/drivers/ble" +) + +const ( + // ledService = "e95dd91d251d470aa062fa1922dfa9a8" + ledMatrixStateChara = "e95d7b77251d470aa062fa1922dfa9a8" + ledTextChara = "e95d93ee251d470aa062fa1922dfa9a8" + ledScrollingDelayChara = "e95d0d2d251d470aa062fa1922dfa9a8" ) // LEDDriver is the Gobot driver for the Microbit's LED array type LEDDriver struct { - name string - connection gobot.Connection + *ble.Driver gobot.Eventer } -const ( - // BLE services - // ledService = "e95dd91d251d470aa062fa1922dfa9a8" - - // BLE characteristics - ledMatrixStateCharacteristic = "e95d7b77251d470aa062fa1922dfa9a8" - ledTextCharacteristic = "e95d93ee251d470aa062fa1922dfa9a8" - ledScrollingDelayCharacteristic = "e95d0d2d251d470aa062fa1922dfa9a8" -) - // NewLEDDriver creates a Microbit LEDDriver -func NewLEDDriver(a ble.BLEConnector) *LEDDriver { - n := &LEDDriver{ - name: gobot.DefaultName("Microbit LED"), - connection: a, - Eventer: gobot.NewEventer(), +func NewLEDDriver(a gobot.BLEConnector) *LEDDriver { + d := &LEDDriver{ + Eventer: gobot.NewEventer(), } - return n -} - -// Connection returns the BLE connection -func (b *LEDDriver) Connection() gobot.Connection { return b.connection } - -// Name returns the Driver Name -func (b *LEDDriver) Name() string { return b.name } + d.Driver = ble.NewDriver(a, "Microbit LED", nil, nil) -// SetName sets the Driver Name -func (b *LEDDriver) SetName(n string) { b.name = n } - -// adaptor returns BLE adaptor -func (b *LEDDriver) adaptor() ble.BLEConnector { - //nolint:forcetypeassert // ok here - return b.Connection().(ble.BLEConnector) + return d } -// Start tells driver to get ready to do work -func (b *LEDDriver) Start() error { return nil } - -// Halt stops LED driver (void) -func (b *LEDDriver) Halt() error { return nil } - // ReadMatrix read the current LED matrix state func (b *LEDDriver) ReadMatrix() ([]byte, error) { - return b.adaptor().ReadCharacteristic(ledMatrixStateCharacteristic) + return b.Adaptor().ReadCharacteristic(ledMatrixStateChara) } // WriteMatrix writes an array of 5 bytes to set the LED matrix func (b *LEDDriver) WriteMatrix(data []byte) error { - return b.adaptor().WriteCharacteristic(ledMatrixStateCharacteristic, data) + return b.Adaptor().WriteCharacteristic(ledMatrixStateChara, data) } // WriteText writes a text message to the Microbit LED matrix func (b *LEDDriver) WriteText(msg string) error { - return b.adaptor().WriteCharacteristic(ledTextCharacteristic, []byte(msg)) + return b.Adaptor().WriteCharacteristic(ledTextChara, []byte(msg)) } func (b *LEDDriver) ReadScrollingDelay() (uint16, error) { @@ -75,7 +50,7 @@ func (b *LEDDriver) ReadScrollingDelay() (uint16, error) { func (b *LEDDriver) WriteScrollingDelay(delay uint16) error { buf := []byte{byte(delay)} - return b.adaptor().WriteCharacteristic(ledScrollingDelayCharacteristic, buf) + return b.Adaptor().WriteCharacteristic(ledScrollingDelayChara, buf) } // Blank clears the LEDs on the Microbit diff --git a/platforms/microbit/led_driver_test.go b/drivers/ble/microbit/led_driver_test.go similarity index 62% rename from platforms/microbit/led_driver_test.go rename to drivers/ble/microbit/led_driver_test.go index a59039dfb..18e09e7ef 100644 --- a/platforms/microbit/led_driver_test.go +++ b/drivers/ble/microbit/led_driver_test.go @@ -8,43 +8,35 @@ import ( "github.com/stretchr/testify/require" "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/ble/testutil" ) var _ gobot.Driver = (*LEDDriver)(nil) func initTestLEDDriver() *LEDDriver { - d := NewLEDDriver(NewBleTestAdaptor()) + d := NewLEDDriver(testutil.NewBleTestAdaptor()) return d } -func TestLEDDriver(t *testing.T) { - d := initTestLEDDriver() +func TestNewLEDDriver(t *testing.T) { + d := NewLEDDriver(testutil.NewBleTestAdaptor()) + assert.IsType(t, &LEDDriver{}, d) assert.True(t, strings.HasPrefix(d.Name(), "Microbit LED")) - d.SetName("NewName") - assert.Equal(t, "NewName", d.Name()) -} - -func TestLEDDriverStartAndHalt(t *testing.T) { - d := initTestLEDDriver() - require.NoError(t, d.Start()) - require.NoError(t, d.Halt()) + assert.NotNil(t, d.Eventer) } -func TestLEDDriverWriteMatrix(t *testing.T) { +func TestLEDWriteMatrix(t *testing.T) { d := initTestLEDDriver() - _ = d.Start() require.NoError(t, d.WriteMatrix([]byte{0x01, 0x02})) } -func TestLEDDriverWriteText(t *testing.T) { +func TestLEDWriteText(t *testing.T) { d := initTestLEDDriver() - _ = d.Start() require.NoError(t, d.WriteText("Hello")) } -func TestLEDDriverCommands(t *testing.T) { +func TestLEDCommands(t *testing.T) { d := initTestLEDDriver() - _ = d.Start() require.NoError(t, d.Blank()) require.NoError(t, d.Solid()) require.NoError(t, d.UpRightArrow()) diff --git a/drivers/ble/microbit/magnetometer_driver.go b/drivers/ble/microbit/magnetometer_driver.go new file mode 100644 index 000000000..2fb8972e0 --- /dev/null +++ b/drivers/ble/microbit/magnetometer_driver.go @@ -0,0 +1,67 @@ +package microbit + +import ( + "bytes" + "encoding/binary" + + "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/ble" +) + +const ( + // magnetometerService = "e95df2d8251d470aa062fa1922dfa9a8" + magnetometerChara = "e95dfb11251d470aa062fa1922dfa9a8" + + MagnetometerEvent = "magnetometer" +) + +// MagnetometerDriver is the Gobot driver for the Microbit's built-in magnetometer +type MagnetometerDriver struct { + *ble.Driver + gobot.Eventer +} + +type MagnetometerData struct { + X float32 + Y float32 + Z float32 +} + +// NewMagnetometerDriver creates a Microbit MagnetometerDriver +func NewMagnetometerDriver(a gobot.BLEConnector) *MagnetometerDriver { + d := &MagnetometerDriver{ + Eventer: gobot.NewEventer(), + } + d.Driver = ble.NewDriver(a, "Microbit Magnetometer", d.initialize, nil) + + d.AddEvent(MagnetometerEvent) + + return d +} + +// initialize tells driver to get ready to do work +func (d *MagnetometerDriver) initialize() error { + // subscribe to magnetometer notifications + return d.Adaptor().Subscribe(magnetometerChara, func(data []byte, e error) { + a := struct{ x, y, z int16 }{x: 0, y: 0, z: 0} + + buf := bytes.NewBuffer(data) + if err := binary.Read(buf, binary.LittleEndian, &a.x); err != nil { + panic(err) + } + if err := binary.Read(buf, binary.LittleEndian, &a.y); err != nil { + panic(err) + } + if err := binary.Read(buf, binary.LittleEndian, &a.z); err != nil { + panic(err) + } + + result := &MagnetometerData{ + X: float32(a.x) / 1000.0, + Y: float32(a.y) / 1000.0, + Z: float32(a.z) / 1000.0, + } + + d.Publish(d.Event(MagnetometerEvent), result) + }) +} diff --git a/platforms/microbit/magnetometer_driver_test.go b/drivers/ble/microbit/magnetometer_driver_test.go similarity index 63% rename from platforms/microbit/magnetometer_driver_test.go rename to drivers/ble/microbit/magnetometer_driver_test.go index 5a7d31f1f..9cd098243 100644 --- a/platforms/microbit/magnetometer_driver_test.go +++ b/drivers/ble/microbit/magnetometer_driver_test.go @@ -10,41 +10,43 @@ import ( "github.com/stretchr/testify/require" "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/ble/testutil" ) var _ gobot.Driver = (*MagnetometerDriver)(nil) func initTestMagnetometerDriver() *MagnetometerDriver { - d := NewMagnetometerDriver(NewBleTestAdaptor()) + d := NewMagnetometerDriver(testutil.NewBleTestAdaptor()) return d } func TestMagnetometerDriver(t *testing.T) { - d := initTestMagnetometerDriver() + d := NewMagnetometerDriver(testutil.NewBleTestAdaptor()) + assert.IsType(t, &MagnetometerDriver{}, d) assert.True(t, strings.HasPrefix(d.Name(), "Microbit Magnetometer")) - d.SetName("NewName") - assert.Equal(t, "NewName", d.Name()) + assert.NotNil(t, d.Eventer) } -func TestMagnetometerDriverStartAndHalt(t *testing.T) { +func TestMagnetometerStartAndHalt(t *testing.T) { d := initTestMagnetometerDriver() require.NoError(t, d.Start()) require.NoError(t, d.Halt()) } -func TestMagnetometerDriverReadData(t *testing.T) { +func TestMagnetometerReadData(t *testing.T) { sem := make(chan bool) - a := NewBleTestAdaptor() + a := testutil.NewBleTestAdaptor() d := NewMagnetometerDriver(a) - _ = d.Start() - _ = d.On(Magnetometer, func(data interface{}) { + require.NoError(t, d.Start()) + err := d.On("magnetometer", func(data interface{}) { assert.InDelta(t, float32(8.738), data.(*MagnetometerData).X, 0.0) assert.InDelta(t, float32(8.995), data.(*MagnetometerData).Y, 0.0) assert.InDelta(t, float32(9.252), data.(*MagnetometerData).Z, 0.0) sem <- true }) + require.NoError(t, err) - a.TestReceiveNotification([]byte{0x22, 0x22, 0x23, 0x23, 0x24, 0x24}, nil) + a.SendTestDataToSubscriber([]byte{0x22, 0x22, 0x23, 0x23, 0x24, 0x24}, nil) select { case <-sem: diff --git a/drivers/ble/microbit/temperature_driver.go b/drivers/ble/microbit/temperature_driver.go new file mode 100644 index 000000000..6eb6ac373 --- /dev/null +++ b/drivers/ble/microbit/temperature_driver.go @@ -0,0 +1,46 @@ +package microbit + +import ( + "bytes" + + "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/ble" +) + +const ( + // temperatureService = "e95d6100251d470aa062fa1922dfa9a8" + temperatureChara = "e95d9250251d470aa062fa1922dfa9a8" + + TemperatureEvent = "temperature" +) + +// TemperatureDriver is the Gobot driver for the Microbit's built-in thermometer +type TemperatureDriver struct { + *ble.Driver + gobot.Eventer +} + +// NewTemperatureDriver creates a Microbit TemperatureDriver +func NewTemperatureDriver(a gobot.BLEConnector) *TemperatureDriver { + d := &TemperatureDriver{ + Eventer: gobot.NewEventer(), + } + d.Driver = ble.NewDriver(a, "Microbit Temperature", d.initialize, nil) + + d.AddEvent(TemperatureEvent) + + return d +} + +// initialize tells driver to get ready to do work +func (d *TemperatureDriver) initialize() error { + // subscribe to temperature notifications + return d.Adaptor().Subscribe(temperatureChara, func(data []byte, e error) { + var l int8 + buf := bytes.NewBuffer(data) + val, _ := buf.ReadByte() + l = int8(val) + + d.Publish(d.Event(TemperatureEvent), l) + }) +} diff --git a/platforms/microbit/temperature_driver_test.go b/drivers/ble/microbit/temperature_driver_test.go similarity index 62% rename from platforms/microbit/temperature_driver_test.go rename to drivers/ble/microbit/temperature_driver_test.go index 3d4a81db9..070537ddd 100644 --- a/platforms/microbit/temperature_driver_test.go +++ b/drivers/ble/microbit/temperature_driver_test.go @@ -9,39 +9,41 @@ import ( "github.com/stretchr/testify/require" "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/ble/testutil" ) var _ gobot.Driver = (*TemperatureDriver)(nil) func initTestTemperatureDriver() *TemperatureDriver { - d := NewTemperatureDriver(NewBleTestAdaptor()) + d := NewTemperatureDriver(testutil.NewBleTestAdaptor()) return d } func TestTemperatureDriver(t *testing.T) { d := initTestTemperatureDriver() + assert.IsType(t, &TemperatureDriver{}, d) assert.True(t, strings.HasPrefix(d.Name(), "Microbit Temperature")) - d.SetName("NewName") - assert.Equal(t, "NewName", d.Name()) + assert.NotNil(t, d.Eventer) } -func TestTemperatureDriverStartAndHalt(t *testing.T) { +func TestTemperatureStartAndHalt(t *testing.T) { d := initTestTemperatureDriver() require.NoError(t, d.Start()) require.NoError(t, d.Halt()) } -func TestTemperatureDriverReadData(t *testing.T) { +func TestTemperatureReadData(t *testing.T) { sem := make(chan bool) - a := NewBleTestAdaptor() + a := testutil.NewBleTestAdaptor() d := NewTemperatureDriver(a) - _ = d.Start() - _ = d.On(Temperature, func(data interface{}) { + require.NoError(t, d.Start()) + err := d.On("temperature", func(data interface{}) { assert.Equal(t, int8(0x22), data) sem <- true }) + require.NoError(t, err) - a.TestReceiveNotification([]byte{0x22}, nil) + a.SendTestDataToSubscriber([]byte{0x22}, nil) select { case <-sem: diff --git a/drivers/ble/parrot/LICENSE b/drivers/ble/parrot/LICENSE new file mode 100644 index 000000000..257d22e97 --- /dev/null +++ b/drivers/ble/parrot/LICENSE @@ -0,0 +1,13 @@ +Copyright (c) 2014-2018 The Hybrid Group + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. diff --git a/drivers/ble/parrot/doc.go b/drivers/ble/parrot/doc.go new file mode 100644 index 000000000..749ce5033 --- /dev/null +++ b/drivers/ble/parrot/doc.go @@ -0,0 +1,7 @@ +/* +Package parrot contains the Gobot driver for the Parrot Minidrone platform. + +For more information refer to the minidrone README: +https://github.com/hybridgroup/gobot/blob/release/platforms/parrot/minidrone/README.md +*/ +package parrot // import "gobot.io/x/gobot/v2/drivers/ble/parrot" diff --git a/drivers/ble/parrot/minidrone_driver.go b/drivers/ble/parrot/minidrone_driver.go new file mode 100644 index 000000000..f1fee75cb --- /dev/null +++ b/drivers/ble/parrot/minidrone_driver.go @@ -0,0 +1,503 @@ +package parrot + +import ( + "bytes" + "encoding/binary" + "fmt" + "math" + "sync" + "time" + + "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/ble" +) + +const ( + // droneCommandService = "9a66fa000800919111e4012d1540cb8e" + // droneNotificationService = "9a66fb000800919111e4012d1540cb8e" + + // send characteristics + pcmdChara = "9a66fa0a0800919111e4012d1540cb8e" + commandChara = "9a66fa0b0800919111e4012d1540cb8e" + priorityChara = "9a66fa0c0800919111e4012d1540cb8e" + + // receive characteristics + flightStatusChara = "9a66fb0e0800919111e4012d1540cb8e" + batteryChara = "9a66fb0f0800919111e4012d1540cb8e" + + // piloting states + flatTrimChanged = 0 + flyingStateChanged = 1 + + // flying states + flyingStateLanded = 0 + flyingStateTakeoff = 1 + flyingStateHovering = 2 + flyingStateFlying = 3 + flyingStateLanding = 4 + flyingStateEmergency = 5 + flyingStateRolling = 6 + + BatteryEvent = "battery" + FlightStatusEvent = "flightstatus" + TakeoffEvent = "takeoff" + HoveringEvent = "hovering" + FlyingEvent = "flying" + LandingEvent = "landing" + LandedEvent = "landed" + EmergencyEvent = "emergency" + RollingEvent = "rolling" + FlatTrimChangeEvent = "flattrimchange" + + // modes for LightControl + LightFixed = 0 + LightBlinked = 1 + LightOscillated = 3 + + // modes for ClawControl + ClawOpen = 0 + ClawClosed = 1 +) + +// MinidroneDriver is the Gobot interface to the Parrot Minidrone +type MinidroneDriver struct { + *ble.Driver + stepsfa0a uint16 + stepsfa0b uint16 + pcmdMutex sync.Mutex + flying bool + Pcmd Pcmd + gobot.Eventer +} + +// Pcmd is the Parrot Command structure for flight control +type Pcmd struct { + Flag int + Roll int + Pitch int + Yaw int + Gaz int + Psi float32 +} + +// NewDriver creates a Parrot Minidrone Driver +func NewMinidroneDriver(a gobot.BLEConnector) *MinidroneDriver { + d := &MinidroneDriver{ + Pcmd: Pcmd{ + Flag: 0, + Roll: 0, + Pitch: 0, + Yaw: 0, + Gaz: 0, + Psi: 0, + }, + Eventer: gobot.NewEventer(), + } + d.Driver = ble.NewDriver(a, "Minidrone", d.initialize, d.shutdown) + + d.AddEvent(BatteryEvent) + d.AddEvent(FlightStatusEvent) + d.AddEvent(TakeoffEvent) + d.AddEvent(FlyingEvent) + d.AddEvent(HoveringEvent) + d.AddEvent(LandingEvent) + d.AddEvent(LandedEvent) + d.AddEvent(EmergencyEvent) + d.AddEvent(RollingEvent) + + return d +} + +// GenerateAllStates sets up all the default states aka settings on the drone +func (d *MinidroneDriver) GenerateAllStates() error { + d.stepsfa0b++ + buf := []byte{ + 0x04, byte(d.stepsfa0b), 0x00, 0x04, 0x01, 0x00, 0x32, 0x30, 0x31, 0x34, 0x2D, 0x31, 0x30, 0x2D, 0x32, 0x38, 0x00, + } + return d.Adaptor().WriteCharacteristic(commandChara, buf) +} + +// TakeOff tells the Minidrone to takeoff +func (d *MinidroneDriver) TakeOff() error { + d.stepsfa0b++ + buf := []byte{0x02, byte(d.stepsfa0b) & 0xff, 0x02, 0x00, 0x01, 0x00} + return d.Adaptor().WriteCharacteristic(commandChara, buf) +} + +// Land tells the Minidrone to land +func (d *MinidroneDriver) Land() error { + d.stepsfa0b++ + buf := []byte{0x02, byte(d.stepsfa0b) & 0xff, 0x02, 0x00, 0x03, 0x00} + return d.Adaptor().WriteCharacteristic(commandChara, buf) +} + +// FlatTrim calibrates the Minidrone to use its current position as being level +func (d *MinidroneDriver) FlatTrim() error { + d.stepsfa0b++ + buf := []byte{0x02, byte(d.stepsfa0b) & 0xff, 0x02, 0x00, 0x00, 0x00} + return d.Adaptor().WriteCharacteristic(commandChara, buf) +} + +// Emergency sets the Minidrone into emergency mode +func (d *MinidroneDriver) Emergency() error { + d.stepsfa0b++ + buf := []byte{0x02, byte(d.stepsfa0b) & 0xff, 0x02, 0x00, 0x04, 0x00} + return d.Adaptor().WriteCharacteristic(priorityChara, buf) +} + +// TakePicture tells the Minidrone to take a picture +func (d *MinidroneDriver) TakePicture() error { + d.stepsfa0b++ + buf := []byte{0x02, byte(d.stepsfa0b) & 0xff, 0x02, 0x06, 0x01, 0x00} + return d.Adaptor().WriteCharacteristic(commandChara, buf) +} + +// StartPcmd starts the continuous Pcmd communication with the Minidrone +func (d *MinidroneDriver) StartPcmd() { + go func() { + // wait a little bit so that there is enough time to get some ACKs + time.Sleep(500 * time.Millisecond) + for { + err := d.Adaptor().WriteCharacteristic(pcmdChara, d.generatePcmd().Bytes()) + if err != nil { + fmt.Println("pcmd write error:", err) + } + time.Sleep(50 * time.Millisecond) + } + }() +} + +// Up tells the drone to ascend. Pass in an int from 0-100. +func (d *MinidroneDriver) Up(val int) error { + d.pcmdMutex.Lock() + defer d.pcmdMutex.Unlock() + + d.Pcmd.Flag = 1 + d.Pcmd.Gaz = validatePitch(val) + return nil +} + +// Down tells the drone to descend. Pass in an int from 0-100. +func (d *MinidroneDriver) Down(val int) error { + d.pcmdMutex.Lock() + defer d.pcmdMutex.Unlock() + + d.Pcmd.Flag = 1 + d.Pcmd.Gaz = validatePitch(val) * -1 + return nil +} + +// Forward tells the drone to go forward. Pass in an int from 0-100. +func (d *MinidroneDriver) Forward(val int) error { + d.pcmdMutex.Lock() + defer d.pcmdMutex.Unlock() + + d.Pcmd.Flag = 1 + d.Pcmd.Pitch = validatePitch(val) + return nil +} + +// Backward tells drone to go in reverse. Pass in an int from 0-100. +func (d *MinidroneDriver) Backward(val int) error { + d.pcmdMutex.Lock() + defer d.pcmdMutex.Unlock() + + d.Pcmd.Flag = 1 + d.Pcmd.Pitch = validatePitch(val) * -1 + return nil +} + +// Right tells drone to go right. Pass in an int from 0-100. +func (d *MinidroneDriver) Right(val int) error { + d.pcmdMutex.Lock() + defer d.pcmdMutex.Unlock() + + d.Pcmd.Flag = 1 + d.Pcmd.Roll = validatePitch(val) + return nil +} + +// Left tells drone to go left. Pass in an int from 0-100. +func (d *MinidroneDriver) Left(val int) error { + d.pcmdMutex.Lock() + defer d.pcmdMutex.Unlock() + + d.Pcmd.Flag = 1 + d.Pcmd.Roll = validatePitch(val) * -1 + return nil +} + +// Clockwise tells drone to rotate in a clockwise directiod. Pass in an int from 0-100. +func (d *MinidroneDriver) Clockwise(val int) error { + d.pcmdMutex.Lock() + defer d.pcmdMutex.Unlock() + + d.Pcmd.Flag = 1 + d.Pcmd.Yaw = validatePitch(val) + return nil +} + +// CounterClockwise tells drone to rotate in a counter-clockwise directiod. +// Pass in an int from 0-100. +func (d *MinidroneDriver) CounterClockwise(val int) error { + d.pcmdMutex.Lock() + defer d.pcmdMutex.Unlock() + + d.Pcmd.Flag = 1 + d.Pcmd.Yaw = validatePitch(val) * -1 + return nil +} + +// Stop tells the drone to stop moving in any direction and simply hover in place +func (d *MinidroneDriver) Stop() error { + d.pcmdMutex.Lock() + defer d.pcmdMutex.Unlock() + + d.Pcmd = Pcmd{ + Flag: 0, + Roll: 0, + Pitch: 0, + Yaw: 0, + Gaz: 0, + Psi: 0, + } + + return nil +} + +// StartRecording is not supported by the Parrot Minidrone +func (d *MinidroneDriver) StartRecording() error { + return nil +} + +// StopRecording is not supported by the Parrot Minidrone +func (d *MinidroneDriver) StopRecording() error { + return nil +} + +// HullProtection is not supported by the Parrot Minidrone +func (d *MinidroneDriver) HullProtection(protect bool) error { + return nil +} + +// Outdoor mode is not supported by the Parrot Minidrone +func (d *MinidroneDriver) Outdoor(outdoor bool) error { + return nil +} + +// FrontFlip tells the drone to perform a front flip +func (d *MinidroneDriver) FrontFlip() error { + return d.Adaptor().WriteCharacteristic(commandChara, d.generateAnimation(0).Bytes()) +} + +// BackFlip tells the drone to perform a backflip +func (d *MinidroneDriver) BackFlip() error { + return d.Adaptor().WriteCharacteristic(commandChara, d.generateAnimation(1).Bytes()) +} + +// RightFlip tells the drone to perform a flip to the right +func (d *MinidroneDriver) RightFlip() error { + return d.Adaptor().WriteCharacteristic(commandChara, d.generateAnimation(2).Bytes()) +} + +// LeftFlip tells the drone to perform a flip to the left +func (d *MinidroneDriver) LeftFlip() error { + return d.Adaptor().WriteCharacteristic(commandChara, d.generateAnimation(3).Bytes()) +} + +// LightControl controls lights on those Minidrone models which +// have the correct hardware, such as the Maclane, Blaze, & Swat. +// Params: +// +// id - always 0 +// mode - either LightFixed, LightBlinked, or LightOscillated +// intensity - Light intensity from 0 (OFF) to 100 (Max intensity). +// Only used in LightFixed mode. +func (d *MinidroneDriver) LightControl(id uint8, mode uint8, intensity uint8) error { + d.stepsfa0b++ + buf := []byte{0x02, byte(d.stepsfa0b) & 0xff, 0x02, 0x10, 0x00, id, mode, intensity, 0x00} + return d.Adaptor().WriteCharacteristic(commandChara, buf) +} + +// ClawControl controls the claw on the Parrot Mambo +// Params: +// +// id - always 0 +// mode - either ClawOpen or ClawClosed +func (d *MinidroneDriver) ClawControl(id uint8, mode uint8) error { + d.stepsfa0b++ + buf := []byte{0x02, byte(d.stepsfa0b) & 0xff, 0x02, 0x10, 0x01, id, mode, 0x00} + return d.Adaptor().WriteCharacteristic(commandChara, buf) +} + +// GunControl fires the gun on the Parrot Mambo +// Params: +// +// id - always 0 +func (d *MinidroneDriver) GunControl(id uint8) error { + d.stepsfa0b++ + buf := []byte{0x02, byte(d.stepsfa0b) & 0xff, 0x02, 0x10, 0x02, id, 0x00} + return d.Adaptor().WriteCharacteristic(commandChara, buf) +} + +// initialize tells driver to get ready to do work +func (d *MinidroneDriver) initialize() error { + d.Adaptor().WithoutResponses(true) + + if err := d.GenerateAllStates(); err != nil { + return err + } + + // subscribe to battery notifications + if err := d.Adaptor().Subscribe(batteryChara, func(data []byte, e error) { + d.Publish(d.Event(BatteryEvent), data[len(data)-1]) + }); err != nil { + return err + } + + // subscribe to flying status notifications + if err := d.Adaptor().Subscribe(flightStatusChara, func(data []byte, e error) { + d.processFlightStatus(data) + }); err != nil { + return err + } + + if err := d.FlatTrim(); err != nil { + return err + } + + d.StartPcmd() + + return d.FlatTrim() +} + +// shutdown stops minidrone driver (void) +func (d *MinidroneDriver) shutdown() error { + err := d.Land() + time.Sleep(500 * time.Millisecond) + return err +} + +func (d *MinidroneDriver) generateAnimation(direction int8) *bytes.Buffer { + d.stepsfa0b++ + buf := []byte{0x02, byte(d.stepsfa0b) & 0xff, 0x02, 0x04, 0x00, 0x00, byte(direction), 0x00, 0x00, 0x00} + return bytes.NewBuffer(buf) +} + +func (d *MinidroneDriver) generatePcmd() *bytes.Buffer { + d.pcmdMutex.Lock() + defer d.pcmdMutex.Unlock() + d.stepsfa0a++ + pcmd := d.Pcmd + + cmd := &bytes.Buffer{} + if err := binary.Write(cmd, binary.LittleEndian, int8(2)); err != nil { + panic(err) + } + if err := binary.Write(cmd, binary.LittleEndian, int8(d.stepsfa0a)); err != nil { + panic(err) + } + if err := binary.Write(cmd, binary.LittleEndian, int8(2)); err != nil { + panic(err) + } + if err := binary.Write(cmd, binary.LittleEndian, int8(0)); err != nil { + panic(err) + } + if err := binary.Write(cmd, binary.LittleEndian, int8(2)); err != nil { + panic(err) + } + if err := binary.Write(cmd, binary.LittleEndian, int8(0)); err != nil { + panic(err) + } + if err := binary.Write(cmd, binary.LittleEndian, int8(pcmd.Flag)); err != nil { + panic(err) + } + if err := binary.Write(cmd, binary.LittleEndian, int8(pcmd.Roll)); err != nil { + panic(err) + } + if err := binary.Write(cmd, binary.LittleEndian, int8(pcmd.Pitch)); err != nil { + panic(err) + } + if err := binary.Write(cmd, binary.LittleEndian, int8(pcmd.Yaw)); err != nil { + panic(err) + } + if err := binary.Write(cmd, binary.LittleEndian, int8(pcmd.Gaz)); err != nil { + panic(err) + } + if err := binary.Write(cmd, binary.LittleEndian, pcmd.Psi); err != nil { + panic(err) + } + if err := binary.Write(cmd, binary.LittleEndian, int16(0)); err != nil { + panic(err) + } + if err := binary.Write(cmd, binary.LittleEndian, int16(0)); err != nil { + panic(err) + } + + return cmd +} + +func (d *MinidroneDriver) processFlightStatus(data []byte) { + if len(data) < 5 { + // ignore, just a sync + return + } + + d.Publish(FlightStatusEvent, data[4]) + + switch data[4] { + case flatTrimChanged: + d.Publish(FlatTrimChangeEvent, true) + + case flyingStateChanged: + switch data[6] { + case flyingStateLanded: + if d.flying { + d.flying = false + d.Publish(LandedEvent, true) + } + case flyingStateTakeoff: + d.Publish(TakeoffEvent, true) + case flyingStateHovering: + if !d.flying { + d.flying = true + d.Publish(HoveringEvent, true) + } + case flyingStateFlying: + if !d.flying { + d.flying = true + d.Publish(FlyingEvent, true) + } + case flyingStateLanding: + d.Publish(LandingEvent, true) + case flyingStateEmergency: + d.Publish(EmergencyEvent, true) + case flyingStateRolling: + d.Publish(RollingEvent, true) + } + } +} + +// ValidatePitch helps validate pitch values such as those created by +// a joystick to values between 0-100 that are required as +// params to Parrot Minidrone PCMDs +func ValidatePitch(data float64, offset float64) int { + value := math.Abs(data) / offset + if value >= 0.1 { + if value <= 1.0 { + return int((float64(int(value*100)) / 100) * 100) + } + return 100 + } + return 0 +} + +func validatePitch(val int) int { + if val > 100 { + return 100 + } else if val < 0 { + return 0 + } + + return val +} diff --git a/platforms/parrot/minidrone/minidrone_driver_test.go b/drivers/ble/parrot/minidrone_driver_test.go similarity index 78% rename from platforms/parrot/minidrone/minidrone_driver_test.go rename to drivers/ble/parrot/minidrone_driver_test.go index cd6bb4e06..bbae0c67e 100644 --- a/platforms/parrot/minidrone/minidrone_driver_test.go +++ b/drivers/ble/parrot/minidrone_driver_test.go @@ -1,4 +1,4 @@ -package minidrone +package parrot import ( "strings" @@ -8,124 +8,110 @@ import ( "github.com/stretchr/testify/require" "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/ble/testutil" ) -var _ gobot.Driver = (*Driver)(nil) +var _ gobot.Driver = (*MinidroneDriver)(nil) -func initTestMinidroneDriver() *Driver { - d := NewDriver(NewBleTestAdaptor()) +func initTestMinidroneDriver() *MinidroneDriver { + d := NewMinidroneDriver(testutil.NewBleTestAdaptor()) + if err := d.Start(); err != nil { + panic(err) + } return d } -func TestMinidroneDriver(t *testing.T) { - d := initTestMinidroneDriver() +func TestNewMinidroneDriver(t *testing.T) { + d := NewMinidroneDriver(testutil.NewBleTestAdaptor()) assert.True(t, strings.HasPrefix(d.Name(), "Minidrone")) - d.SetName("NewName") - assert.Equal(t, "NewName", d.Name()) + assert.NotNil(t, d.Eventer) } -func TestMinidroneDriverStartAndHalt(t *testing.T) { +func TestMinidroneHalt(t *testing.T) { d := initTestMinidroneDriver() - require.NoError(t, d.Start()) require.NoError(t, d.Halt()) } func TestMinidroneTakeoff(t *testing.T) { d := initTestMinidroneDriver() - require.NoError(t, d.Start()) require.NoError(t, d.TakeOff()) } func TestMinidroneEmergency(t *testing.T) { d := initTestMinidroneDriver() - require.NoError(t, d.Start()) require.NoError(t, d.Emergency()) } func TestMinidroneTakePicture(t *testing.T) { d := initTestMinidroneDriver() - require.NoError(t, d.Start()) require.NoError(t, d.TakePicture()) } func TestMinidroneUp(t *testing.T) { d := initTestMinidroneDriver() - require.NoError(t, d.Start()) require.NoError(t, d.Up(25)) } func TestMinidroneUpTooFar(t *testing.T) { d := initTestMinidroneDriver() - require.NoError(t, d.Start()) require.NoError(t, d.Up(125)) require.NoError(t, d.Up(-50)) } func TestMinidroneDown(t *testing.T) { d := initTestMinidroneDriver() - require.NoError(t, d.Start()) require.NoError(t, d.Down(25)) } func TestMinidroneForward(t *testing.T) { d := initTestMinidroneDriver() - require.NoError(t, d.Start()) require.NoError(t, d.Forward(25)) } func TestMinidroneBackward(t *testing.T) { d := initTestMinidroneDriver() - require.NoError(t, d.Start()) require.NoError(t, d.Backward(25)) } func TestMinidroneRight(t *testing.T) { d := initTestMinidroneDriver() - require.NoError(t, d.Start()) require.NoError(t, d.Right(25)) } func TestMinidroneLeft(t *testing.T) { d := initTestMinidroneDriver() - require.NoError(t, d.Start()) require.NoError(t, d.Left(25)) } func TestMinidroneClockwise(t *testing.T) { d := initTestMinidroneDriver() - require.NoError(t, d.Start()) require.NoError(t, d.Clockwise(25)) } func TestMinidroneCounterClockwise(t *testing.T) { d := initTestMinidroneDriver() - require.NoError(t, d.Start()) require.NoError(t, d.CounterClockwise(25)) } func TestMinidroneStop(t *testing.T) { d := initTestMinidroneDriver() - require.NoError(t, d.Start()) require.NoError(t, d.Stop()) } func TestMinidroneStartStopRecording(t *testing.T) { d := initTestMinidroneDriver() - require.NoError(t, d.Start()) require.NoError(t, d.StartRecording()) require.NoError(t, d.StopRecording()) } func TestMinidroneHullProtectionOutdoor(t *testing.T) { d := initTestMinidroneDriver() - require.NoError(t, d.Start()) require.NoError(t, d.HullProtection(true)) require.NoError(t, d.Outdoor(true)) } func TestMinidroneHullFlips(t *testing.T) { d := initTestMinidroneDriver() - require.NoError(t, d.Start()) require.NoError(t, d.FrontFlip()) require.NoError(t, d.BackFlip()) require.NoError(t, d.RightFlip()) @@ -134,25 +120,21 @@ func TestMinidroneHullFlips(t *testing.T) { func TestMinidroneLightControl(t *testing.T) { d := initTestMinidroneDriver() - require.NoError(t, d.Start()) require.NoError(t, d.LightControl(0, LightBlinked, 25)) } func TestMinidroneClawControl(t *testing.T) { d := initTestMinidroneDriver() - require.NoError(t, d.Start()) require.NoError(t, d.ClawControl(0, ClawOpen)) } func TestMinidroneGunControl(t *testing.T) { d := initTestMinidroneDriver() - require.NoError(t, d.Start()) require.NoError(t, d.GunControl(0)) } func TestMinidroneProcessFlightData(t *testing.T) { d := initTestMinidroneDriver() - require.NoError(t, d.Start()) d.processFlightStatus([]byte{0x00, 0x00, 0x00}) d.processFlightStatus([]byte{0x00, 0x00, 0x00, 0x00, 0x00}) @@ -170,3 +152,15 @@ func TestMinidroneProcessFlightData(t *testing.T) { require.NoError(t, d.Stop()) } + +func TestMinidroneValidatePitchWhenEqualOffset(t *testing.T) { + assert.Equal(t, 100, ValidatePitch(32767.0, 32767.0)) +} + +func TestMinidroneValidatePitchWhenTiny(t *testing.T) { + assert.Equal(t, 0, ValidatePitch(1.1, 32767.0)) +} + +func TestMinidroneValidatePitchWhenCentered(t *testing.T) { + assert.Equal(t, 50, ValidatePitch(16383.5, 32767.0)) +} diff --git a/drivers/ble/serial_port.go b/drivers/ble/serial_port.go new file mode 100644 index 000000000..fbf7addcb --- /dev/null +++ b/drivers/ble/serial_port.go @@ -0,0 +1,84 @@ +package ble + +import ( + "sync" + + "gobot.io/x/gobot/v2" +) + +// SerialPortDriver is a implementation of serial over Bluetooth LE +// Inspired by https://github.com/monteslu/ble-serial by @monteslu +type SerialPortDriver struct { + *Driver + rid string + tid string + // buffer of responseData and mutex to protect it + responseData []byte + responseMutex sync.Mutex +} + +// NewSerialPortDriver returns a new serial over Bluetooth LE connection +func NewSerialPortDriver(a gobot.BLEConnector, rid string, tid string) *SerialPortDriver { + d := &SerialPortDriver{ + Driver: NewDriver(a, "BleSerial", nil, nil), + rid: rid, + tid: tid, + } + + return d +} + +// Open opens a connection to a BLE serial device +func (p *SerialPortDriver) Open() error { + if err := p.Adaptor().Connect(); err != nil { + return err + } + + // subscribe to response notifications + return p.Adaptor().Subscribe(p.rid, func(data []byte, e error) { + p.responseMutex.Lock() + defer p.responseMutex.Unlock() + p.responseData = append(p.responseData, data...) + }) +} + +// Read reads bytes from BLE serial port connection +func (p *SerialPortDriver) Read(b []byte) (int, error) { + p.responseMutex.Lock() + defer p.responseMutex.Unlock() + + if len(p.responseData) == 0 { + return 0, nil + } + + n := len(b) + if len(p.responseData) < n { + n = len(p.responseData) + } + copy(b, p.responseData[:n]) + + if len(p.responseData) > n { + p.responseData = p.responseData[n:] + } else { + p.responseData = nil + } + + return n, nil +} + +// Write writes to the BLE serial port connection +func (p *SerialPortDriver) Write(b []byte) (int, error) { + err := p.Adaptor().WriteCharacteristic(p.tid, b) + n := len(b) + return n, err +} + +// Close closes the BLE serial port connection +func (p *SerialPortDriver) Close() error { + return p.Adaptor().Disconnect() +} + +// Address returns the BLE address +func (p *SerialPortDriver) Address() string { + return p.Adaptor().Address() +} diff --git a/drivers/ble/serial_port_test.go b/drivers/ble/serial_port_test.go new file mode 100644 index 000000000..1d045f3b1 --- /dev/null +++ b/drivers/ble/serial_port_test.go @@ -0,0 +1,20 @@ +package ble + +import ( + "io" + "testing" + + "github.com/stretchr/testify/assert" + + "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/ble/testutil" +) + +var _ gobot.Driver = (*SerialPortDriver)(nil) + +var _ io.ReadWriteCloser = (*SerialPortDriver)(nil) + +func TestBLESerialPort(t *testing.T) { + d := NewSerialPortDriver(testutil.NewBleTestAdaptor(), "123", "456") + assert.Equal(t, "01:02:03:0A:0B:0C", d.Address()) +} diff --git a/platforms/sphero/LICENSE b/drivers/ble/sphero/LICENSE similarity index 100% rename from platforms/sphero/LICENSE rename to drivers/ble/sphero/LICENSE diff --git a/drivers/ble/sphero/doc.go b/drivers/ble/sphero/doc.go new file mode 100644 index 000000000..eac84dd14 --- /dev/null +++ b/drivers/ble/sphero/doc.go @@ -0,0 +1,9 @@ +/* +Package sphero provides the Gobot drivers for the Sphero BLE based platforms. + +For further information refer to sphero readme files: +https://github.com/hybridgroup/gobot/blob/master/platforms/sphero/bb8/README.md +https://github.com/hybridgroup/gobot/blob/master/platforms/sphero/ollie/README.md +https://github.com/hybridgroup/gobot/blob/master/platforms/sphero/sprkplus/README.md +*/ +package sphero // import "gobot.io/x/gobot/v2/drivers/ble/sphero" diff --git a/drivers/ble/sphero/sphero_bb8_driver.go b/drivers/ble/sphero/sphero_bb8_driver.go new file mode 100644 index 000000000..e1a459cbc --- /dev/null +++ b/drivers/ble/sphero/sphero_bb8_driver.go @@ -0,0 +1,28 @@ +package sphero + +import ( + "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/common/sphero" +) + +// BB8Driver represents a Sphero BB-8 +type BB8Driver struct { + *OllieDriver +} + +// NewBB8Driver creates a driver for a Sphero BB-8 +func NewBB8Driver(a gobot.BLEConnector) *BB8Driver { + return &BB8Driver{OllieDriver: newOllieBaseDriver(a, "BB8", bb8DefaultCollisionConfig())} +} + +// bb8DefaultCollisionConfig returns a CollisionConfig with sensible collision defaults +func bb8DefaultCollisionConfig() sphero.CollisionConfig { + return sphero.CollisionConfig{ + Method: 0x01, + Xt: 0x20, + Yt: 0x20, + Xs: 0x20, + Ys: 0x20, + Dead: 0x01, + } +} diff --git a/drivers/ble/sphero/sphero_bb8_driver_test.go b/drivers/ble/sphero/sphero_bb8_driver_test.go new file mode 100644 index 000000000..962239cc0 --- /dev/null +++ b/drivers/ble/sphero/sphero_bb8_driver_test.go @@ -0,0 +1,21 @@ +package sphero + +import ( + "strings" + "testing" + + "github.com/stretchr/testify/assert" + + "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/ble/testutil" +) + +var _ gobot.Driver = (*BB8Driver)(nil) + +func TestNewBB8Driver(t *testing.T) { + d := NewBB8Driver(testutil.NewBleTestAdaptor()) + assert.NotNil(t, d.OllieDriver) + assert.True(t, strings.HasPrefix(d.Name(), "BB8")) + assert.NotNil(t, d.OllieDriver) + assert.Equal(t, d.defaultCollisionConfig, bb8DefaultCollisionConfig()) +} diff --git a/drivers/ble/sphero/sphero_ollie_driver.go b/drivers/ble/sphero/sphero_ollie_driver.go new file mode 100644 index 000000000..fffc4a008 --- /dev/null +++ b/drivers/ble/sphero/sphero_ollie_driver.go @@ -0,0 +1,446 @@ +package sphero + +import ( + "bytes" + "encoding/binary" + "fmt" + "time" + + "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/ble" + "gobot.io/x/gobot/v2/drivers/common/sphero" +) + +// MotorModes is used to configure the motor +type MotorModes uint8 + +// MotorModes required for SetRawMotorValues command +const ( + Off MotorModes = iota + Forward + Reverse + Brake + Ignore +) + +const ( + // spheroBLEService = "22bb746f2bb075542d6f726568705327" + // robotControlService = "22bb746f2ba075542d6f726568705327" + + wakeChara = "22bb746f2bbf75542d6f726568705327" + txPowerChara = "22bb746f2bb275542d6f726568705327" + antiDosChara = "22bb746f2bbd75542d6f726568705327" + commandsChara = "22bb746f2ba175542d6f726568705327" + responseChara = "22bb746f2ba675542d6f726568705327" + + // packet header size + packetHeaderSize = 5 + + // Response packet max size + responsePacketMaxSize = 20 + + // Collision packet data size: The number of bytes following the DLEN field through the end of the packet + collisionDataSize = 17 + + // Full size of the collision response + collisionResponseSize = packetHeaderSize + collisionDataSize +) + +// packet describes head, body and checksum for a data package to be sent +type packet struct { + header []uint8 + body []uint8 + checksum uint8 +} + +// Point2D represents a coordinate in 2-Dimensional space, exposed because used in a callback +type Point2D struct { + X int16 + Y int16 +} + +// OllieDriver is the Gobot driver for the Sphero Ollie robot +type OllieDriver struct { + *ble.Driver + gobot.Eventer + defaultCollisionConfig sphero.CollisionConfig + seq uint8 + collisionResponse []uint8 + packetChannel chan *packet + asyncBuffer []byte + asyncMessage []byte + locatorCallback func(p Point2D) + powerstateCallback func(p sphero.PowerStatePacket) +} + +// NewOllieDriver creates a driver for a Sphero Ollie +func NewOllieDriver(a gobot.BLEConnector) *OllieDriver { + return newOllieBaseDriver(a, "Ollie", ollieDefaultCollisionConfig()) +} + +func newOllieBaseDriver(a gobot.BLEConnector, name string, dcc sphero.CollisionConfig) *OllieDriver { + d := &OllieDriver{ + defaultCollisionConfig: dcc, + Eventer: gobot.NewEventer(), + packetChannel: make(chan *packet, 1024), + } + d.Driver = ble.NewDriver(a, name, d.initialize, d.shutdown) + + d.AddEvent(sphero.ErrorEvent) + d.AddEvent(sphero.CollisionEvent) + + return d +} + +// SetTXPower sets transmit level +func (d *OllieDriver) SetTXPower(level int) error { + buf := []byte{byte(level)} + + if err := d.Adaptor().WriteCharacteristic(txPowerChara, buf); err != nil { + return err + } + + return nil +} + +// Wake wakes Ollie up so we can play +func (d *OllieDriver) Wake() error { + buf := []byte{0x01} + + if err := d.Adaptor().WriteCharacteristic(wakeChara, buf); err != nil { + return err + } + + return nil +} + +// ConfigureCollisionDetection configures the sensitivity of the detection. +func (d *OllieDriver) ConfigureCollisionDetection(cc sphero.CollisionConfig) { + d.sendCraftPacket([]uint8{cc.Method, cc.Xt, cc.Yt, cc.Xs, cc.Ys, cc.Dead}, 0x02, 0x12) +} + +// GetLocatorData calls the passed function with the data from the locator +func (d *OllieDriver) GetLocatorData(f func(p Point2D)) { + // CID 0x15 is the code for the locator request + d.sendCraftPacket([]uint8{}, 0x02, 0x15) + d.locatorCallback = f +} + +// GetPowerState calls the passed function with the Power State information from the sphero +func (d *OllieDriver) GetPowerState(f func(p sphero.PowerStatePacket)) { + // CID 0x20 is the code for the power state + d.sendCraftPacket([]uint8{}, 0x00, 0x20) + d.powerstateCallback = f +} + +// SetRGB sets the Ollie to the given r, g, and b values +func (d *OllieDriver) SetRGB(r uint8, g uint8, b uint8) { + d.sendCraftPacket([]uint8{r, g, b, 0x01}, 0x02, 0x20) +} + +// Roll tells the Ollie to roll +func (d *OllieDriver) Roll(speed uint8, heading uint16) { + d.sendCraftPacket([]uint8{speed, uint8(heading >> 8), uint8(heading & 0xFF), 0x01}, 0x02, 0x30) +} + +// Boost executes the boost macro from within the SSB which takes a 1 byte parameter which is +// either 01h to begin boosting or 00h to stop. +func (d *OllieDriver) Boost(state bool) { + s := uint8(0x01) + if !state { + s = 0x00 + } + d.sendCraftPacket([]uint8{s}, 0x02, 0x31) +} + +// SetStabilization enables or disables the built-in auto stabilizing features of the Ollie +func (d *OllieDriver) SetStabilization(state bool) { + s := uint8(0x01) + if !state { + s = 0x00 + } + d.sendCraftPacket([]uint8{s}, 0x02, 0x02) +} + +// SetRotationRate allows you to control the rotation rate that Sphero will use to meet new heading commands. A value +// of 255 jumps to the maximum (currently 400 degrees/sec). A value of zero doesn't make much sense so it's interpreted +// as 1, the minimum. +func (d *OllieDriver) SetRotationRate(speed uint8) { + d.sendCraftPacket([]uint8{speed}, 0x02, 0x03) +} + +// SetRawMotorValues allows you to take over one or both of the motor output values, instead of having the stabilization +// system control them. Each motor (left and right) requires a mode and a power value from 0-255. +func (d *OllieDriver) SetRawMotorValues(lmode MotorModes, lpower uint8, rmode MotorModes, rpower uint8) { + d.sendCraftPacket([]uint8{uint8(lmode), lpower, uint8(rmode), rpower}, 0x02, 0x33) +} + +// SetBackLEDBrightness allows you to control the brightness of the back(tail) LED. +func (d *OllieDriver) SetBackLEDBrightness(value uint8) { + d.sendCraftPacket([]uint8{value}, 0x02, 0x21) +} + +// Stop tells the Ollie to stop +func (d *OllieDriver) Stop() { + d.Roll(0, 0) +} + +// Sleep says Go to sleep +func (d *OllieDriver) Sleep() { + d.sendCraftPacket([]uint8{0x00, 0x00, 0x00, 0x00, 0x00}, 0x00, 0x22) +} + +// SetDataStreamingConfig passes the config to the sphero to stream sensor data +func (d *OllieDriver) SetDataStreamingConfig(dsc sphero.DataStreamingConfig) error { + buf := new(bytes.Buffer) + if err := binary.Write(buf, binary.BigEndian, dsc); err != nil { + return err + } + d.sendCraftPacket(buf.Bytes(), 0x02, 0x11) + return nil +} + +// initialize tells driver to get ready to do work +func (d *OllieDriver) initialize() error { + if err := d.antiDOSOff(); err != nil { + return err + } + if err := d.SetTXPower(7); err != nil { + return err + } + if err := d.Wake(); err != nil { + return err + } + + // subscribe to Sphero response notifications + if err := d.Adaptor().Subscribe(responseChara, d.handleResponses); err != nil { + return err + } + + go func() { + for { + packet := <-d.packetChannel + err := d.writeCommand(packet) + if err != nil { + d.Publish(d.Event(sphero.ErrorEvent), err) + } + } + }() + + go func() { + for { + if _, err := d.Adaptor().ReadCharacteristic(responseChara); err != nil { + panic(err) + } + time.Sleep(100 * time.Millisecond) + } + }() + + d.ConfigureCollisionDetection(d.defaultCollisionConfig) + d.enableStopOnDisconnect() + + return nil +} + +// antiDOSOff turns off Anti-DOS code so we can control Ollie +func (d *OllieDriver) antiDOSOff() error { + str := "011i3" + buf := &bytes.Buffer{} + buf.WriteString(str) + + if err := d.Adaptor().WriteCharacteristic(antiDosChara, buf.Bytes()); err != nil { + return err + } + + return nil +} + +func (d *OllieDriver) writeCommand(packet *packet) error { + d.Mutex().Lock() + defer d.Mutex().Unlock() + + buf := append(packet.header, packet.body...) + buf = append(buf, packet.checksum) + if err := d.Adaptor().WriteCharacteristic(commandsChara, buf); err != nil { + fmt.Println("async send command error:", err) + return err + } + + d.seq++ + return nil +} + +// enableStopOnDisconnect auto-sends a Stop command after losing the connection +func (d *OllieDriver) enableStopOnDisconnect() { + d.sendCraftPacket([]uint8{0x00, 0x00, 0x00, 0x01}, 0x02, 0x37) +} + +// shutdown stops Ollie driver (void) +func (d *OllieDriver) shutdown() error { + d.Sleep() + time.Sleep(750 * time.Microsecond) + return nil +} + +// handleResponses handles responses returned from Ollie +func (d *OllieDriver) handleResponses(data []byte, e error) { + // since packets can only be 20 bytes long, we have to puzzle them together + newMessage := false + + // append message parts to existing + if len(data) > 0 && data[0] != 0xFF { + d.asyncBuffer = append(d.asyncBuffer, data...) + } + + // clear message when new one begins (first byte is always 0xFF) + if len(data) > 0 && data[0] == 0xFF { + d.asyncMessage = d.asyncBuffer + d.asyncBuffer = data + newMessage = true + } + + parts := d.asyncMessage + // 3 is the id of data streaming, located at index 2 byte + if newMessage && len(parts) > 2 && parts[2] == 3 { + d.handleDataStreaming(parts) + } + + // index 1 is the type of the message, 0xFF being a direct response, 0xFE an asynchronous message + if len(data) > 4 && data[1] == 0xFF && data[0] == 0xFF { + // locator request + if data[4] == 0x0B && len(data) == 16 { + d.handleLocatorDetected(data) + } + + if data[4] == 0x09 { + d.handlePowerStateDetected(data) + } + } + + d.handleCollisionDetected(data) +} + +func (d *OllieDriver) handleDataStreaming(data []byte) { + // ensure data is the right length: + if len(data) != 88 { + return + } + + // data packet is the same as for the normal sphero, since the same communication api is used + // only difference in communication is that the "newer" spheros use BLE for communications + var dataPacket sphero.DataStreamingPacket + buffer := bytes.NewBuffer(data[5:]) // skip header + if err := binary.Read(buffer, binary.BigEndian, &dataPacket); err != nil { + panic(err) + } + + d.Publish(sphero.SensorDataEvent, dataPacket) +} + +func (d *OllieDriver) handleLocatorDetected(data []uint8) { + if d.locatorCallback == nil { + return + } + + // read the unsigned raw values + ux := binary.BigEndian.Uint16(data[5:7]) + uy := binary.BigEndian.Uint16(data[7:9]) + + // convert to signed values + var x, y int16 + + if ux > 32255 { + x = int16(ux - 65535) + } else { + x = int16(ux) + } + + if uy > 32255 { + y = int16(uy - 65535) + } else { + y = int16(uy) + } + + d.locatorCallback(Point2D{X: x, Y: y}) +} + +func (d *OllieDriver) handlePowerStateDetected(data []uint8) { + var dataPacket sphero.PowerStatePacket + buffer := bytes.NewBuffer(data[5:]) // skip header + if err := binary.Read(buffer, binary.BigEndian, &dataPacket); err != nil { + panic(err) + } + + d.powerstateCallback(dataPacket) +} + +func (d *OllieDriver) handleCollisionDetected(data []uint8) { + switch len(data) { + case responsePacketMaxSize: + // Check if this is the header of collision response. (i.e. first part of data) + // Collision response is 22 bytes long. (individual packet size is maxed at 20) + if data[1] == 0xFE && data[2] == 0x07 && len(d.collisionResponse) == 0 { + // response code 7 is for a detected collision + d.collisionResponse = append(d.collisionResponse, data...) + } + case collisionResponseSize - responsePacketMaxSize: + // if this is the remaining part of the collision response, + // then make sure the header and first part of data is already received + if len(d.collisionResponse) == responsePacketMaxSize { + d.collisionResponse = append(d.collisionResponse, data...) + } + default: + return // not collision event + } + + // check expected sizes + if len(d.collisionResponse) != collisionResponseSize || d.collisionResponse[4] != collisionDataSize { + return + } + + // confirm checksum + size := len(d.collisionResponse) + chk := d.collisionResponse[size-1] // last byte is checksum + if chk != sphero.CalculateChecksum(d.collisionResponse[2:size-1]) { + return + } + + var collision sphero.CollisionPacket + buffer := bytes.NewBuffer(d.collisionResponse[5:]) // skip header + if err := binary.Read(buffer, binary.BigEndian, &collision); err != nil { + panic(err) + } + d.collisionResponse = nil // clear the current response + + d.Publish(sphero.CollisionEvent, collision) +} + +func (d *OllieDriver) sendCraftPacket(body []uint8, did byte, cid byte) { + d.packetChannel <- d.craftPacket(body, did, cid) +} + +func (d *OllieDriver) craftPacket(body []uint8, did byte, cid byte) *packet { + dlen := len(body) + 1 + hdr := []uint8{0xFF, 0xFF, did, cid, d.seq, uint8(dlen)} + buf := append(hdr, body...) + + packet := &packet{ + body: body, + header: hdr, + checksum: sphero.CalculateChecksum(buf[2:]), + } + + return packet +} + +// ollieDefaultCollisionConfig returns a CollisionConfig with sensible collision defaults +func ollieDefaultCollisionConfig() sphero.CollisionConfig { + return sphero.CollisionConfig{ + Method: 0x01, + Xt: 0x20, + Yt: 0x20, + Xs: 0x20, + Ys: 0x20, + Dead: 0x60, + } +} diff --git a/platforms/sphero/ollie/ollie_driver_test.go b/drivers/ble/sphero/sphero_ollie_driver_test.go similarity index 63% rename from platforms/sphero/ollie/ollie_driver_test.go rename to drivers/ble/sphero/sphero_ollie_driver_test.go index b736c0d82..d4758c970 100644 --- a/platforms/sphero/ollie/ollie_driver_test.go +++ b/drivers/ble/sphero/sphero_ollie_driver_test.go @@ -1,8 +1,7 @@ //nolint:forcetypeassert // ok here -package ollie +package sphero import ( - "fmt" "strconv" "testing" "time" @@ -11,23 +10,26 @@ import ( "github.com/stretchr/testify/require" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/sphero" + "gobot.io/x/gobot/v2/drivers/ble/testutil" + "gobot.io/x/gobot/v2/drivers/common/sphero" ) -var _ gobot.Driver = (*Driver)(nil) +var _ gobot.Driver = (*OllieDriver)(nil) -func initTestOllieDriver() *Driver { - d := NewDriver(NewBleTestAdaptor()) +func initTestOllieDriver() *OllieDriver { + d := NewOllieDriver(testutil.NewBleTestAdaptor()) return d } -func TestOllieDriver(t *testing.T) { - d := initTestOllieDriver() - d.SetName("NewName") - assert.Equal(t, "NewName", d.Name()) +func TestNewOllieDriver(t *testing.T) { + d := NewOllieDriver(testutil.NewBleTestAdaptor()) + assert.NotNil(t, d.Driver) + assert.NotNil(t, d.Eventer) + assert.Equal(t, d.defaultCollisionConfig, ollieDefaultCollisionConfig()) + assert.NotNil(t, d.packetChannel) } -func TestOllieDriverStartAndHalt(t *testing.T) { +func TestOllieStartAndHalt(t *testing.T) { d := initTestOllieDriver() require.NoError(t, d.Start()) require.NoError(t, d.Halt()) @@ -60,22 +62,24 @@ func TestLocatorData(t *testing.T) { d.GetLocatorData(func(p Point2D) { assert.Equal(t, point.y, p.Y) }) - d.HandleResponses(packet, nil) + d.handleResponses(packet, nil) } } func TestDataStreaming(t *testing.T) { d := initTestOllieDriver() - _ = d.SetDataStreamingConfig(sphero.DefaultDataStreamingConfig()) + err := d.SetDataStreamingConfig(sphero.DefaultDataStreamingConfig()) + require.NoError(t, err) responseChan := make(chan bool) - _ = d.On("sensordata", func(data interface{}) { - cont := data.(DataStreamingPacket) - fmt.Printf("got streaming packet: %+v \n", cont) + err = d.On("sensordata", func(data interface{}) { + cont := data.(sphero.DataStreamingPacket) + // fmt.Printf("got streaming packet: %+v \n", cont) assert.Equal(t, int16(10), cont.RawAccX) responseChan <- true }) + require.NoError(t, err) // example data packet p1 := []string{ @@ -90,16 +94,17 @@ func TestDataStreaming(t *testing.T) { var bytes []byte for i := 0; i < len([]rune(elem)); i += 2 { a := []rune(elem)[i : i+2] - b, _ := strconv.ParseUint(string(a), 16, 16) + b, err := strconv.ParseUint(string(a), 16, 16) + require.NoError(t, err) + c := uint16(b) bytes = append(bytes, byte(c)) } - d.HandleResponses(bytes, nil) - + d.handleResponses(bytes, nil) } // send empty packet to indicate start of next message - d.HandleResponses([]byte{0xFF}, nil) + d.handleResponses([]byte{0xFF}, nil) select { case <-responseChan: case <-time.After(10 * time.Millisecond): diff --git a/drivers/ble/sphero/sphero_sprkplus_driver.go b/drivers/ble/sphero/sphero_sprkplus_driver.go new file mode 100644 index 000000000..744d571c9 --- /dev/null +++ b/drivers/ble/sphero/sphero_sprkplus_driver.go @@ -0,0 +1,28 @@ +package sphero + +import ( + "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/common/sphero" +) + +// SPRKPlusDriver represents a Sphero SPRK+ +type SPRKPlusDriver struct { + *OllieDriver +} + +// NewSPRKPlusDriver creates a driver for a Sphero SPRK+ +func NewSPRKPlusDriver(a gobot.BLEConnector) *SPRKPlusDriver { + return &SPRKPlusDriver{OllieDriver: newOllieBaseDriver(a, "SPRKPlus", sprkplusDefaultCollisionConfig())} +} + +// sprkplusDefaultCollisionConfig returns a CollisionConfig with sensible collision defaults +func sprkplusDefaultCollisionConfig() sphero.CollisionConfig { + return sphero.CollisionConfig{ + Method: 0x01, + Xt: 0x20, + Yt: 0x20, + Xs: 0x20, + Ys: 0x20, + Dead: 0x01, + } +} diff --git a/drivers/ble/sphero/sphero_sprkplus_driver_test.go b/drivers/ble/sphero/sphero_sprkplus_driver_test.go new file mode 100644 index 000000000..382c08ed0 --- /dev/null +++ b/drivers/ble/sphero/sphero_sprkplus_driver_test.go @@ -0,0 +1,21 @@ +package sphero + +import ( + "strings" + "testing" + + "github.com/stretchr/testify/assert" + + "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/ble/testutil" +) + +var _ gobot.Driver = (*SPRKPlusDriver)(nil) + +func TestNewSPRKPlusDriver(t *testing.T) { + d := NewSPRKPlusDriver(testutil.NewBleTestAdaptor()) + assert.NotNil(t, d.OllieDriver) + assert.True(t, strings.HasPrefix(d.Name(), "SPRK")) + assert.NotNil(t, d.OllieDriver) + assert.Equal(t, d.defaultCollisionConfig, sprkplusDefaultCollisionConfig()) +} diff --git a/platforms/microbit/helpers_test.go b/drivers/ble/testutil/testutil.go similarity index 58% rename from platforms/microbit/helpers_test.go rename to drivers/ble/testutil/testutil.go index acf446f2d..4724e5689 100644 --- a/platforms/microbit/helpers_test.go +++ b/drivers/ble/testutil/testutil.go @@ -1,12 +1,12 @@ -package microbit +package testutil import ( "sync" - "gobot.io/x/gobot/v2/platforms/ble" + "gobot.io/x/gobot/v2" ) -var _ ble.BLEConnector = (*bleTestClientAdaptor)(nil) +var _ gobot.BLEConnector = (*bleTestClientAdaptor)(nil) type bleTestClientAdaptor struct { name string @@ -14,64 +14,63 @@ type bleTestClientAdaptor struct { mtx sync.Mutex withoutResponses bool - testSubscribe func([]byte, error) - testReadCharacteristic func(string) ([]byte, error) - testWriteCharacteristic func(string, []byte) error + readCharacteristicFunc func(string) ([]byte, error) + writeCharacteristicFunc func(string, []byte) error + subscribeFunc func([]byte, error) } -func (t *bleTestClientAdaptor) Connect() error { return nil } -func (t *bleTestClientAdaptor) Reconnect() error { return nil } -func (t *bleTestClientAdaptor) Disconnect() error { return nil } -func (t *bleTestClientAdaptor) Finalize() error { return nil } -func (t *bleTestClientAdaptor) Name() string { return t.name } -func (t *bleTestClientAdaptor) SetName(n string) { t.name = n } -func (t *bleTestClientAdaptor) Address() string { return t.address } -func (t *bleTestClientAdaptor) WithoutResponses(use bool) { t.withoutResponses = use } +func NewBleTestAdaptor() *bleTestClientAdaptor { + return &bleTestClientAdaptor{ + address: "01:02:03:0A:0B:0C", + readCharacteristicFunc: func(cUUID string) ([]byte, error) { + return []byte(cUUID), nil + }, + writeCharacteristicFunc: func(cUUID string, data []byte) error { + return nil + }, + } +} -func (t *bleTestClientAdaptor) ReadCharacteristic(cUUID string) ([]byte, error) { +func (t *bleTestClientAdaptor) SetReadCharacteristicTestFunc(f func(cUUID string) (data []byte, err error)) { t.mtx.Lock() defer t.mtx.Unlock() - return t.testReadCharacteristic(cUUID) + t.readCharacteristicFunc = f } -func (t *bleTestClientAdaptor) WriteCharacteristic(cUUID string, data []byte) error { +func (t *bleTestClientAdaptor) SetWriteCharacteristicTestFunc(f func(cUUID string, data []byte) error) { t.mtx.Lock() defer t.mtx.Unlock() - return t.testWriteCharacteristic(cUUID, data) -} - -func (t *bleTestClientAdaptor) Subscribe(cUUID string, f func([]byte, error)) error { - t.testSubscribe = f - return nil + t.writeCharacteristicFunc = f } -func (t *bleTestClientAdaptor) TestReadCharacteristic(f func(cUUID string) (data []byte, err error)) { +func (t *bleTestClientAdaptor) SendTestDataToSubscriber(data []byte, err error) { t.mtx.Lock() defer t.mtx.Unlock() - t.testReadCharacteristic = f + t.subscribeFunc(data, err) } -func (t *bleTestClientAdaptor) TestWriteCharacteristic(f func(cUUID string, data []byte) error) { +func (t *bleTestClientAdaptor) Connect() error { return nil } +func (t *bleTestClientAdaptor) Reconnect() error { return nil } +func (t *bleTestClientAdaptor) Disconnect() error { return nil } +func (t *bleTestClientAdaptor) Finalize() error { return nil } +func (t *bleTestClientAdaptor) Name() string { return t.name } +func (t *bleTestClientAdaptor) SetName(n string) { t.name = n } +func (t *bleTestClientAdaptor) Address() string { return t.address } +func (t *bleTestClientAdaptor) WithoutResponses(use bool) { t.withoutResponses = use } + +func (t *bleTestClientAdaptor) ReadCharacteristic(cUUID string) ([]byte, error) { t.mtx.Lock() defer t.mtx.Unlock() - t.testWriteCharacteristic = f + return t.readCharacteristicFunc(cUUID) } -func (t *bleTestClientAdaptor) TestReceiveNotification(data []byte, err error) { +func (t *bleTestClientAdaptor) WriteCharacteristic(cUUID string, data []byte) error { t.mtx.Lock() defer t.mtx.Unlock() - t.testSubscribe(data, err) + return t.writeCharacteristicFunc(cUUID, data) } -func NewBleTestAdaptor() *bleTestClientAdaptor { - return &bleTestClientAdaptor{ - address: "01:02:03:04:05:06", - testReadCharacteristic: func(cUUID string) ([]byte, error) { - return nil, nil - }, - testWriteCharacteristic: func(cUUID string, data []byte) error { - return nil - }, - testSubscribe: func([]byte, error) {}, - } +func (t *bleTestClientAdaptor) Subscribe(cUUID string, f func([]byte, error)) error { + t.subscribeFunc = f + return nil } diff --git a/drivers/common/bit/bit.go b/drivers/common/bit/bit.go new file mode 100644 index 000000000..fbfda283c --- /dev/null +++ b/drivers/common/bit/bit.go @@ -0,0 +1,20 @@ +package bit + +// Set is used to set a bit in the given integer at a given position to 1. +func Set(n int, pos uint8) int { + n |= (1 << pos) + return n +} + +// Clear is used to set a bit in the given integer at a given position to 0. +func Clear(n int, pos uint8) int { + mask := ^int(1 << pos) + n &= mask + return n +} + +// IsSet tests if the bit at the given position is set in the given integer. +func IsSet(n int, pos uint8) bool { + val := n & (1 << uint(pos)) + return (val > 0) +} diff --git a/drivers/common/bit/bit_test.go b/drivers/common/bit/bit_test.go new file mode 100644 index 000000000..cd5a39961 --- /dev/null +++ b/drivers/common/bit/bit_test.go @@ -0,0 +1,34 @@ +package bit + +import ( + "testing" + + "github.com/stretchr/testify/assert" + "github.com/stretchr/testify/require" +) + +func TestSet(t *testing.T) { + const ( + oldVal = 1 + bitPos = 7 + want = 129 + ) + require.False(t, IsSet(oldVal, bitPos)) + + got := Set(1, 7) + assert.Equal(t, want, got) + assert.True(t, IsSet(got, bitPos)) +} + +func TestClear(t *testing.T) { + const ( + oldVal = 128 + bitPos = 7 + want int = 0 + ) + require.True(t, IsSet(oldVal, bitPos)) + + got := Clear(128, 7) + assert.Equal(t, want, got) + assert.False(t, IsSet(got, bitPos)) +} diff --git a/drivers/common/sphero/sphero_driver.go b/drivers/common/sphero/sphero_driver.go new file mode 100644 index 000000000..d8391a150 --- /dev/null +++ b/drivers/common/sphero/sphero_driver.go @@ -0,0 +1,33 @@ +package sphero + +const ( + // ErrorEvent event when error encountered + ErrorEvent = "error" + + // SensorDataEvent event when sensor data is received + SensorDataEvent = "sensordata" + + // CollisionEvent event when collision is detected + CollisionEvent = "collision" +) + +// DefaultDataStreamingConfig returns a config with a sampling rate of 40hz, 1 sample frame per package, +// unlimited streaming, and will stream all available sensor information +func DefaultDataStreamingConfig() DataStreamingConfig { + return DataStreamingConfig{ + N: 10, + M: 1, + Mask: 4294967295, + Pcnt: 0, + Mask2: 4294967295, + } +} + +// CalculateChecksum calculates the checksum for Sphero packets +func CalculateChecksum(buf []byte) byte { + var calculatedChecksum uint16 + for i := range buf { + calculatedChecksum += uint16(buf[i]) + } + return uint8(^(calculatedChecksum % 256)) +} diff --git a/drivers/common/sphero/sphero_driver_test.go b/drivers/common/sphero/sphero_driver_test.go new file mode 100644 index 000000000..03759d7ff --- /dev/null +++ b/drivers/common/sphero/sphero_driver_test.go @@ -0,0 +1,20 @@ +package sphero + +import "testing" + +func TestCalculateChecksum(t *testing.T) { + tests := []struct { + data []byte + checksum byte + }{ + {[]byte{0x00}, 0xff}, + {[]byte{0xf0, 0x0f}, 0x00}, + } + + for _, tt := range tests { + actual := CalculateChecksum(tt.data) + if actual != tt.checksum { + t.Errorf("Expected %x, got %x for data %x.", tt.checksum, actual, tt.data) + } + } +} diff --git a/platforms/sphero/sphero_packets.go b/drivers/common/sphero/sphero_packets.go similarity index 87% rename from platforms/sphero/sphero_packets.go rename to drivers/common/sphero/sphero_packets.go index 33b17c9e8..0f6333f85 100644 --- a/platforms/sphero/sphero_packets.go +++ b/drivers/common/sphero/sphero_packets.go @@ -1,15 +1,5 @@ package sphero -// DefaultLocatorConfig returns a LocatorConfig with defaults -func DefaultLocatorConfig() LocatorConfig { - return LocatorConfig{ - Flags: 0x01, - X: 0x00, - Y: 0x00, - YawTare: 0x00, - } -} - // LocatorConfig provides configuration for the Location api. // https://github.com/orbotix/DeveloperResources/blob/master/docs/Sphero_API_1.50.pdf // The current (X,Y) coordinates of Sphero on the ground plane in centimeters. @@ -24,18 +14,6 @@ type LocatorConfig struct { YawTare int16 } -// DefaultCollisionConfig returns a CollisionConfig with sensible collision defaults -func DefaultCollisionConfig() CollisionConfig { - return CollisionConfig{ - Method: 0x01, - Xt: 0x80, - Yt: 0x80, - Xs: 0x80, - Ys: 0x80, - Dead: 0x60, - } -} - // CollisionConfig provides configuration for the collision detection alogorithm. // For more information refer to the official api specification // https://github.com/orbotix/DeveloperResources/blob/master/docs/Collision%20detection%201.2.pdf. @@ -60,32 +38,6 @@ type CollisionConfig struct { Dead uint8 } -// CollisionPacket represents the response from a Collision event -type CollisionPacket struct { - // Normalized impact components (direction of the collision event): - X, Y, Z int16 - // Thresholds exceeded by X (1h) and/or Y (2h) axis (bitmask): - Axis byte - // Power that cross threshold Xt + Xs: - XMagnitude, YMagnitude int16 - // Sphero's speed when impact detected: - Speed uint8 - // Millisecond timer - Timestamp uint32 -} - -// DefaultDataStreamingConfig returns a DataStreamingConfig with a sampling rate of 40hz, 1 sample frame per package, -// unlimited streaming, and will stream all available sensor information -func DefaultDataStreamingConfig() DataStreamingConfig { - return DataStreamingConfig{ - N: 10, - M: 1, - Mask: 4294967295, - Pcnt: 0, - Mask2: 4294967295, - } -} - // DataStreamingConfig provides configuration for Sensor Data Streaming. // For more information refer to the official api specification // https://github.com/orbotix/DeveloperResources/blob/master/docs/Sphero_API_1.50.pdf page 28 @@ -102,6 +54,34 @@ type DataStreamingConfig struct { Mask2 uint32 } +// PowerStatePacket contains all data relevant to the power state of the sphero +type PowerStatePacket struct { + // record Version Code + RecVer uint8 + // High-Level State of the Battery; 1=charging, 2=battery ok, 3=battery low, 4=battery critical + PowerState uint8 + // Battery Voltage, scaled in 100th of a Volt, 0x02EF would be 7.51 volts + BattVoltage uint16 + // Number of charges in the total lifetime of the sphero + NumCharges uint16 + // Seconds awake since last charge + TimeSinceChg uint16 +} + +// CollisionPacket represents the response from a Collision event +type CollisionPacket struct { + // Normalized impact components (direction of the collision event): + X, Y, Z int16 + // Thresholds exceeded by X (1h) and/or Y (2h) axis (bitmask): + Axis byte + // Power that cross threshold Xt + Xs: + XMagnitude, YMagnitude int16 + // Sphero's speed when impact detected: + Speed uint8 + // Millisecond timer + Timestamp uint32 +} + // DataStreamingPacket represents the response from a Data Streaming event type DataStreamingPacket struct { // 8000 0000h accelerometer axis X, raw -2048 to 2047 4mG diff --git a/drivers/i2c/i2c_connection.go b/drivers/i2c/i2c_connection.go index 3f402a8d3..e1c08a36d 100644 --- a/drivers/i2c/i2c_connection.go +++ b/drivers/i2c/i2c_connection.go @@ -110,19 +110,6 @@ func (c *i2cConnection) WriteBytes(b []byte) error { return c.bus.WriteBytes(c.address, b) } -// setBit is used to set a bit at a given position to 1. -func setBit(n uint8, pos uint8) uint8 { - n |= (1 << pos) - return n -} - -// clearBit is used to set a bit at a given position to 0. -func clearBit(n uint8, pos uint8) uint8 { - mask := ^uint8(1 << pos) - n &= mask - return n -} - func twosComplement16Bit(uValue uint16) int16 { result := int32(uValue) if result&0x8000 != 0 { diff --git a/drivers/i2c/i2c_connection_test.go b/drivers/i2c/i2c_connection_test.go index fdce313d2..248d90f48 100644 --- a/drivers/i2c/i2c_connection_test.go +++ b/drivers/i2c/i2c_connection_test.go @@ -190,15 +190,3 @@ func TestI2CWriteBlockDataAddressError(t *testing.T) { err := c.WriteBlockData(0x01, []byte{0x01, 0x02}) require.ErrorContains(t, err, "Setting address failed with syscall.Errno operation not permitted") } - -func Test_setBit(t *testing.T) { - var wantVal uint8 = 129 - gotVal := setBit(1, 7) - assert.Equal(t, wantVal, gotVal) -} - -func Test_clearBit(t *testing.T) { - var wantVal uint8 - gotVal := clearBit(128, 7) - assert.Equal(t, wantVal, gotVal) -} diff --git a/drivers/i2c/mcp23017_driver.go b/drivers/i2c/mcp23017_driver.go index d54abee76..def7fe527 100644 --- a/drivers/i2c/mcp23017_driver.go +++ b/drivers/i2c/mcp23017_driver.go @@ -6,6 +6,7 @@ import ( "strings" "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/common/bit" ) // default address for device when a2/a1/a0 pins are all tied to ground @@ -323,9 +324,9 @@ func (m *MCP23017Driver) write(reg uint8, pin uint8, state bitState) error { var val uint8 if state == clear { - val = clearBit(valOrg, pin) + val = uint8(bit.Clear(int(valOrg), pin)) } else { - val = setBit(valOrg, pin) + val = uint8(bit.Set(int(valOrg), pin)) } if val != valOrg || m.mcpBehav.forceRefresh { diff --git a/drivers/i2c/pca9501_driver.go b/drivers/i2c/pca9501_driver.go index daa7da583..a71934ca7 100644 --- a/drivers/i2c/pca9501_driver.go +++ b/drivers/i2c/pca9501_driver.go @@ -1,5 +1,7 @@ package i2c +import "gobot.io/x/gobot/v2/drivers/common/bit" + // PCA9501 supports addresses from 0x00 to 0x7F // 0x00 - 0x3F: GPIO, 0x40 - 0x7F: EEPROM // @@ -80,9 +82,9 @@ func (p *PCA9501Driver) WriteGPIO(pin uint8, val uint8) error { return err } // set pin as output by clearing bit - iodirVal := clearBit(iodir, pin) + iodirVal := bit.Clear(int(iodir), pin) // write CTRL register - err = p.connection.WriteByte(iodirVal) + err = p.connection.WriteByte(uint8(iodirVal)) if err != nil { return err } @@ -92,14 +94,14 @@ func (p *PCA9501Driver) WriteGPIO(pin uint8, val uint8) error { return err } // set or reset the bit in value - var nVal uint8 + var nVal int if val == 0 { - nVal = clearBit(cVal, pin) + nVal = bit.Clear(int(cVal), pin) } else { - nVal = setBit(cVal, pin) + nVal = bit.Set(int(cVal), pin) } // write new value to port - err = p.connection.WriteByte(nVal) + err = p.connection.WriteByte(uint8(nVal)) if err != nil { return err } @@ -117,9 +119,9 @@ func (p *PCA9501Driver) ReadGPIO(pin uint8) (uint8, error) { return 0, err } // set pin as input by setting bit - iodirVal := setBit(iodir, pin) + iodirVal := bit.Set(int(iodir), pin) // write CTRL register - err = p.connection.WriteByte(iodirVal) + err = p.connection.WriteByte(uint8(iodirVal)) if err != nil { return 0, err } diff --git a/drivers/serial/LICENSE b/drivers/serial/LICENSE new file mode 100644 index 000000000..09094bb5e --- /dev/null +++ b/drivers/serial/LICENSE @@ -0,0 +1,13 @@ +Copyright (c) 2013-2018 The Hybrid Group + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. diff --git a/drivers/serial/README.md b/drivers/serial/README.md new file mode 100644 index 000000000..40da591f3 --- /dev/null +++ b/drivers/serial/README.md @@ -0,0 +1,15 @@ +# Serial + +This package provides drivers based on [Serial](https://en.wikipedia.org/wiki/Serial_port) communication ([UART](https://en.wikipedia.org/wiki/Universal_asynchronous_receiver-transmitter)). +devices. It is normally used by connecting an adaptor such as [SerialPort](https://gobot.io/documentation/platforms/serialport/) +that supports the needed interfaces for serial devices. + +## Getting Started + +Please refer to the main [README.md](https://github.com/hybridgroup/gobot/blob/release/README.md) + +## Hardware Support + +Gobot has a extensible system for connecting to hardware devices. The following Serial devices are currently supported: + +- Sphero: Sphero diff --git a/drivers/serial/doc.go b/drivers/serial/doc.go new file mode 100644 index 000000000..11dd1b31e --- /dev/null +++ b/drivers/serial/doc.go @@ -0,0 +1,7 @@ +/* +Package serial provides Gobot drivers for Serial Port communication based devices. + +For further information refer to readme: +https://github.com/hybridgroup/gobot/blob/master/drivers/serial/README.md +*/ +package serial // import "gobot.io/x/gobot/v2/drivers/serial" diff --git a/drivers/serial/helpers_test.go b/drivers/serial/helpers_test.go new file mode 100644 index 000000000..b63f659f8 --- /dev/null +++ b/drivers/serial/helpers_test.go @@ -0,0 +1,28 @@ +package serial + +type serialTestAdaptor struct { + isConnected bool + name string +} + +func newSerialTestAdaptor() *serialTestAdaptor { + return &serialTestAdaptor{} +} + +func (t *serialTestAdaptor) IsConnected() bool { + return t.isConnected +} + +func (t *serialTestAdaptor) SerialRead(b []byte) (int, error) { + return len(b), nil +} + +func (t *serialTestAdaptor) SerialWrite(b []byte) (int, error) { + return len(b), nil +} + +// gobot.Adaptor interfaces +func (t *serialTestAdaptor) Connect() error { return nil } +func (t *serialTestAdaptor) Finalize() error { return nil } +func (t *serialTestAdaptor) Name() string { return t.name } +func (t *serialTestAdaptor) SetName(n string) { t.name = n } diff --git a/drivers/serial/serial_driver.go b/drivers/serial/serial_driver.go new file mode 100644 index 000000000..9b1aaf724 --- /dev/null +++ b/drivers/serial/serial_driver.go @@ -0,0 +1,104 @@ +package serial + +import ( + "log" + "sync" + + "gobot.io/x/gobot/v2" +) + +type SerialReader interface { + SerialRead(b []byte) (n int, err error) +} + +type SerialWriter interface { + SerialWrite(b []byte) (n int, err error) +} + +// optionApplier needs to be implemented by each configurable option type +type optionApplier interface { + apply(cfg *configuration) +} + +// configuration contains all changeable attributes of the driver. +type configuration struct { + name string +} + +// nameOption is the type for applying another name to the configuration +type nameOption string + +// Driver implements the interface gobot.Driver. +type driver struct { + gobot.Commander + connection interface{} + driverCfg *configuration + afterStart func() error + beforeHalt func() error + mutex *sync.Mutex +} + +// newDriver creates a new basic serial gobot driver. +func newDriver(a interface{}, name string) *driver { + d := driver{ + Commander: gobot.NewCommander(), + connection: a, + driverCfg: &configuration{name: gobot.DefaultName(name)}, + afterStart: func() error { return nil }, + beforeHalt: func() error { return nil }, + mutex: &sync.Mutex{}, + } + + return &d +} + +// WithName is used to replace the default name of the driver. +func WithName(name string) optionApplier { + return nameOption(name) +} + +// Name returns the name of the driver. +func (d *driver) Name() string { + return d.driverCfg.name +} + +// SetName sets the name of the driver. +// Deprecated: Please use option [aio.WithName] instead. +func (d *driver) SetName(name string) { + WithName(name).apply(d.driverCfg) +} + +// Connection returns the connection of the driver. +func (d *driver) Connection() gobot.Connection { + if conn, ok := d.connection.(gobot.Connection); ok { + return conn + } + + log.Printf("%s has no gobot connection\n", d.driverCfg.name) + return nil +} + +// Start initializes the driver. +func (d *driver) Start() error { + d.mutex.Lock() + defer d.mutex.Unlock() + + // currently there is nothing to do here for the driver + + return d.afterStart() +} + +// Halt halts the driver. +func (d *driver) Halt() error { + d.mutex.Lock() + defer d.mutex.Unlock() + + // currently there is nothing to do after halt for the driver + + return d.beforeHalt() +} + +// apply change the name in the configuration. +func (o nameOption) apply(c *configuration) { + c.name = string(o) +} diff --git a/drivers/serial/serial_driver_test.go b/drivers/serial/serial_driver_test.go new file mode 100644 index 000000000..b2dda80c6 --- /dev/null +++ b/drivers/serial/serial_driver_test.go @@ -0,0 +1,69 @@ +package serial + +import ( + "fmt" + "strings" + "testing" + + "github.com/stretchr/testify/assert" + "github.com/stretchr/testify/require" + + "gobot.io/x/gobot/v2" +) + +var _ gobot.Driver = (*driver)(nil) + +func initTestDriver() *driver { + a := newSerialTestAdaptor() + d := newDriver(a, "SERIAL_BASIC") + return d +} + +func Test_newDriver(t *testing.T) { + // arrange + const name = "mybot" + a := newSerialTestAdaptor() + // act + d := newDriver(a, name) + // assert + assert.IsType(t, &driver{}, d) + assert.NotNil(t, d.driverCfg) + assert.True(t, strings.HasPrefix(d.Name(), name)) + assert.Equal(t, a, d.Connection()) + require.NoError(t, d.afterStart()) + require.NoError(t, d.beforeHalt()) + assert.NotNil(t, d.Commander) + assert.NotNil(t, d.mutex) +} + +func Test_applyWithName(t *testing.T) { + // arrange + const name = "mybot" + cfg := configuration{name: "oldname"} + // act + WithName(name).apply(&cfg) + // assert + assert.Equal(t, name, cfg.name) +} + +func TestStart(t *testing.T) { + // arrange + d := initTestDriver() + // act, assert + require.NoError(t, d.Start()) + // arrange after start function + d.afterStart = func() error { return fmt.Errorf("after start error") } + // act, assert + require.EqualError(t, d.Start(), "after start error") +} + +func TestHalt(t *testing.T) { + // arrange + d := initTestDriver() + // act, assert + require.NoError(t, d.Halt()) + // arrange after start function + d.beforeHalt = func() error { return fmt.Errorf("before halt error") } + // act, assert + require.EqualError(t, d.Halt(), "before halt error") +} diff --git a/drivers/serial/sphero_driver.go b/drivers/serial/sphero_driver.go new file mode 100644 index 000000000..b65c52f7b --- /dev/null +++ b/drivers/serial/sphero_driver.go @@ -0,0 +1,476 @@ +package serial + +import ( + "bytes" + "encoding/binary" + "errors" + "log" + "time" + + "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/common/sphero" +) + +type spheroSerialAdaptor interface { + gobot.Adaptor + SerialReader + SerialWriter + + IsConnected() bool +} + +type packet struct { + header []uint8 + body []uint8 + checksum uint8 +} + +// SpheroDriver Represents a Sphero 2.0 +type SpheroDriver struct { + *driver + gobot.Eventer + seq uint8 + asyncResponse [][]uint8 + syncResponse [][]uint8 + packetChannel chan *packet + responseChannel chan []uint8 + originalColor []uint8 // Only used for calibration. +} + +// NewSpheroDriver returns a new SpheroDriver given a Sphero Adaptor. +// +// Adds the following API Commands: +// +// "ConfigureLocator" - See SpheroDriver.ConfigureLocator +// "Roll" - See SpheroDriver.Roll +// "Stop" - See SpheroDriver.Stop +// "GetRGB" - See SpheroDriver.GetRGB +// "ReadLocator" - See SpheroDriver.ReadLocator +// "SetBackLED" - See SpheroDriver.SetBackLED +// "SetHeading" - See SpheroDriver.SetHeading +// "SetStabilization" - See SpheroDriver.SetStabilization +// "SetDataStreaming" - See SpheroDriver.SetDataStreaming +// "SetRotationRate" - See SpheroDriver.SetRotationRate +func NewSpheroDriver(a spheroSerialAdaptor) *SpheroDriver { + d := &SpheroDriver{ + driver: newDriver(a, "Sphero"), + Eventer: gobot.NewEventer(), + packetChannel: make(chan *packet, 1024), + responseChannel: make(chan []uint8, 1024), + } + d.afterStart = d.initialize + d.beforeHalt = d.shutdown + + d.AddEvent(sphero.ErrorEvent) + d.AddEvent(sphero.CollisionEvent) + d.AddEvent(sphero.SensorDataEvent) + + //nolint:forcetypeassert // ok here + d.AddCommand("SetRGB", func(params map[string]interface{}) interface{} { + r := uint8(params["r"].(float64)) + g := uint8(params["g"].(float64)) + b := uint8(params["b"].(float64)) + d.SetRGB(r, g, b) + return nil + }) + + //nolint:forcetypeassert // ok here + d.AddCommand("Roll", func(params map[string]interface{}) interface{} { + speed := uint8(params["speed"].(float64)) + heading := uint16(params["heading"].(float64)) + d.Roll(speed, heading) + return nil + }) + + d.AddCommand("Stop", func(params map[string]interface{}) interface{} { + d.Stop() + return nil + }) + + d.AddCommand("GetRGB", func(params map[string]interface{}) interface{} { + return d.GetRGB() + }) + + d.AddCommand("ReadLocator", func(params map[string]interface{}) interface{} { + return d.ReadLocator() + }) + + //nolint:forcetypeassert // ok here + d.AddCommand("SetBackLED", func(params map[string]interface{}) interface{} { + level := uint8(params["level"].(float64)) + d.SetBackLED(level) + return nil + }) + //nolint:forcetypeassert // ok here + d.AddCommand("SetRotationRate", func(params map[string]interface{}) interface{} { + level := uint8(params["level"].(float64)) + d.SetRotationRate(level) + return nil + }) + //nolint:forcetypeassert // ok here + d.AddCommand("SetHeading", func(params map[string]interface{}) interface{} { + heading := uint16(params["heading"].(float64)) + d.SetHeading(heading) + return nil + }) + //nolint:forcetypeassert // ok here + d.AddCommand("SetStabilization", func(params map[string]interface{}) interface{} { + on := params["enable"].(bool) + d.SetStabilization(on) + return nil + }) + //nolint:forcetypeassert // ok here + d.AddCommand("SetDataStreaming", func(params map[string]interface{}) interface{} { + N := uint16(params["N"].(float64)) + M := uint16(params["M"].(float64)) + Mask := uint32(params["Mask"].(float64)) + Pcnt := uint8(params["Pcnt"].(float64)) + Mask2 := uint32(params["Mask2"].(float64)) + + d.SetDataStreaming(sphero.DataStreamingConfig{N: N, M: M, Mask2: Mask2, Pcnt: Pcnt, Mask: Mask}) + return nil + }) + //nolint:forcetypeassert // ok here + d.AddCommand("ConfigureLocator", func(params map[string]interface{}) interface{} { + Flags := uint8(params["Flags"].(float64)) + X := int16(params["X"].(float64)) + Y := int16(params["Y"].(float64)) + YawTare := int16(params["YawTare"].(float64)) + + d.ConfigureLocator(sphero.LocatorConfig{Flags: Flags, X: X, Y: Y, YawTare: YawTare}) + return nil + }) + + return d +} + +// SetRGB sets the Sphero to the given r, g, and b values +func (d *SpheroDriver) SetRGB(r uint8, g uint8, b uint8) { + d.sendCraftPacket([]uint8{r, g, b, 0x01}, 0x20) +} + +// GetRGB returns the current r, g, b value of the Sphero +func (d *SpheroDriver) GetRGB() []uint8 { + buf := d.getSyncResponse(d.craftPacket([]uint8{}, 0x22)) + if len(buf) == 9 { + return []uint8{buf[5], buf[6], buf[7]} + } + return []uint8{} +} + +// ReadLocator reads Sphero's current position (X,Y), component velocities and SOG (speed over ground). +func (d *SpheroDriver) ReadLocator() []int16 { + buf := d.getSyncResponse(d.craftPacket([]uint8{}, 0x15)) + if len(buf) == 16 { + vals := make([]int16, 5) + _ = binary.Read(bytes.NewReader(buf[5:15]), binary.BigEndian, &vals) + return vals + } + return []int16{} +} + +// SetBackLED sets the Sphero Back LED to the specified brightness +func (d *SpheroDriver) SetBackLED(level uint8) { + d.sendCraftPacket([]uint8{level}, 0x21) +} + +// SetRotationRate sets the Sphero rotation rate +// A value of 255 jumps to the maximum (currently 400 degrees/sec). +func (d *SpheroDriver) SetRotationRate(level uint8) { + d.sendCraftPacket([]uint8{level}, 0x03) +} + +// SetHeading sets the heading of the Sphero +func (d *SpheroDriver) SetHeading(heading uint16) { + d.sendCraftPacket([]uint8{uint8(heading >> 8), uint8(heading & 0xFF)}, 0x01) +} + +// SetStabilization enables or disables the built-in auto stabilizing features of the Sphero +func (d *SpheroDriver) SetStabilization(on bool) { + b := uint8(0x01) + if !on { + b = 0x00 + } + d.sendCraftPacket([]uint8{b}, 0x02) +} + +// Roll sends a roll command to the Sphero gives a speed and heading +func (d *SpheroDriver) Roll(speed uint8, heading uint16) { + d.sendCraftPacket([]uint8{speed, uint8(heading >> 8), uint8(heading & 0xFF), 0x01}, 0x30) +} + +// ConfigureLocator configures and enables the Locator +func (d *SpheroDriver) ConfigureLocator(lc sphero.LocatorConfig) { + buf := new(bytes.Buffer) + if err := binary.Write(buf, binary.BigEndian, lc); err != nil { + panic(err) + } + + d.sendCraftPacket(buf.Bytes(), 0x13) +} + +// SetDataStreaming enables sensor data streaming +func (d *SpheroDriver) SetDataStreaming(dsc sphero.DataStreamingConfig) { + buf := new(bytes.Buffer) + if err := binary.Write(buf, binary.BigEndian, dsc); err != nil { + panic(err) + } + + d.sendCraftPacket(buf.Bytes(), 0x11) +} + +// Stop sets the Sphero to a roll speed of 0 +func (d *SpheroDriver) Stop() { + d.Roll(0, 0) +} + +// ConfigureCollisionDetection configures the sensitivity of the detection. +func (d *SpheroDriver) ConfigureCollisionDetection(cc sphero.CollisionConfig) { + d.sendCraftPacket([]uint8{cc.Method, cc.Xt, cc.Yt, cc.Xs, cc.Ys, cc.Dead}, 0x12) +} + +// SetCalibration sets up Sphero for manual heading calibration. +// It does this by turning on the tail light (so you can tell where it's +// facing) and disabling stabilization (so you can adjust the heading). +// +// When done, call FinishCalibration to set the new heading, and re-enable +// stabilization. +func (d *SpheroDriver) StartCalibration() { + d.originalColor = d.GetRGB() + d.SetRGB(0, 0, 0) + d.SetBackLED(127) + d.SetStabilization(false) +} + +// FinishCalibration ends Sphero's calibration mode, by setting +// the new heading as current, and re-enabling normal defaults. This is a NOP +// in case StartCalibration was not called. +func (d *SpheroDriver) FinishCalibration() { + if d.originalColor == nil { + // Piggybacking on the original color being set to know if we are + // calibrating or not. + return + } + + d.SetHeading(0) + d.SetRGB(d.originalColor[0], d.originalColor[1], d.originalColor[2]) + d.SetBackLED(0) + d.SetStabilization(true) + d.originalColor = nil +} + +// initialize starts the SpheroDriver and enables Collision Detection. +// Returns true on successful start. +// +// Emits the Events: +// +// Collision sphero.CollisionPacket - On Collision Detected +// SensorData sphero.DataStreamingPacket - On Data Streaming event +// Error error- On error while processing asynchronous response +func (d *SpheroDriver) initialize() error { + go func() { + for { + packet := <-d.packetChannel + err := d.write(packet) + if err != nil { + d.Publish(sphero.ErrorEvent, err) + } + } + }() + + go func() { + for { + response := <-d.responseChannel + d.syncResponse = append(d.syncResponse, response) + } + }() + + go func() { + for { + header := d.readHeader() + if len(header) > 0 { + body := d.readBody(header[4]) + data := append(header, body...) + checksum := data[len(data)-1] + if checksum != sphero.CalculateChecksum(data[2:len(data)-1]) { + continue + } + switch header[1] { + case 0xFE: + d.asyncResponse = append(d.asyncResponse, data) + case 0xFF: + d.responseChannel <- data + } + } + } + }() + + go func() { + for { + var evt []uint8 + for len(d.asyncResponse) != 0 { + evt, d.asyncResponse = d.asyncResponse[len(d.asyncResponse)-1], d.asyncResponse[:len(d.asyncResponse)-1] + if evt[2] == 0x07 { + d.handleCollisionDetected(evt) + } else if evt[2] == 0x03 { + d.handleDataStreaming(evt) + } + } + time.Sleep(100 * time.Millisecond) + } + }() + + d.ConfigureCollisionDetection(spheroDefaultCollisionConfig()) + d.enableStopOnDisconnect() + + return nil +} + +// shutdown halts the SpheroDriver and sends a SpheroDriver.Stop command to the Sphero. +// Returns true on successful halt. +func (d *SpheroDriver) shutdown() error { + if d.adaptor().IsConnected() { + gobot.Every(10*time.Millisecond, func() { + d.Stop() + }) + time.Sleep(1 * time.Second) + } + return nil +} + +func (d *SpheroDriver) enableStopOnDisconnect() { + d.sendCraftPacket([]uint8{0x00, 0x00, 0x00, 0x01}, 0x37) +} + +func (d *SpheroDriver) handleCollisionDetected(data []uint8) { + // ensure data is the right length: + if len(data) != 22 || data[4] != 17 { + return + } + var collision sphero.CollisionPacket + buffer := bytes.NewBuffer(data[5:]) // skip header + if err := binary.Read(buffer, binary.BigEndian, &collision); err != nil { + panic(err) + } + d.Publish(sphero.CollisionEvent, collision) +} + +func (d *SpheroDriver) handleDataStreaming(data []uint8) { + // ensure data is the right length: + if len(data) != 90 { + return + } + var dataPacket sphero.DataStreamingPacket + buffer := bytes.NewBuffer(data[5:]) // skip header + if err := binary.Read(buffer, binary.BigEndian, &dataPacket); err != nil { + panic(err) + } + d.Publish(sphero.SensorDataEvent, dataPacket) +} + +func (d *SpheroDriver) getSyncResponse(packet *packet) []byte { + d.packetChannel <- packet + for i := 0; i < 500; i++ { + for key := range d.syncResponse { + if d.syncResponse[key][3] == packet.header[4] && len(d.syncResponse[key]) > 6 { + var response []byte + response, d.syncResponse = d.syncResponse[len(d.syncResponse)-1], d.syncResponse[:len(d.syncResponse)-1] + return response + } + } + time.Sleep(100 * time.Microsecond) + } + + return []byte{} +} + +func (d *SpheroDriver) sendCraftPacket(body []uint8, cid byte) { + d.packetChannel <- d.craftPacket(body, cid) +} + +func (d *SpheroDriver) craftPacket(body []uint8, cid byte) *packet { + dlen := len(body) + 1 + did := uint8(0x02) + hdr := []uint8{0xFF, 0xFF, did, cid, d.seq, uint8(dlen)} + buf := append(hdr, body...) + + packet := &packet{ + body: body, + header: hdr, + checksum: sphero.CalculateChecksum(buf[2:]), + } + + return packet +} + +func (d *SpheroDriver) write(packet *packet) error { + d.mutex.Lock() + defer d.mutex.Unlock() + + buf := append(packet.header, packet.body...) + buf = append(buf, packet.checksum) + length, err := d.adaptor().SerialWrite(buf) + if err != nil { + return err + } + + if length != len(buf) { + return errors.New("Not enough bytes written") + } + d.seq++ + return nil +} + +func (d *SpheroDriver) readHeader() []uint8 { + return d.readNextChunk(5) +} + +func (d *SpheroDriver) readBody(length uint8) []uint8 { + return d.readNextChunk(int(length)) +} + +func (d *SpheroDriver) readNextChunk(length int) []uint8 { + read := make([]uint8, length) + bytesRead := 0 + + for bytesRead < length { + time.Sleep(1 * time.Millisecond) + n, err := d.adaptor().SerialRead(read[bytesRead:]) + if err != nil { + return nil + } + bytesRead += n + } + return read +} + +func (d *SpheroDriver) adaptor() spheroSerialAdaptor { + if a, ok := d.connection.(spheroSerialAdaptor); ok { + return a + } + + log.Printf("%s has no Sphere serial connector\n", d.driverCfg.name) + return nil +} + +// spheroDefaultCollisionConfig returns a CollisionConfig with sensible collision defaults +func spheroDefaultCollisionConfig() sphero.CollisionConfig { + return sphero.CollisionConfig{ + Method: 0x01, + Xt: 0x80, + Yt: 0x80, + Xs: 0x80, + Ys: 0x80, + Dead: 0x60, + } +} + +// spheroDefaultLocatorConfig returns a LocatorConfig with defaults +func spheroDefaultLocatorConfig() sphero.LocatorConfig { + return sphero.LocatorConfig{ + Flags: 0x01, + X: 0x00, + Y: 0x00, + YawTare: 0x00, + } +} diff --git a/platforms/sphero/sphero_driver_test.go b/drivers/serial/sphero_driver_test.go similarity index 66% rename from platforms/sphero/sphero_driver_test.go rename to drivers/serial/sphero_driver_test.go index 4004d083f..417f474d0 100644 --- a/platforms/sphero/sphero_driver_test.go +++ b/drivers/serial/sphero_driver_test.go @@ -1,5 +1,5 @@ //nolint:forcetypeassert // ok here -package sphero +package serial import ( "bytes" @@ -11,24 +11,23 @@ import ( "github.com/stretchr/testify/require" "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/common/sphero" ) var _ gobot.Driver = (*SpheroDriver)(nil) func initTestSpheroDriver() *SpheroDriver { - a, _ := initTestSpheroAdaptor() - _ = a.Connect() + a := newSerialTestAdaptor() return NewSpheroDriver(a) } -func TestSpheroDriverName(t *testing.T) { +func TestNewSpheroDriver(t *testing.T) { d := initTestSpheroDriver() assert.True(t, strings.HasPrefix(d.Name(), "Sphero")) - d.SetName("NewName") - assert.Equal(t, "NewName", d.Name()) + assert.NotNil(t, d.Eventer) } -func TestSpheroDriver(t *testing.T) { +func TestSpheroCommands(t *testing.T) { d := initTestSpheroDriver() var ret interface{} @@ -80,30 +79,28 @@ func TestSpheroDriver(t *testing.T) { ret = d.Command("ReadLocator")(nil) assert.Equal(t, []int16{}, ret) - - assert.True(t, strings.HasPrefix(d.Name(), "Sphero")) - assert.True(t, strings.HasPrefix(d.Connection().Name(), "Sphero")) } -func TestSpheroDriverStart(t *testing.T) { +func TestSpheroStart(t *testing.T) { d := initTestSpheroDriver() require.NoError(t, d.Start()) } -func TestSpheroDriverHalt(t *testing.T) { - d := initTestSpheroDriver() - d.adaptor().connected = true +func TestSpheroHalt(t *testing.T) { + a := newSerialTestAdaptor() + a.isConnected = true + d := NewSpheroDriver(a) require.NoError(t, d.Halt()) } -func TestSpheroDriverSetDataStreaming(t *testing.T) { +func TestSpheroSetDataStreaming(t *testing.T) { d := initTestSpheroDriver() - d.SetDataStreaming(DefaultDataStreamingConfig()) + d.SetDataStreaming(sphero.DefaultDataStreamingConfig()) data := <-d.packetChannel buf := new(bytes.Buffer) - _ = binary.Write(buf, binary.BigEndian, DefaultDataStreamingConfig()) + _ = binary.Write(buf, binary.BigEndian, sphero.DefaultDataStreamingConfig()) assert.Equal(t, buf.Bytes(), data.body) @@ -119,20 +116,20 @@ func TestSpheroDriverSetDataStreaming(t *testing.T) { assert.Nil(t, ret) data = <-d.packetChannel - dconfig := DataStreamingConfig{N: 100, M: 200, Mask: 300, Pcnt: 255, Mask2: 400} + dconfig := sphero.DataStreamingConfig{N: 100, M: 200, Mask: 300, Pcnt: 255, Mask2: 400} buf = new(bytes.Buffer) _ = binary.Write(buf, binary.BigEndian, dconfig) assert.Equal(t, buf.Bytes(), data.body) } -func TestConfigureLocator(t *testing.T) { +func TestSpheroConfigureLocator(t *testing.T) { d := initTestSpheroDriver() - d.ConfigureLocator(DefaultLocatorConfig()) + d.ConfigureLocator(spheroDefaultLocatorConfig()) data := <-d.packetChannel buf := new(bytes.Buffer) - _ = binary.Write(buf, binary.BigEndian, DefaultLocatorConfig()) + _ = binary.Write(buf, binary.BigEndian, spheroDefaultLocatorConfig()) assert.Equal(t, buf.Bytes(), data.body) @@ -147,26 +144,9 @@ func TestConfigureLocator(t *testing.T) { assert.Nil(t, ret) data = <-d.packetChannel - lconfig := LocatorConfig{Flags: 1, X: 100, Y: 100, YawTare: 0} + lconfig := sphero.LocatorConfig{Flags: 1, X: 100, Y: 100, YawTare: 0} buf = new(bytes.Buffer) _ = binary.Write(buf, binary.BigEndian, lconfig) assert.Equal(t, buf.Bytes(), data.body) } - -func TestCalculateChecksum(t *testing.T) { - tests := []struct { - data []byte - checksum byte - }{ - {[]byte{0x00}, 0xff}, - {[]byte{0xf0, 0x0f}, 0x00}, - } - - for _, tt := range tests { - actual := calculateChecksum(tt.data) - if actual != tt.checksum { - t.Errorf("Expected %x, got %x for data %x.", tt.checksum, actual, tt.data) - } - } -} diff --git a/examples/ble_battery.go b/examples/ble_battery.go index ee8fb04f0..4656954fa 100644 --- a/examples/ble_battery.go +++ b/examples/ble_battery.go @@ -21,11 +21,12 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" + "gobot.io/x/gobot/v2/drivers/ble" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) battery := ble.NewBatteryDriver(bleAdaptor) work := func() { diff --git a/examples/bb8-collision.go b/examples/ble_bb8-collision.go similarity index 78% rename from examples/bb8-collision.go rename to examples/ble_bb8-collision.go index 0b4df325d..6d00daaea 100644 --- a/examples/bb8-collision.go +++ b/examples/ble_bb8-collision.go @@ -20,13 +20,13 @@ import ( "os" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/sphero/bb8" + "gobot.io/x/gobot/v2/drivers/ble/sphero" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) - bb := bb8.NewDriver(bleAdaptor) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) + bb := sphero.NewBB8Driver(bleAdaptor) work := func() { bb.On("collision", func(data interface{}) { diff --git a/examples/bb8.go b/examples/ble_bb8.go similarity index 77% rename from examples/bb8.go rename to examples/ble_bb8.go index 8fc5dfc81..24d082d37 100644 --- a/examples/bb8.go +++ b/examples/ble_bb8.go @@ -20,13 +20,13 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/sphero/bb8" + "gobot.io/x/gobot/v2/drivers/ble/sphero" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) - bb8 := bb8.NewDriver(bleAdaptor) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) + bb8 := sphero.NewBB8Driver(bleAdaptor) work := func() { gobot.Every(1*time.Second, func() { diff --git a/examples/ble_device_info.go b/examples/ble_device_info.go index 5c612d302..bff64e7e9 100644 --- a/examples/ble_device_info.go +++ b/examples/ble_device_info.go @@ -20,11 +20,12 @@ import ( "os" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" + "gobot.io/x/gobot/v2/drivers/ble" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) info := ble.NewDeviceInformationDriver(bleAdaptor) work := func() { diff --git a/examples/ble_generic_access.go b/examples/ble_generic_access.go index 761315192..416571206 100644 --- a/examples/ble_generic_access.go +++ b/examples/ble_generic_access.go @@ -20,11 +20,12 @@ import ( "os" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" + "gobot.io/x/gobot/v2/drivers/ble" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) access := ble.NewGenericAccessDriver(bleAdaptor) work := func() { diff --git a/examples/ble_multiple_generic.go b/examples/ble_multiple_generic.go index 15dd9f82d..bbce13d9e 100644 --- a/examples/ble_multiple_generic.go +++ b/examples/ble_multiple_generic.go @@ -21,11 +21,12 @@ import ( "gobot.io/x/gobot/v2" "gobot.io/x/gobot/v2/api" - "gobot.io/x/gobot/v2/platforms/ble" + "gobot.io/x/gobot/v2/drivers/ble" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func NewSwarmBot(port string) *gobot.Robot { - bleAdaptor := ble.NewClientAdaptor(port) + bleAdaptor := bleclient.NewAdaptor(port) access := ble.NewGenericAccessDriver(bleAdaptor) work := func() { diff --git a/examples/ble_multiple_info.go b/examples/ble_multiple_info.go index 8f8b3100b..6155138b3 100644 --- a/examples/ble_multiple_info.go +++ b/examples/ble_multiple_info.go @@ -21,11 +21,12 @@ import ( "gobot.io/x/gobot/v2" "gobot.io/x/gobot/v2/api" - "gobot.io/x/gobot/v2/platforms/ble" + "gobot.io/x/gobot/v2/drivers/ble" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func NewSwarmBot(port string) *gobot.Robot { - bleAdaptor := ble.NewClientAdaptor(port) + bleAdaptor := bleclient.NewAdaptor(port) info := ble.NewDeviceInformationDriver(bleAdaptor) work := func() { diff --git a/examples/ollie.go b/examples/ble_ollie.go similarity index 72% rename from examples/ollie.go rename to examples/ble_ollie.go index e98da7b11..a2dfe8e19 100644 --- a/examples/ollie.go +++ b/examples/ble_ollie.go @@ -11,13 +11,13 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/sphero/ollie" + "gobot.io/x/gobot/v2/drivers/ble/sphero" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) - ollie := ollie.NewDriver(bleAdaptor) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) + ollie := sphero.NewOllieDriver(bleAdaptor) work := func() { gobot.Every(1*time.Second, func() { diff --git a/examples/ollie_boost.go b/examples/ble_ollie_boost.go similarity index 73% rename from examples/ollie_boost.go rename to examples/ble_ollie_boost.go index 389008c48..ba1859f17 100644 --- a/examples/ollie_boost.go +++ b/examples/ble_ollie_boost.go @@ -11,13 +11,13 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/sphero/ollie" + "gobot.io/x/gobot/v2/drivers/ble/sphero" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) - ollieBot := ollie.NewDriver(bleAdaptor) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) + ollieBot := sphero.NewOllieDriver(bleAdaptor) work := func() { head := 90 diff --git a/examples/ollie_crazy.go b/examples/ble_ollie_crazy.go similarity index 60% rename from examples/ollie_crazy.go rename to examples/ble_ollie_crazy.go index d6a7bf9fb..cef96f22d 100644 --- a/examples/ollie_crazy.go +++ b/examples/ble_ollie_crazy.go @@ -11,19 +11,19 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/sphero/ollie" + "gobot.io/x/gobot/v2/drivers/ble/sphero" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) - ollieBot := ollie.NewDriver(bleAdaptor) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) + ollieBot := sphero.NewOllieDriver(bleAdaptor) work := func() { ollieBot.SetRGB(255, 0, 255) gobot.Every(1*time.Second, func() { // Ollie performs 'crazy-ollie' trick - ollieBot.SetRawMotorValues(ollie.Forward, uint8(255), ollie.Forward, uint8(255)) + ollieBot.SetRawMotorValues(sphero.Forward, uint8(255), sphero.Forward, uint8(255)) }) } diff --git a/examples/ollie_mqtt.go b/examples/ble_ollie_mqtt.go similarity index 88% rename from examples/ollie_mqtt.go rename to examples/ble_ollie_mqtt.go index 3e8998f6f..2d9c07977 100644 --- a/examples/ollie_mqtt.go +++ b/examples/ble_ollie_mqtt.go @@ -12,9 +12,9 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" + "gobot.io/x/gobot/v2/drivers/ble/sphero" + "gobot.io/x/gobot/v2/platforms/bleclient" "gobot.io/x/gobot/v2/platforms/mqtt" - "gobot.io/x/gobot/v2/platforms/sphero/ollie" ) const ( @@ -25,8 +25,8 @@ const ( ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) - ollie := ollie.NewDriver(bleAdaptor) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) + ollie := sphero.NewOllieDriver(bleAdaptor) mqttAdaptor := mqtt.NewAdaptor("tcp://iot.eclipse.org:1883", "ollie") diff --git a/examples/ollie_multiple.go b/examples/ble_ollie_multiple.go similarity index 83% rename from examples/ollie_multiple.go rename to examples/ble_ollie_multiple.go index 01dadf81a..a80f1d413 100644 --- a/examples/ollie_multiple.go +++ b/examples/ble_ollie_multiple.go @@ -20,13 +20,13 @@ import ( "gobot.io/x/gobot/v2" "gobot.io/x/gobot/v2/api" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/sphero/ollie" + "gobot.io/x/gobot/v2/drivers/ble/sphero" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func NewSwarmBot(port string) *gobot.Robot { - bleAdaptor := ble.NewClientAdaptor(port) - ollieDriver := ollie.NewDriver(bleAdaptor) + bleAdaptor := bleclient.NewAdaptor(port) + ollieDriver := sphero.NewOllieDriver(bleAdaptor) work := func() { gobot.Every(1*time.Second, func() { diff --git a/examples/ollie_roll.go b/examples/ble_ollie_roll.go similarity index 69% rename from examples/ollie_roll.go rename to examples/ble_ollie_roll.go index 3ee7c30a4..bf07d7e36 100644 --- a/examples/ollie_roll.go +++ b/examples/ble_ollie_roll.go @@ -11,13 +11,13 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/sphero/ollie" + "gobot.io/x/gobot/v2/drivers/ble/sphero" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) - ollie := ollie.NewDriver(bleAdaptor) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) + ollie := sphero.NewOllieDriver(bleAdaptor) work := func() { ollie.SetRGB(255, 0, 255) diff --git a/examples/ollie_spin.go b/examples/ble_ollie_spin.go similarity index 60% rename from examples/ollie_spin.go rename to examples/ble_ollie_spin.go index 736e9a4e4..027328f79 100644 --- a/examples/ollie_spin.go +++ b/examples/ble_ollie_spin.go @@ -11,19 +11,19 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/sphero/ollie" + "gobot.io/x/gobot/v2/drivers/ble/sphero" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) - ollieBot := ollie.NewDriver(bleAdaptor) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) + ollieBot := sphero.NewOllieDriver(bleAdaptor) work := func() { ollieBot.SetRGB(255, 0, 255) gobot.Every(1*time.Second, func() { // Ollie performs 360 spin trick - ollieBot.SetRawMotorValues(ollie.Forward, uint8(255), ollie.Reverse, uint8(255)) + ollieBot.SetRawMotorValues(sphero.Forward, uint8(255), sphero.Reverse, uint8(255)) }) } diff --git a/examples/sprkplus.go b/examples/ble_sprkplus.go similarity index 77% rename from examples/sprkplus.go rename to examples/ble_sprkplus.go index 8ad732e89..ad85eb50e 100644 --- a/examples/sprkplus.go +++ b/examples/ble_sprkplus.go @@ -20,13 +20,13 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/sphero/sprkplus" + "gobot.io/x/gobot/v2/drivers/ble/sphero" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) - sprk := sprkplus.NewDriver(bleAdaptor) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) + sprk := sphero.NewSPRKPlusDriver(bleAdaptor) work := func() { gobot.Every(1*time.Second, func() { diff --git a/examples/sprkplus_collision.go b/examples/ble_sprkplus_collision.go similarity index 77% rename from examples/sprkplus_collision.go rename to examples/ble_sprkplus_collision.go index 4d58a5f89..c053c2893 100644 --- a/examples/sprkplus_collision.go +++ b/examples/ble_sprkplus_collision.go @@ -20,13 +20,13 @@ import ( "os" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/sphero/sprkplus" + "gobot.io/x/gobot/v2/drivers/ble/sphero" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) - ball := sprkplus.NewDriver(bleAdaptor) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) + ball := sphero.NewSPRKPlusDriver(bleAdaptor) work := func() { ball.On("collision", func(data interface{}) { diff --git a/examples/leap_sphero.go b/examples/leap_sphero.go index 455faec8f..91e6c179d 100644 --- a/examples/leap_sphero.go +++ b/examples/leap_sphero.go @@ -10,16 +10,17 @@ import ( "math" "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/serial" "gobot.io/x/gobot/v2/platforms/leap" - "gobot.io/x/gobot/v2/platforms/sphero" + "gobot.io/x/gobot/v2/platforms/serialport" ) func main() { leapAdaptor := leap.NewAdaptor("127.0.0.1:6437") - spheroAdaptor := sphero.NewAdaptor("/dev/tty.Sphero-YBW-RN-SPP") + spheroAdaptor := serialport.NewAdaptor("/dev/tty.Sphero-YBW-RN-SPP") leapDriver := leap.NewDriver(leapAdaptor) - spheroDriver := sphero.NewSpheroDriver(spheroAdaptor) + spheroDriver := serial.NewSpheroDriver(spheroAdaptor) work := func() { leapDriver.On(leap.MessageEvent, func(data interface{}) { diff --git a/examples/microbit_accelerometer.go b/examples/microbit_accelerometer.go index 0d85c72f8..55fbc5350 100644 --- a/examples/microbit_accelerometer.go +++ b/examples/microbit_accelerometer.go @@ -31,16 +31,16 @@ import ( "os" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/microbit" + "gobot.io/x/gobot/v2/drivers/ble/microbit" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) ubit := microbit.NewAccelerometerDriver(bleAdaptor) work := func() { - ubit.On(microbit.Accelerometer, func(data interface{}) { + ubit.On(microbit.AccelerometerEvent, func(data interface{}) { fmt.Println("Accelerometer", data) }) } diff --git a/examples/microbit_blink.go b/examples/microbit_blink.go index d453254ca..c384c68dc 100644 --- a/examples/microbit_blink.go +++ b/examples/microbit_blink.go @@ -34,13 +34,13 @@ import ( "time" "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/ble/microbit" "gobot.io/x/gobot/v2/drivers/gpio" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/microbit" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) ubit := microbit.NewIOPinDriver(bleAdaptor) led := gpio.NewLedDriver(ubit, "0") diff --git a/examples/microbit_buttons.go b/examples/microbit_buttons.go index afd77abd0..54123a456 100644 --- a/examples/microbit_buttons.go +++ b/examples/microbit_buttons.go @@ -29,20 +29,20 @@ import ( "os" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/microbit" + "gobot.io/x/gobot/v2/drivers/ble/microbit" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) ubit := microbit.NewButtonDriver(bleAdaptor) work := func() { - ubit.On(microbit.ButtonA, func(data interface{}) { + ubit.On(microbit.ButtonAEvent, func(data interface{}) { fmt.Println("button A", data) }) - ubit.On(microbit.ButtonB, func(data interface{}) { + ubit.On(microbit.ButtonBEvent, func(data interface{}) { fmt.Println("button B", data) }) } diff --git a/examples/microbit_buttons_leds.go b/examples/microbit_buttons_leds.go index 77cb3f0f2..222ee9d11 100644 --- a/examples/microbit_buttons_leds.go +++ b/examples/microbit_buttons_leds.go @@ -30,17 +30,17 @@ import ( "os" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/microbit" + "gobot.io/x/gobot/v2/drivers/ble/microbit" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) buttons := microbit.NewButtonDriver(bleAdaptor) leds := microbit.NewLEDDriver(bleAdaptor) work := func() { - buttons.On(microbit.ButtonA, func(data interface{}) { + buttons.On(microbit.ButtonAEvent, func(data interface{}) { if data.([]byte)[0] == 1 { leds.UpLeftArrow() return @@ -49,7 +49,7 @@ func main() { leds.Blank() }) - buttons.On(microbit.ButtonB, func(data interface{}) { + buttons.On(microbit.ButtonBEvent, func(data interface{}) { if data.([]byte)[0] == 1 { leds.UpRightArrow() return diff --git a/examples/microbit_io_button.go b/examples/microbit_io_button.go index c9978138f..c1e80be36 100644 --- a/examples/microbit_io_button.go +++ b/examples/microbit_io_button.go @@ -34,13 +34,13 @@ import ( "os" "gobot.io/x/gobot/v2" + "gobot.io/x/gobot/v2/drivers/ble/microbit" "gobot.io/x/gobot/v2/drivers/gpio" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/microbit" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) ubit := microbit.NewIOPinDriver(bleAdaptor) button := gpio.NewButtonDriver(ubit, "0") diff --git a/examples/microbit_led.go b/examples/microbit_led.go index a034f35b1..99586dd27 100644 --- a/examples/microbit_led.go +++ b/examples/microbit_led.go @@ -31,12 +31,12 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/microbit" + "gobot.io/x/gobot/v2/drivers/ble/microbit" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) ubit := microbit.NewLEDDriver(bleAdaptor) work := func() { diff --git a/examples/microbit_magnetometer.go b/examples/microbit_magnetometer.go index ffae1d67f..8e78bbbff 100644 --- a/examples/microbit_magnetometer.go +++ b/examples/microbit_magnetometer.go @@ -31,16 +31,16 @@ import ( "os" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/microbit" + "gobot.io/x/gobot/v2/drivers/ble/microbit" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) ubit := microbit.NewMagnetometerDriver(bleAdaptor) work := func() { - ubit.On(microbit.Magnetometer, func(data interface{}) { + ubit.On(microbit.MagnetometerEvent, func(data interface{}) { fmt.Println("Magnetometer", data) }) } diff --git a/examples/microbit_temperature.go b/examples/microbit_temperature.go index 821aad37b..a27109768 100644 --- a/examples/microbit_temperature.go +++ b/examples/microbit_temperature.go @@ -31,16 +31,16 @@ import ( "os" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/microbit" + "gobot.io/x/gobot/v2/drivers/ble/microbit" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) ubit := microbit.NewTemperatureDriver(bleAdaptor) work := func() { - ubit.On(microbit.Temperature, func(data interface{}) { + ubit.On(microbit.TemperatureEvent, func(data interface{}) { fmt.Println("Temperature", data) }) } diff --git a/examples/minidrone.go b/examples/minidrone.go index a7a0b93b6..17519d84b 100644 --- a/examples/minidrone.go +++ b/examples/minidrone.go @@ -20,13 +20,13 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/parrot/minidrone" + "gobot.io/x/gobot/v2/drivers/ble/parrot" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) - drone := minidrone.NewDriver(bleAdaptor) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) + drone := parrot.NewMinidroneDriver(bleAdaptor) work := func() { drone.TakeOff() diff --git a/examples/minidrone_events.go b/examples/minidrone_events.go index 6005d29e7..b95eb10a6 100644 --- a/examples/minidrone_events.go +++ b/examples/minidrone_events.go @@ -21,39 +21,39 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/parrot/minidrone" + "gobot.io/x/gobot/v2/drivers/ble/parrot" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) - drone := minidrone.NewDriver(bleAdaptor) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) + drone := parrot.NewMinidroneDriver(bleAdaptor) work := func() { - drone.On(minidrone.Battery, func(data interface{}) { + drone.On(parrot.BatteryEvent, func(data interface{}) { fmt.Printf("battery: %d\n", data) }) - drone.On(minidrone.FlightStatus, func(data interface{}) { + drone.On(parrot.FlightStatusEvent, func(data interface{}) { fmt.Printf("flight status: %d\n", data) }) - drone.On(minidrone.Takeoff, func(data interface{}) { + drone.On(parrot.TakeoffEvent, func(data interface{}) { fmt.Println("taking off...") }) - drone.On(minidrone.Hovering, func(data interface{}) { + drone.On(parrot.HoveringEvent, func(data interface{}) { fmt.Println("hovering!") gobot.After(5*time.Second, func() { drone.Land() }) }) - drone.On(minidrone.Landing, func(data interface{}) { + drone.On(parrot.LandingEvent, func(data interface{}) { fmt.Println("landing...") }) - drone.On(minidrone.Landed, func(data interface{}) { + drone.On(parrot.LandedEvent, func(data interface{}) { fmt.Println("landed.") }) diff --git a/examples/minidrone_mambo_ps3.go b/examples/minidrone_mambo_ps3.go index b7ccd9b3a..b1b443a72 100644 --- a/examples/minidrone_mambo_ps3.go +++ b/examples/minidrone_mambo_ps3.go @@ -28,9 +28,9 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" + "gobot.io/x/gobot/v2/drivers/ble/parrot" + "gobot.io/x/gobot/v2/platforms/bleclient" "gobot.io/x/gobot/v2/platforms/joystick" - "gobot.io/x/gobot/v2/platforms/parrot/minidrone" ) type pair struct { @@ -48,8 +48,8 @@ func main() { "./platforms/joystick/configs/dualshock3.json", ) - droneAdaptor := ble.NewClientAdaptor(os.Args[1]) - drone := minidrone.NewDriver(droneAdaptor) + droneAdaptor := bleclient.NewAdaptor(os.Args[1]) + drone := parrot.NewMinidroneDriver(droneAdaptor) work := func() { leftX.Store(float64(0.0)) @@ -61,20 +61,20 @@ func main() { stick.On(joystick.CirclePress, func(data interface{}) { if clawOpen { - drone.ClawControl(0, minidrone.ClawClosed) + drone.ClawControl(0, parrot.ClawClosed) clawOpen = false } else { - drone.ClawControl(0, minidrone.ClawOpen) + drone.ClawControl(0, parrot.ClawOpen) clawOpen = true } }) stick.On(joystick.R2Press, func(data interface{}) { if clawOpen { - drone.ClawControl(0, minidrone.ClawClosed) + drone.ClawControl(0, parrot.ClawClosed) clawOpen = false } else { - drone.ClawControl(0, minidrone.ClawOpen) + drone.ClawControl(0, parrot.ClawOpen) clawOpen = true } }) @@ -113,18 +113,18 @@ func main() { switch { case rightStick.y < -10: - drone.Forward(minidrone.ValidatePitch(rightStick.y, offset)) + drone.Forward(parrot.ValidatePitch(rightStick.y, offset)) case rightStick.y > 10: - drone.Backward(minidrone.ValidatePitch(rightStick.y, offset)) + drone.Backward(parrot.ValidatePitch(rightStick.y, offset)) default: drone.Forward(0) } switch { case rightStick.x > 10: - drone.Right(minidrone.ValidatePitch(rightStick.x, offset)) + drone.Right(parrot.ValidatePitch(rightStick.x, offset)) case rightStick.x < -10: - drone.Left(minidrone.ValidatePitch(rightStick.x, offset)) + drone.Left(parrot.ValidatePitch(rightStick.x, offset)) default: drone.Right(0) } @@ -134,18 +134,18 @@ func main() { leftStick := getLeftStick() switch { case leftStick.y < -10: - drone.Up(minidrone.ValidatePitch(leftStick.y, offset)) + drone.Up(parrot.ValidatePitch(leftStick.y, offset)) case leftStick.y > 10: - drone.Down(minidrone.ValidatePitch(leftStick.y, offset)) + drone.Down(parrot.ValidatePitch(leftStick.y, offset)) default: drone.Up(0) } switch { case leftStick.x > 20: - drone.Clockwise(minidrone.ValidatePitch(leftStick.x, offset)) + drone.Clockwise(parrot.ValidatePitch(leftStick.x, offset)) case leftStick.x < -20: - drone.CounterClockwise(minidrone.ValidatePitch(leftStick.x, offset)) + drone.CounterClockwise(parrot.ValidatePitch(leftStick.x, offset)) default: drone.Clockwise(0) } diff --git a/examples/minidrone_ps3.go b/examples/minidrone_ps3.go index 7afd96053..a316a3d52 100644 --- a/examples/minidrone_ps3.go +++ b/examples/minidrone_ps3.go @@ -28,9 +28,9 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" + "gobot.io/x/gobot/v2/drivers/ble/parrot" + "gobot.io/x/gobot/v2/platforms/bleclient" "gobot.io/x/gobot/v2/platforms/joystick" - "gobot.io/x/gobot/v2/platforms/parrot/minidrone" ) type pair struct { @@ -46,8 +46,8 @@ func main() { joystickAdaptor := joystick.NewAdaptor("0") stick := joystick.NewDriver(joystickAdaptor, "dualshock3") - droneAdaptor := ble.NewClientAdaptor(os.Args[1]) - drone := minidrone.NewDriver(droneAdaptor) + droneAdaptor := bleclient.NewAdaptor(os.Args[1]) + drone := parrot.NewMinidroneDriver(droneAdaptor) work := func() { leftX.Store(float64(0.0)) @@ -104,18 +104,18 @@ func main() { switch { case rightStick.y < -10: - drone.Forward(minidrone.ValidatePitch(rightStick.y, offset)) + drone.Forward(parrot.ValidatePitch(rightStick.y, offset)) case rightStick.y > 10: - drone.Backward(minidrone.ValidatePitch(rightStick.y, offset)) + drone.Backward(parrot.ValidatePitch(rightStick.y, offset)) default: drone.Forward(0) } switch { case rightStick.x > 10: - drone.Right(minidrone.ValidatePitch(rightStick.x, offset)) + drone.Right(parrot.ValidatePitch(rightStick.x, offset)) case rightStick.x < -10: - drone.Left(minidrone.ValidatePitch(rightStick.x, offset)) + drone.Left(parrot.ValidatePitch(rightStick.x, offset)) default: drone.Right(0) } @@ -125,18 +125,18 @@ func main() { leftStick := getLeftStick() switch { case leftStick.y < -10: - drone.Up(minidrone.ValidatePitch(leftStick.y, offset)) + drone.Up(parrot.ValidatePitch(leftStick.y, offset)) case leftStick.y > 10: - drone.Down(minidrone.ValidatePitch(leftStick.y, offset)) + drone.Down(parrot.ValidatePitch(leftStick.y, offset)) default: drone.Up(0) } switch { case leftStick.x > 20: - drone.Clockwise(minidrone.ValidatePitch(leftStick.x, offset)) + drone.Clockwise(parrot.ValidatePitch(leftStick.x, offset)) case leftStick.x < -20: - drone.CounterClockwise(minidrone.ValidatePitch(leftStick.x, offset)) + drone.CounterClockwise(parrot.ValidatePitch(leftStick.x, offset)) default: drone.Clockwise(0) } diff --git a/examples/sphero.go b/examples/serial_sphero.go similarity index 66% rename from examples/sphero.go rename to examples/serial_sphero.go index f876f4a10..a6d942811 100644 --- a/examples/sphero.go +++ b/examples/serial_sphero.go @@ -11,21 +11,23 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/sphero" + "gobot.io/x/gobot/v2/drivers/common/sphero" + "gobot.io/x/gobot/v2/drivers/serial" + "gobot.io/x/gobot/v2/platforms/serialport" ) func main() { - adaptor := sphero.NewAdaptor("/dev/rfcomm0") - spheroDriver := sphero.NewSpheroDriver(adaptor) + adaptor := serialport.NewAdaptor("/dev/rfcomm0") + spheroDriver := serial.NewSpheroDriver(adaptor) work := func() { spheroDriver.SetDataStreaming(sphero.DefaultDataStreamingConfig()) - spheroDriver.On(sphero.Collision, func(data interface{}) { + spheroDriver.On(sphero.CollisionEvent, func(data interface{}) { fmt.Printf("Collision! %+v\n", data) }) - spheroDriver.On(sphero.SensorData, func(data interface{}) { + spheroDriver.On(sphero.SensorDataEvent, func(data interface{}) { fmt.Printf("Streaming Data! %+v\n", data) }) diff --git a/examples/sphero_api.go b/examples/serial_sphero_api.go similarity index 79% rename from examples/sphero_api.go rename to examples/serial_sphero_api.go index 0c38abcef..33607ae13 100644 --- a/examples/sphero_api.go +++ b/examples/serial_sphero_api.go @@ -9,7 +9,8 @@ package main import ( "gobot.io/x/gobot/v2" "gobot.io/x/gobot/v2/api" - "gobot.io/x/gobot/v2/platforms/sphero" + "gobot.io/x/gobot/v2/drivers/serial" + "gobot.io/x/gobot/v2/platforms/serialport" ) func main() { @@ -21,8 +22,8 @@ func main() { } for name, port := range spheros { - spheroAdaptor := sphero.NewAdaptor(port) - spheroDriver := sphero.NewSpheroDriver(spheroAdaptor) + spheroAdaptor := serialport.NewAdaptor(port) + spheroDriver := serial.NewSpheroDriver(spheroAdaptor) work := func() { spheroDriver.SetRGB(uint8(255), uint8(0), uint8(0)) diff --git a/examples/sphero_calibration.go b/examples/serial_sphero_calibration.go similarity index 85% rename from examples/sphero_calibration.go rename to examples/serial_sphero_calibration.go index d48d0542b..b0f2ae353 100644 --- a/examples/sphero_calibration.go +++ b/examples/serial_sphero_calibration.go @@ -9,8 +9,9 @@ package main import ( "gobot.io/x/gobot/v2" "gobot.io/x/gobot/v2/api" + "gobot.io/x/gobot/v2/drivers/serial" "gobot.io/x/gobot/v2/platforms/keyboard" - "gobot.io/x/gobot/v2/platforms/sphero" + "gobot.io/x/gobot/v2/platforms/serialport" ) func main() { @@ -18,8 +19,8 @@ func main() { a := api.NewAPI(master) a.Start() - ballConn := sphero.NewAdaptor("/dev/rfcomm0") - ball := sphero.NewSpheroDriver(ballConn) + ballConn := serialport.NewAdaptor("/dev/rfcomm0") + ball := serial.NewSpheroDriver(ballConn) keys := keyboard.NewDriver() diff --git a/examples/sphero_conways.go b/examples/serial_sphero_conways.go similarity index 85% rename from examples/sphero_conways.go rename to examples/serial_sphero_conways.go index 23d97fc68..5113ab5a0 100644 --- a/examples/sphero_conways.go +++ b/examples/serial_sphero_conways.go @@ -11,14 +11,16 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/sphero" + "gobot.io/x/gobot/v2/drivers/common/sphero" + "gobot.io/x/gobot/v2/drivers/serial" + "gobot.io/x/gobot/v2/platforms/serialport" ) type conway struct { alive bool age int contacts int - cell *sphero.SpheroDriver + cell *serial.SpheroDriver } func main() { @@ -31,9 +33,9 @@ func main() { } for _, port := range spheros { - spheroAdaptor := sphero.NewAdaptor(port) + spheroAdaptor := serialport.NewAdaptor(port) - cell := sphero.NewSpheroDriver(spheroAdaptor) + cell := serial.NewSpheroDriver(spheroAdaptor) cell.SetName("Sphero" + port) work := func() { @@ -42,7 +44,7 @@ func main() { conway.birth() - cell.On(sphero.Collision, func(data interface{}) { + cell.On(sphero.CollisionEvent, func(data interface{}) { conway.contact() }) diff --git a/examples/sphero_dpad.go b/examples/serial_sphero_dpad.go similarity index 81% rename from examples/sphero_dpad.go rename to examples/serial_sphero_dpad.go index f41c16c63..45d34293c 100644 --- a/examples/sphero_dpad.go +++ b/examples/serial_sphero_dpad.go @@ -11,7 +11,8 @@ import ( "gobot.io/x/gobot/v2" "gobot.io/x/gobot/v2/api" - "gobot.io/x/gobot/v2/platforms/sphero" + "gobot.io/x/gobot/v2/drivers/serial" + "gobot.io/x/gobot/v2/platforms/serialport" ) func main() { @@ -19,8 +20,8 @@ func main() { a := api.NewAPI(master) a.Start() - conn := sphero.NewAdaptor("/dev/rfcomm0") - ball := sphero.NewSpheroDriver(conn) + conn := serialport.NewAdaptor("/dev/rfcomm0") + ball := serial.NewSpheroDriver(conn) robot := gobot.NewRobot("sphero-dpad", []gobot.Connection{conn}, diff --git a/examples/sphero_master.go b/examples/serial_sphero_master.go similarity index 73% rename from examples/sphero_master.go rename to examples/serial_sphero_master.go index 19fe9b439..0c7e16f0c 100644 --- a/examples/sphero_master.go +++ b/examples/serial_sphero_master.go @@ -10,7 +10,8 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/sphero" + "gobot.io/x/gobot/v2/drivers/serial" + "gobot.io/x/gobot/v2/platforms/serialport" ) func main() { @@ -21,8 +22,8 @@ func main() { } for name, port := range spheros { - spheroAdaptor := sphero.NewAdaptor(port) - spheroDriver := sphero.NewSpheroDriver(spheroAdaptor) + spheroAdaptor := serialport.NewAdaptor(port) + spheroDriver := serial.NewSpheroDriver(spheroAdaptor) work := func() { spheroDriver.SetRGB(uint8(255), uint8(0), uint8(0)) @@ -40,7 +41,7 @@ func main() { robot := gobot.NewRobot("", func() { gobot.Every(1*time.Second, func() { - sphero := master.Robot("Sphero-BPO").Device("sphero").(*sphero.SpheroDriver) + sphero := master.Robot("Sphero-BPO").Device("sphero").(*serial.SpheroDriver) sphero.SetRGB(uint8(gobot.Rand(255)), uint8(gobot.Rand(255)), uint8(gobot.Rand(255)), diff --git a/examples/sphero_multiple.go b/examples/serial_sphero_multiple.go similarity index 76% rename from examples/sphero_multiple.go rename to examples/serial_sphero_multiple.go index 5748d7718..c728ce9ff 100644 --- a/examples/sphero_multiple.go +++ b/examples/serial_sphero_multiple.go @@ -12,18 +12,20 @@ import ( "gobot.io/x/gobot/v2" "gobot.io/x/gobot/v2/api" - "gobot.io/x/gobot/v2/platforms/sphero" + "gobot.io/x/gobot/v2/drivers/common/sphero" + "gobot.io/x/gobot/v2/drivers/serial" + "gobot.io/x/gobot/v2/platforms/serialport" ) func NewSwarmBot(port string) *gobot.Robot { - spheroAdaptor := sphero.NewAdaptor(port) - spheroDriver := sphero.NewSpheroDriver(spheroAdaptor) + spheroAdaptor := serialport.NewAdaptor(port) + spheroDriver := serial.NewSpheroDriver(spheroAdaptor) spheroDriver.SetName("Sphero" + port) work := func() { spheroDriver.Stop() - spheroDriver.On(sphero.Collision, func(data interface{}) { + spheroDriver.On(sphero.CollisionEvent, func(data interface{}) { fmt.Println("Collision Detected!") }) diff --git a/platforms/adaptors/pwmpinsadaptoroptions.go b/platforms/adaptors/pwmpinsadaptoroptions.go index 168840470..c9ad49e69 100644 --- a/platforms/adaptors/pwmpinsadaptoroptions.go +++ b/platforms/adaptors/pwmpinsadaptoroptions.go @@ -2,7 +2,7 @@ package adaptors import "time" -// pwmPinOptionApplier needs to be implemented by each configurable option type +// PwmPinsOptionApplier needs to be implemented by each configurable option type type PwmPinsOptionApplier interface { apply(cfg *pwmPinsConfiguration) } diff --git a/platforms/ble/battery_driver.go b/platforms/ble/battery_driver.go deleted file mode 100644 index 4b3e59628..000000000 --- a/platforms/ble/battery_driver.go +++ /dev/null @@ -1,60 +0,0 @@ -package ble - -import ( - "bytes" - "log" - - "gobot.io/x/gobot/v2" -) - -// BatteryDriver represents the Battery Service for a BLE Peripheral -type BatteryDriver struct { - name string - connection gobot.Connection - gobot.Eventer -} - -// NewBatteryDriver creates a BatteryDriver -func NewBatteryDriver(a BLEConnector) *BatteryDriver { - n := &BatteryDriver{ - name: gobot.DefaultName("Battery"), - connection: a, - Eventer: gobot.NewEventer(), - } - - return n -} - -// Connection returns the Driver's Connection to the associated Adaptor -func (b *BatteryDriver) Connection() gobot.Connection { return b.connection } - -// Name returns the Driver name -func (b *BatteryDriver) Name() string { return b.name } - -// SetName sets the Driver name -func (b *BatteryDriver) SetName(n string) { b.name = n } - -// adaptor returns BLE adaptor -func (b *BatteryDriver) adaptor() BLEConnector { - //nolint:forcetypeassert // ok here - return b.Connection().(BLEConnector) -} - -// Start tells driver to get ready to do work -func (b *BatteryDriver) Start() error { return nil } - -// Halt stops battery driver (void) -func (b *BatteryDriver) Halt() error { return nil } - -// GetBatteryLevel reads and returns the current battery level -func (b *BatteryDriver) GetBatteryLevel() uint8 { - c, err := b.adaptor().ReadCharacteristic("2a19") - if err != nil { - log.Println(err) - return 0 - } - buf := bytes.NewBuffer(c) - val, _ := buf.ReadByte() - level := val - return level -} diff --git a/platforms/ble/battery_driver_test.go b/platforms/ble/battery_driver_test.go deleted file mode 100644 index 84e671f70..000000000 --- a/platforms/ble/battery_driver_test.go +++ /dev/null @@ -1,41 +0,0 @@ -package ble - -import ( - "strings" - "testing" - - "github.com/stretchr/testify/assert" - "github.com/stretchr/testify/require" - - "gobot.io/x/gobot/v2" -) - -var _ gobot.Driver = (*BatteryDriver)(nil) - -func initTestBatteryDriver() *BatteryDriver { - d := NewBatteryDriver(NewBleTestAdaptor()) - return d -} - -func TestBatteryDriver(t *testing.T) { - d := initTestBatteryDriver() - assert.True(t, strings.HasPrefix(d.Name(), "Battery")) - d.SetName("NewName") - assert.Equal(t, "NewName", d.Name()) -} - -func TestBatteryDriverStartAndHalt(t *testing.T) { - d := initTestBatteryDriver() - require.NoError(t, d.Start()) - require.NoError(t, d.Halt()) -} - -func TestBatteryDriverRead(t *testing.T) { - a := NewBleTestAdaptor() - d := NewBatteryDriver(a) - a.TestReadCharacteristic(func(cUUID string) ([]byte, error) { - return []byte{20}, nil - }) - - assert.Equal(t, uint8(20), d.GetBatteryLevel()) -} diff --git a/platforms/ble/device_information_driver.go b/platforms/ble/device_information_driver.go deleted file mode 100644 index 79710a9df..000000000 --- a/platforms/ble/device_information_driver.go +++ /dev/null @@ -1,107 +0,0 @@ -package ble - -import ( - "bytes" - "log" - - "gobot.io/x/gobot/v2" -) - -// DeviceInformationDriver represents the Device Information Service for a BLE Peripheral -type DeviceInformationDriver struct { - name string - connection gobot.Connection - gobot.Eventer -} - -// NewDeviceInformationDriver creates a DeviceInformationDriver -func NewDeviceInformationDriver(a BLEConnector) *DeviceInformationDriver { - n := &DeviceInformationDriver{ - name: gobot.DefaultName("DeviceInformation"), - connection: a, - Eventer: gobot.NewEventer(), - } - - return n -} - -// Connection returns the Driver's Connection to the associated Adaptor -func (b *DeviceInformationDriver) Connection() gobot.Connection { return b.connection } - -// Name returns the Driver name -func (b *DeviceInformationDriver) Name() string { return b.name } - -// SetName sets the Driver name -func (b *DeviceInformationDriver) SetName(n string) { b.name = n } - -// adaptor returns BLE adaptor for this device -func (b *DeviceInformationDriver) adaptor() BLEConnector { - //nolint:forcetypeassert // ok here - return b.Connection().(BLEConnector) -} - -// Start tells driver to get ready to do work -func (b *DeviceInformationDriver) Start() error { return nil } - -// Halt stops driver (void) -func (b *DeviceInformationDriver) Halt() error { return nil } - -// GetModelNumber returns the model number for the BLE Peripheral -func (b *DeviceInformationDriver) GetModelNumber() string { - c, err := b.adaptor().ReadCharacteristic("2a24") - if err != nil { - log.Println(err) - return "" - } - buf := bytes.NewBuffer(c) - model := buf.String() - return model -} - -// GetFirmwareRevision returns the firmware revision for the BLE Peripheral -func (b *DeviceInformationDriver) GetFirmwareRevision() string { - c, err := b.adaptor().ReadCharacteristic("2a26") - if err != nil { - log.Println(err) - return "" - } - buf := bytes.NewBuffer(c) - val := buf.String() - return val -} - -// GetHardwareRevision returns the hardware revision for the BLE Peripheral -func (b *DeviceInformationDriver) GetHardwareRevision() string { - c, err := b.adaptor().ReadCharacteristic("2a27") - if err != nil { - log.Println(err) - return "" - } - buf := bytes.NewBuffer(c) - val := buf.String() - return val -} - -// GetManufacturerName returns the manufacturer name for the BLE Peripheral -func (b *DeviceInformationDriver) GetManufacturerName() string { - c, err := b.adaptor().ReadCharacteristic("2a29") - if err != nil { - log.Println(err) - return "" - } - buf := bytes.NewBuffer(c) - val := buf.String() - return val -} - -// GetPnPId returns the PnP ID for the BLE Peripheral -func (b *DeviceInformationDriver) GetPnPId() string { - c, err := b.adaptor().ReadCharacteristic("2a50") - if err != nil { - log.Println(err) - return "" - } - buf := bytes.NewBuffer(c) - val := buf.String() - return val -} diff --git a/platforms/ble/device_information_driver_test.go b/platforms/ble/device_information_driver_test.go deleted file mode 100644 index 1835ce8d1..000000000 --- a/platforms/ble/device_information_driver_test.go +++ /dev/null @@ -1,81 +0,0 @@ -package ble - -import ( - "strings" - "testing" - - "github.com/stretchr/testify/assert" - "github.com/stretchr/testify/require" - - "gobot.io/x/gobot/v2" -) - -var _ gobot.Driver = (*DeviceInformationDriver)(nil) - -func initTestDeviceInformationDriver() *DeviceInformationDriver { - d := NewDeviceInformationDriver(NewBleTestAdaptor()) - return d -} - -func TestDeviceInformationDriver(t *testing.T) { - d := initTestDeviceInformationDriver() - assert.True(t, strings.HasPrefix(d.Name(), "DeviceInformation")) - d.SetName("NewName") - assert.Equal(t, "NewName", d.Name()) -} - -func TestDeviceInformationDriverStartAndHalt(t *testing.T) { - d := initTestDeviceInformationDriver() - require.NoError(t, d.Start()) - require.NoError(t, d.Halt()) -} - -func TestDeviceInformationDriverGetModelNumber(t *testing.T) { - a := NewBleTestAdaptor() - d := NewDeviceInformationDriver(a) - a.TestReadCharacteristic(func(cUUID string) ([]byte, error) { - return []byte("TestDevice"), nil - }) - - assert.Equal(t, "TestDevice", d.GetModelNumber()) -} - -func TestDeviceInformationDriverGetFirmwareRevision(t *testing.T) { - a := NewBleTestAdaptor() - d := NewDeviceInformationDriver(a) - a.TestReadCharacteristic(func(cUUID string) ([]byte, error) { - return []byte("TestDevice"), nil - }) - - assert.Equal(t, "TestDevice", d.GetFirmwareRevision()) -} - -func TestDeviceInformationDriverGetHardwareRevision(t *testing.T) { - a := NewBleTestAdaptor() - d := NewDeviceInformationDriver(a) - a.TestReadCharacteristic(func(cUUID string) ([]byte, error) { - return []byte("TestDevice"), nil - }) - - assert.Equal(t, "TestDevice", d.GetHardwareRevision()) -} - -func TestDeviceInformationDriverGetManufacturerName(t *testing.T) { - a := NewBleTestAdaptor() - d := NewDeviceInformationDriver(a) - a.TestReadCharacteristic(func(cUUID string) ([]byte, error) { - return []byte("TestDevice"), nil - }) - - assert.Equal(t, "TestDevice", d.GetManufacturerName()) -} - -func TestDeviceInformationDriverGetPnPId(t *testing.T) { - a := NewBleTestAdaptor() - d := NewDeviceInformationDriver(a) - a.TestReadCharacteristic(func(cUUID string) ([]byte, error) { - return []byte("TestDevice"), nil - }) - - assert.Equal(t, "TestDevice", d.GetPnPId()) -} diff --git a/platforms/ble/doc.go b/platforms/ble/doc.go deleted file mode 100644 index 39b9e8258..000000000 --- a/platforms/ble/doc.go +++ /dev/null @@ -1,13 +0,0 @@ -/* -Package ble provides the Gobot adaptor for Bluetooth LE. - -It also includes drivers for several well-known BLE Services: - -- Battery Service -- Device Information Service -- Generic Access Service - -For more information refer to the README: -https://github.com/hybridgroup/gobot/blob/master/platforms/ble/README.md -*/ -package ble // import "gobot.io/x/gobot/v2/platforms/ble" diff --git a/platforms/ble/generic_access_driver_test.go b/platforms/ble/generic_access_driver_test.go deleted file mode 100644 index c32b3384e..000000000 --- a/platforms/ble/generic_access_driver_test.go +++ /dev/null @@ -1,51 +0,0 @@ -package ble - -import ( - "strings" - "testing" - - "github.com/stretchr/testify/assert" - "github.com/stretchr/testify/require" - - "gobot.io/x/gobot/v2" -) - -var _ gobot.Driver = (*GenericAccessDriver)(nil) - -func initTestGenericAccessDriver() *GenericAccessDriver { - d := NewGenericAccessDriver(NewBleTestAdaptor()) - return d -} - -func TestGenericAccessDriver(t *testing.T) { - d := initTestGenericAccessDriver() - assert.True(t, strings.HasPrefix(d.Name(), "GenericAccess")) - d.SetName("NewName") - assert.Equal(t, "NewName", d.Name()) -} - -func TestGenericAccessDriverStartAndHalt(t *testing.T) { - d := initTestGenericAccessDriver() - require.NoError(t, d.Start()) - require.NoError(t, d.Halt()) -} - -func TestGenericAccessDriverGetDeviceName(t *testing.T) { - a := NewBleTestAdaptor() - d := NewGenericAccessDriver(a) - a.TestReadCharacteristic(func(cUUID string) ([]byte, error) { - return []byte("TestDevice"), nil - }) - - assert.Equal(t, "TestDevice", d.GetDeviceName()) -} - -func TestGenericAccessDriverGetAppearance(t *testing.T) { - a := NewBleTestAdaptor() - d := NewGenericAccessDriver(a) - a.TestReadCharacteristic(func(cUUID string) ([]byte, error) { - return []byte{128, 0}, nil - }) - - assert.Equal(t, "Generic Computer", d.GetAppearance()) -} diff --git a/platforms/ble/helpers_test.go b/platforms/ble/helpers_test.go deleted file mode 100644 index dfcd7e36d..000000000 --- a/platforms/ble/helpers_test.go +++ /dev/null @@ -1,65 +0,0 @@ -package ble - -import "sync" - -var _ BLEConnector = (*bleTestClientAdaptor)(nil) - -type bleTestClientAdaptor struct { - name string - address string - mtx sync.Mutex - withoutResponses bool - - testReadCharacteristic func(string) ([]byte, error) - testWriteCharacteristic func(string, []byte) error -} - -func (t *bleTestClientAdaptor) Connect() error { return nil } -func (t *bleTestClientAdaptor) Reconnect() error { return nil } -func (t *bleTestClientAdaptor) Disconnect() error { return nil } -func (t *bleTestClientAdaptor) Finalize() error { return nil } -func (t *bleTestClientAdaptor) Name() string { return t.name } -func (t *bleTestClientAdaptor) SetName(n string) { t.name = n } -func (t *bleTestClientAdaptor) Address() string { return t.address } -func (t *bleTestClientAdaptor) WithoutResponses(use bool) { t.withoutResponses = use } - -func (t *bleTestClientAdaptor) ReadCharacteristic(cUUID string) ([]byte, error) { - t.mtx.Lock() - defer t.mtx.Unlock() - return t.testReadCharacteristic(cUUID) -} - -func (t *bleTestClientAdaptor) WriteCharacteristic(cUUID string, data []byte) error { - t.mtx.Lock() - defer t.mtx.Unlock() - return t.testWriteCharacteristic(cUUID, data) -} - -func (t *bleTestClientAdaptor) Subscribe(cUUID string, f func([]byte, error)) error { - // TODO: implement this... - return nil -} - -func (t *bleTestClientAdaptor) TestReadCharacteristic(f func(cUUID string) (data []byte, err error)) { - t.mtx.Lock() - defer t.mtx.Unlock() - t.testReadCharacteristic = f -} - -func (t *bleTestClientAdaptor) TestWriteCharacteristic(f func(cUUID string, data []byte) error) { - t.mtx.Lock() - defer t.mtx.Unlock() - t.testWriteCharacteristic = f -} - -func NewBleTestAdaptor() *bleTestClientAdaptor { - return &bleTestClientAdaptor{ - address: "01:02:03:04:05:06", - testReadCharacteristic: func(cUUID string) ([]byte, error) { - return []byte{}, nil - }, - testWriteCharacteristic: func(cUUID string, data []byte) error { - return nil - }, - } -} diff --git a/platforms/ble/serial_port.go b/platforms/ble/serial_port.go deleted file mode 100644 index 4cbfedbe6..000000000 --- a/platforms/ble/serial_port.go +++ /dev/null @@ -1,77 +0,0 @@ -package ble - -import "sync" - -// SerialPort is a implementation of serial over Bluetooth LE -// Inspired by https://github.com/monteslu/ble-serial by @monteslu -type SerialPort struct { - address string - rid string - tid string - client *ClientAdaptor - - // buffer of responseData and mutex to protect it - responseData []byte - responseMutex sync.Mutex -} - -// NewSerialPort returns a new serial over Bluetooth LE connection -func NewSerialPort(address string, rid string, tid string) *SerialPort { - return &SerialPort{address: address, rid: rid, tid: tid} -} - -// Open opens a connection to a BLE serial device -func (p *SerialPort) Open() error { - p.client = NewClientAdaptor(p.address) - - if err := p.client.Connect(); err != nil { - return err - } - - // subscribe to response notifications - return p.client.Subscribe(p.rid, func(data []byte, e error) { - p.responseMutex.Lock() - p.responseData = append(p.responseData, data...) - p.responseMutex.Unlock() - }) -} - -// Read reads bytes from BLE serial port connection -func (p *SerialPort) Read(b []byte) (int, error) { - if len(p.responseData) == 0 { - return 0, nil - } - - p.responseMutex.Lock() - n := len(b) - if len(p.responseData) < n { - n = len(p.responseData) - } - copy(b, p.responseData[:n]) - - if len(p.responseData) > n { - p.responseData = p.responseData[n:] - } else { - p.responseData = nil - } - p.responseMutex.Unlock() - - return n, nil -} - -// Write writes to the BLE serial port connection -func (p *SerialPort) Write(b []byte) (int, error) { - err := p.client.WriteCharacteristic(p.tid, b) - n := len(b) - return n, err -} - -// Close closes the BLE serial port connection -func (p *SerialPort) Close() error { - return p.client.Disconnect() -} - -// Address returns the BLE address -func (p *SerialPort) Address() string { - return p.address -} diff --git a/platforms/ble/serial_port_test.go b/platforms/ble/serial_port_test.go deleted file mode 100644 index 47dc87cdb..000000000 --- a/platforms/ble/serial_port_test.go +++ /dev/null @@ -1,16 +0,0 @@ -package ble - -import ( - "testing" - - "github.com/stretchr/testify/assert" -) - -func initTestBLESerialPort() *SerialPort { - return NewSerialPort("TEST123", "123", "456") -} - -func TestBLESerialPort(t *testing.T) { - d := initTestBLESerialPort() - assert.Equal(t, "TEST123", d.Address()) -} diff --git a/platforms/ble/LICENSE b/platforms/bleclient/LICENSE similarity index 100% rename from platforms/ble/LICENSE rename to platforms/bleclient/LICENSE diff --git a/platforms/ble/README.md b/platforms/bleclient/README.md similarity index 85% rename from platforms/ble/README.md rename to platforms/bleclient/README.md index 5871bd6fe..c243cf7a7 100644 --- a/platforms/ble/README.md +++ b/platforms/bleclient/README.md @@ -2,15 +2,11 @@ The Gobot BLE adaptor makes it easy to interact with Bluetooth LE aka Bluetooth 4.0 using Go. -It is written using the [TinyGo Bluetooh](tinygo.org/x/bluetooth) package. +It is written using the [TinyGo Bluetooth](tinygo.org/x/bluetooth) package. Learn more about Bluetooth LE at -This package also includes drivers for several well-known BLE Services: - -- Battery Service -- Device Information Service -- Generic Access Service +Drivers for several BLE Services can be found in the according [driver folder](https://github.com/hybridgroup/gobot/tree/release/drivers/ble). ## How to Install @@ -65,11 +61,12 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" + "gobot.io/x/gobot/v2/drivers/ble" + "gobot.io/x/gobot/v2/platforms/bleclient" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) battery := ble.NewBatteryDriver(bleAdaptor) work := func() { diff --git a/platforms/ble/ble_client_adaptor.go b/platforms/bleclient/ble_client_adaptor.go similarity index 63% rename from platforms/ble/ble_client_adaptor.go rename to platforms/bleclient/ble_client_adaptor.go index 8273e1c94..f73605930 100644 --- a/platforms/ble/ble_client_adaptor.go +++ b/platforms/bleclient/ble_client_adaptor.go @@ -1,4 +1,4 @@ -package ble +package bleclient import ( "fmt" @@ -16,22 +16,8 @@ var ( bleMutex sync.Mutex ) -// BLEConnector is the interface that a BLE ClientAdaptor must implement -type BLEConnector interface { - gobot.Adaptor - - Reconnect() error - Disconnect() error - Address() string - - ReadCharacteristic(cUUID string) ([]byte, error) - WriteCharacteristic(cUUID string, data []byte) error - Subscribe(cUUID string, f func([]byte, error)) error - WithoutResponses(use bool) -} - -// ClientAdaptor represents a Client Connection to a BLE Peripheral -type ClientAdaptor struct { +// Adaptor represents a client connection to a BLE Peripheral +type Adaptor struct { name string address string AdapterName string @@ -45,9 +31,9 @@ type ClientAdaptor struct { withoutResponses bool } -// NewClientAdaptor returns a new ClientAdaptor given an address -func NewClientAdaptor(address string) *ClientAdaptor { - return &ClientAdaptor{ +// NewAdaptor returns a new Bluetooth LE client adaptor given an address +func NewAdaptor(address string) *Adaptor { + return &Adaptor{ name: gobot.DefaultName("BLEClient"), address: address, AdapterName: "default", @@ -58,41 +44,41 @@ func NewClientAdaptor(address string) *ClientAdaptor { } // Name returns the name for the adaptor -func (b *ClientAdaptor) Name() string { return b.name } +func (a *Adaptor) Name() string { return a.name } // SetName sets the name for the adaptor -func (b *ClientAdaptor) SetName(n string) { b.name = n } +func (a *Adaptor) SetName(n string) { a.name = n } // Address returns the Bluetooth LE address for the adaptor -func (b *ClientAdaptor) Address() string { return b.address } +func (a *Adaptor) Address() string { return a.address } // WithoutResponses sets if the adaptor should expect responses after // writing characteristics for this device -func (b *ClientAdaptor) WithoutResponses(use bool) { b.withoutResponses = use } +func (a *Adaptor) WithoutResponses(use bool) { a.withoutResponses = use } // Connect initiates a connection to the BLE peripheral. Returns true on successful connection. -func (b *ClientAdaptor) Connect() error { +func (a *Adaptor) Connect() error { bleMutex.Lock() defer bleMutex.Unlock() var err error // enable adaptor - b.adpt, err = getBLEAdapter(b.AdapterName) + a.adpt, err = getBLEAdapter(a.AdapterName) if err != nil { - return fmt.Errorf("can't get adapter %s: %w", b.AdapterName, err) + return fmt.Errorf("can't get adapter %s: %w", a.AdapterName, err) } // handle address - b.addr.Set(b.Address()) + a.addr.Set(a.Address()) // scan for the address ch := make(chan bluetooth.ScanResult, 1) - err = b.adpt.Scan(func(adapter *bluetooth.Adapter, result bluetooth.ScanResult) { - if result.Address.String() == b.Address() { - if err := b.adpt.StopScan(); err != nil { + err = a.adpt.Scan(func(adapter *bluetooth.Adapter, result bluetooth.ScanResult) { + if result.Address.String() == a.Address() { + if err := a.adpt.StopScan(); err != nil { panic(err) } - b.SetName(result.LocalName()) + a.SetName(result.LocalName()) ch <- result } }) @@ -103,13 +89,13 @@ func (b *ClientAdaptor) Connect() error { // wait to connect to peripheral device result := <-ch - b.device, err = b.adpt.Connect(result.Address, bluetooth.ConnectionParams{}) + a.device, err = a.adpt.Connect(result.Address, bluetooth.ConnectionParams{}) if err != nil { return err } // get all services/characteristics - srvcs, err := b.device.DiscoverServices(nil) + srvcs, err := a.device.DiscoverServices(nil) if err != nil { return err } @@ -120,48 +106,48 @@ func (b *ClientAdaptor) Connect() error { continue } for _, char := range chars { - b.characteristics[char.UUID().String()] = char + a.characteristics[char.UUID().String()] = char } } - b.connected = true + a.connected = true return nil } // Reconnect attempts to reconnect to the BLE peripheral. If it has an active connection // it will first close that connection and then establish a new connection. // Returns true on Successful reconnection -func (b *ClientAdaptor) Reconnect() error { - if b.connected { - if err := b.Disconnect(); err != nil { +func (a *Adaptor) Reconnect() error { + if a.connected { + if err := a.Disconnect(); err != nil { return err } } - return b.Connect() + return a.Connect() } // Disconnect terminates the connection to the BLE peripheral. Returns true on successful disconnect. -func (b *ClientAdaptor) Disconnect() error { - err := b.device.Disconnect() +func (a *Adaptor) Disconnect() error { + err := a.device.Disconnect() time.Sleep(500 * time.Millisecond) return err } // Finalize finalizes the BLEAdaptor -func (b *ClientAdaptor) Finalize() error { - return b.Disconnect() +func (a *Adaptor) Finalize() error { + return a.Disconnect() } // ReadCharacteristic returns bytes from the BLE device for the // requested characteristic uuid -func (b *ClientAdaptor) ReadCharacteristic(cUUID string) ([]byte, error) { - if !b.connected { +func (a *Adaptor) ReadCharacteristic(cUUID string) ([]byte, error) { + if !a.connected { return nil, fmt.Errorf("Cannot read from BLE device until connected") } cUUID = convertUUID(cUUID) - if char, ok := b.characteristics[cUUID]; ok { + if char, ok := a.characteristics[cUUID]; ok { buf := make([]byte, 255) n, err := char.Read(buf) if err != nil { @@ -175,14 +161,14 @@ func (b *ClientAdaptor) ReadCharacteristic(cUUID string) ([]byte, error) { // WriteCharacteristic writes bytes to the BLE device for the // requested service and characteristic -func (b *ClientAdaptor) WriteCharacteristic(cUUID string, data []byte) error { - if !b.connected { +func (a *Adaptor) WriteCharacteristic(cUUID string, data []byte) error { + if !a.connected { return fmt.Errorf("Cannot write to BLE device until connected") } cUUID = convertUUID(cUUID) - if char, ok := b.characteristics[cUUID]; ok { + if char, ok := a.characteristics[cUUID]; ok { _, err := char.WriteWithoutResponse(data) if err != nil { return err @@ -195,14 +181,14 @@ func (b *ClientAdaptor) WriteCharacteristic(cUUID string, data []byte) error { // Subscribe subscribes to notifications from the BLE device for the // requested service and characteristic -func (b *ClientAdaptor) Subscribe(cUUID string, f func([]byte, error)) error { - if !b.connected { +func (a *Adaptor) Subscribe(cUUID string, f func([]byte, error)) error { + if !a.connected { return fmt.Errorf("Cannot subscribe to BLE device until connected") } cUUID = convertUUID(cUUID) - if char, ok := b.characteristics[cUUID]; ok { + if char, ok := a.characteristics[cUUID]; ok { fn := func(d []byte) { f(d, nil) } diff --git a/platforms/ble/ble_client_adaptor_test.go b/platforms/bleclient/ble_client_adaptor_test.go similarity index 52% rename from platforms/ble/ble_client_adaptor_test.go rename to platforms/bleclient/ble_client_adaptor_test.go index 0862405e8..2364e9aa7 100644 --- a/platforms/ble/ble_client_adaptor_test.go +++ b/platforms/bleclient/ble_client_adaptor_test.go @@ -1,4 +1,4 @@ -package ble +package bleclient import ( "strings" @@ -9,16 +9,19 @@ import ( "gobot.io/x/gobot/v2" ) -var _ gobot.Adaptor = (*ClientAdaptor)(nil) +var ( + _ gobot.Adaptor = (*Adaptor)(nil) + _ gobot.BLEConnector = (*Adaptor)(nil) +) -func TestBLEClientAdaptor(t *testing.T) { - a := NewClientAdaptor("D7:99:5A:26:EC:38") +func TestNewAdaptor(t *testing.T) { + a := NewAdaptor("D7:99:5A:26:EC:38") assert.Equal(t, "D7:99:5A:26:EC:38", a.Address()) assert.True(t, strings.HasPrefix(a.Name(), "BLEClient")) } -func TestBLEClientAdaptorName(t *testing.T) { - a := NewClientAdaptor("D7:99:5A:26:EC:38") +func TestName(t *testing.T) { + a := NewAdaptor("D7:99:5A:26:EC:38") a.SetName("awesome") assert.Equal(t, "awesome", a.Name()) } diff --git a/platforms/bleclient/doc.go b/platforms/bleclient/doc.go new file mode 100644 index 000000000..b6ba3d1c0 --- /dev/null +++ b/platforms/bleclient/doc.go @@ -0,0 +1,7 @@ +/* +Package bleclient provides the Gobot client adaptor for Bluetooth LE. + +For more information refer to the README: +https://github.com/hybridgroup/gobot/blob/master/platforms/bleclient/README.md +*/ +package bleclient // import "gobot.io/x/gobot/v2/platforms/bleclient" diff --git a/platforms/ble/uuid.go b/platforms/bleclient/uuid.go similarity index 96% rename from platforms/ble/uuid.go rename to platforms/bleclient/uuid.go index da0c47f6f..bf120fbe7 100644 --- a/platforms/ble/uuid.go +++ b/platforms/bleclient/uuid.go @@ -1,4 +1,4 @@ -package ble +package bleclient import ( "fmt" diff --git a/platforms/firmata/ble_firmata_adaptor.go b/platforms/firmata/ble_firmata_adaptor.go index 7150b8de5..1c4f1856a 100644 --- a/platforms/firmata/ble_firmata_adaptor.go +++ b/platforms/firmata/ble_firmata_adaptor.go @@ -7,7 +7,8 @@ import ( "io" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" + "gobot.io/x/gobot/v2/drivers/ble" + "gobot.io/x/gobot/v2/platforms/bleclient" ) const ( @@ -40,7 +41,8 @@ func NewBLEAdaptor(args ...interface{}) *BLEAdaptor { a := NewAdaptor(address) a.SetName(gobot.DefaultName("BLEFirmata")) a.PortOpener = func(port string) (io.ReadWriteCloser, error) { - sp := ble.NewSerialPort(address, rid, wid) + a := bleclient.NewAdaptor(address) + sp := ble.NewSerialPortDriver(a, rid, wid) if err := sp.Open(); err != nil { return sp, err } diff --git a/platforms/microbit/README.md b/platforms/microbit/README.md index f4342e64c..b95e086a2 100644 --- a/platforms/microbit/README.md +++ b/platforms/microbit/README.md @@ -28,14 +28,9 @@ however you do not need this source code to install the firmware using the insta ## How to Use -The Gobot platform for the Microbit includes several different drivers, each one corresponding to a different capability: - -- AccelerometerDriver -- ButtonDriver -- IOPinDriver -- LEDDriver -- MagnetometerDriver -- TemperatureDriver +The Gobot package for the Microbit includes several [different drivers](https://github.com/hybridgroup/gobot/blob/release/drivers/ble/README.md). +The platform itself is represented by the generic Bluetooth LE [Client adaptor](https://github.com/hybridgroup/gobot/blob/release/platforms/bleclient/ble_client_adaptor.go), +see examples below. The following example uses the LEDDriver: @@ -47,12 +42,12 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/microbit" + "gobot.io/x/gobot/v2/platforms/bleclient" + "gobot.io/x/gobot/v2/drivers/ble/microbit" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) ubit := microbit.NewLEDDriver(bleAdaptor) work := func() { @@ -89,12 +84,12 @@ import ( "gobot.io/x/gobot/v2" "gobot.io/x/gobot/v2/drivers/gpio" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/microbit" + "gobot.io/x/gobot/v2/platforms/bleclient" + "gobot.io/x/gobot/v2/drivers/ble/microbit" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) ubit := microbit.NewIOPinDriver(bleAdaptor) button := gpio.NewButtonDriver(ubit, "0") diff --git a/platforms/microbit/accelerometer_driver.go b/platforms/microbit/accelerometer_driver.go deleted file mode 100644 index 76dda8fc3..000000000 --- a/platforms/microbit/accelerometer_driver.go +++ /dev/null @@ -1,97 +0,0 @@ -package microbit - -import ( - "bytes" - "encoding/binary" - - "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" -) - -// AccelerometerDriver is the Gobot driver for the Microbit's built-in accelerometer -type AccelerometerDriver struct { - name string - connection gobot.Connection - gobot.Eventer -} - -type RawAccelerometerData struct { - X int16 - Y int16 - Z int16 -} - -type AccelerometerData struct { - X float32 - Y float32 - Z float32 -} - -const ( - // BLE services - // accelerometerService = "e95d0753251d470aa062fa1922dfa9a8" - - // BLE characteristics - accelerometerCharacteristic = "e95dca4b251d470aa062fa1922dfa9a8" - - // Accelerometer event - Accelerometer = "accelerometer" -) - -// NewAccelerometerDriver creates a Microbit AccelerometerDriver -func NewAccelerometerDriver(a ble.BLEConnector) *AccelerometerDriver { - n := &AccelerometerDriver{ - name: gobot.DefaultName("Microbit Accelerometer"), - connection: a, - Eventer: gobot.NewEventer(), - } - - n.AddEvent(Accelerometer) - - return n -} - -// Connection returns the BLE connection -func (b *AccelerometerDriver) Connection() gobot.Connection { return b.connection } - -// Name returns the Driver Name -func (b *AccelerometerDriver) Name() string { return b.name } - -// SetName sets the Driver Name -func (b *AccelerometerDriver) SetName(n string) { b.name = n } - -// adaptor returns BLE adaptor -func (b *AccelerometerDriver) adaptor() ble.BLEConnector { - //nolint:forcetypeassert // ok here - return b.Connection().(ble.BLEConnector) -} - -// Start tells driver to get ready to do work -func (b *AccelerometerDriver) Start() error { - // subscribe to accelerometer notifications - return b.adaptor().Subscribe(accelerometerCharacteristic, func(data []byte, e error) { - a := &RawAccelerometerData{X: 0, Y: 0, Z: 0} - - buf := bytes.NewBuffer(data) - if err := binary.Read(buf, binary.LittleEndian, &a.X); err != nil { - panic(err) - } - if err := binary.Read(buf, binary.LittleEndian, &a.Y); err != nil { - panic(err) - } - if err := binary.Read(buf, binary.LittleEndian, &a.Z); err != nil { - panic(err) - } - - result := &AccelerometerData{ - X: float32(a.X) / 1000.0, - Y: float32(a.Y) / 1000.0, - Z: float32(a.Z) / 1000.0, - } - - b.Publish(b.Event(Accelerometer), result) - }) -} - -// Halt stops LED driver (void) -func (b *AccelerometerDriver) Halt() error { return nil } diff --git a/platforms/microbit/button_driver.go b/platforms/microbit/button_driver.go deleted file mode 100644 index 35a17bbfb..000000000 --- a/platforms/microbit/button_driver.go +++ /dev/null @@ -1,75 +0,0 @@ -package microbit - -import ( - "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" -) - -// ButtonDriver is the Gobot driver for the Microbit's built-in buttons -type ButtonDriver struct { - name string - connection gobot.Connection - gobot.Eventer -} - -const ( - // BLE services - // buttonService = "e95d9882251d470aa062fa1922dfa9a8" - - // BLE characteristics - buttonACharacteristic = "e95dda90251d470aa062fa1922dfa9a8" - buttonBCharacteristic = "e95dda91251d470aa062fa1922dfa9a8" - - // ButtonA event - ButtonA = "buttonA" - - // ButtonB event - ButtonB = "buttonB" -) - -// NewButtonDriver creates a Microbit ButtonDriver -func NewButtonDriver(a ble.BLEConnector) *ButtonDriver { - n := &ButtonDriver{ - name: gobot.DefaultName("Microbit Button"), - connection: a, - Eventer: gobot.NewEventer(), - } - - n.AddEvent(ButtonA) - n.AddEvent(ButtonB) - - return n -} - -// Connection returns the BLE connection -func (b *ButtonDriver) Connection() gobot.Connection { return b.connection } - -// Name returns the Driver Name -func (b *ButtonDriver) Name() string { return b.name } - -// SetName sets the Driver Name -func (b *ButtonDriver) SetName(n string) { b.name = n } - -// adaptor returns BLE adaptor -func (b *ButtonDriver) adaptor() ble.BLEConnector { - //nolint:forcetypeassert // ok here - return b.Connection().(ble.BLEConnector) -} - -// Start tells driver to get ready to do work -func (b *ButtonDriver) Start() error { - // subscribe to button A notifications - if err := b.adaptor().Subscribe(buttonACharacteristic, func(data []byte, e error) { - b.Publish(b.Event(ButtonA), data) - }); err != nil { - return err - } - - // subscribe to button B notifications - return b.adaptor().Subscribe(buttonBCharacteristic, func(data []byte, e error) { - b.Publish(b.Event(ButtonB), data) - }) -} - -// Halt stops LED driver (void) -func (b *ButtonDriver) Halt() error { return nil } diff --git a/platforms/microbit/doc.go b/platforms/microbit/doc.go index e97b9f952..334516bcc 100644 --- a/platforms/microbit/doc.go +++ b/platforms/microbit/doc.go @@ -2,6 +2,6 @@ Package microbit contains the Gobot drivers for the Microbit. For more information refer to the microbit README: -https://github.com/hybridgroup/gobot/blob/master/platforms/microbit/README.md +https://github.com/hybridgroup/gobot/blob/release/drivers/ble/microbit/README.md */ -package microbit // import "gobot.io/x/gobot/v2/platforms/microbit" +package microbit // import "gobot.io/x/gobot/v2/drivers/ble/microbit" diff --git a/platforms/microbit/io_pin_driver.go b/platforms/microbit/io_pin_driver.go deleted file mode 100644 index b57a11027..000000000 --- a/platforms/microbit/io_pin_driver.go +++ /dev/null @@ -1,272 +0,0 @@ -package microbit - -import ( - "bytes" - "encoding/binary" - "errors" - "strconv" - - "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" -) - -// IOPinDriver is the Gobot driver for the Microbit's built-in digital and -// analog I/O -type IOPinDriver struct { - name string - adMask int - ioMask int - connection gobot.Connection - gobot.Eventer -} - -const ( - // BLE services - // ioPinService = "e95d127b251d470aa062fa1922dfa9a8" - - // BLE characteristics - pinDataCharacteristic = "e95d8d00251d470aa062fa1922dfa9a8" - pinADConfigCharacteristic = "e95d5899251d470aa062fa1922dfa9a8" - pinIOConfigCharacteristic = "e95db9fe251d470aa062fa1922dfa9a8" -) - -// PinData has the read data for a specific digital pin -type PinData struct { - pin uint8 - value uint8 -} - -// NewIOPinDriver creates a Microbit IOPinDriver -func NewIOPinDriver(a ble.BLEConnector) *IOPinDriver { - n := &IOPinDriver{ - name: gobot.DefaultName("Microbit IO Pins"), - connection: a, - Eventer: gobot.NewEventer(), - } - - return n -} - -// Connection returns the BLE connection -func (b *IOPinDriver) Connection() gobot.Connection { return b.connection } - -// Name returns the Driver Name -func (b *IOPinDriver) Name() string { return b.name } - -// SetName sets the Driver Name -func (b *IOPinDriver) SetName(n string) { b.name = n } - -// adaptor returns BLE adaptor -func (b *IOPinDriver) adaptor() ble.BLEConnector { - //nolint:forcetypeassert // ok here - return b.Connection().(ble.BLEConnector) -} - -// Start tells driver to get ready to do work -func (b *IOPinDriver) Start() error { - if _, err := b.ReadPinADConfig(); err != nil { - return err - } - _, err := b.ReadPinIOConfig() - return err -} - -// Halt stops driver (void) -func (b *IOPinDriver) Halt() error { return nil } - -// ReadAllPinData reads and returns the pin data for all pins -func (b *IOPinDriver) ReadAllPinData() []PinData { - c, _ := b.adaptor().ReadCharacteristic(pinDataCharacteristic) - buf := bytes.NewBuffer(c) - pinsData := make([]PinData, buf.Len()/2) - - for i := 0; i < buf.Len()/2; i++ { - pinData := PinData{} - pinData.pin, _ = buf.ReadByte() - pinData.value, _ = buf.ReadByte() - pinsData[i] = pinData - } - - return pinsData -} - -// WritePinData writes the pin data for a single pin -func (b *IOPinDriver) WritePinData(pin string, data byte) error { - i, err := strconv.Atoi(pin) - if err != nil { - return err - } - - buf := []byte{byte(i), data} - err = b.adaptor().WriteCharacteristic(pinDataCharacteristic, buf) - return err -} - -// ReadPinADConfig reads and returns the pin A/D config mask for all pins -func (b *IOPinDriver) ReadPinADConfig() (int, error) { - c, err := b.adaptor().ReadCharacteristic(pinADConfigCharacteristic) - if err != nil { - return 0, err - } - var result byte - for i := 0; i < 4; i++ { - result |= c[i] << uint(i) - } - - b.adMask = int(result) - return int(result), nil -} - -// WritePinADConfig writes the pin A/D config mask for all pins -func (b *IOPinDriver) WritePinADConfig(config int) error { - b.adMask = config - data := &bytes.Buffer{} - if err := binary.Write(data, binary.LittleEndian, uint32(config)); err != nil { - return err - } - - return b.adaptor().WriteCharacteristic(pinADConfigCharacteristic, data.Bytes()) -} - -// ReadPinIOConfig reads and returns the pin IO config mask for all pins -func (b *IOPinDriver) ReadPinIOConfig() (int, error) { - c, err := b.adaptor().ReadCharacteristic(pinIOConfigCharacteristic) - if err != nil { - return 0, err - } - - var result byte - for i := 0; i < 4; i++ { - result |= c[i] << uint(i) - } - - b.ioMask = int(result) - return int(result), nil -} - -// WritePinIOConfig writes the pin I/O config mask for all pins -func (b *IOPinDriver) WritePinIOConfig(config int) error { - b.ioMask = config - data := &bytes.Buffer{} - if err := binary.Write(data, binary.LittleEndian, uint32(config)); err != nil { - return err - } - - return b.adaptor().WriteCharacteristic(pinIOConfigCharacteristic, data.Bytes()) -} - -// DigitalRead reads from a pin -func (b *IOPinDriver) DigitalRead(pin string) (int, error) { - p, err := validatedPin(pin) - if err != nil { - return 0, err - } - - if err := b.ensureDigital(p); err != nil { - return 0, err - } - if err := b.ensureInput(p); err != nil { - return 0, err - } - - pins := b.ReadAllPinData() - return int(pins[p].value), nil -} - -// DigitalWrite writes to a pin -func (b *IOPinDriver) DigitalWrite(pin string, level byte) error { - p, err := validatedPin(pin) - if err != nil { - return err - } - - if err := b.ensureDigital(p); err != nil { - return err - } - if err := b.ensureOutput(p); err != nil { - return err - } - - return b.WritePinData(pin, level) -} - -// AnalogRead reads from a pin -func (b *IOPinDriver) AnalogRead(pin string) (int, error) { - p, err := validatedPin(pin) - if err != nil { - return 0, err - } - - if err := b.ensureAnalog(p); err != nil { - return 0, err - } - if err := b.ensureInput(p); err != nil { - return 0, err - } - - pins := b.ReadAllPinData() - return int(pins[p].value), nil -} - -func (b *IOPinDriver) ensureDigital(pin int) error { - if hasBit(b.adMask, pin) { - return b.WritePinADConfig(clearBit(b.adMask, pin)) - } - - return nil -} - -func (b *IOPinDriver) ensureAnalog(pin int) error { - if !hasBit(b.adMask, pin) { - return b.WritePinADConfig(setBit(b.adMask, pin)) - } - - return nil -} - -func (b *IOPinDriver) ensureInput(pin int) error { - if !hasBit(b.ioMask, pin) { - return b.WritePinIOConfig(setBit(b.ioMask, pin)) - } - - return nil -} - -func (b *IOPinDriver) ensureOutput(pin int) error { - if hasBit(b.ioMask, pin) { - return b.WritePinIOConfig(clearBit(b.ioMask, pin)) - } - - return nil -} - -func validatedPin(pin string) (int, error) { - i, err := strconv.Atoi(pin) - if err != nil { - return 0, err - } - - if i < 0 || i > 2 { - return 0, errors.New("Invalid pin.") - } - - return i, nil -} - -// via http://stackoverflow.com/questions/23192262/how-would-you-set-and-clear-a-single-bit-in-go -// Sets the bit at pos in the integer n. -func setBit(n int, pos int) int { - n |= (1 << uint(pos)) - return n -} - -// Test if the bit at pos is set in the integer n. -func hasBit(n int, pos int) bool { - val := n & (1 << uint(pos)) - return (val > 0) -} - -// Clears the bit at pos in n. -func clearBit(n int, pos int) int { - return n &^ (1 << uint(pos)) -} diff --git a/platforms/microbit/io_pin_driver_test.go b/platforms/microbit/io_pin_driver_test.go deleted file mode 100644 index db8b3e158..000000000 --- a/platforms/microbit/io_pin_driver_test.go +++ /dev/null @@ -1,161 +0,0 @@ -package microbit - -import ( - "errors" - "strings" - "testing" - - "github.com/stretchr/testify/assert" - "github.com/stretchr/testify/require" - - "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/drivers/aio" - "gobot.io/x/gobot/v2/drivers/gpio" -) - -// the IOPinDriver is a Driver -var _ gobot.Driver = (*IOPinDriver)(nil) - -// that supports the DigitalReader, DigitalWriter, & AnalogReader interfaces -var ( - _ gpio.DigitalReader = (*IOPinDriver)(nil) - _ gpio.DigitalWriter = (*IOPinDriver)(nil) - _ aio.AnalogReader = (*IOPinDriver)(nil) -) - -func initTestIOPinDriver() *IOPinDriver { - d := NewIOPinDriver(NewBleTestAdaptor()) - return d -} - -func TestIOPinDriver(t *testing.T) { - d := initTestIOPinDriver() - assert.True(t, strings.HasPrefix(d.Name(), "Microbit IO Pin")) - d.SetName("NewName") - assert.Equal(t, "NewName", d.Name()) -} - -func TestIOPinDriverStartAndHalt(t *testing.T) { - a := NewBleTestAdaptor() - d := NewIOPinDriver(a) - a.TestReadCharacteristic(func(cUUID string) ([]byte, error) { - return []byte{0, 1, 1, 0}, nil - }) - require.NoError(t, d.Start()) - require.NoError(t, d.Halt()) -} - -func TestIOPinDriverStartError(t *testing.T) { - a := NewBleTestAdaptor() - d := NewIOPinDriver(a) - a.TestReadCharacteristic(func(cUUID string) ([]byte, error) { - return nil, errors.New("read error") - }) - require.ErrorContains(t, d.Start(), "read error") -} - -func TestIOPinDriverDigitalRead(t *testing.T) { - a := NewBleTestAdaptor() - d := NewIOPinDriver(a) - a.TestReadCharacteristic(func(cUUID string) ([]byte, error) { - return []byte{0, 1, 1, 0, 2, 1}, nil - }) - - val, _ := d.DigitalRead("0") - assert.Equal(t, 1, val) - - val, _ = d.DigitalRead("1") - assert.Equal(t, 0, val) -} - -func TestIOPinDriverDigitalReadInvalidPin(t *testing.T) { - a := NewBleTestAdaptor() - d := NewIOPinDriver(a) - - _, err := d.DigitalRead("A3") - require.Error(t, err) - - _, err = d.DigitalRead("6") - require.ErrorContains(t, err, "Invalid pin.") -} - -func TestIOPinDriverDigitalWrite(t *testing.T) { - a := NewBleTestAdaptor() - d := NewIOPinDriver(a) - - // TODO: a better test - require.NoError(t, d.DigitalWrite("0", 1)) -} - -func TestIOPinDriverDigitalWriteInvalidPin(t *testing.T) { - a := NewBleTestAdaptor() - d := NewIOPinDriver(a) - - require.Error(t, d.DigitalWrite("A3", 1)) - require.ErrorContains(t, d.DigitalWrite("6", 1), "Invalid pin.") -} - -func TestIOPinDriverAnalogRead(t *testing.T) { - a := NewBleTestAdaptor() - d := NewIOPinDriver(a) - a.TestReadCharacteristic(func(cUUID string) ([]byte, error) { - return []byte{0, 0, 1, 128, 2, 1}, nil - }) - - val, _ := d.AnalogRead("0") - assert.Equal(t, 0, val) - - val, _ = d.AnalogRead("1") - assert.Equal(t, 128, val) -} - -func TestIOPinDriverAnalogReadInvalidPin(t *testing.T) { - a := NewBleTestAdaptor() - d := NewIOPinDriver(a) - - _, err := d.AnalogRead("A3") - require.Error(t, err) - - _, err = d.AnalogRead("6") - require.ErrorContains(t, err, "Invalid pin.") -} - -func TestIOPinDriverDigitalAnalogRead(t *testing.T) { - a := NewBleTestAdaptor() - d := NewIOPinDriver(a) - a.TestReadCharacteristic(func(cUUID string) ([]byte, error) { - return []byte{0, 0, 1, 128, 2, 1}, nil - }) - - val, _ := d.DigitalRead("0") - assert.Equal(t, 0, val) - - val, _ = d.AnalogRead("0") - assert.Equal(t, 0, val) -} - -func TestIOPinDriverDigitalWriteAnalogRead(t *testing.T) { - a := NewBleTestAdaptor() - d := NewIOPinDriver(a) - a.TestReadCharacteristic(func(cUUID string) ([]byte, error) { - return []byte{0, 0, 1, 128, 2, 1}, nil - }) - - require.NoError(t, d.DigitalWrite("1", 0)) - - val, _ := d.AnalogRead("1") - assert.Equal(t, 128, val) -} - -func TestIOPinDriverAnalogReadDigitalWrite(t *testing.T) { - a := NewBleTestAdaptor() - d := NewIOPinDriver(a) - a.TestReadCharacteristic(func(cUUID string) ([]byte, error) { - return []byte{0, 0, 1, 128, 2, 1}, nil - }) - - val, _ := d.AnalogRead("1") - assert.Equal(t, 128, val) - - require.NoError(t, d.DigitalWrite("1", 0)) -} diff --git a/platforms/microbit/magnetometer_driver.go b/platforms/microbit/magnetometer_driver.go deleted file mode 100644 index fa89d7403..000000000 --- a/platforms/microbit/magnetometer_driver.go +++ /dev/null @@ -1,97 +0,0 @@ -package microbit - -import ( - "bytes" - "encoding/binary" - - "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" -) - -// MagnetometerDriver is the Gobot driver for the Microbit's built-in magnetometer -type MagnetometerDriver struct { - name string - connection gobot.Connection - gobot.Eventer -} - -type RawMagnetometerData struct { - X int16 - Y int16 - Z int16 -} - -type MagnetometerData struct { - X float32 - Y float32 - Z float32 -} - -const ( - // BLE services - // magnetometerService = "e95df2d8251d470aa062fa1922dfa9a8" - - // BLE characteristics - magnetometerCharacteristic = "e95dfb11251d470aa062fa1922dfa9a8" - - // Magnetometer event - Magnetometer = "magnetometer" -) - -// NewMagnetometerDriver creates a Microbit MagnetometerDriver -func NewMagnetometerDriver(a ble.BLEConnector) *MagnetometerDriver { - n := &MagnetometerDriver{ - name: gobot.DefaultName("Microbit Magnetometer"), - connection: a, - Eventer: gobot.NewEventer(), - } - - n.AddEvent(Magnetometer) - - return n -} - -// Connection returns the BLE connection -func (b *MagnetometerDriver) Connection() gobot.Connection { return b.connection } - -// Name returns the Driver Name -func (b *MagnetometerDriver) Name() string { return b.name } - -// SetName sets the Driver Name -func (b *MagnetometerDriver) SetName(n string) { b.name = n } - -// adaptor returns BLE adaptor -func (b *MagnetometerDriver) adaptor() ble.BLEConnector { - //nolint:forcetypeassert // ok here - return b.Connection().(ble.BLEConnector) -} - -// Start tells driver to get ready to do work -func (b *MagnetometerDriver) Start() error { - // subscribe to magnetometer notifications - return b.adaptor().Subscribe(magnetometerCharacteristic, func(data []byte, e error) { - a := &RawMagnetometerData{X: 0, Y: 0, Z: 0} - - buf := bytes.NewBuffer(data) - if err := binary.Read(buf, binary.LittleEndian, &a.X); err != nil { - panic(err) - } - if err := binary.Read(buf, binary.LittleEndian, &a.Y); err != nil { - panic(err) - } - if err := binary.Read(buf, binary.LittleEndian, &a.Z); err != nil { - panic(err) - } - - result := &MagnetometerData{ - X: float32(a.X) / 1000.0, - Y: float32(a.Y) / 1000.0, - Z: float32(a.Z) / 1000.0, - } - - b.Publish(b.Event(Magnetometer), result) - }) -} - -// Halt stops LED driver (void) -func (b *MagnetometerDriver) Halt() error { return nil } diff --git a/platforms/microbit/temperature_driver.go b/platforms/microbit/temperature_driver.go deleted file mode 100644 index e8eaab283..000000000 --- a/platforms/microbit/temperature_driver.go +++ /dev/null @@ -1,70 +0,0 @@ -package microbit - -import ( - "bytes" - - "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" -) - -// TemperatureDriver is the Gobot driver for the Microbit's built-in thermometer -type TemperatureDriver struct { - name string - connection gobot.Connection - gobot.Eventer -} - -const ( - // BLE services - // temperatureService = "e95d6100251d470aa062fa1922dfa9a8" - - // BLE characteristics - temperatureCharacteristic = "e95d9250251d470aa062fa1922dfa9a8" - - // Temperature event - Temperature = "temperature" -) - -// NewTemperatureDriver creates a Microbit TemperatureDriver -func NewTemperatureDriver(a ble.BLEConnector) *TemperatureDriver { - n := &TemperatureDriver{ - name: gobot.DefaultName("Microbit Temperature"), - connection: a, - Eventer: gobot.NewEventer(), - } - - n.AddEvent(Temperature) - - return n -} - -// Connection returns the BLE connection -func (b *TemperatureDriver) Connection() gobot.Connection { return b.connection } - -// Name returns the Driver Name -func (b *TemperatureDriver) Name() string { return b.name } - -// SetName sets the Driver Name -func (b *TemperatureDriver) SetName(n string) { b.name = n } - -// adaptor returns BLE adaptor -func (b *TemperatureDriver) adaptor() ble.BLEConnector { - //nolint:forcetypeassert // ok here - return b.Connection().(ble.BLEConnector) -} - -// Start tells driver to get ready to do work -func (b *TemperatureDriver) Start() error { - // subscribe to temperature notifications - return b.adaptor().Subscribe(temperatureCharacteristic, func(data []byte, e error) { - var l int8 - buf := bytes.NewBuffer(data) - val, _ := buf.ReadByte() - l = int8(val) - - b.Publish(b.Event(Temperature), l) - }) -} - -// Halt stops Temperature driver (void) -func (b *TemperatureDriver) Halt() error { return nil } diff --git a/platforms/parrot/README.md b/platforms/parrot/README.md index 1ed4dd144..52c89877e 100644 --- a/platforms/parrot/README.md +++ b/platforms/parrot/README.md @@ -3,6 +3,7 @@ This package contains the Gobot adaptors and drivers for the various Parrot (https://www.parrot.com) drones. This package currently supports the following drones: + - Parrot ARDrone 2.0 - Parrot Bebop/Bebop 2 - Parrot Minidrone diff --git a/platforms/parrot/minidrone/README.md b/platforms/parrot/minidrone/README.md index 279d458dc..dc03a75b7 100644 --- a/platforms/parrot/minidrone/README.md +++ b/platforms/parrot/minidrone/README.md @@ -36,13 +36,13 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/parrot/minidrone" + "gobot.io/x/gobot/v2/platforms/bleclient" + "gobot.io/x/gobot/v2/drivers/ble/parrot" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) - drone := minidrone.NewDriver(bleAdaptor) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) + drone := parrot.NewMinidroneDriver(bleAdaptor) work := func() { drone.On(minidrone.Battery, func(data interface{}) { diff --git a/platforms/parrot/minidrone/doc.go b/platforms/parrot/minidrone/doc.go index bc5524aee..cf1e40c52 100644 --- a/platforms/parrot/minidrone/doc.go +++ b/platforms/parrot/minidrone/doc.go @@ -2,6 +2,6 @@ Package minidrone contains the Gobot driver for the Parrot Minidrone. For more information refer to the minidrone README: -https://github.com/hybridgroup/gobot/blob/master/platforms/parrot/minidrone/README.md +https://github.com/hybridgroup/gobot/blob/release/platforms/parrot/minidrone/README.md */ -package minidrone // import "gobot.io/x/gobot/v2/platforms/parrot/minidrone" +package minidrone // import "gobot.io/x/gobot/v2/drivers/ble/parrot" diff --git a/platforms/parrot/minidrone/helpers_test.go b/platforms/parrot/minidrone/helpers_test.go deleted file mode 100644 index 48106b64d..000000000 --- a/platforms/parrot/minidrone/helpers_test.go +++ /dev/null @@ -1,68 +0,0 @@ -package minidrone - -import ( - "sync" - - "gobot.io/x/gobot/v2/platforms/ble" -) - -var _ ble.BLEConnector = (*bleTestClientAdaptor)(nil) - -type bleTestClientAdaptor struct { - name string - address string - mtx sync.Mutex - withoutResponses bool - testReadCharacteristic func(string) ([]byte, error) - testWriteCharacteristic func(string, []byte) error -} - -func (t *bleTestClientAdaptor) Connect() error { return nil } -func (t *bleTestClientAdaptor) Reconnect() error { return nil } -func (t *bleTestClientAdaptor) Disconnect() error { return nil } -func (t *bleTestClientAdaptor) Finalize() error { return nil } -func (t *bleTestClientAdaptor) Name() string { return t.name } -func (t *bleTestClientAdaptor) SetName(n string) { t.name = n } -func (t *bleTestClientAdaptor) Address() string { return t.address } -func (t *bleTestClientAdaptor) WithoutResponses(use bool) { t.withoutResponses = use } - -func (t *bleTestClientAdaptor) ReadCharacteristic(cUUID string) ([]byte, error) { - t.mtx.Lock() - defer t.mtx.Unlock() - return t.testReadCharacteristic(cUUID) -} - -func (t *bleTestClientAdaptor) WriteCharacteristic(cUUID string, data []byte) error { - t.mtx.Lock() - defer t.mtx.Unlock() - return t.testWriteCharacteristic(cUUID, data) -} - -func (t *bleTestClientAdaptor) Subscribe(cUUID string, f func([]byte, error)) error { - // TODO: implement this... - return nil -} - -func (t *bleTestClientAdaptor) TestReadCharacteristic(f func(cUUID string) (data []byte, err error)) { - t.mtx.Lock() - defer t.mtx.Unlock() - t.testReadCharacteristic = f -} - -func (t *bleTestClientAdaptor) TestWriteCharacteristic(f func(cUUID string, data []byte) error) { - t.mtx.Lock() - defer t.mtx.Unlock() - t.testWriteCharacteristic = f -} - -func NewBleTestAdaptor() *bleTestClientAdaptor { - return &bleTestClientAdaptor{ - address: "01:02:03:04:05:06", - testReadCharacteristic: func(cUUID string) ([]byte, error) { - return nil, nil - }, - testWriteCharacteristic: func(cUUID string, data []byte) error { - return nil - }, - } -} diff --git a/platforms/parrot/minidrone/minidrone_driver.go b/platforms/parrot/minidrone/minidrone_driver.go deleted file mode 100644 index 904fe842d..000000000 --- a/platforms/parrot/minidrone/minidrone_driver.go +++ /dev/null @@ -1,535 +0,0 @@ -package minidrone - -import ( - "bytes" - "encoding/binary" - "fmt" - "sync" - "time" - - "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" -) - -// Driver is the Gobot interface to the Parrot Minidrone -type Driver struct { - name string - connection gobot.Connection - stepsfa0a uint16 - stepsfa0b uint16 - pcmdMutex sync.Mutex - flying bool - Pcmd Pcmd - gobot.Eventer -} - -const ( - // BLE services - // droneCommandService = "9a66fa000800919111e4012d1540cb8e" - // droneNotificationService = "9a66fb000800919111e4012d1540cb8e" - - // send characteristics - pcmdCharacteristic = "9a66fa0a0800919111e4012d1540cb8e" - commandCharacteristic = "9a66fa0b0800919111e4012d1540cb8e" - priorityCharacteristic = "9a66fa0c0800919111e4012d1540cb8e" - - // receive characteristics - flightStatusCharacteristic = "9a66fb0e0800919111e4012d1540cb8e" - batteryCharacteristic = "9a66fb0f0800919111e4012d1540cb8e" - - // piloting states - flatTrimChanged = 0 - flyingStateChanged = 1 - - // flying states - flyingStateLanded = 0 - flyingStateTakeoff = 1 - flyingStateHovering = 2 - flyingStateFlying = 3 - flyingStateLanding = 4 - flyingStateEmergency = 5 - flyingStateRolling = 6 - - // Battery event - Battery = "battery" - - // FlightStatus event - FlightStatus = "flightstatus" - - // Takeoff event - Takeoff = "takeoff" - - // Hovering event - Hovering = "hovering" - - // Flying event - Flying = "flying" - - // Landing event - Landing = "landing" - - // Landed event - Landed = "landed" - - // Emergency event - Emergency = "emergency" - - // Rolling event - Rolling = "rolling" - - // FlatTrimChange event - FlatTrimChange = "flattrimchange" - - // LightFixed mode for LightControl - LightFixed = 0 - - // LightBlinked mode for LightControl - LightBlinked = 1 - - // LightOscillated mode for LightControl - LightOscillated = 3 - - // ClawOpen mode for ClawControl - ClawOpen = 0 - - // ClawClosed mode for ClawControl - ClawClosed = 1 -) - -// Pcmd is the Parrot Command structure for flight control -type Pcmd struct { - Flag int - Roll int - Pitch int - Yaw int - Gaz int - Psi float32 -} - -// NewDriver creates a Parrot Minidrone Driver -func NewDriver(a ble.BLEConnector) *Driver { - n := &Driver{ - name: gobot.DefaultName("Minidrone"), - connection: a, - Pcmd: Pcmd{ - Flag: 0, - Roll: 0, - Pitch: 0, - Yaw: 0, - Gaz: 0, - Psi: 0, - }, - Eventer: gobot.NewEventer(), - } - - n.AddEvent(Battery) - n.AddEvent(FlightStatus) - - n.AddEvent(Takeoff) - n.AddEvent(Flying) - n.AddEvent(Hovering) - n.AddEvent(Landing) - n.AddEvent(Landed) - n.AddEvent(Emergency) - n.AddEvent(Rolling) - - return n -} - -// Connection returns the BLE connection -func (b *Driver) Connection() gobot.Connection { return b.connection } - -// Name returns the Driver Name -func (b *Driver) Name() string { return b.name } - -// SetName sets the Driver Name -func (b *Driver) SetName(n string) { b.name = n } - -// adaptor returns BLE adaptor -func (b *Driver) adaptor() ble.BLEConnector { - //nolint:forcetypeassert // ok here - return b.Connection().(ble.BLEConnector) -} - -// Start tells driver to get ready to do work -func (b *Driver) Start() error { - b.adaptor().WithoutResponses(true) - if err := b.Init(); err != nil { - return err - } - if err := b.FlatTrim(); err != nil { - return err - } - - b.StartPcmd() - - return b.FlatTrim() -} - -// Halt stops minidrone driver (void) -func (b *Driver) Halt() error { - err := b.Land() - time.Sleep(500 * time.Millisecond) - return err -} - -// Init initializes the BLE insterfaces used by the Minidrone -func (b *Driver) Init() error { - if err := b.GenerateAllStates(); err != nil { - return err - } - - // subscribe to battery notifications - if err := b.adaptor().Subscribe(batteryCharacteristic, func(data []byte, e error) { - b.Publish(b.Event(Battery), data[len(data)-1]) - }); err != nil { - return err - } - - // subscribe to flying status notifications - return b.adaptor().Subscribe(flightStatusCharacteristic, func(data []byte, e error) { - b.processFlightStatus(data) - }) -} - -// GenerateAllStates sets up all the default states aka settings on the drone -func (b *Driver) GenerateAllStates() error { - b.stepsfa0b++ - buf := []byte{ - 0x04, byte(b.stepsfa0b), 0x00, 0x04, 0x01, 0x00, 0x32, 0x30, 0x31, 0x34, 0x2D, 0x31, 0x30, 0x2D, 0x32, 0x38, 0x00, - } - return b.adaptor().WriteCharacteristic(commandCharacteristic, buf) -} - -// TakeOff tells the Minidrone to takeoff -func (b *Driver) TakeOff() error { - b.stepsfa0b++ - buf := []byte{0x02, byte(b.stepsfa0b) & 0xff, 0x02, 0x00, 0x01, 0x00} - return b.adaptor().WriteCharacteristic(commandCharacteristic, buf) -} - -// Land tells the Minidrone to land -func (b *Driver) Land() error { - b.stepsfa0b++ - buf := []byte{0x02, byte(b.stepsfa0b) & 0xff, 0x02, 0x00, 0x03, 0x00} - return b.adaptor().WriteCharacteristic(commandCharacteristic, buf) -} - -// FlatTrim calibrates the Minidrone to use its current position as being level -func (b *Driver) FlatTrim() error { - b.stepsfa0b++ - buf := []byte{0x02, byte(b.stepsfa0b) & 0xff, 0x02, 0x00, 0x00, 0x00} - return b.adaptor().WriteCharacteristic(commandCharacteristic, buf) -} - -// Emergency sets the Minidrone into emergency mode -func (b *Driver) Emergency() error { - b.stepsfa0b++ - buf := []byte{0x02, byte(b.stepsfa0b) & 0xff, 0x02, 0x00, 0x04, 0x00} - return b.adaptor().WriteCharacteristic(priorityCharacteristic, buf) -} - -// TakePicture tells the Minidrone to take a picture -func (b *Driver) TakePicture() error { - b.stepsfa0b++ - buf := []byte{0x02, byte(b.stepsfa0b) & 0xff, 0x02, 0x06, 0x01, 0x00} - return b.adaptor().WriteCharacteristic(commandCharacteristic, buf) -} - -// StartPcmd starts the continuous Pcmd communication with the Minidrone -func (b *Driver) StartPcmd() { - go func() { - // wait a little bit so that there is enough time to get some ACKs - time.Sleep(500 * time.Millisecond) - for { - err := b.adaptor().WriteCharacteristic(pcmdCharacteristic, b.generatePcmd().Bytes()) - if err != nil { - fmt.Println("pcmd write error:", err) - } - time.Sleep(50 * time.Millisecond) - } - }() -} - -// Up tells the drone to ascend. Pass in an int from 0-100. -func (b *Driver) Up(val int) error { - b.pcmdMutex.Lock() - defer b.pcmdMutex.Unlock() - - b.Pcmd.Flag = 1 - b.Pcmd.Gaz = validatePitch(val) - return nil -} - -// Down tells the drone to descend. Pass in an int from 0-100. -func (b *Driver) Down(val int) error { - b.pcmdMutex.Lock() - defer b.pcmdMutex.Unlock() - - b.Pcmd.Flag = 1 - b.Pcmd.Gaz = validatePitch(val) * -1 - return nil -} - -// Forward tells the drone to go forward. Pass in an int from 0-100. -func (b *Driver) Forward(val int) error { - b.pcmdMutex.Lock() - defer b.pcmdMutex.Unlock() - - b.Pcmd.Flag = 1 - b.Pcmd.Pitch = validatePitch(val) - return nil -} - -// Backward tells drone to go in reverse. Pass in an int from 0-100. -func (b *Driver) Backward(val int) error { - b.pcmdMutex.Lock() - defer b.pcmdMutex.Unlock() - - b.Pcmd.Flag = 1 - b.Pcmd.Pitch = validatePitch(val) * -1 - return nil -} - -// Right tells drone to go right. Pass in an int from 0-100. -func (b *Driver) Right(val int) error { - b.pcmdMutex.Lock() - defer b.pcmdMutex.Unlock() - - b.Pcmd.Flag = 1 - b.Pcmd.Roll = validatePitch(val) - return nil -} - -// Left tells drone to go left. Pass in an int from 0-100. -func (b *Driver) Left(val int) error { - b.pcmdMutex.Lock() - defer b.pcmdMutex.Unlock() - - b.Pcmd.Flag = 1 - b.Pcmd.Roll = validatePitch(val) * -1 - return nil -} - -// Clockwise tells drone to rotate in a clockwise direction. Pass in an int from 0-100. -func (b *Driver) Clockwise(val int) error { - b.pcmdMutex.Lock() - defer b.pcmdMutex.Unlock() - - b.Pcmd.Flag = 1 - b.Pcmd.Yaw = validatePitch(val) - return nil -} - -// CounterClockwise tells drone to rotate in a counter-clockwise direction. -// Pass in an int from 0-100. -func (b *Driver) CounterClockwise(val int) error { - b.pcmdMutex.Lock() - defer b.pcmdMutex.Unlock() - - b.Pcmd.Flag = 1 - b.Pcmd.Yaw = validatePitch(val) * -1 - return nil -} - -// Stop tells the drone to stop moving in any direction and simply hover in place -func (b *Driver) Stop() error { - b.pcmdMutex.Lock() - defer b.pcmdMutex.Unlock() - - b.Pcmd = Pcmd{ - Flag: 0, - Roll: 0, - Pitch: 0, - Yaw: 0, - Gaz: 0, - Psi: 0, - } - - return nil -} - -// StartRecording is not supported by the Parrot Minidrone -func (b *Driver) StartRecording() error { - return nil -} - -// StopRecording is not supported by the Parrot Minidrone -func (b *Driver) StopRecording() error { - return nil -} - -// HullProtection is not supported by the Parrot Minidrone -func (b *Driver) HullProtection(protect bool) error { - return nil -} - -// Outdoor mode is not supported by the Parrot Minidrone -func (b *Driver) Outdoor(outdoor bool) error { - return nil -} - -// FrontFlip tells the drone to perform a front flip -func (b *Driver) FrontFlip() error { - return b.adaptor().WriteCharacteristic(commandCharacteristic, b.generateAnimation(0).Bytes()) -} - -// BackFlip tells the drone to perform a backflip -func (b *Driver) BackFlip() error { - return b.adaptor().WriteCharacteristic(commandCharacteristic, b.generateAnimation(1).Bytes()) -} - -// RightFlip tells the drone to perform a flip to the right -func (b *Driver) RightFlip() error { - return b.adaptor().WriteCharacteristic(commandCharacteristic, b.generateAnimation(2).Bytes()) -} - -// LeftFlip tells the drone to perform a flip to the left -func (b *Driver) LeftFlip() error { - return b.adaptor().WriteCharacteristic(commandCharacteristic, b.generateAnimation(3).Bytes()) -} - -// LightControl controls lights on those Minidrone models which -// have the correct hardware, such as the Maclane, Blaze, & Swat. -// Params: -// -// id - always 0 -// mode - either LightFixed, LightBlinked, or LightOscillated -// intensity - Light intensity from 0 (OFF) to 100 (Max intensity). -// Only used in LightFixed mode. -func (b *Driver) LightControl(id uint8, mode uint8, intensity uint8) error { - b.stepsfa0b++ - buf := []byte{0x02, byte(b.stepsfa0b) & 0xff, 0x02, 0x10, 0x00, id, mode, intensity, 0x00} - return b.adaptor().WriteCharacteristic(commandCharacteristic, buf) -} - -// ClawControl controls the claw on the Parrot Mambo -// Params: -// -// id - always 0 -// mode - either ClawOpen or ClawClosed -func (b *Driver) ClawControl(id uint8, mode uint8) error { - b.stepsfa0b++ - buf := []byte{0x02, byte(b.stepsfa0b) & 0xff, 0x02, 0x10, 0x01, id, mode, 0x00} - return b.adaptor().WriteCharacteristic(commandCharacteristic, buf) -} - -// GunControl fires the gun on the Parrot Mambo -// Params: -// -// id - always 0 -func (b *Driver) GunControl(id uint8) error { - b.stepsfa0b++ - buf := []byte{0x02, byte(b.stepsfa0b) & 0xff, 0x02, 0x10, 0x02, id, 0x00} - return b.adaptor().WriteCharacteristic(commandCharacteristic, buf) -} - -func (b *Driver) generateAnimation(direction int8) *bytes.Buffer { - b.stepsfa0b++ - buf := []byte{0x02, byte(b.stepsfa0b) & 0xff, 0x02, 0x04, 0x00, 0x00, byte(direction), 0x00, 0x00, 0x00} - return bytes.NewBuffer(buf) -} - -func (b *Driver) generatePcmd() *bytes.Buffer { - b.pcmdMutex.Lock() - defer b.pcmdMutex.Unlock() - b.stepsfa0a++ - pcmd := b.Pcmd - - cmd := &bytes.Buffer{} - if err := binary.Write(cmd, binary.LittleEndian, int8(2)); err != nil { - panic(err) - } - if err := binary.Write(cmd, binary.LittleEndian, int8(b.stepsfa0a)); err != nil { - panic(err) - } - if err := binary.Write(cmd, binary.LittleEndian, int8(2)); err != nil { - panic(err) - } - if err := binary.Write(cmd, binary.LittleEndian, int8(0)); err != nil { - panic(err) - } - if err := binary.Write(cmd, binary.LittleEndian, int8(2)); err != nil { - panic(err) - } - if err := binary.Write(cmd, binary.LittleEndian, int8(0)); err != nil { - panic(err) - } - if err := binary.Write(cmd, binary.LittleEndian, int8(pcmd.Flag)); err != nil { - panic(err) - } - if err := binary.Write(cmd, binary.LittleEndian, int8(pcmd.Roll)); err != nil { - panic(err) - } - if err := binary.Write(cmd, binary.LittleEndian, int8(pcmd.Pitch)); err != nil { - panic(err) - } - if err := binary.Write(cmd, binary.LittleEndian, int8(pcmd.Yaw)); err != nil { - panic(err) - } - if err := binary.Write(cmd, binary.LittleEndian, int8(pcmd.Gaz)); err != nil { - panic(err) - } - if err := binary.Write(cmd, binary.LittleEndian, pcmd.Psi); err != nil { - panic(err) - } - if err := binary.Write(cmd, binary.LittleEndian, int16(0)); err != nil { - panic(err) - } - if err := binary.Write(cmd, binary.LittleEndian, int16(0)); err != nil { - panic(err) - } - - return cmd -} - -func (b *Driver) processFlightStatus(data []byte) { - if len(data) < 5 { - // ignore, just a sync - return - } - - b.Publish(FlightStatus, data[4]) - - switch data[4] { - case flatTrimChanged: - b.Publish(FlatTrimChange, true) - - case flyingStateChanged: - switch data[6] { - case flyingStateLanded: - if b.flying { - b.flying = false - b.Publish(Landed, true) - } - case flyingStateTakeoff: - b.Publish(Takeoff, true) - case flyingStateHovering: - if !b.flying { - b.flying = true - b.Publish(Hovering, true) - } - case flyingStateFlying: - if !b.flying { - b.flying = true - b.Publish(Flying, true) - } - case flyingStateLanding: - b.Publish(Landing, true) - case flyingStateEmergency: - b.Publish(Emergency, true) - case flyingStateRolling: - b.Publish(Rolling, true) - } - } -} - -func validatePitch(val int) int { - if val > 100 { - return 100 - } else if val < 0 { - return 0 - } - - return val -} diff --git a/platforms/parrot/minidrone/pitch.go b/platforms/parrot/minidrone/pitch.go deleted file mode 100644 index deadac85e..000000000 --- a/platforms/parrot/minidrone/pitch.go +++ /dev/null @@ -1,17 +0,0 @@ -package minidrone - -import "math" - -// ValidatePitch helps validate pitch values such as those created by -// a joystick to values between 0-100 that are required as -// params to Parrot Minidrone PCMDs -func ValidatePitch(data float64, offset float64) int { - value := math.Abs(data) / offset - if value >= 0.1 { - if value <= 1.0 { - return int((float64(int(value*100)) / 100) * 100) - } - return 100 - } - return 0 -} diff --git a/platforms/parrot/minidrone/pitch_test.go b/platforms/parrot/minidrone/pitch_test.go deleted file mode 100644 index 51b838bed..000000000 --- a/platforms/parrot/minidrone/pitch_test.go +++ /dev/null @@ -1,19 +0,0 @@ -package minidrone - -import ( - "testing" - - "github.com/stretchr/testify/assert" -) - -func TestMinidroneValidatePitchWhenEqualOffset(t *testing.T) { - assert.Equal(t, 100, ValidatePitch(32767.0, 32767.0)) -} - -func TestMinidroneValidatePitchWhenTiny(t *testing.T) { - assert.Equal(t, 0, ValidatePitch(1.1, 32767.0)) -} - -func TestMinidroneValidatePitchWhenCentered(t *testing.T) { - assert.Equal(t, 50, ValidatePitch(16383.5, 32767.0)) -} diff --git a/platforms/parrot/parrot.go b/platforms/parrot/parrot.go index 11ba96ece..ab041b440 100644 --- a/platforms/parrot/parrot.go +++ b/platforms/parrot/parrot.go @@ -6,6 +6,6 @@ This package currently supports the following Parrot drones: - Intel Joule developer kit For further information refer to Parrot README: -https://gobot.io/x/gobot/v2/blob/master/platforms/parrot/README.md +https://gobot.io/x/gobot/v2/blob/release/platforms/parrot/README.md */ package parrot diff --git a/platforms/serialport/LICENSE b/platforms/serialport/LICENSE new file mode 100644 index 000000000..09094bb5e --- /dev/null +++ b/platforms/serialport/LICENSE @@ -0,0 +1,13 @@ +Copyright (c) 2013-2018 The Hybrid Group + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. diff --git a/platforms/serialport/README.md b/platforms/serialport/README.md new file mode 100644 index 000000000..2209a52a4 --- /dev/null +++ b/platforms/serialport/README.md @@ -0,0 +1,11 @@ +# Serialport + +The adaptor "serialport" is a small wrapper to get access to the serial port. + +## How to Install + +Please refer to the main [README.md](https://github.com/hybridgroup/gobot/blob/release/README.md) + +## How to Use + +See documentation for [Sphero](https://github.com/hybridgroup/gobot/blob/master/platforms/sphero/sphero/README.md) diff --git a/platforms/sphero/sphero_adaptor.go b/platforms/serialport/adaptor.go similarity index 58% rename from platforms/sphero/sphero_adaptor.go rename to platforms/serialport/adaptor.go index d71f10666..688ca3571 100644 --- a/platforms/sphero/sphero_adaptor.go +++ b/platforms/serialport/adaptor.go @@ -1,4 +1,4 @@ -package sphero +package serialport import ( "io" @@ -8,7 +8,7 @@ import ( "gobot.io/x/gobot/v2" ) -// Adaptor represents a Connection to a Sphero +// Adaptor represents a Gobot Adaptor for the Serial Communication type Adaptor struct { name string port string @@ -17,10 +17,10 @@ type Adaptor struct { connect func(string) (io.ReadWriteCloser, error) } -// NewAdaptor returns a new Sphero Adaptor given a port +// NewAdaptor returns a new adaptor given a port for the serial communication func NewAdaptor(port string) *Adaptor { return &Adaptor{ - name: gobot.DefaultName("Sphero"), + name: gobot.DefaultName("Serial"), port: port, connect: func(port string) (io.ReadWriteCloser, error) { return serial.Open(port, &serial.Mode{BaudRate: 115200}) @@ -34,13 +34,7 @@ func (a *Adaptor) Name() string { return a.name } // SetName sets the Adaptor's name func (a *Adaptor) SetName(n string) { a.name = n } -// Port returns the Adaptor's port -func (a *Adaptor) Port() string { return a.port } - -// SetPort sets the Adaptor's port -func (a *Adaptor) SetPort(p string) { a.port = p } - -// Connect initiates a connection to the Sphero. Returns true on successful connection. +// Connect initiates a connection to the serial port. func (a *Adaptor) Connect() error { sp, err := a.connect(a.Port()) if err != nil { @@ -52,19 +46,12 @@ func (a *Adaptor) Connect() error { return nil } -// Reconnect attempts to reconnect to the Sphero. If the Sphero has an active connection -// it will first close that connection and then establish a new connection. -// Returns true on Successful reconnection -func (a *Adaptor) Reconnect() error { - if a.connected { - if err := a.Disconnect(); err != nil { - return err - } - } - return a.Connect() +// Finalize finalizes the adaptor by disconnect +func (a *Adaptor) Finalize() error { + return a.Disconnect() } -// Disconnect terminates the connection to the Sphero. Returns true on successful disconnect. +// Disconnect terminates the connection to the port. func (a *Adaptor) Disconnect() error { if a.connected { if err := a.sp.Close(); err != nil { @@ -75,7 +62,31 @@ func (a *Adaptor) Disconnect() error { return nil } -// Finalize finalizes the Sphero Adaptor -func (a *Adaptor) Finalize() error { - return a.Disconnect() +// 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 err := a.Disconnect(); err != nil { + return err + } + } + return a.Connect() +} + +// Port returns the Adaptor's port +func (a *Adaptor) Port() string { return a.port } + +// IsConnected returns the connection state +func (a *Adaptor) IsConnected() bool { + return a.connected +} + +// SerialRead reads from the port +func (a *Adaptor) SerialRead(p []byte) (int, error) { + return a.sp.Read(p) +} + +// SerialWrite writes to the port +func (a *Adaptor) SerialWrite(p []byte) (int, error) { + return a.sp.Write(p) } diff --git a/platforms/sphero/sphero_adaptor_test.go b/platforms/serialport/adaptor_test.go similarity index 94% rename from platforms/sphero/sphero_adaptor_test.go rename to platforms/serialport/adaptor_test.go index a364e14c0..65d7a4486 100644 --- a/platforms/sphero/sphero_adaptor_test.go +++ b/platforms/serialport/adaptor_test.go @@ -1,4 +1,4 @@ -package sphero +package serialport import ( "errors" @@ -58,14 +58,14 @@ func initTestSpheroAdaptor() (*Adaptor, *nullReadWriteCloser) { func TestSpheroAdaptorName(t *testing.T) { a, _ := initTestSpheroAdaptor() - assert.True(t, strings.HasPrefix(a.Name(), "Sphero")) + assert.True(t, strings.HasPrefix(a.Name(), "Serial")) a.SetName("NewName") assert.Equal(t, "NewName", a.Name()) } func TestSpheroAdaptor(t *testing.T) { a, _ := initTestSpheroAdaptor() - assert.True(t, strings.HasPrefix(a.Name(), "Sphero")) + assert.True(t, strings.HasPrefix(a.Name(), "Serial")) assert.Equal(t, "/dev/null", a.Port()) } diff --git a/platforms/serialport/doc.go b/platforms/serialport/doc.go new file mode 100644 index 000000000..5baa09f4d --- /dev/null +++ b/platforms/serialport/doc.go @@ -0,0 +1,7 @@ +/* +Package serialport provides the Gobot adaptor for serial communication with drivers. + +For further information refer to readme: +https://github.com/hybridgroup/gobot/blob/master/platforms/serialport/README.md +*/ +package serialport // import "gobot.io/x/gobot/v2/platforms/serialport" diff --git a/platforms/sphero/bb8/README.md b/platforms/sphero/bb8/README.md index b097ae57b..e9b2f85b7 100644 --- a/platforms/sphero/bb8/README.md +++ b/platforms/sphero/bb8/README.md @@ -16,13 +16,13 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/sphero/bb8" + "gobot.io/x/gobot/v2/platforms/bleclient" + "gobot.io/x/gobot/v2/drivers/ble/sphero/bb8" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) - bb8 := bb8.NewDriver(bleAdaptor) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) + bb8 := bb8.NewBB8Driver(bleAdaptor) work := func() { gobot.Every(1*time.Second, func() { diff --git a/platforms/sphero/bb8/bb8_driver.go b/platforms/sphero/bb8/bb8_driver.go deleted file mode 100644 index 8e4115833..000000000 --- a/platforms/sphero/bb8/bb8_driver.go +++ /dev/null @@ -1,22 +0,0 @@ -package bb8 - -import ( - "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/sphero/ollie" -) - -// Driver represents a Sphero BB-8 -type BB8Driver struct { - *ollie.Driver -} - -// NewDriver creates a Driver for a Sphero BB-8 -func NewDriver(a ble.BLEConnector) *BB8Driver { - d := ollie.NewDriver(a) - d.SetName(gobot.DefaultName("BB8")) - - return &BB8Driver{ - Driver: d, - } -} diff --git a/platforms/sphero/bb8/bb8_driver_test.go b/platforms/sphero/bb8/bb8_driver_test.go deleted file mode 100644 index b0ec1c1b9..000000000 --- a/platforms/sphero/bb8/bb8_driver_test.go +++ /dev/null @@ -1,31 +0,0 @@ -package bb8 - -import ( - "strings" - "testing" - - "github.com/stretchr/testify/assert" - "github.com/stretchr/testify/require" - - "gobot.io/x/gobot/v2" -) - -var _ gobot.Driver = (*BB8Driver)(nil) - -func initTestBB8Driver() *BB8Driver { - d := NewDriver(NewBleTestAdaptor()) - return d -} - -func TestBB8Driver(t *testing.T) { - d := initTestBB8Driver() - assert.True(t, strings.HasPrefix(d.Name(), "BB8")) - d.SetName("NewName") - assert.Equal(t, "NewName", d.Name()) -} - -func TestBB8DriverStartAndHalt(t *testing.T) { - d := initTestBB8Driver() - require.NoError(t, d.Start()) - require.NoError(t, d.Halt()) -} diff --git a/platforms/sphero/bb8/bb8_packets.go b/platforms/sphero/bb8/bb8_packets.go deleted file mode 100644 index d28d4d852..000000000 --- a/platforms/sphero/bb8/bb8_packets.go +++ /dev/null @@ -1,15 +0,0 @@ -package bb8 - -import "gobot.io/x/gobot/v2/platforms/sphero" - -// DefaultCollisionConfig returns a CollisionConfig with sensible collision defaults -func DefaultCollisionConfig() sphero.CollisionConfig { - return sphero.CollisionConfig{ - Method: 0x01, - Xt: 0x20, - Yt: 0x20, - Xs: 0x20, - Ys: 0x20, - Dead: 0x01, - } -} diff --git a/platforms/sphero/bb8/doc.go b/platforms/sphero/bb8/doc.go deleted file mode 100644 index 80aca8fe6..000000000 --- a/platforms/sphero/bb8/doc.go +++ /dev/null @@ -1,7 +0,0 @@ -/* -Package bb8 contains the Gobot driver for the Sphero BB-8. - -For more information refer to the BB-8 README: -https://github.com/hybridgroup/gobot/blob/master/platforms/sphero/bb8/README.md -*/ -package bb8 // import "gobot.io/x/gobot/v2/platforms/sphero/bb8" diff --git a/platforms/sphero/bb8/helpers_test.go b/platforms/sphero/bb8/helpers_test.go deleted file mode 100644 index 9738f6987..000000000 --- a/platforms/sphero/bb8/helpers_test.go +++ /dev/null @@ -1,69 +0,0 @@ -package bb8 - -import ( - "sync" - - "gobot.io/x/gobot/v2/platforms/ble" -) - -var _ ble.BLEConnector = (*bleTestClientAdaptor)(nil) - -type bleTestClientAdaptor struct { - name string - address string - mtx sync.Mutex - withoutResponses bool - - testReadCharacteristic func(string) ([]byte, error) - testWriteCharacteristic func(string, []byte) error -} - -func (t *bleTestClientAdaptor) Connect() error { return nil } -func (t *bleTestClientAdaptor) Reconnect() error { return nil } -func (t *bleTestClientAdaptor) Disconnect() error { return nil } -func (t *bleTestClientAdaptor) Finalize() error { return nil } -func (t *bleTestClientAdaptor) Name() string { return t.name } -func (t *bleTestClientAdaptor) SetName(n string) { t.name = n } -func (t *bleTestClientAdaptor) Address() string { return t.address } -func (t *bleTestClientAdaptor) WithoutResponses(use bool) { t.withoutResponses = use } - -func (t *bleTestClientAdaptor) ReadCharacteristic(cUUID string) ([]byte, error) { - t.mtx.Lock() - defer t.mtx.Unlock() - return t.testReadCharacteristic(cUUID) -} - -func (t *bleTestClientAdaptor) WriteCharacteristic(cUUID string, data []byte) error { - t.mtx.Lock() - defer t.mtx.Unlock() - return t.testWriteCharacteristic(cUUID, data) -} - -func (t *bleTestClientAdaptor) Subscribe(cUUID string, f func([]byte, error)) error { - // TODO: implement this... - return nil -} - -func (t *bleTestClientAdaptor) TestReadCharacteristic(f func(cUUID string) (data []byte, err error)) { - t.mtx.Lock() - defer t.mtx.Unlock() - t.testReadCharacteristic = f -} - -func (t *bleTestClientAdaptor) TestWriteCharacteristic(f func(cUUID string, data []byte) error) { - t.mtx.Lock() - defer t.mtx.Unlock() - t.testWriteCharacteristic = f -} - -func NewBleTestAdaptor() *bleTestClientAdaptor { - return &bleTestClientAdaptor{ - address: "01:02:03:04:05:06", - testReadCharacteristic: func(cUUID string) ([]byte, error) { - return nil, nil - }, - testWriteCharacteristic: func(cUUID string, data []byte) error { - return nil - }, - } -} diff --git a/platforms/sphero/doc.go b/platforms/sphero/doc.go deleted file mode 100644 index bfe1f7f6c..000000000 --- a/platforms/sphero/doc.go +++ /dev/null @@ -1,42 +0,0 @@ -/* -Package sphero provides the Gobot adaptor and driver for the Sphero. - -Installing: - - Please refer to the main [README.md](https://github.com/hybridgroup/gobot/blob/release/README.md) - -Example: - - package main - - import ( - "fmt" - "time" - - "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/sphero" - ) - - func main() { - adaptor := sphero.NewAdaptor("/dev/rfcomm0") - driver := sphero.NewSpheroDriver(adaptor) - - work := func() { - gobot.Every(3*time.Second, func() { - driver.Roll(30, uint16(gobot.Rand(360))) - }) - } - - robot := gobot.NewRobot("sphero", - []gobot.Connection{adaptor}, - []gobot.Device{driver}, - work, - ) - - robot.Start() - } - -For further information refer to sphero readme: -https://github.com/hybridgroup/gobot/blob/master/platforms/sphero/README.md -*/ -package sphero // import "gobot.io/x/gobot/v2/platforms/sphero" diff --git a/platforms/sphero/ollie/README.md b/platforms/sphero/ollie/README.md index 2f2b541a8..803016f5d 100644 --- a/platforms/sphero/ollie/README.md +++ b/platforms/sphero/ollie/README.md @@ -16,13 +16,13 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/sphero/ollie" + "gobot.io/x/gobot/v2/platforms/bleclient" + "gobot.io/x/gobot/v2/drivers/ble/sphero/ollie" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) - ollie := ollie.NewDriver(bleAdaptor) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) + ollie := ollie.NewOllieDriver(bleAdaptor) work := func() { gobot.Every(1*time.Second, func() { diff --git a/platforms/sphero/ollie/doc.go b/platforms/sphero/ollie/doc.go deleted file mode 100644 index e848ccbad..000000000 --- a/platforms/sphero/ollie/doc.go +++ /dev/null @@ -1,7 +0,0 @@ -/* -Package ollie contains the Gobot driver for the Sphero Ollie. - -For more information refer to the Ollie README: -https://github.com/hybridgroup/gobot/blob/master/platforms/sphero/ollie/README.md -*/ -package ollie // import "gobot.io/x/gobot/v2/platforms/sphero/ollie" diff --git a/platforms/sphero/ollie/helpers_test.go b/platforms/sphero/ollie/helpers_test.go deleted file mode 100644 index 031c4f922..000000000 --- a/platforms/sphero/ollie/helpers_test.go +++ /dev/null @@ -1,69 +0,0 @@ -package ollie - -import ( - "sync" - - "gobot.io/x/gobot/v2/platforms/ble" -) - -var _ ble.BLEConnector = (*bleTestClientAdaptor)(nil) - -type bleTestClientAdaptor struct { - name string - address string - mtx sync.Mutex - withoutResponses bool - - testReadCharacteristic func(string) ([]byte, error) - testWriteCharacteristic func(string, []byte) error -} - -func (t *bleTestClientAdaptor) Connect() error { return nil } -func (t *bleTestClientAdaptor) Reconnect() error { return nil } -func (t *bleTestClientAdaptor) Disconnect() error { return nil } -func (t *bleTestClientAdaptor) Finalize() error { return nil } -func (t *bleTestClientAdaptor) Name() string { return t.name } -func (t *bleTestClientAdaptor) SetName(n string) { t.name = n } -func (t *bleTestClientAdaptor) Address() string { return t.address } -func (t *bleTestClientAdaptor) WithoutResponses(use bool) { t.withoutResponses = use } - -func (t *bleTestClientAdaptor) ReadCharacteristic(cUUID string) ([]byte, error) { - t.mtx.Lock() - defer t.mtx.Unlock() - return t.testReadCharacteristic(cUUID) -} - -func (t *bleTestClientAdaptor) WriteCharacteristic(cUUID string, data []byte) error { - t.mtx.Lock() - defer t.mtx.Unlock() - return t.testWriteCharacteristic(cUUID, data) -} - -func (t *bleTestClientAdaptor) Subscribe(cUUID string, f func([]byte, error)) error { - // TODO: implement this... - return nil -} - -func (t *bleTestClientAdaptor) TestReadCharacteristic(f func(cUUID string) (data []byte, err error)) { - t.mtx.Lock() - defer t.mtx.Unlock() - t.testReadCharacteristic = f -} - -func (t *bleTestClientAdaptor) TestWriteCharacteristic(f func(cUUID string, data []byte) error) { - t.mtx.Lock() - defer t.mtx.Unlock() - t.testWriteCharacteristic = f -} - -func NewBleTestAdaptor() *bleTestClientAdaptor { - return &bleTestClientAdaptor{ - address: "01:02:03:04:05:06", - testReadCharacteristic: func(cUUID string) ([]byte, error) { - return nil, nil - }, - testWriteCharacteristic: func(cUUID string, data []byte) error { - return nil - }, - } -} diff --git a/platforms/sphero/ollie/ollie_driver.go b/platforms/sphero/ollie/ollie_driver.go deleted file mode 100644 index d544d3a66..000000000 --- a/platforms/sphero/ollie/ollie_driver.go +++ /dev/null @@ -1,481 +0,0 @@ -package ollie - -import ( - "bytes" - "encoding/binary" - "fmt" - "sync" - "time" - - "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/sphero" -) - -// Driver is the Gobot driver for the Sphero Ollie robot -type Driver struct { - name string - connection gobot.Connection - seq uint8 - mtx sync.Mutex - collisionResponse []uint8 - packetChannel chan *Packet - asyncBuffer []byte - asyncMessage []byte - locatorCallback func(p Point2D) - powerstateCallback func(p PowerStatePacket) - gobot.Eventer -} - -const ( - // bluetooth service IDs - // spheroBLEService = "22bb746f2bb075542d6f726568705327" - // robotControlService = "22bb746f2ba075542d6f726568705327" - - // BLE characteristic IDs - wakeCharacteristic = "22bb746f2bbf75542d6f726568705327" - txPowerCharacteristic = "22bb746f2bb275542d6f726568705327" - antiDosCharacteristic = "22bb746f2bbd75542d6f726568705327" - commandsCharacteristic = "22bb746f2ba175542d6f726568705327" - responseCharacteristic = "22bb746f2ba675542d6f726568705327" - - // SensorData event - SensorData = "sensordata" - - // Collision event - Collision = "collision" - - // Error event - Error = "error" - - // Packet header size - PacketHeaderSize = 5 - - // Response packet max size - ResponsePacketMaxSize = 20 - - // Collision Packet data size: The number of bytes following the DLEN field through the end of the packet - CollisionDataSize = 17 - - // Full size of the collision response - CollisionResponseSize = PacketHeaderSize + CollisionDataSize -) - -// MotorModes is used to configure the motor -type MotorModes uint8 - -// MotorModes required for SetRawMotorValues command -const ( - Off MotorModes = iota - Forward - Reverse - Brake - Ignore -) - -// Packet describes head, body and checksum for a data package to be sent to the sphero. -type Packet struct { - Header []uint8 - Body []uint8 - Checksum uint8 -} - -// Point2D represents a koordinate in 2-Dimensional space -type Point2D struct { - X int16 - Y int16 -} - -// NewDriver creates a Driver for a Sphero Ollie -func NewDriver(a ble.BLEConnector) *Driver { - n := &Driver{ - name: gobot.DefaultName("Ollie"), - connection: a, - Eventer: gobot.NewEventer(), - packetChannel: make(chan *Packet, 1024), - } - - n.AddEvent(Collision) - - return n -} - -// PacketChannel returns the channel for packets to be sent to the sp -func (b *Driver) PacketChannel() chan *Packet { return b.packetChannel } - -// Sequence returns the Sequence number of the current packet -func (b *Driver) Sequence() uint8 { return b.seq } - -// Connection returns the connection to this Ollie -func (b *Driver) Connection() gobot.Connection { return b.connection } - -// Name returns the name for the Driver -func (b *Driver) Name() string { return b.name } - -// SetName sets the Name for the Driver -func (b *Driver) SetName(n string) { b.name = n } - -// adaptor returns BLE adaptor -func (b *Driver) adaptor() ble.BLEConnector { - //nolint:forcetypeassert // ok here - return b.Connection().(ble.BLEConnector) -} - -// Start tells driver to get ready to do work -func (b *Driver) Start() error { - if err := b.Init(); err != nil { - return err - } - - // send commands - go func() { - for { - packet := <-b.packetChannel - err := b.write(packet) - if err != nil { - b.Publish(b.Event(Error), err) - } - } - }() - - go func() { - for { - if _, err := b.adaptor().ReadCharacteristic(responseCharacteristic); err != nil { - panic(err) - } - time.Sleep(100 * time.Millisecond) - } - }() - - b.ConfigureCollisionDetection(DefaultCollisionConfig()) - - return nil -} - -// Halt stops Ollie driver (void) -func (b *Driver) Halt() error { - b.Sleep() - time.Sleep(750 * time.Microsecond) - return nil -} - -// Init is used to initialize the Ollie -func (b *Driver) Init() error { - if err := b.AntiDOSOff(); err != nil { - return err - } - if err := b.SetTXPower(7); err != nil { - return err - } - if err := b.Wake(); err != nil { - return err - } - - // subscribe to Sphero response notifications - return b.adaptor().Subscribe(responseCharacteristic, b.HandleResponses) -} - -// AntiDOSOff turns off Anti-DOS code so we can control Ollie -func (b *Driver) AntiDOSOff() error { - str := "011i3" - buf := &bytes.Buffer{} - buf.WriteString(str) - - if err := b.adaptor().WriteCharacteristic(antiDosCharacteristic, buf.Bytes()); err != nil { - fmt.Println("AntiDOSOff error:", err) - return err - } - - return nil -} - -// Wake wakes Ollie up so we can play -func (b *Driver) Wake() error { - buf := []byte{0x01} - - if err := b.adaptor().WriteCharacteristic(wakeCharacteristic, buf); err != nil { - fmt.Println("Wake error:", err) - return err - } - - return nil -} - -// SetTXPower sets transmit level -func (b *Driver) SetTXPower(level int) error { - buf := []byte{byte(level)} - - if err := b.adaptor().WriteCharacteristic(txPowerCharacteristic, buf); err != nil { - fmt.Println("SetTXLevel error:", err) - return err - } - - return nil -} - -// HandleResponses handles responses returned from Ollie -func (b *Driver) HandleResponses(data []byte, e error) { - // since packets can only be 20 bytes long, we have to puzzle them together - newMessage := false - - // append message parts to existing - if len(data) > 0 && data[0] != 0xFF { - b.asyncBuffer = append(b.asyncBuffer, data...) - } - - // clear message when new one begins (first byte is always 0xFF) - if len(data) > 0 && data[0] == 0xFF { - b.asyncMessage = b.asyncBuffer - b.asyncBuffer = data - newMessage = true - } - - parts := b.asyncMessage - // 3 is the id of data streaming, located at index 2 byte - if newMessage && len(parts) > 2 && parts[2] == 3 { - b.handleDataStreaming(parts) - } - - // index 1 is the type of the message, 0xFF being a direct response, 0xFE an asynchronous message - if len(data) > 4 && data[1] == 0xFF && data[0] == 0xFF { - // locator request - if data[4] == 0x0B && len(data) == 16 { - b.handleLocatorDetected(data) - } - - if data[4] == 0x09 { - b.handlePowerStateDetected(data) - } - } - - b.handleCollisionDetected(data) -} - -// GetLocatorData calls the passed function with the data from the locator -func (b *Driver) GetLocatorData(f func(p Point2D)) { - // CID 0x15 is the code for the locator request - b.PacketChannel() <- b.craftPacket([]uint8{}, 0x02, 0x15) - b.locatorCallback = f -} - -// GetPowerState calls the passed function with the Power State information from the sphero -func (b *Driver) GetPowerState(f func(p PowerStatePacket)) { - // CID 0x20 is the code for the power state - b.PacketChannel() <- b.craftPacket([]uint8{}, 0x00, 0x20) - b.powerstateCallback = f -} - -func (b *Driver) handleDataStreaming(data []byte) { - // ensure data is the right length: - if len(data) != 88 { - return - } - - // data packet is the same as for the normal sphero, since the same communication api is used - // only difference in communication is that the "newer" spheros use BLE for communinations - var dataPacket DataStreamingPacket - buffer := bytes.NewBuffer(data[5:]) // skip header - if err := binary.Read(buffer, binary.BigEndian, &dataPacket); err != nil { - panic(err) - } - - b.Publish(SensorData, dataPacket) -} - -// SetRGB sets the Ollie to the given r, g, and b values -func (b *Driver) SetRGB(r uint8, g uint8, bl uint8) { - b.packetChannel <- b.craftPacket([]uint8{r, g, bl, 0x01}, 0x02, 0x20) -} - -// Roll tells the Ollie to roll -func (b *Driver) Roll(speed uint8, heading uint16) { - b.packetChannel <- b.craftPacket([]uint8{speed, uint8(heading >> 8), uint8(heading & 0xFF), 0x01}, 0x02, 0x30) -} - -// Boost executes the boost macro from within the SSB which takes a -// 1 byte parameter which is either 01h to begin boosting or 00h to stop. -func (b *Driver) Boost(state bool) { - s := uint8(0x01) - if !state { - s = 0x00 - } - b.packetChannel <- b.craftPacket([]uint8{s}, 0x02, 0x31) -} - -// SetStabilization enables or disables the built-in auto stabilizing features of the Ollie -func (b *Driver) SetStabilization(state bool) { - s := uint8(0x01) - if !state { - s = 0x00 - } - b.packetChannel <- b.craftPacket([]uint8{s}, 0x02, 0x02) -} - -// SetRotationRate allows you to control the rotation rate that Sphero will use to meet new -// heading commands. A value of 255 jumps to the maximum (currently 400 degrees/sec). -// A value of zero doesn't make much sense so it's interpreted as 1, the minimum. -func (b *Driver) SetRotationRate(speed uint8) { - b.packetChannel <- b.craftPacket([]uint8{speed}, 0x02, 0x03) -} - -// SetRawMotorValues allows you to take over one or both of the motor output values, -// instead of having the stabilization system control them. Each motor (left and right) -// requires a mode and a power value from 0-255 -func (b *Driver) SetRawMotorValues(lmode MotorModes, lpower uint8, rmode MotorModes, rpower uint8) { - b.packetChannel <- b.craftPacket([]uint8{uint8(lmode), lpower, uint8(rmode), rpower}, 0x02, 0x33) -} - -// SetBackLEDOutput allows you to control the brightness of the back(tail) LED. -func (b *Driver) SetBackLEDOutput(value uint8) { - b.packetChannel <- b.craftPacket([]uint8{value}, 0x02, 0x21) -} - -// Stop tells the Ollie to stop -func (b *Driver) Stop() { - b.Roll(0, 0) -} - -// Sleep says Go to sleep -func (b *Driver) Sleep() { - b.packetChannel <- b.craftPacket([]uint8{0x00, 0x00, 0x00, 0x00, 0x00}, 0x00, 0x22) -} - -// EnableStopOnDisconnect auto-sends a Stop command after losing the connection -func (b *Driver) EnableStopOnDisconnect() { - b.packetChannel <- b.craftPacket([]uint8{0x00, 0x00, 0x00, 0x01}, 0x02, 0x37) -} - -// ConfigureCollisionDetection configures the sensitivity of the detection. -func (b *Driver) ConfigureCollisionDetection(cc sphero.CollisionConfig) { - b.packetChannel <- b.craftPacket([]uint8{cc.Method, cc.Xt, cc.Yt, cc.Xs, cc.Ys, cc.Dead}, 0x02, 0x12) -} - -// SetDataStreamingConfig passes the config to the sphero to stream sensor data -func (b *Driver) SetDataStreamingConfig(d sphero.DataStreamingConfig) error { - buf := new(bytes.Buffer) - if err := binary.Write(buf, binary.BigEndian, d); err != nil { - return err - } - b.PacketChannel() <- b.craftPacket(buf.Bytes(), 0x02, 0x11) - return nil -} - -func (b *Driver) write(packet *Packet) error { - buf := append(packet.Header, packet.Body...) - buf = append(buf, packet.Checksum) - if err := b.adaptor().WriteCharacteristic(commandsCharacteristic, buf); err != nil { - fmt.Println("send command error:", err) - return err - } - - b.mtx.Lock() - defer b.mtx.Unlock() - b.seq++ - return nil -} - -func (b *Driver) craftPacket(body []uint8, did byte, cid byte) *Packet { - b.mtx.Lock() - defer b.mtx.Unlock() - - packet := new(Packet) - packet.Body = body - dlen := len(packet.Body) + 1 - packet.Header = []uint8{0xFF, 0xFF, did, cid, b.seq, uint8(dlen)} - packet.Checksum = b.calculateChecksum(packet) - return packet -} - -func (b *Driver) handlePowerStateDetected(data []uint8) { - var dataPacket PowerStatePacket - buffer := bytes.NewBuffer(data[5:]) // skip header - if err := binary.Read(buffer, binary.BigEndian, &dataPacket); err != nil { - panic(err) - } - - b.powerstateCallback(dataPacket) -} - -func (b *Driver) handleLocatorDetected(data []uint8) { - // read the unsigned raw values - ux := binary.BigEndian.Uint16(data[5:7]) - uy := binary.BigEndian.Uint16(data[7:9]) - - // convert to signed values - var x, y int16 - - if ux > 32255 { - x = int16(ux - 65535) - } else { - x = int16(ux) - } - - if uy > 32255 { - y = int16(uy - 65535) - } else { - y = int16(uy) - } - - // create point obj - p := new(Point2D) - p.X = x - p.Y = y - - if b.locatorCallback != nil { - b.locatorCallback(*p) - } -} - -func (b *Driver) handleCollisionDetected(data []uint8) { - switch len(data) { - case ResponsePacketMaxSize: - // Check if this is the header of collision response. (i.e. first part of data) - // Collision response is 22 bytes long. (individual packet size is maxed at 20) - if data[1] == 0xFE && data[2] == 0x07 && len(b.collisionResponse) == 0 { - // response code 7 is for a detected collision - b.collisionResponse = append(b.collisionResponse, data...) - } - case CollisionResponseSize - ResponsePacketMaxSize: - // if this is the remaining part of the collision response, - // then make sure the header and first part of data is already received - if len(b.collisionResponse) == ResponsePacketMaxSize { - b.collisionResponse = append(b.collisionResponse, data...) - } - default: - return // not collision event - } - - // check expected sizes - if len(b.collisionResponse) != CollisionResponseSize || b.collisionResponse[4] != CollisionDataSize { - return - } - - // confirm checksum - size := len(b.collisionResponse) - chk := b.collisionResponse[size-1] // last byte is checksum - if chk != calculateChecksum(b.collisionResponse[2:size-1]) { - return - } - - var collision sphero.CollisionPacket - buffer := bytes.NewBuffer(b.collisionResponse[5:]) // skip header - if err := binary.Read(buffer, binary.BigEndian, &collision); err != nil { - panic(err) - } - b.collisionResponse = nil // clear the current response - - b.Publish(Collision, collision) -} - -func (b *Driver) calculateChecksum(packet *Packet) uint8 { - buf := append(packet.Header, packet.Body...) - return calculateChecksum(buf[2:]) -} - -func calculateChecksum(buf []byte) byte { - var calculatedChecksum uint16 - for i := range buf { - calculatedChecksum += uint16(buf[i]) - } - return uint8(^(calculatedChecksum % 256)) -} diff --git a/platforms/sphero/ollie/ollie_packets.go b/platforms/sphero/ollie/ollie_packets.go deleted file mode 100644 index 673c18954..000000000 --- a/platforms/sphero/ollie/ollie_packets.go +++ /dev/null @@ -1,115 +0,0 @@ -package ollie - -import "gobot.io/x/gobot/v2/platforms/sphero" - -// DefaultCollisionConfig returns a CollisionConfig with sensible collision defaults -func DefaultCollisionConfig() sphero.CollisionConfig { - return sphero.CollisionConfig{ - Method: 0x01, - Xt: 0x20, - Yt: 0x20, - Xs: 0x20, - Ys: 0x20, - Dead: 0x60, - } -} - -// PowerStatePacket contains all data relevant to the power state of the sphero -type PowerStatePacket struct { - // record Version Code - RecVer uint8 - // High-Level State of the Battery; 1=charging, 2=battery ok, 3=battery low, 4=battery critical - PowerState uint8 - // Battery Voltage, scaled in 100th of a Volt, 0x02EF would be 7.51 volts - BattVoltage uint16 - // Number of charges in the total lifetime of the sphero - NumCharges uint16 - // Seconds awake since last charge - TimeSinceChg uint16 -} - -// DataStreamingPacket represents the response from a Data Streaming event -type DataStreamingPacket struct { - // 8000 0000h accelerometer axis X, raw -2048 to 2047 4mG - RawAccX int16 - // 4000 0000h accelerometer axis Y, raw -2048 to 2047 4mG - RawAccY int16 - // 2000 0000h accelerometer axis Z, raw -2048 to 2047 4mG - RawAccZ int16 - // 1000 0000h gyro axis X, raw -32768 to 32767 0.068 degrees - RawGyroX int16 - // 0800 0000h gyro axis Y, raw -32768 to 32767 0.068 degrees - RawGyroY int16 - // 0400 0000h gyro axis Z, raw -32768 to 32767 0.068 degrees - RawGyroZ int16 - // 0200 0000h Reserved - Rsrv1 int16 - // 0100 0000h Reserved - Rsrv2 int16 - // 0080 0000h Reserved - Rsrv3 int16 - // 0040 0000h right motor back EMF, raw -32768 to 32767 22.5 cm - RawRMotorBack int16 - // 0020 0000h left motor back EMF, raw -32768 to 32767 22.5 cm - RawLMotorBack int16 - // 0010 0000h left motor, PWM, raw -2048 to 2047 duty cycle - RawLMotor int16 - // 0008 0000h right motor, PWM raw -2048 to 2047 duty cycle - RawRMotor int16 - // 0004 0000h IMU pitch angle, filtered -179 to 180 degrees - FiltPitch int16 - // 0002 0000h IMU roll angle, filtered -179 to 180 degrees - FiltRoll int16 - // 0001 0000h IMU yaw angle, filtered -179 to 180 degrees - FiltYaw int16 - // 0000 8000h accelerometer axis X, filtered -32768 to 32767 1/4096 G - FiltAccX int16 - // 0000 4000h accelerometer axis Y, filtered -32768 to 32767 1/4096 G - FiltAccY int16 - // 0000 2000h accelerometer axis Z, filtered -32768 to 32767 1/4096 G - FiltAccZ int16 - // 0000 1000h gyro axis X, filtered -20000 to 20000 0.1 dps - FiltGyroX int16 - // 0000 0800h gyro axis Y, filtered -20000 to 20000 0.1 dps - FiltGyroY int16 - // 0000 0400h gyro axis Z, filtered -20000 to 20000 0.1 dps - FiltGyroZ int16 - // 0000 0200h Reserved - Rsrv4 int16 - // 0000 0100h Reserved - Rsrv5 int16 - // 0000 0080h Reserved - Rsrv6 int16 - // 0000 0040h right motor back EMF, filtered -32768 to 32767 22.5 cm - FiltRMotorBack int16 - // 0000 0020h left motor back EMF, filtered -32768 to 32767 22.5 cm - FiltLMotorBack int16 - // 0000 0010h Reserved 1 - Rsrv7 int16 - // 0000 0008h Reserved 2 - Rsrv8 int16 - // // 0000 0004h Reserved 3 - // Rsrv9 int16 - // // 0000 0002h Reserved 4 - // Rsrv10 int16 - // // 0000 0001h Reserved 5 - // Rsrv11 int16 - // 8000 0000h Quaternion Q0 -10000 to 10000 1/10000 Q - Quat0 int16 - // 4000 0000h Quaternion Q1 -10000 to 10000 1/10000 Q - Quat1 int16 - // 2000 0000h Quaternion Q2 -10000 to 10000 1/10000 Q - Quat2 int16 - // 1000 0000h Quaternion Q3 -10000 to 10000 1/10000 Q - Quat3 int16 - // 0800 0000h Odometer X -32768 to 32767 cm - OdomX int16 - // 0400 0000h Odometer Y -32768 to 32767 cm - OdomY int16 - // 0200 0000h AccelOne 0 to 8000 1 mG - AccelOne int16 - // 0100 0000h Velocity X -32768 to 32767 mm/s - VeloX int16 - // 0080 0000h Velocity Y -32768 to 32767 mm/s - VeloY int16 -} diff --git a/platforms/sphero/sphero/LICENSE b/platforms/sphero/sphero/LICENSE new file mode 100644 index 000000000..09094bb5e --- /dev/null +++ b/platforms/sphero/sphero/LICENSE @@ -0,0 +1,13 @@ +Copyright (c) 2013-2018 The Hybrid Group + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. diff --git a/platforms/sphero/README.md b/platforms/sphero/sphero/README.md similarity index 77% rename from platforms/sphero/README.md rename to platforms/sphero/sphero/README.md index 9132c6368..e2cdb0087 100644 --- a/platforms/sphero/README.md +++ b/platforms/sphero/sphero/README.md @@ -2,9 +2,9 @@ Sphero is a sophisticated and programmable robot housed in a polycarbonate sphere shell. -The Gobot Sphero Adaptor & Driver makes it easy to interact with Sphero using Go. Once you have your Sphero setup and connected -to your computer you can start writing code to make Sphero move, change direction, speed and colors, or detect Sphero events -and execute some code when they occur. +The Gobot Serial Adaptor & Sphero Driver makes it easy to interact with Sphero using Go. Once you have your Sphero setup +and connected to your computer you can start writing code to make Sphero move, change direction, speed and colors, or +detect Sphero events and execute some code when they occur. Learn more about the Sphero robot go here: @@ -49,7 +49,8 @@ gort bluetooth connect
### Windows -You should be able to pair your Sphero using your normal system tray applet for Bluetooth, and then connect to the COM port that is bound to the device, such as `COM3`. +You should be able to pair your Sphero using your normal system tray applet for Bluetooth, and then connect to the COM +port that is bound to the device, such as `COM3`. ## How to Use @@ -63,12 +64,13 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/sphero" + "gobot.io/x/gobot/v2/drivers/serial" + "gobot.io/x/gobot/v2/platforms/serialport" ) func main() { - adaptor := sphero.NewAdaptor("/dev/rfcomm0") - driver := sphero.NewSpheroDriver(adaptor) + adaptor := serialport.NewAdaptor("/dev/rfcomm0") + driver := serial.NewSpheroDriver(adaptor) work := func() { gobot.Every(3*time.Second, func() { diff --git a/platforms/sphero/sphero_driver.go b/platforms/sphero/sphero_driver.go deleted file mode 100644 index 81963422e..000000000 --- a/platforms/sphero/sphero_driver.go +++ /dev/null @@ -1,472 +0,0 @@ -package sphero - -import ( - "bytes" - "encoding/binary" - "errors" - "sync" - "time" - - "gobot.io/x/gobot/v2" -) - -const ( - // Error event when error encountered - Error = "error" - - // SensorData event when sensor data is received - SensorData = "sensordata" - - // Collision event when collision is detected - Collision = "collision" -) - -type packet struct { - header []uint8 - body []uint8 - checksum uint8 -} - -// SpheroDriver Represents a Sphero 2.0 -type SpheroDriver struct { - name string - connection gobot.Connection - mtx sync.Mutex - seq uint8 - asyncResponse [][]uint8 - syncResponse [][]uint8 - packetChannel chan *packet - responseChannel chan []uint8 - originalColor []uint8 // Only used for calibration. - gobot.Eventer - gobot.Commander -} - -// NewSpheroDriver returns a new SpheroDriver given a Sphero Adaptor. -// -// Adds the following API Commands: -// -// "ConfigureLocator" - See SpheroDriver.ConfigureLocator -// "Roll" - See SpheroDriver.Roll -// "Stop" - See SpheroDriver.Stop -// "GetRGB" - See SpheroDriver.GetRGB -// "ReadLocator" - See SpheroDriver.ReadLocator -// "SetBackLED" - See SpheroDriver.SetBackLED -// "SetHeading" - See SpheroDriver.SetHeading -// "SetStabilization" - See SpheroDriver.SetStabilization -// "SetDataStreaming" - See SpheroDriver.SetDataStreaming -// "SetRotationRate" - See SpheroDriver.SetRotationRate -func NewSpheroDriver(a *Adaptor) *SpheroDriver { - s := &SpheroDriver{ - name: gobot.DefaultName("Sphero"), - connection: a, - Eventer: gobot.NewEventer(), - Commander: gobot.NewCommander(), - packetChannel: make(chan *packet, 1024), - responseChannel: make(chan []uint8, 1024), - } - - s.AddEvent(Error) - s.AddEvent(Collision) - s.AddEvent(SensorData) - - //nolint:forcetypeassert // ok here - s.AddCommand("SetRGB", func(params map[string]interface{}) interface{} { - r := uint8(params["r"].(float64)) - g := uint8(params["g"].(float64)) - b := uint8(params["b"].(float64)) - s.SetRGB(r, g, b) - return nil - }) - - //nolint:forcetypeassert // ok here - s.AddCommand("Roll", func(params map[string]interface{}) interface{} { - speed := uint8(params["speed"].(float64)) - heading := uint16(params["heading"].(float64)) - s.Roll(speed, heading) - return nil - }) - - s.AddCommand("Stop", func(params map[string]interface{}) interface{} { - s.Stop() - return nil - }) - - s.AddCommand("GetRGB", func(params map[string]interface{}) interface{} { - return s.GetRGB() - }) - - s.AddCommand("ReadLocator", func(params map[string]interface{}) interface{} { - return s.ReadLocator() - }) - - //nolint:forcetypeassert // ok here - s.AddCommand("SetBackLED", func(params map[string]interface{}) interface{} { - level := uint8(params["level"].(float64)) - s.SetBackLED(level) - return nil - }) - //nolint:forcetypeassert // ok here - s.AddCommand("SetRotationRate", func(params map[string]interface{}) interface{} { - level := uint8(params["level"].(float64)) - s.SetRotationRate(level) - return nil - }) - //nolint:forcetypeassert // ok here - s.AddCommand("SetHeading", func(params map[string]interface{}) interface{} { - heading := uint16(params["heading"].(float64)) - s.SetHeading(heading) - return nil - }) - //nolint:forcetypeassert // ok here - s.AddCommand("SetStabilization", func(params map[string]interface{}) interface{} { - on := params["enable"].(bool) - s.SetStabilization(on) - return nil - }) - //nolint:forcetypeassert // ok here - s.AddCommand("SetDataStreaming", func(params map[string]interface{}) interface{} { - N := uint16(params["N"].(float64)) - M := uint16(params["M"].(float64)) - Mask := uint32(params["Mask"].(float64)) - Pcnt := uint8(params["Pcnt"].(float64)) - Mask2 := uint32(params["Mask2"].(float64)) - - s.SetDataStreaming(DataStreamingConfig{N: N, M: M, Mask2: Mask2, Pcnt: Pcnt, Mask: Mask}) - return nil - }) - //nolint:forcetypeassert // ok here - s.AddCommand("ConfigureLocator", func(params map[string]interface{}) interface{} { - Flags := uint8(params["Flags"].(float64)) - X := int16(params["X"].(float64)) - Y := int16(params["Y"].(float64)) - YawTare := int16(params["YawTare"].(float64)) - - s.ConfigureLocator(LocatorConfig{Flags: Flags, X: X, Y: Y, YawTare: YawTare}) - return nil - }) - - return s -} - -// Name returns the Driver Name -func (s *SpheroDriver) Name() string { return s.name } - -// SetName sets the Driver Name -func (s *SpheroDriver) SetName(n string) { s.name = n } - -// Connection returns the Driver's Connection -func (s *SpheroDriver) Connection() gobot.Connection { return s.connection } - -func (s *SpheroDriver) adaptor() *Adaptor { - //nolint:forcetypeassert // ok here - return s.Connection().(*Adaptor) -} - -// Start starts the SpheroDriver and enables Collision Detection. -// Returns true on successful start. -// -// Emits the Events: -// -// Collision sphero.CollisionPacket - On Collision Detected -// SensorData sphero.DataStreamingPacket - On Data Streaming event -// Error error- On error while processing asynchronous response -func (s *SpheroDriver) Start() error { - go func() { - for { - packet := <-s.packetChannel - err := s.write(packet) - if err != nil { - s.Publish(Error, err) - } - } - }() - - go func() { - for { - response := <-s.responseChannel - s.syncResponse = append(s.syncResponse, response) - } - }() - - go func() { - for { - header := s.readHeader() - if len(header) > 0 { - body := s.readBody(header[4]) - data := append(header, body...) - checksum := data[len(data)-1] - if checksum != calculateChecksum(data[2:len(data)-1]) { - continue - } - switch header[1] { - case 0xFE: - s.asyncResponse = append(s.asyncResponse, data) - case 0xFF: - s.responseChannel <- data - } - } - } - }() - - go func() { - for { - var evt []uint8 - for len(s.asyncResponse) != 0 { - evt, s.asyncResponse = s.asyncResponse[len(s.asyncResponse)-1], s.asyncResponse[:len(s.asyncResponse)-1] - if evt[2] == 0x07 { - s.handleCollisionDetected(evt) - } else if evt[2] == 0x03 { - s.handleDataStreaming(evt) - } - } - time.Sleep(100 * time.Millisecond) - } - }() - - s.ConfigureCollisionDetection(DefaultCollisionConfig()) - s.enableStopOnDisconnect() - - return nil -} - -// Halt halts the SpheroDriver and sends a SpheroDriver.Stop command to the Sphero. -// Returns true on successful halt. -func (s *SpheroDriver) Halt() error { - if s.adaptor().connected { - gobot.Every(10*time.Millisecond, func() { - s.Stop() - }) - time.Sleep(1 * time.Second) - } - return nil -} - -// SetRGB sets the Sphero to the given r, g, and b values -func (s *SpheroDriver) SetRGB(r uint8, g uint8, b uint8) { - s.packetChannel <- s.craftPacket([]uint8{r, g, b, 0x01}, 0x02, 0x20) -} - -// GetRGB returns the current r, g, b value of the Sphero -func (s *SpheroDriver) GetRGB() []uint8 { - buf := s.getSyncResponse(s.craftPacket([]uint8{}, 0x02, 0x22)) - if len(buf) == 9 { - return []uint8{buf[5], buf[6], buf[7]} - } - return []uint8{} -} - -// ReadLocator reads Sphero's current position (X,Y), component velocities and SOG (speed over ground). -func (s *SpheroDriver) ReadLocator() []int16 { - buf := s.getSyncResponse(s.craftPacket([]uint8{}, 0x02, 0x15)) - if len(buf) == 16 { - vals := make([]int16, 5) - _ = binary.Read(bytes.NewReader(buf[5:15]), binary.BigEndian, &vals) - return vals - } - return []int16{} -} - -// SetBackLED sets the Sphero Back LED to the specified brightness -func (s *SpheroDriver) SetBackLED(level uint8) { - s.packetChannel <- s.craftPacket([]uint8{level}, 0x02, 0x21) -} - -// SetRotationRate sets the Sphero rotation rate -// A value of 255 jumps to the maximum (currently 400 degrees/sec). -func (s *SpheroDriver) SetRotationRate(level uint8) { - s.packetChannel <- s.craftPacket([]uint8{level}, 0x02, 0x03) -} - -// SetHeading sets the heading of the Sphero -func (s *SpheroDriver) SetHeading(heading uint16) { - s.packetChannel <- s.craftPacket([]uint8{uint8(heading >> 8), uint8(heading & 0xFF)}, 0x02, 0x01) -} - -// SetStabilization enables or disables the built-in auto stabilizing features of the Sphero -func (s *SpheroDriver) SetStabilization(on bool) { - b := uint8(0x01) - if !on { - b = 0x00 - } - s.packetChannel <- s.craftPacket([]uint8{b}, 0x02, 0x02) -} - -// Roll sends a roll command to the Sphero gives a speed and heading -func (s *SpheroDriver) Roll(speed uint8, heading uint16) { - s.packetChannel <- s.craftPacket([]uint8{speed, uint8(heading >> 8), uint8(heading & 0xFF), 0x01}, 0x02, 0x30) -} - -// ConfigureLocator configures and enables the Locator -func (s *SpheroDriver) ConfigureLocator(d LocatorConfig) { - buf := new(bytes.Buffer) - if err := binary.Write(buf, binary.BigEndian, d); err != nil { - panic(err) - } - - s.packetChannel <- s.craftPacket(buf.Bytes(), 0x02, 0x13) -} - -// SetDataStreaming enables sensor data streaming -func (s *SpheroDriver) SetDataStreaming(d DataStreamingConfig) { - buf := new(bytes.Buffer) - if err := binary.Write(buf, binary.BigEndian, d); err != nil { - panic(err) - } - - s.packetChannel <- s.craftPacket(buf.Bytes(), 0x02, 0x11) -} - -// Stop sets the Sphero to a roll speed of 0 -func (s *SpheroDriver) Stop() { - s.Roll(0, 0) -} - -// ConfigureCollisionDetection configures the sensitivity of the detection. -func (s *SpheroDriver) ConfigureCollisionDetection(cc CollisionConfig) { - s.packetChannel <- s.craftPacket([]uint8{cc.Method, cc.Xt, cc.Yt, cc.Xs, cc.Ys, cc.Dead}, 0x02, 0x12) -} - -// SetCalibration sets up Sphero for manual heading calibration. -// It does this by turning on the tail light (so you can tell where it's -// facing) and disabling stabilization (so you can adjust the heading). -// -// When done, call FinishCalibration to set the new heading, and re-enable -// stabilization. -func (s *SpheroDriver) StartCalibration() { - s.mtx.Lock() - s.originalColor = s.GetRGB() - s.SetRGB(0, 0, 0) - s.SetBackLED(127) - s.SetStabilization(false) - s.mtx.Unlock() -} - -// FinishCalibration ends Sphero's calibration mode, by setting -// the new heading as current, and re-enabling normal defaults. This is a NOP -// in case StartCalibration was not called. -func (s *SpheroDriver) FinishCalibration() { - s.mtx.Lock() - if s.originalColor == nil { - // Piggybacking on the original color being set to know if we are - // calibrating or not. - return - } - - s.SetHeading(0) - s.SetRGB(s.originalColor[0], s.originalColor[1], s.originalColor[2]) - s.SetBackLED(0) - s.SetStabilization(true) - s.originalColor = nil - s.mtx.Unlock() -} - -func (s *SpheroDriver) enableStopOnDisconnect() { - s.packetChannel <- s.craftPacket([]uint8{0x00, 0x00, 0x00, 0x01}, 0x02, 0x37) -} - -func (s *SpheroDriver) handleCollisionDetected(data []uint8) { - // ensure data is the right length: - if len(data) != 22 || data[4] != 17 { - return - } - var collision CollisionPacket - buffer := bytes.NewBuffer(data[5:]) // skip header - if err := binary.Read(buffer, binary.BigEndian, &collision); err != nil { - panic(err) - } - s.Publish(Collision, collision) -} - -func (s *SpheroDriver) handleDataStreaming(data []uint8) { - // ensure data is the right length: - if len(data) != 90 { - return - } - var dataPacket DataStreamingPacket - buffer := bytes.NewBuffer(data[5:]) // skip header - if err := binary.Read(buffer, binary.BigEndian, &dataPacket); err != nil { - panic(err) - } - s.Publish(SensorData, dataPacket) -} - -func (s *SpheroDriver) getSyncResponse(packet *packet) []byte { - s.packetChannel <- packet - for i := 0; i < 500; i++ { - for key := range s.syncResponse { - if s.syncResponse[key][3] == packet.header[4] && len(s.syncResponse[key]) > 6 { - var response []byte - response, s.syncResponse = s.syncResponse[len(s.syncResponse)-1], s.syncResponse[:len(s.syncResponse)-1] - return response - } - } - time.Sleep(100 * time.Microsecond) - } - - return []byte{} -} - -func (s *SpheroDriver) craftPacket(body []uint8, did byte, cid byte) *packet { //nolint:unparam // keep did as parameter - s.mtx.Lock() - defer s.mtx.Unlock() - packet := new(packet) - packet.body = body - dlen := len(packet.body) + 1 - packet.header = []uint8{0xFF, 0xFF, did, cid, s.seq, uint8(dlen)} - packet.checksum = s.calculateChecksum(packet) - return packet -} - -func (s *SpheroDriver) write(packet *packet) error { - s.mtx.Lock() - defer s.mtx.Unlock() - buf := append(packet.header, packet.body...) - buf = append(buf, packet.checksum) - length, err := s.adaptor().sp.Write(buf) - if err != nil { - return err - } - - if length != len(buf) { - return errors.New("Not enough bytes written") - } - s.seq++ - return nil -} - -func (s *SpheroDriver) calculateChecksum(packet *packet) uint8 { - buf := append(packet.header, packet.body...) - return calculateChecksum(buf[2:]) -} - -func calculateChecksum(buf []byte) byte { - var calculatedChecksum uint16 - for i := range buf { - calculatedChecksum += uint16(buf[i]) - } - return uint8(^(calculatedChecksum % 256)) -} - -func (s *SpheroDriver) readHeader() []uint8 { - return s.readNextChunk(5) -} - -func (s *SpheroDriver) readBody(length uint8) []uint8 { - return s.readNextChunk(int(length)) -} - -func (s *SpheroDriver) readNextChunk(length int) []uint8 { - read := make([]uint8, length) - bytesRead := 0 - - for bytesRead < length { - time.Sleep(1 * time.Millisecond) - n, err := s.adaptor().sp.Read(read[bytesRead:]) - if err != nil { - return nil - } - bytesRead += n - } - return read -} diff --git a/platforms/sphero/sprkplus/README.md b/platforms/sphero/sprkplus/README.md index 64da9b203..87fcdb3c4 100644 --- a/platforms/sphero/sprkplus/README.md +++ b/platforms/sphero/sprkplus/README.md @@ -16,13 +16,13 @@ import ( "time" "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/sphero/sprkplus" + "gobot.io/x/gobot/v2/platforms/bleclient" + "gobot.io/x/gobot/v2/drivers/ble/sphero" ) func main() { - bleAdaptor := ble.NewClientAdaptor(os.Args[1]) - sprk := sprkplus.NewDriver(bleAdaptor) + bleAdaptor := bleclient.NewAdaptor(os.Args[1]) + sprk := sphero.NewSPRKPlusDriver(bleAdaptor) work := func() { gobot.Every(1*time.Second, func() { diff --git a/platforms/sphero/sprkplus/doc.go b/platforms/sphero/sprkplus/doc.go deleted file mode 100644 index 93ed883d3..000000000 --- a/platforms/sphero/sprkplus/doc.go +++ /dev/null @@ -1,7 +0,0 @@ -/* -Package sprkplus contains the Gobot driver for the Sphero SPRK+. - -For more information refer to the sprkplus README: -https://github.com/hybridgroup/gobot/blob/master/platforms/sphero/sprkplus/README.md -*/ -package sprkplus // import "gobot.io/x/gobot/v2/platforms/sphero/sprkplus" diff --git a/platforms/sphero/sprkplus/helpers_test.go b/platforms/sphero/sprkplus/helpers_test.go deleted file mode 100644 index b26be641e..000000000 --- a/platforms/sphero/sprkplus/helpers_test.go +++ /dev/null @@ -1,69 +0,0 @@ -package sprkplus - -import ( - "sync" - - "gobot.io/x/gobot/v2/platforms/ble" -) - -var _ ble.BLEConnector = (*bleTestClientAdaptor)(nil) - -type bleTestClientAdaptor struct { - name string - address string - mtx sync.Mutex - withoutResponses bool - - testReadCharacteristic func(string) ([]byte, error) - testWriteCharacteristic func(string, []byte) error -} - -func (t *bleTestClientAdaptor) Connect() error { return nil } -func (t *bleTestClientAdaptor) Reconnect() error { return nil } -func (t *bleTestClientAdaptor) Disconnect() error { return nil } -func (t *bleTestClientAdaptor) Finalize() error { return nil } -func (t *bleTestClientAdaptor) Name() string { return t.name } -func (t *bleTestClientAdaptor) SetName(n string) { t.name = n } -func (t *bleTestClientAdaptor) Address() string { return t.address } -func (t *bleTestClientAdaptor) WithoutResponses(use bool) { t.withoutResponses = use } - -func (t *bleTestClientAdaptor) ReadCharacteristic(cUUID string) ([]byte, error) { - t.mtx.Lock() - defer t.mtx.Unlock() - return t.testReadCharacteristic(cUUID) -} - -func (t *bleTestClientAdaptor) WriteCharacteristic(cUUID string, data []byte) error { - t.mtx.Lock() - defer t.mtx.Unlock() - return t.testWriteCharacteristic(cUUID, data) -} - -func (t *bleTestClientAdaptor) Subscribe(cUUID string, f func([]byte, error)) error { - // TODO: implement this... - return nil -} - -func (t *bleTestClientAdaptor) TestReadCharacteristic(f func(cUUID string) (data []byte, err error)) { - t.mtx.Lock() - defer t.mtx.Unlock() - t.testReadCharacteristic = f -} - -func (t *bleTestClientAdaptor) TestWriteCharacteristic(f func(cUUID string, data []byte) error) { - t.mtx.Lock() - defer t.mtx.Unlock() - t.testWriteCharacteristic = f -} - -func NewBleTestAdaptor() *bleTestClientAdaptor { - return &bleTestClientAdaptor{ - address: "01:02:03:04:05:06", - testReadCharacteristic: func(cUUID string) ([]byte, error) { - return nil, nil - }, - testWriteCharacteristic: func(cUUID string, data []byte) error { - return nil - }, - } -} diff --git a/platforms/sphero/sprkplus/sprkplus_driver.go b/platforms/sphero/sprkplus/sprkplus_driver.go deleted file mode 100644 index 3f30bb89e..000000000 --- a/platforms/sphero/sprkplus/sprkplus_driver.go +++ /dev/null @@ -1,22 +0,0 @@ -package sprkplus - -import ( - "gobot.io/x/gobot/v2" - "gobot.io/x/gobot/v2/platforms/ble" - "gobot.io/x/gobot/v2/platforms/sphero/ollie" -) - -// Driver represents a Sphero SPRK+ -type SPRKPlusDriver struct { - *ollie.Driver -} - -// NewDriver creates a Driver for a Sphero SPRK+ -func NewDriver(a ble.BLEConnector) *SPRKPlusDriver { - d := ollie.NewDriver(a) - d.SetName(gobot.DefaultName("SPRKPlus")) - - return &SPRKPlusDriver{ - Driver: d, - } -} diff --git a/platforms/sphero/sprkplus/sprkplus_driver_test.go b/platforms/sphero/sprkplus/sprkplus_driver_test.go deleted file mode 100644 index 67742a13a..000000000 --- a/platforms/sphero/sprkplus/sprkplus_driver_test.go +++ /dev/null @@ -1,31 +0,0 @@ -package sprkplus - -import ( - "strings" - "testing" - - "github.com/stretchr/testify/assert" - "github.com/stretchr/testify/require" - - "gobot.io/x/gobot/v2" -) - -var _ gobot.Driver = (*SPRKPlusDriver)(nil) - -func initTestSPRKPlusDriver() *SPRKPlusDriver { - d := NewDriver(NewBleTestAdaptor()) - return d -} - -func TestSPRKPlusDriver(t *testing.T) { - d := initTestSPRKPlusDriver() - assert.True(t, strings.HasPrefix(d.Name(), "SPRK")) - d.SetName("NewName") - assert.Equal(t, "NewName", d.Name()) -} - -func TestSPRKPlusDriverStartAndHalt(t *testing.T) { - d := initTestSPRKPlusDriver() - require.NoError(t, d.Start()) - require.NoError(t, d.Halt()) -} diff --git a/platforms/sphero/sprkplus/sprkplus_packets.go b/platforms/sphero/sprkplus/sprkplus_packets.go deleted file mode 100644 index 7ffeb53a8..000000000 --- a/platforms/sphero/sprkplus/sprkplus_packets.go +++ /dev/null @@ -1,15 +0,0 @@ -package sprkplus - -import "gobot.io/x/gobot/v2/platforms/sphero" - -// DefaultCollisionConfig returns a CollisionConfig with sensible collision defaults -func DefaultCollisionConfig() sphero.CollisionConfig { - return sphero.CollisionConfig{ - Method: 0x01, - Xt: 0x20, - Yt: 0x20, - Xs: 0x20, - Ys: 0x20, - Dead: 0x01, - } -}