implementation instructions for lx servo

Signed-off-by: jet tsang zeon-git <zeon-git@jettsang.com>

8 files changed, 582 insertions, 0 deletions

diff --git a/iface/iface.go b/iface/iface.go
new file mode 100644
index 0000000..5c73252
--- /dev/null
+++ b/iface/iface.go
@@ -0,0 +1,19 @@
+package iface
+
+// TODO: Use an io.writer instead?
+type Logger interface {
+	Printf(format string, v ...interface{})
+}
+
+// Protocol provides an abstract interface to command servos. This exists so
+// that our abstract Servo type can communicate with actual servos regardless
+// which protocol version they speak.
+//
+// The interface must be the union of all protocol versions, but (so far) they
+// all have roughly the same instructions, so this isn't a big deal.
+type Protocol interface {
+	ReadData(ident int, instruction byte) ([]byte, error)
+	// WriteData writes a slice of bytes to the control table of the given servo
+	// ID.
+	WriteData(ident int, instruction byte, data []byte) error
+}
diff --git a/network/network.go b/network/network.go
new file mode 100644
index 0000000..2b79ad7
--- /dev/null
+++ b/network/network.go
@@ -0,0 +1,96 @@
+package network
+
+import (
+	"fmt"
+	"io"
+	"time"
+
+	"git.jettsang.com/drivers/lobot/iface"
+)
+
+const (
+
+	// Send an instruction to all servos
+	BroadcastIdent byte = 0xFE // 254
+)
+
+type Network struct {
+	Serial io.ReadWriteCloser
+
+	// The time to wait for a single read to complete before giving up.
+	Timeout time.Duration
+
+	// Optional Logger (which only implements Printf) to log network traffic. If
+	// nil (the default), nothing is logged.
+	Logger iface.Logger
+}
+
+func New(serial io.ReadWriteCloser) *Network {
+	return &Network{
+		Serial:  serial,
+		Timeout: 10 * time.Millisecond,
+		Logger:  nil,
+	}
+}
+
+// read receives the next n bytes from the network, blocking if they're not
+// immediately available. Returns a slice containing the bytes read. If the
+// network timeout is reached, returns the bytes read so far (which might be
+// none) and an error.
+func (nw *Network) Read(p []byte) (n int, err error) {
+	start := time.Now()
+	retry := 1 * time.Millisecond
+
+	for n < len(p) {
+		m, err := nw.Serial.Read(p[n:])
+		n += m
+
+		nw.Logf("~~ n=%d, m=%d, err=%v\n", n, m, err)
+
+		// It's okay if we reached the end of the available bytes. They're
+		// probably just not available yet. Other errors are fatal.
+		if err != nil && err != io.EOF {
+			return m, err
+		}
+
+		// If the timeout has been exceeded, abort.
+		if time.Since(start) >= nw.Timeout {
+			return n, fmt.Errorf("read timed out")
+		}
+
+		// If no bytes were read, back off exponentially. This is just to avoid
+		// flooding the network with retries if a servo isn't responding.
+		if m == 0 {
+			time.Sleep(retry)
+			retry *= 2
+		}
+	}
+
+	nw.Logf("<< %#v\n", p)
+	return n, nil
+}
+
+func (nw *Network) Write(p []byte) (int, error) {
+	nw.Logf(">> %#v\n", p)
+	return nw.Serial.Write(p)
+}
+
+func (nw *Network) Flush() {
+	buf := make([]byte, 128)
+	var n int
+
+	for {
+		n, _ = nw.Serial.Read(buf)
+		nw.Logf(".. %v\n", buf)
+		if n == 0 {
+			break
+		}
+	}
+}
+
+// Logf writes a message to the network logger, unless it's nil.
+func (nw *Network) Logf(format string, v ...interface{}) {
+	if nw.Logger != nil {
+		nw.Logger.Printf(format, v...)
+	}
+}
diff --git a/servo/instname_string.go b/servo/instname_string.go
new file mode 100644
index 0000000..f096eb4
--- /dev/null
+++ b/servo/instname_string.go
@@ -0,0 +1,64 @@
+// Code generated by "stringer -type=InstName"; DO NOT EDIT.
+
+package servo
+
+import (
+	"strconv"
+	"strings"
+)
+
+func _() {
+	// An "invalid array index" compiler error signifies that the constant values have changed.
+	// Re-run the stringer command to generate them again.
+	var x [1]struct{}
+	_ = x[MoveTimeWrite-0]
+	_ = x[MoveTimeRead-1]
+	_ = x[MoveTimeWaitWrite-2]
+	_ = x[MoveTimeWaitRead-3]
+	_ = x[MoveStart-4]
+	_ = x[MoveStop-5]
+	_ = x[IDWrite-6]
+	_ = x[IDRead-7]
+	_ = x[AngleOffsetAdjust-8]
+	_ = x[AngleOffsetWrite-9]
+	_ = x[AngleOffsetRead-10]
+	_ = x[AngleLimitWrite-11]
+	_ = x[AngleLimitRead-12]
+	_ = x[VoltageLimitWrite-13]
+	_ = x[VoltageLimitRead-14]
+	_ = x[TemperatureMaxLimitWrite-15]
+	_ = x[TemperatureMaxLimitRead-16]
+	_ = x[TemperatureRead-17]
+	_ = x[VoltageRead-18]
+	_ = x[PositionRead-19]
+	_ = x[MotorModeWrite-20]
+	_ = x[MotorModeRead-21]
+	_ = x[LoadOrUnloadWrite-22]
+	_ = x[LoadOrUnloadRead-23]
+	_ = x[LEDControlWrite-24]
+	_ = x[LEDControlRead-25]
+	_ = x[LEDErrorWrite-26]
+	_ = x[LEDErrorRead-27]
+}
+
+const _InstName_name = "MoveTimeWriteMoveTimeReadMoveTimeWaitWriteMoveTimeWaitReadMoveStartMoveStopIDWriteIDReadAngleOffsetAdjustAngleOffsetWriteAngleOffsetReadAngleLimitWriteAngleLimitReadVoltageLimitWriteVoltageLimitReadTemperatureMaxLimitWriteTemperatureMaxLimitReadTemperatureReadVoltageReadPositionReadMotorModeWriteMotorModeReadLoadOrUnloadWriteLoadOrUnloadReadLEDControlWriteLEDControlReadLEDErrorWriteLEDErrorRead"
+
+var _InstName_index = [...]uint16{0, 13, 25, 42, 58, 67, 75, 82, 88, 105, 121, 136, 151, 165, 182, 198, 222, 245, 260, 271, 283, 297, 310, 327, 343, 358, 372, 385, 397}
+
+func (i InstName) String() string {
+	if i < 0 || i >= InstName(len(_InstName_index)-1) {
+		return "InstName(" + strconv.FormatInt(int64(i), 10) + ")"
+	}
+	return _InstName_name[_InstName_index[i]:_InstName_index[i+1]]
+}
+
+func GetInstName(text string) InstName {
+	keyIndex := strings.Index(_InstName_name, text)
+	for k, v := range _InstName_index {
+		if uint16(keyIndex) == v {
+			return InstName(k)
+		}
+	}
+	return MoveTimeWrite
+
+}
diff --git a/servo/instructions.go b/servo/instructions.go
new file mode 100644
index 0000000..1465827
--- /dev/null
+++ b/servo/instructions.go
@@ -0,0 +1,60 @@
+// +build !windows
+
+/* SPDX-License-Identifier: MIT
+ *
+ * Copyright (C) 2020 jet tsang zeon-git. All Rights Reserved.
+ */
+
+package servo
+
+//go:generate stringer -type=InstName
+type InstName int
+type InstByte byte
+type Access bool
+
+type InstructionMap map[InstName]*Instruction
+
+type Instruction struct {
+	//
+	InstByte byte
+
+	Length int
+
+	Access Access
+	// Min  int
+	// Max  int
+}
+
+const (
+	MoveTimeWrite InstName = iota
+	MoveTimeRead
+	MoveTimeWaitWrite
+	MoveTimeWaitRead
+	MoveStart
+	MoveStop
+	IDWrite
+	IDRead
+	AngleOffsetAdjust
+	AngleOffsetWrite
+	AngleOffsetRead
+	AngleLimitWrite
+	AngleLimitRead
+	VoltageLimitWrite
+	VoltageLimitRead
+	TemperatureMaxLimitWrite
+	TemperatureMaxLimitRead
+	TemperatureRead
+	VoltageRead
+	PositionRead
+	MotorModeWrite
+	MotorModeRead
+	LoadOrUnloadWrite
+	LoadOrUnloadRead
+	LEDControlWrite
+	LEDControlRead
+	LEDErrorWrite
+	LEDErrorRead
+
+	RO Access = false
+	RW Access = true
+)
diff --git a/servo/lx/lx.go b/servo/lx/lx.go
new file mode 100644
index 0000000..773cd97
--- /dev/null
+++ b/servo/lx/lx.go
@@ -0,0 +1,48 @@
+package lx
+
+import (
+	"io"
+
+	"git.jettsang.com/drivers/lobot/protocal"
+	"git.jettsang.com/drivers/lobot/servo"
+)
+
+var Instructions servo.InstructionMap
+
+func init() {
+	Instructions = servo.InstructionMap{
+		servo.MoveTimeWrite:            {1, 4, servo.RW},
+		servo.MoveTimeRead:             {2, 0, servo.RO},
+		servo.MoveTimeWaitWrite:        {7, 4, servo.RW},
+		servo.MoveTimeWaitRead:         {8, 0, servo.RO},
+		servo.MoveStart:                {11, 0, servo.RW},
+		servo.MoveStop:                 {12, 0, servo.RW},
+		servo.IDWrite:                  {13, 1, servo.RW},
+		servo.IDRead:                   {14, 0, servo.RO},
+		servo.AngleOffsetAdjust:        {17, 1, servo.RW},
+		servo.AngleOffsetWrite:         {18, 0, servo.RW},
+		servo.AngleOffsetRead:          {19, 0, servo.RO},
+		servo.AngleLimitWrite:          {20, 4, servo.RW},
+		servo.AngleLimitRead:           {21, 0, servo.RO},
+		servo.VoltageLimitWrite:        {22, 4, servo.RW},
+		servo.VoltageLimitRead:         {23, 0, servo.RO},
+		servo.TemperatureMaxLimitWrite: {24, 1, servo.RW},
+		servo.TemperatureMaxLimitRead:  {25, 0, servo.RO},
+		servo.TemperatureRead:          {26, 0, servo.RO},
+		servo.VoltageRead:              {27, 0, servo.RO},
+		servo.PositionRead:             {28, 0, servo.RO},
+		servo.MotorModeWrite:           {29, 4, servo.RW},
+		servo.MotorModeRead:            {30, 0, servo.RO},
+		servo.LoadOrUnloadWrite:        {31, 1, servo.RW},
+		servo.LoadOrUnloadRead:         {32, 0, servo.RO},
+		servo.LEDControlWrite:          {33, 1, servo.RW},
+		servo.LEDControlRead:           {34, 0, servo.RO},
+		servo.LEDErrorWrite:            {35, 1, servo.RW},
+		servo.LEDErrorRead:             {36, 0, servo.RO},
+	}
+}
+
+// New returns a new LX servo with the given ID.
+func New(network io.ReadWriter, ID int) (*servo.Servo, error) {
+	return servo.New(protocal.New(network), Instructions, ID), nil
+}
diff --git a/servo/servo.go b/servo/servo.go
new file mode 100644
index 0000000..e3f608c
--- /dev/null
+++ b/servo/servo.go
@@ -0,0 +1,88 @@
+/* SPDX-License-Identifier: MIT
+ *
+ * Copyright (C) 2020 jet tsang zeon-git. All Rights Reserved.
+ */
+
+package servo
+
+import (
+	"fmt"
+
+	"git.jettsang.com/drivers/lobot/iface"
+	"git.jettsang.com/drivers/lobot/utils"
+)
+
+const (
+
+	// Unit conversions
+	maxPos          uint16  = 1000
+	maxAngle        float64 = 240
+	positionToAngle float64 = maxAngle / float64(maxPos) // 0.24
+	angleToPosition float64 = 1 / positionToAngle        // 4.16
+)
+
+type Servo struct {
+	Protocol     iface.Protocol
+	ID           int
+	instructions InstructionMap
+}
+
+// New returns a new Servo.
+func New(proto iface.Protocol, instructions InstructionMap, ID int) *Servo {
+	return &Servo{
+		Protocol:     proto,
+		ID:           ID,
+		instructions: instructions,
+	}
+}
+
+func (s *Servo) sendInstruction(i InstName, values []uint16) error {
+	r, ok := s.instructions[i]
+	if !ok {
+		return fmt.Errorf("can't send a unsupported instruction: %v", i)
+	}
+
+	if r.Access == RO {
+		return fmt.Errorf("can't sned a read-only instruction.")
+	}
+
+	var params []byte
+	switch r.Length {
+	case 1:
+		if len(values) != 1 {
+			return fmt.Errorf("invalid values length inputs.")
+		}
+		params = []byte{utils.Low(values[0])}
+	case 4:
+		if len(values) != 2 {
+			return fmt.Errorf("invalid values length inputs.")
+		}
+		params = []byte{
+			utils.Low(values[0]),
+			utils.High(values[0]),
+			utils.Low(values[1]),
+			utils.High(values[1]),
+		}
+	default:
+		return fmt.Errorf("invalid instruction length: %d", r.Length)
+	}
+
+	return s.Protocol.WriteData(s.ID, r.InstByte, params)
+}
+
+func (s *Servo) SendInstruction(i InstName, values []uint16) error {
+	return s.sendInstruction(i, values)
+}
+
+func (s *Servo) getData(i InstName) ([]byte, error) {
+	r, ok := s.instructions[i]
+	if !ok {
+		return nil, fmt.Errorf("can't send a unsupported instruction: %v", i)
+	}
+
+	if r.Access != RO {
+		return nil, fmt.Errorf("can't sned a read-only instruction.")
+	}
+
+	return s.Protocol.ReadData(s.ID, r.InstByte)
+}
diff --git a/servo/servo_accessors.go b/servo/servo_accessors.go
new file mode 100644
index 0000000..3258e74
--- /dev/null
+++ b/servo/servo_accessors.go
@@ -0,0 +1,149 @@
+/* SPDX-License-Identifier: MIT
+ *
+ * Copyright (C) 2020 jet tsang zeon-git. All Rights Reserved.
+ */
+
+package servo
+
+// MoveTimeWrite
+func (s *Servo) MoveTimeWrite(position uint16, ms uint16) error {
+	return s.sendInstruction(MoveTimeWrite, []uint16{position, ms})
+}
+
+// MoveTimeRead
+func (s *Servo) MoveTimeRead() ([]byte, error) {
+	return s.getData(MoveTimeRead)
+}
+
+// MoveTimeWaitWrite
+func (s *Servo) MoveTimeWaitWrite(position uint16, ms uint16) error {
+	return s.sendInstruction(MoveTimeWaitWrite, []uint16{position, ms})
+}
+
+// MoveTimeWaitRead
+func (s *Servo) MoveTimeWaitRead() ([]byte, error) {
+	return s.getData(MoveTimeWaitRead)
+}
+
+// MoveStart
+func (s *Servo) MoveStart() error {
+	return s.sendInstruction(MoveStart, nil)
+}
+
+// MoveStop
+func (s *Servo) MoveStop() ([]byte, error) {
+	return s.getData(MoveStop)
+}
+
+// IDWrite
+func (s *Servo) IDWrite(newID uint16) error {
+	return s.sendInstruction(IDWrite, []uint16{newID})
+}
+
+// IDRead
+func (s *Servo) IDRead() ([]byte, error) {
+	return s.getData(IDRead)
+}
+
+// AngleOffsetAdjust
+func (s *Servo) AngleOffsetAdjust(adjust uint16) error {
+	return s.sendInstruction(AngleOffsetAdjust, []uint16{adjust})
+}
+
+// AngleOffsetWrite: Save Offset to Servo's EEPROM
+func (s *Servo) AngleOffsetWrite() error {
+	return s.sendInstruction(AngleOffsetWrite, nil)
+}
+
+// AngleOffsetRead: Read Offset from Servo's EEPROM
+func (s *Servo) AngleOffsetRead() ([]byte, error) {
+	return s.getData(AngleOffsetRead)
+}
+
+// AngleLimitWrite: Set angle limit
+func (s *Servo) AngleLimitWrite(minAngle uint16, maxAngle uint16) error {
+	return s.sendInstruction(AngleLimitWrite, []uint16{minAngle, maxAngle})
+}
+
+// AngleLimitRead
+// TODO: translate byte to angle and position
+func (s *Servo) AngleLimitRead() ([]byte, error) {
+	return s.getData(AngleLimitRead)
+}
+
+// VoltageLimitWrite
+func (s *Servo) VoltageLimitWrite(minVoltage uint16, maxVoltage uint16) error {
+	return s.sendInstruction(VoltageLimitWrite, []uint16{minVoltage, maxVoltage})
+}
+
+// VoltageLimitRead
+func (s *Servo) VoltageLimitRead() ([]byte, error) {
+	return s.getData(VoltageLimitRead)
+}
+
+// TemperatureMaxLimitWrite
+func (s *Servo) TemperatureMaxLimitWrite(maxTemperature uint16) error {
+	return s.sendInstruction(TemperatureMaxLimitWrite, []uint16{maxTemperature})
+}
+
+// TemperatureMaxLimitRead
+func (s *Servo) TemperatureMaxLimitRead() ([]byte, error) {
+	return s.getData(TemperatureMaxLimitRead)
+}
+
+// TemperatureRead
+func (s *Servo) TemperatureRead() ([]byte, error) {
+	return s.getData(TemperatureRead)
+}
+
+// VoltageRead
+func (s *Servo) VoltageRead() ([]byte, error) {
+	return s.getData(VoltageRead)
+}
+
+// PositionRead
+func (s *Servo) PositionRead() ([]byte, error) {
+	return s.getData(PositionRead)
+}
+
+// MotorModeWrite
+// speed: -1000 to 1000
+func (s *Servo) MotorModeWrite(isMotor uint16, speed uint16) error {
+	return s.sendInstruction(MotorModeWrite, []uint16{isMotor << 8, speed})
+}
+
+// MotorModeRead
+func (s *Servo) MotorModeRead() ([]byte, error) {
+	return s.getData(MotorModeRead)
+}
+
+// LoadOrUnloadWrite
+func (s *Servo) LoadOrUnloadWrite(isLoad uint16) error {
+	return s.sendInstruction(LoadOrUnloadWrite, []uint16{isLoad})
+}
+
+// LoadOrUnloadRead
+func (s *Servo) LoadOrUnloadRead() ([]byte, error) {
+	return s.getData(LoadOrUnloadRead)
+}
+
+// LEDControlWrite
+func (s *Servo) LEDControlWrite(isAlwayOn uint16) error {
+	return s.sendInstruction(LEDControlWrite, []uint16{isAlwayOn})
+}
+
+// LEDControlRead
+func (s *Servo) LEDControlRead() ([]byte, error) {
+	return s.getData(LEDControlRead)
+}
+
+// LEDErrorWrite
+// lightColor: 1~7
+func (s *Servo) LEDErrorWrite(lightColor uint16) error {
+	return s.sendInstruction(LEDErrorWrite, []uint16{lightColor})
+}
+
+// LEDErrorRead
+func (s *Servo) LEDErrorRead() ([]byte, error) {
+	return s.getData(LEDErrorRead)
+}
diff --git a/servo/servo_highlevel.go b/servo/servo_highlevel.go
new file mode 100644
index 0000000..66ec261
--- /dev/null
+++ b/servo/servo_highlevel.go
@@ -0,0 +1,58 @@
+package servo
+
+// High-level interface. (Most of this should be removed, or moved to a separate
+// type which embeds or interacts with the servo type.)
+
+func (s *Servo) posToAngle(p int) float64 {
+	return (positionToAngle * float64(p))
+}
+
+func (s *Servo) angleToPos(angle float64) int {
+	return int((angle) * angleToPosition)
+}
+
+/*
+// Returns the current position of the servo, relative to the zero angle.
+func (s *Servo) Angle() (float64, error) {
+	p, err := s.Position()
+
+	if err != nil {
+		return 0, err
+	}
+
+	return s.posToAngle(p), nil
+}
+
+// MoveTo sets the goal position of the servo by angle (in degrees), where zero
+// is the midpoint, 150 deg is max left (clockwise), and -150 deg is max right
+// (counter-clockwise). This is generally preferable to calling SetGoalPosition,
+// which uses the internal uint16 representation.
+func (s *Servo) MoveTo(angle float64) error {
+	p := s.angleToPos(normalizeAngle(angle))
+	return s.SetGoalPosition(p)
+}
+
+// Voltage returns the current voltage supplied. Unlike the underlying register,
+// this is the actual voltage, not multiplied by ten.
+func (s *Servo) Voltage() (float64, error) {
+	val, err := s.PresentVoltage()
+	if err != nil {
+		return 0.0, err
+	}
+
+	// Convert the return value into actual volts.
+	return (float64(val) / 10), nil
+}
+*/
+//
+func normalizeAngle(d float64) float64 {
+	if d > 180 {
+		return normalizeAngle(d - 360)
+
+	} else if d < -180 {
+		return normalizeAngle(d + 360)
+
+	} else {
+		return d
+	}
+}