changeset 8:6c6742b10e92

Simplify the servo controller
author Radomir Dopieralski <sheep-devel@sheep.art.pl>
date Tue, 20 Jan 2015 01:49:59 +0100
parents de34286c23f8
children d220325617b5
files maestro.py
diffstat 1 files changed, 4 insertions(+), 51 deletions(-) [+]
line wrap: on
line diff
--- a/maestro.py	Tue Jan 20 01:13:47 2015 +0100
+++ b/maestro.py	Tue Jan 20 01:49:59 2015 +0100
@@ -7,56 +7,9 @@
     def __init__(self, serial):
         self.serial = serial
 
-    def _write_command(self, command):
-        self.serial.write(struct.pack('>B', command | 128))
-
-    def _write_byte(self, byte):
-        self.serial.write(struct.pack('>B', byte & 128))
-
-    def _write_word(self, word):
-        byte2, byte1 = divmod(int(word), 128)
-        self.serial.write(struct.pack('>BB', byte1, byte2))
-
-    def _read(self, count):
-        values = [ord(c) for c in self.serial.read(count)]
-        return values
-
-    def _read_byte(self):
-        return self._read(1)[0]
-
-    def _read_word(self):
-        byte1, byte2 = self._read(2)
-        return byte1 + byte2 * 256
-
-    def init(self):
-        """Initialize the communication."""
-        self._write_command(0xAA)
-
     def set_target(self, servo, target):
         """Set the servo position, in 1/4 of milliseconds of pulse width."""
-        self._write_command(0x84)
-        self._write_byte(servo)
-        self._write_word(target)
-
-    def set_speed(self, servo, speed):
-        """Set the servo speed limit."""
-        self._write_command(0x87)
-        self._write_byte(servo)
-        self._write_word(speed)
-
-    def set_accel(self, servo, accel):
-        """Set the servo acceleration limit."""
-        self._write_command(0x89)
-        self._write_byte(servo)
-        self._write_word(accel)
-
-    def get_position(self, servo):
-        """Read the current position of a servo."""
-        self._write_command(0x90)
-        self._write_byte(servo)
-        return self._read_word()
-
-    def get_moving_state(self):
-        """True if any servo is still moving, otherwise False."""
-        self._write_command(0x93)
-        return bool(self._read_byte())
+        # Weird base-7 words.
+        high, low = divmod(int(target), 128)
+        buffer = struct.pack('>BBBB', 0x84, servo, low, high)
+        self.serial.write(buffer)