Skip to content

Commit

Permalink
core: Continue refactoring to allow 'metal' development using Gobot l…
Browse files Browse the repository at this point in the history
…ibs.

Signed-off-by: deadprogram <[email protected]>
  • Loading branch information
deadprogram committed Sep 12, 2016
1 parent 0e25f29 commit e159613
Show file tree
Hide file tree
Showing 8 changed files with 46 additions and 40 deletions.
4 changes: 4 additions & 0 deletions eventer.go
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,10 @@ type Eventer interface {
Publish(name string, data interface{})
// Subscribe to any events from this eventer
Subscribe() (events eventChannel)
// Event handler
On(name string, f func(s interface{})) (err error)
// Event handler, only exectues one time
Once(name string, f func(s interface{})) (err error)
}

// NewEventer returns a new Eventer.
Expand Down
28 changes: 14 additions & 14 deletions platforms/firmata/client/client.go
Original file line number Diff line number Diff line change
Expand Up @@ -143,19 +143,19 @@ func (b *Client) Connect(conn io.ReadWriteCloser) (err error) {

initFunc := b.ProtocolVersionQuery

gobot.Once(b.Event("ProtocolVersion"), func(data interface{}) {
b.Once(b.Event("ProtocolVersion"), func(data interface{}) {
initFunc = b.FirmwareQuery
})

gobot.Once(b.Event("FirmwareQuery"), func(data interface{}) {
b.Once(b.Event("FirmwareQuery"), func(data interface{}) {
initFunc = b.CapabilitiesQuery
})

gobot.Once(b.Event("CapabilityQuery"), func(data interface{}) {
b.Once(b.Event("CapabilityQuery"), func(data interface{}) {
initFunc = b.AnalogMappingQuery
})

gobot.Once(b.Event("AnalogMappingQuery"), func(data interface{}) {
b.Once(b.Event("AnalogMappingQuery"), func(data interface{}) {
initFunc = func() error { return nil }
b.ReportDigital(0, 1)
b.ReportDigital(1, 1)
Expand All @@ -177,7 +177,7 @@ func (b *Client) Connect(conn io.ReadWriteCloser) (err error) {
}

if err := b.process(); err != nil {
gobot.Publish(b.Event("Error"), err)
b.Publish(b.Event("Error"), err)
}
}
}()
Expand Down Expand Up @@ -343,7 +343,7 @@ func (b *Client) process() (err error) {
case ProtocolVersion == messageType:
b.ProtocolVersion = fmt.Sprintf("%v.%v", buf[1], buf[2])

gobot.Publish(b.Event("ProtocolVersion"), b.ProtocolVersion)
b.Publish(b.Event("ProtocolVersion"), b.ProtocolVersion)
case AnalogMessageRangeStart <= messageType &&
AnalogMessageRangeEnd >= messageType:

Expand All @@ -353,7 +353,7 @@ func (b *Client) process() (err error) {
if len(b.analogPins) > pin {
if len(b.pins) > b.analogPins[pin] {
b.pins[b.analogPins[pin]].Value = int(value)
gobot.Publish(b.Event(fmt.Sprintf("AnalogRead%v", pin)), b.pins[b.analogPins[pin]].Value)
b.Publish(b.Event(fmt.Sprintf("AnalogRead%v", pin)), b.pins[b.analogPins[pin]].Value)
}
}
case DigitalMessageRangeStart <= messageType &&
Expand All @@ -367,7 +367,7 @@ func (b *Client) process() (err error) {
if len(b.pins) > pinNumber {
if b.pins[pinNumber].Mode == Input {
b.pins[pinNumber].Value = int((portValue >> (byte(i) & 0x07)) & 0x01)
gobot.Publish(b.Event(fmt.Sprintf("DigitalRead%v", pinNumber)), b.pins[pinNumber].Value)
b.Publish(b.Event(fmt.Sprintf("DigitalRead%v", pinNumber)), b.pins[pinNumber].Value)
}
}
}
Expand Down Expand Up @@ -412,7 +412,7 @@ func (b *Client) process() (err error) {
}
n ^= 1
}
gobot.Publish(b.Event("CapabilityQuery"), nil)
b.Publish(b.Event("CapabilityQuery"), nil)
case AnalogMappingResponse:
pinIndex := 0
b.analogPins = []int{}
Expand All @@ -427,7 +427,7 @@ func (b *Client) process() (err error) {
b.AddEvent(fmt.Sprintf("AnalogRead%v", pinIndex))
pinIndex++
}
gobot.Publish(b.Event("AnalogMappingQuery"), nil)
b.Publish(b.Event("AnalogMappingQuery"), nil)
case PinStateResponse:
pin := currentBuffer[2]
b.pins[pin].Mode = int(currentBuffer[3])
Expand All @@ -440,7 +440,7 @@ func (b *Client) process() (err error) {
b.pins[pin].State = int(uint(b.pins[pin].State) | uint(currentBuffer[6])<<14)
}

gobot.Publish(b.Event(fmt.Sprintf("PinState%v", pin)), b.pins[pin])
b.Publish(b.Event(fmt.Sprintf("PinState%v", pin)), b.pins[pin])
case I2CReply:
reply := I2cReply{
Address: int(byte(currentBuffer[2]) | byte(currentBuffer[3])<<7),
Expand All @@ -458,7 +458,7 @@ func (b *Client) process() (err error) {
byte(currentBuffer[i])|byte(currentBuffer[i+1])<<7,
)
}
gobot.Publish(b.Event("I2cReply"), reply)
b.Publish(b.Event("I2cReply"), reply)
case FirmwareQuery:
name := []byte{}
for _, val := range currentBuffer[4:(len(currentBuffer) - 1)] {
Expand All @@ -467,10 +467,10 @@ func (b *Client) process() (err error) {
}
}
b.FirmwareName = string(name[:])
gobot.Publish(b.Event("FirmwareQuery"), b.FirmwareName)
b.Publish(b.Event("FirmwareQuery"), b.FirmwareName)
case StringData:
str := currentBuffer[2:]
gobot.Publish(b.Event("StringData"), string(str[:len(str)-1]))
b.Publish(b.Event("StringData"), string(str[:len(str)-1]))
}
}
return
Expand Down
10 changes: 5 additions & 5 deletions platforms/firmata/client/client_test.go
Original file line number Diff line number Diff line change
Expand Up @@ -174,7 +174,7 @@ func TestProcess(t *testing.T) {

for _, test := range tests {
test.init()
gobot.Once(b.Event(test.event), func(data interface{}) {
b.Once(b.Event(test.event), func(data interface{}) {
gobottest.Assert(t, data, test.expected)
sem <- true
})
Expand Down Expand Up @@ -202,19 +202,19 @@ func TestConnect(t *testing.T) {
}
}()

gobot.Once(b.Event("ProtocolVersion"), func(data interface{}) {
b.Once(b.Event("ProtocolVersion"), func(data interface{}) {
response = testFirmwareResponse()
})

gobot.Once(b.Event("FirmwareQuery"), func(data interface{}) {
b.Once(b.Event("FirmwareQuery"), func(data interface{}) {
response = testCapabilitiesResponse()
})

gobot.Once(b.Event("CapabilityQuery"), func(data interface{}) {
b.Once(b.Event("CapabilityQuery"), func(data interface{}) {
response = testAnalogMappingResponse()
})

gobot.Once(b.Event("AnalogMappingQuery"), func(data interface{}) {
b.Once(b.Event("AnalogMappingQuery"), func(data interface{}) {
response = testProtocolResponse()
})

Expand Down
6 changes: 4 additions & 2 deletions platforms/firmata/firmata_adaptor.go
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ type firmataBoard interface {
I2cWrite(int, []byte) error
I2cConfig(int) error
ServoConfig(int, int, int) error
Event(string) *gobot.Event
Event(string) string
}

// FirmataAdaptor is the Gobot Adaptor for Firmata based boards
Expand All @@ -33,6 +33,7 @@ type FirmataAdaptor struct {
board firmataBoard
conn io.ReadWriteCloser
openSP func(port string) (io.ReadWriteCloser, error)
gobot.Eventer
}

// NewFirmataAdaptor returns a new FirmataAdaptor with specified name and optionally accepts:
Expand All @@ -53,6 +54,7 @@ func NewFirmataAdaptor(name string, args ...interface{}) *FirmataAdaptor {
openSP: func(port string) (io.ReadWriteCloser, error) {
return serial.OpenPort(&serial.Config{Name: port, Baud: 57600})
},
Eventer: gobot.NewEventer(),
}

for _, arg := range args {
Expand Down Expand Up @@ -230,7 +232,7 @@ func (f *FirmataAdaptor) I2cRead(address int, size int) (data []byte, err error)
return
}

gobot.Once(f.board.Event("I2cReply"), func(data interface{}) {
f.Once(f.board.Event("I2cReply"), func(data interface{}) {
ret <- data.(client.I2cReply).Data
})

Expand Down
2 changes: 1 addition & 1 deletion platforms/firmata/firmata_adaptor_test.go
Original file line number Diff line number Diff line change
Expand Up @@ -173,7 +173,7 @@ func TestFirmataAdaptorI2cRead(t *testing.T) {
i2cReply := client.I2cReply{Data: i}
go func() {
<-time.After(10 * time.Millisecond)
gobot.Publish(a.board.Event("I2cReply"), i2cReply)
a.Publish(a.board.Event("I2cReply"), i2cReply)
}()
data, err := a.I2cRead(0x00, 1)
gobottest.Assert(t, err, nil)
Expand Down
6 changes: 3 additions & 3 deletions platforms/i2c/mpl115a2_driver.go
Original file line number Diff line number Diff line change
Expand Up @@ -72,20 +72,20 @@ func (h *MPL115A2Driver) Start() (errs []error) {
go func() {
for {
if err := h.connection.I2cWrite(mpl115a2Address, []byte{MPL115A2_REGISTER_STARTCONVERSION, 0}); err != nil {
gobot.Publish(h.Event(Error), err)
h.Publish(h.Event(Error), err)
continue

}
<-time.After(5 * time.Millisecond)

if err := h.connection.I2cWrite(mpl115a2Address, []byte{MPL115A2_REGISTER_PRESSURE_MSB}); err != nil {
gobot.Publish(h.Event(Error), err)
h.Publish(h.Event(Error), err)
continue
}

ret, err := h.connection.I2cRead(mpl115a2Address, 4)
if err != nil {
gobot.Publish(h.Event(Error), err)
h.Publish(h.Event(Error), err)
continue
}
if len(ret) == 4 {
Expand Down
16 changes: 8 additions & 8 deletions platforms/i2c/mpu6050_driver.go
Original file line number Diff line number Diff line change
Expand Up @@ -74,13 +74,13 @@ func (h *MPU6050Driver) Start() (errs []error) {
go func() {
for {
if err := h.connection.I2cWrite(mpu6050Address, []byte{MPU6050_RA_ACCEL_XOUT_H}); err != nil {
gobot.Publish(h.Event(Error), err)
h.Publish(h.Event(Error), err)
continue
}

ret, err := h.connection.I2cRead(mpu6050Address, 14)
if err != nil {
gobot.Publish(h.Event(Error), err)
h.Publish(h.Event(Error), err)
continue
}
buf := bytes.NewBuffer(ret)
Expand Down Expand Up @@ -111,12 +111,12 @@ func (h *MPU6050Driver) initialize() (err error) {
}

// setFullScaleGyroRange
if err = h.connection.I2cWrite(mpu6050Address, []byte{MPU6050_RA_GYRO_CONFIG,
MPU6050_GCONFIG_FS_SEL_BIT,
MPU6050_GCONFIG_FS_SEL_LENGTH,
MPU6050_GYRO_FS_250}); err != nil {
return
}
if err = h.connection.I2cWrite(mpu6050Address, []byte{MPU6050_RA_GYRO_CONFIG,
MPU6050_GCONFIG_FS_SEL_BIT,
MPU6050_GCONFIG_FS_SEL_LENGTH,
MPU6050_GYRO_FS_250}); err != nil {
return
}

// setFullScaleAccelRange
if err = h.connection.I2cWrite(mpu6050Address, []byte{MPU6050_RA_ACCEL_CONFIG,
Expand Down
14 changes: 7 additions & 7 deletions platforms/i2c/wiichuck_driver.go
Original file line number Diff line number Diff line change
Expand Up @@ -69,23 +69,23 @@ func (w *WiichuckDriver) Start() (errs []error) {
go func() {
for {
if err := w.connection.I2cWrite(wiichuckAddress, []byte{0x40, 0x00}); err != nil {
gobot.Publish(w.Event(Error), err)
w.Publish(w.Event(Error), err)
continue
}
<-time.After(w.pauseTime)
if err := w.connection.I2cWrite(wiichuckAddress, []byte{0x00}); err != nil {
gobot.Publish(w.Event(Error), err)
w.Publish(w.Event(Error), err)
continue
}
<-time.After(w.pauseTime)
newValue, err := w.connection.I2cRead(wiichuckAddress, 6)
if err != nil {
gobot.Publish(w.Event(Error), err)
w.Publish(w.Event(Error), err)
continue
}
if len(newValue) == 6 {
if err = w.update(newValue); err != nil {
gobot.Publish(w.Event(Error), err)
w.Publish(w.Event(Error), err)
continue
}
}
Expand Down Expand Up @@ -146,16 +146,16 @@ func (w *WiichuckDriver) adjustOrigins() {
// updateButtons publishes "c" and "x" events if present in data
func (w *WiichuckDriver) updateButtons() {
if w.data["c"] == 0 {
gobot.Publish(w.Event(C), true)
w.Publish(w.Event(C), true)
}
if w.data["z"] == 0 {
gobot.Publish(w.Event(Z), true)
w.Publish(w.Event(Z), true)
}
}

// updateJoystick publishes event with current x and y values for joystick
func (w *WiichuckDriver) updateJoystick() {
gobot.Publish(w.Event(Joystick), map[string]float64{
w.Publish(w.Event(Joystick), map[string]float64{
"x": w.calculateJoystickValue(w.data["sx"], w.joystick["sx_origin"]),
"y": w.calculateJoystickValue(w.data["sy"], w.joystick["sy_origin"]),
})
Expand Down

0 comments on commit e159613

Please sign in to comment.