Mercurial > henk
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)
