mirror of
https://github.com/hybridgroup/gobot.git
synced 2025-05-11 19:29:20 +08:00
386 lines
9.8 KiB
Go
386 lines
9.8 KiB
Go
package firmata
|
|
|
|
import (
|
|
"bytes"
|
|
"fmt"
|
|
"io"
|
|
"math"
|
|
"time"
|
|
)
|
|
|
|
const (
|
|
Open byte = 1
|
|
Close byte = 0
|
|
Input byte = 0x00
|
|
Output byte = 0x01
|
|
Analog byte = 0x02
|
|
PWM byte = 0x03
|
|
Servo byte = 0x04
|
|
Low byte = 0
|
|
High byte = 1
|
|
ReportVersion byte = 0xF9
|
|
SystemReset byte = 0xFF
|
|
DigitalMessage byte = 0x90
|
|
DigitalMessageRangeStart byte = 0x90
|
|
DigitalMessageRangeEnd byte = 0x9F
|
|
AnalogMessage byte = 0xE0
|
|
AnalogMessageRangeStart byte = 0xE0
|
|
AnalogMessageRangeEnd byte = 0xEF
|
|
ReportAnalog byte = 0xC0
|
|
ReportDigital byte = 0xD0
|
|
PinMode byte = 0xF4
|
|
StartSysex byte = 0xF0
|
|
EndSysex byte = 0xF7
|
|
CapabilityQuery byte = 0x6B
|
|
CapabilityResponse byte = 0x6C
|
|
PinStateQuery byte = 0x6D
|
|
PinStateResponse byte = 0x6E
|
|
AnalogMappingQuery byte = 0x69
|
|
AnalogMappingResponse byte = 0x6A
|
|
StringData byte = 0x71
|
|
I2CRequest byte = 0x76
|
|
I2CReply byte = 0x77
|
|
I2CConfig byte = 0x78
|
|
FirmwareQuery byte = 0x79
|
|
I2CModeWrite byte = 0x00
|
|
I2CModeRead byte = 0x01
|
|
I2CmodeContinuousRead byte = 0x02
|
|
I2CModeStopReading byte = 0x03
|
|
)
|
|
|
|
type board struct {
|
|
Serial io.ReadWriteCloser
|
|
Pins []pin
|
|
AnalogPins []byte
|
|
FirmwareName string
|
|
MajorVersion byte
|
|
MinorVersion byte
|
|
Events []event
|
|
Connected bool
|
|
}
|
|
|
|
type pin struct {
|
|
SupportedModes []byte
|
|
Mode byte
|
|
Value int
|
|
AnalogChannel byte
|
|
}
|
|
|
|
type event struct {
|
|
Name string
|
|
Data []byte
|
|
I2cReply map[string][]byte
|
|
}
|
|
|
|
func newBoard(sp io.ReadWriteCloser) *board {
|
|
board := new(board)
|
|
board.MajorVersion = 0
|
|
board.MinorVersion = 0
|
|
board.Serial = sp
|
|
board.FirmwareName = ""
|
|
board.Pins = make([]pin, 100)
|
|
board.AnalogPins = make([]byte, 0)
|
|
board.Connected = false
|
|
board.Events = make([]event, 0)
|
|
return board
|
|
}
|
|
|
|
func (b *board) connect() {
|
|
if b.Connected == false {
|
|
b.initBoard()
|
|
b.Connected = true
|
|
|
|
go func() {
|
|
for {
|
|
b.queryReportVersion()
|
|
time.Sleep(50 * time.Millisecond)
|
|
b.readAndProcess()
|
|
}
|
|
}()
|
|
}
|
|
}
|
|
|
|
func (b *board) initBoard() {
|
|
for {
|
|
b.queryFirmware()
|
|
time.Sleep(50 * time.Millisecond)
|
|
b.readAndProcess()
|
|
if len(b.findEvents("firmware_query")) > 0 {
|
|
break
|
|
}
|
|
}
|
|
for {
|
|
b.queryCapabilities()
|
|
time.Sleep(50 * time.Millisecond)
|
|
b.readAndProcess()
|
|
if len(b.findEvents("capability_query")) > 0 {
|
|
break
|
|
}
|
|
}
|
|
for {
|
|
b.queryAnalogMapping()
|
|
time.Sleep(50 * time.Millisecond)
|
|
b.readAndProcess()
|
|
if len(b.findEvents("analog_mapping_query")) > 0 {
|
|
break
|
|
}
|
|
}
|
|
b.togglePinReporting(0, High, ReportDigital)
|
|
time.Sleep(50 * time.Millisecond)
|
|
b.togglePinReporting(1, High, ReportDigital)
|
|
time.Sleep(50 * time.Millisecond)
|
|
}
|
|
|
|
func (b *board) findEvents(name string) []event {
|
|
ret := []event{}
|
|
for key, val := range b.Events {
|
|
if val.Name == name {
|
|
ret = append(ret, val)
|
|
if len(b.Events) > key+1 {
|
|
b.Events = append(b.Events[:key], b.Events[key+1:]...)
|
|
}
|
|
}
|
|
}
|
|
return ret
|
|
}
|
|
|
|
func (b *board) readAndProcess() {
|
|
b.process(b.read())
|
|
}
|
|
|
|
func (b *board) reset() {
|
|
b.write([]byte{SystemReset})
|
|
}
|
|
|
|
func (b *board) setPinMode(pin byte, mode byte) {
|
|
b.Pins[pin].Mode = mode
|
|
b.write([]byte{PinMode, pin, mode})
|
|
}
|
|
|
|
func (b *board) digitalWrite(pin byte, value byte) {
|
|
port := byte(math.Floor(float64(pin) / 8))
|
|
portValue := byte(0)
|
|
|
|
b.Pins[pin].Value = int(value)
|
|
|
|
for i := byte(0); i < 8; i++ {
|
|
if b.Pins[8*port+i].Value != 0 {
|
|
portValue = portValue | (1 << i)
|
|
}
|
|
}
|
|
b.write([]byte{DigitalMessage | port, portValue & 0x7F, (portValue >> 7) & 0x7F})
|
|
}
|
|
|
|
func (b *board) analogWrite(pin byte, value byte) {
|
|
b.Pins[pin].Value = int(value)
|
|
b.write([]byte{AnalogMessage | pin, value & 0x7F, (value >> 7) & 0x7F})
|
|
}
|
|
|
|
func (b *board) version() string {
|
|
return fmt.Sprintf("%v.%v", b.MajorVersion, b.MinorVersion)
|
|
}
|
|
|
|
func (b *board) reportVersion() {
|
|
b.write([]byte{ReportVersion})
|
|
}
|
|
|
|
func (b *board) queryFirmware() {
|
|
b.write([]byte{StartSysex, FirmwareQuery, EndSysex})
|
|
}
|
|
|
|
func (b *board) queryPinState(pin byte) {
|
|
b.write([]byte{StartSysex, PinStateQuery, pin, EndSysex})
|
|
}
|
|
|
|
func (b *board) queryReportVersion() {
|
|
b.write([]byte{ReportVersion})
|
|
}
|
|
|
|
func (b *board) queryCapabilities() {
|
|
b.write([]byte{StartSysex, CapabilityQuery, EndSysex})
|
|
}
|
|
|
|
func (b *board) queryAnalogMapping() {
|
|
b.write([]byte{StartSysex, AnalogMappingQuery, EndSysex})
|
|
}
|
|
|
|
func (b *board) togglePinReporting(pin byte, state byte, mode byte) {
|
|
b.write([]byte{mode | pin, state})
|
|
}
|
|
|
|
func (b *board) i2cReadRequest(slaveAddress byte, numBytes uint) {
|
|
b.write([]byte{StartSysex, I2CRequest, slaveAddress, (I2CModeRead << 3), byte(numBytes & 0x7F), byte(((numBytes >> 7) & 0x7F)), EndSysex})
|
|
}
|
|
|
|
func (b *board) i2cWriteRequest(slaveAddress byte, data []byte) {
|
|
ret := []byte{StartSysex, I2CRequest, slaveAddress, (I2CModeWrite << 3)}
|
|
for _, val := range data {
|
|
ret = append(ret, byte(val&0x7F))
|
|
ret = append(ret, byte((val>>7)&0x7F))
|
|
}
|
|
ret = append(ret, EndSysex)
|
|
b.write(ret)
|
|
}
|
|
|
|
func (b *board) i2cConfig(data []byte) {
|
|
ret := []byte{StartSysex, I2CConfig}
|
|
for _, val := range data {
|
|
ret = append(ret, byte(val&0xFF))
|
|
ret = append(ret, byte((val>>8)&0xFF))
|
|
}
|
|
ret = append(ret, EndSysex)
|
|
b.write(ret)
|
|
}
|
|
|
|
func (b *board) write(commands []byte) {
|
|
b.Serial.Write(commands[:])
|
|
}
|
|
|
|
func (b *board) read() []byte {
|
|
buf := make([]byte, 1024)
|
|
b.Serial.Read(buf)
|
|
return buf
|
|
}
|
|
|
|
func (b *board) process(data []byte) {
|
|
buf := bytes.NewBuffer(data)
|
|
for {
|
|
messageType, err := buf.ReadByte()
|
|
if err != nil {
|
|
break
|
|
}
|
|
switch {
|
|
case ReportVersion == messageType:
|
|
b.MajorVersion, _ = buf.ReadByte()
|
|
b.MinorVersion, _ = buf.ReadByte()
|
|
b.Events = append(b.Events, event{Name: "report_version"})
|
|
case AnalogMessageRangeStart <= messageType && AnalogMessageRangeEnd >= messageType:
|
|
leastSignificantByte, _ := buf.ReadByte()
|
|
mostSignificantByte, _ := buf.ReadByte()
|
|
|
|
value := uint(leastSignificantByte) | uint(mostSignificantByte)<<7
|
|
pin := (messageType & 0x0F)
|
|
|
|
b.Pins[b.AnalogPins[pin]].Value = int(value)
|
|
b.Events = append(b.Events, event{Name: fmt.Sprintf("analog_read_%v", pin), Data: []byte{byte(value >> 24), byte(value >> 16), byte(value >> 8), byte(value & 0xff)}})
|
|
|
|
case DigitalMessageRangeStart <= messageType && DigitalMessageRangeEnd >= messageType:
|
|
port := messageType & 0x0F
|
|
firstBitmask, _ := buf.ReadByte()
|
|
secondBitmask, _ := buf.ReadByte()
|
|
portValue := firstBitmask | (secondBitmask << 7)
|
|
|
|
for i := 0; i < 8; i++ {
|
|
pinNumber := (8*byte(port) + byte(i))
|
|
pin := b.Pins[pinNumber]
|
|
if byte(pin.Mode) == Input {
|
|
pin.Value = int((portValue >> (byte(i) & 0x07)) & 0x01)
|
|
b.Events = append(b.Events, event{Name: fmt.Sprintf("digital_read_%v", pinNumber), Data: []byte{byte(pin.Value & 0xff)}})
|
|
}
|
|
}
|
|
|
|
case StartSysex == messageType:
|
|
currentBuffer := []byte{messageType}
|
|
for {
|
|
b, err := buf.ReadByte()
|
|
if err != nil {
|
|
break
|
|
}
|
|
currentBuffer = append(currentBuffer, b)
|
|
if currentBuffer[len(currentBuffer)-1] == EndSysex {
|
|
break
|
|
}
|
|
}
|
|
command := currentBuffer[1]
|
|
switch command {
|
|
case CapabilityResponse:
|
|
supportedModes := 0
|
|
n := 0
|
|
|
|
for _, val := range currentBuffer[2:(len(currentBuffer) - 5)] {
|
|
if val == 127 {
|
|
modes := []byte{}
|
|
for _, mode := range []byte{Input, Output, Analog, PWM, Servo} {
|
|
if (supportedModes & (1 << mode)) != 0 {
|
|
modes = append(modes, mode)
|
|
}
|
|
}
|
|
b.Pins = append(b.Pins, pin{modes, Output, 0, 0})
|
|
supportedModes = 0
|
|
n = 0
|
|
continue
|
|
}
|
|
|
|
if n == 0 {
|
|
supportedModes = supportedModes | (1 << val)
|
|
}
|
|
n ^= 1
|
|
}
|
|
b.Events = append(b.Events, event{Name: "capability_query"})
|
|
|
|
case AnalogMappingResponse:
|
|
pinIndex := byte(0)
|
|
|
|
for _, val := range currentBuffer[2 : len(currentBuffer)-1] {
|
|
|
|
b.Pins[pinIndex].AnalogChannel = val
|
|
|
|
if val != 127 {
|
|
b.AnalogPins = append(b.AnalogPins, pinIndex)
|
|
}
|
|
|
|
pinIndex++
|
|
}
|
|
|
|
b.Events = append(b.Events, event{Name: "analog_mapping_query"})
|
|
|
|
case PinStateResponse:
|
|
pin := b.Pins[currentBuffer[2]]
|
|
pin.Mode = currentBuffer[3]
|
|
pin.Value = int(currentBuffer[4])
|
|
|
|
if len(currentBuffer) > 6 {
|
|
pin.Value = int(uint(pin.Value) | uint(currentBuffer[5])<<7)
|
|
}
|
|
if len(currentBuffer) > 7 {
|
|
pin.Value = int(uint(pin.Value) | uint(currentBuffer[6])<<14)
|
|
}
|
|
|
|
b.Events = append(b.Events, event{Name: fmt.Sprintf("pin_%v_state", currentBuffer[2]), Data: []byte{byte(pin.Value & 0xff)}})
|
|
case I2CReply:
|
|
i2cReply := map[string][]byte{
|
|
"slave_address": []byte{byte(currentBuffer[2]) | byte(currentBuffer[3])<<7},
|
|
"register": []byte{byte(currentBuffer[4]) | byte(currentBuffer[5])<<7},
|
|
"data": []byte{byte(currentBuffer[6]) | byte(currentBuffer[7])<<7},
|
|
}
|
|
for i := 8; i < len(currentBuffer); i = i + 2 {
|
|
if currentBuffer[i] == byte(0xF7) {
|
|
break
|
|
}
|
|
if i+2 > len(currentBuffer) {
|
|
break
|
|
}
|
|
i2cReply["data"] = append(i2cReply["data"], byte(currentBuffer[i])|byte(currentBuffer[i+1])<<7)
|
|
}
|
|
b.Events = append(b.Events, event{Name: "i2c_reply", I2cReply: i2cReply})
|
|
|
|
case FirmwareQuery:
|
|
name := []byte{}
|
|
for _, val := range currentBuffer[4:(len(currentBuffer) - 1)] {
|
|
if val != 0 {
|
|
name = append(name, val)
|
|
}
|
|
}
|
|
b.FirmwareName = string(name[:])
|
|
b.Events = append(b.Events, event{Name: "firmware_query"})
|
|
case StringData:
|
|
str := currentBuffer[2 : len(currentBuffer)-1]
|
|
fmt.Println(string(str[:len(str)]))
|
|
b.Events = append(b.Events, event{Name: "string_data", Data: str})
|
|
default:
|
|
fmt.Println("bad byte", fmt.Sprintf("0x%x", command))
|
|
}
|
|
}
|
|
}
|
|
}
|