<commit_before>package dynamixel

import (
	"errors"
	"math"
)

const (

	// Control Table Addresses
	addrTorqueEnable byte = 0x18 // 1
	addrLed          byte = 0x19 // 1
	addrGoalPosition byte = 0x1E // 2
	addrMovingSpeed  byte = 0x20 // 2
	addrTorqueLimit  byte = 0x22 // 2

	// Read Only
	addrCurrentPosition byte = 0x24 // 2

	// Limits (from dxl_ax_actuator.htm)
	maxPos   uint16  = 1023
	maxSpeed uint16  = 1023
	maxAngle float64 = 300

	// Unit conversions
	positionToAngle float64 = maxAngle / float64(maxPos) // 0.293255132
	angleToPosition float64 = 1 / positionToAngle        // 3.41
)

type DynamixelServo struct {
	Network   *DynamixelNetwork
	Ident     uint8
	zeroAngle float64
}

// http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm
func NewServo(network *DynamixelNetwork, ident uint8) *DynamixelServo {
	return &DynamixelServo{
		Network:   network,
		Ident:     ident,
		zeroAngle: 150,
	}
}

// Converts a bool to an int.
func btoi(b bool) uint8 {
	if b {
		return 1
	}
	return 0
}

func low(i int) byte {
	return byte(i & 0xFF)
}

func high(i int) byte {
	return low(i >> 8)
}

func (servo *DynamixelServo) readData(startAddress byte, length int) (uint16, error) {
	return servo.Network.ReadData(servo.Ident, startAddress, length)
}

func (servo *DynamixelServo) writeData(params ...byte) error {
	return servo.Network.WriteData(servo.Ident, params...)
}

func posDistance(a uint16, b uint16) uint16 {
	return uint16(math.Abs(float64(a) - float64(b)))
}

//
// -- High-level interface
//
//    These methods should provide as useful and friendly of an interface to the
//    servo as possible.

func (servo *DynamixelServo) posToAngle(pos uint16) float64 {
	return (positionToAngle * float64(pos)) - servo.zeroAngle
}

func (servo *DynamixelServo) angleToPos(angle float64) uint16 {
	pos := uint16((servo.zeroAngle + angle) * angleToPosition)
	return pos
}

// Sets the origin angle (in degrees).
func (servo *DynamixelServo) SetZero(offset float64) {
	servo.zeroAngle = offset
}

//
// Returns the current position of the servo, relative to the zero angle.
//
func (servo *DynamixelServo) Angle() (float64, error) {
	pos, err := servo.Position()

	if err != nil {
		return 0, err

	} else {
		return servo.posToAngle(pos), nil
	}
}

// Sets the goal position of the servo, by angle.
func (servo *DynamixelServo) MoveTo(angle float64) error {
	pos := int(servo.angleToPos(angle))
	return servo.SetGoalPosition(pos)
}

//
// -- Low-level interface
//
//    These methods should follow the Dynamixel protocol docs as closely as
//    possible, with no fancy stuff.
//

// Enables or disables torque.
func (servo *DynamixelServo) SetTorqueEnable(state bool) error {
	return servo.writeData(addrTorqueEnable, btoi(state))
}

// Enables or disables the LED.
func (servo *DynamixelServo) SetLed(state bool) error {
	return servo.writeData(addrLed, btoi(state))
}

// Sets the goal position.
// See: http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm#Actuator_Address_1E
func (servo *DynamixelServo) SetGoalPosition(pos int) error {
	if pos < 0 || pos > int(maxPos) {<commit_msg>Normalize multiply-rotated angles<commit_after>package dynamixel

import (
	"errors"
	"math"
)

const (

	// Control Table Addresses
	addrTorqueEnable byte = 0x18 // 1
	addrLed          byte = 0x19 // 1
	addrGoalPosition byte = 0x1E // 2
	addrMovingSpeed  byte = 0x20 // 2
	addrTorqueLimit  byte = 0x22 // 2

	// Read Only
	addrCurrentPosition byte = 0x24 // 2

	// Limits (from dxl_ax_actuator.htm)
	maxPos   uint16  = 1023
	maxSpeed uint16  = 1023
	maxAngle float64 = 300

	// Unit conversions
	positionToAngle float64 = maxAngle / float64(maxPos) // 0.293255132
	angleToPosition float64 = 1 / positionToAngle        // 3.41
)

type DynamixelServo struct {
	Network   *DynamixelNetwork
	Ident     uint8
	zeroAngle float64
}

// http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm
func NewServo(network *DynamixelNetwork, ident uint8) *DynamixelServo {
	return &DynamixelServo{
		Network:   network,
		Ident:     ident,
		zeroAngle: 150,
	}
}

// Converts a bool to an int.
func btoi(b bool) uint8 {
	if b {
		return 1
	}
	return 0
}

func low(i int) byte {
	return byte(i & 0xFF)
}

func high(i int) byte {
	return low(i >> 8)
}

func (servo *DynamixelServo) readData(startAddress byte, length int) (uint16, error) {
	return servo.Network.ReadData(servo.Ident, startAddress, length)
}

func (servo *DynamixelServo) writeData(params ...byte) error {
	return servo.Network.WriteData(servo.Ident, params...)
}

func posDistance(a uint16, b uint16) uint16 {
	return uint16(math.Abs(float64(a) - float64(b)))
}

//
func normalizeAngle(d float64) float64 {
	if d > 180 {
		return normalizeAngle(d - 360)

	} else if d < -180 {
		return normalizeAngle(d + 360)

	} else {
		return d
	}
}

//
// -- High-level interface
//
//    These methods should provide as useful and friendly of an interface to the
//    servo as possible.

func (servo *DynamixelServo) posToAngle(pos uint16) float64 {
	return (positionToAngle * float64(pos)) - servo.zeroAngle
}

func (servo *DynamixelServo) angleToPos(angle float64) uint16 {
	pos := uint16((servo.zeroAngle + angle) * angleToPosition)
	return pos
}

// Sets the origin angle (in degrees).
func (servo *DynamixelServo) SetZero(offset float64) {
	servo.zeroAngle = offset
}

//
// Returns the current position of the servo, relative to the zero angle.
//
func (servo *DynamixelServo) Angle() (float64, error) {
	pos, err := servo.Position()

	if err != nil {
		return 0, err

	} else {
		return servo.posToAngle(pos), 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.
//
// If the angle is out of bounds
//
func (servo *DynamixelServo) MoveTo(angle float64) error {
	pos := int(servo.angleToPos(normalizeAngle(angle)))
	return servo.SetGoalPosition(pos)
}

//
// -- Low-level interface
//
//    These methods should follow the Dynamixel protocol docs as closely as
//    possible, with no fancy stuff.
//

// Enables or disables torque.
func (servo *DynamixelServo) SetTorqueEnable(state bool) error {
	return servo.writeData(addrTorqueEnable, btoi(state))
}

// Enables or disables the LED.
func (servo *DynamixelServo) SetLed(state bool) error {
	return servo.writeData(addrLed, btoi(state))
}

// Sets the goal position.
// See: http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm#Actuator_Address_1E
func (servo *DynamixelServo) SetGoalPosition(pos int) error {
	if pos < 0 || pos > int(maxPos) {
{'fraction_non_alphanumeric': 0.11232311320754718, 'fraction_numerical': 0.03508254716981132, 'mean_word_length': 3.634562841530055, 'pattern_counts': {'":': 0, '<': 6, '<?xml version=': 0, '>': 10, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '544158', 'n_tokens_mistral': 2613, 'n_tokens_neox': 2385, 'n_words': 681}