From 87f1f9828b02e3dad06fa935776296605459fcee Mon Sep 17 00:00:00 2001 From: jet tsang zeon-git Date: Wed, 5 Aug 2020 15:39:11 +0800 Subject: implementation instructions for lx servo Signed-off-by: jet tsang zeon-git --- iface/iface.go | 19 ++++++ network/network.go | 96 ++++++++++++++++++++++++++++++ servo/instname_string.go | 64 ++++++++++++++++++++ servo/instructions.go | 60 +++++++++++++++++++ servo/lx/lx.go | 48 +++++++++++++++ servo/servo.go | 88 ++++++++++++++++++++++++++++ servo/servo_accessors.go | 149 +++++++++++++++++++++++++++++++++++++++++++++++ servo/servo_highlevel.go | 58 ++++++++++++++++++ 8 files changed, 582 insertions(+) create mode 100644 iface/iface.go create mode 100644 network/network.go create mode 100644 servo/instname_string.go create mode 100644 servo/instructions.go create mode 100644 servo/lx/lx.go create mode 100644 servo/servo.go create mode 100644 servo/servo_accessors.go create mode 100644 servo/servo_highlevel.go 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 + } +} -- cgit v1.2.3