158 lines
4.3 KiB
Python
158 lines
4.3 KiB
Python
"""
|
|
A wrapper for the Webots servo device.
|
|
|
|
The servo will apply a small amount of variation to the power setting to simulate
|
|
inaccuracies in the servo.
|
|
"""
|
|
from __future__ import annotations
|
|
|
|
from abc import ABC, abstractmethod
|
|
from typing import TYPE_CHECKING
|
|
|
|
from sbot_interface.devices.util import (
|
|
WebotsDevice,
|
|
add_jitter,
|
|
get_globals,
|
|
get_robot_device,
|
|
map_to_range,
|
|
)
|
|
|
|
if TYPE_CHECKING:
|
|
from controller import PositionSensor
|
|
|
|
MAX_POSITION = 4000
|
|
MIN_POSITION = 500
|
|
SERVO_MAX = 2000
|
|
SERVO_MIN = 1000
|
|
|
|
|
|
class BaseServo(ABC):
|
|
"""The base class for all the servos that can be connected to the Servo board."""
|
|
|
|
@abstractmethod
|
|
def disable(self) -> None:
|
|
"""Disable the servo."""
|
|
pass
|
|
|
|
@abstractmethod
|
|
def set_position(self, value: int) -> None:
|
|
"""
|
|
Set the position of the servo.
|
|
|
|
Position is the pulse width in microseconds.
|
|
"""
|
|
pass
|
|
|
|
@abstractmethod
|
|
def get_position(self) -> int:
|
|
"""Return the current position of the servo."""
|
|
pass
|
|
|
|
@abstractmethod
|
|
def get_current(self) -> int:
|
|
"""Return the current draw of the servo in mA."""
|
|
pass
|
|
|
|
@abstractmethod
|
|
def enabled(self) -> bool:
|
|
"""Return whether the servo is enabled."""
|
|
pass
|
|
|
|
|
|
class NullServo(BaseServo):
|
|
"""A servo that does nothing. Used for servos that are not connected."""
|
|
|
|
def __init__(self) -> None:
|
|
self.position = 1500
|
|
self._enabled = False
|
|
|
|
def disable(self) -> None:
|
|
"""Disable the servo."""
|
|
self._enabled = False
|
|
|
|
def set_position(self, value: int) -> None:
|
|
"""
|
|
Set the position of the servo.
|
|
|
|
Position is the pulse width in microseconds.
|
|
"""
|
|
self.position = value
|
|
self._enabled = True
|
|
|
|
def get_position(self) -> int:
|
|
"""
|
|
Return the current position of the servo.
|
|
|
|
Position is the pulse width in microseconds.
|
|
"""
|
|
return self.position
|
|
|
|
def get_current(self) -> int:
|
|
"""Return the current draw of the servo in mA."""
|
|
return 0
|
|
|
|
def enabled(self) -> bool:
|
|
"""Return whether the servo is enabled."""
|
|
return self._enabled
|
|
|
|
|
|
class Servo(BaseServo):
|
|
"""A servo connected to the Servo board."""
|
|
|
|
def __init__(self, device_name: str) -> None:
|
|
self.position = (SERVO_MAX + SERVO_MIN) // 2
|
|
# TODO use setAvailableForce to simulate disabled
|
|
self._enabled = False
|
|
g = get_globals()
|
|
self._device = get_robot_device(g.robot, device_name, WebotsDevice.Motor)
|
|
self._pos_sensor: PositionSensor | None = self._device.getPositionSensor() # type: ignore[no-untyped-call]
|
|
self._max_position = self._device.getMaxPosition()
|
|
self._min_position = self._device.getMinPosition()
|
|
if self._pos_sensor is not None:
|
|
self._pos_sensor.enable(g.timestep)
|
|
|
|
def disable(self) -> None:
|
|
"""Disable the servo."""
|
|
self._enabled = False
|
|
|
|
def set_position(self, value: int) -> None:
|
|
"""
|
|
Set the position of the servo.
|
|
|
|
Position is the pulse width in microseconds.
|
|
"""
|
|
# Apply a small amount of variation to the power setting to simulate
|
|
# inaccuracies in the servo
|
|
value = int(add_jitter(value, (SERVO_MIN, SERVO_MAX), std_dev_percent=0.5))
|
|
|
|
self._device.setPosition(map_to_range(
|
|
value,
|
|
(SERVO_MIN, SERVO_MAX),
|
|
(self._min_position + 0.001, self._max_position - 0.001),
|
|
))
|
|
self.position = value
|
|
self._enabled = True
|
|
|
|
def get_position(self) -> int:
|
|
"""
|
|
Return the current position of the servo.
|
|
|
|
Position is the pulse width in microseconds.
|
|
"""
|
|
if self._pos_sensor is not None:
|
|
self.position = int(map_to_range(
|
|
self._pos_sensor.getValue(),
|
|
(self._min_position + 0.001, self._max_position - 0.001),
|
|
(MIN_POSITION, MAX_POSITION),
|
|
))
|
|
return self.position
|
|
|
|
def get_current(self) -> int:
|
|
"""Return the current draw of the servo in mA."""
|
|
# TODO calculate from torque feedback
|
|
return 0
|
|
|
|
def enabled(self) -> bool:
|
|
"""Return whether the servo is enabled."""
|
|
return self._enabled
|