From 12356d38401f4fa08075ed7bb5281223f0d73596 Mon Sep 17 00:00:00 2001 From: imle Date: Mon, 4 Oct 2021 02:35:35 -0400 Subject: [PATCH 1/4] Working firmata and onewire --- firmata/analog_mapping.go | 27 ++ firmata/capability.go | 51 +++ firmata/client.go | 697 +++++++++++++++++++++++++++++++ firmata/errors.go | 24 ++ firmata/feature.go | 7 + firmata/firmatareg/firmatareg.go | 216 ++++++++++ firmata/firmware.go | 15 + firmata/i2c.go | 33 ++ firmata/i2c_packet.go | 20 + firmata/message.go | 50 +++ firmata/onewire.go | 181 ++++++++ firmata/onewire_cmd.go | 27 ++ firmata/pin.go | 233 +++++++++++ firmata/pin_mode.go | 62 +++ firmata/pin_state.go | 17 + firmata/sysex.go | 109 +++++ firmata/util.go | 114 +++++ firmata/util_test.go | 129 ++++++ 18 files changed, 2012 insertions(+) create mode 100644 firmata/analog_mapping.go create mode 100644 firmata/capability.go create mode 100644 firmata/client.go create mode 100644 firmata/errors.go create mode 100644 firmata/feature.go create mode 100644 firmata/firmatareg/firmatareg.go create mode 100644 firmata/firmware.go create mode 100644 firmata/i2c.go create mode 100644 firmata/i2c_packet.go create mode 100644 firmata/message.go create mode 100644 firmata/onewire.go create mode 100644 firmata/onewire_cmd.go create mode 100644 firmata/pin.go create mode 100644 firmata/pin_mode.go create mode 100644 firmata/pin_state.go create mode 100644 firmata/sysex.go create mode 100644 firmata/util.go create mode 100644 firmata/util_test.go diff --git a/firmata/analog_mapping.go b/firmata/analog_mapping.go new file mode 100644 index 0000000..83a9944 --- /dev/null +++ b/firmata/analog_mapping.go @@ -0,0 +1,27 @@ +package firmata + +import ( + "bytes" + "fmt" +) + +type AnalogMappingResponse struct { + AnalogPinToDigital []uint8 + DigitalPinToAnalog map[uint8]uint8 +} + +func (a AnalogMappingResponse) String() string { + str := bytes.Buffer{} + for analogPin, digitalPin := range a.AnalogPinToDigital { + _, _ = fmt.Fprintf(&str, "A%d: %d\n", analogPin, digitalPin) + } + return str.String() +} + +type ExtendedAnalogMappingResponse struct { + Pin uint8 +} + +func (a ExtendedAnalogMappingResponse) String() string { + return fmt.Sprintf("%d", a.Pin) +} diff --git a/firmata/capability.go b/firmata/capability.go new file mode 100644 index 0000000..c430a2e --- /dev/null +++ b/firmata/capability.go @@ -0,0 +1,51 @@ +package firmata + +import ( + "bytes" + "fmt" + + "periph.io/x/conn/v3/pin" +) + +var pinModeOrder = []pin.Func{ + PinFuncDigitalInput, + PinFuncDigitalOutput, + PinFuncAnalogInput, + PinFuncPWM, + PinFuncServo, + PinFuncShift, + PinFuncI2C, + PinFuncOneWire, + PinFuncStepper, + PinFuncEncoder, + PinFuncSerial, + PinFuncInputPullUp, + PinFuncSPI, + PinFuncSonar, + PinFuncTone, + PinFuncDHT, +} + +const CapabilityResponsePinDelimiter = 0x7F + +type CapabilityResponse struct { + PinToModeToResolution []map[pin.Func]uint8 + SupportedPinModes [][]pin.Func +} + +func (c CapabilityResponse) String() string { + str := bytes.Buffer{} + for p, modeMap := range c.PinToModeToResolution { + _, _ = fmt.Fprintf(&str, "pin %2v: [", p) + if len(modeMap) > 0 { + for _, mode := range pinModeOrder { + if resolution, ok := modeMap[mode]; ok { + _, _ = fmt.Fprintf(&str, "%s: %d, ", mode, resolution) + } + } + str.Truncate(str.Len() - 2) + } + _, _ = fmt.Fprintf(&str, "]\n") + } + return str.String() +} diff --git a/firmata/client.go b/firmata/client.go new file mode 100644 index 0000000..08e0e5b --- /dev/null +++ b/firmata/client.go @@ -0,0 +1,697 @@ +package firmata + +import ( + "bufio" + "errors" + "fmt" + "io" + "sync" + + "periph.io/x/conn/v3/gpio" + "periph.io/x/conn/v3/onewire" + "periph.io/x/conn/v3/pin" +) + +// These max values are for data bytes as, within firmata, data is 7 bits long. +const ( + MaxUInt8 uint8 = (1<<8 - 1) >> 1 + MaxUInt16 uint16 = (1<<16 - 1) >> 2 +) + +var commandResponseMap = map[SysExCmd]SysExCmd{ + SysExAnalogMappingQuery: SysExAnalogMappingResponse, + SysExCapabilityQuery: SysExCapabilityResponse, + SysExPinStateQuery: SysExPinStateResponse, +} + +type ClientI interface { + SendSysEx(SysExCmd, ...byte) (chan []byte, error) + SendReset() error + ExtendedReportAnalogPin(uint8, int) error + CapabilityQuery() (chan CapabilityResponse, error) + PinStateQuery(uint8) (chan PinStateResponse, error) + ReportFirmware() (chan FirmwareReport, error) + SetPinMode(uint8, pin.Func) error + SetAnalogPinReporting(uint8, bool) error + SetDigitalPinReporting(uint8, bool) error + SetDigitalPortReporting(uint8, bool) error + SetSamplingInterval(uint16) error + SetDigitalPinValue(p uint8, value gpio.Level) error + SendAnalogMappingQuery() (chan AnalogMappingResponse, error) + SetI2CMessageChannel(address uint8, ch chan I2CPacket) + WriteI2CData(address uint8, restart bool, data []uint8) error + ReadI2CData(address uint8, restart bool, len uint16) error + ReadI2CRegister(address uint8, restart bool, register uint8, len uint16) error + SendI2CConfig(delayMicroseconds uint8) error + AnalogPinToDigitalPin(p uint8) (uint8, error) + SetAnalogIOMessageListener(p uint8, ch chan uint16) (release func(), err error) + SetDigitalIOMessageListener(p uint8, ch chan gpio.Level) (release func(), err error) + SendAnalogIOMessage(uint8, uint16) error + + OpenOneWireBus(p uint8) (bus onewire.BusCloser, err error) + SetOneWireListener(uint8, chan []byte) (release func(), err error) + + GetPinName(uint8) string + GetPinFunctions(uint8) []pin.Func + + Close() error +} + +type Client struct { + board io.ReadWriteCloser + responseChannels map[SysExCmd][]chan []byte + sysExListenerChannels map[SysExCmd]chan []byte + i2cListeners map[uint8]chan I2CPacket + + onewireListeners map[uint8]chan []byte + onewireMU sync.Mutex + + digitalIOMessageChannels map[uint8]chan gpio.Level + digitalPinMU sync.Mutex + analogIOMessageChannels map[uint8]chan uint16 + analogPinMU sync.Mutex + + mu sync.Mutex + started bool + i2cStarted bool + + // We want to report these to the requester, but also save them for internal use. + cr CapabilityResponse + amr AnalogMappingResponse +} + +func NewClient(board io.ReadWriteCloser) *Client { + return &Client{ + board: board, + responseChannels: map[SysExCmd][]chan []byte{}, + sysExListenerChannels: map[SysExCmd]chan []byte{}, + i2cListeners: map[uint8]chan I2CPacket{}, + + onewireListeners: map[uint8]chan []byte{}, + + digitalIOMessageChannels: map[uint8]chan gpio.Level{}, + analogIOMessageChannels: map[uint8]chan uint16{}, + } +} + +func (c *Client) Close() error { + c.mu.Lock() + defer c.mu.Unlock() + + c.started = false + return c.board.Close() +} + +type flusher interface { + Flush() +} + +type flusherErr interface { + Flush() error +} + +func (c *Client) Start() error { + c.mu.Lock() + if c.started { + c.mu.Unlock() + return ErrAlreadyStarted + } + c.started = true + c.mu.Unlock() + + if b, ok := c.board.(flusher); ok { + b.Flush() + } else if b, ok := c.board.(flusherErr); ok { + if err := b.Flush(); err != nil { + return err + } + } + + firmChannel := make(chan []byte, 1) + // Don't call ReportFirmware as it is automatic, but we want to register a listener for it. + c.mu.Lock() + c.responseChannels[SysExReportFirmware] = []chan []byte{firmChannel} + c.mu.Unlock() + report := c.parseReportFirmware(firmChannel) + + go func() { + err := c.responseWatcher() + if err != nil { + panic(err) + } + }() + + fmt.Println("Firmware Info:", <-report) + + return nil +} + +func (c *Client) write(payload []byte, withinMutex func()) error { + // Cannot allow multiple writes at the same time. + c.mu.Lock() + defer c.mu.Unlock() + + //fmt.Println(SprintHexArray(payload)) + + // Write to the board. + _, err := c.board.Write(payload) + if err != nil { + return err + } + + if withinMutex != nil { + withinMutex() + } + + return nil +} + +func (c *Client) responseWatcher() (err error) { + defer func() { + if errors.Is(err, io.EOF) { + err = ErrDeviceDisconnected + } + }() + + reader := bufio.NewReader(c.board) + for { + var data []byte + b0, err := reader.ReadByte() + if err != nil { + return err + } + + mt := MessageType(b0) + switch { + case mt == ProtocolVersion: + var version [2]byte + _, err := reader.Read(version[:]) + if err != nil { + return err + } + + fmt.Printf("Protocol Version: 0x%0.2X 0x%0.2X\n", version[0], version[1]) + case AnalogIOMessage <= mt && mt <= (AnalogIOMessage+0xF): + v1, err := reader.ReadByte() + if err != nil { + return err + } + v2, err := reader.ReadByte() + if err != nil { + return err + } + + c.analogIOMessageChannels[b0&0xF] <- TwoByteToWord(v1, v2) + case DigitalIOMessage <= mt && mt <= (DigitalIOMessage+0xF): + v1, err := reader.ReadByte() + if err != nil { + return err + } + v2, err := reader.ReadByte() + if err != nil { + return err + } + + values := TwoByteToByte(v1, v2) + + port := b0 & 0xF + pinMin := port * 8 + pinMax := (port+1)*8 - 1 + for p := pinMin; p <= pinMax; p++ { + if ch, ok := c.digitalIOMessageChannels[p]; ok { + lvl := gpio.Low + if values>>p%8 > 0 { + lvl = gpio.High + } + ch <- lvl + } + } + case mt == StartSysEx: + data, err = reader.ReadBytes(byte(EndSysEx)) + if err != nil { + return err + } + + if len(data) == 0 { + return ErrNoDataRead + } + + cmd := SysExCmd(data[0]) + data = data[1 : len(data)-1] + + switch { + case cmd == SysExSerialDataV1: + fallthrough + case cmd == SysExSerialDataV2: + return fmt.Errorf("%w: %s", ErrUnsupportedFeature, cmd) + case cmd == SysExOneWireData: + p := data[1] + + if l, ok := c.onewireListeners[p]; ok { + l <- data + } else { + return fmt.Errorf("%w: onewire cmd:0x%02X pin 0x%02X", ErrUnhandledMessage, data[0], p) + } + case cmd == SysExI2CReply: + address := TwoByteToByte(data[0], data[1]) + register := TwoByteToByte(data[2], data[3]) + ch, ok := c.i2cListeners[address] + if !ok { + return fmt.Errorf("%w: 0x%02X", ErrNoI2CListenerForAddress, address) + } + + ch <- I2CPacket{ + Register: register, + Data: TwoByteRepresentationToByteSlice(data[4:]), + } + case c.sysExListenerChannels[cmd] != nil: + c.sysExListenerChannels[cmd] <- data + case len(c.responseChannels[cmd]) != 0: + c.mu.Lock() + resp := c.responseChannels[cmd][0] + c.responseChannels[cmd] = c.responseChannels[cmd] + c.mu.Unlock() + + resp <- data + close(resp) + case cmd == SysExStringData: + fmt.Printf("device: [%s]\n", TwoByteString(data)) + default: + str := "" + if cmd == SysExStringData { + str = TwoByteString(data) + } else { + for _, b := range data { + str += fmt.Sprintf("%d", b) + } + } + + return fmt.Errorf("%w: 0x%0.2X: %s", ErrUnexpectedSysExMessageTypeReceived, byte(cmd), str) + } + default: + return fmt.Errorf("%w: 0x%0.2X", ErrInvalidMessageTypeStart, b0) + } + } +} + +func (c *Client) SendReset() error { + return c.write([]byte{byte(SystemReset)}, nil) +} + +func (c *Client) AnalogPinToDigitalPin(p uint8) (uint8, error) { + if int(p) > len(c.amr.AnalogPinToDigital) { + return 0, ErrInvalidAnalogPin + } + + return c.amr.AnalogPinToDigital[p], nil +} + +func (c *Client) SetPinMode(p uint8, mode pin.Func) error { + return c.write([]uint8{uint8(SetPinMode), p, pinFuncToModeMap[mode]}, nil) +} + +func (c *Client) SetDigitalPinValue(p uint8, value gpio.Level) error { + v := byte(0) + if value { + v = 1 + } + return c.write([]uint8{uint8(SetDigitalPinValue), p, v}, nil) +} + +func (c *Client) SendSysEx(cmd SysExCmd, payload ...byte) (chan []byte, error) { + // Create a response channel. + var data chan []byte + + err := c.write(append([]byte{byte(StartSysEx), byte(cmd)}, append(payload, byte(EndSysEx))...), func() { + // This assumes that SysEx commands of the same type are responded to in order. + if resp, ok := commandResponseMap[cmd]; ok { + data = make(chan []byte, 1) + c.responseChannels[resp] = append(c.responseChannels[resp], data) + } + }) + if err != nil { + return nil, err + } + + return data, nil +} + +func (c *Client) CapabilityQuery() (chan CapabilityResponse, error) { + future, err := c.SendSysEx(SysExCapabilityQuery) + if err != nil { + return nil, err + } + + resp := c.parseCapabilityCommand(future) + + return resp, nil +} + +func (c *Client) parseCapabilityCommand(future chan []byte) chan CapabilityResponse { + resp := make(chan CapabilityResponse, 1) + + go func() { + data := <-future + var response = CapabilityResponse{ + PinToModeToResolution: []map[pin.Func]uint8{{}}, + SupportedPinModes: [][]pin.Func{{}}, + } + var pindex = 0 + for i := 0; i < len(data); { + if data[i] == CapabilityResponsePinDelimiter { + response.PinToModeToResolution = append(response.PinToModeToResolution, map[pin.Func]uint8{}) + response.SupportedPinModes = append(response.SupportedPinModes, []pin.Func{}) + i += 1 + pindex++ + } else { + pinFunc := pinModeToFuncMap[data[i]] + response.PinToModeToResolution[pindex][pinFunc] = data[i+1] + response.SupportedPinModes[pindex] = append(response.SupportedPinModes[pindex], pinFunc) + i += 2 + } + } + + c.cr = response + resp <- response + close(resp) + }() + + return resp +} + +func (c *Client) SendAnalogMappingQuery() (chan AnalogMappingResponse, error) { + future, err := c.SendSysEx(SysExAnalogMappingQuery) + if err != nil { + return nil, err + } + + resp := c.parseAnalogMappingQuery(future) + + return resp, nil +} + +func (c *Client) parseAnalogMappingQuery(future chan []byte) chan AnalogMappingResponse { + resp := make(chan AnalogMappingResponse, 1) + + go func() { + data := <-future + var response = AnalogMappingResponse{ + AnalogPinToDigital: []uint8{}, + DigitalPinToAnalog: map[uint8]uint8{}, + } + for i := 0; i < len(data); i++ { + if data[i] != CapabilityResponsePinDelimiter { + response.DigitalPinToAnalog[uint8(i)] = uint8(len(response.AnalogPinToDigital)) + response.AnalogPinToDigital = append(response.AnalogPinToDigital, uint8(i)) + } + } + + c.amr = response + resp <- response + close(resp) + }() + + return resp +} + +func (c *Client) ExtendedReportAnalogPin(p uint8, value int) error { + if value > 0xFFFFFFFFFFFFFF { + return fmt.Errorf("%w: 0x0 - 0xFFFFFFFFFFFFFF", ErrValueOutOfRange) + } + + _, err := c.SendSysEx(SysExExtendedAnalog, p, uint8(value), uint8(value>>7), uint8(value>>14)) + if err != nil { + return err + } + + return nil +} + +func (c *Client) PinStateQuery(p uint8) (chan PinStateResponse, error) { + future, err := c.SendSysEx(SysExPinStateQuery, p) + if err != nil { + return nil, err + } + + resp := c.parsePinStateQuery(future) + + return resp, nil +} + +func (c *Client) parsePinStateQuery(future chan []byte) chan PinStateResponse { + resp := make(chan PinStateResponse, 1) + + go func() { + data := <-future + var ps = PinStateResponse{ + Pin: data[0], + Mode: pinModeToFuncMap[data[1]], + State: 0, + } + + for i, b := range data[2:] { + ps.State |= int(b << (i * 7)) + } + + resp <- ps + close(resp) + }() + + return resp +} + +func (c *Client) ReportFirmware() (chan FirmwareReport, error) { + future, err := c.SendSysEx(SysExReportFirmware) + if err != nil { + return nil, err + } + + resp := c.parseReportFirmware(future) + + return resp, nil +} + +func (c *Client) parseReportFirmware(future chan []byte) chan FirmwareReport { + resp := make(chan FirmwareReport, 1) + + go func() { + data := <-future + var rc = FirmwareReport{ + Major: data[0], + Minor: data[1], + Name: data[2:], + } + + resp <- rc + close(resp) + }() + + return resp +} + +func (c *Client) SetAnalogPinReporting(analogPin uint8, report bool) error { + v := byte(0) + if report { + v = 1 + } + + return c.write([]byte{byte(ReportAnalogPin) | (analogPin & 0xF), v}, nil) +} + +func (c *Client) SetDigitalPinReporting(p uint8, report bool) error { + return c.SetDigitalPortReporting(p%8, report) +} + +func (c *Client) SetDigitalPortReporting(port uint8, report bool) error { + v := byte(0) + if report { + v = 1 + } + + return c.write([]byte{byte(ReportDigitalPort) | (port & 0xF), v}, nil) +} + +func (c *Client) SetSamplingInterval(ms uint16) error { + if ms > MaxUInt16 { + return fmt.Errorf("%w: 0x0 - 0x%X", ErrValueOutOfRange, MaxUInt16) + } + return c.write([]byte{byte(SysExSamplingInterval), byte(ms), byte(ms >> 7)}, nil) +} + +// This function only supports 7-bit I2C addresses +func (c *Client) WriteI2CData(address uint8, restart bool, data []uint8) error { + if !c.i2cStarted { + return ErrI2CNotEnabled + } + + byte2 := byte(I2CModeWrite) + if restart { + byte2 &= I2CRestartTransmission + } + + payload := append([]byte{address, byte2}, ByteSliceToTwoByteRepresentation(data)...) + _, err := c.SendSysEx(SysExI2CRequest, payload...) + return err +} + +// This function only supports 7-bit I2C addresses +func (c *Client) ReadI2CData(address uint8, restart bool, length uint16) error { + if !c.i2cStarted { + return ErrI2CNotEnabled + } + + if length > MaxUInt16 { + return fmt.Errorf("%w: 0x0 - 0xFFFFFFFFFFFFFF", ErrValueOutOfRange) + } + + byte2 := byte(I2CModeRead) + if restart { + byte2 &= I2CRestartTransmission + } + + lLSB, lMSB := WordToTwoByte(length) + + _, err := c.SendSysEx(SysExI2CRequest, address, byte2, lLSB, lMSB) + return err +} + +// This function only supports 7-bit I2C addresses +func (c *Client) ReadI2CRegister(address uint8, restart bool, register uint8, length uint16) error { + if !c.i2cStarted { + return ErrI2CNotEnabled + } + + if length > MaxUInt16 { + return fmt.Errorf("%w: 0x0 - 0xFFFFFFFFFFFFFF", ErrValueOutOfRange) + } + + byte2 := byte(I2CModeRead) + if restart { + byte2 &= I2CRestartTransmission + } + + rLSB, rMSB := ByteToTwoByte(register) + lLSB, lMSB := WordToTwoByte(length) + + _, err := c.SendSysEx(SysExI2CRequest, address, byte2, rLSB, rMSB, lLSB, lMSB) + return err +} + +func (c *Client) SendI2CConfig(delayMicroseconds uint8) error { + micLSB, micMSB := ByteToTwoByte(delayMicroseconds) + _, err := c.SendSysEx(SysExI2CConfig, micLSB, micMSB) + if err != nil { + return err + } + c.i2cStarted = true + return nil +} + +// This function only supports 7-bit I2C addresses +func (c *Client) SetI2CMessageChannel(address uint8, ch chan I2CPacket) { + if c.i2cListeners[address] != nil { + close(c.i2cListeners[address]) + } + + c.i2cListeners[address] = ch +} + +func (c *Client) releaseAnalogIOMessageListener(p uint8) { + c.analogPinMU.Lock() + defer c.analogPinMU.Unlock() + + delete(c.analogIOMessageChannels, p) +} + +func (c *Client) SetAnalogIOMessageListener(p uint8, ch chan uint16) (release func(), err error) { + c.analogPinMU.Lock() + defer c.analogPinMU.Unlock() + + if c.analogIOMessageChannels[p] != nil { + return nil, ErrPinListenerNotReleased + } + + c.analogIOMessageChannels[p] = ch + + return func() { c.releaseAnalogIOMessageListener(p) }, nil +} + +func (c *Client) releaseDigitalIOMessageListener(p uint8) { + c.digitalPinMU.Lock() + defer c.digitalPinMU.Unlock() + + delete(c.digitalIOMessageChannels, p) +} + +func (c *Client) SetDigitalIOMessageListener(p uint8, ch chan gpio.Level) (release func(), err error) { + c.digitalPinMU.Lock() + defer c.digitalPinMU.Unlock() + + if c.digitalIOMessageChannels[p] != nil { + return nil, ErrPinListenerNotReleased + } + + c.digitalIOMessageChannels[p] = ch + + return func() { c.releaseDigitalIOMessageListener(p) }, nil +} + +func (c *Client) releaseOneWireListener(p uint8) { + c.onewireMU.Lock() + defer c.onewireMU.Unlock() + + delete(c.onewireListeners, p) +} + +func (c *Client) SetOneWireListener(p uint8, ch chan []byte) (release func(), err error) { + c.onewireMU.Lock() + defer c.onewireMU.Unlock() + + if c.onewireListeners[p] != nil { + return nil, ErrPinListenerNotReleased + } + + c.onewireListeners[p] = ch + + return func() { c.releaseOneWireListener(p) }, nil +} + +func (c *Client) OpenOneWireBus(p uint8) (bus onewire.BusCloser, err error) { + c.onewireMU.Lock() + defer c.onewireMU.Unlock() + + if c.onewireListeners[p] != nil { + return nil, ErrPinListenerNotReleased + } + + // Need to run configure or firmata will not initialize. + if _, err := c.SendSysEx(SysExOneWireData, byte(OneWireInstructionConfigure), p, 0x00); err != nil { + return nil, err + } + + return newOneWireBus(c, newPin(c, p), func() error { + c.releaseOneWireListener(p) + return nil + }), nil +} + +func (c *Client) SendAnalogIOMessage(p uint8, value uint16) error { + if p > 0xF { + return ErrAnalogIOMessagePinOutOfRange + } + + lsb, msb := WordToTwoByte(value) + + return c.write([]byte{byte(AnalogIOMessage) | p, lsb, msb}, nil) +} + +func (c *Client) GetPinName(p uint8) string { + if v, ok := c.amr.DigitalPinToAnalog[p]; ok { + return fmt.Sprintf("A%d", v) + } + return fmt.Sprintf("%d", p) +} + +func (c *Client) GetPinFunctions(p uint8) []pin.Func { + return c.cr.SupportedPinModes[int(p)] +} diff --git a/firmata/errors.go b/firmata/errors.go new file mode 100644 index 0000000..6919a33 --- /dev/null +++ b/firmata/errors.go @@ -0,0 +1,24 @@ +package firmata + +import ( + "errors" +) + +var ( + ErrDeviceDisconnected = errors.New("device disconnected") + ErrUnsupportedFeature = errors.New("unsupported feature") + ErrUnhandledMessage = errors.New("message did not have an active listener") + ErrInvalidMessageTypeStart = errors.New("invalid message type start") + ErrNoDataRead = errors.New("no data read") + ErrUnexpectedSysExMessageTypeReceived = errors.New("unexpected sysex message type") + ErrAlreadyStarted = errors.New("client already started") + ErrValueOutOfRange = errors.New("value is out of range") + ErrNoI2CListenerForAddress = errors.New("no i2c listener registered for address") + ErrInvalidFirmataI2CBus = errors.New("firmata does not support multiple i2c buses") + ErrInvalidAnalogPin = errors.New("analog pin is outside of range") + ErrI2CNotEnabled = errors.New("i2c must started to use") + ErrInvalidOneWirePin = errors.New("onewire pin number cannot exceed 0x7F") + ErrAnalogIOMessagePinOutOfRange = errors.New("analog io message pin number cannot exceed 0xF") + ErrDigitalIOMessagePinOutOfRange = errors.New("digital io message port number cannot exceed 0xF") + ErrPinListenerNotReleased = errors.New("pin listener is already set for pin") +) diff --git a/firmata/feature.go b/firmata/feature.go new file mode 100644 index 0000000..41c9b3e --- /dev/null +++ b/firmata/feature.go @@ -0,0 +1,7 @@ +package firmata + +type Feature struct { + ID uint8 + VersionMajor uint8 + VersionMinor uint8 +} diff --git a/firmata/firmatareg/firmatareg.go b/firmata/firmatareg/firmatareg.go new file mode 100644 index 0000000..af86d3b --- /dev/null +++ b/firmata/firmatareg/firmatareg.go @@ -0,0 +1,216 @@ +package firmatareg + +import ( + "errors" + "strconv" + "strings" + "sync" + + "periph.io/x/devices/v3/firmata" +) + +// Opener opens a handle to a firmata device. +// +// It is provided by the actual device driver. +type Opener func() (firmata.ClientI, error) + +// Ref references a Firmata device. +// +// It is returned by All() to enumerate all registered devices. +type Ref struct { + // Name of the device. + // + // It must be unique across the host. + Name string + // Aliases are the alternative names that can be used to reference this bus. + Aliases []string + // Open is the factory to open a handle to this Firmata device. + Open Opener +} + +// Open opens a firmata device by its name, an alias or its file path and returns +// a handle to it. +// +// Specify the empty string "" to get the first available device. This is the +// recommended default value unless an application knows the exact device to use. +// +// Each device can register multiple aliases, each leading to the same device handle. +// +// "file path" is a generic concept that is highly dependent on the platform +// and OS. Depending on the OS, a serial port can be `/dev/tty*` or `COM*`. +func Open(name string) (firmata.ClientI, error) { + var r *Ref + var err error + func() { + mu.Lock() + defer mu.Unlock() + if len(byName) == 0 { + err = errors.New("firmatareg: no device found; did you forget to call Init()") + return + } + if len(name) == 0 { + r = getDefault() + return + } + // Try by name, by alias, by number. + if r = byName[name]; r == nil { + r = byAlias[name] + } + }() + if err != nil { + return nil, err + } + if r == nil { + return nil, errors.New("firmatareg: can't open unknown device: " + strconv.Quote(name)) + } + return r.Open() +} + +// All returns a copy of all the registered references to all know firmata devices +// available on this host. +// +// The list is sorted by the bus name. +func All() []*Ref { + mu.Lock() + defer mu.Unlock() + out := make([]*Ref, 0, len(byName)) + for _, v := range byName { + r := &Ref{Name: v.Name, Aliases: make([]string, len(v.Aliases)), Open: v.Open} + copy(r.Aliases, v.Aliases) + out = insertRef(out, r) + } + return out +} + +// Register registers a firmata device. +// +// Registering the same device name twice is an error, e.g. o.Name(). +func Register(name string, aliases []string, o Opener) error { + if len(name) == 0 { + return errors.New("firmatareg: can't register a bus with no name") + } + if o == nil { + return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " with nil Opener") + } + if _, err := strconv.Atoi(name); err == nil { + return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " with name being only a number") + } + if strings.Contains(name, ":") { + return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " with name containing ':'") + } + for _, alias := range aliases { + if len(alias) == 0 { + return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " with an empty alias") + } + if name == alias { + return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " with an alias the same as the bus name") + } + if _, err := strconv.Atoi(alias); err == nil { + return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " with an alias that is a number: " + strconv.Quote(alias)) + } + if strings.Contains(alias, ":") { + return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " with an alias containing ':': " + strconv.Quote(alias)) + } + } + + mu.Lock() + defer mu.Unlock() + if _, ok := byName[name]; ok { + return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " twice") + } + if _, ok := byAlias[name]; ok { + return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " twice; it is already an alias") + } + for _, alias := range aliases { + if _, ok := byName[alias]; ok { + return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " twice; alias " + strconv.Quote(alias) + " is already a bus") + } + if _, ok := byAlias[alias]; ok { + return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " twice; alias " + strconv.Quote(alias) + " is already an alias") + } + } + + r := &Ref{Name: name, Aliases: make([]string, len(aliases)), Open: o} + copy(r.Aliases, aliases) + byName[name] = r + for _, alias := range aliases { + byAlias[alias] = r + } + return nil +} + +// Unregister removes a previously registered firmata device. +// +// This can happen when a firmata device is exposed via a USB device and the device +// is unplugged. +func Unregister(name string) error { + mu.Lock() + defer mu.Unlock() + r := byName[name] + if r == nil { + return errors.New("firmatareg: can't unregister unknown bus name " + strconv.Quote(name)) + } + delete(byName, name) + for _, alias := range r.Aliases { + delete(byAlias, alias) + } + return nil +} + +// + +var ( + mu sync.Mutex + byName = map[string]*Ref{} + // Caches + byNumber = map[int]*Ref{} + byAlias = map[string]*Ref{} +) + +// getDefault returns the Ref that should be used as the default bus. +func getDefault() *Ref { + var o *Ref + if len(byNumber) == 0 { + // Fallback to use byName using a lexical sort. + name := "" + for n, o2 := range byName { + if len(name) == 0 || n < name { + o = o2 + name = n + } + } + return o + } + number := int((^uint(0)) >> 1) + for n, o2 := range byNumber { + if number > n { + number = n + o = o2 + } + } + return o +} + +func insertRef(l []*Ref, r *Ref) []*Ref { + n := r.Name + i := search(len(l), func(i int) bool { return l[i].Name > n }) + l = append(l, nil) + copy(l[i+1:], l[i:]) + l[i] = r + return l +} + +// search implements the same algorithm as sort.Search(). +// +// It was extracted to not depend on sort, which depends on reflect. +func search(n int, f func(int) bool) int { + lo := 0 + for hi := n; lo < hi; { + if i := int(uint(lo+hi) >> 1); !f(i) { + lo = i + 1 + } else { + hi = i + } + } + return lo +} diff --git a/firmata/firmware.go b/firmata/firmware.go new file mode 100644 index 0000000..7a0b00c --- /dev/null +++ b/firmata/firmware.go @@ -0,0 +1,15 @@ +package firmata + +import ( + "fmt" +) + +type FirmwareReport struct { + Major byte + Minor byte + Name []byte +} + +func (r FirmwareReport) String() string { + return fmt.Sprintf("%s [%d.%d]", TwoByteString(r.Name), r.Major, r.Minor) +} diff --git a/firmata/i2c.go b/firmata/i2c.go new file mode 100644 index 0000000..8be3e9b --- /dev/null +++ b/firmata/i2c.go @@ -0,0 +1,33 @@ +package firmata + +import ( + "periph.io/x/conn/v3/gpio" + "periph.io/x/conn/v3/physic" +) + +type I2CBus struct { +} + +func (i *I2CBus) SCL() gpio.PinIO { + panic("implement me") +} + +func (i *I2CBus) SDA() gpio.PinIO { + panic("implement me") +} + +func (i *I2CBus) Close() error { + panic("implement me") +} + +func (i *I2CBus) String() string { + panic("implement me") +} + +func (i *I2CBus) Tx(addr uint16, w, r []byte) error { + panic("implement me") +} + +func (i *I2CBus) SetSpeed(f physic.Frequency) error { + panic("implement me") +} diff --git a/firmata/i2c_packet.go b/firmata/i2c_packet.go new file mode 100644 index 0000000..5817541 --- /dev/null +++ b/firmata/i2c_packet.go @@ -0,0 +1,20 @@ +package firmata + +const ( + I2CRestartTransmission uint8 = 0b01000000 + I2CModeMask uint8 = 0b00011000 +) + +type I2CMode uint8 + +const ( + I2CModeWrite I2CMode = 0b00000000 + I2CModeRead I2CMode = 0b00001000 + I2CModeReadContinuously I2CMode = 0b00010000 + I2CModeStopReading I2CMode = 0b00011000 +) + +type I2CPacket struct { + Register uint8 + Data []byte +} diff --git a/firmata/message.go b/firmata/message.go new file mode 100644 index 0000000..81876f6 --- /dev/null +++ b/firmata/message.go @@ -0,0 +1,50 @@ +package firmata + +type ( + MessageType uint8 +) + +const ( + AnalogIOMessage MessageType = 0xE0 // pin # LSB(bits 0-6) MSB(bits 7-13) + DigitalIOMessage MessageType = 0x90 // port LSB(bits 0-6) MSB(bits 7-13) + ReportAnalogPin MessageType = 0xC0 // pin # disable/enable(0/1) - n/a - + ReportDigitalPort MessageType = 0xD0 // port disable/enable(0/1) - n/a - + StartSysEx MessageType = 0xF0 // + SetPinMode MessageType = 0xF4 // pin # (0-127) pin mode + SetDigitalPinValue MessageType = 0xF5 // pin # (0-127) pin value(0/1) + EndSysEx MessageType = 0xF7 // + ProtocolVersion MessageType = 0xF9 // major version minor version + SystemReset MessageType = 0xFF // +) + +var messageTypeToStringMap = map[MessageType]string{ + AnalogIOMessage: "AnalogIOMessage", + DigitalIOMessage: "DigitalIOMessage", + ReportAnalogPin: "ReportAnalogPin", + ReportDigitalPort: "ReportDigitalPort", + StartSysEx: "StartSysEx", + SetPinMode: "SetPinMode", + SetDigitalPinValue: "SetDigitalPinValue", + EndSysEx: "EndSysEx", + ProtocolVersion: "ProtocolVersion", + SystemReset: "SystemReset", +} + +func (m MessageType) String() string { + switch { + case AnalogIOMessage <= m && m <= (AnalogIOMessage+0xF): + return messageTypeToStringMap[AnalogIOMessage] + case DigitalIOMessage <= m && m <= (DigitalIOMessage+0xF): + return messageTypeToStringMap[DigitalIOMessage] + case ReportAnalogPin <= m && m <= (ReportAnalogPin+0xF): + return messageTypeToStringMap[ReportAnalogPin] + case ReportDigitalPort <= m && m <= (ReportDigitalPort+0xF): + return messageTypeToStringMap[ReportDigitalPort] + } + + if v, ok := messageTypeToStringMap[m]; ok { + return v + } + + return "Unknown" +} diff --git a/firmata/onewire.go b/firmata/onewire.go new file mode 100644 index 0000000..9e480bd --- /dev/null +++ b/firmata/onewire.go @@ -0,0 +1,181 @@ +package firmata + +import ( + "encoding/binary" + "fmt" + "math" + "sync" + + "periph.io/x/conn/v3/gpio" + "periph.io/x/conn/v3/onewire" +) + +type OneWireBus struct { + c ClientI + q *Pin + mu sync.Mutex + closer func() error + cid uint16 +} + +func newOneWireBus(c ClientI, q *Pin, closer func() error) *OneWireBus { + return &OneWireBus{ + c: c, + q: q, + closer: closer, + } +} + +func (b *OneWireBus) Close() error { + return b.closer() +} + +func (b *OneWireBus) String() string { + panic("implement me") +} + +func (b *OneWireBus) Search(alarmOnly bool) ([]onewire.Address, error) { + b.mu.Lock() + defer b.mu.Unlock() + + ch := make(chan []byte) + defer close(ch) + + release, err := b.c.SetOneWireListener(b.q.pin, ch) + if err != nil { + return nil, err + } + defer release() + + if alarmOnly { + if _, err := b.c.SendSysEx(SysExOneWireData, byte(OneWireInstructionSearchAlarmed), b.q.pin); err != nil { + return nil, err + } + } else { + if _, err := b.c.SendSysEx(SysExOneWireData, byte(OneWireInstructionSearch), b.q.pin); err != nil { + return nil, err + } + } + + data := <-ch + + ins := OneWireInstruction(data[0]) + pin := data[1] + + if ins != OneWireInstructionSearchAlarmedReply && ins != OneWireInstructionSearchReply { + return nil, fmt.Errorf("%w: did not receive search reply", ErrUnhandledMessage) + } + if pin != b.q.pin { + return nil, fmt.Errorf("%w: received message from wrong bus", ErrInvalidOneWirePin) + } + + data = Decoder7Bit(data[2:]) + + addresses := make([]onewire.Address, len(data)/8) + for i := range addresses { + addresses[i] = onewire.Address(binary.LittleEndian.Uint64(data[i*8:])) + } + return addresses, nil +} + +func (b *OneWireBus) Tx(w, r []byte, power onewire.Pullup) error { + if len(w) == 0 && len(r) == 0 { + return nil + } + + if len(r) > math.MaxUint16 { + return fmt.Errorf("%w: cannot reach more than %d bytes", ErrValueOutOfRange, math.MaxUint16) + } + + b.mu.Lock() + defer b.mu.Unlock() + + ch := make(chan []byte) + defer close(ch) + + release, err := b.c.SetOneWireListener(b.q.pin, ch) + if err != nil { + return err + } + defer release() + + var powerVal byte = 0x00 + if power { + powerVal = 0x01 + } + + if _, err := b.c.SendSysEx(SysExOneWireData, byte(OneWireInstructionConfigure), b.q.pin, powerVal); err != nil { + return err + } + + cmd := OneWireCommandReset + + if len(w) > 0 { + switch w[0] { + case 0xF0: // search rom + if _, err := b.c.SendSysEx(SysExOneWireData, byte(OneWireInstructionSearch), b.q.pin); err != nil { + return err + } + case 0xEC: // search rom (alarmed) + if _, err := b.c.SendSysEx(SysExOneWireData, byte(OneWireInstructionSearchAlarmed), b.q.pin); err != nil { + return err + } + case 0xCC: // skip rom + panic("not implemented") + case 0x33: // read rom + panic("not implemented") + case 0x55: // match rom + cmd |= OneWireCommandSelect + + payload := w[1:9] + + if len(w) > 9 { // command length (1) + address length (8) + cmd |= OneWireCommandWrite + } + if len(r) > 0 { + cmd |= OneWireCommandRead + // Write the amount of bytes to read. + payload = append(payload, byte(len(r)&0xFF), byte((len(r)>>8)&0xFF)) + // Write the correlation id for sanity checking. + payload = append(payload, byte(b.cid&0xFF), byte((b.cid>>8)&0xFF)) + b.cid++ + } + + cmd |= OneWireCommandDelay + payload = append(payload, 10, 0, 0, 0) + + payload = append(payload, w[9:]...) + payload = append([]byte{byte(cmd), b.q.pin}, Encoder7Bit(payload)...) + if _, err := b.c.SendSysEx(SysExOneWireData, payload...); err != nil { + return err + } + default: + return fmt.Errorf("%w: no onewire command matching 0x%02X", ErrUnsupportedFeature, w[0]) + } + } + + if len(r) > 0 { + data := <-ch + + ins := OneWireInstruction(data[0]) + pin := data[1] + + if ins != OneWireInstructionReadReply { + return fmt.Errorf("%w: did not receive read reply", ErrUnhandledMessage) + } + if pin != b.q.pin { + return fmt.Errorf("%w: received message from wrong bus", ErrInvalidOneWirePin) + } + + data = Decoder7Bit(data[2:]) + + // Drop the correlation id when copying. + copy(r, data[2:]) + } + + return nil +} + +func (b *OneWireBus) Q() gpio.PinIO { + return b.q +} diff --git a/firmata/onewire_cmd.go b/firmata/onewire_cmd.go new file mode 100644 index 0000000..ecd6db1 --- /dev/null +++ b/firmata/onewire_cmd.go @@ -0,0 +1,27 @@ +package firmata + +type OneWireInstruction uint8 + +const ( + OneWireInstructionSearch OneWireInstruction = 0x40 + OneWireInstructionConfigure OneWireInstruction = 0x41 + OneWireInstructionSearchReply OneWireInstruction = 0x42 + OneWireInstructionReadReply OneWireInstruction = 0x43 + OneWireInstructionSearchAlarmed OneWireInstruction = 0x44 + OneWireInstructionSearchAlarmedReply OneWireInstruction = 0x45 +) + +type OneWireConnectorWrapper struct { + client ClientI +} + +type OneWireCommand uint8 + +const ( + OneWireCommandReset OneWireCommand = 0b00000001 + OneWireCommandSkip OneWireCommand = 0b00000010 + OneWireCommandSelect OneWireCommand = 0b00000100 + OneWireCommandRead OneWireCommand = 0b00001000 + OneWireCommandDelay OneWireCommand = 0b00010000 + OneWireCommandWrite OneWireCommand = 0b00100000 +) diff --git a/firmata/pin.go b/firmata/pin.go new file mode 100644 index 0000000..285ed12 --- /dev/null +++ b/firmata/pin.go @@ -0,0 +1,233 @@ +package firmata + +import ( + "context" + "errors" + "sync" + "time" + + "periph.io/x/conn/v3/gpio" + "periph.io/x/conn/v3/physic" + "periph.io/x/conn/v3/pin" +) + +var ( + ErrUnsupportedGPIOPull = errors.New("firmata: PullDown is not supported") + ErrNoMatchingGPIOPull = errors.New("firmata: pin was previously in a non-input mode") +) + +type Pin struct { + c ClientI + pin uint8 + edge gpio.Edge + ch chan gpio.Level + release func() + done chan struct{} + valueLast gpio.Level + valueNew gpio.Level + edgeChange chan gpio.Edge + mu sync.Mutex +} + +func newPin(c ClientI, num uint8) *Pin { + p := &Pin{ + c: c, + pin: num, + ch: make(chan gpio.Level), + } + + go p.run() + + return p +} + +func (p *Pin) run() { + p.done = make(chan struct{}) + + for { + select { + case <-p.done: + close(p.ch) + return + case v := <-p.ch: + p.valueLast = p.valueNew + p.valueNew = v + + func() { + p.mu.Lock() + defer p.mu.Unlock() + + if p.edgeChange == nil { + return + } + + if p.valueLast == true || p.valueNew == false { + p.edgeChange <- gpio.FallingEdge + } + if p.valueLast == false || p.valueNew == true { + p.edgeChange <- gpio.RisingEdge + } + }() + } + } +} + +func (p *Pin) In(pull gpio.Pull, edge gpio.Edge) error { + var mode = PinFuncDigitalInput + switch pull { + case gpio.PullDown: + return ErrUnsupportedGPIOPull + case gpio.PullNoChange: + ch, err := p.c.PinStateQuery(p.pin) + if err != nil { + return err + } + + s := <-ch + mode = s.Mode + + switch mode { + case PinFuncInputPullUp: + case PinFuncDigitalInput: + default: + return ErrNoMatchingGPIOPull + } + case gpio.PullUp: + mode = PinFuncInputPullUp + case gpio.Float: + mode = PinFuncDigitalInput + } + + if err := p.c.SetPinMode(p.pin, mode); err != nil { + return err + } + + var err error + if p.release, err = p.c.SetDigitalIOMessageListener(p.pin, p.ch); err != nil { + return err + } + + if err = p.c.SetDigitalPinReporting(p.pin, true); err != nil { + return err + } + + p.edge = edge + + return nil +} + +func (p *Pin) Read() gpio.Level { + return p.valueNew +} + +func (p *Pin) WaitForEdge(timeout time.Duration) bool { + defer func() { + p.mu.Lock() + defer p.mu.Unlock() + + close(p.edgeChange) + p.edgeChange = nil + }() + + func() { + p.mu.Lock() + defer p.mu.Unlock() + + p.edgeChange = make(chan gpio.Edge) + }() + + ctx, cancel := context.WithTimeout(context.Background(), timeout) + defer cancel() + + for { + select { + case change := <-p.edgeChange: + if p.edge == gpio.BothEdges || change == p.edge { + return true + } + case <-ctx.Done(): + return false + } + } +} + +func (p *Pin) Pull() gpio.Pull { + ch, err := p.c.PinStateQuery(p.pin) + if err != nil { + return gpio.PullNoChange + } + + s := <-ch + switch s.Mode { + case PinFuncInputPullUp: + return gpio.PullUp + case PinFuncDigitalInput: + return gpio.Float + } + + return gpio.PullNoChange +} + +func (p *Pin) DefaultPull() gpio.Pull { + return gpio.PullNoChange +} + +func (p *Pin) Out(l gpio.Level) error { + // TODO: + panic("implement me") +} + +func (p *Pin) PWM(duty gpio.Duty, f physic.Frequency) error { + // TODO: + panic("implement me") +} + +func (p *Pin) Func() pin.Func { + ch, err := p.c.PinStateQuery(p.pin) + if err != nil { + return pin.FuncNone + } + + s := <-ch + + return s.Mode +} + +func (p *Pin) SetFunc(f pin.Func) error { + return p.c.SetPinMode(p.pin, f) +} + +func (p *Pin) SupportedFuncs() []pin.Func { + return p.c.GetPinFunctions(p.pin) +} + +func (p *Pin) Halt() error { + close(p.done) + p.release() + if err := p.c.SetDigitalPinReporting(p.pin, false); err != nil { + return err + } + if err := p.c.SetAnalogPinReporting(p.pin, false); err != nil { + return err + } + if err := p.c.SetDigitalPinValue(p.pin, gpio.Low); err != nil { + return err + } + return nil +} + +func (p *Pin) Name() string { + return p.c.GetPinName(p.pin) +} + +func (p *Pin) String() string { + return p.Name() +} + +func (p *Pin) Number() int { + return int(p.pin) +} + +func (p *Pin) Function() string { + return string(p.Func()) +} diff --git a/firmata/pin_mode.go b/firmata/pin_mode.go new file mode 100644 index 0000000..7cd9a9e --- /dev/null +++ b/firmata/pin_mode.go @@ -0,0 +1,62 @@ +package firmata + +import ( + "periph.io/x/conn/v3/pin" +) + +const ( + PinFuncDigitalInput pin.Func = "DigitalInput" + PinFuncDigitalOutput pin.Func = "DigitalOutput" + PinFuncAnalogInput pin.Func = "AnalogInput" + PinFuncPWM pin.Func = "PWM" + PinFuncServo pin.Func = "Servo" + PinFuncShift pin.Func = "Shift" + PinFuncI2C pin.Func = "I2C" + PinFuncOneWire pin.Func = "OneWire" + PinFuncStepper pin.Func = "Stepper" + PinFuncEncoder pin.Func = "Encoder" + PinFuncSerial pin.Func = "Serial" + PinFuncInputPullUp pin.Func = "InputPullUp" + PinFuncSPI pin.Func = "SPI" + PinFuncSonar pin.Func = "Sonar" + PinFuncTone pin.Func = "Tone" + PinFuncDHT pin.Func = "DHT" +) + +var pinFuncToModeMap = map[pin.Func]uint8{ + PinFuncDigitalInput: 0x0, + PinFuncDigitalOutput: 0x1, + PinFuncAnalogInput: 0x2, + PinFuncPWM: 0x3, + PinFuncServo: 0x4, + PinFuncShift: 0x5, + PinFuncI2C: 0x6, + PinFuncOneWire: 0x7, + PinFuncStepper: 0x8, + PinFuncEncoder: 0x9, + PinFuncSerial: 0xA, + PinFuncInputPullUp: 0xB, + PinFuncSPI: 0xC, + PinFuncSonar: 0xD, + PinFuncTone: 0xE, + PinFuncDHT: 0xF, +} + +var pinModeToFuncMap = map[uint8]pin.Func{ + 0x0: PinFuncDigitalInput, + 0x1: PinFuncDigitalOutput, + 0x2: PinFuncAnalogInput, + 0x3: PinFuncPWM, + 0x4: PinFuncServo, + 0x5: PinFuncShift, + 0x6: PinFuncI2C, + 0x7: PinFuncOneWire, + 0x8: PinFuncStepper, + 0x9: PinFuncEncoder, + 0xA: PinFuncSerial, + 0xB: PinFuncInputPullUp, + 0xC: PinFuncSPI, + 0xD: PinFuncSonar, + 0xE: PinFuncTone, + 0xF: PinFuncDHT, +} diff --git a/firmata/pin_state.go b/firmata/pin_state.go new file mode 100644 index 0000000..9815eb5 --- /dev/null +++ b/firmata/pin_state.go @@ -0,0 +1,17 @@ +package firmata + +import ( + "fmt" + + "periph.io/x/conn/v3/pin" +) + +type PinStateResponse struct { + Pin uint8 + Mode pin.Func + State int +} + +func (p PinStateResponse) String() string { + return fmt.Sprintf("pin(%d) mode(%s) state(%d)", p.Pin, p.Mode, p.State) +} diff --git a/firmata/sysex.go b/firmata/sysex.go new file mode 100644 index 0000000..e7ba0a9 --- /dev/null +++ b/firmata/sysex.go @@ -0,0 +1,109 @@ +package firmata + +type ( + SysExCmd uint8 + SysExExtendedCmd uint16 +) + +// Base Features +const ( + SysExExtendedId SysExCmd = 0x00 // A value of 0x00 indicates the next 2 bytes define the extended ID + SysExAnalogMappingQuery SysExCmd = 0x69 // ask for mapping of analog pin names to pin numbers + SysExAnalogMappingResponse SysExCmd = 0x6A // reply with mapping info + SysExCapabilityQuery SysExCmd = 0x6B // ask for supported modes and resolution of all pins + SysExCapabilityResponse SysExCmd = 0x6C // reply with supported modes and resolution + SysExPinStateQuery SysExCmd = 0x6D // ask for a pin's current mode and state (different from value) + SysExPinStateResponse SysExCmd = 0x6E // reply with a pin's current mode and state (different from value) + SysExExtendedAnalog SysExCmd = 0x6F // analog write (PWM, Servo, etc.) to any pin + SysExStringData SysExCmd = 0x71 // a string message with 14-bits per char + SysExReportFirmware SysExCmd = 0x79 // report name and version of the firmware + SysExSamplingInterval SysExCmd = 0x7A // the interval at which analog input is sampled (default = 19ms) + SysExNonRealtime SysExCmd = 0x7E // MIDI Reserved for non-realtime messages + SysExRealtime SysExCmd = 0x7F // MIDI Reserved for realtime messages +) + +// User Defined Feature Codes +// Assign these to whatever feature constant you define +const ( + UserFeature1 SysExCmd = 0x01 + UserFeature2 SysExCmd = 0x02 + UserFeature3 SysExCmd = 0x03 + UserFeature4 SysExCmd = 0x04 + UserFeature5 SysExCmd = 0x05 + UserFeature6 SysExCmd = 0x06 + UserFeature7 SysExCmd = 0x07 + UserFeature8 SysExCmd = 0x08 + UserFeature9 SysExCmd = 0x09 + UserFeatureA SysExCmd = 0x0A + UserFeatureB SysExCmd = 0x0B + UserFeatureC SysExCmd = 0x0C + UserFeatureD SysExCmd = 0x0D + UserFeatureE SysExCmd = 0x0E + UserFeatureF SysExCmd = 0x0F +) + +// Optionally Included Features +const ( + SysExRCOutputData SysExCmd = 0x5C // https://github.com/firmata/protocol/blob/master/proposals/rcswitch-proposal.md + SysExRCInputData SysExCmd = 0x5D // https://github.com/firmata/protocol/blob/master/proposals/rcswitch-proposal.md + SysExDeviceQuery SysExCmd = 0x5E // https://github.com/finson-release/Luni/blob/master/extras/v0.9/v0.8-device-driver-C-firmata-messages.md + SysExDeviceResponse SysExCmd = 0x5F // https://github.com/finson-release/Luni/blob/master/extras/v0.9/v0.8-device-driver-C-firmata-messages.md + SysExSerialDataV1 SysExCmd = 0x60 // https://github.com/firmata/protocol/blob/master/serial-1.0.md + SysExEncoderData SysExCmd = 0x61 // https://github.com/firmata/protocol/blob/master/encoder.md + SysExAccelStepperData SysExCmd = 0x62 // https://github.com/firmata/protocol/blob/master/accelStepperFirmata.md + SysExSerialDataV2 SysExCmd = 0x67 // https://github.com/firmata/protocol/blob/master/proposals/serial-2.0-proposal.md + SysExSPIData SysExCmd = 0x68 // https://github.com/firmata/protocol/blob/master/spi.md + SysExServoConfig SysExCmd = 0x70 // https://github.com/firmata/protocol/blob/master/servos.md + SysExStepperData SysExCmd = 0x72 // https://github.com/firmata/protocol/blob/master/stepper-legacy.md + SysExOneWireData SysExCmd = 0x73 // https://github.com/firmata/protocol/blob/master/onewire.md + SysExDHTSensorData SysExCmd = 0x74 // https://github.com/firmata/protocol/blob/master/dhtsensor.md + SysExShiftData SysExCmd = 0x75 // https://github.com/firmata/protocol/blob/master/proposals/shift-proposal.md + SysExI2CRequest SysExCmd = 0x76 // https://github.com/firmata/protocol/blob/master/i2c.md + SysExI2CReply SysExCmd = 0x77 // https://github.com/firmata/protocol/blob/master/i2c.md + SysExI2CConfig SysExCmd = 0x78 // https://github.com/firmata/protocol/blob/master/i2c.md + SysExSchedulerData SysExCmd = 0x7B // https://github.com/firmata/protocol/blob/master/scheduler.md + SysExFrequencyCommand SysExCmd = 0x7D // https://github.com/firmata/protocol/blob/master/frequency.md +) + +var sysExCmdToStringMap = map[SysExCmd]string{ + SysExExtendedId: "ExtendedId", + SysExAnalogMappingQuery: "AnalogMappingQuery", + SysExAnalogMappingResponse: "AnalogMappingResponse", + SysExCapabilityQuery: "CapabilityQuery", + SysExCapabilityResponse: "CapabilityResponse", + SysExPinStateQuery: "PinStateQuery", + SysExPinStateResponse: "PinStateResponse", + SysExExtendedAnalog: "ExtendedAnalog", + SysExStringData: "StringData", + SysExReportFirmware: "ReportFirmware", + SysExSamplingInterval: "SamplingInterval", + SysExNonRealtime: "NonRealtime", + SysExRealtime: "Realtime", + SysExRCOutputData: "RCOutputData", + SysExRCInputData: "RCInputData", + SysExDeviceQuery: "DeviceQuery", + SysExDeviceResponse: "DeviceResponse", + SysExSerialDataV1: "SerialDataV1", + SysExEncoderData: "EncoderData", + SysExAccelStepperData: "AccelStepperData", + SysExSerialDataV2: "SerialDataV2", + SysExSPIData: "SPIData", + SysExServoConfig: "ServoConfig", + SysExStepperData: "StepperData", + SysExOneWireData: "OneWireData", + SysExDHTSensorData: "DHTSensorData", + SysExShiftData: "ShiftData", + SysExI2CRequest: "I2CRequest", + SysExI2CReply: "I2CReply", + SysExI2CConfig: "I2CConfig", + SysExSchedulerData: "SchedulerData", + SysExFrequencyCommand: "FrequencyCommand", +} + +func (s SysExCmd) String() string { + if v, ok := sysExCmdToStringMap[s]; ok { + return v + } + + return "Unknown" +} diff --git a/firmata/util.go b/firmata/util.go new file mode 100644 index 0000000..f725a5b --- /dev/null +++ b/firmata/util.go @@ -0,0 +1,114 @@ +package firmata + +import ( + "fmt" +) + +const SevenBitMask byte = 0b01111111 + +func TwoByteToByte(a, b byte) byte { + return (a & SevenBitMask) | ((b & SevenBitMask) << 7) +} + +func TwoByteToWord(a, b byte) uint16 { + return uint16(a&SevenBitMask) | (uint16(b&SevenBitMask) << 7) +} + +func TwoByteString(bytes []byte) string { + if len(bytes)%2 == 1 { + bytes = append(bytes, 0) + } + + var s string + for i := 0; i < len(bytes); i += 2 { + s += string(TwoByteToByte(bytes[i], bytes[i+1])) + } + return s +} + +func TwoByteRepresentationToByteSlice(bytes []byte) []byte { + if len(bytes)%2 == 1 { + bytes = append(bytes, 0) + } + + d := make([]byte, len(bytes)/2) + i := 0 + for di := range d { + d[di] = TwoByteToByte(bytes[i], bytes[i+1]) + i += 2 + } + return d +} + +func ByteToTwoByte(b byte) (lsb, msb byte) { + return b & SevenBitMask, (b >> 7) & SevenBitMask +} + +func WordToTwoByte(b uint16) (lsb, msb byte) { + return byte(b) & SevenBitMask, byte(b>>7) & SevenBitMask +} + +func ByteSliceToTwoByteRepresentation(bytes []byte) []byte { + d := make([]byte, len(bytes)*2) + i := 0 + for _, b := range bytes { + d[i], d[i+1] = ByteToTwoByte(b) + i += 2 + } + return d +} + +func SprintHexArray(data []byte) string { + s := "" + if len(data) == 0 { + return s + } + for _, b := range data { + s += fmt.Sprintf("0x%02X ", b) + } + return s[:len(s)-1] +} + +// Encoder7Bit logic determined from here: +// - ConfigurableFirmata@2.10.1/src/Encoder7Bit.cpp:34 +func Encoder7Bit(inData []byte) []byte { + var outData []byte + var previous byte + var shift = 0 + for _, data := range inData { + if shift == 0 { + outData = append(outData, data&0x7f) + shift++ + previous = data >> 7 + } else { + outData = append(outData, ((data<>1) + shift = 0 + } else { + shift++ + previous = data >> (8 - shift) + } + } + } + if shift > 0 { + outData = append(outData, previous) + } + return outData +} + +// Decoder7Bit logic determined from here: +// - ConfigurableFirmata@2.10.1/src/Encoder7Bit.h:17 +// - ConfigurableFirmata@2.10.1/src/Encoder7Bit.cpp:54 +func Decoder7Bit(inData []byte) []byte { + var outBytes = ((len(inData)) * 7) >> 3 + + var outData = make([]byte, outBytes) + for i := 0; i < outBytes; i++ { + var j = i << 3 + var pos = j / 7 + var shift = byte(j % 7) + outData[i] = (inData[pos] >> shift) | ((inData[pos+1] << (7 - shift)) & 0xFF) + } + return outData +} diff --git a/firmata/util_test.go b/firmata/util_test.go new file mode 100644 index 0000000..c5b53e4 --- /dev/null +++ b/firmata/util_test.go @@ -0,0 +1,129 @@ +package firmata + +import ( + "fmt" + "reflect" + "testing" +) + +func TestByteConversion(t *testing.T) { + for i := uint16(0x00); i <= 0xFF; i++ { + t.Run(fmt.Sprintf("0x%02X", i), func(t *testing.T) { + a, b := ByteToTwoByte(byte(i)) + o := TwoByteToByte(a, b) + if byte(i) != o { + t.Errorf("ByteToTwoByte(0x%02X) = 0x%02X, 0x%02X => TwoByteToByte() = 0x%02X", i, a, b, o) + } + }) + } +} + +func TestTwoByteString(t *testing.T) { + tests := []struct { + name string + bytes []byte + want string + }{ + { + name: "nil", + bytes: nil, + want: "", + }, + { + name: "empty", + bytes: []byte{}, + want: "", + }, + { + name: "test string", + bytes: ByteSliceToTwoByteRepresentation([]byte{ + 0x74, 0x65, 0x73, 0x74, 0x20, 0x73, 0x74, 0x72, 0x69, 0x6E, 0x67, + }), + want: "test string", + }, + } + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + if got := TwoByteString(tt.bytes); got != tt.want { + t.Errorf("TwoByteString() = %v, want %v", got, tt.want) + } + }) + } +} + +func TestByteSliceTo2ByteRepresentation(t *testing.T) { + tests := []struct { + name string + input []byte + expected []byte + }{ + { + name: "nil", + input: nil, + expected: []byte{}, + }, + { + name: "empty", + input: []byte{}, + expected: []byte{}, + }, + { + name: "7 lsb set", + input: []byte{0b01111111, 0b11111111}, + expected: []byte{0b01111111, 0b00000000, 0b01111111, 0b00000001}, + }, + { + name: "7 lsb not set", + input: []byte{0b00000000, 0b10000000}, + expected: []byte{0b00000000, 0b00000000, 0b00000000, 0b00000001}, + }, + } + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + if got := ByteSliceToTwoByteRepresentation(tt.input); !reflect.DeepEqual(got, tt.expected) { + t.Errorf("ByteSliceToTwoByteRepresentation() = %v, want %v", got, tt.expected) + } + }) + } +} + +func TestTwoByteRepresentationToByteSlice(t *testing.T) { + tests := []struct { + name string + input []byte + expected []byte + }{ + { + name: "nil", + input: nil, + expected: []byte{}, + }, + { + name: "empty", + input: []byte{}, + expected: []byte{}, + }, + { + name: "7 lsb set", + input: []byte{0b01111111, 0b00000000, 0b01111111, 0b00000001}, + expected: []byte{0b01111111, 0b11111111}, + }, + { + name: "7 lsb not set", + input: []byte{0b00000000, 0b00000000, 0b00000000, 0b00000001}, + expected: []byte{0b00000000, 0b10000000}, + }, + { + name: "only 1 byte", + input: []byte{0b01000000}, + expected: []byte{0b01000000}, + }, + } + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + if got := TwoByteRepresentationToByteSlice(tt.input); !reflect.DeepEqual(got, tt.expected) { + t.Errorf("TwoByteRepresentationToByteSlice() = %v, want %v", got, tt.expected) + } + }) + } +} From 050bc7a20a41befd8307b87c6c8b15f8a0ef932c Mon Sep 17 00:00:00 2001 From: imle Date: Mon, 4 Oct 2021 10:25:06 -0400 Subject: [PATCH 2/4] add i2c implementation --- firmata/client.go | 41 +++++++++++++++++++------ firmata/errors.go | 1 + firmata/i2c.go | 77 +++++++++++++++++++++++++++++++++++++++-------- 3 files changed, 97 insertions(+), 22 deletions(-) diff --git a/firmata/client.go b/firmata/client.go index 08e0e5b..03a5549 100644 --- a/firmata/client.go +++ b/firmata/client.go @@ -8,6 +8,7 @@ import ( "sync" "periph.io/x/conn/v3/gpio" + "periph.io/x/conn/v3/i2c" "periph.io/x/conn/v3/onewire" "periph.io/x/conn/v3/pin" ) @@ -38,16 +39,18 @@ type ClientI interface { SetSamplingInterval(uint16) error SetDigitalPinValue(p uint8, value gpio.Level) error SendAnalogMappingQuery() (chan AnalogMappingResponse, error) - SetI2CMessageChannel(address uint8, ch chan I2CPacket) - WriteI2CData(address uint8, restart bool, data []uint8) error - ReadI2CData(address uint8, restart bool, len uint16) error - ReadI2CRegister(address uint8, restart bool, register uint8, len uint16) error - SendI2CConfig(delayMicroseconds uint8) error AnalogPinToDigitalPin(p uint8) (uint8, error) SetAnalogIOMessageListener(p uint8, ch chan uint16) (release func(), err error) SetDigitalIOMessageListener(p uint8, ch chan gpio.Level) (release func(), err error) SendAnalogIOMessage(uint8, uint16) error + OpenI2CBus() (i2c.Bus, error) + SetI2CAddressListener(addr uint8, ch chan I2CPacket) (release func(), err error) + WriteI2CData(address uint8, restart bool, data []uint8) error + ReadI2CData(address uint8, restart bool, len uint16) error + ReadI2CRegister(address uint8, restart bool, register uint8, len uint16) error + SendI2CConfig(delayMicroseconds uint8) error + OpenOneWireBus(p uint8) (bus onewire.BusCloser, err error) SetOneWireListener(uint8, chan []byte) (release func(), err error) @@ -61,7 +64,9 @@ type Client struct { board io.ReadWriteCloser responseChannels map[SysExCmd][]chan []byte sysExListenerChannels map[SysExCmd]chan []byte - i2cListeners map[uint8]chan I2CPacket + + i2cListeners map[uint8]chan I2CPacket + i2cMU sync.Mutex onewireListeners map[uint8]chan []byte onewireMU sync.Mutex @@ -587,13 +592,25 @@ func (c *Client) SendI2CConfig(delayMicroseconds uint8) error { return nil } +func (c *Client) releaseI2CAddressListener(addr uint8) { + c.i2cMU.Lock() + defer c.i2cMU.Unlock() + + delete(c.i2cListeners, addr) +} + // This function only supports 7-bit I2C addresses -func (c *Client) SetI2CMessageChannel(address uint8, ch chan I2CPacket) { - if c.i2cListeners[address] != nil { - close(c.i2cListeners[address]) +func (c *Client) SetI2CAddressListener(addr uint8, ch chan I2CPacket) (release func(), err error) { + c.i2cMU.Lock() + defer c.i2cMU.Unlock() + + if c.i2cListeners[addr] != nil { + return nil, ErrI2CAddressListenerNotReleased } - c.i2cListeners[address] = ch + c.i2cListeners[addr] = ch + + return func() { c.releaseI2CAddressListener(addr) }, nil } func (c *Client) releaseAnalogIOMessageListener(p uint8) { @@ -695,3 +712,7 @@ func (c *Client) GetPinName(p uint8) string { func (c *Client) GetPinFunctions(p uint8) []pin.Func { return c.cr.SupportedPinModes[int(p)] } + +func (c *Client) OpenI2CBus() (i2c.Bus, error) { + return newI2CBus(c) +} diff --git a/firmata/errors.go b/firmata/errors.go index 6919a33..4f6418a 100644 --- a/firmata/errors.go +++ b/firmata/errors.go @@ -21,4 +21,5 @@ var ( ErrAnalogIOMessagePinOutOfRange = errors.New("analog io message pin number cannot exceed 0xF") ErrDigitalIOMessagePinOutOfRange = errors.New("digital io message port number cannot exceed 0xF") ErrPinListenerNotReleased = errors.New("pin listener is already set for pin") + ErrI2CAddressListenerNotReleased = errors.New("address listener is already set") ) diff --git a/firmata/i2c.go b/firmata/i2c.go index 8be3e9b..7aa50d4 100644 --- a/firmata/i2c.go +++ b/firmata/i2c.go @@ -1,33 +1,86 @@ package firmata import ( - "periph.io/x/conn/v3/gpio" + "errors" + "fmt" + "math" + "sync" + "periph.io/x/conn/v3/physic" ) +var ( + Err10BitAddressingNotSupported = errors.New("10-bit addressing not supported") +) + type I2CBus struct { + c ClientI + mu sync.Mutex } -func (i *I2CBus) SCL() gpio.PinIO { - panic("implement me") -} +func newI2CBus(c ClientI) (*I2CBus, error) { + b := &I2CBus{ + c: c, + } -func (i *I2CBus) SDA() gpio.PinIO { - panic("implement me") + err := c.SendI2CConfig(0) + if err != nil { + return nil, err + } + + return b, nil } -func (i *I2CBus) Close() error { +func (b *I2CBus) Close() error { panic("implement me") } -func (i *I2CBus) String() string { +func (b *I2CBus) String() string { panic("implement me") } -func (i *I2CBus) Tx(addr uint16, w, r []byte) error { - panic("implement me") +func (b *I2CBus) Tx(addr uint16, w, r []byte) error { + if addr >= 0b11111111 { + return fmt.Errorf("%w: 0x%04X", Err10BitAddressingNotSupported, addr) + } + + if len(r) > math.MaxUint16 { + return fmt.Errorf("%w: cannot reach more than %d bytes", ErrValueOutOfRange, math.MaxUint16) + } + + b.mu.Lock() + defer b.mu.Unlock() + + ch := make(chan I2CPacket) + defer close(ch) + + address := uint8(addr) + + release, err := b.c.SetI2CAddressListener(address, ch) + if err != nil { + return err + } + defer release() + + if len(w) > 0 { + if err := b.c.WriteI2CData(address, false, w); err != nil { + return err + } + } + + if len(r) > 0 { + if err := b.c.ReadI2CData(address, false, uint16(len(r))); err != nil { + return err + } + + pck := <-ch + + copy(r, pck.Data) + } + + return nil } -func (i *I2CBus) SetSpeed(f physic.Frequency) error { - panic("implement me") +func (b *I2CBus) SetSpeed(f physic.Frequency) error { + return fmt.Errorf("%w: firmata does not support setting bus frequency", ErrUnsupportedFeature) } From 7b71c747f2014e414bb3d3444af1b6961d0486ca Mon Sep 17 00:00:00 2001 From: imle Date: Sun, 10 Oct 2021 22:53:08 -0400 Subject: [PATCH 3/4] updates and testing of implementations --- bmxx80/bmxx80.go | 3 + firmata/analog_mapping.go | 16 +++ firmata/capability.go | 24 ++++ firmata/client.go | 159 +++++++++-------------- firmata/firmatareg/firmatareg.go | 216 ------------------------------- firmata/firmware.go | 8 ++ firmata/pin.go | 12 +- firmata/pin_state.go | 14 ++ pca9685/pins.go | 2 +- 9 files changed, 134 insertions(+), 320 deletions(-) delete mode 100644 firmata/firmatareg/firmatareg.go diff --git a/bmxx80/bmxx80.go b/bmxx80/bmxx80.go index 3885ea2..dda92a1 100644 --- a/bmxx80/bmxx80.go +++ b/bmxx80/bmxx80.go @@ -20,6 +20,9 @@ import ( "periph.io/x/conn/v3/spi" ) +// I2CAddr i2c default address. +const I2CAddr uint16 = 0x77 + // Oversampling affects how much time is taken to measure each of temperature, // pressure and humidity. // diff --git a/firmata/analog_mapping.go b/firmata/analog_mapping.go index 83a9944..b9cc1fd 100644 --- a/firmata/analog_mapping.go +++ b/firmata/analog_mapping.go @@ -10,6 +10,22 @@ type AnalogMappingResponse struct { DigitalPinToAnalog map[uint8]uint8 } +func ParseAnalogMappingResponse(data []byte) AnalogMappingResponse { + var response = AnalogMappingResponse{ + AnalogPinToDigital: []uint8{}, + DigitalPinToAnalog: map[uint8]uint8{}, + } + + for i := 0; i < len(data); i++ { + if data[i] != CapabilityResponsePinDelimiter { + response.DigitalPinToAnalog[uint8(i)] = uint8(len(response.AnalogPinToDigital)) + response.AnalogPinToDigital = append(response.AnalogPinToDigital, uint8(i)) + } + } + + return response +} + func (a AnalogMappingResponse) String() string { str := bytes.Buffer{} for analogPin, digitalPin := range a.AnalogPinToDigital { diff --git a/firmata/capability.go b/firmata/capability.go index c430a2e..71d2978 100644 --- a/firmata/capability.go +++ b/firmata/capability.go @@ -33,6 +33,30 @@ type CapabilityResponse struct { SupportedPinModes [][]pin.Func } +func ParseCapabilityResponse(data []byte) CapabilityResponse { + var response = CapabilityResponse{ + PinToModeToResolution: []map[pin.Func]uint8{{}}, + SupportedPinModes: [][]pin.Func{{}}, + } + + var pindex = 0 + for i := 0; i < len(data); { + if data[i] == CapabilityResponsePinDelimiter { + response.PinToModeToResolution = append(response.PinToModeToResolution, map[pin.Func]uint8{}) + response.SupportedPinModes = append(response.SupportedPinModes, []pin.Func{}) + i += 1 + pindex++ + } else { + pinFunc := pinModeToFuncMap[data[i]] + response.PinToModeToResolution[pindex][pinFunc] = data[i+1] + response.SupportedPinModes[pindex] = append(response.SupportedPinModes[pindex], pinFunc) + i += 2 + } + } + + return response +} + func (c CapabilityResponse) String() string { str := bytes.Buffer{} for p, modeMap := range c.PinToModeToResolution { diff --git a/firmata/client.go b/firmata/client.go index 03a5549..0af9ace 100644 --- a/firmata/client.go +++ b/firmata/client.go @@ -15,8 +15,10 @@ import ( // These max values are for data bytes as, within firmata, data is 7 bits long. const ( - MaxUInt8 uint8 = (1<<8 - 1) >> 1 - MaxUInt16 uint16 = (1<<16 - 1) >> 2 + MaxUInt8 uint8 = (1<<8 - 1) >> (8 / 8) + MaxUInt16 uint16 = (1<<16 - 1) >> (16 / 8) + MaxUInt24 uint32 = (1<<24 - 1) >> (24 / 8) + MaxUInt32 uint32 = (1<<32 - 1) >> (32 / 8) ) var commandResponseMap = map[SysExCmd]SysExCmd{ @@ -28,7 +30,7 @@ var commandResponseMap = map[SysExCmd]SysExCmd{ type ClientI interface { SendSysEx(SysExCmd, ...byte) (chan []byte, error) SendReset() error - ExtendedReportAnalogPin(uint8, int) error + ExtendedReportAnalogPin(uint8, uint8) error CapabilityQuery() (chan CapabilityResponse, error) PinStateQuery(uint8) (chan PinStateResponse, error) ReportFirmware() (chan FirmwareReport, error) @@ -124,6 +126,10 @@ func (c *Client) Start() error { c.started = true c.mu.Unlock() + if err := c.SendReset(); err != nil { + return err + } + if b, ok := c.board.(flusher); ok { b.Flush() } else if b, ok := c.board.(flusherErr); ok { @@ -132,12 +138,16 @@ func (c *Client) Start() error { } } - firmChannel := make(chan []byte, 1) // Don't call ReportFirmware as it is automatic, but we want to register a listener for it. + firmChannel := make(chan []byte, 1) c.mu.Lock() c.responseChannels[SysExReportFirmware] = []chan []byte{firmChannel} c.mu.Unlock() - report := c.parseReportFirmware(firmChannel) + + responseChannel := make(chan FirmwareReport, 1) + go func() { + responseChannel <- ParseFirmwareReport(<-firmChannel) + }() go func() { err := c.responseWatcher() @@ -146,7 +156,22 @@ func (c *Client) Start() error { } }() - fmt.Println("Firmware Info:", <-report) + // Do not move on until we receive our firmware report. This is a signal the device is ready. + // Anything sent before we receive this message will not be read by the device. + fmt.Println("Firmware Info:", <-responseChannel) + + analogMappingQuery, err := c.SendAnalogMappingQuery() + if err != nil { + return err + } + + capabilityQuery, err := c.CapabilityQuery() + if err != nil { + return err + } + + fmt.Println(<-analogMappingQuery) + fmt.Println(<-capabilityQuery) return nil } @@ -156,8 +181,6 @@ func (c *Client) write(payload []byte, withinMutex func()) error { c.mu.Lock() defer c.mu.Unlock() - //fmt.Println(SprintHexArray(payload)) - // Write to the board. _, err := c.board.Write(payload) if err != nil { @@ -280,6 +303,8 @@ func (c *Client) responseWatcher() (err error) { resp <- data close(resp) case cmd == SysExStringData: + // TODO: The Firmata spec defines this as mostly an error statement. + // Should we fail on receiving a string message? fmt.Printf("device: [%s]\n", TwoByteString(data)) default: str := "" @@ -347,41 +372,18 @@ func (c *Client) CapabilityQuery() (chan CapabilityResponse, error) { return nil, err } - resp := c.parseCapabilityCommand(future) - - return resp, nil -} - -func (c *Client) parseCapabilityCommand(future chan []byte) chan CapabilityResponse { resp := make(chan CapabilityResponse, 1) go func() { data := <-future - var response = CapabilityResponse{ - PinToModeToResolution: []map[pin.Func]uint8{{}}, - SupportedPinModes: [][]pin.Func{{}}, - } - var pindex = 0 - for i := 0; i < len(data); { - if data[i] == CapabilityResponsePinDelimiter { - response.PinToModeToResolution = append(response.PinToModeToResolution, map[pin.Func]uint8{}) - response.SupportedPinModes = append(response.SupportedPinModes, []pin.Func{}) - i += 1 - pindex++ - } else { - pinFunc := pinModeToFuncMap[data[i]] - response.PinToModeToResolution[pindex][pinFunc] = data[i+1] - response.SupportedPinModes[pindex] = append(response.SupportedPinModes[pindex], pinFunc) - i += 2 - } - } + var response = ParseCapabilityResponse(data) c.cr = response resp <- response close(resp) }() - return resp + return resp, nil } func (c *Client) SendAnalogMappingQuery() (chan AnalogMappingResponse, error) { @@ -390,46 +392,18 @@ func (c *Client) SendAnalogMappingQuery() (chan AnalogMappingResponse, error) { return nil, err } - resp := c.parseAnalogMappingQuery(future) - - return resp, nil -} - -func (c *Client) parseAnalogMappingQuery(future chan []byte) chan AnalogMappingResponse { resp := make(chan AnalogMappingResponse, 1) go func() { data := <-future - var response = AnalogMappingResponse{ - AnalogPinToDigital: []uint8{}, - DigitalPinToAnalog: map[uint8]uint8{}, - } - for i := 0; i < len(data); i++ { - if data[i] != CapabilityResponsePinDelimiter { - response.DigitalPinToAnalog[uint8(i)] = uint8(len(response.AnalogPinToDigital)) - response.AnalogPinToDigital = append(response.AnalogPinToDigital, uint8(i)) - } - } + var response = ParseAnalogMappingResponse(data) c.amr = response resp <- response close(resp) }() - return resp -} - -func (c *Client) ExtendedReportAnalogPin(p uint8, value int) error { - if value > 0xFFFFFFFFFFFFFF { - return fmt.Errorf("%w: 0x0 - 0xFFFFFFFFFFFFFF", ErrValueOutOfRange) - } - - _, err := c.SendSysEx(SysExExtendedAnalog, p, uint8(value), uint8(value>>7), uint8(value>>14)) - if err != nil { - return err - } - - return nil + return resp, nil } func (c *Client) PinStateQuery(p uint8) (chan PinStateResponse, error) { @@ -438,31 +412,17 @@ func (c *Client) PinStateQuery(p uint8) (chan PinStateResponse, error) { return nil, err } - resp := c.parsePinStateQuery(future) - - return resp, nil -} - -func (c *Client) parsePinStateQuery(future chan []byte) chan PinStateResponse { resp := make(chan PinStateResponse, 1) go func() { data := <-future - var ps = PinStateResponse{ - Pin: data[0], - Mode: pinModeToFuncMap[data[1]], - State: 0, - } + var response = ParsePinStateResponse(data) - for i, b := range data[2:] { - ps.State |= int(b << (i * 7)) - } - - resp <- ps + resp <- response close(resp) }() - return resp + return resp, nil } func (c *Client) ReportFirmware() (chan FirmwareReport, error) { @@ -471,27 +431,28 @@ func (c *Client) ReportFirmware() (chan FirmwareReport, error) { return nil, err } - resp := c.parseReportFirmware(future) - - return resp, nil -} - -func (c *Client) parseReportFirmware(future chan []byte) chan FirmwareReport { resp := make(chan FirmwareReport, 1) go func() { data := <-future - var rc = FirmwareReport{ - Major: data[0], - Minor: data[1], - Name: data[2:], - } + var response = ParseFirmwareReport(data) - resp <- rc + resp <- response close(resp) }() - return resp + return resp, nil +} + +func (c *Client) ExtendedReportAnalogPin(p uint8, value uint8) error { + lsb, msb := ByteToTwoByte(value) + + _, err := c.SendSysEx(SysExExtendedAnalog, p, lsb, msb) + if err != nil { + return err + } + + return nil } func (c *Client) SetAnalogPinReporting(analogPin uint8, report bool) error { @@ -523,7 +484,7 @@ func (c *Client) SetSamplingInterval(ms uint16) error { return c.write([]byte{byte(SysExSamplingInterval), byte(ms), byte(ms >> 7)}, nil) } -// This function only supports 7-bit I2C addresses +// WriteI2CData only supports 7-bit I2C addresses func (c *Client) WriteI2CData(address uint8, restart bool, data []uint8) error { if !c.i2cStarted { return ErrI2CNotEnabled @@ -539,14 +500,14 @@ func (c *Client) WriteI2CData(address uint8, restart bool, data []uint8) error { return err } -// This function only supports 7-bit I2C addresses +// ReadI2CData only supports 7-bit I2C addresses func (c *Client) ReadI2CData(address uint8, restart bool, length uint16) error { if !c.i2cStarted { return ErrI2CNotEnabled } if length > MaxUInt16 { - return fmt.Errorf("%w: 0x0 - 0xFFFFFFFFFFFFFF", ErrValueOutOfRange) + return fmt.Errorf("%w: 0x0 - 0x%X", ErrValueOutOfRange, MaxUInt16) } byte2 := byte(I2CModeRead) @@ -560,14 +521,14 @@ func (c *Client) ReadI2CData(address uint8, restart bool, length uint16) error { return err } -// This function only supports 7-bit I2C addresses +// ReadI2CRegister only supports 7-bit I2C addresses func (c *Client) ReadI2CRegister(address uint8, restart bool, register uint8, length uint16) error { if !c.i2cStarted { return ErrI2CNotEnabled } if length > MaxUInt16 { - return fmt.Errorf("%w: 0x0 - 0xFFFFFFFFFFFFFF", ErrValueOutOfRange) + return fmt.Errorf("%w: 0x0 - 0x%X", ErrValueOutOfRange, MaxUInt16) } byte2 := byte(I2CModeRead) @@ -599,7 +560,7 @@ func (c *Client) releaseI2CAddressListener(addr uint8) { delete(c.i2cListeners, addr) } -// This function only supports 7-bit I2C addresses +// SetI2CAddressListener only supports 7-bit I2C addresses func (c *Client) SetI2CAddressListener(addr uint8, ch chan I2CPacket) (release func(), err error) { c.i2cMU.Lock() defer c.i2cMU.Unlock() diff --git a/firmata/firmatareg/firmatareg.go b/firmata/firmatareg/firmatareg.go deleted file mode 100644 index af86d3b..0000000 --- a/firmata/firmatareg/firmatareg.go +++ /dev/null @@ -1,216 +0,0 @@ -package firmatareg - -import ( - "errors" - "strconv" - "strings" - "sync" - - "periph.io/x/devices/v3/firmata" -) - -// Opener opens a handle to a firmata device. -// -// It is provided by the actual device driver. -type Opener func() (firmata.ClientI, error) - -// Ref references a Firmata device. -// -// It is returned by All() to enumerate all registered devices. -type Ref struct { - // Name of the device. - // - // It must be unique across the host. - Name string - // Aliases are the alternative names that can be used to reference this bus. - Aliases []string - // Open is the factory to open a handle to this Firmata device. - Open Opener -} - -// Open opens a firmata device by its name, an alias or its file path and returns -// a handle to it. -// -// Specify the empty string "" to get the first available device. This is the -// recommended default value unless an application knows the exact device to use. -// -// Each device can register multiple aliases, each leading to the same device handle. -// -// "file path" is a generic concept that is highly dependent on the platform -// and OS. Depending on the OS, a serial port can be `/dev/tty*` or `COM*`. -func Open(name string) (firmata.ClientI, error) { - var r *Ref - var err error - func() { - mu.Lock() - defer mu.Unlock() - if len(byName) == 0 { - err = errors.New("firmatareg: no device found; did you forget to call Init()") - return - } - if len(name) == 0 { - r = getDefault() - return - } - // Try by name, by alias, by number. - if r = byName[name]; r == nil { - r = byAlias[name] - } - }() - if err != nil { - return nil, err - } - if r == nil { - return nil, errors.New("firmatareg: can't open unknown device: " + strconv.Quote(name)) - } - return r.Open() -} - -// All returns a copy of all the registered references to all know firmata devices -// available on this host. -// -// The list is sorted by the bus name. -func All() []*Ref { - mu.Lock() - defer mu.Unlock() - out := make([]*Ref, 0, len(byName)) - for _, v := range byName { - r := &Ref{Name: v.Name, Aliases: make([]string, len(v.Aliases)), Open: v.Open} - copy(r.Aliases, v.Aliases) - out = insertRef(out, r) - } - return out -} - -// Register registers a firmata device. -// -// Registering the same device name twice is an error, e.g. o.Name(). -func Register(name string, aliases []string, o Opener) error { - if len(name) == 0 { - return errors.New("firmatareg: can't register a bus with no name") - } - if o == nil { - return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " with nil Opener") - } - if _, err := strconv.Atoi(name); err == nil { - return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " with name being only a number") - } - if strings.Contains(name, ":") { - return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " with name containing ':'") - } - for _, alias := range aliases { - if len(alias) == 0 { - return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " with an empty alias") - } - if name == alias { - return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " with an alias the same as the bus name") - } - if _, err := strconv.Atoi(alias); err == nil { - return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " with an alias that is a number: " + strconv.Quote(alias)) - } - if strings.Contains(alias, ":") { - return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " with an alias containing ':': " + strconv.Quote(alias)) - } - } - - mu.Lock() - defer mu.Unlock() - if _, ok := byName[name]; ok { - return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " twice") - } - if _, ok := byAlias[name]; ok { - return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " twice; it is already an alias") - } - for _, alias := range aliases { - if _, ok := byName[alias]; ok { - return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " twice; alias " + strconv.Quote(alias) + " is already a bus") - } - if _, ok := byAlias[alias]; ok { - return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " twice; alias " + strconv.Quote(alias) + " is already an alias") - } - } - - r := &Ref{Name: name, Aliases: make([]string, len(aliases)), Open: o} - copy(r.Aliases, aliases) - byName[name] = r - for _, alias := range aliases { - byAlias[alias] = r - } - return nil -} - -// Unregister removes a previously registered firmata device. -// -// This can happen when a firmata device is exposed via a USB device and the device -// is unplugged. -func Unregister(name string) error { - mu.Lock() - defer mu.Unlock() - r := byName[name] - if r == nil { - return errors.New("firmatareg: can't unregister unknown bus name " + strconv.Quote(name)) - } - delete(byName, name) - for _, alias := range r.Aliases { - delete(byAlias, alias) - } - return nil -} - -// - -var ( - mu sync.Mutex - byName = map[string]*Ref{} - // Caches - byNumber = map[int]*Ref{} - byAlias = map[string]*Ref{} -) - -// getDefault returns the Ref that should be used as the default bus. -func getDefault() *Ref { - var o *Ref - if len(byNumber) == 0 { - // Fallback to use byName using a lexical sort. - name := "" - for n, o2 := range byName { - if len(name) == 0 || n < name { - o = o2 - name = n - } - } - return o - } - number := int((^uint(0)) >> 1) - for n, o2 := range byNumber { - if number > n { - number = n - o = o2 - } - } - return o -} - -func insertRef(l []*Ref, r *Ref) []*Ref { - n := r.Name - i := search(len(l), func(i int) bool { return l[i].Name > n }) - l = append(l, nil) - copy(l[i+1:], l[i:]) - l[i] = r - return l -} - -// search implements the same algorithm as sort.Search(). -// -// It was extracted to not depend on sort, which depends on reflect. -func search(n int, f func(int) bool) int { - lo := 0 - for hi := n; lo < hi; { - if i := int(uint(lo+hi) >> 1); !f(i) { - lo = i + 1 - } else { - hi = i - } - } - return lo -} diff --git a/firmata/firmware.go b/firmata/firmware.go index 7a0b00c..f47e36e 100644 --- a/firmata/firmware.go +++ b/firmata/firmware.go @@ -10,6 +10,14 @@ type FirmwareReport struct { Name []byte } +func ParseFirmwareReport(data []byte) FirmwareReport { + return FirmwareReport{ + Major: data[0], + Minor: data[1], + Name: data[2:], + } +} + func (r FirmwareReport) String() string { return fmt.Sprintf("%s [%d.%d]", TwoByteString(r.Name), r.Major, r.Minor) } diff --git a/firmata/pin.go b/firmata/pin.go index 285ed12..0ea9e9e 100644 --- a/firmata/pin.go +++ b/firmata/pin.go @@ -173,13 +173,17 @@ func (p *Pin) DefaultPull() gpio.Pull { } func (p *Pin) Out(l gpio.Level) error { - // TODO: - panic("implement me") + // No need to set mode as firmata does so automatically + return p.c.SetDigitalPinValue(p.pin, l) } +const dutyMax gpio.Duty = 1<<8 - 1 + +// PWM ignores physic.Frequency as there is no way to set it through firmata func (p *Pin) PWM(duty gpio.Duty, f physic.Frequency) error { - // TODO: - panic("implement me") + // No need to set mode as firmata does so automatically + // PWM duty scaled down from 24 to 8 bits + return p.c.ExtendedReportAnalogPin(p.pin, uint8((duty>>16)&0xFF)) } func (p *Pin) Func() pin.Func { diff --git a/firmata/pin_state.go b/firmata/pin_state.go index 9815eb5..56b84fa 100644 --- a/firmata/pin_state.go +++ b/firmata/pin_state.go @@ -12,6 +12,20 @@ type PinStateResponse struct { State int } +func ParsePinStateResponse(data []byte) PinStateResponse { + var response = PinStateResponse{ + Pin: data[0], + Mode: pinModeToFuncMap[data[1]], + State: 0, + } + + for i, b := range data[2:] { + response.State |= int(b << (i * 7)) + } + + return response +} + func (p PinStateResponse) String() string { return fmt.Sprintf("pin(%d) mode(%s) state(%d)", p.Pin, p.Mode, p.State) } diff --git a/pca9685/pins.go b/pca9685/pins.go index d5f75bc..6d007a9 100644 --- a/pca9685/pins.go +++ b/pca9685/pins.go @@ -96,7 +96,7 @@ func (p *pin) Read() gpio.Level { } func (p *pin) WaitForEdge(timeout time.Duration) bool { - return false + return gpio.INVALID.WaitForEdge(timeout) } func (p *pin) Pull() gpio.Pull { From b9ac491105564d4eb3a6c37b6449cb2a879a6b6b Mon Sep 17 00:00:00 2001 From: imle Date: Mon, 11 Oct 2021 21:47:48 -0400 Subject: [PATCH 4/4] cleanup of pin function names --- firmata/pin_mode.go | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/firmata/pin_mode.go b/firmata/pin_mode.go index 7cd9a9e..96ddf3f 100644 --- a/firmata/pin_mode.go +++ b/firmata/pin_mode.go @@ -5,9 +5,9 @@ import ( ) const ( - PinFuncDigitalInput pin.Func = "DigitalInput" - PinFuncDigitalOutput pin.Func = "DigitalOutput" - PinFuncAnalogInput pin.Func = "AnalogInput" + PinFuncDigitalInput pin.Func = "Digital Input" + PinFuncDigitalOutput pin.Func = "Digital Output" + PinFuncAnalogInput pin.Func = "Analog Input" PinFuncPWM pin.Func = "PWM" PinFuncServo pin.Func = "Servo" PinFuncShift pin.Func = "Shift" @@ -16,7 +16,7 @@ const ( PinFuncStepper pin.Func = "Stepper" PinFuncEncoder pin.Func = "Encoder" PinFuncSerial pin.Func = "Serial" - PinFuncInputPullUp pin.Func = "InputPullUp" + PinFuncInputPullUp pin.Func = "Input Pull-Up" PinFuncSPI pin.Func = "SPI" PinFuncSonar pin.Func = "Sonar" PinFuncTone pin.Func = "Tone"