OrgGo
/
hybridgroup.gobot
mirror of https://github.com/hybridgroup/gobot.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

Slight refactor and add default config constructors

This commit is contained in:
Adrian Zankich 2014-12-25 08:32:24 -08:00
parent f043f98c28
commit 4a9954a5f9
3 changed files with 79 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
examples/sphero.go
View File

@ -15,8 +15,14 @@ func main() {
spheroDriver := sphero.NewSpheroDriver(adaptor, "sphero")
work := func() {
gobot.On(spheroDriver.Event("collision"), func(data interface{}) {
fmt.Printf("Collision Detected! %+v\n", data)
spheroDriver.SetDataStreaming(sphero.DefaultDataStreamingConfig())
gobot.On(spheroDriver.Event(sphero.Collision), func(data interface{}) {
fmt.Printf("Collision! %+v\n", data)
})
gobot.On(spheroDriver.Event(sphero.SensorData), func(data interface{}) {
fmt.Printf("Streaming Data! %+v\n", data)
})
gobot.Every(3*time.Second, func() {

74
platforms/sphero/sphero_driver.go
View File

@ -12,11 +12,9 @@ import (
var _ gobot.Driver = (*SpheroDriver)(nil)
const (
SensorFrequencyDefault = 10
SensorFrequencyMax = 420
ChannelSensordata = "sensordata"
ChannelCollisions = "collision"
Error = "error"
SensorData = "sensordata"
Collision = "collision"
Error = "error"
)
type packet struct {
@ -36,7 +34,6 @@ type SpheroDriver struct {
responseChannel chan []uint8
gobot.Eventer
gobot.Commander
cdConfigured bool // Indicates if collision detection is configured.
}
// NewSpheroDriver returns a new SpheroDriver given a SpheroAdaptor and name.
@ -60,8 +57,8 @@ func NewSpheroDriver(a *SpheroAdaptor, name string) *SpheroDriver {
}
s.AddEvent(Error)
s.AddEvent(ChannelCollisions)
s.AddEvent(ChannelSensordata)
s.AddEvent(Collision)
s.AddEvent(SensorData)
s.AddCommand("SetRGB", func(params map[string]interface{}) interface{} {
r := uint8(params["r"].(float64))
@ -106,13 +103,13 @@ func NewSpheroDriver(a *SpheroAdaptor, name string) *SpheroDriver {
})
s.AddCommand("SetDataStreaming", func(params map[string]interface{}) interface{} {
freq := params["freq"].(uint16)
frames := params["frames"].(uint16)
mask := params["mask"].(uint32)
count := params["count"].(uint8)
mask2 := params["mask2"].(uint32)
N := params["N"].(uint16)
M := params["M"].(uint16)
Mask := params["Mask"].(uint32)
Pcnt := params["Pcnt"].(uint8)
Mask2 := params["Mask2"].(uint32)
s.SetDataStreaming(freq, frames, mask, count, mask2)
s.SetDataStreaming(DataStreamingConfig{N: N, M: M, Mask2: Mask2, Pcnt: Pcnt, Mask: Mask})
return nil
})
@ -130,7 +127,9 @@ func (s *SpheroDriver) adaptor() *SpheroAdaptor {
// Returns true on successful start.
//
// Emits the Events:
// "collision" SpheroDriver.Collision - On Collision Detected
// 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() (errs []error) {
go func() {
for {
@ -184,17 +183,7 @@ func (s *SpheroDriver) Start() (errs []error) {
}
}()
if !s.cdConfigured {
fmt.Println("Using default collision detection parameters...")
s.ConfigureCollisionDetection(CollisionConfig{
Method: 0x01,
Xt: 0x80,
Yt: 0x80,
Xs: 0x80,
Ys: 0x80,
Dead: 0x60,
})
}
s.ConfigureCollisionDetection(DefaultCollisionConfig())
s.enableStopOnDisconnect()
return
@ -251,43 +240,20 @@ func (s *SpheroDriver) Roll(speed uint8, heading uint16) {
}
// Enables sensor data streaming
func (s *SpheroDriver) SetDataStreaming(freq uint16, frames uint16, mask uint32, count uint8, mask2 uint32) {
var n uint16
if freq == 0 || freq >= SensorFrequencyMax {
fmt.Printf("Invalid data streaming frequency. Setting to default (%v Hz)\n", SensorFrequencyDefault)
n = SensorFrequencyMax / SensorFrequencyDefault
} else {
n = SensorFrequencyMax / freq
}
if frames == 0 {
frames = 1
}
cmd := DataStreamingSetting{
N: n,
M: frames,
Pcnt: count,
Mask: mask,
Mask2: mask2,
}
func (s *SpheroDriver) SetDataStreaming(d DataStreamingConfig) {
buf := new(bytes.Buffer)
binary.Write(buf, binary.BigEndian, cmd)
binary.Write(buf, binary.BigEndian, d)
s.packetChannel <- s.craftPacket(buf.Bytes(), 0x02, 0x11)
}
// Stop sets the Sphero to a roll speed of 0 and disables data streaming
// Stop sets the Sphero to a roll speed of 0
func (s *SpheroDriver) Stop() {
s.SetDataStreaming(SensorFrequencyDefault, 0, 0, 0, 0)
s.Roll(0, 0)
}
// ConfigureCollisionDetection configures the sensitivity of the detection.
// https://github.com/orbotix/DeveloperResources/blob/master/docs/Collision%20detection%201.2.pdf.
func (s *SpheroDriver) ConfigureCollisionDetection(cc CollisionConfig) {
s.cdConfigured = true
s.packetChannel <- s.craftPacket([]uint8{cc.Method, cc.Xt, cc.Yt, cc.Xs, cc.Ys, cc.Dead}, 0x02, 0x12)
}
@ -303,7 +269,7 @@ func (s *SpheroDriver) handleCollisionDetected(data []uint8) {
var collision CollisionPacket
buffer := bytes.NewBuffer(data[5:]) // skip header
binary.Read(buffer, binary.BigEndian, &collision)
gobot.Publish(s.Event(ChannelCollisions), collision)
gobot.Publish(s.Event(Collision), collision)
}
func (s *SpheroDriver) handleDataStreaming(data []uint8) {
@ -314,7 +280,7 @@ func (s *SpheroDriver) handleDataStreaming(data []uint8) {
var dataPacket DataStreamingPacket
buffer := bytes.NewBuffer(data[5:]) // skip header
binary.Read(buffer, binary.BigEndian, &dataPacket)
gobot.Publish(s.Event(ChannelSensordata), dataPacket)
gobot.Publish(s.Event(SensorData), dataPacket)
}
func (s *SpheroDriver) getSyncResponse(packet *packet) []byte {

74
platforms/sphero/sphero_packets.go
View File

@ -1,5 +1,41 @@
package sphero
// 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 offical api specification https://github.com/orbotix/DeveloperResources/blob/master/docs/Collision%20detection%201.2.pdf.
type CollisionConfig struct {
// Detection method type to use. Methods 01h and 02h are supported as
// of FW ver 1.42. Use 00h to completely disable this service.
Method uint8
// An 8-bit settable threshold for the X (left/right) axes of Sphero.
// A value of 00h disables the contribution of that axis.
Xt uint8
// An 8-bit settable threshold for the Y (front/back) axes of Sphero.
// A value of 00h disables the contribution of that axis.
Yt uint8
// An 8-bit settable speed value for the X axes. This setting is ranged
// by the speed, then added to Xt to generate the final threshold value.
Xs uint8
// An 8-bit settable speed value for the Y axes. This setting is ranged
// by the speed, then added to Yt to generate the final threshold value.
Ys uint8
// An 8-bit post-collision dead time to prevent retriggering; specified
// in 10ms increments.
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
@ -13,7 +49,20 @@ type CollisionPacket struct {
Timestamp uint32
}
type DataStreamingSetting struct {
// 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 offical api specification https://github.com/orbotix/DeveloperResources/blob/master/docs/Sphero_API_1.50.pdf page 28
type DataStreamingConfig struct {
// Divisor of the maximum sensor sampling rate
N uint16
// Number of sample frames emitted per packet
@ -26,6 +75,7 @@ type DataStreamingSetting struct {
Mask2 uint32
}
// DataStreamingPacket represents the response from a Data Streaming event
type DataStreamingPacket struct {
// 8000 0000h accelerometer axis X, raw -2048 to 2047 4mG
RawAccX int16
@ -110,25 +160,3 @@ type DataStreamingPacket struct {
// 0080 0000h Velocity Y -32768 to 32767 mm/s
VeloY int16
}
// CollisionConfig provides configuration for the collision detection alogorithm.
type CollisionConfig struct {
// Detection method type to use. Methods 01h and 02h are supported as
// of FW ver 1.42. Use 00h to completely disable this service.
Method uint8
// An 8-bit settable threshold for the X (left/right) axes of Sphero.
// A value of 00h disables the contribution of that axis.
Xt uint8
// An 8-bit settable threshold for the Y (front/back) axes of Sphero.
// A value of 00h disables the contribution of that axis.
Yt uint8
// An 8-bit settable speed value for the X axes. This setting is ranged
// by the speed, then added to Xt to generate the final threshold value.
Xs uint8
// An 8-bit settable speed value for the Y axes. This setting is ranged
// by the speed, then added to Yt to generate the final threshold value.
Ys uint8
// An 8-bit post-collision dead time to prevent retriggering; specified
// in 10ms increments.
Dead uint8
}