1 files changed, 88 insertions, 0 deletions
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)
+}